Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve AMT22 reliability #348

Merged
merged 4 commits into from
May 3, 2024
Merged

Improve AMT22 reliability #348

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
80b5d1f
enable manually setting of device select pin
yconst May 3, 2024
fb247a7
implement correct SPI delays according to amt22 datasheet
yconst May 3, 2024
54d0023
compute checksum
yconst May 3, 2024
84d7af9
always set calibration current before calibration
yconst May 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions firmware/src/sensor/amt22.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
57 changes: 40 additions & 17 deletions firmware/src/sensor/amt22.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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)
Expand All @@ -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;

}


2 changes: 1 addition & 1 deletion firmware/src/sensor/as5047.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion firmware/src/sensor/ma7xx.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
3 changes: 2 additions & 1 deletion firmware/src/ssp/ssp_func.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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);
Expand Down
9 changes: 8 additions & 1 deletion firmware/src/ssp/ssp_func.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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);
Expand Down
3 changes: 0 additions & 3 deletions studio/Python/tests/test_amt22.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
5 changes: 0 additions & 5 deletions studio/Python/tests/test_as5047.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
4 changes: 0 additions & 4 deletions studio/Python/tests/test_base_function.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
1 change: 0 additions & 1 deletion studio/Python/tests/test_board.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
14 changes: 13 additions & 1 deletion studio/Python/tests/tm_test_case.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Loading