diff --git a/firmware/src/sensor/sensors.c b/firmware/src/sensor/sensors.c index 072d475a..65e22dfd 100644 --- a/firmware/src/sensor/sensors.c +++ b/firmware/src/sensor/sensors.c @@ -206,6 +206,8 @@ void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options && (controller_get_state() == CONTROLLER_STATE_IDLE) && (internal_type != sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type)) { + frames_reset_calibrated(); + Sensor *s = &(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor); if (s->initialized) { @@ -222,6 +224,8 @@ void sensor_set_connection(Sensor** target_sensor_p, Sensor** other_sensor_p, se && new_connection != sensor_get_connection(*target_sensor_p) && new_connection >= 0 && new_connection < SENSOR_CONNECTION_MAX) { + frames_reset_calibrated(); + if (sensor_get_connection(*target_sensor_p) != sensor_get_connection(*other_sensor_p)) { observer_reset_state(observer_get_for_sensor(*target_sensor_p)); @@ -295,7 +299,7 @@ bool sensors_calibrate_pole_pair_count_and_transforms(void) if (commutation_sensor_p == position_sensor_p) { - frame_set_position_sensor_to_motor(frames.commutation_sensor_to_motor); + frame_set_position_sensor_to_motor(*(frame_commutation_sensor_to_motor_p())); } else if (sensor_get_type(position_sensor_p) != SENSOR_TYPE_HALL) { diff --git a/firmware/src/system/system.c b/firmware/src/system/system.c index 94da8a23..4d28c22b 100644 --- a/firmware/src/system/system.c +++ b/firmware/src/system/system.c @@ -141,7 +141,8 @@ TM_RAMFUNC float system_get_Vbus(void) TM_RAMFUNC bool system_get_calibrated(void) { - return (motor_get_calibrated() && + return (frames_get_calibrated() && + motor_get_calibrated() && commutation_sensor_p->is_calibrated_func(commutation_sensor_p) && position_sensor_p->is_calibrated_func(position_sensor_p)); } diff --git a/firmware/src/xfs.c b/firmware/src/xfs.c index ba1f4939..286acfa6 100644 --- a/firmware/src/xfs.c +++ b/firmware/src/xfs.c @@ -10,5 +10,6 @@ FramesConfig frames = { .commutation_sensor_to_motor = DEFAULT_TRANSFORM, .motor_to_commutation_sensor = DEFAULT_TRANSFORM, .motor_to_user = DEFAULT_TRANSFORM, - .user_to_motor = DEFAULT_TRANSFORM + .user_to_motor = DEFAULT_TRANSFORM, + .calibrated = default_calibrated }; diff --git a/firmware/src/xfs.h b/firmware/src/xfs.h index 0c236443..a38a416b 100644 --- a/firmware/src/xfs.h +++ b/firmware/src/xfs.h @@ -16,8 +16,24 @@ #pragma once +#include #include +typedef enum { + none_calibrated = 0, + position_sensor_to_user_calibrated = 1, + user_to_position_sensor_calibrated = 1 << 1, + position_sensor_to_motor_calibrated = 1 << 2, + motor_to_position_sensor_calibrated = 1 << 3, + commutation_sensor_to_motor_calibrated = 1 << 4, + motor_to_commutation_sensor_calibrated = 1 << 5, + motor_to_user_calibrated = 1 << 6, + user_to_motor_calibrated = 1 << 7, + default_calibrated = (position_sensor_to_user_calibrated | user_to_position_sensor_calibrated), + all_calibrated = 0xFF +} FramesOrder; + + typedef struct { FrameTransform position_sensor_to_user; FrameTransform user_to_position_sensor; @@ -27,10 +43,21 @@ typedef struct { FrameTransform motor_to_commutation_sensor; FrameTransform motor_to_user; FrameTransform user_to_motor; + FramesOrder calibrated; } FramesConfig; extern FramesConfig frames; +static inline bool frames_get_calibrated(void) +{ + return (frames.calibrated == all_calibrated); +} + +static inline void frames_reset_calibrated(void) +{ + frames.calibrated = default_calibrated; +} + static inline void frames_get_config(FramesConfig *_config) { *_config = frames; @@ -97,6 +124,8 @@ static inline void frame_user_to_position_sensor_set_offset(float value) frames.position_sensor_to_user = derive_inverse_transform(frame_user_to_position_sensor_p()); frames.user_to_motor = combine_transforms(frame_user_to_position_sensor_p(), frame_position_sensor_to_motor_p()); frames.motor_to_user = derive_inverse_transform(frame_user_to_motor_p()); + frames.calibrated |= (position_sensor_to_user_calibrated | + user_to_position_sensor_calibrated | motor_to_user_calibrated | user_to_motor_calibrated); } static inline void frame_user_to_position_sensor_set_multiplier(float value) @@ -107,6 +136,8 @@ static inline void frame_user_to_position_sensor_set_multiplier(float value) frames.position_sensor_to_user = derive_inverse_transform(frame_user_to_position_sensor_p()); frames.user_to_motor = combine_transforms(frame_user_to_position_sensor_p(), frame_position_sensor_to_motor_p()); frames.motor_to_user = derive_inverse_transform(frame_user_to_motor_p()); + frames.calibrated |= (position_sensor_to_user_calibrated | + user_to_position_sensor_calibrated | motor_to_user_calibrated | user_to_motor_calibrated); } } @@ -114,6 +145,7 @@ static inline void frame_set_commutation_sensor_to_motor(const FrameTransform va { frames.commutation_sensor_to_motor = value; frames.motor_to_commutation_sensor = derive_inverse_transform(frame_commutation_sensor_to_motor_p()); + frames.calibrated |= (commutation_sensor_to_motor_calibrated | motor_to_commutation_sensor_calibrated); } static inline void frame_set_position_sensor_to_motor(const FrameTransform value) @@ -122,4 +154,6 @@ static inline void frame_set_position_sensor_to_motor(const FrameTransform value frames.motor_to_position_sensor = derive_inverse_transform(&value); frames.user_to_motor = combine_transforms(frame_user_to_position_sensor_p(), frame_position_sensor_to_motor_p()); frames.motor_to_user = derive_inverse_transform(frame_user_to_motor_p()); + frames.calibrated |= (position_sensor_to_motor_calibrated | + motor_to_position_sensor_calibrated | motor_to_user_calibrated | user_to_motor_calibrated); }