diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst index 106b1a22..d24de64f 100644 --- a/docs/protocol/reference.rst +++ b/docs/protocol/reference.rst @@ -126,6 +126,11 @@ Any system errors, as a bitmask Flags: - UNDERVOLTAGE - DRIVER_FAULT +- CHARGE_PUMP_FAULT_STAT +- CHARGE_PUMP_FAULT +- DRV10_DISABLE +- DRV32_DISABLE +- DRV54_DISABLE save_config() -> void @@ -158,44 +163,20 @@ Return Type: void Reset the device. -invoke_bootloader() -> void +enter_dfu() -> void ------------------------------------------------------------------- ID: 13 Return Type: void -Invoke the bootloader. - - -scheduler.total -------------------------------------------------------------------- - -ID: 14 -Type: uint32 - - -Total processor cycles in a single PWM cycle. - - - - -scheduler.busy -------------------------------------------------------------------- - -ID: 15 -Type: uint32 - - -Busy processor cycles in a single PWM cycle. - - +Enter DFU mode. scheduler.errors ------------------------------------------------------------------- -ID: 16 +ID: 14 Type: uint8 @@ -208,7 +189,7 @@ Flags: controller.state ------------------------------------------------------------------- -ID: 17 +ID: 15 Type: uint8 @@ -220,7 +201,7 @@ The state of the controller. controller.mode ------------------------------------------------------------------- -ID: 18 +ID: 16 Type: uint8 @@ -232,7 +213,7 @@ The control mode of the controller. controller.warnings ------------------------------------------------------------------- -ID: 19 +ID: 17 Type: uint8 @@ -247,7 +228,7 @@ Flags: controller.errors ------------------------------------------------------------------- -ID: 20 +ID: 18 Type: uint8 @@ -260,7 +241,7 @@ Flags: controller.position.setpoint ------------------------------------------------------------------- -ID: 21 +ID: 19 Type: float Units: tick @@ -272,7 +253,7 @@ The position setpoint. controller.position.p_gain ------------------------------------------------------------------- -ID: 22 +ID: 20 Type: float @@ -284,7 +265,7 @@ The proportional gain of the position controller. controller.velocity.setpoint ------------------------------------------------------------------- -ID: 23 +ID: 21 Type: float Units: tick / second @@ -296,7 +277,7 @@ The velocity setpoint. controller.velocity.limit ------------------------------------------------------------------- -ID: 24 +ID: 22 Type: float Units: tick / second @@ -308,7 +289,7 @@ The velocity limit. controller.velocity.p_gain ------------------------------------------------------------------- -ID: 25 +ID: 23 Type: float @@ -320,7 +301,7 @@ The proportional gain of the velocity controller. controller.velocity.i_gain ------------------------------------------------------------------- -ID: 26 +ID: 24 Type: float @@ -334,7 +315,7 @@ The integral gain of the velocity controller. controller.velocity.deadband ------------------------------------------------------------------- -ID: 27 +ID: 25 Type: float Units: tick @@ -346,7 +327,7 @@ The deadband of the velocity integrator. A region around the position setpoint w controller.velocity.increment ------------------------------------------------------------------- -ID: 28 +ID: 26 Type: float @@ -358,7 +339,7 @@ Max velocity setpoint increment (ramping) rate. Set to 0 to disable. controller.current.Iq_setpoint ------------------------------------------------------------------- -ID: 29 +ID: 27 Type: float Units: ampere @@ -370,7 +351,7 @@ The Iq setpoint. controller.current.Id_setpoint ------------------------------------------------------------------- -ID: 30 +ID: 28 Type: float Units: ampere @@ -382,7 +363,7 @@ The Id setpoint. controller.current.Iq_limit ------------------------------------------------------------------- -ID: 31 +ID: 29 Type: float Units: ampere @@ -394,7 +375,7 @@ The Iq limit. controller.current.Iq_estimate ------------------------------------------------------------------- -ID: 32 +ID: 30 Type: float Units: ampere @@ -406,7 +387,7 @@ The Iq estimate. controller.current.bandwidth ------------------------------------------------------------------- -ID: 33 +ID: 31 Type: float Units: hertz @@ -418,7 +399,7 @@ The current controller bandwidth. controller.current.Iq_p_gain ------------------------------------------------------------------- -ID: 34 +ID: 32 Type: float @@ -430,7 +411,7 @@ The current controller proportional gain. controller.current.max_Ibus_regen ------------------------------------------------------------------- -ID: 35 +ID: 33 Type: float Units: ampere @@ -442,7 +423,7 @@ The max current allowed to be fed back to the power source before flux braking a controller.current.max_Ibrake ------------------------------------------------------------------- -ID: 36 +ID: 34 Type: float Units: ampere @@ -454,7 +435,7 @@ The max current allowed to be dumped to the motor windings during flux braking. controller.voltage.Vq_setpoint ------------------------------------------------------------------- -ID: 37 +ID: 35 Type: float Units: volt @@ -466,7 +447,7 @@ The Vq setpoint. calibrate() -> void ------------------------------------------------------------------- -ID: 38 +ID: 36 Return Type: void @@ -476,7 +457,7 @@ Calibrate the device. idle() -> void ------------------------------------------------------------------- -ID: 39 +ID: 37 Return Type: void @@ -486,7 +467,7 @@ Set idle mode, disabling the driver. position_mode() -> void ------------------------------------------------------------------- -ID: 40 +ID: 38 Return Type: void @@ -496,7 +477,7 @@ Set position control mode. velocity_mode() -> void ------------------------------------------------------------------- -ID: 41 +ID: 39 Return Type: void @@ -506,7 +487,7 @@ Set velocity control mode. current_mode() -> void ------------------------------------------------------------------- -ID: 42 +ID: 40 Return Type: void @@ -516,7 +497,7 @@ Set current control mode. set_pos_vel_setpoints(pos_setpoint, vel_setpoint) -> float ------------------------------------------------------------------- -ID: 43 +ID: 41 Return Type: float @@ -528,7 +509,7 @@ Set the position and velocity setpoints in one go, and retrieve the position est comms.can.rate ------------------------------------------------------------------- -ID: 44 +ID: 42 Type: uint32 @@ -540,7 +521,7 @@ The baud rate of the CAN interface. comms.can.id ------------------------------------------------------------------- -ID: 45 +ID: 43 Type: uint32 @@ -552,7 +533,7 @@ The ID of the CAN interface. motor.R ------------------------------------------------------------------- -ID: 46 +ID: 44 Type: float Units: ohm @@ -564,7 +545,7 @@ The motor Resistance value. motor.L ------------------------------------------------------------------- -ID: 47 +ID: 45 Type: float Units: henry @@ -576,7 +557,7 @@ The motor Inductance value. motor.pole_pairs ------------------------------------------------------------------- -ID: 48 +ID: 46 Type: uint8 @@ -588,7 +569,7 @@ The motor pole pair count. motor.type ------------------------------------------------------------------- -ID: 49 +ID: 47 Type: uint8 @@ -602,7 +583,7 @@ Options: motor.offset ------------------------------------------------------------------- -ID: 50 +ID: 48 Type: float @@ -614,7 +595,7 @@ User-defined offset of the motor. motor.direction ------------------------------------------------------------------- -ID: 51 +ID: 49 Type: int8 @@ -626,7 +607,7 @@ User-defined direction of the motor. motor.calibrated ------------------------------------------------------------------- -ID: 52 +ID: 50 Type: bool @@ -638,7 +619,7 @@ Whether the motor has been calibrated. motor.I_cal ------------------------------------------------------------------- -ID: 53 +ID: 51 Type: float Units: ampere @@ -650,7 +631,7 @@ The calibration current. motor.errors ------------------------------------------------------------------- -ID: 54 +ID: 52 Type: uint8 @@ -665,7 +646,7 @@ Flags: encoder.position_estimate ------------------------------------------------------------------- -ID: 55 +ID: 53 Type: float Units: tick @@ -677,7 +658,7 @@ The filtered encoder position estimate. encoder.velocity_estimate ------------------------------------------------------------------- -ID: 56 +ID: 54 Type: float Units: tick / second @@ -689,7 +670,7 @@ The filtered encoder velocity estimate. encoder.type ------------------------------------------------------------------- -ID: 57 +ID: 55 Type: uint8 @@ -703,7 +684,7 @@ Options: encoder.bandwidth ------------------------------------------------------------------- -ID: 58 +ID: 56 Type: float Units: hertz @@ -715,7 +696,7 @@ The encoder observer bandwidth. encoder.calibrated ------------------------------------------------------------------- -ID: 59 +ID: 57 Type: bool @@ -727,7 +708,7 @@ Whether the encoder has been calibrated. encoder.errors ------------------------------------------------------------------- -ID: 60 +ID: 58 Type: uint8 @@ -741,7 +722,7 @@ Flags: traj_planner.max_accel ------------------------------------------------------------------- -ID: 61 +ID: 59 Type: float Units: tick / second @@ -753,7 +734,7 @@ The max allowed acceleration of the generated trajectory. traj_planner.max_decel ------------------------------------------------------------------- -ID: 62 +ID: 60 Type: float Units: tick / second ** 2 @@ -765,7 +746,7 @@ The max allowed deceleration of the generated trajectory. traj_planner.max_vel ------------------------------------------------------------------- -ID: 63 +ID: 61 Type: float Units: tick / second @@ -777,7 +758,7 @@ The max allowed cruise velocity of the generated trajectory. traj_planner.t_accel ------------------------------------------------------------------- -ID: 64 +ID: 62 Type: float Units: second @@ -789,7 +770,7 @@ In time mode, the acceleration time of the generated trajectory. traj_planner.t_decel ------------------------------------------------------------------- -ID: 65 +ID: 63 Type: float Units: second @@ -801,7 +782,7 @@ In time mode, the deceleration time of the generated trajectory. traj_planner.t_total ------------------------------------------------------------------- -ID: 66 +ID: 64 Type: float Units: second @@ -813,7 +794,7 @@ In time mode, the total time of the generated trajectory. move_to(pos_setpoint) -> void ------------------------------------------------------------------- -ID: 67 +ID: 65 Return Type: void @@ -823,7 +804,7 @@ Move to target position respecting velocity and acceleration limits. move_to_tlimit(pos_setpoint) -> void ------------------------------------------------------------------- -ID: 68 +ID: 66 Return Type: void @@ -833,7 +814,7 @@ Move to target position respecting time limits for each sector. traj_planner.errors ------------------------------------------------------------------- -ID: 69 +ID: 67 Type: uint8 @@ -847,7 +828,7 @@ Flags: homing.velocity ------------------------------------------------------------------- -ID: 70 +ID: 68 Type: float Units: tick / second @@ -859,7 +840,7 @@ The velocity at which the motor performs homing. homing.max_homing_t ------------------------------------------------------------------- -ID: 71 +ID: 69 Type: float Units: second @@ -871,7 +852,7 @@ The maximum time the motor is allowed to travel before homing times out and abor homing.retract_dist ------------------------------------------------------------------- -ID: 72 +ID: 70 Type: float Units: tick @@ -883,7 +864,7 @@ The retraction distance the motor travels after the endstop has been found. homing.warnings ------------------------------------------------------------------- -ID: 73 +ID: 71 Type: uint8 @@ -896,7 +877,7 @@ Flags: homing.stall_detect.velocity ------------------------------------------------------------------- -ID: 74 +ID: 72 Type: float Units: tick / second @@ -908,7 +889,7 @@ The velocity below which (and together with `stall_detect.delta_pos`) stall dete homing.stall_detect.delta_pos ------------------------------------------------------------------- -ID: 75 +ID: 73 Type: float Units: tick @@ -920,7 +901,7 @@ The velocity below which (and together with `stall_detect.delta_pos`) stall dete homing.stall_detect.t ------------------------------------------------------------------- -ID: 76 +ID: 74 Type: float Units: second @@ -932,7 +913,7 @@ The time to remain in stall detection mode before the motor is considered stalle home() -> void ------------------------------------------------------------------- -ID: 77 +ID: 75 Return Type: void @@ -942,7 +923,7 @@ Perform the homing operation. watchdog.enabled ------------------------------------------------------------------- -ID: 78 +ID: 76 Type: bool @@ -954,7 +935,7 @@ Whether the watchdog is enabled or not. watchdog.triggered ------------------------------------------------------------------- -ID: 79 +ID: 77 Type: bool @@ -966,7 +947,7 @@ Whether the watchdog has been triggered or not. watchdog.timeout ------------------------------------------------------------------- -ID: 80 +ID: 78 Type: float Units: second diff --git a/firmware/bootloader/bootloader_M51.bin b/firmware/bootloader/bootloader_M51.bin index c559ca3d..b99bde8a 100755 Binary files a/firmware/bootloader/bootloader_M51.bin and b/firmware/bootloader/bootloader_M51.bin differ diff --git a/firmware/bootloader/bootloader_R32.bin b/firmware/bootloader/bootloader_R32.bin index 4c5eb57e..064c691d 100755 Binary files a/firmware/bootloader/bootloader_R32.bin and b/firmware/bootloader/bootloader_R32.bin differ diff --git a/firmware/bootloader/bootloader_R33.bin b/firmware/bootloader/bootloader_R33.bin index 3729fdc2..246d9e96 100755 Binary files a/firmware/bootloader/bootloader_R33.bin and b/firmware/bootloader/bootloader_R33.bin differ diff --git a/firmware/bootloader/bootloader_R50.bin b/firmware/bootloader/bootloader_R50.bin index 10235711..02945894 100755 Binary files a/firmware/bootloader/bootloader_R50.bin and b/firmware/bootloader/bootloader_R50.bin differ diff --git a/firmware/bootloader/bootloader_R51.bin b/firmware/bootloader/bootloader_R51.bin index 68b07f74..62b4cbb1 100755 Binary files a/firmware/bootloader/bootloader_R51.bin and b/firmware/bootloader/bootloader_R51.bin differ diff --git a/firmware/bootloader/bootloader_R52.bin b/firmware/bootloader/bootloader_R52.bin index a45a5359..db2df6a0 100755 Binary files a/firmware/bootloader/bootloader_R52.bin and b/firmware/bootloader/bootloader_R52.bin differ diff --git a/firmware/src/can/can.c b/firmware/src/can/can.c index d8fc77cb..512f5212 100644 --- a/firmware/src/can/can.c +++ b/firmware/src/can/can.c @@ -161,7 +161,7 @@ void CAN_restore_config(CANConfig *config_) config = *config_; } -void CAN_task(void) { +void CAN_update(void) { // Transmit heartbeat const uint32_t msg_diff = msTicks - state.last_msg_ms; if (msg_diff >= config.heartbeat_period && PAC55XX_CAN->SR.TBS != 0) diff --git a/firmware/src/can/can.h b/firmware/src/can/can.h index a218bab5..4b56340a 100644 --- a/firmware/src/can/can.h +++ b/firmware/src/can/can.h @@ -40,4 +40,4 @@ void CAN_process_interrupt(void); CANConfig *CAN_get_config(void); void CAN_restore_config(CANConfig *config_); -void CAN_task(void); +void CAN_update(void); diff --git a/firmware/src/can/can_endpoints.c b/firmware/src/can/can_endpoints.c index 97f4ad10..58e478dc 100644 --- a/firmware/src/can/can_endpoints.c +++ b/firmware/src/can/can_endpoints.c @@ -18,8 +18,8 @@ #include -uint8_t (*avlos_endpoints[81])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) = {&avlos_protocol_hash, &avlos_uid, &avlos_fw_version, &avlos_hw_revision, &avlos_Vbus, &avlos_Ibus, &avlos_power, &avlos_temp, &avlos_calibrated, &avlos_errors, &avlos_save_config, &avlos_erase_config, &avlos_reset, &avlos_invoke_bootloader, &avlos_scheduler_total, &avlos_scheduler_busy, &avlos_scheduler_errors, &avlos_controller_state, &avlos_controller_mode, &avlos_controller_warnings, &avlos_controller_errors, &avlos_controller_position_setpoint, &avlos_controller_position_p_gain, &avlos_controller_velocity_setpoint, &avlos_controller_velocity_limit, &avlos_controller_velocity_p_gain, &avlos_controller_velocity_i_gain, &avlos_controller_velocity_deadband, &avlos_controller_velocity_increment, &avlos_controller_current_Iq_setpoint, &avlos_controller_current_Id_setpoint, &avlos_controller_current_Iq_limit, &avlos_controller_current_Iq_estimate, &avlos_controller_current_bandwidth, &avlos_controller_current_Iq_p_gain, &avlos_controller_current_max_Ibus_regen, &avlos_controller_current_max_Ibrake, &avlos_controller_voltage_Vq_setpoint, &avlos_controller_calibrate, &avlos_controller_idle, &avlos_controller_position_mode, &avlos_controller_velocity_mode, &avlos_controller_current_mode, &avlos_controller_set_pos_vel_setpoints, &avlos_comms_can_rate, &avlos_comms_can_id, &avlos_motor_R, &avlos_motor_L, &avlos_motor_pole_pairs, &avlos_motor_type, &avlos_motor_offset, &avlos_motor_direction, &avlos_motor_calibrated, &avlos_motor_I_cal, &avlos_motor_errors, &avlos_encoder_position_estimate, &avlos_encoder_velocity_estimate, &avlos_encoder_type, &avlos_encoder_bandwidth, &avlos_encoder_calibrated, &avlos_encoder_errors, &avlos_traj_planner_max_accel, &avlos_traj_planner_max_decel, &avlos_traj_planner_max_vel, &avlos_traj_planner_t_accel, &avlos_traj_planner_t_decel, &avlos_traj_planner_t_total, &avlos_traj_planner_move_to, &avlos_traj_planner_move_to_tlimit, &avlos_traj_planner_errors, &avlos_homing_velocity, &avlos_homing_max_homing_t, &avlos_homing_retract_dist, &avlos_homing_warnings, &avlos_homing_stall_detect_velocity, &avlos_homing_stall_detect_delta_pos, &avlos_homing_stall_detect_t, &avlos_homing_home, &avlos_watchdog_enabled, &avlos_watchdog_triggered, &avlos_watchdog_timeout }; -uint32_t avlos_proto_hash = 4244018107; +uint8_t (*avlos_endpoints[79])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) = {&avlos_protocol_hash, &avlos_uid, &avlos_fw_version, &avlos_hw_revision, &avlos_Vbus, &avlos_Ibus, &avlos_power, &avlos_temp, &avlos_calibrated, &avlos_errors, &avlos_save_config, &avlos_erase_config, &avlos_reset, &avlos_enter_dfu, &avlos_scheduler_errors, &avlos_controller_state, &avlos_controller_mode, &avlos_controller_warnings, &avlos_controller_errors, &avlos_controller_position_setpoint, &avlos_controller_position_p_gain, &avlos_controller_velocity_setpoint, &avlos_controller_velocity_limit, &avlos_controller_velocity_p_gain, &avlos_controller_velocity_i_gain, &avlos_controller_velocity_deadband, &avlos_controller_velocity_increment, &avlos_controller_current_Iq_setpoint, &avlos_controller_current_Id_setpoint, &avlos_controller_current_Iq_limit, &avlos_controller_current_Iq_estimate, &avlos_controller_current_bandwidth, &avlos_controller_current_Iq_p_gain, &avlos_controller_current_max_Ibus_regen, &avlos_controller_current_max_Ibrake, &avlos_controller_voltage_Vq_setpoint, &avlos_controller_calibrate, &avlos_controller_idle, &avlos_controller_position_mode, &avlos_controller_velocity_mode, &avlos_controller_current_mode, &avlos_controller_set_pos_vel_setpoints, &avlos_comms_can_rate, &avlos_comms_can_id, &avlos_motor_R, &avlos_motor_L, &avlos_motor_pole_pairs, &avlos_motor_type, &avlos_motor_offset, &avlos_motor_direction, &avlos_motor_calibrated, &avlos_motor_I_cal, &avlos_motor_errors, &avlos_encoder_position_estimate, &avlos_encoder_velocity_estimate, &avlos_encoder_type, &avlos_encoder_bandwidth, &avlos_encoder_calibrated, &avlos_encoder_errors, &avlos_traj_planner_max_accel, &avlos_traj_planner_max_decel, &avlos_traj_planner_max_vel, &avlos_traj_planner_t_accel, &avlos_traj_planner_t_decel, &avlos_traj_planner_t_total, &avlos_traj_planner_move_to, &avlos_traj_planner_move_to_tlimit, &avlos_traj_planner_errors, &avlos_homing_velocity, &avlos_homing_max_homing_t, &avlos_homing_retract_dist, &avlos_homing_warnings, &avlos_homing_stall_detect_velocity, &avlos_homing_stall_detect_delta_pos, &avlos_homing_stall_detect_t, &avlos_homing_home, &avlos_watchdog_enabled, &avlos_watchdog_triggered, &avlos_watchdog_timeout }; +uint32_t avlos_proto_hash = 4118115615; uint32_t _avlos_get_proto_hash(void) { @@ -164,42 +164,18 @@ uint8_t avlos_reset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) return AVLOS_RET_CALL; } -uint8_t avlos_invoke_bootloader(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) +uint8_t avlos_enter_dfu(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) { - system_invoke_bootloader(); + system_enter_dfu(); return AVLOS_RET_CALL; } -uint8_t avlos_scheduler_total(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) -{ -if (AVLOS_CMD_READ == cmd) { - uint32_t v; - v = Scheduler_GetTotalCycles(); - *buffer_len = sizeof(v); - memcpy(buffer, &v, sizeof(v)); - return AVLOS_RET_READ; - } - return AVLOS_RET_NOACTION; -} - -uint8_t avlos_scheduler_busy(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) -{ -if (AVLOS_CMD_READ == cmd) { - uint32_t v; - v = Scheduler_GetBusyCycles(); - *buffer_len = sizeof(v); - memcpy(buffer, &v, sizeof(v)); - return AVLOS_RET_READ; - } - return AVLOS_RET_NOACTION; -} - uint8_t avlos_scheduler_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) { if (AVLOS_CMD_READ == cmd) { uint8_t v; - v = controller_get_errors(); + v = scheduler_get_errors(); *buffer_len = sizeof(v); memcpy(buffer, &v, sizeof(v)); return AVLOS_RET_READ; diff --git a/firmware/src/can/can_endpoints.h b/firmware/src/can/can_endpoints.h index 5e630039..0f773a13 100644 --- a/firmware/src/can/can_endpoints.h +++ b/firmware/src/can/can_endpoints.h @@ -27,7 +27,12 @@ typedef enum { ERRORS_NONE = 0, ERRORS_UNDERVOLTAGE = (1 << 0), - ERRORS_DRIVER_FAULT = (1 << 1) + ERRORS_DRIVER_FAULT = (1 << 1), + ERRORS_CHARGE_PUMP_FAULT_STAT = (1 << 2), + ERRORS_CHARGE_PUMP_FAULT = (1 << 3), + ERRORS_DRV10_DISABLE = (1 << 4), + ERRORS_DRV32_DISABLE = (1 << 5), + ERRORS_DRV54_DISABLE = (1 << 6) } errors_flags; typedef enum @@ -91,7 +96,7 @@ typedef enum } encoder_type_options; extern uint32_t avlos_proto_hash; -extern uint8_t (*avlos_endpoints[81])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); +extern uint8_t (*avlos_endpoints[79])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); extern uint32_t _avlos_get_proto_hash(void); /* @@ -225,34 +230,14 @@ uint8_t avlos_erase_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command uint8_t avlos_reset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); /* -* avlos_invoke_bootloader +* avlos_enter_dfu * -* Invoke the bootloader. +* Enter DFU mode. * * @param buffer * @param buffer_len */ -uint8_t avlos_invoke_bootloader(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); - -/* -* avlos_scheduler_total -* -* Total processor cycles in a single PWM cycle. -* -* @param buffer -* @param buffer_len -*/ -uint8_t avlos_scheduler_total(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); - -/* -* avlos_scheduler_busy -* -* Busy processor cycles in a single PWM cycle. -* -* @param buffer -* @param buffer_len -*/ -uint8_t avlos_scheduler_busy(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); +uint8_t avlos_enter_dfu(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); /* * avlos_scheduler_errors diff --git a/firmware/src/config.h b/firmware/src/config.h index ad2f5f69..ab0bd536 100644 --- a/firmware/src/config.h +++ b/firmware/src/config.h @@ -27,8 +27,9 @@ // Timer clock divider #define TXCTL_PS_DIV TXCTL_PS_DIV2 -// Desired PWM Frequency (Hz). +// PWM and Systick frequency #define PWM_FREQ_HZ (20000) +#define SYSTICK_FREQ_HZ (1000) // Control parameters #define PWM_LIMIT (0.8f) diff --git a/firmware/src/controller/controller.c b/firmware/src/controller/controller.c index 7a671884..20853127 100644 --- a/firmware/src/controller/controller.c +++ b/firmware/src/controller/controller.c @@ -124,11 +124,11 @@ void Controller_ControlLoop(void) reset_calibration(); if (ENCODER_MA7XX == encoder_get_type()) { - (void)((CalibrateResistance() && CalibrateInductance()) && CalibrateDirectionAndPolePairs() && calibrate_offset_and_rectification()); + (void)((CalibrateADCOffset() && CalibrateResistance() && CalibrateInductance()) && CalibrateDirectionAndPolePairs() && calibrate_offset_and_rectification()); } else if (ENCODER_HALL == encoder_get_type()) { - (void)((CalibrateResistance() && CalibrateInductance()) && calibrate_hall_sequence()); + (void)((CalibrateADCOffset() && CalibrateResistance() && CalibrateInductance()) && calibrate_hall_sequence()); } state.is_calibrating = false; controller_set_state(STATE_IDLE); diff --git a/firmware/src/gatedriver/gatedriver.c b/firmware/src/gatedriver/gatedriver.c index eaf96832..0572390c 100644 --- a/firmware/src/gatedriver/gatedriver.c +++ b/firmware/src/gatedriver/gatedriver.c @@ -19,13 +19,6 @@ #include #include -struct GateDriver_ gateDriver = -{ - .state = GATEDRIVER_DISABLED -}; - -void GateDriver_Init(void) {} - TM_RAMFUNC void gate_driver_enable(void) { // Select PWMA peripheral for Port B @@ -47,7 +40,6 @@ TM_RAMFUNC void gate_driver_enable(void) pac5xxx_tile_register_write(ADDR_CFGDRV4, pac5xxx_tile_register_read(ADDR_CFGDRV4) | 0x1); // BBM is bit 0 - gateDriver.state = GATEDRIVER_ENABLED; } TM_RAMFUNC void gate_driver_disable(void) @@ -66,12 +58,11 @@ TM_RAMFUNC void gate_driver_disable(void) // Turn on output enables PAC55XX_GPIOB->OUTMASK.w = 0x00; - gateDriver.state = GATEDRIVER_DISABLED; } TM_RAMFUNC bool gate_driver_is_enabled(void) { - return (GATEDRIVER_ENABLED == gateDriver.state); + return ((pac5xxx_tile_register_read(ADDR_ENDRV) & 0x1) == 1); } TM_RAMFUNC void gate_driver_set_duty_cycle(const FloatTriplet *dutycycles) diff --git a/firmware/src/gatedriver/gatedriver.h b/firmware/src/gatedriver/gatedriver.h index a70f65dc..388f73ee 100644 --- a/firmware/src/gatedriver/gatedriver.h +++ b/firmware/src/gatedriver/gatedriver.h @@ -18,17 +18,7 @@ #ifndef GATEDRIVER_GATEDRIVER_H_ #define GATEDRIVER_GATEDRIVER_H_ -typedef enum { - GATEDRIVER_DISABLED = 0, - GATEDRIVER_ENABLED = 1 -} GateDriverState; -struct GateDriver_ -{ - GateDriverState state; -}; - -void GateDriver_Init(void); void gate_driver_enable(void); void gate_driver_disable(void); bool gate_driver_is_enabled(void); diff --git a/firmware/src/main.c b/firmware/src/main.c index d0704fd2..4a96126f 100644 --- a/firmware/src/main.c +++ b/firmware/src/main.c @@ -39,7 +39,6 @@ int main(void) UART_Init(); observer_init(); ADC_Init(); - GateDriver_Init(); CAN_init(); Timer_Init(); Watchdog_init(); diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c index eee8b20d..e1249f28 100644 --- a/firmware/src/motor/calibration.c +++ b/firmware/src/motor/calibration.c @@ -30,6 +30,14 @@ static inline void set_epos_and_wait(float angle, float I_setpoint); static inline void wait_a_while(void); +bool CalibrateADCOffset(void) +{ + // We only need to wait here, the ADC loop will + // perform the offset calibration automatically + wait_a_while(); + return true; +} + bool CalibrateResistance(void) { if (!motor_get_is_gimbal()) diff --git a/firmware/src/motor/calibration.h b/firmware/src/motor/calibration.h index 23be4c27..7b442b4d 100644 --- a/firmware/src/motor/calibration.h +++ b/firmware/src/motor/calibration.h @@ -34,6 +34,7 @@ #define CAL_V_INDUCTANCE (5.0f) #endif +bool CalibrateADCOffset(void); bool CalibrateResistance(void); bool CalibrateInductance(void); bool CalibrateDirectionAndPolePairs(void); diff --git a/firmware/src/scheduler/scheduler.c b/firmware/src/scheduler/scheduler.c index 43018af0..6082b1a5 100644 --- a/firmware/src/scheduler/scheduler.c +++ b/firmware/src/scheduler/scheduler.c @@ -63,7 +63,6 @@ void WaitForControlLoopInterrupt(void) else { state.busy = false; - state.busy_cycles = DWT->CYCCNT - state.busy_loop_start; // Go back to sleep __DSB(); __ISB(); @@ -72,7 +71,6 @@ void WaitForControlLoopInterrupt(void) } state.busy = true; state.adc_interrupt = false; - state.busy_loop_start = DWT->CYCCNT; // We have to service the control loop by updating // current measurements and encoder estimates. if (ENCODER_MA7XX == encoder_get_type()) @@ -80,7 +78,7 @@ void WaitForControlLoopInterrupt(void) ma7xx_send_angle_cmd(); } ADC_update(); - system_update(); + encoder_update(true); observer_update(); // At this point control is returned to main loop. @@ -103,10 +101,6 @@ void ADC_IRQHandler(void) { state.adc_interrupt = true; } - - const uint32_t current_timestamp = DWT->CYCCNT; - state.total_cycles = current_timestamp - state.total_loop_start; - state.total_loop_start = current_timestamp; } void CAN_IRQHandler(void) @@ -118,7 +112,8 @@ void CAN_IRQHandler(void) void SysTick_Handler(void) { msTicks = msTicks + 1; - CAN_task(); + CAN_update(); + system_update(); } void UART_ReceiveMessageHandler(void) @@ -134,16 +129,6 @@ void Wdt_IRQHandler(void) PAC55XX_WWDT->WWDTFLAG.IF = 1; } -TM_RAMFUNC uint32_t Scheduler_GetTotalCycles(void) -{ - return state.total_cycles; -} - -TM_RAMFUNC uint32_t Scheduler_GetBusyCycles(void) -{ - return state.busy_cycles; -} - TM_RAMFUNC uint8_t scheduler_get_errors(void) { return state.errors; diff --git a/firmware/src/scheduler/scheduler.h b/firmware/src/scheduler/scheduler.h index 542d86e3..e4af643d 100644 --- a/firmware/src/scheduler/scheduler.h +++ b/firmware/src/scheduler/scheduler.h @@ -26,15 +26,8 @@ typedef struct bool busy; uint8_t errors; - uint32_t busy_cycles; - uint32_t total_cycles; - uint32_t busy_loop_start; - uint32_t total_loop_start; } SchedulerState; void WaitForControlLoopInterrupt(void); -uint32_t Scheduler_GetTotalCycles(void); -uint32_t Scheduler_GetBusyCycles(void); - uint8_t scheduler_get_errors(void); diff --git a/firmware/src/system/system.c b/firmware/src/system/system.c index 1351dba2..4f7ad078 100644 --- a/firmware/src/system/system.c +++ b/firmware/src/system/system.c @@ -82,14 +82,9 @@ void system_init(void) // Vp = 10V , 440mA-540mA, Charge Pump Enable pac5xxx_tile_register_write(ADDR_SYSCONF, 0x01); - // Ensure ADC GP0 register is zero, to bypass bootloader on next boot + // Ensure ADC GP0 register is zero, to bypass DFU mode on next boot pac5xxx_tile_register_write(ADDR_GP0, 0); - // Configure reporting of mcu cycles - CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; - DWT->CYCCNT = 0; - DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; - // Configure error handling SCB->CCR |= 0x10; @@ -97,7 +92,7 @@ void system_init(void) state.Vbus = 12.0f; // Derive VBus D value for given tau value - config.Vbus_D = 1.0f - powf(EPSILON, -1.0f / (config.Vbus_tau * PWM_FREQ_HZ)); + config.Vbus_D = 1.0f - powf(EPSILON, -1.0f / (config.Vbus_tau * SYSTICK_FREQ_HZ)); /* Initialize Systick per 1ms */ SysTick_Config(150000); // TODO: Use var @@ -110,6 +105,18 @@ TM_RAMFUNC void system_update(void) { state.errors |= ERRORS_UNDERVOLTAGE; } + const uint8_t drv_status = pac5xxx_tile_register_read(ADDR_STATDRV); + if (drv_status > 0) + { + state.errors |= ((drv_status & 0x7) << 4); + } + const uint8_t drv_fault = pac5xxx_tile_register_read(ADDR_DRV_FLT); + if (drv_fault > 0) + { + // We use 0x5 mask because we ignore CHARGE_PUMP_FAULT_STAT + // for the time being, as it seems to always be set. + state.errors |= ((drv_fault & 0x5) << 1); + } } void system_reset(void) @@ -118,7 +125,7 @@ void system_reset(void) NVIC_SystemReset(); } -void system_invoke_bootloader(void) +void system_enter_dfu(void) { pac5xxx_tile_register_write(ADDR_GP0, BTL_TRIGGER_PATTERN); NVIC_SystemReset(); diff --git a/firmware/src/system/system.h b/firmware/src/system/system.h index 598ad7c4..91bf0497 100644 --- a/firmware/src/system/system.h +++ b/firmware/src/system/system.h @@ -36,7 +36,7 @@ typedef struct { void system_init(void); void system_update(void); void system_reset(void); -void system_invoke_bootloader(void); +void system_enter_dfu(void); inline uint8_t system_get_fw_version_string(char *buffer) { diff --git a/studio/Python/Manifest.in b/studio/Python/Manifest.in index 97299015..95c364bd 100644 --- a/studio/Python/Manifest.in +++ b/studio/Python/Manifest.in @@ -7,4 +7,5 @@ include tinymovr/resources/icons/empty.png include tinymovr/resources/icons/empty@2x.png include tinymovr/resources/icons/empty_dark.png include tinymovr/resources/icons/empty_dark@2x.png -include tinymovr/config/device.yaml +include tinymovr/specs/device.yaml +include tinymovr/specs/dfu.yaml diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py index 589d6243..16464edb 100644 --- a/studio/Python/tests/test_board.py +++ b/studio/Python/tests/test_board.py @@ -191,13 +191,13 @@ def test_g_limits(self): self.tm.controller.velocity.setpoint = 0 time.sleep(0.5) - def test_h_timings(self): - """ - Test DWT busy/total cycle timings - """ - self.assertGreater(self.tm.scheduler.total, 0) - self.assertGreater(self.tm.scheduler.busy, 0) - self.assertLess(self.tm.scheduler.busy, 3000) + # def test_h_timings(self): + # """ + # Test DWT busy/total cycle timings + # """ + # self.assertGreater(self.tm.scheduler.total, 0) + # self.assertGreater(self.tm.scheduler.busy, 0) + # self.assertLess(self.tm.scheduler.busy, 3000) def test_i_states(self): """ diff --git a/studio/Python/tests/test_bootloader.py b/studio/Python/tests/test_dfu.py similarity index 81% rename from studio/Python/tests/test_bootloader.py rename to studio/Python/tests/test_dfu.py index e62f34c3..19a4567e 100644 --- a/studio/Python/tests/test_bootloader.py +++ b/studio/Python/tests/test_dfu.py @@ -1,5 +1,5 @@ """ -Tinymovr Bootloader Test Class +Tinymovr DFU Test Class Copyright Ioannis Chatzikonstantinou 2020-2023 Implements convenience functionality. @@ -23,8 +23,7 @@ from tinymovr.config import ( get_bus_config, create_device, - tinymovr_definition, - bl_definition, + definitions ) import unittest @@ -38,17 +37,17 @@ def setUp(cls): params["bitrate"] = 1000000 cls.can_bus = can.Bus(**params) - def test_bootloader(self, node_id=1): + def test_dfu(self, node_id=1): init_tee(self.can_bus) - tm = create_device(node_id=node_id, device_definition=tinymovr_definition) + tm = create_device(node_id=node_id) tm_hash = tm.protocol_hash - tm.invoke_bootloader() + tm.enter_dfu() time.sleep(1) - bl = create_device(node_id=node_id, device_definition=bl_definition) + bl = create_device(node_id=node_id) bl_hash = bl.protocol_hash bl.reset() time.sleep(0.1) - tm = create_device(node_id=node_id, device_definition=tinymovr_definition) + tm = create_device(node_id=node_id) tm_hash2 = tm.protocol_hash tm.reset() time.sleep(0.1) diff --git a/studio/Python/tinymovr/cli.py b/studio/Python/tinymovr/cli.py index 21821142..df5f3303 100644 --- a/studio/Python/tinymovr/cli.py +++ b/studio/Python/tinymovr/cli.py @@ -65,16 +65,16 @@ def spawn(): user_ns["tms"] = tms def node_appeared(node, node_id): - node_name = "{}{}".format(base_node_name, node_id) - print("Found {} with device id {}".format(node_name, node.uid)) + display_name = "{}{}".format(node.name, node_id) + print("Found {} with device uid {}".format(display_name, node.uid)) tms[node_id] = node - user_ns[node_name] = node + user_ns[display_name] = node def node_disappeared(node_id): - node_name = "{}{}".format(base_node_name, node_id) - print("Lost {}".format(node_name)) + display_name = "{}{}".format(tms[node_id].name, node_id) + print("Lost {}".format(display_name)) + del user_ns[display_name] del tms[node_id] - del user_ns[node_name] print(app_name + " " + str(version)) diff --git a/studio/Python/tinymovr/config/__init__.py b/studio/Python/tinymovr/config/__init__.py index 8a642ab6..f48375b6 100644 --- a/studio/Python/tinymovr/config/__init__.py +++ b/studio/Python/tinymovr/config/__init__.py @@ -1,8 +1,7 @@ from tinymovr.config.config import ( get_bus_config, configure_logging, - tinymovr_definition, - bl_definition, + definitions, create_device, create_device_with_hash_msg, ProtocolVersionError, diff --git a/studio/Python/tinymovr/config/config.py b/studio/Python/tinymovr/config/config.py index 87bdbeec..91dc61d0 100644 --- a/studio/Python/tinymovr/config/config.py +++ b/studio/Python/tinymovr/config/config.py @@ -16,6 +16,7 @@ """ import yaml +from pathlib import Path import logging from importlib_resources import files import can @@ -24,14 +25,14 @@ from tinymovr.codec import DataType from tinymovr.channel import CANChannel -tinymovr_definition = None -bl_definition = None +definitions = {"hash_uint32": {}, "name": {}} -with open(str(files("tinymovr").joinpath("config/device.yaml"))) as def_raw: - tinymovr_definition = yaml.safe_load(def_raw) - -with open(str(files("tinymovr").joinpath("config/bl.yaml"))) as def_raw: - bl_definition = yaml.safe_load(def_raw) +for yaml_file in Path(files("tinymovr").joinpath("specs/")).glob("*.yaml"): + with open(str(yaml_file)) as def_raw: + definition = yaml.safe_load(def_raw) + tmp_node = deserialize(definition) + definitions["hash_uint32"][tmp_node.hash_uint32] = definition + definitions["name"][definition["name"]] = definition class ProtocolVersionError(Exception): @@ -62,39 +63,56 @@ def get_bus_config(suggested_types=None): raise can.CanInitializationError("No active interface found") from exc -def create_device(node_id, device_definition=tinymovr_definition): +def create_device(node_id): """ Create a device with the defined ID. The hash value will be retrieved from the remote. """ chan = CANChannel(node_id) + + # Temporarily using a default definition to get the protocol_hash + # This assumes that `protocol_hash` is standard across different definitions + # Get the first definition as a temp + tmp_definition = list(definitions["hash_uint32"].values())[0] + node = deserialize(tmp_definition) + node._channel = chan + + # Check for the correct definition using the remote hash + protocol_hash = node.protocol_hash + device_definition = definitions["hash_uint32"].get(protocol_hash) + + if not device_definition: + raise ValueError(f"No device definition found for hash {protocol_hash}.") + node = deserialize(device_definition) - # We use the generated node to retrieve the hash from - # the remote. This is ok as long as we know that the - # hash endpoint will always be the 0th one. If there - # is a hash mismatch, we raise an exception, otherwise - # we return the device node as is. node._channel = chan - # hash_uint32 is local, proto_hash is remote - if node.hash_uint32 != node.protocol_hash: + if node.hash_uint32 != protocol_hash: raise ProtocolVersionError(node_id, "") + return node -def create_device_with_hash_msg(heartbeat_msg, device_definition=tinymovr_definition): +def create_device_with_hash_msg(heartbeat_msg): """ Create a device, the heartbeat msg will be used - to decode the actual hash value and id + to decode the actual hash value and id. """ node_id = heartbeat_msg.arbitration_id & 0x3F chan = CANChannel(node_id) - node = deserialize(device_definition) + hash, *_ = chan.serializer.deserialize(heartbeat_msg.data[:4], DataType.UINT32) - if node.hash_uint32 != hash: # hash_uint32 is local, hash is remote + device_definition = definitions["hash_uint32"].get(hash) + + if not device_definition: + raise ValueError(f"No device definition found for hash {hash}.") + + node = deserialize(device_definition) + if node.hash_uint32 != hash: version_str = "".join([chr(n) for n in heartbeat_msg.data[4:]]) if not version_str.strip(): version_str = "1.3.1" raise ProtocolVersionError(node_id, version_str) + node._channel = chan return node diff --git a/studio/Python/tinymovr/dfu.py b/studio/Python/tinymovr/dfu.py index 39e7f45f..5920444c 100644 --- a/studio/Python/tinymovr/dfu.py +++ b/studio/Python/tinymovr/dfu.py @@ -1,9 +1,9 @@ """ Usage: - dfu.py --node_id=ID [--bin=PATH] [--no-reset] + dfu.py --node_id=ID --bin=PATH [--no-reset] Options: - --node_id=ID The CAN Node id of the bootloader to address. + --node_id=ID The CAN Node ID of the device in DFU mode. --bin=PATH The path of the .bin file to upload. --no-reset Do not perform a reset following successful flashing. """ @@ -22,8 +22,7 @@ from tinymovr.config import ( get_bus_config, create_device, - tinymovr_definition, - bl_definition, + definitions ) """ @@ -117,10 +116,13 @@ def spawn(): params = get_bus_config(["canine", "slcan_disco"]) params["bitrate"] = 1000000 init_tee(can.Bus(**params), timeout=1.0) - device = create_device(node_id=node_id, device_definition=bl_definition) + device = create_device(node_id=node_id) + + if not bin_path: + raise FileNotFoundError(f"No bin file specified!") # If a non-existing .bin file is specified, raise error - if bin_path and not Path(bin_path).is_file(): + elif bin_path and not Path(bin_path).is_file(): raise FileNotFoundError(f"Bin file {bin_path} not found!") # If an existing .bin file is specified, upload it to the device @@ -133,14 +135,6 @@ def spawn(): if not args["--no-reset"]: print("Resetting device...") device.reset() - - # If no bin file specified, enter the iPython CLI - else: - user_ns = {} - user_ns["device"] = device - c = Config() - c.TerminalIPythonApp.display_banner = False - IPython.start_ipython(argv=[], config=c, user_ns=user_ns) destroy_tee() diff --git a/studio/Python/tinymovr/gui/__init__.py b/studio/Python/tinymovr/gui/__init__.py index 66250ad0..555bde3e 100644 --- a/studio/Python/tinymovr/gui/__init__.py +++ b/studio/Python/tinymovr/gui/__init__.py @@ -16,7 +16,8 @@ ) from tinymovr.gui.widgets import ( OurQTreeWidget, - IconComboBoxWidget + IconComboBoxWidget, + ArgumentInputDialog ) from tinymovr.gui.worker import Worker from tinymovr.gui.window import MainWindow diff --git a/studio/Python/tinymovr/gui/gui.py b/studio/Python/tinymovr/gui/gui.py index a11c57af..0d1902ca 100644 --- a/studio/Python/tinymovr/gui/gui.py +++ b/studio/Python/tinymovr/gui/gui.py @@ -1,7 +1,7 @@ """Tinymovr Studio GUI Usage: - tinymovr [--bus=] [--chan=] [--bitrate=] + tinymovr [--bus=] [--chan=] [--bitrate=] [--max-timeouts=] tinymovr -h | --help tinymovr --version @@ -9,6 +9,7 @@ --bus= One or more interfaces to use, first available is used [default: canine,slcan_disco]. --chan= The bus device "channel". --bitrate= CAN bitrate [default: 1000000]. + --max-timeouts= Max timeouts before nodes are rescanned [default: 5]. """ import sys diff --git a/studio/Python/tinymovr/gui/helpers.py b/studio/Python/tinymovr/gui/helpers.py index dd40c65e..d138b566 100644 --- a/studio/Python/tinymovr/gui/helpers.py +++ b/studio/Python/tinymovr/gui/helpers.py @@ -22,7 +22,7 @@ from PySide6 import QtGui from PySide6.QtGui import QGuiApplication, QPalette from PySide6.QtWidgets import QMessageBox, QFileDialog -from avlos.definitions import RemoteAttribute +from avlos.definitions import RemoteAttribute, RemoteEnum, RemoteBitmask import tinymovr @@ -336,7 +336,9 @@ def format_value(value, include_unit=True): type and return the formatted string """ if isinstance(value, enum.IntFlag): - return str(value) if value > 0 else "(no flags)" + return bitmask_string_representation(value) if value > 0 else "(no flags)" + if isinstance(value, enum.IntEnum): + return value.name if not include_unit and isinstance(value, pint.Quantity): return str(value.magnitude) if isinstance(value, float): @@ -344,7 +346,14 @@ def format_value(value, include_unit=True): return str(value) -def load_pixmap(filename, dark_mode_suffix='_dark', high_dpi_suffix='@2x'): +def bitmask_string_representation(bitmask_value): + labels_in_bitmask = [ + label.name for label in type(bitmask_value) if label & bitmask_value + ] + return ", ".join(labels_in_bitmask) + + +def load_pixmap(filename, dark_mode_suffix="_dark", high_dpi_suffix="@2x"): """ Load an image from a file and return it as a QPixmap. Load appropriate variants based on pixel ratio and @@ -354,12 +363,12 @@ def load_pixmap(filename, dark_mode_suffix='_dark', high_dpi_suffix='@2x'): pixel_ratio = QtGui.QGuiApplication.primaryScreen().devicePixelRatio() # Prepare the filename based on the conditions - parts = filename.split('.') + parts = filename.split(".") if is_dark_mode(): parts[0] += dark_mode_suffix if pixel_ratio > 1: parts[0] += high_dpi_suffix - adjusted_filename = '.'.join(parts) + adjusted_filename = ".".join(parts) file_path = os.path.join( os.path.dirname(tinymovr.__file__), "resources", "icons", adjusted_filename @@ -412,19 +421,15 @@ class TimedGetter: information for the getter function """ - def __init__(self, error_handler): - self.error_handler = error_handler + def __init__(self): self.dt = 0 def get_value(self, getter): - try: - get_start_time = time.time() - val = getter() - get_dt = time.time() - get_start_time - self.dt = self.dt * 0.99 + get_dt * 0.01 - return val - except Exception as e: - self.error_handler(e) + get_start_time = time.time() + val = getter() + get_dt = time.time() - get_start_time + self.dt = self.dt * 0.99 + get_dt * 0.01 + return val class RateLimitedFunction: @@ -511,10 +516,13 @@ def get_dynamic_attrs(attr_dict): Get the attributes that are marked as dynamic in the spec. """ dynamic_attrs = [] - for _, attr in attr_dict.items(): - if isinstance(attr, RemoteAttribute): - if "dynamic" in attr.meta and attr.meta["dynamic"] == True: - dynamic_attrs.append(attr) + + for attr in attr_dict.values(): + if isinstance( + attr, (RemoteAttribute, RemoteEnum, RemoteBitmask) + ) and attr.meta.get("dynamic"): + dynamic_attrs.append(attr) elif hasattr(attr, "remote_attributes"): dynamic_attrs.extend(get_dynamic_attrs(attr.remote_attributes)) + return dynamic_attrs diff --git a/studio/Python/tinymovr/gui/widgets.py b/studio/Python/tinymovr/gui/widgets.py index fee21b3a..05cee938 100644 --- a/studio/Python/tinymovr/gui/widgets.py +++ b/studio/Python/tinymovr/gui/widgets.py @@ -1,8 +1,22 @@ from PySide6 import QtGui from PySide6.QtGui import QPixmap -from PySide6.QtWidgets import QWidget, QTreeWidget, QHBoxLayout, QLabel, QComboBox +from PySide6.QtWidgets import ( + QWidget, + QTreeWidget, + QHBoxLayout, + QLabel, + QComboBox, + QDialog, + QVBoxLayout, + QLabel, + QLineEdit, + QPushButton, + QFormLayout, +) + from tinymovr.gui.helpers import load_pixmap + class OurQTreeWidget(QTreeWidget): def __init__(self, parent=None): super().__init__(parent) @@ -45,3 +59,36 @@ def __init__(self, icon_path, parent=None): layout.addWidget(self.combo) self.setLayout(layout) + + +class ArgumentInputDialog(QDialog): + def __init__(self, arguments, parent=None): + super(ArgumentInputDialog, self).__init__(parent) + self.arguments = arguments + self.inputs = {} + + layout = QFormLayout(self) + + # For each argument, create a label and input line edit + for arg in arguments: + label = QLabel(f"{arg.name} ({arg.dtype.nickname}):") + line_edit = QLineEdit(self) + layout.addRow(label, line_edit) + self.inputs[arg.name] = line_edit + + # Add Ok and Cancel buttons + button_layout = QVBoxLayout() + ok_button = QPushButton("Ok", self) + ok_button.clicked.connect(self.accept) + cancel_button = QPushButton("Cancel", self) + cancel_button.clicked.connect(self.reject) + button_layout.addWidget(ok_button) + button_layout.addWidget(cancel_button) + + layout.addRow(button_layout) + self.setLayout(layout) + + def get_values(self): + return { + arg_name: line_edit.text() for arg_name, line_edit in self.inputs.items() + } diff --git a/studio/Python/tinymovr/gui/window.py b/studio/Python/tinymovr/gui/window.py index 504d5c69..03fc42c1 100644 --- a/studio/Python/tinymovr/gui/window.py +++ b/studio/Python/tinymovr/gui/window.py @@ -24,6 +24,7 @@ from PySide6.QtCore import Signal from PySide6.QtWidgets import ( QMainWindow, + QDialog, QMenu, QMenuBar, QWidget, @@ -34,7 +35,7 @@ QLabel, QTreeWidgetItem, QPushButton, - QMessageBox + QMessageBox, ) from pint.errors import UndefinedUnitError from PySide6.QtGui import QAction @@ -48,6 +49,7 @@ Worker, OurQTreeWidget, IconComboBoxWidget, + ArgumentInputDialog, format_value, load_icon, display_file_open_dialog, @@ -58,7 +60,6 @@ class MainWindow(QMainWindow): - TreeItemCheckedSignal = Signal(dict) def __init__(self, app, arguments): @@ -69,7 +70,6 @@ def __init__(self, app, arguments): self.start_time = time.time() self.logger = configure_logging() - bitrate = int(arguments["--bitrate"]) self.attr_widgets_by_id = {} self.graphs_by_id = {} @@ -133,9 +133,12 @@ def __init__(self, app, arguments): main_widget.setMinimumHeight(600) self.setCentralWidget(main_widget) + self.timeout_count = 0 + buses = arguments["--bus"].rsplit(sep=",") channel = arguments["--chan"] bitrate = int(arguments["--bitrate"]) + self.max_timeouts = int(arguments["--max-timeouts"]) if channel == None: params = get_bus_config(buses) @@ -163,7 +166,14 @@ def about_to_quit(self): @QtCore.Slot() def handle_worker_error(self, e): if isinstance(e, ChannelResponseError): - self.logger.warn("Timeout occured") + if self.timeout_count > self.max_timeouts: + self.timeout_count = 0 + self.logger.warn("Max timeouts exceeded. Triggering rescan...") + self.worker.reset() + else: + self.logger.warn("Timeout while getting value.") + self.timeout_count += 1 + else: raise e @@ -193,7 +203,7 @@ def add_graph_for_attr(self, attr): self.right_layout.addWidget(graph_widget) @QtCore.Slot() - def regen_tree(self, tms_by_id): + def regen_tree(self, devices_by_name): """ Regenerate the attribute tree """ @@ -202,8 +212,8 @@ def regen_tree(self, tms_by_id): self.tree_widget.clear() self.tree_widget.setEnabled(False) all_items = [] - for name, node in tms_by_id.items(): - widget, items_list = self.parse_node(node, name) + for name, device in devices_by_name.items(): + widget, items_list = self.parse_node(device, name) self.tree_widget.addTopLevelItem(widget) all_items.extend(items_list) for item in all_items: @@ -306,7 +316,22 @@ def attrs_updated(self, data): @QtCore.Slot() def f_call_clicked(self, f): - f() + args = [] + + # Check if the function has any arguments + if f.arguments: + dialog = ArgumentInputDialog(f.arguments, self) + if dialog.exec_() == QDialog.Accepted: + input_values = dialog.get_values() + args = [input_values[arg.name] for arg in f.arguments] + else: + return # User cancelled, stop the entire process + + # Convert arguments as required using pint + args = [get_registry()(arg) for arg in args] + + # Call the function with the collected arguments + f(*args) if "reload_data" in f.meta and f.meta["reload_data"]: self.worker.reset() @@ -353,7 +378,12 @@ def delete_graphs(self): self.delete_graph_by_attr_name(attr_name) def show_about_box(self): - version_str = (pkg_resources.require("tinymovr")[0].version) + version_str = pkg_resources.require("tinymovr")[0].version app_str = "{} {}".format(app_name, version_str) - QMessageBox.about(self, "About Tinymovr", "{}\nhttps://tinymovr.com\n\nCat Sleeping Icon by Denis Sazhin from Noun Project".format(app_str)) - + QMessageBox.about( + self, + "About Tinymovr", + "{}\nhttps://tinymovr.com\n\nCat Sleeping Icon by Denis Sazhin from Noun Project".format( + app_str + ), + ) diff --git a/studio/Python/tinymovr/gui/worker.py b/studio/Python/tinymovr/gui/worker.py index a31af7a9..29611452 100644 --- a/studio/Python/tinymovr/gui/worker.py +++ b/studio/Python/tinymovr/gui/worker.py @@ -18,7 +18,6 @@ import time import can -from canine import CANineBus from PySide6 import QtCore from PySide6.QtCore import QObject from PySide6.QtWidgets import ( @@ -42,8 +41,8 @@ def __init__(self, busparams, logger): self.mutx = QtCore.QMutex() init_tee(can.Bus(**busparams)) self._init_containers() - self.dsc = Discovery(self._node_appeared, self._node_disappeared, self.logger) - self.timed_getter = TimedGetter(lambda e: self.handle_error.emit(e)) + self.dsc = Discovery(self._device_appeared, self._device_disappeared, self.logger) + self.timed_getter = TimedGetter() self._rate_limited_update = RateLimitedFunction(lambda: self._update(), 0.040) self.running = True @@ -58,14 +57,14 @@ def stop(self): def force_regen(self): self.mutx.lock() - self.regen.emit(dict(self.tms_by_id)) + self.regen.emit(dict(self.devices_by_name)) self.mutx.unlock() def reset(self): self.mutx.lock() self.dsc.reset() self._init_containers() - self.regen.emit(dict(self.tms_by_id)) + self.regen.emit(dict(self.devices_by_name)) self.mutx.unlock() @QtCore.Slot(dict) @@ -78,7 +77,10 @@ def update_active_attrs(self, d): def _get_attr_values(self): vals = {} for attr in self.active_attrs: - vals[attr.full_name] = self.timed_getter.get_value(attr.get_value) + try: + vals[attr.full_name] = self.timed_getter.get_value(attr.get_value) + except Exception as e: + self.handle_error.emit(e) start_time = time.time() self.dynamic_attrs.sort( key=lambda attr: self.dynamic_attrs_last_update[attr.full_name] @@ -92,15 +94,19 @@ def _get_attr_values(self): else 0 ) if (attr.full_name not in vals) and (start_time - t > 0.5): - vals[attr.full_name] = self.timed_getter.get_value(attr.get_value) - self.dynamic_attrs_last_update[attr.full_name] = start_time + try: + vals[attr.full_name] = self.timed_getter.get_value(attr.get_value) + self.dynamic_attrs_last_update[attr.full_name] = start_time + except Exception as e: + self.handle_error.emit(e) break return vals def _init_containers(self): self.active_attrs = set() self.dynamic_attrs = [] - self.tms_by_id = {} + self.devices_by_name = {} + self.names_by_id = {} self.dynamic_attrs_last_update = {} def _update(self): @@ -111,20 +117,22 @@ def _update(self): self.mutx.unlock() QApplication.processEvents() - def _node_appeared(self, node, name): + def _device_appeared(self, device, node_id): self.mutx.lock() - node_name = "{}{}".format(base_node_name, name) - self.tms_by_id[node_name] = node - node.name = node_name - node.include_base_name = True - self.dynamic_attrs = get_dynamic_attrs(self.tms_by_id) - self.regen.emit(dict(self.tms_by_id)) + display_name = "{}{}".format(device.name, node_id) + self.devices_by_name[display_name] = device + self.names_by_id[node_id] = display_name + device.name = display_name + device.include_base_name = True + self.dynamic_attrs = get_dynamic_attrs(self.devices_by_name) + self.regen.emit(dict(self.devices_by_name)) self.mutx.unlock() - def _node_disappeared(self, name): + def _device_disappeared(self, node_id): self.mutx.lock() - node_name = "{}{}".format(base_node_name, name) - del self.tms_by_id[node_name] - self.dynamic_attrs = get_dynamic_attrs(self.tms_by_id) - self.regen.emit(dict(self.tms_by_id)) + display_name = "{}{}".format(self.names_by_id[node_id], node_id) + del self.devices_by_name[display_name] + del self.names_by_id[node_id] + self.dynamic_attrs = get_dynamic_attrs(self.devices_by_name) + self.regen.emit(dict(self.devices_by_name)) self.mutx.unlock() diff --git a/studio/Python/tinymovr/config/device.yaml b/studio/Python/tinymovr/specs/device.yaml similarity index 97% rename from studio/Python/tinymovr/config/device.yaml rename to studio/Python/tinymovr/specs/device.yaml index febda580..1e7d93cd 100644 --- a/studio/Python/tinymovr/config/device.yaml +++ b/studio/Python/tinymovr/specs/device.yaml @@ -47,7 +47,7 @@ remote_attributes: getter_name: system_get_calibrated summary: Whether the system has been calibrated. - name: errors - flags: [UNDERVOLTAGE, DRIVER_FAULT] + flags: [UNDERVOLTAGE, DRIVER_FAULT, CHARGE_PUMP_FAULT_STAT, CHARGE_PUMP_FAULT, DRV10_DISABLE, DRV32_DISABLE, DRV54_DISABLE] meta: {dynamic: True} getter_name: system_get_errors summary: Any system errors, as a bitmask @@ -68,28 +68,18 @@ remote_attributes: dtype: void arguments: [] meta: {reload_data: True} - - name: invoke_bootloader - summary: Invoke the bootloader. - caller_name: system_invoke_bootloader + - name: enter_dfu + summary: Enter DFU mode. + caller_name: system_enter_dfu dtype: void arguments: [] meta: {reload_data: True} - name: scheduler remote_attributes: - - name: total - dtype: uint32 - meta: {dynamic: True} - getter_name: Scheduler_GetTotalCycles - summary: Total processor cycles in a single PWM cycle. - - name: busy - dtype: uint32 - meta: {dynamic: True} - getter_name: Scheduler_GetBusyCycles - summary: Busy processor cycles in a single PWM cycle. - name: errors flags: [CONTROL_BLOCK_REENTERED] meta: {dynamic: True} - getter_name: controller_get_errors + getter_name: scheduler_get_errors summary: Any scheduler errors, as a bitmask - name: controller remote_attributes: @@ -260,8 +250,10 @@ remote_attributes: arguments: - name: pos_setpoint dtype: float + unit: tick - name: vel_setpoint dtype: float + unit: tick - name: comms remote_attributes: - name: can @@ -441,6 +433,7 @@ remote_attributes: arguments: - name: pos_setpoint dtype: float + unit: tick - name: move_to_tlimit summary: Move to target position respecting time limits for each sector. caller_name: planner_move_to_tlimit @@ -448,6 +441,7 @@ remote_attributes: arguments: - name: pos_setpoint dtype: float + unit: tick - name: errors flags: [INVALID_INPUT, VCRUISE_OVER_LIMIT] getter_name: planner_get_errors diff --git a/studio/Python/tinymovr/config/bl.yaml b/studio/Python/tinymovr/specs/dfu.yaml similarity index 96% rename from studio/Python/tinymovr/config/bl.yaml rename to studio/Python/tinymovr/specs/dfu.yaml index b906b79a..4b2bdaae 100644 --- a/studio/Python/tinymovr/config/bl.yaml +++ b/studio/Python/tinymovr/specs/dfu.yaml @@ -1,5 +1,5 @@ -name: tm_bl +name: tm_dfu remote_attributes: - name: protocol_hash dtype: uint32 @@ -26,7 +26,7 @@ remote_attributes: - name: error options: [NONE, START_EXECUTION_STACK_POINTER_OUT_OF_RANGE, START_EXECUTION_ADDRESS_OUT_OF_RANGE, START_EXECUTION_ADDRESS_LSB_NOT_SET] getter_name: system_get_error - summary: The last error encountered by the bootloader + summary: The last error encountered in DFU - name: read_flash_32 summary: Read a 32 bit value from the flash caller_name: nvm_read_flash_32