diff --git a/firmware/src/sensor/amt22.c b/firmware/src/sensor/amt22.c index 7481ee72..d2dd575f 100644 --- a/firmware/src/sensor/amt22.c +++ b/firmware/src/sensor/amt22.c @@ -60,11 +60,12 @@ bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c) 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); - + ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_8, SWSEL_SW, 0, 0); + delay_us(50000); + amt22_send_angle_cmd(s); amt22_update(s, false); + s->initialized = true; return true; } diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h index a7e8a0ab..86a11d6e 100644 --- a/firmware/src/sensor/amt22.h +++ b/firmware/src/sensor/amt22.h @@ -46,6 +46,9 @@ typedef struct AMT22SensorConfig config; uint8_t errors; int32_t angle; + uint8_t val_l; + uint8_t val_h; + } AMT22Sensor; void amt22_make_blank_sensor(Sensor *s); @@ -69,13 +72,19 @@ static inline uint8_t amt22_get_errors(const Sensor *s) static inline void amt22_send_angle_cmd(const Sensor *s) { // AMT22 has specific timing requirements with respect to reading - // the first and second bytes of the angle. For this reason we - // insert the 6us delay that corresponds to the datasheet-specified - // 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(5); - ssp_write_one(((const AMT22Sensor *)s)->config.ssp_struct, AMT22_CMD_READ_ANGLE); + // the first and second bytes of the angle. + // TODO: These delays make reading of the sensor really slow + // refactor using interrupts + AMT22Sensor *as = ((AMT22Sensor *)s); + as->config.ssp_struct->SSCR.SWSS = 0; + delay_us(3); + ssp_write_one(as->config.ssp_struct, AMT22_CMD_READ_ANGLE); + as->val_h = ssp_read_one(as->config.ssp_struct); + delay_us(3); + ssp_write_one(as->config.ssp_struct, AMT22_CMD_READ_ANGLE); + as->val_l = ssp_read_one(as->config.ssp_struct); + delay_us(3); + as->config.ssp_struct->SSCR.SWSS = 1; } static inline int32_t amt22_get_raw_angle(const Sensor *s) @@ -86,21 +95,35 @@ static inline int32_t amt22_get_raw_angle(const Sensor *s) static inline void amt22_update(Sensor *s, bool check_error) { AMT22Sensor *as = (AMT22Sensor *)s; - volatile uint8_t val_l = ssp_read_one(as->config.ssp_struct); - volatile uint8_t val_h = ssp_read_one(as->config.ssp_struct); - const int32_t angle = (((val_h & 0xff) << 8) | (val_l & 0xff)) & 0x3FFF; - if (check_error) + const int32_t value = (((as->val_h & 0xff) << 8) | (as->val_l & 0xff)); + + // TODO: This checksum calculation is sub-optimal + bool binaryArray[16]; + + for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) & (value >> (i)); + + if ((binaryArray[15] == !(binaryArray[13] ^ binaryArray[11] ^ binaryArray[9] ^ binaryArray[7] ^ binaryArray[5] ^ binaryArray[3] ^ binaryArray[1])) + && (binaryArray[14] == !(binaryArray[12] ^ binaryArray[10] ^ binaryArray[8] ^ binaryArray[6] ^ binaryArray[4] ^ binaryArray[2] ^ binaryArray[0]))) { - const int32_t delta = as->angle - angle; - if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) && - ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) && - ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) ) + const int32_t angle = value & 0x3FFF; + if (check_error) { - as->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE; + const int32_t delta = as->angle - angle; + if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) && + ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) && + ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) ) + { + + } + else + { + as->angle = value & 0x3FFF; + } } + } - as->angle = angle; + } diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c index 611401a8..0659bee7 100644 --- a/firmware/src/sensor/as5047.c +++ b/firmware/src/sensor/as5047.c @@ -60,7 +60,7 @@ bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c) 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); + ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_16, SWSEL_SPI, 1, 0); delay_us(10000); // Example delay, adjust based on AS5047P datasheet as5047p_send_angle_cmd(s); diff --git a/firmware/src/sensor/ma7xx.c b/firmware/src/sensor/ma7xx.c index bfae672a..1d011626 100644 --- a/firmware/src/sensor/ma7xx.c +++ b/firmware/src/sensor/ma7xx.c @@ -60,7 +60,7 @@ bool ma7xx_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_T 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); + ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 4, SSP_DATA_SIZE_16, SWSEL_SPI, 0, 0); delay_us(16000); // ensure 16ms sensor startup time as per the datasheet ma7xx_send_angle_cmd(s); ma7xx_update(s, false); diff --git a/firmware/src/ssp/ssp_func.c b/firmware/src/ssp/ssp_func.c index a1903913..86569870 100644 --- a/firmware/src/ssp/ssp_func.c +++ b/firmware/src/ssp/ssp_func.c @@ -759,7 +759,7 @@ void ssp_interrupt_disable(SSP_TYPE ssp) } } -void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t cph, uint8_t cpol) +void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, SSP_SWSEL_TYPE swsel_type, uint8_t cph, uint8_t cpol) { PAC55XX_SSP_TYPEDEF *ssp_ptr; @@ -806,6 +806,7 @@ void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data ssp_ptr->CON.CPO = cpol; // Clock Out Polarity ssp_ptr->CON.DSS = data_size; // Data Size Select ssp_ptr->CON.SOD = SSP_OUTPUT_NOT_DRIVE; // Slave Output Disable + ssp_ptr->SSCR.SWSEL = swsel_type; // Device Select Pin to be controlled by SPI (0) or software (1) ssp_io_config(ssp, ms_mode); ssp_interrupt_enable(ssp); diff --git a/firmware/src/ssp/ssp_func.h b/firmware/src/ssp/ssp_func.h index df22cc7b..3acea62d 100644 --- a/firmware/src/ssp/ssp_func.h +++ b/firmware/src/ssp/ssp_func.h @@ -25,6 +25,13 @@ #define DF_SSP_BUSY_TICK (25000u) // use to check the busy tick +// Device Select Enumeration Type +typedef enum +{ + SWSEL_SPI = 0, + SWSEL_SW = 1 +} SSP_SWSEL_TYPE; + // Interrupt Enable Enumeration Type typedef enum { @@ -38,7 +45,7 @@ typedef enum volatile uint16_t ssp_data[10]; volatile uint16_t data_num; -extern void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t cph, uint8_t cpol); +extern void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t swsel, uint8_t cph, uint8_t cpol); extern void ssp_deinit(SSP_TYPE ssp); extern uint32_t ssp_write_one(PAC55XX_SSP_TYPEDEF *ssp_ptr, uint16_t data); extern uint32_t ssp_write_multi(PAC55XX_SSP_TYPEDEF *ssp_ptr, uint16_t *data, uint32_t byte_num); diff --git a/studio/Python/tests/test_amt22.py b/studio/Python/tests/test_amt22.py index 5f2d672b..2d282cb8 100644 --- a/studio/Python/tests/test_amt22.py +++ b/studio/Python/tests/test_amt22.py @@ -57,8 +57,6 @@ def test_a_position_control_before_after_save_and_load_config(self): self.configure_sensors(self.tm.sensors.setup.external_spi.type.AMT22) 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 = 1.0 - self.try_calibrate() self.tm.controller.position.p_gain = 9 @@ -118,7 +116,6 @@ def test_b_position_control_following_sensor_change(self): # Initially configure with external SPI sensor self.configure_sensors(self.tm.sensors.setup.external_spi.type.AMT22) 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 = 1.0 self.try_calibrate() # Set initial controller gains diff --git a/studio/Python/tests/test_as5047.py b/studio/Python/tests/test_as5047.py index f3971d53..0be284b3 100644 --- a/studio/Python/tests/test_as5047.py +++ b/studio/Python/tests/test_as5047.py @@ -57,8 +57,6 @@ def test_a_position_control_before_after_save_and_load_config(self): 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 @@ -117,7 +115,6 @@ def test_b_position_control_following_sensor_change(self): # Initially configure with external SPI sensor 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() # Set initial controller gains @@ -179,8 +176,6 @@ def test_c_position_control_change_baud_rates(self): 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 diff --git a/studio/Python/tests/test_base_function.py b/studio/Python/tests/test_base_function.py index 7912225d..1a0206b5 100644 --- a/studio/Python/tests/test_base_function.py +++ b/studio/Python/tests/test_base_function.py @@ -23,10 +23,6 @@ def test_position_control(self): """ hw_rev = self.tm.hw_revision self.check_state(0) - if hw_rev > 20: - self.tm.motor.I_cal = 0.8 - else: - self.tm.motor.I_cal = 5 self.tm.controller.current.Iq_limit = 5 self.try_calibrate() self.tm.controller.position_mode() diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py index f460eabd..7c922675 100644 --- a/studio/Python/tests/test_board.py +++ b/studio/Python/tests/test_board.py @@ -235,7 +235,6 @@ def test_j_gimbal_mode(self): # Ensure we're idle self.check_state(0) self.tm.motor.type = 1 # gimbal - self.tm.motor.I_cal = 5.0 self.tm.motor.R = 0.2 self.tm.motor.L = 20 * 1e-5 self.try_calibrate() diff --git a/studio/Python/tests/tm_test_case.py b/studio/Python/tests/tm_test_case.py index dbcb2be0..00a61cae 100644 --- a/studio/Python/tests/tm_test_case.py +++ b/studio/Python/tests/tm_test_case.py @@ -61,8 +61,20 @@ def reset_and_wait(cls, timeout=0.5): cls.tm.reset() time.sleep(timeout) - def try_calibrate(self, force=False, precheck_callback=None, *args, **kwargs): + def try_calibrate(self, I_cal=None, force=False, precheck_callback=None,*args, **kwargs): if True == force or not self.tm.calibrated: + hw_rev = self.tm.hw_revision + self.check_state(0) + if I_cal and I_cal > 0: + self.tm.motor.I_cal = I_cal + elif hw_rev > 20: + self.tm.motor.I_cal = 0.8 + else: + self.tm.motor.I_cal = 5 + + if hw_rev > 20: + self.tm.controller.velocity.P_gain = 2e-05 + self.tm.controller.calibrate() self.wait_for_calibration(*args, **kwargs) if precheck_callback: