From 03c2b44a02ec16ee0ccc77e618d904563fc2db22 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Thu, 11 Jan 2024 01:13:31 +0200 Subject: [PATCH] consolidate sensor return types --- firmware/src/observer/observer.c | 10 ++-------- firmware/src/observer/observer.h | 2 +- firmware/src/sensor/as5047.h | 8 ++++---- firmware/src/sensor/hall.h | 4 ++-- firmware/src/sensor/ma7xx.h | 8 ++++---- firmware/src/sensor/sensor.h | 14 +++++++------- 6 files changed, 20 insertions(+), 26 deletions(-) diff --git a/firmware/src/observer/observer.c b/firmware/src/observer/observer.c index c79c46a0..d51af118 100644 --- a/firmware/src/observer/observer.c +++ b/firmware/src/observer/observer.c @@ -21,12 +21,6 @@ #include #include -static ObserverConfig config = { - .track_bw = 350.0f, - .kp = 0.0f, - .ki = 0.0f, -}; - bool observer_init_with_defaults(Observer *o, Sensor *s) { ObserverConfig c = {.track_bw=350}; @@ -51,8 +45,8 @@ void observer_set_bandwidth(Observer *o, float bw) if (bw > 0.0f) { o->config.track_bw = bw; - o->config.kp = 2.0f * config.track_bw; - o->config.ki = 0.25f * (config.kp * config.kp); + o->config.kp = 2.0f * o->config.track_bw; + o->config.ki = 0.25f * (o->config.kp * o->config.kp); } } diff --git a/firmware/src/observer/observer.h b/firmware/src/observer/observer.h index 50440ff9..400e4405 100644 --- a/firmware/src/observer/observer.h +++ b/firmware/src/observer/observer.h @@ -66,7 +66,7 @@ static inline void observer_update(Observer *o) { const float sensor_ticks = sensor_get_ticks(o->sensor_ptr); const float sensor_half_ticks = sensor_ticks * 0.5f; - const int16_t angle_meas = sensor_get_angle_rectified(o->sensor_ptr); + const int32_t angle_meas = sensor_get_angle_rectified(o->sensor_ptr); const float delta_pos_est = PWM_PERIOD_S * o->vel_estimate; float delta_pos_meas = angle_meas - o->pos_estimate_wrapped; if (delta_pos_meas < -sensor_half_ticks) diff --git a/firmware/src/sensor/as5047.h b/firmware/src/sensor/as5047.h index c18910cb..0b94686f 100644 --- a/firmware/src/sensor/as5047.h +++ b/firmware/src/sensor/as5047.h @@ -40,7 +40,7 @@ typedef struct Sensor base; AS5047PSensorConfig config; uint8_t errors; - uint16_t angle; + int32_t angle; } AS5047PSensor; bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); @@ -64,7 +64,7 @@ static inline void as5047p_send_angle_cmd(const Sensor *s) ssp_write_one(((const AS5047PSensor *)s)->config.ssp_struct, AS5047P_CMD_READ_ANGLE); } -static inline uint16_t as5047p_get_raw_angle(const Sensor *s) +static inline int32_t as5047p_get_raw_angle(const Sensor *s) { return ((const AS5047PSensor *)s)->angle; } @@ -72,10 +72,10 @@ static inline uint16_t as5047p_get_raw_angle(const Sensor *s) static inline void as5047p_update(Sensor *s, bool check_error) { AS5047PSensor *as = (AS5047PSensor *)s; - const int16_t angle = ssp_read_one(as->config.ssp_struct) & 0x3FFF; // Mask to get the angle value + const int32_t angle = ssp_read_one(as->config.ssp_struct) & 0x3FFF; // Mask to get the angle value if (check_error) { - const int16_t delta = as->angle - angle; + const int32_t delta = as->angle - angle; if ( ((delta > MAX_ALLOWED_DELTA) || (delta < -MAX_ALLOWED_DELTA)) && ((delta > MAX_ALLOWED_DELTA_ADD) || (delta < MIN_ALLOWED_DELTA_ADD)) && ((delta > MAX_ALLOWED_DELTA_SUB) || (delta < MIN_ALLOWED_DELTA_SUB)) ) diff --git a/firmware/src/sensor/hall.h b/firmware/src/sensor/hall.h index 10aaa552..cccfdd9c 100644 --- a/firmware/src/sensor/hall.h +++ b/firmware/src/sensor/hall.h @@ -32,7 +32,7 @@ typedef struct Sensor base; HallSensorConfig config; uint8_t errors; - uint16_t angle; + int32_t angle; uint8_t sector; uint8_t hw_defaults[3]; } HallSensor; @@ -48,7 +48,7 @@ static inline uint8_t hall_get_errors(const Sensor *s) return ((HallSensor *)s)->errors; } -static inline int16_t hall_get_angle(const Sensor *s) +static inline int32_t hall_get_angle(const Sensor *s) { return ((HallSensor *)s)->angle; } diff --git a/firmware/src/sensor/ma7xx.h b/firmware/src/sensor/ma7xx.h index 8ffbe72a..72d213ac 100644 --- a/firmware/src/sensor/ma7xx.h +++ b/firmware/src/sensor/ma7xx.h @@ -41,7 +41,7 @@ typedef struct Sensor base; MA7xxSensorConfig config; uint8_t errors; - int16_t angle; + int32_t angle; } MA7xxSensor; bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct); @@ -65,7 +65,7 @@ static inline void ma7xx_send_angle_cmd(const Sensor *s) ssp_write_one(((const MA7xxSensor *)s)->config.ssp_struct, MA_CMD_ANGLE); } -static inline int16_t ma7xx_get_raw_angle(const Sensor *s) +static inline int32_t ma7xx_get_raw_angle(const Sensor *s) { return ((const MA7xxSensor *)s)->angle; } @@ -73,11 +73,11 @@ static inline int16_t ma7xx_get_raw_angle(const Sensor *s) static inline void ma7xx_update(Sensor *s, bool check_error) { MA7xxSensor *ms = (MA7xxSensor *)s; - const int16_t angle = ssp_read_one(ms->config.ssp_struct); + const int32_t angle = ssp_read_one(ms->config.ssp_struct); if (check_error) { - const int16_t delta = ms->angle - angle; + const int32_t delta = ms->angle - angle; if ( ((delta > MAX_ALLOWED_DELTA) || (delta < -MAX_ALLOWED_DELTA)) && ((delta > MAX_ALLOWED_DELTA_ADD) || (delta < MIN_ALLOWED_DELTA_ADD)) && ((delta > MAX_ALLOWED_DELTA_SUB) || (delta < MIN_ALLOWED_DELTA_SUB)) ) diff --git a/firmware/src/sensor/sensor.h b/firmware/src/sensor/sensor.h index 7c88dd8c..181364a4 100644 --- a/firmware/src/sensor/sensor.h +++ b/firmware/src/sensor/sensor.h @@ -43,7 +43,7 @@ typedef struct Observer Observer; typedef bool (*sensor_is_calibrated_func_t)(const Sensor *); typedef bool (*sensor_calibrate_func_t)(Sensor *, Observer *); -typedef int16_t (*sensor_get_raw_angle_func_t)(const Sensor *); +typedef int32_t (*sensor_get_raw_angle_func_t)(const Sensor *); typedef void (*sensor_deinit_func_t)(Sensor *); typedef void (*sensor_reset_func_t)(Sensor *); typedef void (*sensor_prepare_func_t)(const Sensor *); @@ -93,13 +93,13 @@ void sensor_reset(Sensor *s); bool sensor_calibrate_offset_and_rectification(Sensor *s, Observer *o); bool sensor_calibrate_direction_and_pole_pair_count(Sensor *s, Observer *o); -static inline int16_t sensor_get_angle_rectified(const Sensor *s) +static inline int32_t sensor_get_angle_rectified(const Sensor *s) { const uint8_t offset_bits = (ENCODER_BITS - ECN_BITS); - const int16_t angle = s->get_raw_angle_func(s); - const int16_t off_1 = s->config.rec_table[angle>>offset_bits]; - const int16_t off_2 = s->config.rec_table[((angle>>offset_bits) + 1) % ECN_SIZE]; - const int16_t off_interp = off_1 + ((off_2 - off_1)* (angle - ((angle>>offset_bits)<>offset_bits); + const int32_t angle = s->get_raw_angle_func(s); + const int32_t off_1 = s->config.rec_table[angle>>offset_bits]; + const int32_t off_2 = s->config.rec_table[((angle>>offset_bits) + 1) % ECN_SIZE]; + const int32_t off_interp = off_1 + ((off_2 - off_1)* (angle - ((angle>>offset_bits)<>offset_bits); return angle + off_interp; } @@ -117,7 +117,7 @@ static inline void sensor_invalidate(Sensor *s) s->current = false; } -static inline uint16_t sensor_get_ticks(Sensor *s) +static inline uint32_t sensor_get_ticks(Sensor *s) { return s->ticks; }