From ba9b4c0e6ec424ebafb0e61babe822b58d12a66e Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 19:11:41 +0300 Subject: [PATCH 1/7] update protocol headers based on new spec --- docs/protocol/reference.rst | 93 ++++++++++++------- firmware/src/can/can_endpoints.c | 20 +++- firmware/src/can/can_endpoints.h | 86 +++++++++-------- firmware/src/tm_enums.h | 54 +++++++---- .../Python/tinymovr/specs/tinymovr_2_0_x.yaml | 8 +- 5 files changed, 168 insertions(+), 93 deletions(-) diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst index 7ba464d9..2b047516 100644 --- a/docs/protocol/reference.rst +++ b/docs/protocol/reference.rst @@ -846,7 +846,7 @@ Type: uint8 -The type of external sensor type. Either MA7XX, AS5047 or AMT22. +The type of the external sensor. Options: @@ -856,11 +856,34 @@ Options: - AMT22 -sensors.setup.external_spi.calibrated +sensors.setup.external_spi.rate ------------------------------------------------------------------- ID: 62 +Type: uint8 + + + +The rate of the external sensor. + +Options: + +- 1_5Mbps + +- 3Mbps + +- 6Mbps + +- 8Mbps + +- 12Mbps + +sensors.setup.external_spi.calibrated +------------------------------------------------------------------- + +ID: 63 + Type: bool @@ -872,7 +895,7 @@ Whether the sensor has been calibrated. sensors.setup.external_spi.errors ------------------------------------------------------------------- -ID: 63 +ID: 64 Type: uint8 @@ -889,7 +912,7 @@ Flags: sensors.setup.hall.calibrated ------------------------------------------------------------------- -ID: 64 +ID: 65 Type: bool @@ -902,7 +925,7 @@ Whether the sensor has been calibrated. sensors.setup.hall.errors ------------------------------------------------------------------- -ID: 65 +ID: 66 Type: uint8 @@ -919,7 +942,7 @@ Flags: sensors.select.position_sensor.connection ------------------------------------------------------------------- -ID: 66 +ID: 67 Type: uint8 @@ -938,7 +961,7 @@ Options: sensors.select.position_sensor.bandwidth ------------------------------------------------------------------- -ID: 67 +ID: 68 Type: float @@ -951,7 +974,7 @@ The position sensor observer bandwidth. sensors.select.position_sensor.raw_angle ------------------------------------------------------------------- -ID: 68 +ID: 69 Type: int32 @@ -964,7 +987,7 @@ The raw position sensor angle. sensors.select.position_sensor.position_estimate ------------------------------------------------------------------- -ID: 69 +ID: 70 Type: float @@ -977,7 +1000,7 @@ The filtered position estimate in the position sensor reference frame. sensors.select.position_sensor.velocity_estimate ------------------------------------------------------------------- -ID: 70 +ID: 71 Type: float @@ -990,7 +1013,7 @@ The filtered velocity estimate in the position sensor reference frame. sensors.select.commutation_sensor.connection ------------------------------------------------------------------- -ID: 71 +ID: 72 Type: uint8 @@ -1009,7 +1032,7 @@ Options: sensors.select.commutation_sensor.bandwidth ------------------------------------------------------------------- -ID: 72 +ID: 73 Type: float @@ -1022,7 +1045,7 @@ The commutation sensor observer bandwidth. sensors.select.commutation_sensor.raw_angle ------------------------------------------------------------------- -ID: 73 +ID: 74 Type: int32 @@ -1035,7 +1058,7 @@ The raw commutation sensor angle. sensors.select.commutation_sensor.position_estimate ------------------------------------------------------------------- -ID: 74 +ID: 75 Type: float @@ -1048,7 +1071,7 @@ The filtered position estimate in the commutation sensor reference frame. sensors.select.commutation_sensor.velocity_estimate ------------------------------------------------------------------- -ID: 75 +ID: 76 Type: float @@ -1061,7 +1084,7 @@ The filtered velocity estimate in the commutation sensor reference frame. traj_planner.max_accel ------------------------------------------------------------------- -ID: 76 +ID: 77 Type: float @@ -1074,7 +1097,7 @@ The max allowed acceleration of the generated trajectory. traj_planner.max_decel ------------------------------------------------------------------- -ID: 77 +ID: 78 Type: float @@ -1087,7 +1110,7 @@ The max allowed deceleration of the generated trajectory. traj_planner.max_vel ------------------------------------------------------------------- -ID: 78 +ID: 79 Type: float @@ -1100,7 +1123,7 @@ The max allowed cruise velocity of the generated trajectory. traj_planner.t_accel ------------------------------------------------------------------- -ID: 79 +ID: 80 Type: float @@ -1113,7 +1136,7 @@ In time mode, the acceleration time of the generated trajectory. traj_planner.t_decel ------------------------------------------------------------------- -ID: 80 +ID: 81 Type: float @@ -1126,7 +1149,7 @@ In time mode, the deceleration time of the generated trajectory. traj_planner.t_total ------------------------------------------------------------------- -ID: 81 +ID: 82 Type: float @@ -1139,7 +1162,7 @@ In time mode, the total time of the generated trajectory. move_to(float pos_setpoint) -> void -------------------------------------------------------------------------------------------- -ID: 82 +ID: 83 Return Type: void @@ -1150,7 +1173,7 @@ Move to target position in the user reference frame respecting velocity and acce move_to_tlimit(float pos_setpoint) -> void -------------------------------------------------------------------------------------------- -ID: 83 +ID: 84 Return Type: void @@ -1161,7 +1184,7 @@ Move to target position in the user reference frame respecting time limits for e traj_planner.errors ------------------------------------------------------------------- -ID: 84 +ID: 85 Type: uint8 @@ -1178,7 +1201,7 @@ Flags: homing.velocity ------------------------------------------------------------------- -ID: 85 +ID: 86 Type: float @@ -1191,7 +1214,7 @@ The velocity at which the motor performs homing. homing.max_homing_t ------------------------------------------------------------------- -ID: 86 +ID: 87 Type: float @@ -1204,7 +1227,7 @@ The maximum time the motor is allowed to travel before homing times out and abor homing.retract_dist ------------------------------------------------------------------- -ID: 87 +ID: 88 Type: float @@ -1217,7 +1240,7 @@ The retraction distance the motor travels after the endstop has been found. homing.warnings ------------------------------------------------------------------- -ID: 88 +ID: 89 Type: uint8 @@ -1232,7 +1255,7 @@ Flags: homing.stall_detect.velocity ------------------------------------------------------------------- -ID: 89 +ID: 90 Type: float @@ -1245,7 +1268,7 @@ The velocity below which (and together with `stall_detect.delta_pos`) stall dete homing.stall_detect.delta_pos ------------------------------------------------------------------- -ID: 90 +ID: 91 Type: float @@ -1258,7 +1281,7 @@ The velocity below which (and together with `stall_detect.delta_pos`) stall dete homing.stall_detect.t ------------------------------------------------------------------- -ID: 91 +ID: 92 Type: float @@ -1271,7 +1294,7 @@ The time to remain in stall detection mode before the motor is considered stalle home() -> void -------------------------------------------------------------------------------------------- -ID: 92 +ID: 93 Return Type: void @@ -1282,7 +1305,7 @@ Perform the homing operation. watchdog.enabled ------------------------------------------------------------------- -ID: 93 +ID: 94 Type: bool @@ -1295,7 +1318,7 @@ Whether the watchdog is enabled or not. watchdog.triggered ------------------------------------------------------------------- -ID: 94 +ID: 95 Type: bool @@ -1308,7 +1331,7 @@ Whether the watchdog has been triggered or not. watchdog.timeout ------------------------------------------------------------------- -ID: 95 +ID: 96 Type: float diff --git a/firmware/src/can/can_endpoints.c b/firmware/src/can/can_endpoints.c index 1878b5f6..cc4f6cfc 100644 --- a/firmware/src/can/can_endpoints.c +++ b/firmware/src/can/can_endpoints.c @@ -18,7 +18,7 @@ #include -uint8_t (*avlos_endpoints[96])(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_warnings, &avlos_save_config, &avlos_erase_config, &avlos_reset, &avlos_enter_dfu, &avlos_config_size, &avlos_scheduler_load, &avlos_scheduler_warnings, &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_comms_can_heartbeat, &avlos_motor_R, &avlos_motor_L, &avlos_motor_pole_pairs, &avlos_motor_type, &avlos_motor_calibrated, &avlos_motor_I_cal, &avlos_motor_errors, &avlos_sensors_user_frame_position_estimate, &avlos_sensors_user_frame_velocity_estimate, &avlos_sensors_user_frame_offset, &avlos_sensors_user_frame_multiplier, &avlos_sensors_setup_onboard_calibrated, &avlos_sensors_setup_onboard_errors, &avlos_sensors_setup_external_spi_type, &avlos_sensors_setup_external_spi_calibrated, &avlos_sensors_setup_external_spi_errors, &avlos_sensors_setup_hall_calibrated, &avlos_sensors_setup_hall_errors, &avlos_sensors_select_position_sensor_connection, &avlos_sensors_select_position_sensor_bandwidth, &avlos_sensors_select_position_sensor_raw_angle, &avlos_sensors_select_position_sensor_position_estimate, &avlos_sensors_select_position_sensor_velocity_estimate, &avlos_sensors_select_commutation_sensor_connection, &avlos_sensors_select_commutation_sensor_bandwidth, &avlos_sensors_select_commutation_sensor_raw_angle, &avlos_sensors_select_commutation_sensor_position_estimate, &avlos_sensors_select_commutation_sensor_velocity_estimate, &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 }; +uint8_t (*avlos_endpoints[97])(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_warnings, &avlos_save_config, &avlos_erase_config, &avlos_reset, &avlos_enter_dfu, &avlos_config_size, &avlos_scheduler_load, &avlos_scheduler_warnings, &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_comms_can_heartbeat, &avlos_motor_R, &avlos_motor_L, &avlos_motor_pole_pairs, &avlos_motor_type, &avlos_motor_calibrated, &avlos_motor_I_cal, &avlos_motor_errors, &avlos_sensors_user_frame_position_estimate, &avlos_sensors_user_frame_velocity_estimate, &avlos_sensors_user_frame_offset, &avlos_sensors_user_frame_multiplier, &avlos_sensors_setup_onboard_calibrated, &avlos_sensors_setup_onboard_errors, &avlos_sensors_setup_external_spi_type, &avlos_sensors_setup_external_spi_rate, &avlos_sensors_setup_external_spi_calibrated, &avlos_sensors_setup_external_spi_errors, &avlos_sensors_setup_hall_calibrated, &avlos_sensors_setup_hall_errors, &avlos_sensors_select_position_sensor_connection, &avlos_sensors_select_position_sensor_bandwidth, &avlos_sensors_select_position_sensor_raw_angle, &avlos_sensors_select_position_sensor_position_estimate, &avlos_sensors_select_position_sensor_velocity_estimate, &avlos_sensors_select_commutation_sensor_connection, &avlos_sensors_select_commutation_sensor_bandwidth, &avlos_sensors_select_commutation_sensor_raw_angle, &avlos_sensors_select_commutation_sensor_position_estimate, &avlos_sensors_select_commutation_sensor_velocity_estimate, &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_get_proto_hash(void) { @@ -881,6 +881,24 @@ else if (AVLOS_CMD_WRITE == cmd) { return AVLOS_RET_NOACTION; } +uint8_t avlos_sensors_setup_external_spi_rate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) +{ +if (AVLOS_CMD_READ == cmd) { + uint8_t v; + v = sensor_external_spi_get_rate_avlos(); + *buffer_len = sizeof(v); + memcpy(buffer, &v, sizeof(v)); + return AVLOS_RET_READ; + } +else if (AVLOS_CMD_WRITE == cmd) { + uint8_t v; + memcpy(&v, buffer, sizeof(v)); + sensor_external_spi_set_rate_avlos(v); + return AVLOS_RET_WRITE; + } + return AVLOS_RET_NOACTION; +} + uint8_t avlos_sensors_setup_external_spi_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) { if (AVLOS_CMD_READ == cmd) { diff --git a/firmware/src/can/can_endpoints.h b/firmware/src/can/can_endpoints.h index 49bac632..a41bc38e 100644 --- a/firmware/src/can/can_endpoints.h +++ b/firmware/src/can/can_endpoints.h @@ -10,8 +10,8 @@ #include #include -static const uint32_t avlos_proto_hash = 318558145; -extern uint8_t (*avlos_endpoints[96])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); +static const uint32_t avlos_proto_hash = 3704894695; +extern uint8_t (*avlos_endpoints[97])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); extern uint32_t _avlos_get_proto_hash(void); /* @@ -749,7 +749,7 @@ uint8_t avlos_sensors_setup_onboard_errors(uint8_t * buffer, uint8_t * buffer_le /* * avlos_sensors_setup_external_spi_type * -* The type of external sensor type. Either MA7XX, AS5047 or AMT22. +* The type of the external sensor. * * Endpoint ID: 61 * @@ -758,12 +758,24 @@ uint8_t avlos_sensors_setup_onboard_errors(uint8_t * buffer, uint8_t * buffer_le */ uint8_t avlos_sensors_setup_external_spi_type(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); +/* +* avlos_sensors_setup_external_spi_rate +* +* The rate of the external sensor. +* +* Endpoint ID: 62 +* +* @param buffer +* @param buffer_len +*/ +uint8_t avlos_sensors_setup_external_spi_rate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd); + /* * avlos_sensors_setup_external_spi_calibrated * * Whether the sensor has been calibrated. * -* Endpoint ID: 62 +* Endpoint ID: 63 * * @param buffer * @param buffer_len @@ -775,7 +787,7 @@ uint8_t avlos_sensors_setup_external_spi_calibrated(uint8_t * buffer, uint8_t * * * Any sensor errors, as a bitmask * -* Endpoint ID: 63 +* Endpoint ID: 64 * * @param buffer * @param buffer_len @@ -787,7 +799,7 @@ uint8_t avlos_sensors_setup_external_spi_errors(uint8_t * buffer, uint8_t * buff * * Whether the sensor has been calibrated. * -* Endpoint ID: 64 +* Endpoint ID: 65 * * @param buffer * @param buffer_len @@ -799,7 +811,7 @@ uint8_t avlos_sensors_setup_hall_calibrated(uint8_t * buffer, uint8_t * buffer_l * * Any sensor errors, as a bitmask * -* Endpoint ID: 65 +* Endpoint ID: 66 * * @param buffer * @param buffer_len @@ -811,7 +823,7 @@ uint8_t avlos_sensors_setup_hall_errors(uint8_t * buffer, uint8_t * buffer_len, * * The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL. * -* Endpoint ID: 66 +* Endpoint ID: 67 * * @param buffer * @param buffer_len @@ -823,7 +835,7 @@ uint8_t avlos_sensors_select_position_sensor_connection(uint8_t * buffer, uint8_ * * The position sensor observer bandwidth. * -* Endpoint ID: 67 +* Endpoint ID: 68 * * @param buffer * @param buffer_len @@ -835,7 +847,7 @@ uint8_t avlos_sensors_select_position_sensor_bandwidth(uint8_t * buffer, uint8_t * * The raw position sensor angle. * -* Endpoint ID: 68 +* Endpoint ID: 69 * * @param buffer * @param buffer_len @@ -847,7 +859,7 @@ uint8_t avlos_sensors_select_position_sensor_raw_angle(uint8_t * buffer, uint8_t * * The filtered position estimate in the position sensor reference frame. * -* Endpoint ID: 69 +* Endpoint ID: 70 * * @param buffer * @param buffer_len @@ -859,7 +871,7 @@ uint8_t avlos_sensors_select_position_sensor_position_estimate(uint8_t * buffer, * * The filtered velocity estimate in the position sensor reference frame. * -* Endpoint ID: 70 +* Endpoint ID: 71 * * @param buffer * @param buffer_len @@ -871,7 +883,7 @@ uint8_t avlos_sensors_select_position_sensor_velocity_estimate(uint8_t * buffer, * * The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL. * -* Endpoint ID: 71 +* Endpoint ID: 72 * * @param buffer * @param buffer_len @@ -883,7 +895,7 @@ uint8_t avlos_sensors_select_commutation_sensor_connection(uint8_t * buffer, uin * * The commutation sensor observer bandwidth. * -* Endpoint ID: 72 +* Endpoint ID: 73 * * @param buffer * @param buffer_len @@ -895,7 +907,7 @@ uint8_t avlos_sensors_select_commutation_sensor_bandwidth(uint8_t * buffer, uint * * The raw commutation sensor angle. * -* Endpoint ID: 73 +* Endpoint ID: 74 * * @param buffer * @param buffer_len @@ -907,7 +919,7 @@ uint8_t avlos_sensors_select_commutation_sensor_raw_angle(uint8_t * buffer, uint * * The filtered position estimate in the commutation sensor reference frame. * -* Endpoint ID: 74 +* Endpoint ID: 75 * * @param buffer * @param buffer_len @@ -919,7 +931,7 @@ uint8_t avlos_sensors_select_commutation_sensor_position_estimate(uint8_t * buff * * The filtered velocity estimate in the commutation sensor reference frame. * -* Endpoint ID: 75 +* Endpoint ID: 76 * * @param buffer * @param buffer_len @@ -931,7 +943,7 @@ uint8_t avlos_sensors_select_commutation_sensor_velocity_estimate(uint8_t * buff * * The max allowed acceleration of the generated trajectory. * -* Endpoint ID: 76 +* Endpoint ID: 77 * * @param buffer * @param buffer_len @@ -943,7 +955,7 @@ uint8_t avlos_traj_planner_max_accel(uint8_t * buffer, uint8_t * buffer_len, Avl * * The max allowed deceleration of the generated trajectory. * -* Endpoint ID: 77 +* Endpoint ID: 78 * * @param buffer * @param buffer_len @@ -955,7 +967,7 @@ uint8_t avlos_traj_planner_max_decel(uint8_t * buffer, uint8_t * buffer_len, Avl * * The max allowed cruise velocity of the generated trajectory. * -* Endpoint ID: 78 +* Endpoint ID: 79 * * @param buffer * @param buffer_len @@ -967,7 +979,7 @@ uint8_t avlos_traj_planner_max_vel(uint8_t * buffer, uint8_t * buffer_len, Avlos * * In time mode, the acceleration time of the generated trajectory. * -* Endpoint ID: 79 +* Endpoint ID: 80 * * @param buffer * @param buffer_len @@ -979,7 +991,7 @@ uint8_t avlos_traj_planner_t_accel(uint8_t * buffer, uint8_t * buffer_len, Avlos * * In time mode, the deceleration time of the generated trajectory. * -* Endpoint ID: 80 +* Endpoint ID: 81 * * @param buffer * @param buffer_len @@ -991,7 +1003,7 @@ uint8_t avlos_traj_planner_t_decel(uint8_t * buffer, uint8_t * buffer_len, Avlos * * In time mode, the total time of the generated trajectory. * -* Endpoint ID: 81 +* Endpoint ID: 82 * * @param buffer * @param buffer_len @@ -1003,7 +1015,7 @@ uint8_t avlos_traj_planner_t_total(uint8_t * buffer, uint8_t * buffer_len, Avlos * * Move to target position in the user reference frame respecting velocity and acceleration limits. * -* Endpoint ID: 82 +* Endpoint ID: 83 * * @param buffer * @param buffer_len @@ -1015,7 +1027,7 @@ uint8_t avlos_traj_planner_move_to(uint8_t * buffer, uint8_t * buffer_len, Avlos * * Move to target position in the user reference frame respecting time limits for each sector. * -* Endpoint ID: 83 +* Endpoint ID: 84 * * @param buffer * @param buffer_len @@ -1027,7 +1039,7 @@ uint8_t avlos_traj_planner_move_to_tlimit(uint8_t * buffer, uint8_t * buffer_len * * Any errors in the trajectory planner, as a bitmask * -* Endpoint ID: 84 +* Endpoint ID: 85 * * @param buffer * @param buffer_len @@ -1039,7 +1051,7 @@ uint8_t avlos_traj_planner_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_ * * The velocity at which the motor performs homing. * -* Endpoint ID: 85 +* Endpoint ID: 86 * * @param buffer * @param buffer_len @@ -1051,7 +1063,7 @@ uint8_t avlos_homing_velocity(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comm * * The maximum time the motor is allowed to travel before homing times out and aborts. * -* Endpoint ID: 86 +* Endpoint ID: 87 * * @param buffer * @param buffer_len @@ -1063,7 +1075,7 @@ uint8_t avlos_homing_max_homing_t(uint8_t * buffer, uint8_t * buffer_len, Avlos_ * * The retraction distance the motor travels after the endstop has been found. * -* Endpoint ID: 87 +* Endpoint ID: 88 * * @param buffer * @param buffer_len @@ -1075,7 +1087,7 @@ uint8_t avlos_homing_retract_dist(uint8_t * buffer, uint8_t * buffer_len, Avlos_ * * Any homing warnings, as a bitmask * -* Endpoint ID: 88 +* Endpoint ID: 89 * * @param buffer * @param buffer_len @@ -1087,7 +1099,7 @@ uint8_t avlos_homing_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comm * * The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered. * -* Endpoint ID: 89 +* Endpoint ID: 90 * * @param buffer * @param buffer_len @@ -1099,7 +1111,7 @@ uint8_t avlos_homing_stall_detect_velocity(uint8_t * buffer, uint8_t * buffer_le * * The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered. * -* Endpoint ID: 90 +* Endpoint ID: 91 * * @param buffer * @param buffer_len @@ -1111,7 +1123,7 @@ uint8_t avlos_homing_stall_detect_delta_pos(uint8_t * buffer, uint8_t * buffer_l * * The time to remain in stall detection mode before the motor is considered stalled. * -* Endpoint ID: 91 +* Endpoint ID: 92 * * @param buffer * @param buffer_len @@ -1123,7 +1135,7 @@ uint8_t avlos_homing_stall_detect_t(uint8_t * buffer, uint8_t * buffer_len, Avlo * * Perform the homing operation. * -* Endpoint ID: 92 +* Endpoint ID: 93 * * @param buffer * @param buffer_len @@ -1135,7 +1147,7 @@ uint8_t avlos_homing_home(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command * * Whether the watchdog is enabled or not. * -* Endpoint ID: 93 +* Endpoint ID: 94 * * @param buffer * @param buffer_len @@ -1147,7 +1159,7 @@ uint8_t avlos_watchdog_enabled(uint8_t * buffer, uint8_t * buffer_len, Avlos_Com * * Whether the watchdog has been triggered or not. * -* Endpoint ID: 94 +* Endpoint ID: 95 * * @param buffer * @param buffer_len @@ -1159,7 +1171,7 @@ uint8_t avlos_watchdog_triggered(uint8_t * buffer, uint8_t * buffer_len, Avlos_C * * The watchdog timeout period. * -* Endpoint ID: 95 +* Endpoint ID: 96 * * @param buffer * @param buffer_len diff --git a/firmware/src/tm_enums.h b/firmware/src/tm_enums.h index e8b9f801..6b97558d 100644 --- a/firmware/src/tm_enums.h +++ b/firmware/src/tm_enums.h @@ -103,43 +103,59 @@ typedef enum typedef enum { - CONTROLLER_STATE_IDLE = 0, - CONTROLLER_STATE_CALIBRATE = 1, - CONTROLLER_STATE_CL_CONTROL = 2 + CONTROLLER_STATE_IDLE = 0, + CONTROLLER_STATE_CALIBRATE = 1, + CONTROLLER_STATE_CL_CONTROL = 2, + CONTROLLER_STATE__MAX } controller_state_options; typedef enum { - CONTROLLER_MODE_CURRENT = 0, - CONTROLLER_MODE_VELOCITY = 1, - CONTROLLER_MODE_POSITION = 2, - CONTROLLER_MODE_TRAJECTORY = 3, - CONTROLLER_MODE_HOMING = 4 + CONTROLLER_MODE_CURRENT = 0, + CONTROLLER_MODE_VELOCITY = 1, + CONTROLLER_MODE_POSITION = 2, + CONTROLLER_MODE_TRAJECTORY = 3, + CONTROLLER_MODE_HOMING = 4, + CONTROLLER_MODE__MAX } controller_mode_options; typedef enum { - MOTOR_TYPE_HIGH_CURRENT = 0, - MOTOR_TYPE_GIMBAL = 1 + MOTOR_TYPE_HIGH_CURRENT = 0, + MOTOR_TYPE_GIMBAL = 1, + MOTOR_TYPE__MAX } motor_type_options; typedef enum { - SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX = 0, - SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047 = 1, - SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22 = 2 + SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX = 0, + SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047 = 1, + SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22 = 2, + SENSORS_SETUP_EXTERNAL_SPI_TYPE__MAX } sensors_setup_external_spi_type_options; typedef enum { - SENSORS_SELECT_POSITION_SENSOR_CONNECTION_ONBOARD = 0, - SENSORS_SELECT_POSITION_SENSOR_CONNECTION_EXTERNAL_SPI = 1, - SENSORS_SELECT_POSITION_SENSOR_CONNECTION_HALL = 2 + SENSORS_SETUP_EXTERNAL_SPI_RATE_1_5Mbps = 0, + SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps = 1, + SENSORS_SETUP_EXTERNAL_SPI_RATE_6Mbps = 2, + SENSORS_SETUP_EXTERNAL_SPI_RATE_8Mbps = 3, + SENSORS_SETUP_EXTERNAL_SPI_RATE_12Mbps = 4, + SENSORS_SETUP_EXTERNAL_SPI_RATE__MAX +} sensors_setup_external_spi_rate_options; + +typedef enum +{ + SENSORS_SELECT_POSITION_SENSOR_CONNECTION_ONBOARD = 0, + SENSORS_SELECT_POSITION_SENSOR_CONNECTION_EXTERNAL_SPI = 1, + SENSORS_SELECT_POSITION_SENSOR_CONNECTION_HALL = 2, + SENSORS_SELECT_POSITION_SENSOR_CONNECTION__MAX } sensors_select_position_sensor_connection_options; typedef enum { - SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_ONBOARD = 0, - SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_EXTERNAL_SPI = 1, - SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_HALL = 2 + SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_ONBOARD = 0, + SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_EXTERNAL_SPI = 1, + SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_HALL = 2, + SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION__MAX } sensors_select_commutation_sensor_connection_options; diff --git a/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml b/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml index aabaacec..a1ad30f2 100644 --- a/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml +++ b/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml @@ -386,7 +386,13 @@ remote_attributes: meta: {export: True} getter_name: sensor_external_spi_get_type_avlos setter_name: sensor_external_spi_set_type_avlos - summary: The type of external sensor type. Either MA7XX, AS5047 or AMT22. + 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} From e0cd57ccc9a46819a6da6935c11b6b70d55745dc Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 19:12:16 +0300 Subject: [PATCH 2/7] add init_func for initialization using sensor pointer only --- firmware/src/sensor/amt22.c | 8 +++++++- firmware/src/sensor/amt22.h | 4 +++- firmware/src/sensor/as5047.c | 8 +++++++- firmware/src/sensor/as5047.h | 5 +++-- firmware/src/sensor/hall.c | 7 +++++++ firmware/src/sensor/hall.h | 1 + firmware/src/sensor/ma7xx.c | 7 +++++++ firmware/src/sensor/ma7xx.h | 2 ++ firmware/src/sensor/sensor.h | 2 ++ 9 files changed, 39 insertions(+), 5 deletions(-) diff --git a/firmware/src/sensor/amt22.c b/firmware/src/sensor/amt22.c index 6a58b8ee..05d75674 100644 --- a/firmware/src/sensor/amt22.c +++ b/firmware/src/sensor/amt22.c @@ -32,7 +32,8 @@ void amt22_make_blank_sensor(Sensor *s) s->update_func = amt22_update; s->prepare_func = amt22_send_angle_cmd; s->reset_func = amt22_reset; - s->deinit_func = amt22_deinit; + s->init_func = amt22_init; + s->deinit_func = amt22_deinit; s->get_errors_func = amt22_get_errors; s->is_calibrated_func = amt22_is_calibrated; s->get_ss_config_func = amt22_get_ss_config; @@ -48,6 +49,11 @@ bool amt22_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *s bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c) { AMT22Sensor *as = (AMT22Sensor *)s; as->config = *c; + return amt22_init(s); +} + +bool amt22_init(Sensor *s) { + AMT22Sensor *as = (AMT22Sensor *)s; ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_8, 0, 0); delay_us(10000); diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h index c84881f7..ac7faa8d 100644 --- a/firmware/src/sensor/amt22.h +++ b/firmware/src/sensor/amt22.h @@ -37,6 +37,7 @@ typedef struct { SSP_TYPE ssp_port; PAC55XX_SSP_TYPEDEF *ssp_struct; + sensors_setup_external_spi_rate_options rate; } AMT22SensorConfig; typedef struct @@ -50,6 +51,7 @@ typedef struct void amt22_make_blank_sensor(Sensor *s); bool amt22_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c); +bool amt22_init(Sensor *s); void amt22_deinit(Sensor *s); void amt22_reset(Sensor *s); void amt22_get_ss_config(Sensor *s, void* buffer); @@ -72,7 +74,7 @@ static inline void amt22_send_angle_cmd(const Sensor *s) // 3us + an experimentally defined value to account for transmission // delay. ssp_write_one(((const AMT22Sensor *)s)->config.ssp_struct, AMT22_CMD_READ_ANGLE); - delay_us(6); + delay_us(5); ssp_write_one(((const AMT22Sensor *)s)->config.ssp_struct, AMT22_CMD_READ_ANGLE); } diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c index 7b8368f0..14bdd230 100644 --- a/firmware/src/sensor/as5047.c +++ b/firmware/src/sensor/as5047.c @@ -31,7 +31,8 @@ void as5047p_make_blank_sensor(Sensor *s) s->get_raw_angle_func = as5047p_get_raw_angle; s->update_func = as5047p_update; s->prepare_func = as5047p_send_angle_cmd; - s->reset_func = as5047p_reset; + s->reset_func = as5047p_reset; + s->init_func = as5047p_init; s->deinit_func = as5047p_deinit; s->get_errors_func = as5047p_get_errors; s->is_calibrated_func = as5047p_is_calibrated; @@ -48,6 +49,11 @@ bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c) { AS5047PSensor *as = (AS5047PSensor *)s; as->config = *c; + return as5047p_init(s); +} + +bool as5047p_init(Sensor *s) { + AS5047PSensor *as = (AS5047PSensor *)s; ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_16, 1, 0); delay_us(10000); // Example delay, adjust based on AS5047P datasheet diff --git a/firmware/src/sensor/as5047.h b/firmware/src/sensor/as5047.h index afd86c18..c84d2db5 100644 --- a/firmware/src/sensor/as5047.h +++ b/firmware/src/sensor/as5047.h @@ -31,14 +31,14 @@ typedef enum { AS5047P_CMD_NOP = 0x0000, - AS5047P_CMD_READ_ANGLE = 0x3FFF // Adjust based on the AS5047P datasheet + AS5047P_CMD_READ_ANGLE = 0x3FFF } AS5047PCommand; typedef struct { SSP_TYPE ssp_port; PAC55XX_SSP_TYPEDEF *ssp_struct; - // Add any additional configuration parameters needed for AS5047P + sensors_setup_external_spi_rate_options rate; } AS5047PSensorConfig; typedef struct @@ -52,6 +52,7 @@ typedef struct void as5047p_make_blank_sensor(Sensor *s); bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c); +bool as5047p_init(Sensor *s); void as5047p_deinit(Sensor *s); void as5047p_reset(Sensor *s); void as5047p_get_ss_config(Sensor *s, void* buffer); diff --git a/firmware/src/sensor/hall.c b/firmware/src/sensor/hall.c index 84308a6d..38d7a368 100644 --- a/firmware/src/sensor/hall.c +++ b/firmware/src/sensor/hall.c @@ -33,6 +33,7 @@ void hall_make_blank_sensor(Sensor *s) s->get_raw_angle_func = hall_get_angle; s->update_func = hall_update; s->reset_func = hall_reset; + s->init_func = hall_init; s->deinit_func = hall_deinit; s->get_errors_func = hall_get_errors; s->is_calibrated_func = hall_sector_map_is_calibrated; @@ -50,6 +51,12 @@ bool hall_init_with_config(Sensor *s, const HallSensorConfig *c) { HallSensor *ms = (HallSensor *)s; ms->config = *c; + return hall_init(s); +} + +bool hall_init(Sensor *s) +{ + HallSensor *ms = (HallSensor *)s; ms->hw_defaults[0] = pac5xxx_tile_register_read(ADDR_CFGAIO7); ms->hw_defaults[1] = pac5xxx_tile_register_read(ADDR_CFGAIO8); ms->hw_defaults[2] = pac5xxx_tile_register_read(ADDR_CFGAIO9); diff --git a/firmware/src/sensor/hall.h b/firmware/src/sensor/hall.h index d1cb3078..c2fcf5ca 100644 --- a/firmware/src/sensor/hall.h +++ b/firmware/src/sensor/hall.h @@ -45,6 +45,7 @@ typedef struct void hall_make_blank_sensor(Sensor *s); bool hall_init_with_defaults(Sensor *s); bool hall_init_with_config(Sensor *s, const HallSensorConfig *c); +bool hall_init(Sensor *s); void hall_deinit(Sensor *s); void hall_reset(Sensor *s); bool hall_calibrate_sequence(Sensor *s, Observer *o); diff --git a/firmware/src/sensor/ma7xx.c b/firmware/src/sensor/ma7xx.c index e86e156c..440a77f8 100644 --- a/firmware/src/sensor/ma7xx.c +++ b/firmware/src/sensor/ma7xx.c @@ -30,6 +30,7 @@ void ma7xx_make_blank_sensor(Sensor *s) s->normalization_factor = SENSOR_COMMON_RES_TICKS_FLOAT / s->ticks; s->is_calibrated_func = ma7xx_rec_is_calibrated; s->get_raw_angle_func = ma7xx_get_raw_angle; + s->init_func = ma7xx_init; s->deinit_func = ma7xx_deinit; s->reset_func = ma7xx_reset; s->update_func = ma7xx_update; @@ -50,6 +51,12 @@ bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *s { MA7xxSensor *ms = (MA7xxSensor *)s; ms->config = *c; + return ma7xx_init(s); +} + + bool ma7xx_init(Sensor *s) +{ + MA7xxSensor *ms = (MA7xxSensor *)s; ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 4, SSP_DATA_SIZE_16, 0, 0); delay_us(16000); // ensure 16ms sensor startup time as per the datasheet ma7xx_send_angle_cmd(s); diff --git a/firmware/src/sensor/ma7xx.h b/firmware/src/sensor/ma7xx.h index 21cf8beb..cbed16a4 100644 --- a/firmware/src/sensor/ma7xx.h +++ b/firmware/src/sensor/ma7xx.h @@ -40,6 +40,7 @@ typedef struct { SSP_TYPE ssp_port; PAC55XX_SSP_TYPEDEF *ssp_struct; + sensors_setup_external_spi_rate_options rate; } MA7xxSensorConfig; typedef struct @@ -53,6 +54,7 @@ typedef struct void ma7xx_make_blank_sensor(Sensor *s); bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c); +bool ma7xx_init(Sensor *s); void ma7xx_deinit(Sensor *s); void ma7xx_reset(Sensor *s); void ma7xx_get_ss_config(Sensor *s, void* buffer); diff --git a/firmware/src/sensor/sensor.h b/firmware/src/sensor/sensor.h index 46e230b8..ffac1076 100644 --- a/firmware/src/sensor/sensor.h +++ b/firmware/src/sensor/sensor.h @@ -39,6 +39,7 @@ typedef struct Observer Observer; typedef bool (*sensor_is_calibrated_func_t)(const Sensor *); typedef bool (*sensor_calibrate_func_t)(Sensor *, Observer *); typedef int32_t (*sensor_get_raw_angle_func_t)(const Sensor *); +typedef bool (*sensor_init_func_t)(Sensor *); typedef void (*sensor_deinit_func_t)(Sensor *); typedef void (*sensor_reset_func_t)(Sensor *); typedef void (*sensor_prepare_func_t)(const Sensor *); @@ -74,6 +75,7 @@ struct Sensor { // typedefd earlier sensor_is_calibrated_func_t is_calibrated_func; sensor_calibrate_func_t calibrate_func; sensor_get_raw_angle_func_t get_raw_angle_func; + sensor_init_func_t init_func; sensor_deinit_func_t deinit_func; sensor_reset_func_t reset_func; sensor_update_func_t update_func; From b628c28f351d21a55e99a6a62fe6e9d9c555319f Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 19:12:25 +0300 Subject: [PATCH 3/7] add getter and setter for rate --- firmware/src/sensor/sensors.c | 82 +++++++++++++++++++++++++---------- firmware/src/sensor/sensors.h | 20 +++++++++ 2 files changed, 80 insertions(+), 22 deletions(-) diff --git a/firmware/src/sensor/sensors.c b/firmware/src/sensor/sensors.c index 65e22dfd..86649966 100644 --- a/firmware/src/sensor/sensors.c +++ b/firmware/src/sensor/sensors.c @@ -183,29 +183,24 @@ void position_sensor_set_connection(sensor_connection_t new_connection) void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options type) { - sensor_type_t internal_type; - bool valid = false; - switch (type) - { - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX: - internal_type = SENSOR_TYPE_MA7XX; - valid = true; - break; - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047: - internal_type = SENSOR_TYPE_AS5047; - valid = true; - break; - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22: - internal_type = SENSOR_TYPE_AMT22; - valid = true; - break; - default: - break; - } - if (valid - && (controller_get_state() == CONTROLLER_STATE_IDLE) - && (internal_type != sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type)) + if (type < SENSORS_SETUP_EXTERNAL_SPI_TYPE__MAX + && (controller_get_state() == CONTROLLER_STATE_IDLE)) { + sensor_type_t internal_type; + switch (type) + { + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX: + internal_type = SENSOR_TYPE_MA7XX; + break; + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047: + internal_type = SENSOR_TYPE_AS5047; + break; + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22: + internal_type = SENSOR_TYPE_AMT22; + break; + default: + break; + } frames_reset_calibrated(); Sensor *s = &(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor); @@ -218,6 +213,49 @@ void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options } } +void sensor_external_spi_set_rate_avlos(sensors_setup_external_spi_rate_options rate) +{ + if (rate < SENSORS_SETUP_EXTERNAL_SPI_RATE__MAX + && (controller_get_state() == CONTROLLER_STATE_IDLE)) + { + sensors_setup_external_spi_rate_options current_rate; + switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type) + { + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX: + current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate; + break; + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047: + current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate; + break; + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22: + current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate; + break; + default: + current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate; + break; + } + if (rate != current_rate) + { + sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.deinit_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor)); + switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type) + { + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX: + sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate = rate; + break; + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047: + sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate = rate; + break; + case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22: + current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate = rate; + break; + default: + break; + } + sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.init_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor)); + } + } +} + void sensor_set_connection(Sensor** target_sensor_p, Sensor** other_sensor_p, sensor_connection_t new_connection) { if (controller_get_state() == CONTROLLER_STATE_IDLE diff --git a/firmware/src/sensor/sensors.h b/firmware/src/sensor/sensors.h index 7846fb8c..b9af4439 100644 --- a/firmware/src/sensor/sensors.h +++ b/firmware/src/sensor/sensors.h @@ -149,7 +149,27 @@ static inline sensors_setup_external_spi_type_options sensor_external_spi_get_ty } } +static inline sensors_setup_external_spi_rate_options sensor_external_spi_get_rate_avlos(void) +{ + switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type) + { + case SENSOR_TYPE_MA7XX: + return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate; + break; + case SENSOR_TYPE_AS5047: + return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate; + break; + case SENSOR_TYPE_AMT22: + return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate; + break; + default: + return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate; + break; + } +} + void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options type); +void sensor_external_spi_set_rate_avlos(sensors_setup_external_spi_rate_options rate); static inline bool sensor_onboard_get_is_calibrated(void) { From 1f41182c25c1817a903a5f1a161f37d2ff640482 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 19:22:55 +0300 Subject: [PATCH 4/7] include spi rate as an init parameter --- firmware/src/sensor/amt22.c | 10 +++++++--- firmware/src/sensor/amt22.h | 2 +- firmware/src/sensor/as5047.c | 10 +++++++--- firmware/src/sensor/as5047.h | 2 +- firmware/src/sensor/ma7xx.c | 3 ++- firmware/src/sensor/ma7xx.h | 2 +- firmware/src/sensor/sensors.c | 8 ++++---- 7 files changed, 23 insertions(+), 14 deletions(-) diff --git a/firmware/src/sensor/amt22.c b/firmware/src/sensor/amt22.c index 05d75674..f6287c59 100644 --- a/firmware/src/sensor/amt22.c +++ b/firmware/src/sensor/amt22.c @@ -39,20 +39,24 @@ void amt22_make_blank_sensor(Sensor *s) s->get_ss_config_func = amt22_get_ss_config; } -bool amt22_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct) { +bool amt22_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate) +{ AMT22SensorConfig c = {0}; c.ssp_port = port; c.ssp_struct = ssp_struct; + c.rate = rate; return amt22_init_with_config(s, &c); } -bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c) { +bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c) +{ AMT22Sensor *as = (AMT22Sensor *)s; as->config = *c; return amt22_init(s); } -bool amt22_init(Sensor *s) { +bool amt22_init(Sensor *s) +{ AMT22Sensor *as = (AMT22Sensor *)s; ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_8, 0, 0); delay_us(10000); diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h index ac7faa8d..a7e8a0ab 100644 --- a/firmware/src/sensor/amt22.h +++ b/firmware/src/sensor/amt22.h @@ -49,7 +49,7 @@ typedef struct } AMT22Sensor; void amt22_make_blank_sensor(Sensor *s); -bool amt22_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); +bool amt22_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate); bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c); bool amt22_init(Sensor *s); void amt22_deinit(Sensor *s); diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c index 14bdd230..77d297ed 100644 --- a/firmware/src/sensor/as5047.c +++ b/firmware/src/sensor/as5047.c @@ -39,20 +39,24 @@ void as5047p_make_blank_sensor(Sensor *s) s->get_ss_config_func = as5047p_get_ss_config; } -bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct) { +bool as5047p_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate) +{ AS5047PSensorConfig c = {0}; c.ssp_port = port; c.ssp_struct = ssp_struct; + c.rate = rate; return as5047p_init_with_config(s, &c); } -bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c) { +bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c) +{ AS5047PSensor *as = (AS5047PSensor *)s; as->config = *c; return as5047p_init(s); } -bool as5047p_init(Sensor *s) { +bool as5047p_init(Sensor *s) +{ AS5047PSensor *as = (AS5047PSensor *)s; ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_16, 1, 0); delay_us(10000); // Example delay, adjust based on AS5047P datasheet diff --git a/firmware/src/sensor/as5047.h b/firmware/src/sensor/as5047.h index c84d2db5..81b17fe9 100644 --- a/firmware/src/sensor/as5047.h +++ b/firmware/src/sensor/as5047.h @@ -50,7 +50,7 @@ typedef struct } AS5047PSensor; void as5047p_make_blank_sensor(Sensor *s); -bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); +bool as5047p_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate); bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c); bool as5047p_init(Sensor *s); void as5047p_deinit(Sensor *s); diff --git a/firmware/src/sensor/ma7xx.c b/firmware/src/sensor/ma7xx.c index 440a77f8..ec0f14b0 100644 --- a/firmware/src/sensor/ma7xx.c +++ b/firmware/src/sensor/ma7xx.c @@ -39,11 +39,12 @@ void ma7xx_make_blank_sensor(Sensor *s) s->get_ss_config_func = ma7xx_get_ss_config; } -bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct) +bool ma7xx_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate) { MA7xxSensorConfig c = {0}; c.ssp_port = port; c.ssp_struct = ssp_struct; + c.rate = rate; return ma7xx_init_with_config(s, &c); } diff --git a/firmware/src/sensor/ma7xx.h b/firmware/src/sensor/ma7xx.h index cbed16a4..b36e0e2a 100644 --- a/firmware/src/sensor/ma7xx.h +++ b/firmware/src/sensor/ma7xx.h @@ -52,7 +52,7 @@ typedef struct } MA7xxSensor; void ma7xx_make_blank_sensor(Sensor *s); -bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); +bool ma7xx_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate); bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c); bool ma7xx_init(Sensor *s); void ma7xx_deinit(Sensor *s); diff --git a/firmware/src/sensor/sensors.c b/firmware/src/sensor/sensors.c index 86649966..de4799e5 100644 --- a/firmware/src/sensor/sensors.c +++ b/firmware/src/sensor/sensors.c @@ -70,20 +70,20 @@ bool sensor_init_with_defaults(Sensor *s) switch (connection) { case SENSOR_CONNECTION_ONBOARD_SPI: - return ma7xx_init_with_port(s, ONBOARD_SENSOR_SSP_PORT, ONBOARD_SENSOR_SSP_STRUCT); + return ma7xx_init_with_port_and_rate(s, ONBOARD_SENSOR_SSP_PORT, ONBOARD_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_12Mbps); break; #if defined BOARD_REV_R5 || defined BOARD_REV_M5 case SENSOR_CONNECTION_EXTERNAL_SPI: switch (s->config.type) { case SENSOR_TYPE_MA7XX: - return ma7xx_init_with_port(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT); + return ma7xx_init_with_port_and_rate(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps); break; case SENSOR_TYPE_AS5047: - return as5047p_init_with_port(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT); + return as5047p_init_with_port_and_rate(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps); break; case SENSOR_TYPE_AMT22: - return amt22_init_with_port(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT); + return amt22_init_with_port_and_rate(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps); break; default: break; From f555ea2c7272c812f006e3c7d4916d5e2be68ffe Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 23:40:03 +0300 Subject: [PATCH 5/7] config rates in blank objects --- firmware/src/sensor/amt22.c | 2 ++ firmware/src/sensor/as5047.c | 2 ++ firmware/src/sensor/ma7xx.c | 2 ++ 3 files changed, 6 insertions(+) diff --git a/firmware/src/sensor/amt22.c b/firmware/src/sensor/amt22.c index f6287c59..7481ee72 100644 --- a/firmware/src/sensor/amt22.c +++ b/firmware/src/sensor/amt22.c @@ -24,6 +24,8 @@ void amt22_make_blank_sensor(Sensor *s) { + AMT22Sensor *as = (AMT22Sensor *)s; + as->config.rate = SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps; s->config.type = SENSOR_TYPE_AMT22; s->bits = AMT22_BITS; s->ticks = AMT22_TICKS; diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c index 77d297ed..611401a8 100644 --- a/firmware/src/sensor/as5047.c +++ b/firmware/src/sensor/as5047.c @@ -24,6 +24,8 @@ void as5047p_make_blank_sensor(Sensor *s) { + AS5047PSensor *as = (AS5047PSensor *)s; + as->config.rate = SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps; s->config.type = SENSOR_TYPE_AS5047; s->bits = AS5047_BITS; s->ticks = AS5047_TICKS; diff --git a/firmware/src/sensor/ma7xx.c b/firmware/src/sensor/ma7xx.c index ec0f14b0..bfae672a 100644 --- a/firmware/src/sensor/ma7xx.c +++ b/firmware/src/sensor/ma7xx.c @@ -24,6 +24,8 @@ void ma7xx_make_blank_sensor(Sensor *s) { + MA7xxSensor *as = (MA7xxSensor *)s; + as->config.rate = SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps; s->config.type = SENSOR_TYPE_MA7XX; s->bits = MA7XX_BITS; s->ticks = MA7XX_TICKS; From f464ddb7f09fa1c03bf777c8ff6003b0ac9510f8 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 23:40:28 +0300 Subject: [PATCH 6/7] add test for different baud rates --- studio/Python/tests/test_as5047.py | 54 ++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/studio/Python/tests/test_as5047.py b/studio/Python/tests/test_as5047.py index a0cabc74..f3971d53 100644 --- a/studio/Python/tests/test_as5047.py +++ b/studio/Python/tests/test_as5047.py @@ -167,5 +167,59 @@ def test_b_position_control_following_sensor_change(self): time.sleep(0.2) + def test_c_position_control_change_baud_rates(self): + """ + Test position control with external sensor and + different baud rates. + """ + self.check_state(0) + self.tm.erase_config() + time.sleep(0.2) + + self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047) + self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI) + + self.tm.motor.I_cal = 0.8 + + self.try_calibrate() + + self.tm.controller.position.p_gain = 9 + self.tm.controller.velocity.p_gain = 2e-5 + self.tm.controller.velocity.i_gain = 0 + + self.tm.controller.position_mode() + self.check_state(2) + + for _ in range(10): + new_pos = random.uniform(-20000, 20000) + self.tm.controller.position.setpoint = new_pos * tick + time.sleep(tsleep) + self.assertAlmostEqual( + self.tm.sensors.user_frame.position_estimate, + self.tm.controller.position.setpoint, + delta=2000 * tick, + ) + + self.tm.controller.idle() + + self.tm.sensors.setup.external_spi.rate = "1_5Mbps" + time.sleep(0.1) + + self.tm.controller.position_mode() + self.check_state(2) + + for _ in range(10): + new_pos = random.uniform(-20000, 20000) + self.tm.controller.position.setpoint = new_pos * tick + time.sleep(tsleep) + self.assertAlmostEqual( + self.tm.sensors.user_frame.position_estimate, + self.tm.controller.position.setpoint, + delta=2000 * tick, + ) + + self.tm.controller.idle() + + if __name__ == "__main__": unittest.main() From 2d5fa50dd3c3733bbfc1b36eff13176db6937356 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sun, 21 Apr 2024 23:40:37 +0300 Subject: [PATCH 7/7] use internal enums --- firmware/src/sensor/sensors.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/firmware/src/sensor/sensors.c b/firmware/src/sensor/sensors.c index de4799e5..41cafa38 100644 --- a/firmware/src/sensor/sensors.c +++ b/firmware/src/sensor/sensors.c @@ -199,6 +199,7 @@ void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options internal_type = SENSOR_TYPE_AMT22; break; default: + internal_type = SENSOR_TYPE_MA7XX; break; } frames_reset_calibrated(); @@ -221,13 +222,13 @@ void sensor_external_spi_set_rate_avlos(sensors_setup_external_spi_rate_options sensors_setup_external_spi_rate_options current_rate; switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type) { - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX: + case SENSOR_TYPE_MA7XX: current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate; break; - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047: + case SENSOR_TYPE_AS5047: current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate; break; - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22: + case SENSOR_TYPE_AMT22: current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate; break; default: @@ -239,13 +240,13 @@ void sensor_external_spi_set_rate_avlos(sensors_setup_external_spi_rate_options sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.deinit_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor)); switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type) { - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX: + case SENSOR_TYPE_MA7XX: sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate = rate; break; - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047: + case SENSOR_TYPE_AS5047: sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate = rate; break; - case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22: + case SENSOR_TYPE_AMT22: current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate = rate; break; default: