From b545300587ac429b3d0b514dc72a736082736d98 Mon Sep 17 00:00:00 2001 From: Luke Dempsey Date: Fri, 2 Aug 2024 15:02:52 +1000 Subject: [PATCH 1/2] Update interface, adding parameter fetching, firmware updates etc --- .gitignore | 1 + docs/source/conf.py | 1 + pyvesc/.DS_Store | Bin 0 -> 6148 bytes pyvesc/VESC.py | 366 ++++++++ pyvesc/VESC/VESC.py | 161 ---- pyvesc/VESC/__init__.py | 1 - pyvesc/VESC/messages/__init__.py | 3 - pyvesc/VESC/messages/setters.py | 103 --- pyvesc/__init__.py | 3 - pyvesc/examples/get_values.py | 12 +- pyvesc/examples/motor_example.py | 126 ++- pyvesc/examples/simple.py | 1 - pyvesc/firmware.py | 133 +++ .../messages/Vedder_BLDC_Commands.py | 0 pyvesc/messages/__init__.py | 0 pyvesc/{VESC => }/messages/getters.py | 64 +- pyvesc/messages/setters.py | 206 +++++ pyvesc/params/__init__.py | 0 pyvesc/params/confgenerator.py | 809 ++++++++++++++++++ pyvesc/params/config_param.py | 249 ++++++ pyvesc/params/config_params.py | 3 + pyvesc/params/datatypes.py | 14 + pyvesc/params/test.py | 79 ++ pyvesc/protocol/__init__.py | 3 - pyvesc/protocol/base.py | 236 +++-- pyvesc/protocol/interface.py | 75 +- pyvesc/protocol/packet/__init__.py | 3 - pyvesc/protocol/packet/codec.py | 25 +- pyvesc/protocol/packet/exceptions.py | 2 +- pyvesc/protocol/packet/structure.py | 8 +- setup.py | 22 +- test.py | 51 +- unit_test_report.xml | 1 + 33 files changed, 2326 insertions(+), 435 deletions(-) create mode 100644 pyvesc/.DS_Store create mode 100644 pyvesc/VESC.py delete mode 100644 pyvesc/VESC/VESC.py delete mode 100644 pyvesc/VESC/__init__.py delete mode 100644 pyvesc/VESC/messages/__init__.py delete mode 100644 pyvesc/VESC/messages/setters.py create mode 100644 pyvesc/firmware.py rename pyvesc/{VESC => }/messages/Vedder_BLDC_Commands.py (100%) create mode 100644 pyvesc/messages/__init__.py rename pyvesc/{VESC => }/messages/getters.py (61%) create mode 100644 pyvesc/messages/setters.py create mode 100644 pyvesc/params/__init__.py create mode 100644 pyvesc/params/confgenerator.py create mode 100644 pyvesc/params/config_param.py create mode 100644 pyvesc/params/config_params.py create mode 100644 pyvesc/params/datatypes.py create mode 100644 pyvesc/params/test.py create mode 100644 unit_test_report.xml diff --git a/.gitignore b/.gitignore index 06415a6..3c32131 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ docs/build/ MANIFEST dist/ pyvesc.egg-info/ +build/ diff --git a/docs/source/conf.py b/docs/source/conf.py index af51246..75426a3 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +# flake8: noqa # -*- coding: utf-8 -*- # # PyVESC documentation build configuration file, created by diff --git a/pyvesc/.DS_Store b/pyvesc/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..64067494c6c230fdc3f733bce4d648d2445dcafe GIT binary patch literal 6148 zcmeHKy-veG47Njsf>=5-p_|zvULaIqMM!x8C`DyxlmKEvLU|^50VW=TM`7ag*`!U= zN=yhMRAXac=9yMh{i;eLIslp3>!rJq610HB2JETT+^Ix>2Wd7%0aXn z{v!jrxs~i1ek=QTQ&)`^eDVoQX)UIzi(0Kr4NxIzPXhuKQQ(xj%mUsi!wbRWEDV7SpSP-=z*0XTTY726l}B^lXvzP|-(c zz!`7`ItJwX5TJs&VXGKF9T?&f062m<3a;fWAvwV?H*6K*fv~0mHI?m%!I}Frqh|M~Xwe?Q6ZoB?NGrx@T-Q5F+C zlJ(ZX!%43V&?~5j#I=fD3NB(RMy#~rBWM)ZgIoY}!&VU%i2o6YH2B~Q{3!$9#YRsz literal 0 HcmV?d00001 diff --git a/pyvesc/VESC.py b/pyvesc/VESC.py new file mode 100644 index 0000000..372f8b2 --- /dev/null +++ b/pyvesc/VESC.py @@ -0,0 +1,366 @@ +from .protocol.interface import encode_request, encode, decode +from .messages.getters import GetVersion, GetMotorConfig, GetAppConfig, GetValues +from .messages.setters import ( + SetMotorConfig, SetAppConfig, SetRPM, SetCurrent, SetDutyCycle, + SetServoPosition, EraseNewApp, WriteNewAppData, WriteNewAppDataLZO, + JumpToBootloader, TerminalCmd, SetRotorPositionMode, Reboot, alive_msg +) +import time +import threading +import logging + + +logger = logging.getLogger(__name__) + +# because people may want to use this library for their own messaging, do not make this a required package +try: + import serial +except ImportError: + serial = None + +read_lock = threading.Lock() + + +class VESC(object): + def __init__(self, serial_port, has_sensor=False, start_heartbeat=True, baudrate=115200, timeout=0.05): + """ + :param serial_port: Serial device to use for communication (i.e. "COM3" or "/dev/tty.usbmodem0") + :param has_sensor: Whether or not the bldc motor is using a hall effect sensor + :param start_heartbeat: Whether or not to automatically start the heartbeat thread that will keep commands + alive. + :param baudrate: baudrate for the serial communication. Shouldn't need to change this. + :param timeout: timeout for the serial communication + """ + + if serial is None: + raise ImportError("Need to install pyserial in order to use the VESCMotor class.") + + self.serial_port = serial.Serial(port=serial_port, baudrate=baudrate, timeout=timeout) + if has_sensor: + self.serial_port.write(encode(SetRotorPositionMode(SetRotorPositionMode.DISP_POS_OFF))) + + self.heart_beat_thread = threading.Thread(target=self._heartbeat_cmd_func) + self._stop_heartbeat = threading.Event() + + if start_heartbeat: + self.start_heartbeat() + + self._message_monitor_thread = threading.Thread(target=self._message_monitor) + + # thread to monitor messages to receive unscheuled prints from ESC for debugging, + # currently disabled as it was interfering sometimes and is not needed + self._stop_message_monitor = threading.Event() + # self._message_monitor_thread.start() + + # store message info for getting values so it doesn't need to calculate it every time + msg = GetValues() + self._get_values_msg = encode_request(msg) + self._get_values_msg_expected_length = msg._recv_full_msg_size + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.stop_heartbeat() + try: + if self.serial_port.is_open: + self.serial_port.flush() + self.serial_port.close() + except Exception as e: + logging.error("Error closing serial port: ", e) + logging.error("This is likely due to the motor being disconnected before the connection could be closed.") + + def _message_monitor(self): + """ + A function that continuously reads the serial port for messages and decodes them. + """ + while not self._stop_message_monitor.isSet(): + ret = self.read(1, expect_string=True, expect_anything=False) + if ret is not None: + print(ret) + time.sleep(0.01) + + def _heartbeat_cmd_func(self): + """ + Continuous function calling that keeps the motor alive + """ + while not self._stop_heartbeat.isSet(): + time.sleep(0.1) + self.write(alive_msg, is_heartbeat=True) + + def start_heartbeat(self): + """ + Starts a repetitive calling of the last set cmd to keep the motor alive. + """ + self.heart_beat_thread.start() + + def stop_heartbeat(self): + """ + Stops the heartbeat thread and resets the last cmd function. THIS MUST BE CALLED BEFORE THE OBJECT GOES OUT OF + SCOPE UNLESS WRAPPING IN A WITH STATEMENT (Assuming the heartbeat was started). + """ + if self.heart_beat_thread.is_alive(): + try: + self._stop_heartbeat.set() + self.heart_beat_thread.join() + except Exception as e: + logger.error("Error stopping heartbeat: {}".format(e)) + + def write(self, data, num_read_bytes=None, is_heartbeat=False, expect_string=False): + """ + A write wrapper function implemented like this to try and make it easier to incorporate other communication + methods than UART in the future. + :param data: the byte string to be sent + :param num_read_bytes: number of bytes to read for decoding response + :param is_heartbeat: whether or not this is a heartbeat message, can be used for filtering debug prints + :param expect_string: whether or not to expect a string response + :param expect_anything: whether or not to expect any response + :return: decoded response from buffer + """ + try: + self.serial_port.write(data) + except Exception as e: + logging.error("Error writing to serial port: ", e) + if not is_heartbeat: + logging.debug("Data sent: {}".format(data)) + if num_read_bytes is not None: + return self.read(num_read_bytes, expect_string=expect_string) + + def read(self, num_read_bytes, timeout=0.01, expect_string=False, expect_anything=True): + response = None + payload = b'' + last_payload_len = 0 + t_start = None + + # allow more time for parsing string of unknown length + if expect_string: + timeout = 0.1 + + with read_lock: + while True: + time.sleep(0.01) + + # first data received + if len(payload) > 0 and last_payload_len == 0 or len(payload) > 0 and t_start is None: + t_start = time.time() + + # if data stops for a long time + if last_payload_len == len(payload): + if t_start is not None: + if time.time() - t_start > timeout: + break + + # read in data gradually to ensure buffer doesn't fill + payload += self.serial_port.read(self.serial_port.in_waiting) + + last_payload_len = len(payload) + + # if we are just probing the serial line, exit early as we are not waiting for a response + if len(payload) == 0 and not expect_anything: + return None + + response, consumed, msg_payload = decode(payload, recv=True) + logging.debug("Data response: {}".format(msg_payload)) + return response + + def update_firmware(self, firmware, progress_callback=None): + + logging.info("Erasing") + + erase_res = self.fw_erase_new_app(firmware.size) + if erase_res.erase_new_app_result != 1: + logging.error("Erase failed") + progress_callback("Erase Failed") + return False + + logging.info("Sending firmware") + + offset = 0 + time_since_last_progress_update = time.time() + + while firmware.size > 0: + fw_chunk = firmware.get_next_chunk() + + # check if the chunk is empty, don't send + has_data = False + for i in fw_chunk: + if i != 0xff: + has_data = True + break + + if has_data: + fw_result = self.fw_write_new_app_data(offset, fw_chunk) + + if fw_result.write_new_app_result != 1 or fw_result.write_new_app_result is None: + logging.error("Write failed") + logging.error(fw_result) + progress_callback("Flashing Failed") + return False + + offset += firmware.chunk_size + + UPDATE_INTERVAL_SECS = 10 + if time.time() - time_since_last_progress_update > UPDATE_INTERVAL_SECS: + time_since_last_progress_update = time.time() + logging.info( + "Progress: {:.2f}%, Size: {}/{}kB".format(firmware.get_progress(offset), offset, firmware.original_size)) + if progress_callback is not None: + progress_callback(int(firmware.get_progress(offset))) + + # stream updates quickly to stdout + print("\rProgress: {:.2f}%, Size: {}kB, to be written to {}".format( + firmware.get_progress(offset), offset, offset + firmware.chunk_size), end='\r') + firmware.clear_chunk() + + logging.info("Firmware upload complete, jumping to bootloader.") + try: + self.fw_jump_to_bootloader() + except Exception as e: + logging.error( + "Error jumping to bootloader, this is likely the motor rebooting before a connection could be closed: {}".format(e)) + + return True + + def set_rpm(self, new_rpm): + """ + Set the electronic RPM value (a.k.a. the RPM value of the stator) + :param new_rpm: new rpm value + """ + self.write(encode(SetRPM(new_rpm))) + + def set_current(self, new_current): + """ + :param new_current: new current in milli-amps for the motor + """ + self.write(encode(SetCurrent(new_current))) + + def set_duty_cycle(self, new_duty_cycle): + """ + :param new_duty_cycle: Value of duty cycle to be set (range [-1e5, 1e5]). + """ + self.write(encode(SetDutyCycle(new_duty_cycle))) + + def set_servo(self, new_servo_pos): + """ + :param new_servo_pos: New servo position. valid range [0, 1] + """ + self.write(encode(SetServoPosition(new_servo_pos))) + + def get_measurements(self): + """ + :return: A msg object with attributes containing the measurement values + """ + return self.write(self._get_values_msg, num_read_bytes=self._get_values_msg_expected_length) + + def get_firmware_version(self): + msg = GetVersion() + return self.write(encode_request(msg), num_read_bytes=msg._recv_full_msg_size) + + def get_rpm(self): + """ + :return: Current motor rpm + """ + return self.get_measurements().rpm + + def get_duty_cycle(self): + """ + :return: Current applied duty-cycle + """ + return self.get_measurements().duty_now + + def get_v_in(self): + """ + :return: Current input voltage + """ + return self.get_measurements().v_in + + def get_motor_current(self): + """ + :return: Current motor current + """ + return self.get_measurements().current_motor + + def get_incoming_current(self): + """ + :return: Current incoming current + """ + return self.get_measurements().current_in + + def fw_erase_new_app(self, fw_size): + """ + Erase app data + """ + # TODO: Revert this to actual fw size + msg = EraseNewApp(fw_size) + return self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size) + + def reboot(self): + """ + Reboot VESC + """ + msg = Reboot() + return str(self.write(encode_request(msg), num_read_bytes=msg._recv_full_msg_size)) + + def fw_write_new_app_data(self, offset, data): + """ + Write new app data + """ + msg = WriteNewAppData(offset, data) + return self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size) + + def fw_write_new_app_data_lzo(self, offset, data): + """ + Write new app data + """ + msg = WriteNewAppDataLZO(offset, data) + return self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size) + + def fw_jump_to_bootloader(self): + """ + Jump to bootloader + set number of read bytes to None as we don't expect a response + """ + msg = JumpToBootloader() + # stop heartbeat, as we are about to reset the device + self.stop_heartbeat() + + return self.write(encode_request(msg), num_read_bytes=None) + + def send_terminal_cmd(self, cmd): + """ + Send terminal command + """ + msg = TerminalCmd(cmd) + return self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size, expect_string=True) + + def get_motor_configuration(self): + """ + Get the motor configuration parameters + """ + msg = GetMotorConfig() + res = self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size, expect_string=True) + return res + + def set_motor_configuration(self, data): + """ + Set the motor configuration parameters + """ + msg = SetMotorConfig(data) + res = self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size, expect_string=True) + return res + + def get_app_configuration(self): + """ + Get the app configuration parameters + """ + msg = GetAppConfig() + res = self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size, expect_string=True) + return res + + def set_app_configuration(self, data): + """ + Set the app configuration parameters + """ + msg = SetAppConfig(data) + res = self.write(encode(msg), num_read_bytes=msg._recv_full_msg_size, expect_string=True) + return res diff --git a/pyvesc/VESC/VESC.py b/pyvesc/VESC/VESC.py deleted file mode 100644 index 27d5453..0000000 --- a/pyvesc/VESC/VESC.py +++ /dev/null @@ -1,161 +0,0 @@ -from pyvesc.protocol.interface import encode_request, encode, decode -from pyvesc.VESC.messages import * -import time -import threading - -# because people may want to use this library for their own messaging, do not make this a required package -try: - import serial -except ImportError: - serial = None - - -class VESC(object): - def __init__(self, serial_port, has_sensor=False, start_heartbeat=True, baudrate=115200, timeout=0.05): - """ - :param serial_port: Serial device to use for communication (i.e. "COM3" or "/dev/tty.usbmodem0") - :param has_sensor: Whether or not the bldc motor is using a hall effect sensor - :param start_heartbeat: Whether or not to automatically start the heartbeat thread that will keep commands - alive. - :param baudrate: baudrate for the serial communication. Shouldn't need to change this. - :param timeout: timeout for the serial communication - """ - - if serial is None: - raise ImportError("Need to install pyserial in order to use the VESCMotor class.") - - self.serial_port = serial.Serial(port=serial_port, baudrate=baudrate, timeout=timeout) - if has_sensor: - self.serial_port.write(encode(SetRotorPositionMode(SetRotorPositionMode.DISP_POS_OFF))) - - self.heart_beat_thread = threading.Thread(target=self._heartbeat_cmd_func) - self._stop_heartbeat = threading.Event() - - if start_heartbeat: - self.start_heartbeat() - - # check firmware version and set GetValue fields to old values if pre version 3.xx - version = self.get_firmware_version() - if int(version.split('.')[0]) < 3: - GetValues.fields = pre_v3_33_fields - - # store message info for getting values so it doesn't need to calculate it every time - msg = GetValues() - self._get_values_msg = encode_request(msg) - self._get_values_msg_expected_length = msg._full_msg_size - - def __enter__(self): - return self - - def __exit__(self, exc_type, exc_val, exc_tb): - self.stop_heartbeat() - if self.serial_port.is_open: - self.serial_port.flush() - self.serial_port.close() - - def _heartbeat_cmd_func(self): - """ - Continuous function calling that keeps the motor alive - """ - while not self._stop_heartbeat.isSet(): - time.sleep(0.1) - self.write(alive_msg) - - def start_heartbeat(self): - """ - Starts a repetitive calling of the last set cmd to keep the motor alive. - """ - self.heart_beat_thread.start() - - def stop_heartbeat(self): - """ - Stops the heartbeat thread and resets the last cmd function. THIS MUST BE CALLED BEFORE THE OBJECT GOES OUT OF - SCOPE UNLESS WRAPPING IN A WITH STATEMENT (Assuming the heartbeat was started). - """ - if self.heart_beat_thread.is_alive(): - self._stop_heartbeat.set() - self.heart_beat_thread.join() - - def write(self, data, num_read_bytes=None): - """ - A write wrapper function implemented like this to try and make it easier to incorporate other communication - methods than UART in the future. - :param data: the byte string to be sent - :param num_read_bytes: number of bytes to read for decoding response - :return: decoded response from buffer - """ - self.serial_port.write(data) - if num_read_bytes is not None: - while self.serial_port.in_waiting <= num_read_bytes: - time.sleep(0.000001) # add some delay just to help the CPU - response, consumed = decode(self.serial_port.read(self.serial_port.in_waiting)) - return response - - def set_rpm(self, new_rpm): - """ - Set the electronic RPM value (a.k.a. the RPM value of the stator) - :param new_rpm: new rpm value - """ - self.write(encode(SetRPM(new_rpm))) - - def set_current(self, new_current): - """ - :param new_current: new current in milli-amps for the motor - """ - self.write(encode(SetCurrent(new_current))) - - def set_duty_cycle(self, new_duty_cycle): - """ - :param new_duty_cycle: Value of duty cycle to be set (range [-1e5, 1e5]). - """ - self.write(encode(SetDutyCycle(new_duty_cycle))) - - def set_servo(self, new_servo_pos): - """ - :param new_servo_pos: New servo position. valid range [0, 1] - """ - self.write(encode(SetServoPosition(new_servo_pos))) - - def get_measurements(self): - """ - :return: A msg object with attributes containing the measurement values - """ - return self.write(self._get_values_msg, num_read_bytes=self._get_values_msg_expected_length) - - def get_firmware_version(self): - msg = GetVersion() - return str(self.write(encode_request(msg), num_read_bytes=msg._full_msg_size)) - - def get_rpm(self): - """ - :return: Current motor rpm - """ - return self.get_measurements().rpm - - def get_duty_cycle(self): - """ - :return: Current applied duty-cycle - """ - return self.get_measurements().duty_now - - def get_v_in(self): - """ - :return: Current input voltage - """ - return self.get_measurements().v_in - - def get_motor_current(self): - """ - :return: Current motor current - """ - return self.get_measurements().current_motor - - def get_incoming_current(self): - """ - :return: Current incoming current - """ - return self.get_measurements().current_in - - - - diff --git a/pyvesc/VESC/__init__.py b/pyvesc/VESC/__init__.py deleted file mode 100644 index 5a469fc..0000000 --- a/pyvesc/VESC/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .VESC import VESC \ No newline at end of file diff --git a/pyvesc/VESC/messages/__init__.py b/pyvesc/VESC/messages/__init__.py deleted file mode 100644 index ea73c2e..0000000 --- a/pyvesc/VESC/messages/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .Vedder_BLDC_Commands import VedderCmd -from .getters import * -from .setters import * diff --git a/pyvesc/VESC/messages/setters.py b/pyvesc/VESC/messages/setters.py deleted file mode 100644 index 42e8ae6..0000000 --- a/pyvesc/VESC/messages/setters.py +++ /dev/null @@ -1,103 +0,0 @@ -from pyvesc.protocol.base import VESCMessage -from pyvesc.protocol.interface import encode -from pyvesc.VESC.messages import VedderCmd - - -class SetDutyCycle(metaclass=VESCMessage): - """ Set the duty cycle. - - :ivar duty_cycle: Value of duty cycle to be set (range [-1e5, 1e5]). - """ - id = VedderCmd.COMM_SET_DUTY - fields = [ - ('duty_cycle', 'i', 100000) - ] - - -class SetRPM(metaclass=VESCMessage): - """ Set the RPM. - - :ivar rpm: Value to set the RPM to. - """ - id = VedderCmd.COMM_SET_RPM - fields = [ - ('rpm', 'i') - ] - - -class SetCurrent(metaclass=VESCMessage): - """ Set the current (in milliamps) to the motor. - - :ivar current: Value to set the current to (in milliamps). - """ - id = VedderCmd.COMM_SET_CURRENT - fields = [ - ('current', 'i', 1000) - ] - - -class SetCurrentBrake(metaclass=VESCMessage): - """ Set the current brake (in milliamps). - - :ivar current_brake: Value to set the current brake to (in milliamps). - """ - id = VedderCmd.COMM_SET_CURRENT_BRAKE - fields = [ - ('current_brake', 'i', 1000) - ] - - -class SetPosition(metaclass=VESCMessage): - """Set the rotor angle based off of an encoder or sensor - - :ivar pos: Value to set the current position or angle to. - """ - id = VedderCmd.COMM_SET_POS - fields = [ - ('pos', 'i', 1000000) - ] - - -class SetRotorPositionMode(metaclass=VESCMessage): - """Sets the rotor position feedback mode. - - It is reccomended to use the defined modes as below: - * DISP_POS_OFF - * DISP_POS_MODE_ENCODER - * DISP_POS_MODE_PID_POS - * DISP_POS_MODE_PID_POS_ERROR - - :ivar pos_mode: Value of the mode - """ - - DISP_POS_OFF = 0 - DISP_POS_MODE_ENCODER = 3 - DISP_POS_MODE_PID_POS = 4 - DISP_POS_MODE_PID_POS_ERROR = 5 - - id = VedderCmd.COMM_SET_DETECT - fields = [ - ('pos_mode', 'b') - ] - - -class SetServoPosition(metaclass=VESCMessage): - """Sets the position of s servo connected to the VESC. - - :ivar servo_pos: Value of position (range [0, 1]) - """ - - id = VedderCmd.COMM_SET_SERVO_POS - fields = [ - ('servo_pos', 'h', 1000) - ] - - -class Alive(metaclass=VESCMessage): - """Heartbeat signal to keep VESC alive""" - id = VedderCmd.COMM_ALIVE - fields = [] - - -# statically save this message because it does not need to be recalculated -alive_msg = encode(Alive()) diff --git a/pyvesc/__init__.py b/pyvesc/__init__.py index 1a83924..a8733e7 100644 --- a/pyvesc/__init__.py +++ b/pyvesc/__init__.py @@ -1,6 +1,3 @@ import sys if sys.version_info < (3, 3): raise SystemExit("Invalid Python version. PyVESC requires Python 3.3 or greater.") - -from pyvesc.protocol import * -from pyvesc.VESC import * diff --git a/pyvesc/examples/get_values.py b/pyvesc/examples/get_values.py index a9e8f44..6d6f847 100644 --- a/pyvesc/examples/get_values.py +++ b/pyvesc/examples/get_values.py @@ -27,17 +27,7 @@ def get_values_example(): (response, consumed) = pyvesc.decode(ser.read(61)) # Print out the values - try: - print(response.rpm) - - except: - # ToDo: Figure out how to isolate rotor position and other sensor data - # in the incoming datastream - #try: - # print(response.rotor_pos) - #except: - # pass - pass + print(response.rpm) time.sleep(0.1) diff --git a/pyvesc/examples/motor_example.py b/pyvesc/examples/motor_example.py index 70539da..56a9a01 100644 --- a/pyvesc/examples/motor_example.py +++ b/pyvesc/examples/motor_example.py @@ -1,12 +1,19 @@ -from pyvesc import VESC +from pyvesc.firmware import Firmware +from pyvesc.VESC import VESC +from pyvesc.params import confgenerator import time +import logging +import argparse +import json -# serial port that VESC is connected to. Something like "COM3" for windows and as below for linux/mac -serial_port = '/dev/serial/by-id/usb-STMicroelectronics_ChibiOS_RT_Virtual_COM_Port_301-if00' +logger = logging.getLogger(__name__) +logging.basicConfig(format='%(asctime)s - %(message)s', datefmt='%d-%m-%y %H:%M:%S', level=logging.INFO) +console = logging.StreamHandler() +logger.addHandler(console) # a function to show how to use the class with a with-statement -def run_motor_using_with(): +def run_motor_using_with(serial_port): with VESC(serial_port=serial_port) as motor: print("Firmware: ", motor.get_firmware_version()) motor.set_duty_cycle(.02) @@ -19,30 +26,127 @@ def run_motor_using_with(): # a function to show how to use the class as a static object. -def run_motor_as_object(): +def run_motor_as_object(serial_port): motor = VESC(serial_port=serial_port) print("Firmware: ", motor.get_firmware_version()) # sweep servo through full range for i in range(100): time.sleep(0.01) - motor.set_servo(i/100) + motor.set_servo(i / 100) # IMPORTANT: YOU MUST STOP THE HEARTBEAT IF IT IS RUNNING BEFORE IT GOES OUT OF SCOPE. Otherwise, it will not # clean-up properly. motor.stop_heartbeat() -def time_get_values(): +def time_get_values(serial_port): with VESC(serial_port=serial_port) as motor: start = time.time() motor.get_measurements() stop = time.time() - print("Getting values takes ", stop-start, "seconds.") + print("Getting values takes ", stop - start, "seconds.") + + +def commands_example(port, firmware, compressed): + """ + Example of using the terminal commands with some additional commands defined here which allow erase and firmware updates + """ + + with VESC(serial_port=port) as motor: + print(motor.send_terminal_cmd("hw_status")) + + print("\nAll VESC commands are supported, plus the following:") + print("fw\t\t- update firmware") + print("erase\t\t- erase firmware ") + print("\n\nEntering Terminal:\n====================\n") + print("\n\n") + + while True: + # terminal console that reads in text on a newline, assigns it to the user_in string + user_in = input("") + if user_in == "fw": + if firmware is None: + print("No firmware file specified") + continue + fw = Firmware(firmware, lzss=compressed) + motor.update_firmware(fw) + logging.info("This script will exit as it doesn't handle serial ports reconnecting yet.") + break + + if user_in == "erase": + print("sending erase") + erase_res = motor.fw_erase_new_app(fw.size) + print("Erase status:", erase_res.erase_new_app_result) + break + + print(motor.send_terminal_cmd(user_in)) + + +def save_parameters(serial_port, save_file): + with VESC(serial_port=serial_port) as motor: + motor_config_bytes = motor.get_motor_configuration() + app_config_bytes = motor.get_app_configuration() + mcconfig = confgenerator.confgenerator_deserialise_mcconf(bytearray(motor_config_bytes), None) + appconfig = confgenerator.confgenerator_deserialise_appconf(bytearray(app_config_bytes), None) + + # convert to dictionary + # TODO: add datatype to the dictionary + mcconfig_dict = [{param.name: param.value} for param in mcconfig] + appconfig_dict = [{param.name: param.value} for param in appconfig] + + output = {"mcconfig": mcconfig_dict, "appconfig": appconfig_dict} + + # convert to json + config_json = json.dumps(output, indent=4) + print(config_json) + + # save the parameters to a file + with open(save_file, 'wb') as f: + f.write(config_json.encode('utf-8')) + + +def load_parameters(serial_port, load_file): + with VESC(serial_port=serial_port) as motor: + with open(load_file, 'rb') as f: + config_json = f.read() + + config = json.loads(config_json) + + mcconfig = [] + for param in config['mcconfig']: + name, value = list(param.items())[0] + mcconfig.append(confgenerator.confgenerator_param_from_dict(name, value)) + + appconfig = [] + for param in config['appconfig']: + name, value = list(param.items())[0] + appconfig.append(confgenerator.confgenerator_param_from_dict(name, value)) + + mcconfig_bytes = confgenerator.confgenerator_serialise_mcconf(mcconfig) + appconfig_bytes = confgenerator.confgenerator_serialise_appconf(appconfig) + + motor.set_motor_configuration(mcconfig_bytes) + motor.set_app_configuration(appconfig_bytes) if __name__ == '__main__': - run_motor_using_with() - run_motor_as_object() - time_get_values() + # arguments + parser = argparse.ArgumentParser(description='VESC Motor Example') + parser.add_argument('-p', '--port', type=str, help='Serial port', default='') + parser.add_argument('-f', '--firmware', type=str, help='Firmware file', default=None) + parser.add_argument('-c', '--compressed', action='store_true', help='Compressed firmware', default=True) + parser.add_argument('-s', '--save', type=str, help='Save parameters to file', default=None) + parser.add_argument('-l', '--load', type=str, help='Load parameters from file', default=None) + args = parser.parse_args() + + if args.save: + save_parameters(serial_port=args.port, save_file=args.save) + + if args.load: + load_parameters(serial_port=args.port, load_file=args.load) + # run_motor_using_with(args.port) + # run_motor_as_object(args.port) + # time_get_values(args.port) + commands_example(args.port, args.firmware, args.compressed) diff --git a/pyvesc/examples/simple.py b/pyvesc/examples/simple.py index e04888c..dcaa365 100644 --- a/pyvesc/examples/simple.py +++ b/pyvesc/examples/simple.py @@ -22,6 +22,5 @@ def simple_example(): print("Success!") - if __name__ == "__main__": simple_example() diff --git a/pyvesc/firmware.py b/pyvesc/firmware.py new file mode 100644 index 0000000..156d2c0 --- /dev/null +++ b/pyvesc/firmware.py @@ -0,0 +1,133 @@ +import logging + +logger = logging.getLogger(__name__) + +# CRC Table +crc16_tab = [0x0000, 0x1021, 0x2042, 0x3063, 0x4084, + 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, + 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, + 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, + 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, + 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, + 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, + 0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, + 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, + 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, + 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, + 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, + 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, + 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, + 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, + 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, + 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, + 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, + 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, + 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, + 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, + 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, + 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, + 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, + 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0] + + +def crc16(data): + cksum = 0 + for i in range(len(data)): + cksum = crc16_tab[((cksum >> 8) ^ data[i]) & 0xFF] ^ (cksum << 8) + cksum &= 0xFFFF # Keep as 16-bit + return cksum + + +class Firmware: + def __init__(self, filepath, lzss=False): + self.chunk_size = 384 + self.filepath = filepath + self.name = filepath.split('/')[-1] + self.version = filepath.split('-')[-1].split('.')[0] + self.fw_bytes = None + self.original_size = None + self.size = None + self.chunk = None + self.lzss = lzss + + self.load() + self.process() + + def load(self): + """ + Load the firmware file + + :param lzss: whether the firmware file is compressed with lzss, newer firmwares that are larger require lzss compression + """ + with open(self.filepath, 'rb') as f: + self.fw_bytes = f.read() + self.original_size = len(self.fw_bytes) + self.size = self.original_size + + def process(self): + """ + Process the firmware file, adds size and crc to a header for the vesc to read + """ + # newer firmwares are larger and require lzss compression + # lzss compression must be generated with heatshrink + # for now this is done externally, but could be implemented within this class + # sudo apt install heatshrink + # heatshrink -w 13 -l 5 + + # sense check to account for lack of automated compression + if self.lzss and self.size > 393208: + logger.warning(f'Firmware file seems large {0.001*self.size}KB, but you have said it is lzss compressed. Are you sure it is?') + + if self.lzss: + size_shift = (0xCC << 24) # tells vesc bootloader to decompress + size_shift |= self.size + size = size_shift.to_bytes(4, byteorder='big') + else: + size = self.size.to_bytes(4, byteorder='big') + + header = size + crc16(self.fw_bytes).to_bytes(2, byteorder='big') + self.fw_bytes = header + self.fw_bytes + + def __str__(self): + return f'{self.name} {self.version} {self.size*1e-3}kB, ({self.original_size}B)' + + def print_bytes(self): + """ + Print the firmware bytes + """ + print(self.fw_bytes) + + def get_next_chunk(self): + """ + :return: next chunk of firmware to be sent + """ + self.chunk = self.fw_bytes[:self.chunk_size] + + return self.chunk + + # pad chunk if smaller than send packet + if len(self.chunk) < self.chunk_size: + self.chunk += b'\xFF' * (self.chunk_size - len(self.chunk)) + + return self.chunk + + def clear_chunk(self): + """ + Clear the chunk that was sent, remove that chunk from the firmware bytes too so that the next chunk can be sent + """ + self.chunk = None + + # remove the chunk that was sent from the loaded firmware + self.fw_bytes = self.fw_bytes[self.chunk_size:] + self.size = len(self.fw_bytes) + + def get_progress(self, offset): + """ + :param offset: current offset + :return: progress in percentage + """ + return 100 * (offset / self.original_size) diff --git a/pyvesc/VESC/messages/Vedder_BLDC_Commands.py b/pyvesc/messages/Vedder_BLDC_Commands.py similarity index 100% rename from pyvesc/VESC/messages/Vedder_BLDC_Commands.py rename to pyvesc/messages/Vedder_BLDC_Commands.py diff --git a/pyvesc/messages/__init__.py b/pyvesc/messages/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyvesc/VESC/messages/getters.py b/pyvesc/messages/getters.py similarity index 61% rename from pyvesc/VESC/messages/getters.py rename to pyvesc/messages/getters.py index c596862..aa6aab1 100644 --- a/pyvesc/VESC/messages/getters.py +++ b/pyvesc/messages/getters.py @@ -1,5 +1,5 @@ -from pyvesc.protocol.base import VESCMessage -from pyvesc.VESC.messages import VedderCmd +from ..protocol.base import VESCMessage +from .Vedder_BLDC_Commands import VedderCmd pre_v3_33_fields = [('temp_mos1', 'h', 10), @@ -8,15 +8,15 @@ ('temp_mos4', 'h', 10), ('temp_mos5', 'h', 10), ('temp_mos6', 'h', 10), - ('temp_pcb', 'h', 10), + ('temp_pcb', 'h', 10), ('current_motor', 'i', 100), - ('current_in', 'i', 100), - ('duty_now', 'h', 1000), - ('rpm', 'i', 1), - ('v_in', 'h', 10), - ('amp_hours', 'i', 10000), + ('current_in', 'i', 100), + ('duty_now', 'h', 1000), + ('rpm', 'i', 1), + ('v_in', 'h', 10), + ('amp_hours', 'i', 10000), ('amp_hours_charged', 'i', 10000), - ('watt_hours', 'i', 10000), + ('watt_hours', 'i', 10000), ('watt_hours_charged', 'i', 10000), ('tachometer', 'i', 1), ('tachometer_abs', 'i', 1), @@ -28,22 +28,48 @@ class GetVersion(metaclass=VESCMessage): """ id = VedderCmd.COMM_FW_VERSION - fields = [ - ('comm_fw_version', 'b', 0), - ('fw_version_major', 'b', 0), - ('fw_version_minor', 'b', 0) + recv_fields = [ + ('comm_fw_version', 'b', 0), + ('fw_version_major', 'b', 0), + ('fw_version_minor', 'b', 0) ] def __str__(self): return f"{self.comm_fw_version}.{self.fw_version_major}.{self.fw_version_minor}" +class GetMotorConfig(metaclass=VESCMessage): + """ + Get the motor configuration values + + Receives a bytestring, so scalar is set to -1 to represent this + """ + id = VedderCmd.COMM_GET_MCCONF + + recv_fields = [ + ('mcconf', 's', -1) + ] + + +class GetAppConfig(metaclass=VESCMessage): + """ + Get the app configuration values + + Receives a bytestring, so scalar is set to -1 to represent this + """ + id = VedderCmd.COMM_GET_APPCONF + + recv_fields = [ + ('appconf', 's', -1) + ] + + class GetValues(metaclass=VESCMessage): """ Gets internal sensor data """ id = VedderCmd.COMM_GET_VALUES - fields = [ + recv_fields = [ ('temp_fet', 'h', 10), ('temp_motor', 'h', 10), ('avg_motor_current', 'i', 100), @@ -68,12 +94,12 @@ class GetValues(metaclass=VESCMessage): class GetRotorPosition(metaclass=VESCMessage): """ Gets rotor position data - - Must be set to DISP_POS_MODE_ENCODER or DISP_POS_MODE_PID_POS (Mode 3 or + + Must be set to DISP_POS_MODE_ENCODER or DISP_POS_MODE_PID_POS (Mode 3 or Mode 4). This is set by SetRotorPositionMode (id=21). """ id = VedderCmd.COMM_ROTOR_POSITION - fields = [ - ('rotor_pos', 'i', 100000) - ] \ No newline at end of file + recv_fields = [ + ('rotor_pos', 'i', 100000) + ] diff --git a/pyvesc/messages/setters.py b/pyvesc/messages/setters.py new file mode 100644 index 0000000..0e8e9b8 --- /dev/null +++ b/pyvesc/messages/setters.py @@ -0,0 +1,206 @@ +from ..protocol.base import VESCMessage +from ..protocol.interface import encode +from .Vedder_BLDC_Commands import VedderCmd + + +class SetDutyCycle(metaclass=VESCMessage): + """ Set the duty cycle. + + :ivar duty_cycle: Value of duty cycle to be set (range [-1e5, 1e5]). + """ + id = VedderCmd.COMM_SET_DUTY + send_fields = [ + ('duty_cycle', 'i', 100000) + ] + + +class SetMotorConfig(metaclass=VESCMessage): + """ + Set the motor configuration values + + Sends a bytestring, so scalar is set to -1 to represent this + """ + id = VedderCmd.COMM_SET_MCCONF + + send_fields = [ + ('mcconf', 's', -1) + ] + + +class SetAppConfig(metaclass=VESCMessage): + """ + Set the app configuration values + + Sends a bytestring, so scalar is set to -1 to represent this + """ + id = VedderCmd.COMM_SET_APPCONF + + send_fields = [ + ('appconf', 's', -1) + ] + + +class SetRPM(metaclass=VESCMessage): + """ Set the RPM. + + :ivar rpm: Value to set the RPM to. + """ + id = VedderCmd.COMM_SET_RPM + send_fields = [ + ('rpm', 'i') + ] + + +class SetCurrent(metaclass=VESCMessage): + """ Set the current (in milliamps) to the motor. + + :ivar current: Value to set the current to (in milliamps). + """ + id = VedderCmd.COMM_SET_CURRENT + send_fields = [ + ('current', 'i', 1000) + ] + + +class SetCurrentBrake(metaclass=VESCMessage): + """ Set the current brake (in milliamps). + + :ivar current_brake: Value to set the current brake to (in milliamps). + """ + id = VedderCmd.COMM_SET_CURRENT_BRAKE + send_fields = [ + ('current_brake', 'i', 1000) + ] + + +class SetPosition(metaclass=VESCMessage): + """Set the rotor angle based off of an encoder or sensor + + :ivar pos: Value to set the current position or angle to. + """ + id = VedderCmd.COMM_SET_POS + send_fields = [ + ('pos', 'i', 1000000) + ] + + +class SetRotorPositionMode(metaclass=VESCMessage): + """ Sets the rotor position feedback mode. + It is reccomended to use the defined modes as below: + * DISP_POS_OFF + * DISP_POS_MODE_ENCODER + * DISP_POS_MODE_PID_POS + * DISP_POS_MODE_PID_POS_ERROR + + :ivar pos_mode: Value of the mode + """ + + DISP_POS_OFF = 0 + DISP_POS_MODE_ENCODER = 3 + DISP_POS_MODE_PID_POS = 4 + DISP_POS_MODE_PID_POS_ERROR = 5 + + id = VedderCmd.COMM_SET_DETECT + send_fields = [ + ('pos_mode', 'b') + ] + + +class SetServoPosition(metaclass=VESCMessage): + """Sets the position of s servo connected to the VESC. + + :ivar servo_pos: Value of position (range [0, 1]) + """ + + id = VedderCmd.COMM_SET_SERVO_POS + send_fields = [ + ('servo_pos', 'h', 1000) + ] + + +class Reboot(metaclass=VESCMessage): + """Reboot the VESC""" + id = VedderCmd.COMM_REBOOT + send_fields = [] + + +class Alive(metaclass=VESCMessage): + """Heartbeat signal to keep VESC alive""" + id = VedderCmd.COMM_ALIVE + send_fields = [] + + +class EraseNewApp(metaclass=VESCMessage): + """Erase the flash area on the VESC which stores the new APP, get response""" + id = VedderCmd.COMM_ERASE_NEW_APP + send_fields = [ + ('data', 'I') + ] + recv_fields = [ + ('erase_new_app_result', 'b', 0) + ] + + +class WriteNewAppData(metaclass=VESCMessage): + """Write the new APP data to the VESC, get response + write_new_app_data_result = 0 if failed, 1 if successful + """ + id = VedderCmd.COMM_WRITE_NEW_APP_DATA + send_fields = [ + ('offset', 'I'), + ('data', f'{384}s') + ] + recv_fields = [ + ('write_new_app_result', '?', 0), + ('new_app_offset', 'I', 0) + ] + + +class WriteNewAppDataLZO(metaclass=VESCMessage): + """Write the new APP data to the VESC, get response + write_new_app_data_result = 0 if failed, 1 if successful + + This is a clone of WriteNewAppData, but with a different id to indicate that the data is compressed with LZO + and the VESC is required to decompress + """ + id = VedderCmd.COMM_WRITE_NEW_APP_DATA_LZO + send_fields = [ + ('offset', 'I'), + ('data', f'{384}s') + ] + recv_fields = [ + ('write_new_app_result', '?', 0), + ('new_app_offset', 'I', 0) + ] + + +class JumpToBootloader(metaclass=VESCMessage): + """Jump to the bootloader, get response + jump_to_bootloader_result = 0 if failed, 1 if successful + """ + id = VedderCmd.COMM_JUMP_TO_BOOTLOADER + send_fields = [] + recv_fields = [] + + +class TerminalCmd(metaclass=VESCMessage): + """Send a terminal command to the VESC, get response""" + id = VedderCmd.COMM_TERMINAL_CMD + send_fields = [ + ('cmd', 's') + ] + recv_fields = [ + ('terminal_cmd_result', 's') + ] + + +class TerminalPrint(metaclass=VESCMessage): + """Print a message to the terminal""" + id = VedderCmd.COMM_PRINT + recv_fields = [ + ('msg', 's') + ] + + +# statically save this message because it does not need to be recalculated +alive_msg = encode(Alive()) diff --git a/pyvesc/params/__init__.py b/pyvesc/params/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyvesc/params/confgenerator.py b/pyvesc/params/confgenerator.py new file mode 100644 index 0000000..1093f96 --- /dev/null +++ b/pyvesc/params/confgenerator.py @@ -0,0 +1,809 @@ +# flake8: noqa +# TODO: update vesctool repo to generate this without flake8 issues + +# This file is autogenerated by VESC Tool +from .config_param import * + +# Constants +MCCONF_SIGNATURE = 776184161 +APPCONF_SIGNATURE = 486554156 + +def buffer_pop(buffer, length): + popped = buffer[:length] + del buffer[:length] + return popped + +def confgenerator_serialise_mcconf(mc_configuration): + params = mc_configuration + buffer = [] + + buffer.append(int.to_bytes(MCCONF_SIGNATURE, 4, byteorder='big')) + + buffer.append([param.serialise() for param in params if param.name=="pwm_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="comm_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="motor_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="sensor_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_current_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_current_min"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_in_current_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_in_current_min"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_abs_current_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_min_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_max_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_erpm_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_max_erpm_fbrake"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_max_erpm_fbrake_cc"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_min_vin"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_max_vin"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_battery_cut_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_battery_cut_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_slow_abs_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_temp_fet_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_temp_fet_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_temp_motor_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_temp_motor_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_temp_accel_dec"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_min_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_max_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_watt_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_watt_min"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_current_max_scale"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_current_min_scale"][0]) + buffer.append([param.serialise() for param in params if param.name=="l_duty_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_min_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_min_erpm_cycle_int_limit"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_max_fullbreak_current_dir_change"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_cycle_int_limit"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_phase_advance_at_br"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_cycle_int_rpm_br"][0]) + buffer.append([param.serialise() for param in params if param.name=="sl_bemf_coupling_k"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[3]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[4]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[5]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[6]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_table[7]"][0]) + buffer.append([param.serialise() for param in params if param.name=="hall_sl_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_current_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_current_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_f_zv"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_dt_us"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_encoder_inverted"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_encoder_offset"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_encoder_ratio"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sensor_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_pll_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_pll_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_motor_l"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_motor_ld_lq_diff"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_motor_r"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_motor_flux_linkage"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_observer_gain"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_observer_gain_slow"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_observer_offset"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_duty_dowmramp_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_duty_dowmramp_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_start_curr_dec"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_start_curr_dec_rpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_openloop_rpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_openloop_rpm_low"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_d_gain_scale_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_d_gain_scale_max_mod"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_openloop_hyst"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_openloop_time_lock"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_openloop_time_ramp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_openloop_time"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_openloop_boost_q"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_openloop_max_q"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[3]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[4]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[5]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[6]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_table[7]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hall_interp_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sample_v0_v7"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sample_high_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sat_comp_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sat_comp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_temp_comp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_temp_comp_base_temp"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_current_filter_const"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_cc_decoupling"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_observer_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_voltage_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_voltage_run"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_voltage_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_gain"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_hyst"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_sl_erpm_hfi"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_start_samples"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_obs_ovr_sec"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_hfi_samples"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_cal_on_boot"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_current[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_current[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_current[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_voltage[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_voltage[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_voltage[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_voltage_undriven[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_voltage_undriven[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_offsets_voltage_undriven[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_phase_filter_enable"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_phase_filter_disable_fault"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_phase_filter_max_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_mtpa_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_fw_current_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_fw_duty_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_fw_ramp_time"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_fw_q_current_factor"][0]) + buffer.append([param.serialise() for param in params if param.name=="foc_speed_soure"][0]) + buffer.append([param.serialise() for param in params if param.name=="gpd_buffer_notify_left"][0]) + buffer.append([param.serialise() for param in params if param.name=="gpd_buffer_interpol"][0]) + buffer.append([param.serialise() for param in params if param.name=="gpd_current_filter_const"][0]) + buffer.append([param.serialise() for param in params if param.name=="gpd_current_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="gpd_current_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="sp_pid_loop_rate"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_kd"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_kd_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_min_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_allow_braking"][0]) + buffer.append([param.serialise() for param in params if param.name=="s_pid_ramp_erpms_s"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_kd"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_kd_proc"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_kd_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_ang_div"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_gain_dec_angle"][0]) + buffer.append([param.serialise() for param in params if param.name=="p_pid_offset"][0]) + buffer.append([param.serialise() for param in params if param.name=="cc_startup_boost_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="cc_min_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="cc_gain"][0]) + buffer.append([param.serialise() for param in params if param.name=="cc_ramp_step_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_fault_stop_time_ms"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_duty_ramp_step"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_current_backoff_gain"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_counts"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_sin_amp"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_cos_amp"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_sin_offset"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_cos_offset"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_sincos_filter_constant"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_encoder_sincos_phase_correction"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_sensor_port_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_invert_direction"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_drv8301_oc_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_drv8301_oc_adj"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_bldc_f_sw_min"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_bldc_f_sw_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_dc_f_sw"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_ntc_motor_beta"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_out_aux_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_motor_temp_sens_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_ptc_motor_coeff"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_ntcx_ptcx_res"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_ntcx_ptcx_temp_base"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_hall_extra_samples"][0]) + buffer.append([param.serialise() for param in params if param.name=="m_batt_filter_const"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_motor_poles"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_gear_ratio"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_wheel_diameter"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_battery_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_battery_cells"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_battery_ah"][0]) + buffer.append([param.serialise() for param in params if param.name=="si_motor_nl_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.type"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.limit_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.t_limit_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.t_limit_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.soc_limit_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.soc_limit_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="bms.fwd_can_mode"][0]) + + return b''.join(buffer) + +def confgenerator_serialise_appconf(app_configuration): + params = app_configuration + buffer = [] + + buffer.append(int.to_bytes(APPCONF_SIGNATURE, 4, byteorder='big')) + + buffer.append([param.serialise() for param in params if param.name=="controller_id"][0]) + buffer.append([param.serialise() for param in params if param.name=="timeout_msec"][0]) + buffer.append([param.serialise() for param in params if param.name=="timeout_brake_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="can_status_rate_1"][0]) + buffer.append([param.serialise() for param in params if param.name=="can_status_rate_2"][0]) + buffer.append([param.serialise() for param in params if param.name=="can_status_msgs_r1"][0]) + buffer.append([param.serialise() for param in params if param.name=="can_status_msgs_r2"][0]) + buffer.append([param.serialise() for param in params if param.name=="can_baud_rate"][0]) + buffer.append([param.serialise() for param in params if param.name=="pairing_done"][0]) + buffer.append([param.serialise() for param in params if param.name=="permanent_uart_enabled"][0]) + buffer.append([param.serialise() for param in params if param.name=="shutdown_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="can_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="uavcan_esc_index"][0]) + buffer.append([param.serialise() for param in params if param.name=="uavcan_raw_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="uavcan_raw_rpm_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="uavcan_status_current_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="servo_out_enable"][0]) + buffer.append([param.serialise() for param in params if param.name=="kill_sw_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_to_use"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.ctrl_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.pid_max_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.hyst"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.pulse_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.pulse_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.pulse_center"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.median_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.safe_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.throttle_exp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.throttle_exp_brake"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.throttle_exp_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.ramp_time_pos"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.ramp_time_neg"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.multi_esc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.tc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.tc_max_diff"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.max_erpm_for_dir"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.smart_rev_max_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_ppm_conf.smart_rev_ramp_time"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.ctrl_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.hyst"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage_min"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage_center"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage2_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage2_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.use_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.safe_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.buttons"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage_inverted"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.voltage2_inverted"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.throttle_exp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.throttle_exp_brake"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.throttle_exp_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.ramp_time_pos"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.ramp_time_neg"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.multi_esc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.tc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.tc_max_diff"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_adc_conf.update_rate_hz"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_uart_baudrate"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.ctrl_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.hyst"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.ramp_time_pos"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.ramp_time_neg"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.stick_erpm_per_s_in_cc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.throttle_exp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.throttle_exp_brake"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.throttle_exp_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.multi_esc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.tc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.tc_max_diff"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.use_smart_rev"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.smart_rev_max_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_chuk_conf.smart_rev_ramp_time"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.power"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.crc_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.retry_delay"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.retries"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.channel"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.address[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.address[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.address[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_nrf_conf.send_crc_ack"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.pid_mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.kd"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.kp2"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.ki2"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.kd2"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.hertz"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.loop_time_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_pitch"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_roll"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_adc1"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_adc2"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_delay_pitch"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_delay_roll"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_delay_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_delay_switch_half"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_delay_switch_full"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_adc_half_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.fault_is_dual_switch"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_duty_angle"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_duty_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_duty"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_hv_angle"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_hv_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_hv"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_lv_angle"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_lv_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_lv"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_return_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_constant"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_constant_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_variable"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.tiltback_variable_max"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.noseangling_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.startup_pitch_tolerance"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.startup_roll_tolerance"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.startup_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.deadzone"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.multi_esc"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.yaw_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.yaw_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.yaw_kd"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.roll_steer_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.roll_steer_erpm_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.brake_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.brake_timeout"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.yaw_current_clamp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.ki_limit"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.kd_pt1_lowpass_frequency"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.kd_pt1_highpass_frequency"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.booster_angle"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.booster_ramp"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.booster_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.torquetilt_start_current"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.torquetilt_angle_limit"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.torquetilt_on_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.torquetilt_off_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.torquetilt_strength"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.torquetilt_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_strength"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_angle_limit"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_start_angle"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_start_erpm"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_speed"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_erpm_boost"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_balance_conf.turntilt_erpm_boost_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.ctrl_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.sensor_type"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.current_scaling"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.pedal_rpm_start"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.pedal_rpm_end"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.invert_pedal_direction"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.magnets"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.use_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.ramp_time_pos"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.ramp_time_neg"][0]) + buffer.append([param.serialise() for param in params if param.name=="app_pas_conf.update_rate_hz"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.type"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.mode"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_lowpass_filter_x"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_lowpass_filter_y"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_lowpass_filter_z"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.gyro_lowpass_filter"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.sample_rate_hz"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.use_magnetometer"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_confidence_decay"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.mahony_kp"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.mahony_ki"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.madgwick_beta"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.rot_roll"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.rot_pitch"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.rot_yaw"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_offsets[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_offsets[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.accel_offsets[2]"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.gyro_offsets[0]"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.gyro_offsets[1]"][0]) + buffer.append([param.serialise() for param in params if param.name=="imu_conf.gyro_offsets[2]"][0]) + + return b''.join(buffer) + +def confgenerator_deserialise_mcconf(buffer, mc_configuration): + params = [] + signature = int.from_bytes(buffer_pop(buffer, 4), byteorder='big') + if signature != MCCONF_SIGNATURE: + return False + + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("pwm_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("comm_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("motor_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("sensor_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_current_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_current_min"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_in_current_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_in_current_min"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_abs_current_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_min_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_max_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_erpm_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_max_erpm_fbrake"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_max_erpm_fbrake_cc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_min_vin"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_max_vin"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_battery_cut_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_battery_cut_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("l_slow_abs_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_temp_fet_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_temp_fet_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_temp_motor_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_temp_motor_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_temp_accel_dec"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_min_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_max_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_watt_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("l_watt_min"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_current_max_scale"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_current_min_scale"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("l_duty_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("sl_min_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("sl_min_erpm_cycle_int_limit"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("sl_max_fullbreak_current_dir_change"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("sl_cycle_int_limit"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("sl_phase_advance_at_br"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("sl_cycle_int_rpm_br"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("sl_bemf_coupling_k"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[3]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[4]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[5]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[6]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("hall_table[7]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("hall_sl_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_current_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_current_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_f_zv"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_dt_us"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_encoder_inverted"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_encoder_offset"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_encoder_ratio"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_sensor_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_pll_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_pll_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_motor_l"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_motor_ld_lq_diff"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_motor_r"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_motor_flux_linkage"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_observer_gain"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_observer_gain_slow"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_observer_offset"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_duty_dowmramp_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_duty_dowmramp_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_start_curr_dec"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_start_curr_dec_rpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_openloop_rpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_openloop_rpm_low"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_d_gain_scale_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_d_gain_scale_max_mod"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sl_openloop_hyst"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sl_openloop_time_lock"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sl_openloop_time_ramp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sl_openloop_time"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sl_openloop_boost_q"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sl_openloop_max_q"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[3]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[4]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[5]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[6]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("foc_hall_table[7]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_hall_interp_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_sl_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_sample_v0_v7"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_sample_high_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_sat_comp_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_sat_comp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_temp_comp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_temp_comp_base_temp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_current_filter_const"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_cc_decoupling"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_observer_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_hfi_voltage_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_hfi_voltage_run"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_hfi_voltage_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_hfi_gain"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_hfi_hyst"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_sl_erpm_hfi"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("foc_hfi_start_samples"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_hfi_obs_ovr_sec"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_hfi_samples"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_offsets_cal_on_boot"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_offsets_current[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_offsets_current[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_offsets_current[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_offsets_voltage[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_offsets_voltage[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_offsets_voltage[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_offsets_voltage_undriven[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_offsets_voltage_undriven[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_offsets_voltage_undriven[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_phase_filter_enable"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_phase_filter_disable_fault"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_phase_filter_max_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_mtpa_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("foc_fw_current_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_fw_duty_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_fw_ramp_time"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("foc_fw_q_current_factor"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("foc_speed_soure"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Int16("gpd_buffer_notify_left"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Int16("gpd_buffer_interpol"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("gpd_current_filter_const"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("gpd_current_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("gpd_current_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("sp_pid_loop_rate"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("s_pid_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("s_pid_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("s_pid_kd"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("s_pid_kd_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("s_pid_min_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("s_pid_allow_braking"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("s_pid_ramp_erpms_s"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("p_pid_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("p_pid_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("p_pid_kd"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("p_pid_kd_proc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("p_pid_kd_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("p_pid_ang_div"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("p_pid_gain_dec_angle"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("p_pid_offset"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("cc_startup_boost_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("cc_min_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("cc_gain"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("cc_ramp_step_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Int32("m_fault_stop_time_ms"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_duty_ramp_step"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("m_current_backoff_gain"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_UInt32("m_encoder_counts"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_encoder_sin_amp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_encoder_cos_amp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_encoder_sin_offset"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_encoder_cos_offset"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_encoder_sincos_filter_constant"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_encoder_sincos_phase_correction"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("m_sensor_port_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("m_invert_direction"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("m_drv8301_oc_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("m_drv8301_oc_adj"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("m_bldc_f_sw_min"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("m_bldc_f_sw_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("m_dc_f_sw"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("m_ntc_motor_beta"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("m_out_aux_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("m_motor_temp_sens_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("m_ptc_motor_coeff"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_ntcx_ptcx_res"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("m_ntcx_ptcx_temp_base"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("m_hall_extra_samples"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("m_batt_filter_const"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("si_motor_poles"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("si_gear_ratio"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("si_wheel_diameter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("si_battery_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("si_battery_cells"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("si_battery_ah"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("si_motor_nl_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("bms.type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("bms.limit_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("bms.t_limit_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("bms.t_limit_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("bms.soc_limit_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("bms.soc_limit_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("bms.fwd_can_mode"))) + + return params + +def confgenerator_deserialise_appconf(buffer, app_configuration): + params = [] + signature = int.from_bytes(buffer_pop(buffer, 4), byteorder='big') + if signature != APPCONF_SIGNATURE: + return False + + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("controller_id"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_UInt32("timeout_msec"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("timeout_brake_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("can_status_rate_1"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("can_status_rate_2"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("can_status_msgs_r1"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("can_status_msgs_r2"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("can_baud_rate"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("pairing_done"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("permanent_uart_enabled"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("shutdown_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("can_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("uavcan_esc_index"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("uavcan_raw_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("uavcan_raw_rpm_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("uavcan_status_current_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("servo_out_enable"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("kill_sw_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_to_use"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_ppm_conf.ctrl_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.pid_max_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.hyst"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.pulse_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.pulse_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.pulse_center"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_ppm_conf.median_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_ppm_conf.safe_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.throttle_exp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.throttle_exp_brake"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_ppm_conf.throttle_exp_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.ramp_time_pos"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.ramp_time_neg"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_ppm_conf.multi_esc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_ppm_conf.tc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.tc_max_diff"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_ppm_conf.max_erpm_for_dir"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.smart_rev_max_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_ppm_conf.smart_rev_ramp_time"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.ctrl_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_adc_conf.hyst"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage_min"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage_center"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage2_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_adc_conf.voltage2_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.use_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.safe_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.buttons"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.voltage_inverted"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.voltage2_inverted"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_adc_conf.throttle_exp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_adc_conf.throttle_exp_brake"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.throttle_exp_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_adc_conf.ramp_time_pos"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_adc_conf.ramp_time_neg"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.multi_esc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_adc_conf.tc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_adc_conf.tc_max_diff"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_adc_conf.update_rate_hz"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_UInt32("app_uart_baudrate"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_chuk_conf.ctrl_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.hyst"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.ramp_time_pos"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.ramp_time_neg"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.stick_erpm_per_s_in_cc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.throttle_exp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.throttle_exp_brake"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_chuk_conf.throttle_exp_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_chuk_conf.multi_esc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_chuk_conf.tc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.tc_max_diff"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_chuk_conf.use_smart_rev"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.smart_rev_max_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_chuk_conf.smart_rev_ramp_time"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_nrf_conf.speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_nrf_conf.power"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_nrf_conf.crc_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_nrf_conf.retry_delay"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("app_nrf_conf.retries"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Int8("app_nrf_conf.channel"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("app_nrf_conf.address[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("app_nrf_conf.address[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_UInt8("app_nrf_conf.address[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_nrf_conf.send_crc_ack"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_balance_conf.pid_mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.kd"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.kp2"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.ki2"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.kd2"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.hertz"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.loop_time_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.fault_pitch"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.fault_roll"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.fault_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.fault_adc1"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.fault_adc2"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.fault_delay_pitch"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.fault_delay_roll"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.fault_delay_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.fault_delay_switch_half"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.fault_delay_switch_full"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.fault_adc_half_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_balance_conf.fault_is_dual_switch"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_duty_angle"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_duty_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_duty"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_hv_angle"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_hv_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.tiltback_hv"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_lv_angle"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_lv_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.tiltback_lv"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.tiltback_return_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.tiltback_constant"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.tiltback_constant_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.tiltback_variable"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.tiltback_variable_max"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_balance_conf.noseangling_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.startup_pitch_tolerance"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.startup_roll_tolerance"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.startup_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.deadzone"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_balance_conf.multi_esc"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.yaw_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.yaw_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.yaw_kd"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.roll_steer_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.roll_steer_erpm_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.brake_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.brake_timeout"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.yaw_current_clamp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.ki_limit"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.kd_pt1_lowpass_frequency"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.kd_pt1_highpass_frequency"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.booster_angle"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.booster_ramp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.booster_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.torquetilt_start_current"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.torquetilt_angle_limit"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.torquetilt_on_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.torquetilt_off_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.torquetilt_strength"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.torquetilt_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.turntilt_strength"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.turntilt_angle_limit"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.turntilt_start_angle"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.turntilt_start_erpm"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("app_balance_conf.turntilt_speed"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.turntilt_erpm_boost"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_balance_conf.turntilt_erpm_boost_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_pas_conf.ctrl_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_pas_conf.sensor_type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_pas_conf.current_scaling"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_pas_conf.pedal_rpm_start"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_pas_conf.pedal_rpm_end"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_pas_conf.invert_pedal_direction"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_pas_conf.magnets"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("app_pas_conf.use_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_pas_conf.ramp_time_pos"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("app_pas_conf.ramp_time_neg"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("app_pas_conf.update_rate_hz"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("imu_conf.type"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("imu_conf.mode"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("imu_conf.filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("imu_conf.accel_lowpass_filter_x"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("imu_conf.accel_lowpass_filter_y"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("imu_conf.accel_lowpass_filter_z"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_Double16("imu_conf.gyro_lowpass_filter"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 2)), obj)[1])(Param_UInt16("imu_conf.sample_rate_hz"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 1)), obj)[1])(Param_Bool("imu_conf.use_magnetometer"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.accel_confidence_decay"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.mahony_kp"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.mahony_ki"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.madgwick_beta"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.rot_roll"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.rot_pitch"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.rot_yaw"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.accel_offsets[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.accel_offsets[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.accel_offsets[2]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.gyro_offsets[0]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.gyro_offsets[1]"))) + params.append((lambda obj: (obj.deserialise(buffer_pop(buffer, 4)), obj)[1])(Param_Double32_Auto("imu_conf.gyro_offsets[2]"))) + + return params + +def confgenerator_set_defaults_mcconf(mc_configuration): + raise NotImplementedError("Python not yet implemented") + +def confgenerator_set_defaults_appconf(app_configuration): + raise NotImplementedError("Python not yet implemented") + diff --git a/pyvesc/params/config_param.py b/pyvesc/params/config_param.py new file mode 100644 index 0000000..95f529d --- /dev/null +++ b/pyvesc/params/config_param.py @@ -0,0 +1,249 @@ +from abc import ABC, abstractmethod +import struct +from math import ldexp, frexp + + +def buffer_pop(buffer, length, ind): + """ + Pop the first `length` bytes from the buffer and return them. + The buffer is updated in place. + + :ind: Index of the buffer to pop from, unused - to be removed + """ + # Ensure buffer is a bytearray for in-place modification + if not isinstance(buffer, bytearray): + raise TypeError("Buffer must be of type 'bytearray'") + + popped = buffer[:length] + del buffer[:length] # Modify the buffer in place + return popped + + +class ConfigParam(ABC): + + def __init__(self, name, value=None): + self.name = name + self.value = value + + @abstractmethod + def deserialise(self, buffer): + pass + + @abstractmethod + def serialise(self): + pass + + def __repr__(self): + return f"{self.name}: {self.value}" + + +class Param_Bool(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = '?' + + def deserialise(self, buffer): + if len(buffer) != 1: + raise ValueError("Buffer must be exactly 1 byte long.") + self.value = struct.unpack(self.format, buffer)[0] + + def serialise(self): + return struct.pack(self.format, self.value) + + +class Param_UInt8(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'B' # Format for unsigned 8-bit integer + + def deserialise(self, buffer): + if len(buffer) != 1: + raise ValueError("Buffer must be exactly 1 byte long.") + self.value = int.from_bytes(buffer, byteorder='big', signed=False) + + def serialise(self): + if self.value < 0 or self.value > 255: + raise ValueError("Value must be between 0 and 255 for uint8.") + return self.value.to_bytes(1, byteorder='big', signed=False) + + +class Param_Int8(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'b' # Format for signed 8-bit integer + + def deserialise(self, buffer): + if len(buffer) != 1: + raise ValueError("Buffer must be exactly 1 byte long.") + self.value = int.from_bytes(buffer, byteorder='big', signed=True) + + def serialise(self): + if self.value < -128 or self.value > 127: + raise ValueError("Value must be between -128 and 127 for int8.") + return self.value.to_bytes(1, byteorder='big', signed=True) + + +class Param_UInt16(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'H' # Format for unsigned 16-bit integer + + def deserialise(self, buffer): + if len(buffer) != 2: + raise ValueError("Buffer must be exactly 2 bytes long.") + self.value = int.from_bytes(buffer, byteorder='big', signed=False) + + def serialise(self): + if self.value < 0 or self.value > 65535: + raise ValueError("Value must be between 0 and 65535 for uint16.") + return self.value.to_bytes(2, byteorder='big', signed=False) + + +class Param_Int16(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'h' # Format for signed 16-bit integer + + def deserialise(self, buffer): + if len(buffer) != 2: + raise ValueError("Buffer must be exactly 2 bytes long.") + self.value = int.from_bytes(buffer, byteorder='big', signed=True) + + def serialise(self): + if self.value < -32768 or self.value > 32767: + raise ValueError("Value must be between -32768 and 32767 for int16.") + return self.value.to_bytes(2, byteorder='big', signed=True) + + +class Param_UInt32(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'I' # Format for unsigned 32-bit integer + + def deserialise(self, buffer): + if len(buffer) != 4: + raise ValueError("Buffer must be exactly 4 bytes long.") + self.value = int.from_bytes(buffer, byteorder='big', signed=False) + + def serialise(self): + if self.value < 0 or self.value > 4294967295: + raise ValueError("Value must be between 0 and 4294967295 for uint32.") + return self.value.to_bytes(4, byteorder='big', signed=False) + + +class Param_Int32(ConfigParam): + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'i' # Format for signed 32-bit integer + + def deserialise(self, buffer): + if len(buffer) != 4: + raise ValueError("Buffer must be exactly 4 bytes long.") + self.value = int.from_bytes(buffer, byteorder='big', signed=True) + + def serialise(self): + if self.value < -2147483648 or self.value > 2147483647: + raise ValueError("Value must be between -2147483648 and 2147483647 for int32.") + return self.value.to_bytes(4, byteorder='big', signed=True) + + +class Param_Double16(Param_Int16): + """ + In vesc, is rounded to nearest integer and then sent as a uint16 + """ + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'e' # Format for 16-bit floating point (half precision) + + def deserialise(self, buffer): + + super().deserialise(buffer) + self.value = float(self.value) # Convert to float + + def serialise(self): + rounded = round(self.value) + if rounded < -32768 or rounded > 32767: + raise ValueError("Value must be between -32768 and 32767 for int16.") + return rounded.to_bytes(2, byteorder='big', signed=True) + + +class Param_Double32(Param_Int32): + """ + In vesc, is rounded to nearest integer and then sent as a uint32 + """ + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'f' # Format for 32-bit floating point (single precision) + + def deserialise(self, buffer): + super().deserialise(buffer) + self.value = float(self.value) # Convert to float + + def serialise(self): + rounded = round(self.value) + if rounded < -2147483648 or rounded > 2147483647: + raise ValueError("Value must be between -2147483648 and 2147483647 for int32.") + return rounded.to_bytes(4, byteorder='big', signed=True) + + +class Param_Double32_Auto(Param_Int32): + """ + In vesc, is properly cast as a float, to be implemented + """ + + def __init__(self, name, value=None): + super().__init__(name, value) + self.format = 'f' # Assumed to be similar to Double32 (single precision) + + def deserialise(self, buffer): + # TODO: Implement proper deserialisation + buffer = int.from_bytes(buffer, byteorder='big', signed=False) + e = (buffer >> 23) & 0xFF + m = buffer & 0x7FFFFF + negative = buffer & (1 << 31) + + f = 0.0 + if (e != 0 or m != 0): + f = float(m) / (8388608.0 * 2.0) + 0.5 + e -= 126 + + if (negative): + f = -f + + self.value = ldexp(f, e) + + def serialise(self): + + number = self.value + + # Set subnormal numbers to 0 + if abs(number) < 1.5e-38: + number = 0.0 + + fr, e = frexp(number) # Use math.frexp to get mantissa and exponent + fr_abs = abs(fr) + fr_s = 0 + + # Handle the mantissa for normalized numbers + if fr_abs >= 0.5: + fr_s = int((fr_abs - 0.5) * 2.0 * 8388608.0) # Convert mantissa to 23-bit fraction + e += 126 # Adjust exponent + + # Combine exponent and mantissa into a 32-bit integer + res = ((e & 0xFF) << 23) | (fr_s & 0x7FFFFF) + + # Set the sign bit if the original number was negative + if fr < 0: + res |= 1 << 31 + + # Return the result as 4 bytes (big-endian, unsigned) + return res.to_bytes(4, byteorder='big', signed=False) diff --git a/pyvesc/params/config_params.py b/pyvesc/params/config_params.py new file mode 100644 index 0000000..eea436a --- /dev/null +++ b/pyvesc/params/config_params.py @@ -0,0 +1,3 @@ +import logging + +logger = logging.getLogger(__name__) diff --git a/pyvesc/params/datatypes.py b/pyvesc/params/datatypes.py new file mode 100644 index 0000000..561b30f --- /dev/null +++ b/pyvesc/params/datatypes.py @@ -0,0 +1,14 @@ +from enum import Enum + + +class VESC_FMT(Enum): + VESC_TX_UNDEFINED = 0 + VESC_TX_UINT8 = 1 + VESC_TX_INT8 = 2 + VESC_TX_UINT16 = 3 + VESC_TX_INT16 = 4 + VESC_TX_UINT32 = 5 + VESC_TX_INT32 = 6 + VESC_TX_DOUBLE16 = 7 + VESC_TX_DOUBLE32 = 8 + VESC_TX_DOUBLE32_AUTO = 9 diff --git a/pyvesc/params/test.py b/pyvesc/params/test.py new file mode 100644 index 0000000..3ca5f06 --- /dev/null +++ b/pyvesc/params/test.py @@ -0,0 +1,79 @@ +import random +import unittest +from .config_param import Param_UInt8, Param_Int8, Param_UInt16, Param_Int16, Param_UInt32, Param_Int32, Param_Double16, Param_Double32, Param_Double32_Auto + + +class TestConfigParam(unittest.TestCase): + + num_tests = 20 + + def assert_serialization(self, param_class, value, round_result=False, assert_places=None): + # Test serialization + param = param_class("test_param_serialised", value=value) + serialised = param.serialise() + + # Test deserialization + param_deserialised = param_class("test_param_deserialised") + param_deserialised.deserialise(serialised) + if round_result: + self.assertAlmostEqual(param_deserialised.value, round(value), places=assert_places) + else: + self.assertAlmostEqual(param_deserialised.value, value, places=assert_places) + + # Ensure that the deserialised value can be serialised back to the same data + re_serialised = param_deserialised.serialise() + self.assertAlmostEqual(re_serialised, serialised, places=assert_places) + + def test_param_uint8(self): + # Test random uint8 values (0 to 255) + for _ in range(self.num_tests): + VALUE = random.randint(0, 255) + self.assert_serialization(Param_UInt8, VALUE) + + def test_param_int8(self): + # Test random int8 values (-128 to 127) + for _ in range(self.num_tests): + VALUE = random.randint(-128, 127) + self.assert_serialization(Param_Int8, VALUE) + + def test_param_uint16(self): + # Test random uint16 values (0 to 65535) + for _ in range(self.num_tests): + VALUE = random.randint(0, 65535) + self.assert_serialization(Param_UInt16, VALUE) + + def test_param_int16(self): + # Test random int16 values (-32768 to 32767) + for _ in range(self.num_tests): + VALUE = random.randint(-32768, 32767) + self.assert_serialization(Param_Int16, VALUE) + + def test_param_uint32(self): + # Test random uint32 values (0 to 4294967295) + for _ in range(self.num_tests): + VALUE = random.randint(0, 4294967295) + self.assert_serialization(Param_UInt32, VALUE) + + def test_param_int32(self): + # Test random int32 values (-2147483648 to 2147483647) + for _ in range(self.num_tests): + VALUE = random.randint(-2147483648, 2147483647) + self.assert_serialization(Param_Int32, VALUE) + + def test_param_double16(self): + # Test random double16 values within a sensible range + for _ in range(self.num_tests): + VALUE = random.uniform(-32768.0, 32767.99) + self.assert_serialization(Param_Double16, VALUE, round_result=True) + + def test_param_double32(self): + # Test random double32 values within a sensible range + for _ in range(self.num_tests): + VALUE = random.uniform(-1e6, 1e6) + self.assert_serialization(Param_Double32, VALUE, round_result=True) + + def test_param_double32_auto(self): + # Test random double32_auto values within a sensible range + for _ in range(self.num_tests): + VALUE = random.uniform(-100000.0, 100000.0) + self.assert_serialization(Param_Double32_Auto, VALUE, assert_places=1) diff --git a/pyvesc/protocol/__init__.py b/pyvesc/protocol/__init__.py index 4ac8840..e69de29 100644 --- a/pyvesc/protocol/__init__.py +++ b/pyvesc/protocol/__init__.py @@ -1,3 +0,0 @@ -from .interface import * -from .packet import * -from .base import * diff --git a/pyvesc/protocol/base.py b/pyvesc/protocol/base.py index 73651e5..87db342 100644 --- a/pyvesc/protocol/base.py +++ b/pyvesc/protocol/base.py @@ -1,5 +1,7 @@ import struct +STRING_MAX_LEN = 5000 + class VESCMessage(type): """ Metaclass for VESC messages. @@ -29,27 +31,83 @@ def __init__(cls, name, bases, clsdict): raise TypeError("ID conflict with %s" % str(VESCMessage._msg_registry[msg_id])) else: VESCMessage._msg_registry[msg_id] = cls + # initialize cls static variables - cls._string_field = None - cls._fmt_fields = '' - cls._field_names = [] - cls._field_scalars = [] - for field, idx in zip(cls.fields, range(0, len(cls.fields))): - cls._field_names.append(field[0]) - if len(field) >= 3: - cls._field_scalars.append(field[2]) - if field[1] is 's': - # string field, add % so we can vary the length - cls._fmt_fields += '%u' - cls._string_field = idx - cls._fmt_fields += field[1] - cls._full_msg_size = struct.calcsize(cls._fmt_fields) - # check that at most 1 field is a string - string_field_count = cls._fmt_fields.count('s') - if string_field_count > 1: - raise TypeError("Max number of string fields is 1.") - if 'p' in cls._fmt_fields: - raise TypeError("Field with format character 'p' detected. For string field use 's'.") + cls._send_string_field = None + cls._send_field_formats = [] + cls._send_field_names = [] + cls._send_field_scalars = [] + # receive fields can be different from send fields + cls._recv_string_field = None + cls._recv_field_formats = [] + cls._recv_field_names = [] + cls._recv_field_scalars = [] + + # check that there are any fields defined + if not hasattr(cls, 'send_fields') and not hasattr(cls, 'recv_fields'): + raise AttributeError("No fields defined for ", VESCMessage._msg_registry[msg_id]) + + # format the fields to send + if hasattr(cls, 'send_fields'): + for field, idx in zip(cls.send_fields, range(0, len(cls.send_fields))): + cls._send_field_names.append(field[0]) + + # apply scalar if defined + if len(field) >= 3: + cls._send_field_scalars.append(field[2]) + if field[1] == 's': + # string field, add % so we can vary the length + # edit: we will use a fixed length for now + cls._send_field_formats.append('%us') + cls._send_string_field = idx + else: + cls._send_field_formats.append(field[1]) + + # calc size, iterating through each format to eliminate padding + msg_size = 0 + for fmt in cls._recv_field_formats: + msg_size += struct.calcsize(fmt) + cls._recv_full_msg_size = msg_size + + # check that at most 1 field is a string + string_field_count = cls._send_field_formats.count('s') + cls._send_field_formats.count('%us') + if string_field_count > 1: + raise TypeError("Max number of string fields is 1.") + if 'p' in cls._send_field_formats: + raise TypeError("Field with format character 'p' detected. For string field use 's'.") + + # format the fields to receive, if we didn't define any - don't make any + # TODO: this is duplicated code, should be refactored + if hasattr(cls, 'recv_fields'): + for field, idx in zip(cls.recv_fields, range(0, len(cls.recv_fields))): + cls._recv_field_names.append(field[0]) + + # apply scalar if defined + if len(field) >= 3: + cls._recv_field_scalars.append(field[2]) + if field[1] == 's': + # for now, maximum string size is 1000 + cls._recv_field_formats.append('%us') + cls._recv_string_field = idx + else: + cls._recv_field_formats.append(field[1]) + + # calc size, iterating through each format to eliminate padding + msg_size = 0 + for fmt in cls._recv_field_formats: + if '%us' in fmt: + msg_size += STRING_MAX_LEN + else: + msg_size += struct.calcsize(fmt) + cls._recv_full_msg_size = msg_size + + # check that at most 1 field is a string + string_field_count = cls._recv_field_formats.count('s') + if string_field_count > 1: + raise TypeError("Max number of string fields is 1.") + if 'p' in cls._recv_field_formats: + raise TypeError("Field with format character 'p' detected. For string field use 's'.") + super(VESCMessage, cls).__init__(name, bases, clsdict) def __call__(cls, *args, **kwargs): @@ -58,10 +116,33 @@ def __call__(cls, *args, **kwargs): instance.can_id = kwargs['can_id'] else: instance.can_id = None + + # Select whether we want to unpack the send or receive fields + # If there are only one defined, we will unpack that one + # If both are defined, we will refer to the unpack_send_fields flag + available_fields = {'send_fields': hasattr(cls, 'send_fields'), 'recv_fields': hasattr(cls, 'recv_fields')} + field_to_unpack = None + if available_fields['send_fields'] and not available_fields['recv_fields']: + field_to_unpack = 'send_fields' + elif not available_fields['send_fields'] and available_fields['recv_fields']: + field_to_unpack = 'recv_fields' + elif available_fields['send_fields'] and available_fields['recv_fields']: + if 'unpack_send_fields' in kwargs and kwargs['unpack_send_fields'] is False: + field_to_unpack = 'recv_fields' + else: + field_to_unpack = 'send_fields' + + if field_to_unpack == 'send_fields': + fields = cls.send_fields + field_names = cls._send_field_names + else: + fields = cls.recv_fields + field_names = cls._recv_field_names + if args: - if len(args) != len(cls.fields): - raise AttributeError("Expected %u arguments, received %u" % (len(cls.fields), len(args))) - for name, value in zip(cls._field_names, args): + if len(args) != len(fields): + raise AttributeError("Expected %u arguments, received %u" % (len(fields), len(args))) + for name, value in zip(field_names, args): setattr(instance, name, value) return instance @@ -70,36 +151,62 @@ def msg_type(id): return VESCMessage._msg_registry[id] @staticmethod - def unpack(msg_bytes): + def unpack(msg_bytes, unpack_send_fields=False): msg_id = struct.unpack_from(VESCMessage._endian_fmt + VESCMessage._id_fmt, msg_bytes, 0) msg_type = VESCMessage.msg_type(*msg_id) data = None - if not (msg_type._string_field is None): - # string field - fmt_wo_string = msg_type._fmt_fields.replace('%u', '') - fmt_wo_string = fmt_wo_string.replace('s', '') + + # Select whether we want to unpack the send or receive fields + # We will unpack the send fields if there are none received + if unpack_send_fields or not hasattr(msg_type, 'recv_fields'): + fields = msg_type.send_fields + string_field = msg_type._send_string_field + field_names = msg_type._send_field_names + field_formats = "".join([char for tup in msg_type._send_field_formats for char in tup]) + field_scalars = msg_type._send_field_scalars + else: + fields = msg_type.recv_fields + string_field = msg_type._recv_string_field + field_names = msg_type._recv_field_names + field_formats = "".join([char for tup in msg_type._recv_field_formats for char in tup]) # stringify formats + field_scalars = msg_type._recv_field_scalars + + if not (string_field is None): + # remove the %u and s from the format string + if '%u' or 's' in field_formats: + fmt_wo_string = field_formats.replace('%u', '') + fmt_wo_string = fmt_wo_string.replace('s', '') len_string = len(msg_bytes) - struct.calcsize(VESCMessage._endian_fmt + fmt_wo_string) - 1 - fmt_w_string = msg_type._fmt_fields % (len_string) + fmt_w_string = field_formats % (len_string) data = struct.unpack_from(VESCMessage._endian_fmt + fmt_w_string, msg_bytes, 1) else: - data = list(struct.unpack_from(VESCMessage._endian_fmt + msg_type._fmt_fields, msg_bytes, 1)) + data = list(struct.unpack_from(VESCMessage._endian_fmt + field_formats, msg_bytes, 1)) for k, field in enumerate(data): try: - if msg_type._field_scalars[k] != 0: - data[k] = data[k]/msg_type._field_scalars[k] + if field_scalars[k] != 0: + data[k] = data[k] / field_scalars[k] except (TypeError, IndexError) as e: - print("Error ecountered on field " + msg_type.fields[k][0]) + print("Error encountered on field " + fields[k][0]) print(e) - msg = msg_type(*data) - if not (msg_type._string_field is None): - string_field_name = msg_type._field_names[msg_type._string_field] - setattr(msg, - string_field_name, - getattr(msg, string_field_name).decode('ascii')) + + msg = msg_type(*data, unpack_send_fields=unpack_send_fields) + if not (string_field is None): + string_field_name = field_names[string_field] + + # if scalar is -1, we do not interpret the string as ascii, instead as a bytestring + if len(field_scalars) > 0 and field_scalars[string_field] == -1: + setattr(msg, + string_field_name, + getattr(msg, string_field_name)) + else: + setattr(msg, + string_field_name, + getattr(msg, string_field_name).decode('ascii')) + return msg @staticmethod - def pack(instance, header_only=None): + def pack(instance, header_only=None, pack_send_fields=True): if header_only: if instance.can_id is not None: fmt = VESCMessage._endian_fmt + VESCMessage._can_id_fmt + VESCMessage._id_fmt @@ -109,31 +216,56 @@ def pack(instance, header_only=None): values = (instance.id,) return struct.pack(fmt, *values) + # Select whether we want to unpack the send or receive fields + # we may only ever unpack the receive fields, but the send fields + # are used for unit testing at least + if pack_send_fields: + string_field = instance._send_string_field + field_names = instance._send_field_names + field_formats = "".join([char for tup in instance._send_field_formats for char in tup]) # stringify formats + field_scalars = instance._send_field_scalars + else: + string_field = instance._recv_string_field + field_names = instance._recv_field_names + field_formats = "".join([char for tup in instance._recv_field_formats for char in tup]) # stringify formats + field_scalars = instance._recv_field_scalars + field_values = [] - if not instance._field_scalars: - for field_name in instance._field_names: + if not field_scalars: + for field_name in field_names: field_values.append(getattr(instance, field_name)) else: - for field_name, field_scalar in zip(instance._field_names, instance._field_scalars): - field_values.append(int(getattr(instance, field_name) * field_scalar)) - if not (instance._string_field is None): + for index, (field_name, field_scalar) in enumerate(zip(field_names, field_scalars)): + # if it's a string field, don't apply the scalar as it represents the format, not the data + if instance.send_fields[index][1] == 's': + field_values.append(getattr(instance, field_name)) + else: + field_values.append(int(getattr(instance, field_name) * field_scalar)) + if not (string_field is None): # string field - string_field_name = instance._field_names[instance._string_field] + string_field_name = field_names[string_field] + # remove the %u and s from the format string + if '%u' or 's' in field_formats: + fmt_wo_string = field_formats.replace('%u', '') + fmt_wo_string = fmt_wo_string.replace('s', '') string_length = len(getattr(instance, string_field_name)) - field_values[instance._string_field] = field_values[instance._string_field].encode('ascii') + # if scalar is -1, we do not interpret the string as ascii, instead as a bytestring + if len(field_scalars) > string_field and field_scalars[string_field] == -1: + field_values[string_field] = field_values[string_field] + else: + field_values[string_field] = field_values[string_field].encode('ascii') values = ((instance.id,) + tuple(field_values)) if instance.can_id is not None: - fmt = VESCMessage._endian_fmt + VESCMessage._can_id_fmt + VESCMessage._id_fmt\ - + (instance._fmt_fields % (string_length)) + fmt = VESCMessage._endian_fmt + VESCMessage._can_id_fmt + VESCMessage._id_fmt + (fmt % (string_length)) values = (VESCMessage._comm_forward_can, instance.can_id) + values else: - fmt = VESCMessage._endian_fmt + VESCMessage._id_fmt + (instance._fmt_fields % (string_length)) + fmt = VESCMessage._endian_fmt + VESCMessage._id_fmt + (field_formats % (string_length)) return struct.pack(fmt, *values) else: values = ((instance.id,) + tuple(field_values)) if instance.can_id is not None: - fmt = VESCMessage._endian_fmt + VESCMessage._can_id_fmt + VESCMessage._id_fmt + instance._fmt_fields + fmt = VESCMessage._endian_fmt + VESCMessage._can_id_fmt + VESCMessage._id_fmt + field_formats values = (VESCMessage._comm_forward_can, instance.can_id) + values else: - fmt = VESCMessage._endian_fmt + VESCMessage._id_fmt + instance._fmt_fields + fmt = VESCMessage._endian_fmt + VESCMessage._id_fmt + field_formats return struct.pack(fmt, *values) diff --git a/pyvesc/protocol/interface.py b/pyvesc/protocol/interface.py index 8d740d4..cacfab7 100644 --- a/pyvesc/protocol/interface.py +++ b/pyvesc/protocol/interface.py @@ -1,8 +1,8 @@ -import pyvesc.protocol.base -import pyvesc.protocol.packet.codec +from . import base +from .packet import codec -def decode(buffer): +def decode(buffer, recv=True): """ Decodes the next valid VESC message in a buffer. @@ -13,11 +13,64 @@ def decode(buffer): was parsed returns (None, 0). :rtype: `tuple`: (PyVESC message, int) """ - msg_payload, consumed = pyvesc.protocol.packet.codec.unframe(buffer) - if msg_payload: - return pyvesc.protocol.base.VESCMessage.unpack(msg_payload), consumed + + messages = [] + + # return values + unpacked_messages = [] + consumed_total = 0 + msg_payload_total = b'' + + # multi-frame messages, loop until buffer is empty + while len(buffer) > 0: + msg_payload, consumed = codec.unframe(buffer) + messages.append((msg_payload, consumed)) + buffer = buffer[consumed:] + consumed_total += consumed + msg_payload_total += msg_payload + + if len(messages) > 0: + for msg_payload, consumed in messages: + if msg_payload is not None: + unpacked_messages.append((base.VESCMessage.unpack(msg_payload))) + else: + raise ValueError("Invalid message payload") + # combine unpacked messages into one string if there is a string field + # we use the first message to determine if there is a string field, they will all have the + # same field data as they are just repeated messages + string_field_name = None + string_field_scalar = None + + if recv: + field = 'recv_fields' + else: + field = 'send_fields' + if hasattr(unpacked_messages[0], field): + field_list = getattr(unpacked_messages[0], field) + for i, f in enumerate(field_list): + if 's' in f[1]: # f1 is the formats (there is a '[send/recv]_field_formats attr, but cbf getting it) + # there is a string, get the field name + string_field_name = f[0] + # if there is a scalar field + if len(f) > 2: + string_field_scalar = field_list[i][2] + + if string_field_name is not None: + # check if string is an ascii or bytes by looking at the string scalar. -1 is bytestring, None is ascii + if string_field_scalar is None: + message_res = "".join([getattr(unpacked_message, string_field_name) + "\n" for unpacked_message in unpacked_messages]) + elif string_field_scalar == -1: + message_res = b"".join([getattr(unpacked_message, string_field_name) for unpacked_message in unpacked_messages]) + + else: + if len(unpacked_messages) == 1: + message_res = unpacked_messages[0] + else: + raise ValueError("Don't currently support multiple message results from one field that aren't strings") + + return message_res, consumed_total, msg_payload_total else: - return None, consumed + return None, consumed_total, msg_payload_total def encode(msg): @@ -31,8 +84,8 @@ def encode(msg): :return: The packet. :rtype: bytes """ - msg_payload = pyvesc.protocol.base.VESCMessage.pack(msg) - packet = pyvesc.protocol.packet.codec.frame(msg_payload) + msg_payload = base.VESCMessage.pack(msg) + packet = codec.frame(msg_payload) return packet @@ -48,6 +101,6 @@ def encode_request(msg_cls): :return: The encoded PyVESC message which can be sent. :rtype: bytes """ - msg_payload = pyvesc.protocol.base.VESCMessage.pack(msg_cls, header_only=True) - packet = pyvesc.protocol.packet.codec.frame(msg_payload) + msg_payload = base.VESCMessage.pack(msg_cls, header_only=True) + packet = codec.frame(msg_payload) return packet diff --git a/pyvesc/protocol/packet/__init__.py b/pyvesc/protocol/packet/__init__.py index a158b24..e69de29 100644 --- a/pyvesc/protocol/packet/__init__.py +++ b/pyvesc/protocol/packet/__init__.py @@ -1,3 +0,0 @@ -from .structure import * -from .codec import * -from .exceptions import * diff --git a/pyvesc/protocol/packet/codec.py b/pyvesc/protocol/packet/codec.py index d36f816..937cbda 100644 --- a/pyvesc/protocol/packet/codec.py +++ b/pyvesc/protocol/packet/codec.py @@ -1,5 +1,5 @@ -from .exceptions import * -from .structure import * +from .exceptions import CorruptPacket, InvalidPayload +from .structure import struct, Header, Footer from crccheck.crc import CrcXmodem crc_checker = CrcXmodem() @@ -50,13 +50,13 @@ def _next_possible_packet_index(buffer): :param buffer: buffer object. :return: Index of next valid start byte. Returns -1 if no valid start bytes are found. """ - if len(buffer) < 2: # too short to find next + if len(buffer) < 2: # too short to find next return -1 next_short_sb = buffer[1:].find(b'\x02') - next_long_sb= buffer[1:].find(b'\x03') + next_long_sb = buffer[1:].find(b'\x03') possible_index = [] - if next_short_sb >= 0: # exclude index zero's as we know the current first packet is corrupt - possible_index.append(next_short_sb + 1) # +1 because we want found from second byte + if next_short_sb >= 0: # exclude index zero's as we know the current first packet is corrupt + possible_index.append(next_short_sb + 1) # +1 because we want found from second byte if next_long_sb >= 0: possible_index.append(next_long_sb + 1) if possible_index == []: @@ -72,10 +72,10 @@ def _consume_after_corruption_detected(buffer): :return: Number of bytes to consume in the buffer. """ next_index = UnpackerBase._next_possible_packet_index(buffer) - if next_index == -1: # no valid start byte was found - return len(buffer) # consume entire buffer + if next_index == -1: # no valid start byte was found + return len(buffer) # consume entire buffer else: - return next_index # consume up to next index + return next_index # consume up to next index @staticmethod def _packet_size(header): @@ -155,10 +155,10 @@ def _unpack(buffer, header, errors, recovery_mode=False): header = None return payload, consumed except CorruptPacket as corrupt_packet: - if errors is 'ignore': + if errors == 'ignore': # find the next possible start byte in the buffer return Stateless._recovery_recurse(buffer, header, errors, True) - elif errors is 'strict': + elif errors == 'strict': raise corrupt_packet @staticmethod @@ -183,7 +183,6 @@ def _recovery_recurse(buffer, header, errors, consume_on_not_recovered): return payload, consumed + next_sb - class PackerBase(object): """ Packing is the same for stated and stateless. Therefore its implemented in this base class. @@ -227,8 +226,10 @@ def pack(payload): """ return Stateless._pack(payload) + def frame(bytestring): return Stateless.pack(bytestring) + def unframe(buffer, errors='ignore'): return Stateless.unpack(buffer, errors) diff --git a/pyvesc/protocol/packet/exceptions.py b/pyvesc/protocol/packet/exceptions.py index e916062..9c9316c 100644 --- a/pyvesc/protocol/packet/exceptions.py +++ b/pyvesc/protocol/packet/exceptions.py @@ -3,4 +3,4 @@ class CorruptPacket(ValueError): class InvalidPayload(ValueError): - pass \ No newline at end of file + pass diff --git a/pyvesc/protocol/packet/structure.py b/pyvesc/protocol/packet/structure.py index 3dc45a7..d609390 100644 --- a/pyvesc/protocol/packet/structure.py +++ b/pyvesc/protocol/packet/structure.py @@ -1,6 +1,6 @@ import collections import struct -from pyvesc.protocol.packet.exceptions import * +from .exceptions import InvalidPayload, CorruptPacket from crccheck.crc import CrcXmodem crc_checker = CrcXmodem() @@ -42,9 +42,9 @@ def fmt(start_byte): :param start_byte: The first byte in the buffer. :return: The character format of the packet header. """ - if start_byte is 0x2: + if start_byte == 0x2: return '>BB' - elif start_byte is 0x3: + elif start_byte == 0x3: return '>BH' else: raise CorruptPacket("Invalid start byte: %u" % start_byte) @@ -54,7 +54,7 @@ class Footer(collections.namedtuple('Footer', ['crc', 'terminator'])): """ Footer of a VESC packet. """ - TERMINATOR = 0x3 # Terminator character + TERMINATOR = 0x3 # Terminator character @staticmethod def parse(buffer, header): diff --git a/setup.py b/setup.py index edaa3de..9c145de 100644 --- a/setup.py +++ b/setup.py @@ -3,15 +3,15 @@ VERSION = '1.0.5' setup( - name='pyvesc', - packages=['pyvesc', 'pyvesc.protocol', 'pyvesc.protocol.packet', 'pyvesc.VESC', 'pyvesc.VESC.messages'], - version=VERSION, - description='Python implementation of the VESC communication protocol.', - author='Liam Bindle', - author_email='liambindle@gmail.com', - url='https://github.com/LiamBindle/PyVESC', - download_url='https://github.com/LiamBindle/PyVESC/tarball/' + VERSION, - keywords=['vesc', 'VESC', 'communication', 'protocol', 'packet'], - classifiers=[], - install_requires=['crccheck'] + name='pyvesc', + packages=['pyvesc', 'pyvesc.protocol', 'pyvesc.protocol.packet', 'pyvesc.VESC', 'pyvesc.VESC.messages'], + version=VERSION, + description='Python implementation of the VESC communication protocol.', + author='Liam Bindle', + author_email='liambindle@gmail.com', + url='https://github.com/LiamBindle/PyVESC', + download_url='https://github.com/LiamBindle/PyVESC/tarball/' + VERSION, + keywords=['vesc', 'VESC', 'communication', 'protocol', 'packet'], + classifiers=[], + install_requires=['crccheck'] ) diff --git a/test.py b/test.py index da630fa..246ad92 100644 --- a/test.py +++ b/test.py @@ -163,9 +163,9 @@ def test_corrupt_recovery(self): # make a good packet test_payload = b'Te!' good_packet = b'\x02\x03Te!B\x92\x03' - packet_to_recover = b'\x02\x04!\xe1$ 8\xbb\x03' # goal is to recover this packet + packet_to_recover = b'\x02\x04!\xe1$ 8\xbb\x03' # goal is to recover this packet payload_to_recover = b'!\xe1$ ' - after_goal = b'\x05\x09\x01' # mimic another corrupt packet after + after_goal = b'\x05\x09\x01' # mimic another corrupt packet after corrupt_packets = [] # corrupt first byte corrupt = b'\x01\x03Te!B\x92\x03' @@ -202,6 +202,7 @@ def test_corrupt_recovery(self): self.assertEqual(parsed, test_payload) self.assertEqual(out_buffer, b'') + class TestMsg(TestCase): def setUp(self): import copy @@ -218,7 +219,7 @@ def verify_packing_and_unpacking(self, msg): payload_bytestring = VESCMessage.pack(msg) parsed_msg = VESCMessage.unpack(payload_bytestring) self.assertEqual(parsed_msg.id, msg.id) - for name in [names[0] for names in msg.fields]: + for name in [names[0] for names in msg.send_fields]: self.assertEqual(getattr(parsed_msg, name), getattr(msg, name)) def test_single_message(self): @@ -226,7 +227,7 @@ def test_single_message(self): class TestMsg1(metaclass=VESCMessage): id = 0x12 - fields = [ + send_fields = [ ('f1', 'B'), ('f2', 'H'), ('f3', 'i'), @@ -243,7 +244,7 @@ def test_multiple_messages(self): class testMsg1(metaclass=VESCMessage): id = 0x45 - fields = [ + send_fields = [ ('f1', 'B'), ('f2', 'H'), ('f3', 'i'), @@ -254,21 +255,21 @@ class testMsg1(metaclass=VESCMessage): class testMsg2(metaclass=VESCMessage): id = 0x19 - fields = [ + send_fields = [ ('f1', 'B'), ('f2', 'B'), ] class testMsg3(metaclass=VESCMessage): id = 0x11 - fields = [ + send_fields = [ ('f1', 'i'), ('f2', 'i'), ] class testMsg4(metaclass=VESCMessage): id = 0x24 - fields = [ + send_fields = [ ('f1', 'i'), ('f2', 's'), ('f3', 'i'), @@ -293,16 +294,16 @@ def test_errors(self): # try to make two messages with the same ID class testMsg1(metaclass=VESCMessage): - id = 0x01 - fields = [ + id = 0x99 + send_fields = [ ('f1', 'H'), ('f2', 'H'), ] caught = False try: class testMsg2(metaclass=VESCMessage): - id = 0x01 - fields = [ + id = 0x99 + send_fields = [ ('f1', 'B'), ('f2', 'B'), ] @@ -315,7 +316,7 @@ class testMsg2(metaclass=VESCMessage): try: class testMsg4(testMsg1): id = 0x01 - fields = [ + send_fields = [ ('f1', 'B'), ('f2', 'B'), ] @@ -328,7 +329,7 @@ class testMsg4(testMsg1): try: class testMsg7(metaclass=VESCMessage): id = 0x02 - fields = [ + send_fields = [ ('f1', 's'), ('f2', 's'), ] @@ -341,7 +342,7 @@ class testMsg7(metaclass=VESCMessage): try: class testMsg8(metaclass=VESCMessage): id = 0x31 - fields = [ + send_fields = [ ('f1', 'p'), ] except TypeError as e: @@ -351,7 +352,7 @@ class testMsg8(metaclass=VESCMessage): # try to fill a message with the wrong number of arguments caught = False try: - testmessage1 = testMsg1(2, 4, 5) # should be 2 args + testmessage1 = testMsg1(2, 4, 5) # should be 2 args except AttributeError as e: caught = True self.assertTrue(caught) @@ -369,19 +370,19 @@ def tearDown(self): self._initial_registry = None def verify_encode_decode(self, msg): - import pyvesc - encoded = pyvesc.encode(msg) - decoded, consumed = pyvesc.decode(encoded) + from pyvesc.protocol.interface import encode, decode + encoded = encode(msg) + decoded, consumed, payload_total = decode(encoded) self.assertEqual(consumed, len(encoded)) - for field in msg._field_names: + for field in msg._send_field_names: self.assertEqual(getattr(msg, field), getattr(decoded, field)) def test_interface(self): - from pyvesc.VESCMotor.messages import VESCMessage + from pyvesc.protocol.base import VESCMessage class testMsg1(metaclass=VESCMessage): id = 0x45 - fields = [ + send_fields = [ ('f1', 'B'), ('f2', 'H'), ('f3', 'i'), @@ -392,21 +393,21 @@ class testMsg1(metaclass=VESCMessage): class testMsg2(metaclass=VESCMessage): id = 0x19 - fields = [ + send_fields = [ ('f1', 'B'), ('f2', 'B'), ] class testMsg3(metaclass=VESCMessage): id = 0x11 - fields = [ + send_fields = [ ('f1', 'i'), ('f2', 'i'), ] class testMsg4(metaclass=VESCMessage): id = 0x24 - fields = [ + send_fields = [ ('f1', 'i'), ('f2', 's'), ('f3', 'i'), diff --git a/unit_test_report.xml b/unit_test_report.xml new file mode 100644 index 0000000..f867c13 --- /dev/null +++ b/unit_test_report.xml @@ -0,0 +1 @@ + \ No newline at end of file From 662f16c97bfd3c4e64bfe81b8b230450d692640f Mon Sep 17 00:00:00 2001 From: Luke Dempsey Date: Tue, 10 Dec 2024 16:48:48 +1100 Subject: [PATCH 2/2] Cleanup unit test results --- .gitignore | 1 + unit_test_report.xml | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 100644 unit_test_report.xml diff --git a/.gitignore b/.gitignore index 3c32131..226cb3b 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ MANIFEST dist/ pyvesc.egg-info/ build/ +unit_test_report.xml diff --git a/unit_test_report.xml b/unit_test_report.xml deleted file mode 100644 index f867c13..0000000 --- a/unit_test_report.xml +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file