From 5fbcd6f7d7da7ce0b14af57d67fb8fe3f6cfd9a8 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 14 Jul 2024 20:46:03 +0300 Subject: [PATCH 1/4] update spec --- docs/protocol/reference.rst | 2 + firmware/src/can/can_endpoints.h | 2 +- firmware/src/tm_enums.h | 3 +- .../Python/tinymovr/specs/tinymovr_2_3_x.yaml | 622 ++++++++++++++++++ 4 files changed, 627 insertions(+), 2 deletions(-) create mode 100644 studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst index a2cc7735..baf94b7f 100644 --- a/docs/protocol/reference.rst +++ b/docs/protocol/reference.rst @@ -324,6 +324,8 @@ Flags: - CURRENT_LIMIT_EXCEEDED +- PRE_CL_I_SD_EXCEEDED + controller.position.setpoint ------------------------------------------------------------------- diff --git a/firmware/src/can/can_endpoints.h b/firmware/src/can/can_endpoints.h index 531c1eed..a606d63f 100644 --- a/firmware/src/can/can_endpoints.h +++ b/firmware/src/can/can_endpoints.h @@ -10,7 +10,7 @@ #include #include -static const uint32_t avlos_proto_hash = 3296093882; +static const uint32_t avlos_proto_hash = 1031937702; extern uint8_t (*avlos_endpoints[97])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); extern uint32_t _avlos_get_proto_hash(void); diff --git a/firmware/src/tm_enums.h b/firmware/src/tm_enums.h index 976305fe..b5182773 100644 --- a/firmware/src/tm_enums.h +++ b/firmware/src/tm_enums.h @@ -56,7 +56,8 @@ typedef enum typedef enum { CONTROLLER_ERRORS_NONE = 0, - CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0) + CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0), + CONTROLLER_ERRORS_PRE_CL_I_SD_EXCEEDED = (1 << 1) } controller_errors_flags; typedef enum diff --git a/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml b/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml new file mode 100644 index 00000000..e91876bb --- /dev/null +++ b/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml @@ -0,0 +1,622 @@ + +name: tm +remote_attributes: + - name: protocol_hash + dtype: uint32 + getter_name: _avlos_get_proto_hash + summary: The Avlos protocol hash. + - name: uid + dtype: uint32 + getter_name: system_get_uid + summary: The unique device ID, unique to each PAC55xx chip produced. + - name: fw_version + dtype: string + getter_name: system_get_fw_version_string + summary: The firmware version. + - name: hw_revision + dtype: uint32 + getter_name: system_get_hw_revision + summary: The hardware revision. + - name: Vbus + dtype: float + unit: volt + meta: {dynamic: True} + getter_name: system_get_Vbus + summary: The measured bus voltage. + - name: Ibus + dtype: float + unit: ampere + meta: {dynamic: True} + getter_name: controller_get_Ibus_est + summary: The estimated bus current. Only estimates current drawn by motor. + - name: power + dtype: float + unit: watt + meta: {dynamic: True} + getter_name: controller_get_power_est + summary: The estimated power. Only estimates power drawn by motor. + - name: temp + dtype: float + unit: degC + meta: {dynamic: True} + getter_name: ADC_get_mcu_temp + summary: The internal temperature of the PAC55xx MCU. + - name: calibrated + dtype: bool + meta: {dynamic: True} + getter_name: system_get_calibrated + summary: Whether the system has been calibrated. + - name: errors + flags: [UNDERVOLTAGE] + meta: {dynamic: True} + getter_name: system_get_errors + summary: Any system errors, as a bitmask + - name: warnings + flags: [DRIVER_FAULT, CHARGE_PUMP_FAULT_STAT, CHARGE_PUMP_FAULT, DRV10_DISABLE, DRV32_DISABLE, DRV54_DISABLE] + meta: {dynamic: True} + getter_name: system_get_warnings + summary: Any system warnings, as a bitmask + - name: save_config + summary: Save configuration to non-volatile memory. + caller_name: nvm_save_config + dtype: void + arguments: [] + - name: erase_config + summary: Erase the config stored in non-volatile memory and reset the device. + caller_name: nvm_erase_and_reset + dtype: void + arguments: [] + meta: {reload_data: True} + - name: reset + summary: Reset the device. + caller_name: system_reset + dtype: void + arguments: [] + meta: {reload_data: True} + - name: enter_dfu + summary: Enter DFU mode. + caller_name: system_enter_dfu + dtype: void + arguments: [] + meta: {reload_data: True} + - name: config_size + summary: Size (in bytes) of the configuration object. + getter_name: system_get_config_size + dtype: uint32 + - name: scheduler + remote_attributes: + - name: load + summary: Processor load in ticks per PWM cycle. + getter_name: scheduler_get_load + meta: {dynamic: True} + dtype: uint32 + - name: warnings + flags: [CONTROL_BLOCK_REENTERED] + meta: {dynamic: True} + getter_name: scheduler_get_warnings + summary: Any scheduler warnings, as a bitmask + - name: controller + remote_attributes: + - name: state + options: [IDLE, CALIBRATE, CL_CONTROL] + meta: {dynamic: True} + getter_name: controller_get_state + setter_name: controller_set_state + summary: The state of the controller. + - name: mode + options: [CURRENT, VELOCITY, POSITION, TRAJECTORY, HOMING] + meta: {dynamic: True} + getter_name: controller_get_mode + setter_name: controller_set_mode + summary: The control mode of the controller. + - name: warnings + meta: {dynamic: True} + flags: [VELOCITY_LIMITED, CURRENT_LIMITED, MODULATION_LIMITED] + getter_name: controller_get_warnings + summary: Any controller warnings, as a bitmask + - name: errors + meta: {dynamic: True} + flags: [CURRENT_LIMIT_EXCEEDED, PRE_CL_I_SD_EXCEEDED] + getter_name: controller_get_errors + summary: Any controller errors, as a bitmask + - name: position + remote_attributes: + - name: setpoint + dtype: float + unit: tick + meta: {jog_step: 100} + getter_name: controller_get_pos_setpoint_user_frame + setter_name: controller_set_pos_setpoint_user_frame + summary: The position setpoint in the user reference frame. + - name: p_gain + dtype: float + meta: {export: True} + getter_name: controller_get_pos_gain + setter_name: controller_set_pos_gain + summary: The proportional gain of the position controller. + - name: velocity + remote_attributes: + - name: setpoint + dtype: float + unit: tick/sec + meta: {jog_step: 200} + getter_name: controller_get_vel_setpoint_user_frame + setter_name: controller_set_vel_setpoint_user_frame + summary: The velocity setpoint in the user reference frame. + - name: limit + dtype: float + unit: tick/sec + meta: {export: True} + getter_name: controller_get_vel_limit + setter_name: controller_set_vel_limit + summary: The velocity limit. + - name: p_gain + dtype: float + meta: {export: True} + getter_name: controller_get_vel_gain + setter_name: controller_set_vel_gain + summary: The proportional gain of the velocity controller. + - name: i_gain + dtype: float + meta: {export: True} + getter_name: controller_get_vel_integral_gain + setter_name: controller_set_vel_integral_gain + summary: The integral gain of the velocity controller. + - name: deadband + dtype: float + unit: tick + meta: {export: True} + getter_name: controller_get_vel_integral_deadband + setter_name: controller_set_vel_integral_deadband + rst_target: integrator-deadband + summary: The deadband of the velocity integrator. A region around the position setpoint where the velocity integrator is not updated. + - name: increment + dtype: float + meta: {export: True} + getter_name: controller_get_vel_increment + setter_name: controller_set_vel_increment + summary: Max velocity setpoint increment (ramping) rate. Set to 0 to disable. + - name: current + remote_attributes: + - name: Iq_setpoint + dtype: float + unit: ampere + meta: {jog_step: 0.005} + getter_name: controller_get_Iq_setpoint_user_frame + setter_name: controller_set_Iq_setpoint_user_frame + summary: The Iq setpoint in the user reference frame. + - name: Id_setpoint + dtype: float + unit: ampere + meta: {dynamic: True} + getter_name: controller_get_Id_setpoint_user_frame + summary: The Id setpoint in the user reference frame. + - name: Iq_limit + dtype: float + unit: ampere + getter_name: controller_get_Iq_limit + setter_name: controller_set_Iq_limit + summary: The Iq limit. + - name: Iq_estimate + dtype: float + unit: ampere + meta: {dynamic: True} + getter_name: controller_get_Iq_estimate_user_frame + summary: The Iq estimate in the user reference frame. + - name: bandwidth + dtype: float + unit: Hz + meta: {export: True} + getter_name: controller_get_I_bw + setter_name: controller_set_I_bw + summary: The current controller bandwidth. + - name: Iq_p_gain + dtype: float + getter_name: controller_get_Iq_gain + summary: The current controller proportional gain. + - name: max_Ibus_regen + dtype: float + unit: ampere + getter_name: controller_get_max_Ibus_regen + setter_name: controller_set_max_Ibus_regen + summary: The max current allowed to be fed back to the power source before flux braking activates. + - name: max_Ibrake + dtype: float + unit: ampere + meta: {export: True} + getter_name: controller_get_max_Ibrake + setter_name: controller_set_max_Ibrake + summary: The max current allowed to be dumped to the motor windings during flux braking. Set to zero to deactivate flux braking. + - name: voltage + remote_attributes: + - name: Vq_setpoint + dtype: float + unit: volt + meta: {dynamic: True} + getter_name: controller_get_Vq_setpoint_user_frame + summary: The Vq setpoint. + - name: calibrate + summary: Calibrate the device. + caller_name: controller_calibrate + dtype: void + arguments: [] + - name: idle + summary: Set idle mode, disabling the driver. + caller_name: controller_idle + dtype: void + arguments: [] + - name: position_mode + summary: Set position control mode. + caller_name: controller_position_mode + dtype: void + arguments: [] + - name: velocity_mode + summary: Set velocity control mode. + caller_name: controller_velocity_mode + dtype: void + arguments: [] + - name: current_mode + summary: Set current control mode. + caller_name: controller_current_mode + dtype: void + arguments: [] + - name: set_pos_vel_setpoints + summary: Set the position and velocity setpoints in the user reference frame in one go, and retrieve the position estimate + caller_name: controller_set_pos_vel_setpoints_user_frame + dtype: float + arguments: + - name: pos_setpoint + dtype: float + unit: tick + - name: vel_setpoint + dtype: float + unit: tick + - name: comms + remote_attributes: + - name: can + remote_attributes: + - name: rate + dtype: uint32 + meta: {export: True} + getter_name: CAN_get_kbit_rate + setter_name: CAN_set_kbit_rate + rst_target: api-can-rate + summary: The baud rate of the CAN interface. + - name: id + dtype: uint32 + meta: {export: True, reload_data: True} + getter_name: CAN_get_ID + setter_name: CAN_set_ID + summary: The ID of the CAN interface. + - name: heartbeat + dtype: bool + getter_name: CAN_get_send_heartbeat + setter_name: CAN_set_send_heartbeat + summary: Toggle sending of heartbeat messages. + - name: motor + remote_attributes: + - name: R + dtype: float + unit: ohm + meta: {dynamic: True, export: True} + getter_name: motor_get_phase_resistance + setter_name: motor_set_phase_resistance + summary: The motor Resistance value. + - name: L + dtype: float + unit: henry + meta: {dynamic: True, export: True} + getter_name: motor_get_phase_inductance + setter_name: motor_set_phase_inductance + summary: The motor Inductance value. + - name: pole_pairs + dtype: uint8 + meta: {dynamic: True, export: True} + getter_name: motor_get_pole_pairs + setter_name: motor_set_pole_pairs + summary: The motor pole pair count. + - name: type + options: [HIGH_CURRENT, GIMBAL] + meta: {export: True} + getter_name: motor_get_is_gimbal + setter_name: motor_set_is_gimbal + summary: The type of the motor. Either high current or gimbal. + - name: calibrated + dtype: bool + meta: {dynamic: True} + getter_name: motor_get_calibrated + summary: Whether the motor has been calibrated. + - name: I_cal + dtype: float + unit: ampere + meta: {export: True} + getter_name: motor_get_I_cal + setter_name: motor_set_I_cal + summary: The calibration current. + - name: errors + flags: [PHASE_RESISTANCE_OUT_OF_RANGE, PHASE_INDUCTANCE_OUT_OF_RANGE, POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE, POLE_PAIRS_OUT_OF_RANGE] + meta: {dynamic: True} + getter_name: motor_get_errors + summary: Any motor/calibration errors, as a bitmask + - name: sensors + remote_attributes: + - name: user_frame + remote_attributes: + - name: position_estimate + dtype: float + unit: ticks + meta: {dynamic: True} + getter_name: user_frame_get_pos_estimate + summary: The filtered position estimate in the user reference frame. + - name: velocity_estimate + dtype: float + unit: ticks/second + meta: {dynamic: True} + getter_name: user_frame_get_vel_estimate + summary: The filtered velocity estimate in the user reference frame. + - name: offset + dtype: float + unit: ticks + getter_name: frame_user_to_position_sensor_get_offset + setter_name: frame_user_to_position_sensor_set_offset + summary: The user defined offset. + - name: multiplier + dtype: float + getter_name: frame_user_to_position_sensor_get_multiplier + setter_name: frame_user_to_position_sensor_set_multiplier + summary: The user defined multipler. + - name: setup + remote_attributes: + - name: onboard + remote_attributes: + - name: calibrated + dtype: bool + meta: {dynamic: True} + getter_name: sensor_onboard_get_is_calibrated + summary: Whether the sensor has been calibrated. + - name: errors + flags: [CALIBRATION_FAILED, READING_UNSTABLE] + meta: {dynamic: True} + getter_name: sensor_onboard_get_errors + summary: Any sensor errors, as a bitmask + - name: external_spi + remote_attributes: + - name: type + options: [MA7XX, AS5047, AMT22] + meta: {export: True} + getter_name: sensor_external_spi_get_type_avlos + setter_name: sensor_external_spi_set_type_avlos + summary: The type of the external sensor. + - name: rate + options: [1_5Mbps, 3Mbps, 6Mbps, 8Mbps, 12Mbps] + meta: {export: True, dynamic: True} + getter_name: sensor_external_spi_get_rate_avlos + setter_name: sensor_external_spi_set_rate_avlos + summary: The rate of the external sensor. + - name: calibrated + dtype: bool + meta: {dynamic: True} + getter_name: sensor_external_spi_get_is_calibrated + summary: Whether the sensor has been calibrated. + - name: errors + flags: [CALIBRATION_FAILED, READING_UNSTABLE] + meta: {dynamic: True} + getter_name: sensor_external_spi_get_errors + summary: Any sensor errors, as a bitmask + - name: hall + remote_attributes: + - name: calibrated + dtype: bool + meta: {dynamic: True} + getter_name: sensor_hall_get_is_calibrated + summary: Whether the sensor has been calibrated. + - name: errors + flags: [CALIBRATION_FAILED, READING_UNSTABLE] + meta: {dynamic: True} + getter_name: sensor_hall_get_errors + summary: Any sensor errors, as a bitmask + - name: select + remote_attributes: + - name: position_sensor + remote_attributes: + - name: connection + options: [ONBOARD, EXTERNAL_SPI, HALL] + meta: {export: True} + getter_name: position_sensor_get_connection + setter_name: position_sensor_set_connection + summary: The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL. + - name: bandwidth + dtype: float + unit: Hz + meta: {export: True} + getter_name: position_observer_get_bandwidth + setter_name: position_observer_set_bandwidth + summary: The position sensor observer bandwidth. + - name: raw_angle + dtype: int32 + meta: {dynamic: True} + getter_name: sensor_position_get_raw_angle + summary: The raw position sensor angle. + - name: position_estimate + dtype: float + unit: ticks + meta: {dynamic: True} + getter_name: position_observer_get_pos_estimate + summary: The filtered position estimate in the position sensor reference frame. + - name: velocity_estimate + dtype: float + unit: ticks/second + meta: {dynamic: True} + getter_name: position_observer_get_vel_estimate + summary: The filtered velocity estimate in the position sensor reference frame. + - name: commutation_sensor + remote_attributes: + - name: connection + options: [ONBOARD, EXTERNAL_SPI, HALL] + meta: {export: True} + getter_name: commutation_sensor_get_connection + setter_name: commutation_sensor_set_connection + summary: The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL. + - name: bandwidth + dtype: float + unit: Hz + meta: {export: True} + getter_name: commutation_observer_get_bandwidth + setter_name: commutation_observer_set_bandwidth + summary: The commutation sensor observer bandwidth. + - name: raw_angle + dtype: int32 + meta: {dynamic: True} + getter_name: sensor_commutation_get_raw_angle + summary: The raw commutation sensor angle. + - name: position_estimate + dtype: float + unit: ticks + meta: {dynamic: True} + getter_name: commutation_observer_get_pos_estimate + summary: The filtered position estimate in the commutation sensor reference frame. + - name: velocity_estimate + dtype: float + unit: ticks/second + meta: {dynamic: True} + getter_name: commutation_observer_get_vel_estimate + summary: The filtered velocity estimate in the commutation sensor reference frame. + - name: traj_planner + remote_attributes: + - name: max_accel + dtype: float + unit: ticks/s + meta: {export: True} + getter_name: planner_get_max_accel + setter_name: planner_set_max_accel + summary: The max allowed acceleration of the generated trajectory. + - name: max_decel + dtype: float + unit: ticks/second/second + meta: {export: True} + getter_name: planner_get_max_decel + setter_name: planner_set_max_decel + summary: The max allowed deceleration of the generated trajectory. + - name: max_vel + dtype: float + unit: ticks/second + meta: {export: True} + getter_name: planner_get_max_vel + setter_name: planner_set_max_vel + summary: The max allowed cruise velocity of the generated trajectory. + - name: t_accel + dtype: float + unit: second + meta: {export: True} + getter_name: planner_get_deltat_accel + setter_name: planner_set_deltat_accel + summary: In time mode, the acceleration time of the generated trajectory. + - name: t_decel + dtype: float + unit: second + meta: {export: True} + getter_name: planner_get_deltat_decel + setter_name: planner_set_deltat_decel + summary: In time mode, the deceleration time of the generated trajectory. + - name: t_total + dtype: float + unit: second + meta: {export: True} + getter_name: planner_get_deltat_total + setter_name: planner_set_deltat_total + summary: In time mode, the total time of the generated trajectory. + - name: move_to + summary: Move to target position in the user reference frame respecting velocity and acceleration limits. + caller_name: planner_move_to_vlimit + dtype: void + arguments: + - name: pos_setpoint + dtype: float + unit: tick + - name: move_to_tlimit + summary: Move to target position in the user reference frame respecting time limits for each sector. + caller_name: planner_move_to_tlimit + dtype: void + arguments: + - name: pos_setpoint + dtype: float + unit: tick + - name: errors + flags: [INVALID_INPUT, VCRUISE_OVER_LIMIT] + getter_name: planner_get_errors + summary: Any errors in the trajectory planner, as a bitmask + - name: homing + remote_attributes: + - name: velocity + dtype: float + unit: ticks/s + meta: {export: True} + getter_name: homing_planner_get_homing_velocity + setter_name: homing_planner_set_homing_velocity + summary: The velocity at which the motor performs homing. + - name: max_homing_t + dtype: float + unit: s + meta: {export: True} + getter_name: homing_planner_get_max_homing_t + setter_name: homing_planner_set_max_homing_t + summary: The maximum time the motor is allowed to travel before homing times out and aborts. + - name: retract_dist + dtype: float + unit: ticks + meta: {export: True} + getter_name: homing_planner_get_retract_distance + setter_name: homing_planner_set_retract_distance + summary: The retraction distance the motor travels after the endstop has been found. + - name: warnings + meta: {dynamic: True} + flags: [HOMING_TIMEOUT] + getter_name: homing_planner_get_warnings + summary: Any homing warnings, as a bitmask + - name: stall_detect + remote_attributes: + - name: velocity + dtype: float + unit: ticks/s + meta: {export: True} + getter_name: homing_planner_get_max_stall_vel + setter_name: homing_planner_set_max_stall_vel + summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered. + - name: delta_pos + dtype: float + unit: ticks + meta: {export: True} + getter_name: homing_planner_get_max_stall_delta_pos + setter_name: homing_planner_set_max_stall_delta_pos + summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered. + - name: t + dtype: float + unit: s + meta: {export: True} + getter_name: homing_planner_get_max_stall_t + setter_name: homing_planner_set_max_stall_t + summary: The time to remain in stall detection mode before the motor is considered stalled. + - name: home + summary: Perform the homing operation. + caller_name: homing_planner_home + dtype: void + arguments: [] + - name: watchdog + remote_attributes: + - name: enabled + dtype: bool + getter_name: Watchdog_get_enabled + setter_name: Watchdog_set_enabled + summary: Whether the watchdog is enabled or not. + - name: triggered + dtype: bool + meta: {dynamic: True} + getter_name: Watchdog_triggered + summary: Whether the watchdog has been triggered or not. + - name: timeout + dtype: float + unit: s + meta: {export: True} + getter_name: Watchdog_get_timeout_seconds + setter_name: Watchdog_set_timeout_seconds + summary: The watchdog timeout period. From 01de8998d20f5a9470c740b8c6fd7770ed71d625 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 14 Jul 2024 20:46:19 +0300 Subject: [PATCH 2/4] first reset setpoint and wait --- studio/Python/tests/test_board.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py index 8d55fc77..330f6aaf 100644 --- a/studio/Python/tests/test_board.py +++ b/studio/Python/tests/test_board.py @@ -97,6 +97,9 @@ def test_d_position_control(self): self.tm.controller.position_mode() self.check_state(2) + self.tm.controller.position.setpoint = 0 + time.sleep(0.4) + for i in range(5): self.tm.controller.position.setpoint = i * 3000 * ticks if hw_rev > 20: From a2e16e61665483882420ccd444ca435730953d72 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 14 Jul 2024 20:46:34 +0300 Subject: [PATCH 3/4] relax some tests --- studio/Python/tests/test_board.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py index 330f6aaf..0203b860 100644 --- a/studio/Python/tests/test_board.py +++ b/studio/Python/tests/test_board.py @@ -161,6 +161,8 @@ def test_f_random_pos_control(self): self.tm.controller.position_mode() self.check_state(2) + self.tm.controller.velocity.limit = 200000 * ticks / s + for _ in range(15): new_pos = random.uniform(-24000, 24000) self.tm.controller.position.setpoint = new_pos * ticks @@ -262,11 +264,11 @@ def test_j_gimbal_mode(self): self.tm.controller.position_mode() self.check_state(2) - for i in range(10): - self.tm.controller.position.setpoint = i * 1000 * ticks + for i in range(5): + self.tm.controller.position.setpoint = i * 2000 * ticks time.sleep(0.3) self.assertAlmostEqual( - i * 1000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=1000 * ticks + i * 2000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=2000 * ticks ) @pytest.mark.hitl_default From 47bc4d1552e3eaabc2c8b367568e8a6e7bd86e63 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 14 Jul 2024 20:46:47 +0300 Subject: [PATCH 4/4] add pre-cl checks --- firmware/src/config.h | 2 ++ firmware/src/controller/controller.c | 41 ++++++++++++++++++++++++---- firmware/src/utils/utils.h | 25 +++++++++++------ 3 files changed, 55 insertions(+), 13 deletions(-) diff --git a/firmware/src/config.h b/firmware/src/config.h index c2fda7f9..91644f40 100644 --- a/firmware/src/config.h +++ b/firmware/src/config.h @@ -38,6 +38,8 @@ #define VBUS_LOW_THRESHOLD (10.4f) // V #define VEL_HARD_LIMIT (600000.0f) // ticks/s #define I_HARD_LIMIT (60.0f) // A +#define MAX_CL_INIT_STEPS (200) +#define PRE_CL_I_SD_MAX (0.4f) // Encoder rectification lookup table size #define ECN_BITS (6) diff --git a/firmware/src/controller/controller.c b/firmware/src/controller/controller.c index 379674c4..d7cd55ae 100644 --- a/firmware/src/controller/controller.c +++ b/firmware/src/controller/controller.c @@ -15,11 +15,11 @@ // * You should have received a copy of the GNU General Public License // * along with this program. If not, see . -#include "src/system/system.h" +#include #include #include -#include "src/adc/adc.h" -#include "src/motor/motor.h" +#include +#include #include #include #include @@ -27,6 +27,8 @@ #include #include "src/watchdog/watchdog.h" +void CLPreStep(void); +void CLPreCheck(void); void CLControlStep(void); static inline bool Controller_LimitVelocity(float min_limit, float max_limit, float vel_estimate, float vel_gain, float *I); @@ -61,7 +63,10 @@ static ControllerState state = { .Iq_integrator = 0.0f, .Id_integrator = 0.0f, - .t_plan = 0.0f}; + .t_plan = 0.0f +}; + +Statistics pre_cl_stats = {0}; #if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5 @@ -134,6 +139,16 @@ void Controller_ControlLoop(void) controller_set_state(CONTROLLER_STATE_IDLE); Watchdog_reset(); } + else if ((motor_get_is_gimbal() == false) && pre_cl_stats.size < MAX_CL_INIT_STEPS) + { + CLPreStep(); + } + else if ((motor_get_is_gimbal() == false) && pre_cl_stats.size == MAX_CL_INIT_STEPS) + { + state.pos_setpoint = observer_get_pos_estimate(&position_observer); + CLPreStep(); + CLPreCheck(); + } else { CLControlStep(); @@ -143,6 +158,22 @@ void Controller_ControlLoop(void) } } +TM_RAMFUNC void CLPreStep(void) +{ + gate_driver_set_duty_cycle(&three_phase_zero); + // Should approximate zero as from Kirchoff + float Iphase_meas_sum = state.I_phase_meas.A + state.I_phase_meas.B + state.I_phase_meas.C; + update_statistics(&pre_cl_stats, Iphase_meas_sum); +} + +TM_RAMFUNC void CLPreCheck(void) +{ + if (calculate_standard_deviation(&pre_cl_stats) > PRE_CL_I_SD_MAX) + { + state.errors |= CONTROLLER_ERRORS_PRE_CL_I_SD_EXCEEDED; + } +} + TM_RAMFUNC void CLControlStep(void) { switch (state.mode) @@ -312,7 +343,6 @@ TM_RAMFUNC void controller_set_state(controller_state_options new_state) { if ((new_state == CONTROLLER_STATE_CL_CONTROL) && (state.state == CONTROLLER_STATE_IDLE) && (!errors_exist()) && motor_get_calibrated()) { - state.pos_setpoint = observer_get_pos_estimate(&position_observer); gate_driver_enable(); state.state = CONTROLLER_STATE_CL_CONTROL; } @@ -325,6 +355,7 @@ TM_RAMFUNC void controller_set_state(controller_state_options new_state) { gate_driver_set_duty_cycle(&three_phase_zero); gate_driver_disable(); + memset(&pre_cl_stats, 0, sizeof(pre_cl_stats)); state.state = CONTROLLER_STATE_IDLE; } } diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h index 8c3ebd1d..0c052310 100644 --- a/firmware/src/utils/utils.h +++ b/firmware/src/utils/utils.h @@ -115,7 +115,6 @@ static inline float our_floorf(float x) return (float)((int)x - 1); } - // based on https://github.com/divideconcept/FastTrigo/blob/master/fasttrigo.cpp static inline float cos_32s(float x) { @@ -144,14 +143,24 @@ static inline float fast_sin(float angle) return fast_cos(halfpi-angle); } -static inline int calculate_parity(int x, int mask) +typedef struct { + float sum_current; + float sum_current_squared; + uint32_t size; +} Statistics; + +static inline void update_statistics(Statistics *s, float new_current) +{ + s->sum_current += new_current; + s->sum_current_squared += new_current * new_current; + s->size++; +} + +static inline float calculate_standard_deviation(Statistics *s) { - x &= mask; - x ^= x >> 8; - x ^= x >> 4; - x ^= x >> 2; - x ^= x >> 1; - return x & 1; + float mean = s->sum_current / s->size; + float mean_squares = s->sum_current_squared / s->size; + return fast_sqrt(mean_squares - mean * mean); } // https://github.com/madcowswe/ODrive/blob/3113aedf081cf40e942d25d3b0b36c8806f11f23/Firmware/MotorControl/utils.c