diff --git a/conf/messages.xml b/conf/messages.xml index 0a8ea1fb64..d98cf7ca32 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -332,18 +332,63 @@ - - - + + + - - - + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -353,7 +398,7 @@ - + @@ -363,14 +408,14 @@ - + - + - + @@ -378,7 +423,7 @@ - + diff --git a/sw/airborne/i2c.h b/sw/airborne/i2c.h index 5813bc44dc..ae2c1f4dca 100644 --- a/sw/airborne/i2c.h +++ b/sw/airborne/i2c.h @@ -28,7 +28,7 @@ extern void i2c_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finis extern volatile uint8_t i2c_status; #ifndef I2C_BUF_LEN -#define I2C_BUF_LEN 8 +#define I2C_BUF_LEN 16 #endif extern volatile uint8_t i2c_buf[I2C_BUF_LEN]; diff --git a/sw/airborne/imu_v3.c b/sw/airborne/imu_v3.c index cdba9db58d..f15e3ed631 100644 --- a/sw/airborne/imu_v3.c +++ b/sw/airborne/imu_v3.c @@ -12,14 +12,118 @@ int16_t imu_mag[AXIS_NB]; /* battery in volts */ float imu_bat; +uint16_t imu_accel_raw[AXIS_NB]; +uint16_t imu_gyro_raw[AXIS_NB]; +int16_t imu_mag_raw[AXIS_NB]; + struct adc_buf buf_ax; struct adc_buf buf_ay; struct adc_buf buf_az; struct adc_buf buf_bat; + +#define IMU_DETECT_STILL_LEN 256 +bool_t imu_vehicle_still; + +uint16_t imu_vs_accel_raw[AXIS_NB][IMU_DETECT_STILL_LEN]; +uint32_t imu_vs_accel_raw_sum[AXIS_NB]; +uint32_t imu_vs_accel_raw_avg[AXIS_NB]; +float imu_vs_accel_raw_var[AXIS_NB]; + +uint16_t imu_vs_gyro_raw[AXIS_NB][IMU_DETECT_STILL_LEN]; +uint32_t imu_vs_gyro_raw_sum[AXIS_NB]; +uint32_t imu_vs_gyro_raw_avg[AXIS_NB]; +float imu_vs_gyro_raw_var[AXIS_NB]; + +int16_t imu_vs_mag_raw[AXIS_NB][IMU_DETECT_STILL_LEN]; +int32_t imu_vs_mag_raw_sum[AXIS_NB]; +int32_t imu_vs_mag_raw_avg[AXIS_NB]; +float imu_vs_mag_raw_var[AXIS_NB]; + +static uint8_t imu_vs_buf_head; +static bool_t imu_vs_buf_filled; + +#define IMU_VS_ACCEL_VAR_MAX 1100. + + void imu_init(void) { adc_buf_channel(ADC_CHANNEL_AX, &buf_ax, DEFAULT_AV_NB_SAMPLE); adc_buf_channel(ADC_CHANNEL_AY, &buf_ay, DEFAULT_AV_NB_SAMPLE); adc_buf_channel(ADC_CHANNEL_AZ, &buf_az, DEFAULT_AV_NB_SAMPLE); adc_buf_channel(ADC_CHANNEL_BAT, &buf_bat, DEFAULT_AV_NB_SAMPLE); + + imu_vehicle_still = FALSE; + imu_vs_buf_filled = FALSE; + imu_vs_buf_head = 0; + + uint8_t i, j; + for (i=0; i= IMU_DETECT_STILL_LEN) { + imu_vs_buf_filled = TRUE; + imu_vs_buf_head = 0; + } + + uint8_t i, j; + for (i=0; i - +#include "std.h" #include "6dof.h" -/* accelerometers in arbitrary unit */ -extern float imu_accel[AXIS_NB]; -/* gyros in rad/s */ -extern float imu_gyro[AXIS_NB]; -/* magnetometer in arbitrary unit */ -extern int16_t imu_mag[AXIS_NB]; -/* battery in volts */ -extern float imu_bat; +/* calibrated sensor readings */ +extern float imu_accel[AXIS_NB]; /* accelerometers in arbitrary unit */ +extern float imu_gyro[AXIS_NB]; /* gyros in rad/s */ +extern int16_t imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */ +extern float imu_bat; /* battery in volts */ +/* raw sensors readings */ +extern uint16_t imu_accel_raw[AXIS_NB]; +extern uint16_t imu_gyro_raw[AXIS_NB]; +extern int16_t imu_mag_raw[AXIS_NB]; extern void imu_init(void); +extern void imu_detect_vehicle_still(void); +extern bool_t imu_vehicle_still; + +/* sliding average for still vehicle detection */ +extern uint32_t imu_vs_accel_raw_avg[AXIS_NB]; +extern float imu_vs_accel_raw_var[AXIS_NB]; +extern uint32_t imu_vs_gyro_raw_avg[AXIS_NB]; +extern float imu_vs_gyro_raw_var[AXIS_NB]; +extern int32_t imu_vs_mag_raw_avg[AXIS_NB]; +extern float imu_vs_mag_raw_var[AXIS_NB]; extern struct adc_buf buf_ax; extern struct adc_buf buf_ay; @@ -26,21 +36,24 @@ extern struct adc_buf buf_bat; #define IMU_ACCEL_Z_NEUTRAL 506 #if 0 -#define ImuUpdateAccels() { \ - int_disable(); \ - imu_accel[AXIS_X]= buf_ax.sum; \ - imu_accel[AXIS_Y]= buf_ay.sum; \ - imu_accel[AXIS_Z]= buf_az.sum; \ - int_enable(); \ - imu_accel[AXIS_X] = -((imu_accel[AXIS_X] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \ - imu_accel[AXIS_Y] = (imu_accel[AXIS_Y] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ - imu_accel[AXIS_Z] = (imu_accel[AXIS_Z] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ +#define ImuUpdateAccels() { \ + int_disable(); \ + imu_accel_raw[AXIS_X]= buf_ax.sum; \ + imu_accel_raw[AXIS_Y]= buf_ay.sum; \ + imu_accel_raw[AXIS_Z]= buf_az.sum; \ + int_enable(); \ + imu_accel[AXIS_X] = -((imu_accel_raw[AXIS_X] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \ + imu_accel[AXIS_Y] = (imu_accel_raw[AXIS_Y] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ + imu_accel[AXIS_Z] = (imu_accel_raw[AXIS_Z] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ } #else -#define ImuUpdateAccels() { \ +#define ImuUpdateAccels() { \ + imu_accel_raw[AXIS_X]= buf_ax.sum; \ + imu_accel_raw[AXIS_Y]= buf_ay.sum; \ + imu_accel_raw[AXIS_Z]= buf_az.sum; \ imu_accel[AXIS_X]= -((buf_ax.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \ - imu_accel[AXIS_Y]= (buf_ay.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ - imu_accel[AXIS_Z]= (buf_az.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ + imu_accel[AXIS_Y]= (buf_ay.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \ + imu_accel[AXIS_Z]= (buf_az.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \ } #endif @@ -53,8 +66,10 @@ extern struct adc_buf buf_bat; #define IMU_GYRO_Y_GAIN -0.000214915108532129801 #define IMU_GYRO_Z_GAIN 0.0002104 -#define ImuUpdateGyros() { \ - \ +#define ImuUpdateGyros() { \ + imu_gyro_raw[AXIS_X] = max1167_values[0]; \ + imu_gyro_raw[AXIS_Y] = max1167_values[1]; \ + imu_gyro_raw[AXIS_Z] = max1167_values[2]; \ imu_gyro[AXIS_X] = (float)((int32_t)max1167_values[0] - IMU_GYRO_X_NEUTRAL) \ * IMU_GYRO_X_GAIN; \ imu_gyro[AXIS_Y] = (float)((int32_t)max1167_values[1] - IMU_GYRO_Y_NEUTRAL) \ @@ -63,10 +78,13 @@ extern struct adc_buf buf_bat; * IMU_GYRO_Z_GAIN; \ } -#define ImuUpdateMag() { \ - imu_mag[AXIS_X] = -micromag_values[0]; \ - imu_mag[AXIS_Y] = -micromag_values[1]; \ - imu_mag[AXIS_Z] = micromag_values[2]; \ +#define ImuUpdateMag() { \ + imu_mag_raw[AXIS_X] = micromag_values[0]; \ + imu_mag_raw[AXIS_Y] = micromag_values[1]; \ + imu_mag_raw[AXIS_Z] = micromag_values[2]; \ + imu_mag[AXIS_X] = -micromag_values[0]; \ + imu_mag[AXIS_Y] = -micromag_values[1]; \ + imu_mag[AXIS_Z] = micromag_values[2]; \ } diff --git a/sw/airborne/main_imu.c b/sw/airborne/main_imu.c index f654caf6c8..6b95317d03 100644 --- a/sw/airborne/main_imu.c +++ b/sw/airborne/main_imu.c @@ -19,9 +19,15 @@ #include "messages.h" #include "downlink.h" -#define SEND_ACCEL 1 -#define SEND_MAG 1 -#define SEND_GYRO 1 +//#define SEND_ACCEL 1 +//#define SEND_MAG 1 +//#define SEND_GYRO 1 +#define SEND_ACCEL_RAW 1 +#define SEND_MAG_RAW 1 +#define SEND_GYRO_RAW 1 +#define SEND_ACCEL_RAW_AVG 1 +#define SEND_MAG_RAW_AVG 1 +#define SEND_GYRO_RAW_AVG 1 #define SEND_AHRS_STATE 1 #define SEND_AHRS_COV 1 @@ -82,6 +88,9 @@ static inline void main_event_task( void ) { ImuUpdateMag(); #ifdef SEND_MAG DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]); +#endif +#ifdef SEND_MAG_RAW + DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], &imu_mag_raw[AXIS_Y], &imu_mag_raw[AXIS_Z]); #endif spi_cur_slave = SPI_SLAVE_MAX; max1167_read(); @@ -94,11 +103,18 @@ static inline void main_event_task( void ) { ImuUpdateGyros(); #ifdef SEND_GYRO DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]); +#endif +#ifdef SEND_GYRO_RAW + DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_X], &imu_gyro_raw[AXIS_Y], &imu_gyro_raw[AXIS_Z]); #endif ImuUpdateAccels(); #ifdef SEND_ACCEL DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]); #endif +#ifdef SEND_ACCEL_RAW + DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], &imu_accel_raw[AXIS_Y], &imu_accel_raw[AXIS_Z]); +#endif + ahrs_task(); link_imu_send(); @@ -124,11 +140,22 @@ static inline void main_periodic_task( void ) { } static inline void ahrs_task( void ) { - /* discard first 100 measures */ + if (ahrs_step == AHRS_STEP_UNINIT) { - static uint8_t init_count = 0; - init_count++; - if (init_count > 100) { + imu_detect_vehicle_still(); +#ifdef SEND_GYRO_RAW_AVG + DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], &imu_vs_gyro_raw_avg[AXIS_Y], &imu_vs_gyro_raw_avg[AXIS_Z], \ + &imu_vs_gyro_raw_var[AXIS_X], &imu_vs_gyro_raw_var[AXIS_Y], &imu_vs_gyro_raw_var[AXIS_Z]); +#endif +#ifdef SEND_ACCEL_RAW_AVG + DOWNLINK_SEND_IMU_ACCEL_RAW_AVG(&imu_vs_accel_raw_avg[AXIS_X], &imu_vs_accel_raw_avg[AXIS_Y], &imu_vs_accel_raw_avg[AXIS_Z], \ + &imu_vs_accel_raw_var[AXIS_X], &imu_vs_accel_raw_var[AXIS_Y], &imu_vs_accel_raw_var[AXIS_Z]); +#endif +#ifdef SEND_MAG_RAW_AVG + DOWNLINK_SEND_IMU_MAG_RAW_AVG(&imu_vs_mag_raw_avg[AXIS_X], &imu_vs_mag_raw_avg[AXIS_Y], &imu_vs_mag_raw_avg[AXIS_Z], \ + &imu_vs_mag_raw_var[AXIS_X], &imu_vs_mag_raw_var[AXIS_Y], &imu_vs_mag_raw_var[AXIS_Z]); +#endif + if (imu_vehicle_still) { ahrs_step = AHRS_STEP_ROLL; afe_init(imu_mag, imu_accel, imu_gyro); }