def record(message):
msg = message.payload.decode()
lst = msg.replace("[", "").replace("]", "").replace("'", "")
lst = lst.split(', ')
action = lst[0]
sensors = lst[1:]
if action == "start":
if 'IMU_acc' in sensors:
IMU.record('IMU_acc')
if 'IMU_vel' in sensors:
IMU.record('IMU_vel')
if 'Razor_acc' in sensors:
Razor_IMU.record('Razor_acc')
if 'Razor_vel' in sensors:
Razor_IMU.record('Razor_vel')
if 'Load_Cell' in sensors:
LoadCell.record()
if 'CAN' in sensors:
CAN.record()
elif action == "stop":
if 'IMU_acc' in sensors:
IMU.record_stop('IMU_acc')
if 'IMU_vel' in sensors:
IMU.record_stop('IMU_vel')
if 'Razor_acc' in sensors:
Razor_IMU.record_stop('Razor_acc')
if 'Razor_vel' in sensors:
Razor_IMU.record_stop('Razor_vel')
if 'Load_Cell' in sensors:
LoadCell.record_stop()
if 'CAN' in sensors:
CAN.record_stop()
CSV.unifile(sensors)
def record(sid, data):
action = data[0]
sensors = data[1]
if action == "start":
if 'IMU_acc' in sensors:
IMU.record('IMU_acc')
if 'IMU_vel' in sensors:
IMU.record('IMU_vel')
if 'Razor_acc' in sensors:
Razor_IMU.record('Razor_acc')
if 'Razor_vel' in sensors:
Razor_IMU.record('Razor_vel')
if 'Load_Cell' in sensors:
LoadCell.record()
if 'CAN' in sensors:
CAN.record()
elif action == "stop":
if 'IMU_acc' in sensors:
IMU.record_stop('IMU_acc')
if 'IMU_vel' in sensors:
IMU.record_stop('IMU_vel')
if 'Razor_acc' in sensors:
Razor_IMU.record_stop('Razor_acc')
if 'Razor_vel' in sensors:
Razor_IMU.record_stop('Razor_vel')
if 'Load_Cell' in sensors:
LoadCell.record_stop()
if 'CAN' in sensors:
CAN.record_stop()
CSV.unifile(sensors)
def parse_request(request):
interface = request.get('bus')
if interface == None:
interface = CAN_SEND_INTERFACE
else:
interface = interface[0]
try:
bus = CAN.Bus(interface)
except OSError:
raise BadRequest("bus '" + interface + "' does not exist")
id = request.get('id')
if id == None:
raise BadRequest("id not specified")
id = int(id[0])
data = request.get('data[]') # POST
if data == None:
data = request.get('data') # GET
if data == None:
data = [] # parse_url removes data field if empty array
else:
data = data[0].strip('[]').split(',')
if len(data) == 1:
if data[0] == '':
data = []
data = list(map(int, data))
msg = CAN.Message(id, data)
return (bus, msg)
def display(battery=None): #print watchdog countdown
global stdscr
# Read ADC values
adc_values = ADC.read()
global frequency
frequency = PLL.Get_Frequency()
stdscr.addstr(0, 10, "Battery")
stdscr.addstr(1, 0, "==============================")
stdscr.addstr(2, 0, f"ADC:")
stdscr.addstr(2, 10, f"Low Precision: {adc_values[0][0]}")
stdscr.addstr(3, 10, f"High Precision: {adc_values[0][1]}")
# Read Current values
stdscr.addstr(4, 0, f"Current:")
stdscr.addstr(4, 10, f"{adc_values[1]} A ")
#Read CAN data
CANdata = CAN.Get_CAN_Info()
text = ' '.join(CANdata) #put elements of the list of CAN data bytes into a string
CANbox.erase() #clear previous data in the box
CANbox.addstr(4, 0, textwrap.fill(text, 40))
CANbox.addstr(3, 2, "CAN ID and Message:")
# Display Watchdog ticks
ticks = WDTimer.Tick()
stdscr.addstr(10, 0, f" ") #clear previous tick
stdscr.addstr(10, 0, f"WDTimer Countdown: {ticks}")
#Display current frequency
stdscr.addstr(6, 0, f" ")
stdscr.addstr(6, 0, f"Clock Frequency: {frequency} Hz")
# Read Module values
stdscr.addstr(0, 54, "Modules")
stdscr.addstr(1, 40, "====================================")
module_values = SPI.read()
for i, module in enumerate(module_values):
stdscr.addstr(i+2, 37, f"{i+1}")
stdscr.addstr(i+2, 40, f"| {'X' if module[0] else ' '} | {module[1]/10000:.4f}V | {module[2]/1000:.3f}°C | {module[3]/1000:.3f}°C |")
# Read LED values
stdscr.addstr(0, 90, "LEDs")
stdscr.addstr(1, 80, "=======================")
lights = Lights.read()
curses.init_pair(1, curses.COLOR_RED, curses.COLOR_RED)
curses.init_pair(2, curses.COLOR_GREEN, curses.COLOR_GREEN)
curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_BLACK)
for i in range(9):
stdscr.addstr(i+2, 80, lights_names[i])
if lights & (0x1<<i):
if i == 7:
stdscr.addstr(i+2, 100, "[]", curses.color_pair(2))
else:
stdscr.addstr(i+2, 100, "[]", curses.color_pair(1))
else:
stdscr.addstr(i+2, 100, "[]", curses.color_pair(3))
strobe = Strobelight.read()
stdscr.addstr(11, 80, 'S_PULSING')
if strobe:
stdscr.addstr(11, 100, "[]", curses.color_pair(2))
else:
stdscr.addstr(11, 100, "[]", curses.color_pair(3))
stdscr.refresh()
def parse_net(net):
global default_net
if net == 'default':
net = default_net
else:
net = CAN_INTERFACES[int(net) - 1]
return CAN.Bus(net)
def _send_sync(self):
sync_object = self.od.get(ODI_SYNC)
if sync_object is not None:
sync_value = sync_object.get(ODSI_VALUE)
if sync_value is not None:
sync_id = sync_value & 0x3FF
msg = CAN.Message(sync_id)
self._send(msg)
def update_CAN():
"""Periodically update the display state of the CAN bus"""
global id_text, message_text
can = CAN.read()
id_text.set(f"ID: {can[0]}")
message_text.set(f"Message: {can[1]}")
window.after(CAN1_FREQ, update_CAN)
def heartbeat_consumer_timeout(id):
global active_bus, canopen_od, nmt_state, node_id
if nmt_state != CANopen.NMT_STATE_STOPPED:
msg = CAN.Message(
canopen_od.get(CANopen.ODI_EMCY_ID),
(CANopen.EMCY_HEARTBEAT_BY_NODE + id).to_bytes(
2, byteorder='big') + canopen_od.get(CANopen.ODI_ERROR).get(
CANopen.ODSI_VALUE).to_bytes(1, byteorder='big') +
b'\x00\x00\x00\x00\x00')
active_bus.send(msg)
def _heartbeat_consumer_timeout(self, id):
if self.nmt_state != NMT_STATE_STOPPED:
emcy_id = self.od.get(ODI_EMCY_ID)
if emcy_id is not None:
msg = CAN.Message(
emcy_id,
(EMCY_HEARTBEAT_BY_NODE + id).to_bytes(2, byteorder='big')
+ self.od.get(ODI_ERROR).get(ODSI_VALUE).to_bytes(
1, byteorder='big') + b'\x00\x00\x00\x00\x00')
self._send(msg)
def listen(self):
bus = CAN.Bus(DEFAULT_CAN_INTERFACE)
while True:
rlist, _, _ = select([self.websocket, bus], [], [])
for s in rlist:
if isinstance(s, CAN.Bus):
msg = s.recv()
self.send(bytes(msg))
elif isinstance(s, WebSocket):
msg = s.recv()
if msg is None:
return
bus.send(CAN.Message.from_bytes(msg))
def write(id_, message):
"""Writes message to CAN2
Args:
id_ (int): ID of message to be sent
message (int): contents of message to be sent
"""
CAN1 = CAN.read()
with open(file, "w") as csvfile:
fcntl.flock(csvfile.fileno(), fcntl.LOCK_EX)
csvwriter = csv.writer(csvfile)
csvwriter.writerow(CAN1)
csvwriter.writerow(
[hex(id_), hex(message),
len(hex(message)[2:]) // 2])
fcntl.flock(csvfile.fileno(), fcntl.LOCK_UN)
def _send_pdo(self, i):
i = i - 1
data = bytes()
tpdo_mp = self.od.get(ODI_TPDO1_MAPPING_PARAMETER + i)
if tpdo_mp is not None:
for j in range(tpdo_mp.get(ODSI_VALUE, 0)):
mapping_param = tpdo_mp.get(j + 1)
if mapping_param is not None:
mapping_object = self.od.get(mapping_param >> 16)
if mapping_object is not None:
mapping_value = mapping_object.get((mapping_param >> 8)
& 0xFF)
if mapping_value is not None:
data = data + mapping_value.to_bytes(
(mapping_param & 0xFF) // 8, byteorder='big')
msg = CAN.Message(((FUNCTION_CODE_TPDO1 +
(2 * i)) << FUNCTION_CODE_BITNUM) + self.id,
data)
self._send(msg)
def _send_heartbeat(self):
msg = CAN.Message(
(FUNCTION_CODE_NMT_ERROR_CONTROL << FUNCTION_CODE_BITNUM) +
self.id, [self.nmt_state])
self._send(msg)
def _send_bootup(self):
msg = CAN.Message((
FUNCTION_CODE_NMT_ERROR_CONTROL << FUNCTION_CODE_BITNUM) + self.id)
self._send(msg)
def send_sync():
global active_bus, canopen_od
sync_id = canopen_od.get(CANopen.ODI_SYNC).get(CANopen.ODSI_VALUE) & 0x3FF
msg = CAN.Message(sync_id)
active_bus.send(msg)
CAN_SEND_INTERFACE = "vcan0"
UDP_LISTEN_IP = "127.0.0.1"
UDP_LISTEN_PORT = 5005
UDP_SEND_IP = "127.0.0.1"
UDP_SEND_PORT = 5006
def sigterm_handler(signum, frame):
sys.exit()
signal.signal(signal.SIGTERM, sigterm_handler)
sockets = []
for interface in CAN_LISTEN_INTERFACES:
can_socket = CAN.Bus(interface)
sockets.append(can_socket)
for s in sockets:
if s.getsockname()[0] == CAN_SEND_INTERFACE:
can_socket = s
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_socket.bind((UDP_LISTEN_IP, UDP_LISTEN_PORT))
sockets.append(udp_socket)
while True:
try:
rlist, _, _ = select(sockets, [], [])
for s in rlist:
socket_type = s.getsockopt(socket.SOL_SOCKET, socket.SO_TYPE)
if isinstance(s, CAN.Bus):
msg = s.recv()
def main():
print("Welcome to the BPS Simulator")
print("Type 'start' to start BeVolt. Otherwise, you can specify the types of data to simulate.")
print(">>", end="")
if input() == 'start':
# Initial capacity is (2500*14)/(2950*14)=0.847 i.e. 84.7% charged
init_capacity_mah = 2500 * config.num_batt_cells_parallel_per_module
# Amperes current draw of the electrical system
ampere_draw = 30
# Create state of the battery
BeVolt = battery.Battery(ampere_draw, config.total_batt_pack_capacity_mah, init_capacity_mah)
PLL.PLL_Init()
else:
BeVolt = None
configure()
try:
launch_bevolt()
except Exception as e:
print(repr(e))
global stdscr
global CANbox
stdscr = curses.initscr()
curses.start_color()
curses.noecho()
curses.cbreak()
#box is for CAN messages
CANbox = curses.newwin(7, 21, 12, 78)
CANbox.immedok(True)
CANbox.box()
CANbox.refresh()
#Start background thread for timer
timerThread = Timer.timer_Thread
timerThread.start()
while True:
try:
# Generate all values
generate(BeVolt)
# Display all values
display(BeVolt)
time.sleep(1) # one second delay
except KeyboardInterrupt:
curses.endwin()
if BeVolt is not None:
print("\n\rWould you like to change 'wires', 'quit', or 'PLL'?")
print(">>", end="")
choice = input()
if choice == 'wires':
change_wires(BeVolt)
stdscr = curses.initscr()
curses.start_color()
elif choice == 'quit':
break
elif choice == 'PLL':
print("Enter the frequency you would like to change the clock to in Hz.")
frequency = int(input())
PLL.Change_Frequency(frequency)
else:
print("That is not a valid option. Continuing simulation...")
stdscr = curses.initscr()
curses.start_color()
else:
print("\n\rWould you like to change 'config', 'quit', or send a CAN message ('CAN')?")
choice = input()
if choice == 'config':
configure()
stdscr = curses.initscr()
elif choice == 'quit':
break
elif choice == 'CAN':
print("Enter the CAN ID for the system you wish to simulate. Leave out '0x'.")
id = input()
while(CAN.Invalid_CAN_ID(id) == True):
print("Invalid CAN ID. Try again.")
id = input()
print("Enter up to 8 bytes of the CAN message that you would like to send, and separate each byte by a ','. Leave out '0x'.")
message = input().split(',')
CAN.Send_Message(id, message, len(message))
else:
print("That is not a valid option. Continuing simulation...")
stdscr = curses.initscr()
curses.start_color()
except Exception as e:
curses.echo()
curses.nocbreak()
curses.endwin()
print("ERROR:", end=" ")
print(repr(e), end="\r\n")
print("If addwstr() returned ERR, make your terminal window bigger.")
print("\n\rContinue? (Y/n): ", end="")
cont = input()
if(cont.lower() == "n" or cont.lower() == "no"):
break
print("Continuing...")
main()
curses.echo()
curses.nocbreak()
curses.endwin()
Timer.terminate(True)
#!/usr/bin/python3
import CAN
import CANopen
CAN_INTERFACE = "vcan0"
can_bus = CAN.Bus(CAN_INTERFACE)
node_id = 0x02
canopen_od = CANopen.ObjectDictionary.from_eds('node.eds')
node = CANopen.Node(can_bus, node_id, canopen_od)
while True:
pass # Run forever
def send_heartbeat():
global active_bus, node_id, nmt_state
msg = CAN.Message((CANopen.FUNCTION_CODE_NMT_ERROR_CONTROL <<
CANopen.FUNCTION_CODE_BITNUM) + node_id, [nmt_state])
active_bus.send(msg)
def sendCommands(sid, data):
CAN.command(data)
def sendCommands(message):
msg = message.payload.decode('utf-8').replace("'", '"')
msg = json.loads(msg)
CAN.command(msg)
def main():
print("Welcome to the BPS Simulator")
print(
"Type 'start' to start BeVolt. Otherwise, you can specify the types of data to simulate."
)
print(">>", end="")
if input() == 'start':
# Initial capacity is (2500*14)/(2950*14)=0.847 i.e. 84.7% charged
init_capacity_mah = 2500 * config.num_batt_cells_parallel_per_module
# Amperes current draw of the electrical system
ampere_draw = 30
# Create state of the battery
BeVolt = battery.Battery(ampere_draw,
config.total_batt_pack_capacity_mah,
init_capacity_mah)
PLL.PLL_Init()
SPI.init(battery=BeVolt)
else:
BeVolt = None
configure()
SPI.init(state=state, mode=mode)
try:
launch_bevolt()
except Exception as e:
print(repr(e))
logging.basicConfig(filename='debug.log', level=logging.DEBUG)
global stdscr
global CANbox
stdscr = curses.initscr()
curses.start_color()
curses.noecho()
curses.cbreak()
#box is for CAN messages
CANbox = curses.newwin(7, 21, 12, 80)
CANbox.immedok(True)
CANbox.box()
CANbox.refresh()
#Start background thread for timer
timerThread = Timer.timer_Thread
timerThread.start()
spiThread = SPI.spi_thread
spiThread.start()
while True:
try:
# Generate all values
generate(BeVolt)
# Display all values
display(BeVolt)
time.sleep(1) # one second delay
except KeyboardInterrupt:
curses.endwin()
if BeVolt is not None:
print(
"\n\rWould you like to change \n\r1. 'wires'\n\r2. 'quit'\n\r3. 'PLL'\n\r4. send a CAN message ('CAN')\n\r5. 'EEPROM'"
)
else:
print(
"\n\rWould you like to change\n\r1. 'config'\n\r2. 'quit'\n\r3. 'PLL'\n\r4. send a CAN message ('CAN')\n\r5. 'EEPROM'"
)
print(">>", end="")
choice = input()
if (choice == 'wires' or choice == '1') and BeVolt is not None:
change_wires(BeVolt)
stdscr = curses.initscr()
curses.start_color()
elif (choice == 'config' or choice == '1') and BeVolt is None:
configure()
stdscr = curses.initscr()
elif choice == 'quit' or choice == '2':
break
elif choice == 'PLL' or choice == '3':
print(
"Enter the frequency you would like to change the clock to in Hz."
)
frequency = int(input())
PLL.Change_Frequency(frequency)
elif choice == 'CAN' or choice == '4':
print(
"Enter the CAN ID for the system you wish to simulate. Leave out '0x'."
)
id = input()
while (CAN.Invalid_CAN_ID(id) == True):
print("Invalid CAN ID. Try again.")
id = input()
print(
"Enter up to 8 bytes of the CAN message that you would like to send, and separate each byte by a ','. Leave out '0x'."
)
message = input().split(',')
CAN.Send_Message(id, message, len(message))
elif choice == 'EEPROM' or choice == '5':
returnErrorCodes = 0
print(
"Enter 'all' for all data or 'read' to enter specific address to read."
)
print(">>", end="")
choiceEEPROM1 = input()
print(
"Enter 'raw' to read the raw hex values or 'msg' for the translated error messages.",
end="\n")
print("If invalid response is given, default is raw data.")
print(">>", end="")
choiceEEPROM2 = input()
if choiceEEPROM2 == 'raw':
returnErrorCodes = 0
elif choiceEEPROM2 == 'msg':
returnErrorCodes = 1
else:
print("Invalid entry...", end="\n")
print("Defaulted to raw data.")
if choiceEEPROM1 == 'all':
print(I2C.EEPROM_Dump(returnErrorCodes))
print("Enter to continue simulator:")
print(">>", end="")
choice = input()
elif choiceEEPROM1 == 'read':
print(
"Enter address to start reading faults from (in hex format)."
)
print(">>", end="")
EEPROMstartAddress = input()
print(
"Enter address to stop reading faults from (in hex format)."
)
print(">>", end="")
EEPROMendAddress = input()
EEPROMstartAddress = int(EEPROMstartAddress, 16)
EEPROMendAddress = int(EEPROMendAddress, 16)
if EEPROMstartAddress >= 0 and EEPROMstartAddress <= maxEEPROMAddress and EEPROMendAddress >= 0 and EEPROMendAddress <= maxEEPROMAddress:
print(
I2C.I2C_Read(EEPROMstartAddress, EEPROMendAddress,
returnErrorCodes))
print("Enter to continue simulator:")
choiceEEPROM = input()
else:
print("Invalid address...", end="\n")
print("Enter to continue simulator:")
choiceEEPROM = input()
else:
print("Invalid entry given for 1st choice (all/read)...",
end="\n")
print("Enter to continue simulator:")
choiceEEPROM = input()
else:
print("That is not a valid option. Continuing simulation...")
stdscr = curses.initscr()
curses.start_color()
except Exception as e:
curses.echo()
curses.nocbreak()
curses.endwin()
print("ERROR:", end=" ")
print(repr(e), end="\r\n")
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
print(exc_type, fname, exc_tb.tb_lineno)
print(
"If addwstr() returned ERR, make your terminal window bigger.")
print("\n\rContinue? (Y/n): ", end="")
cont = input()
if (cont.lower() == "n" or cont.lower() == "no"):
break
print("Continuing...")
main()
curses.echo()
curses.nocbreak()
curses.endwin()
Timer.terminate(True)
id_text2 = tk.StringVar(value="ID: ")
id2_ = tk.Label(master=can2_frame, textvariable=id_text2)
id2_.grid(row=0, column=0, sticky="nsew")
message_text2 = tk.StringVar(value="Message: ")
message2 = tk.Label(master=can2_frame, textvariable=message_text2)
message2.grid(row=1, column=0, sticky="nsew")
### CAN messages input ###
can_id_input = tk.Entry(master=CAN_messages_frame)
can_id_input.grid(row=0, column=0)
can_msg_input = tk.Entry(master=CAN_messages_frame)
can_msg_input.grid(row=1, column=0)
can_button = tk.Button(
master=CAN_messages_frame,
text="Send",
command=lambda: CAN.write(can_id_input.get(), can_msg_input.get()),
)
can_button.grid(row=2, column=0)
### Motor ###
desired_velocity_text = tk.StringVar(value="Desired Velocity: ")
desired_velocity = tk.Label(master=motor_frame, textvariable=desired_velocity_text)
desired_velocity.grid(row=0, column=0, sticky="nsew")
current_velocity_text = tk.StringVar(value="Current Velocity: ")
current_velocity = tk.Label(master=motor_frame, textvariable=current_velocity_text)
current_velocity.grid(row=1, column=0, sticky="nsew")
### Precharge ###
precharge_motor_status = tk.StringVar(value="Motor Precharge: ")
precharge_motor_ = tk.Label(master=precharge_frame, textvariable=precharge_motor_status)
precharge_motor_.grid(row=0, column=0, sticky="nsew")
GPIO.setup(PIN_ENABLE_N, GPIO.IN)
GPIO.setup(PIN_ADDRESS_N, GPIO.IN)
GPIO.setup(PIN_ADDRESS_PARITY_N, GPIO.IN)
runled0 = CANopenIndicator(PIN_RUNLED0, CANopenIndicator.OFF)
errled0 = CANopenIndicator(PIN_ERRLED0, CANopenIndicator.ON)
runled1 = CANopenIndicator(PIN_RUNLED1, CANopenIndicator.OFF)
errled1 = CANopenIndicator(PIN_ERRLED1, CANopenIndicator.ON)
nmt_state = None
error_indicator_timer = None
heartbeat_consumer_timers = {}
heartbeat_producer_timer = None
sync_timer = None
default_bus = CAN.Bus(DEFAULT_CAN_INTERFACE)
redundant_bus = CAN.Bus(REDUNDANT_CAN_INTERFACE)
active_bus = default_bus
error_indicator_timer = IntervalTimer(1, process_error_indicators)
error_indicator_timer.start()
while True:
try:
set_nmt_state(CANopen.NMT_STATE_INITIALISATION)
# set_nmt_state(CANopen.NMT_STATE_RESET_NODE)
reset_timers()
if GPIO.input(PIN_ENABLE_N) == GPIO.HIGH:
raise ResetNode
canopen_od = CANopen.ObjectDictionary()
while True:
def send_bootup():
global active_bus, node_id
msg = CAN.Message((CANopen.FUNCTION_CODE_NMT_ERROR_CONTROL <<
CANopen.FUNCTION_CODE_BITNUM) + node_id)
active_bus.sendall(msg)
def recv(self, msg: CAN.Message):
id = msg.arbitration_id
data = msg.data
rtr = msg.is_remote_frame
fc = (id >> FUNCTION_CODE_BITNUM) & 0xF
if rtr:
target_node = id & 0x7F
if target_node == self.id or target_node == BROADCAST_NODE_ID:
if self.nmt_state == NMT_STATE_OPERATIONAL:
if fc == FUNCTION_CODE_TPDO1:
tpdo1_cp = self.od.get(
ODI_TPDO1_COMMUNICATION_PARAMETER)
if tpdo1_cp is not None:
tpdo1_cp_id = tpdo1_cp.get(ODSI_TPDO_COMM_PARAM_ID)
if tpdo1_cp_id is not None and (
tpdo1_cp_id >>
TPDO_COMM_PARAM_ID_VALID_BITNUM
) & 1 == 0 and (
tpdo1_cp_id >>
TPDO_COMM_PARAM_ID_RTR_BITNUM) & 1 == 0:
self._send_pdo(1)
elif fc == FUNCTION_CODE_NMT_ERROR_CONTROL:
self._send_heartbeat()
elif fc == FUNCTION_CODE_NMT:
command = id & 0x7F
if command == NMT_NODE_CONTROL:
target_node = data[1]
if target_node == self.id or target_node == BROADCAST_NODE_ID:
cs = data[0]
if cs == NMT_NODE_CONTROL_START:
self.nmt_state = NMT_STATE_OPERATIONAL
elif cs == NMT_NODE_CONTROL_STOP:
self.nmt_state = NMT_STATE_STOPPED
elif cs == NMT_NODE_CONTROL_PREOPERATIONAL:
self.nmt_state = NMT_STATE_PREOPERATIONAL
elif cs == NMT_NODE_CONTROL_RESET_NODE:
self.reset()
elif cs == NMT_NODE_CONTROL_RESET_COMMUNICATION:
self.reset_communication()
elif fc == FUNCTION_CODE_SYNC and self.nmt_state == NMT_STATE_OPERATIONAL:
for i in range(4):
tpdo_cp = self.od.get(ODI_TPDO1_COMMUNICATION_PARAMETER + i)
if tpdo_cp is not None:
tpdo_cp_id = tpdo_cp.get(ODSI_TPDO_COMM_PARAM_ID)
if tpdo_cp_id is not None and (
tpdo_cp_id >>
TPDO_COMM_PARAM_ID_VALID_BITNUM) & 1 == 0:
tpdo_cp_type = tpdo_cp.get(ODSI_TPDO_COMM_PARAM_TYPE)
if tpdo_cp_type is not None and (
(tpdo_cp_type >= 0 and tpdo_cp_type <= 240)
or tpdo_cp_type == 252):
self._send_pdo(i + 1)
elif fc == FUNCTION_CODE_SDO_RX and self.nmt_state != NMT_STATE_STOPPED:
sdo_server_object = self.od.get(ODI_SDO_SERVER)
if sdo_server_object is not None:
sdo_server_id = sdo_server_object.get(ODSI_SERVER_DEFAULT_CSID)
if sdo_server_id is not None and id == sdo_server_id:
ccs = (data[0] >> SDO_CCS_BITNUM) & (2**SDO_CCS_LENGTH - 1)
if len(data) >= 4:
odi = (data[1] << 8) + data[2]
odsi = data[3]
if odi in self.od:
obj = self.od.get(odi)
if odsi in obj:
if ccs == SDO_CCS_UPLOAD:
scs = SDO_SCS_UPLOAD
sdo_data = obj.get(odsi)
elif ccs == SDO_CCS_DOWNLOAD:
scs = SDO_SCS_DOWNLOAD
n = (data[0] >> SDO_N_BITNUM) & (
2**SDO_N_LENGTH - 1)
self.od.update(
{
odi:
int.from_bytes(data[4:],
byteorder='big')
}
) # Could be data[4:7-n], or different based on data type
sdo_data = 0
else:
scs = SDO_CS_ABORT
sdo_data = SDO_ABORT_INVALID_CS
else:
scs = SDO_CS_ABORT
sdo_data = SDO_ABORT_SUBINDEX_DNE
else:
scs = SDO_CS_ABORT
sdo_data = SDO_ABORT_OBJECT_DNE
else:
odi = 0x0000
odsi = 0x00
scs = SDO_CS_ABORT
sdo_data = SDO_ABORT_GENERAL # Don't see one specifically for sending not enough data bytes in the CAN msg (malformed SDO msg)
sdo_data = sdo_data.to_bytes(4, byteorder='big')
n = 4 - len(sdo_data)
data = [(scs << SDO_SCS_BITNUM) + (n << SDO_N_BITNUM) +
(1 << SDO_E_BITNUM) + (1 << SDO_S_BITNUM),
(odi >> 8), (odi & 0xFF), (odsi)] + list(sdo_data)
msg = CAN.Message(sdo_server_id, data)
self._send(msg)
elif fc == FUNCTION_CODE_NMT_ERROR_CONTROL:
producer_id = id & 0x7F
if producer_id in self._heartbeat_consumer_timers:
self._heartbeat_consumer_timers.get(
producer_id).cancel()
heartbeat_consumer_time_object = self.od.get(
ODI_HEARTBEAT_CONSUMER_TIME)
if heartbeat_consumer_time_object is not None:
heartbeat_consumer_time = heartbeat_consumer_time_object.get(
ODSI_HEARTBEAT_CONSUMER_TIME, 0) / 1000
else:
heartbeat_consumer_time = 0
if heartbeat_consumer_time != 0:
heartbeat_consumer_timer = Timer(
heartbeat_consumer_time,
self._heartbeat_consumer_timeout, [producer_id])
heartbeat_consumer_timer.start()
self._heartbeat_consumer_timers.update(
{producer_id: heartbeat_consumer_timer})
GPIO.cleanup()
exit()
signal.signal(signal.SIGTERM, sigterm_handler)
GPIO.setmode(GPIO.BCM)
GPIO.setup(PIN_ENABLE_N, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(PIN_ADDRESS_N, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(PIN_ADDRESS_PARITY_N, GPIO.IN, pull_up_down=GPIO.PUD_UP)
runled0 = CANopen.RunIndicator(PIN_RUNLED0)
errled0 = CANopen.ErrorIndicator(PIN_ERRLED0)
runled1 = CANopen.Indicator(PIN_RUNLED1, CANopen.Indicator.OFF)
errled1 = CANopen.Indicator(PIN_ERRLED1, CANopen.Indicator.ON)
default_bus = CAN.Bus(DEFAULT_CAN_INTERFACE)
redundant_bus = CAN.Bus(REDUNDANT_CAN_INTERFACE)
active_bus = default_bus
class ResetNode(Exception):
pass
class ResetCommunication(Exception):
pass
while True:
try:
if GPIO.input(PIN_ENABLE_N) == GPIO.HIGH:
print("Enable_n is high")
sleep(1)
raise ResetNode
ms = ms / 1000
while True:
sio.sleep(ms)
raw_data = getBuffers(IMU)
sio.emit('sendIMU_buffer', raw_data, namespace='/sensors')
raw_data = getBuffers(Razor_IMU)
sio.emit('sendRazorIMU_buffer', raw_data, namespace='/sensors')
raw_data = getBuffer(LoadCell)
sio.emit('sendLC_buffer', raw_data, namespace='/sensors')
raw_data = getBuffer(CAN)
sio.emit('sendCAN_buffer', raw_data, namespace='/sensors')
if __name__ == '__main__':
''' Sensors initialization '''
IMU.IMU_init()
Razor_IMU.init()
CAN.init()
LoadCell.init()
#start the background thread
thread = sio.start_background_task(startStream, ms=100)
print("starting Local server")
eventlet.wsgi.server(eventlet.listen(('', 5500)), app)
print("Something went wrong, local server is no longer running!")
def do_GET(self):
parsed_path = urlparse(self.path)
try:
# Command
if parsed_path.query != '':
bus, msg = parse_request(parse_qs(parsed_path.query))
bus.send(msg)
self.send_response(204);
self.send_header('Access-Control-Allow-Origin', '*')
self.end_headers();
return
# Telemetry
if parsed_path.path == '/':
self.send_response(200)
self.send_header('Content-type', 'text/event-stream')
self.send_header('Cache-control', 'no-cache')
self.send_header('Access-Control-Allow-Origin', '*')
self.end_headers()
busses = []
for interface in CAN_LISTEN_INTERFACES:
bus = CAN.Bus(interface)
busses.append(bus)
while True:
try:
rlist, _, _ = select(busses, [], [])
for bus in rlist:
msg = bus.recv()
id = msg.arbitration_id
data = msg.data
data = ",".join(map(str, data))
self.wfile.write(bytes('data: {"bus":"' + bus.name + '","id":' + str(id) + ',"data":[' + data + '],"ts":"' + datetime.fromtimestamp(msg.timestamp).isoformat() + '"}' + "\n\n", 'utf8'))
except CAN.BusDown:
self.wfile.write(bytes('event: error' + "\n" + 'data: ' + bus.name + ' is down.' + "\n\n", 'utf-8'))
sleep(1)
continue
except SystemExit:
# This doesn't get called. Find a way if possible
self.wfile.write(bytes('event: error' + "\n" + 'data: System is shutting down.' + "\n\n", 'utf-8'))
raise
# File
filepath = WWW_DIR + self.path
if path.isfile(filepath):
f = open(filepath)
self.send_response(200)
if self.path.endswith(".html"):
self.send_header('Content-type', 'text/html')
elif self.path.endswith(".js"):
self.send_header('Content-type', 'text/javascript')
elif self.path.endswith(".css"):
self.send_header('Content-type', 'text/css')
self.end_headers()
self.wfile.write(bytes(f.read(), 'UTF-8'))
f.close()
return
self.send_response(404)
self.end_headers()
except BadRequest as e:
self.send_response(400)
self.send_error(400, 'Bad Request: %s' % str(e.args))
except BrokenPipeError:
print('Connection closed.')
except IOError:
self.send_response(500)
print("\n*** do_GET except ***")
print("Unexpected error:", sys.exc_info()[0])
traceback.print_exc()