Skip to content

Commit

Permalink
refactor sensor initialization
Browse files Browse the repository at this point in the history
  • Loading branch information
@yconst
yconst committed Jan 28, 2024
1 parent 44cdd48 commit 91a7ff3
Show file tree
Hide file tree
Showing 11 changed files with 137 additions and 88 deletions.
4 changes: 2 additions & 2 deletions firmware/src/can/can_endpoints.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -790,15 +790,15 @@ uint8_t avlos_sensors_setup_external_spi_type(uint8_t * buffer, uint8_t * buffer
{
if (AVLOS_CMD_READ == cmd) {
uint8_t v;
v = sensor_external_spi_get_type();
v = sensor_external_spi_get_type_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_type(v);
sensor_external_spi_set_type_avlos(v);
return AVLOS_RET_WRITE;
}
return AVLOS_RET_NOACTION;
Expand Down
24 changes: 14 additions & 10 deletions firmware/src/sensor/as5047.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,20 @@
#include <src/observer/observer.h>
#include <src/sensor/as5047.h>

void as5047p_make_blank_sensor(Sensor *s)
{
s->config.type = SENSOR_TYPE_AS5047;
s->ticks = ENCODER_TICKS;
s->get_raw_angle_func = ma7xx_get_raw_angle;
s->update_func = as5047p_update;
s->prepare_func = as5047p_send_angle_cmd;
s->reset_func = as5047p_reset;
s->deinit_func = as5047p_deinit;
s->get_errors_func = as5047p_get_errors;
s->is_calibrated_func = as5047p_is_calibrated;
s->calibrate_func = as5047p_calibrate;
}

bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct) {
AS5047PSensorConfig c = {0};
c.ssp_port = port;
Expand All @@ -32,16 +46,6 @@ 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;
s->get_raw_angle_func = ma7xx_get_raw_angle;
s->update_func = as5047p_update;
s->prepare_func = as5047p_send_angle_cmd;
s->reset_func = as5047p_reset;
s->deinit_func = as5047p_deinit;
s->get_errors_func = as5047p_get_errors;
s->is_calibrated_func = as5047p_is_calibrated;
s->calibrate_func = as5047p_calibrate;
s->config.type = SENSOR_TYPE_AS5047;
s->ticks = ENCODER_TICKS;
as->config = *c;
ssp_init(as->config.ssp_port, SSP_MS_MASTER, 0, 0);
delay_us(10000); // Example delay, adjust based on AS5047P datasheet
Expand Down
3 changes: 2 additions & 1 deletion firmware/src/sensor/as5047.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,13 +43,14 @@ typedef struct
int32_t angle;
} 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_config(Sensor *s, const AS5047PSensorConfig *c);
void as5047p_deinit(Sensor *s);
void as5047p_reset(Sensor *s);
bool as5047p_calibrate(Sensor *s, Observer *o);

bool as5047p_is_calibrated(const Sensor *s)
static inline bool as5047p_is_calibrated(const Sensor *s)
{
return s->config.rec_calibrated;
}
Expand Down
28 changes: 19 additions & 9 deletions firmware/src/sensor/hall.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,20 @@
#define AIO6789_IO_MODE 0x00
#define AIO_INPUT 0x00

void hall_make_blank_sensor(Sensor *s)
{
s->config.type = SENSOR_TYPE_HALL;
s->ticks = HALL_SECTORS;
s->get_raw_angle_func = hall_get_angle;
s->update_func = hall_update;
s->reset_func = hall_reset;
s->deinit_func = hall_deinit;
s->get_errors_func = hall_get_errors;
s->is_calibrated_func = hall_sector_map_is_calibrated;
s->calibrate_func = hall_calibrate_sequence;
s->get_ss_config_func = hall_get_ss_config;
}

bool hall_init_with_defaults(Sensor *s)
{
HallSensorConfig c = {0};
Expand All @@ -33,15 +47,6 @@ bool hall_init_with_defaults(Sensor *s)
bool hall_init_with_config(Sensor *s, const HallSensorConfig *c)
{
HallSensor *ms = (HallSensor *)s;
s->get_raw_angle_func = hall_get_angle;
s->update_func = hall_update;
s->reset_func = hall_reset;
s->deinit_func = hall_deinit;
s->get_errors_func = hall_get_errors;
s->is_calibrated_func = hall_sector_map_is_calibrated;
s->calibrate_func = hall_calibrate_sequence;
s->config.type = SENSOR_TYPE_HALL;
s->ticks = HALL_SECTORS;
ms->config = *c;
ms->hw_defaults[0] = pac5xxx_tile_register_read(ADDR_CFGAIO7);
ms->hw_defaults[1] = pac5xxx_tile_register_read(ADDR_CFGAIO8);
Expand Down Expand Up @@ -137,4 +142,9 @@ bool hall_calibrate_sequence(Sensor *s, Observer *o)
ms->errors |= SENSORS_SETUP_HALL_ERRORS_CALIBRATION_FAILED;
}
return success;
}

void hall_get_ss_config(Sensor *s, void* buffer)
{

}
2 changes: 2 additions & 0 deletions firmware/src/sensor/hall.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,13 @@ typedef struct
uint8_t hw_defaults[3];
} HallSensor;

void hall_make_blank_sensor(Sensor *s);
bool hall_init_with_defaults(Sensor *s);
bool hall_init_with_config(Sensor *s, const HallSensorConfig *c);
void hall_deinit(Sensor *s);
void hall_reset(Sensor *s);
bool hall_calibrate_sequence(Sensor *s, Observer *o);
void hall_get_ss_config(Sensor *s, void* buffer);

static inline uint8_t hall_get_errors(const Sensor *s)
{
Expand Down
30 changes: 20 additions & 10 deletions firmware/src/sensor/ma7xx.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,21 @@
#include <src/observer/observer.h>
#include <src/sensor/ma7xx.h>

void ma7xx_make_blank_sensor(Sensor *s)
{
s->config.type = SENSOR_TYPE_MA7XX;
s->ticks = ENCODER_TICKS;
s->is_calibrated_func = ma7xx_rec_is_calibrated;
s->calibrate_func = ma7xx_calibrate;
s->get_raw_angle_func = ma7xx_get_raw_angle;
s->deinit_func = ma7xx_deinit;
s->reset_func = ma7xx_reset;
s->update_func = ma7xx_update;
s->prepare_func = ma7xx_send_angle_cmd;
s->get_errors_func = ma7xx_get_errors;
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)
{
MA7xxSensorConfig c = {0};
Expand All @@ -34,16 +49,6 @@ bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *s
bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c)
{
MA7xxSensor *ms = (MA7xxSensor *)s;
s->get_raw_angle_func = ma7xx_get_raw_angle;
s->update_func = ma7xx_update;
s->prepare_func = ma7xx_send_angle_cmd;
s->reset_func = ma7xx_reset;
s->deinit_func = ma7xx_deinit;
s->get_errors_func = ma7xx_get_errors;
s->is_calibrated_func = ma7xx_rec_is_calibrated;
s->calibrate_func = ma7xx_calibrate;
s->config.type = SENSOR_TYPE_MA7XX;
s->ticks = ENCODER_TICKS;
ms->config = *c;
ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 0, 0);
delay_us(16000); // ensure 16ms sensor startup time as per the datasheet
Expand All @@ -68,3 +73,8 @@ bool ma7xx_calibrate(Sensor *s, Observer *o)
{
return sensor_calibrate_direction_and_pole_pair_count(s, o) && sensor_calibrate_offset_and_rectification(s, o);
}

void ma7xx_get_ss_config(Sensor *s, void* buffer)
{

}
2 changes: 2 additions & 0 deletions firmware/src/sensor/ma7xx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,13 @@ typedef struct
int32_t angle;
} 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_config(Sensor *s, const MA7xxSensorConfig *c);
void ma7xx_deinit(Sensor *s);
void ma7xx_reset(Sensor *s);
bool ma7xx_calibrate(Sensor *s, Observer *o);
void ma7xx_get_ss_config(Sensor *s, void* buffer);

static inline bool ma7xx_rec_is_calibrated(const Sensor *s)
{
Expand Down
8 changes: 5 additions & 3 deletions firmware/src/sensor/sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,14 +48,15 @@ typedef void (*sensor_deinit_func_t)(Sensor *);
typedef void (*sensor_reset_func_t)(Sensor *);
typedef void (*sensor_prepare_func_t)(const Sensor *);
typedef void (*sensor_update_func_t)(Sensor *, bool);
typedef void (*sensor_get_ss_config_func_t)(Sensor* sensor, void* buffer);
typedef uint8_t (*sensor_get_errors_func_t)(const Sensor *);

typedef enum {
SENSOR_TYPE_INVALID = 0,
SENSOR_TYPE_MA7XX = 1,
SENSOR_TYPE_HALL = 2,
SENSOR_TYPE_AS5047 = 3,
SENSOR_TYPE_AMT22 = 4,
SENSOR_TYPE_AS5047 = 2,
SENSOR_TYPE_AMT22 = 3,
SENSOR_TYPE_HALL = 4,
SENSOR_TYPE_MAX
} sensor_type_t;

Expand All @@ -82,6 +83,7 @@ struct Sensor { // typedefd earlier
sensor_reset_func_t reset_func;
sensor_update_func_t update_func;
sensor_prepare_func_t prepare_func;
sensor_get_ss_config_func_t get_ss_config_func;
sensor_get_errors_func_t get_errors_func;
uint32_t ticks;
bool initialized : 1;
Expand Down
97 changes: 49 additions & 48 deletions firmware/src/sensor/sensors.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,41 +18,7 @@
#include <src/sensor/sensors.h>
#include <src/controller/controller.h>

GenSensor sensors[SENSOR_COUNT] = {
{.sensor = {
.config = { .type = SENSOR_TYPE_MA7XX },
.is_calibrated_func = ma7xx_rec_is_calibrated,
.calibrate_func = ma7xx_calibrate,
.get_raw_angle_func = ma7xx_get_raw_angle,
.deinit_func = ma7xx_deinit,
.reset_func = ma7xx_reset,
.update_func = ma7xx_update,
.prepare_func = ma7xx_send_angle_cmd,
.get_errors_func = ma7xx_get_errors
}},
{.sensor = {
.config = { .type = SENSOR_TYPE_MA7XX },
.is_calibrated_func = ma7xx_rec_is_calibrated,
.calibrate_func = ma7xx_calibrate,
.get_raw_angle_func = ma7xx_get_raw_angle,
.deinit_func = ma7xx_deinit,
.reset_func = ma7xx_reset,
.update_func = ma7xx_update,
.prepare_func = ma7xx_send_angle_cmd,
.get_errors_func = ma7xx_get_errors
}},
{.sensor = {
.config = { .type = SENSOR_TYPE_HALL },
.is_calibrated_func = hall_sector_map_is_calibrated,
.calibrate_func = hall_calibrate_sequence,
.get_raw_angle_func = hall_get_angle,
.deinit_func = hall_deinit,
.reset_func = hall_reset,
.update_func = hall_update,
.prepare_func = 0x0,
.get_errors_func = hall_get_errors
}}
};
GenSensor sensors[SENSOR_COUNT] = {0};

bool sensor_init_with_defaults(Sensor *s)
{
Expand All @@ -64,12 +30,15 @@ bool sensor_init_with_defaults(Sensor *s)
switch (connection)
{
case SENSOR_CONNECTION_ONBOARD_SPI:
ma7xx_make_blank_sensor(s);
return ma7xx_init_with_port(s, ONBOARD_SENSOR_SSP_PORT, ONBOARD_SENSOR_SSP_STRUCT);
break;
case SENSOR_CONNECTION_EXTERNAL_SPI:
ma7xx_make_blank_sensor(s);
return ma7xx_init_with_port(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT);
break;
case SENSOR_CONNECTION_HALL:
hall_make_blank_sensor(s);
return hall_init_with_defaults(s);
break;
default:
Expand All @@ -83,33 +52,44 @@ bool sensor_init_with_configs(Sensor *s, SensorConfig *sc, GenSensorConfig *gsc)
switch (sc->type)
{
case SENSOR_TYPE_MA7XX:
ma7xx_init_with_config(s, &(gsc->ma7xx_sensor_config));
ma7xx_make_blank_sensor(s);
return ma7xx_init_with_config(s, &(gsc->ma7xx_sensor_config));
break;
case SENSOR_TYPE_HALL:
hall_init_with_config(s, &(gsc->hall_sensor_config));
hall_make_blank_sensor(s);
return hall_init_with_config(s, &(gsc->hall_sensor_config));
break;
case SENSOR_TYPE_AS5047:
as5047p_make_blank_sensor(s);
return as5047p_init_with_config(s, &(gsc->as5047p_sensor_config));
break;
default:
break;
}
return false;
}

void sensors_init_with_defaults(void)
{
sensor_init_with_defaults(&(sensors[0].sensor));
ma7xx_make_blank_sensor(&(sensors[1].sensor));
hall_make_blank_sensor(&(sensors[2].sensor));
commutation_sensor_p = &(sensors[0].sensor);
position_sensor_p = &(sensors[0].sensor);
}

void sensors_get_config(SensorsConfig *config_)
{
// Serialize SensorConfig array
for (int i = 0; i < SENSOR_COUNT; ++i) {
for (int i = 0; i < SENSOR_COUNT; ++i)
{
config_->config[i] = sensors[i].sensor.config;
}

// Serialize GenSensorConfig array
for (int i = 0; i < SENSOR_COUNT; ++i) {
sensors[i].sensor.get_ss_config_func(sensors[i].sensor, &(config_->ss_config[i]));
for (int i = 0; i < SENSOR_COUNT; ++i)
{
sensors[i].sensor.get_ss_config_func(&(sensors[i].sensor), &(config_->ss_config[i]));
}
config_->commutation_connection = sensor_get_connection(commutation_sensor_p);
config_->position_connection = sensor_get_connection(position_sensor_p);
Expand All @@ -123,10 +103,12 @@ void sensors_restore_config(SensorsConfig *config_)
sensor_set_pointer_with_connection(&position_sensor_p, config_->position_connection);

// Restore SensorConfig array
for (int i = 0; i < SENSOR_COUNT; ++i) {
for (int i = 0; i < SENSOR_COUNT; ++i)
{
sensors[i].sensor.config = config_->config[i];
if ((sensors[i].sensor.initialized == false) &&
(commutation_sensor_p == &(sensors[i].sensor) || position_sensor_p == &(sensors[i].sensor)))
if ((sensors[i].sensor.initialized == false)
&& (commutation_sensor_p == &(sensors[i].sensor)
|| position_sensor_p == &(sensors[i].sensor)))
{
sensor_init_with_configs(commutation_sensor_p, &(config_->config[i]), &(config_->ss_config[i]));
}
Expand All @@ -143,15 +125,34 @@ void position_sensor_set_connection(sensor_connection_t new_connection)
sensor_set_connection(&(position_sensor_p), &(commutation_sensor_p), new_connection);
}

void sensor_external_spi_set_type(sensor_type_t type)
void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options type)
{
if (controller_get_state() == STATE_IDLE
&& type > SENSOR_TYPE_INVALID && type < SENSOR_TYPE_MAX
&& type != sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.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() == STATE_IDLE)
&& (internal_type != sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type))
{
Sensor *s = &(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor);
s->deinit_func(s);
sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type = type;
sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type = internal_type;
sensors_init_with_defaults();
}
}
Expand Down
Loading

0 comments on commit 91a7ff3

Please sign in to comment.