diff --git a/conf/autopilot/conf_booz.makefile b/conf/autopilot/conf_booz.makefile index 4f072c059f..c37404c2e7 100644 --- a/conf/autopilot/conf_booz.makefile +++ b/conf/autopilot/conf_booz.makefile @@ -80,6 +80,8 @@ flt.srcs += $(SRC_ARCH)/adc_hw.c flt.srcs += max1167.c $(SRC_ARCH)/max1167_hw.c +flt.CFLAGS += -DDISABLE_MAGNETOMETER +flt.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c flt.srcs += multitilt.c diff --git a/conf/messages.xml b/conf/messages.xml index 1cacfbf938..a6564653e8 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -500,6 +500,13 @@ + + + + + + + diff --git a/conf/settings/booz.xml b/conf/settings/booz.xml index 5ecba3a714..e19c4ead88 100644 --- a/conf/settings/booz.xml +++ b/conf/settings/booz.xml @@ -8,10 +8,12 @@ + diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml index b649a3bda7..b4916065cb 100644 --- a/conf/telemetry/booz.xml +++ b/conf/telemetry/booz.xml @@ -34,6 +34,8 @@ + + diff --git a/sw/airborne/arm7/micromag.c b/sw/airborne/arm7/micromag_hw.c similarity index 84% rename from sw/airborne/arm7/micromag.c rename to sw/airborne/arm7/micromag_hw.c index d3bc42bb7c..e8649ee90e 100644 --- a/sw/airborne/arm7/micromag.c +++ b/sw/airborne/arm7/micromag_hw.c @@ -5,13 +5,15 @@ */ #include "micromag.h" -volatile uint8_t micromag_data_available; -volatile int16_t micromag_values[MM_NB_AXIS]; + volatile uint8_t micromag_cur_axe; +#ifndef DISABLE_MAGNETOMETER static void EXTINT2_ISR(void) __attribute__((naked)); +#endif /* DISABLE_MAGNETOMETER */ -void micromag_init( void ) { +void micromag_hw_init( void ) { +#ifndef DISABLE_MAGNETOMETER /* configure SS pin */ SetBit(MM_SS_IODIR, MM_SS_PIN); /* pin is output */ MmUnselect(); /* pin idles high */ @@ -32,16 +34,19 @@ void micromag_init( void ) { VICIntEnable = VIC_BIT( VIC_EINT2 ); /* enable it */ VICVectCntl9 = VIC_ENABLE | VIC_EINT2; VICVectAddr9 = (uint32_t)EXTINT2_ISR; // address of the ISR - +#endif /* DISABLE_MAGNETOMETER */ } void micromag_read( void ) { +#ifndef DISABLE_MAGNETOMETER MmSelect(); SpiEnable(); // micromag_cur_axe = 0; MmTriggerRead(); +#endif /* DISABLE_MAGNETOMETER */ } +#ifndef DISABLE_MAGNETOMETER void EXTINT2_ISR(void) { ISR_ENTRY(); /* read dummy control byte reply */ @@ -57,3 +62,4 @@ void EXTINT2_ISR(void) { VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ ISR_EXIT(); } +#endif /* DISABLE_MAGNETOMETER */ diff --git a/sw/airborne/arm7/micromag.h b/sw/airborne/arm7/micromag_hw.h similarity index 84% rename from sw/airborne/arm7/micromag.h rename to sw/airborne/arm7/micromag_hw.h index 856f5be0a0..f4b5f8273f 100644 --- a/sw/airborne/arm7/micromag.h +++ b/sw/airborne/arm7/micromag_hw.h @@ -1,18 +1,12 @@ -#ifndef MICROMAG_H -#define MICROMAG_H +#ifndef MICROMAG_HW_H +#define MICROMAG_HW_H #include "std.h" #include "LPC21xx.h" #include "interrupt_hw.h" #include "spi_hw.h" -#define MM_NB_AXIS 3 -extern void micromag_init( void ); -extern void micromag_read( void ); - -extern volatile uint8_t micromag_data_available; -extern volatile int16_t micromag_values[MM_NB_AXIS]; extern volatile uint8_t micromag_cur_axe; #define MM_SS_PIN 20 @@ -64,4 +58,4 @@ extern volatile uint8_t micromag_cur_axe; } -#endif /* MICROMAG_H */ +#endif /* MICROMAG_HW_H */ diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c index e3f795e561..57d69a93be 100644 --- a/sw/airborne/booz_filter_main.c +++ b/sw/airborne/booz_filter_main.c @@ -78,14 +78,14 @@ static inline void on_imu_event( void ) { imu_v3_detect_vehicle_still(); if (imu_vehicle_still) { ImuUpdateFromAvg(); - multitilt_start(imu_accel, imu_gyro); + multitilt_start(imu_accel, imu_gyro, imu_mag); } break; case MT_STATUS_RUNNING : // t0 = T0TC; multitilt_predict(imu_gyro_prev); - multitilt_update(imu_accel); + multitilt_update(imu_accel, imu_mag); // t1 = T0TC; // diff = t1 - t0; // DOWNLINK_SEND_TIME(&diff); diff --git a/sw/airborne/booz_filter_telemetry.h b/sw/airborne/booz_filter_telemetry.h index e7b07ea2b4..1ecf13e81f 100644 --- a/sw/airborne/booz_filter_telemetry.h +++ b/sw/airborne/booz_filter_telemetry.h @@ -37,6 +37,7 @@ &imu_accel_raw[AXIS_Y], \ &imu_accel_raw[AXIS_Z]); + #define PERIODIC_SEND_IMU_GYRO_RAW_AVG() \ DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], \ &imu_vs_gyro_raw_avg[AXIS_Y], \ @@ -53,8 +54,19 @@ &imu_vs_accel_raw_var[AXIS_Y], \ &imu_vs_accel_raw_var[AXIS_Z]); -#define PERIODIC_SEND_AHRS_STATE() \ - DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &mtt_theta, &mtt_theta, \ +#define PERIODIC_SEND_IMU_MAG() \ + DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], \ + &imu_mag[AXIS_Y], \ + &imu_mag[AXIS_Z]); + +#define PERIODIC_SEND_IMU_MAG_RAW() \ + DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], \ + &imu_mag_raw[AXIS_Y], \ + &imu_mag_raw[AXIS_Z]); + + +#define PERIODIC_SEND_AHRS_STATE() \ + DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &mtt_psi, &mtt_psi, \ &mtt_bp, &mtt_bq, &mtt_br); #define PERIODIC_SEND_AHRS_COV() \ diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h index 0f9d619d94..a673bc535d 100644 --- a/sw/airborne/imu_v3.h +++ b/sw/airborne/imu_v3.h @@ -5,6 +5,7 @@ #include "6dof.h" #include "max1167.h" +#include "micromag.h" #include "imu_v3_hw.h" /* calibrated sensor readings */ @@ -138,6 +139,7 @@ extern struct adc_buf buf_bat; max1167_status = STA_MAX1167_IDLE; \ ImuUpdateGyros(); \ ImuUpdateAccels(); \ + ImuUpdateMag(); \ user_handler(); \ } \ } @@ -148,6 +150,7 @@ extern struct adc_buf buf_bat; } \ else { \ max1167_read(); \ + micromag_read(); \ } \ } diff --git a/sw/airborne/micromag.c b/sw/airborne/micromag.c new file mode 100644 index 0000000000..4d10d6a437 --- /dev/null +++ b/sw/airborne/micromag.c @@ -0,0 +1,14 @@ +#include "micromag.h" + +volatile uint8_t micromag_data_available; +volatile int16_t micromag_values[MM_NB_AXIS]; + +void micromag_init( void ) { + + micromag_hw_init(); + + uint8_t i; + for (i=0; i a) x -= 2 * a; while (x <= -a) x += 2 * a;} + +static inline float phi_of_accel( const float* accel) { + return atan2(accel[AXIS_Y], accel[AXIS_Z]); +} + +static inline float theta_of_accel( const float* accel) { + const float g2 = + accel[AXIS_X]*accel[AXIS_X] + + accel[AXIS_Y]*accel[AXIS_Y] + + accel[AXIS_Z]*accel[AXIS_Z]; + return -asin( accel[AXIS_X] / sqrt( g2 ) ); +} + +static inline float psi_of_mag( const int16_t* mag) { + /* untilt magnetometer */ + const float ctheta = cos( mtt_theta ); + const float stheta = sin( mtt_theta ); + const float cphi = cos( mtt_phi ); + const float sphi = sin( mtt_phi ); + + const float mn = + ctheta* mag[0]+ + sphi*stheta* mag[1]+ + cphi*stheta* mag[2]; + const float me = + /* 0* mag[0]+ */ + cphi* mag[1]+ + -sphi* mag[2]; + return -atan2( me, mn ); +} + void multitilt_init(void) { mtt_status = MT_STATUS_UNINIT; @@ -47,13 +82,13 @@ void multitilt_init(void) { mtt_br = 0.; } -void multitilt_start(const float* accel, const float* gyro) { +void multitilt_start(const float* accel, const float* gyro, const int16_t* mag) { /* reset covariance matrices */ const float cov_init[2][2] = {{1., 0.}, {0., 1.}}; memcpy(mtt_P_phi, cov_init, sizeof(cov_init)); memcpy(mtt_P_theta, cov_init, sizeof(cov_init)); - // memcpy(mtt_P_psi, cov_init, sizeof(cov_init)); + memcpy(mtt_P_psi, cov_init, sizeof(cov_init)); /* initialise state */ mtt_p = 0.; @@ -64,20 +99,39 @@ void multitilt_start(const float* accel, const float* gyro) { mtt_bq = gyro[AXIS_Q]; mtt_br = gyro[AXIS_R]; - const float init_phi = atan2(accel[AXIS_Y], accel[AXIS_Z]); - const float g2 = - accel[AXIS_X]*accel[AXIS_X] + - accel[AXIS_Y]*accel[AXIS_Y] + - accel[AXIS_Z]*accel[AXIS_Z]; - const float init_theta = -asin( accel[AXIS_X] / sqrt( g2 ) ); - + const float init_phi = phi_of_accel(accel); + const float init_theta = theta_of_accel(accel); +#ifndef DISABLE_MAGNETOMETER + const float init_psi = psi_of_mag(mag); +#endif mtt_phi = init_phi; mtt_theta = init_theta; +#ifndef DISABLE_MAGNETOMETER + mtt_psi = init_psi; +#endif mtt_status = MT_STATUS_RUNNING; } +static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]) { + + (*angle) += angle_dot * DT; + + const float P_dot00 = Q_angle - P[0][1] - P[1][0]; + const float P_dot01 = - P[1][1]; + const float P_dot10 = - P[1][1]; + const float P_dot11 = Q_bias; + + /* P += Pdot * dt */ + P[0][0] += P_dot00 * DT; + P[0][1] += P_dot01 * DT; + P[1][0] += P_dot10 * DT; + P[1][1] += P_dot11 * DT; + +} + + void multitilt_predict( const float* gyro ) { /* unbias gyro */ mtt_p = gyro[AXIS_P] - mtt_bp; @@ -90,99 +144,67 @@ void multitilt_predict( const float* gyro ) { float t_theta = tan(mtt_theta); float phi_dot = mtt_p + s_phi*t_theta*mtt_q + c_phi*t_theta*mtt_r; - mtt_phi += phi_dot * DT; float theta_dot = c_phi*mtt_q - s_phi*mtt_r; - mtt_theta += theta_dot * DT; + mtt_predict_axis(&mtt_phi, phi_dot, mtt_P_phi); + mtt_predict_axis(&mtt_theta, theta_dot, mtt_P_theta); - /* Pdot = A*P + P*A' + Q */ - const float P_phi_dot00 = Q_angle - mtt_P_phi[0][1] - mtt_P_phi[1][0]; - const float P_phi_dot01 = - mtt_P_phi[1][1]; - const float P_phi_dot10 = - mtt_P_phi[1][1]; - const float P_phi_dot11 = Q_bias; - - /* P += Pdot * dt */ - mtt_P_phi[0][0] += P_phi_dot00 * DT; - mtt_P_phi[0][1] += P_phi_dot01 * DT; - mtt_P_phi[1][0] += P_phi_dot10 * DT; - mtt_P_phi[1][1] += P_phi_dot11 * DT; - - /* Pdot = A*P + P*A' + Q */ - const float P_theta_dot00 = Q_angle - mtt_P_theta[0][1] - mtt_P_theta[1][0]; - const float P_theta_dot01 = - mtt_P_theta[1][1]; - const float P_theta_dot10 = - mtt_P_theta[1][1]; - const float P_theta_dot11 = Q_bias; - - /* P += Pdot * dt */ - mtt_P_theta[0][0] += P_theta_dot00 * DT; - mtt_P_theta[0][1] += P_theta_dot01 * DT; - mtt_P_theta[1][0] += P_theta_dot10 * DT; - mtt_P_theta[1][1] += P_theta_dot11 * DT; -} - -void multitilt_update( const float* accel ) { - - const float measure_phi = atan2(accel[AXIS_Y], accel[AXIS_Z]); - - const float err_phi = measure_phi - mtt_phi; - - const float Pct_0_phi = mtt_P_phi[0][0]; - const float Pct_1_phi = mtt_P_phi[1][0]; - - /* E = C P C' + R */ - const float E_phi = R_accel + Pct_0_phi; - - /* K = P C' inv(E) */ - const float K_0_phi = Pct_0_phi / E_phi; - const float K_1_phi = Pct_1_phi / E_phi; - - /* P = P - K C P */ - const float t_0_phi = Pct_0_phi; - const float t_1_phi = mtt_P_phi[0][1]; - - mtt_P_phi[0][0] -= K_0_phi * t_0_phi; - mtt_P_phi[0][1] -= K_0_phi * t_1_phi; - mtt_P_phi[1][0] -= K_1_phi * t_0_phi; - mtt_P_phi[1][1] -= K_1_phi * t_1_phi; - - /* X += K * err */ - mtt_phi += K_0_phi * err_phi; - mtt_bp += K_1_phi * err_phi; - +#ifndef DISABLE_MAGNETOMETER + float c_theta = cos(mtt_theta); + float psi_dot = s_phi/c_theta*mtt_q + c_phi/c_theta*mtt_r; + mtt_predict_axis(&mtt_psi, psi_dot, mtt_P_psi); +#endif - - const float g2 = - accel[AXIS_X]*accel[AXIS_X] + - accel[AXIS_Y]*accel[AXIS_Y] + - accel[AXIS_Z]*accel[AXIS_Z]; - const float measure_theta = -asin( accel[AXIS_X] / sqrt( g2 ) ); - - const float err_theta = measure_theta - mtt_theta; - - const float Pct_0_theta = mtt_P_theta[0][0]; - const float Pct_1_theta = mtt_P_theta[1][0]; - - /* E = C P C' + R */ - const float E_theta = R_accel + Pct_0_theta; - - /* K = P C' inv(E) */ - const float K_0_theta = Pct_0_theta / E_theta; - const float K_1_theta = Pct_1_theta / E_theta; - - /* P = P - K C P */ - const float t_0_theta = Pct_0_theta; - const float t_1_theta = mtt_P_theta[0][1]; - - mtt_P_theta[0][0] -= K_0_theta * t_0_theta; - mtt_P_theta[0][1] -= K_0_theta * t_1_theta; - mtt_P_theta[1][0] -= K_1_theta * t_0_theta; - mtt_P_theta[1][1] -= K_1_theta * t_1_theta; - - /* X += K * err */ - mtt_theta += K_0_theta * err_theta; - mtt_bq += K_1_theta * err_theta; - - +} + + + +static void inline MttUpdateAxis(float _err, float _P[2][2], float* angle, float* bias) { + const float Pct_0 = _P[0][0]; + const float Pct_1 = _P[1][0]; + /* E = C P C' + R */ + const float E = R_accel + Pct_0; + /* K = P C' inv(E) */ + const float K_0 = Pct_0 / E; + const float K_1 = Pct_1 / E; + /* P = P - K C P */ + const float t_0 = Pct_0; + const float t_1 = _P[0][1]; + + _P[0][0] -= K_0 * t_0; + _P[0][1] -= K_0 * t_1; + _P[1][0] -= K_1 * t_0; + _P[1][1] -= K_1 * t_1; + + /* X += K * err */ + (*angle) += K_0 * _err; + (*bias) += K_1 * _err; + +} + + + +void multitilt_update( const float* accel, const int16_t* mag ) { + + const float measure_phi = phi_of_accel(accel); + float err_phi = measure_phi - mtt_phi; + WRAP(err_phi, M_PI); + MttUpdateAxis(err_phi, mtt_P_phi, &mtt_phi, &mtt_bp); + WRAP(mtt_phi, M_PI); + + const float measure_theta = theta_of_accel(accel); + float err_theta = measure_theta - mtt_theta; + WRAP(err_theta, M_PI_2); + MttUpdateAxis(err_theta, mtt_P_theta, &mtt_theta, &mtt_bq); + WRAP(mtt_theta, M_PI_2); + +#ifndef DISABLE_MAGNETOMETER + float measure_psi = psi_of_mag(mag); + float err_psi = measure_psi - mtt_psi; + WRAP(err_psi, M_PI); + MttUpdateAxis(err_psi, mtt_P_psi, &mtt_psi, &mtt_br); + WRAP(mtt_psi, M_PI); +#endif } diff --git a/sw/airborne/multitilt.h b/sw/airborne/multitilt.h index 908d1b8ed4..c56566cec7 100644 --- a/sw/airborne/multitilt.h +++ b/sw/airborne/multitilt.h @@ -25,9 +25,9 @@ extern float mtt_br; extern void multitilt_init(void); -extern void multitilt_start( const float* accel, const float* gyro); +extern void multitilt_start( const float* accel, const float* gyro, const int16_t* mag); extern void multitilt_predict( const float* gyro ); -extern void multitilt_update( const float* accel ); +extern void multitilt_update( const float* accel, const int16_t* mag ); #endif /* MULTITILT_H */ diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 4ec5d65ae6..891287dddf 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -143,14 +143,14 @@ BOOZ_AB_SRCS += $(AB)/spi.c $(AB_ARCH)/spi_hw.c #CFLAGS += -DDATALINK=PPRZ #BOOZ_AB_SRCS += ../airborne/datalink.c - -CFLAGS += -DBOOZ_FILTER_MCU BOOZ_AB_SRCS += $(AB)/booz_estimator.c BOOZ_AB_SRCS += $(AB)/booz_control.c BOOZ_AB_SRCS += $(AB)/booz_nav.c BOOZ_AB_SRCS += $(AB)/booz_autopilot.c BOOZ_AB_SRCS += $(AB)/commands.c + +CFLAGS += -DBOOZ_FILTER_MCU BOOZ_AB_SRCS += $(AB)/booz_filter_main.c CFLAGS += -DADC_CHANNEL_AX=1 -DADC_CHANNEL_AY=2 -DADC_CHANNEL_AZ=3 -DADC_CHANNEL_BAT=4 @@ -160,9 +160,10 @@ BOOZ_AB_SRCS += $(AB_ARCH)/adc_hw.c BOOZ_AB_SRCS += $(AB)/booz_filter_telemetry.c BOOZ_AB_SRCS += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c - +BOOZ_AB_SRCS += $(AB)/micromag.c $(AB_ARCH)/micromag_hw.c BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c +CFLAGS += -DBOOZ_ATT_FILTER_TYPE=multitilt BOOZ_AB_SRCS += $(AB)/multitilt.c diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c index 3848b74999..2245f83b20 100644 --- a/sw/simulator/booz_flight_model.c +++ b/sw/simulator/booz_flight_model.c @@ -30,6 +30,12 @@ void booz_flight_model_init( void ) { bfm.g_earth->ve[AXIS_Y] = 0.; bfm.g_earth->ve[AXIS_Z] = G; + /* FIXME */ + bfm.h_earth = v_get(AXIS_NB); + bfm.h_earth->ve[AXIS_X] = 1.; + bfm.h_earth->ve[AXIS_Y] = 0.; + bfm.h_earth->ve[AXIS_Z] = 1.; + bfm.thrust_factor = 0.5 * RHO * PROP_AREA * C_t * PROP_RADIUS * PROP_RADIUS; bfm.torque_factor = 0.5 * RHO * PROP_AREA * C_q * PROP_RADIUS * PROP_RADIUS; diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h index b7b8af7cac..1d55242b34 100644 --- a/sw/simulator/booz_flight_model.h +++ b/sw/simulator/booz_flight_model.h @@ -33,6 +33,8 @@ struct BoozFlightModel { VEC* state; /* constants used in derivative computation */ + /* magnetic field in earth frame */ + VEC* h_earth; /* gravitation in earth frame */ VEC* g_earth; /* propeller thrust factor */ diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c index 2bac8bcf49..32e8e135a1 100644 --- a/sw/simulator/booz_sensors_model.c +++ b/sw/simulator/booz_sensors_model.c @@ -14,6 +14,9 @@ static void booz_sensors_model_accel_run(MAT* dcm); static void booz_sensors_model_gyro_init(void); static void booz_sensors_model_gyro_run(double dt); +static void booz_sensors_model_mag_init(void); +static void booz_sensors_model_mag_run(MAT* dcm); + static void booz_sensors_model_gps_init(void); static void booz_sensors_model_gps_run(double dt, MAT* dcm_t); @@ -23,6 +26,7 @@ static VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out); void booz_sensors_model_init(void) { booz_sensors_model_accel_init(); booz_sensors_model_gyro_init(); + booz_sensors_model_mag_init(); booz_sensors_model_gps_init(); } @@ -45,6 +49,7 @@ void booz_sensors_model_run(double dt) { booz_sensors_model_accel_run(dcm); booz_sensors_model_gyro_run(dt); + booz_sensors_model_mag_run(dcm); booz_sensors_model_gps_run(dt, dcm_t); } @@ -54,12 +59,13 @@ static void booz_sensors_model_accel_init(void) { bsm.accel->ve[AXIS_X] = 0.; bsm.accel->ve[AXIS_Y] = 0.; bsm.accel->ve[AXIS_Z] = 0.; + bsm.accel_resolution = 1024 * 32; bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.accel_sensitivity); - bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(1024. * 32) / (6. * 9.81); - bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (1024. * 32) / (6. * 9.81); - bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (1024. * 32) / (6. * 9.81); + bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(double)(bsm.accel_resolution) / (6. * 9.81); + bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (double)(bsm.accel_resolution) / (6. * 9.81); + bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (double)(bsm.accel_resolution) / (6. * 9.81); bsm.accel_neutral = v_get(AXIS_NB); bsm.accel_neutral->ve[AXIS_X] = 538 * 32; @@ -67,14 +73,14 @@ static void booz_sensors_model_accel_init(void) { bsm.accel_neutral->ve[AXIS_Z] = 506 * 32; bsm.accel_noise_std_dev = v_get(AXIS_NB); - bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X]; - bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y]; - bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z]; + bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1; /* m2s-4 */ + bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1; /* m2s-4 */ + bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1; /* m2s-4 */ bsm.accel_bias = v_get(AXIS_NB); - bsm.accel_bias->ve[AXIS_P] = 1e-3 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X]; - bsm.accel_bias->ve[AXIS_Q] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y]; - bsm.accel_bias->ve[AXIS_R] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z]; + bsm.accel_bias->ve[AXIS_P] = 1e-3; /* ms-2 */ + bsm.accel_bias->ve[AXIS_Q] = 1e-3; /* ms-2 */ + bsm.accel_bias->ve[AXIS_R] = 1e-3; /* ms-2 */ } @@ -85,32 +91,33 @@ static void booz_sensors_model_gyro_init(void) { bsm.gyro->ve[AXIS_P] = 0.; bsm.gyro->ve[AXIS_Q] = 0.; bsm.gyro->ve[AXIS_R] = 0.; + bsm.gyro_resolution = 65536; bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.gyro_sensitivity); - bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = 32768. / RadOfDeg(-413.41848); /* degres/s - nominal 300 */ - bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = 32768. / RadOfDeg(-403.65564); /* degres/s - nominal 300 */ - bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = 32768. / RadOfDeg( 395.01929); /* degres/s - nominal 300 */ + bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = (double)bsm.gyro_resolution / (2.*RadOfDeg(-413.41848)); /* degres/s - nominal 300 */ + bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = (double)bsm.gyro_resolution / (2.*RadOfDeg(-403.65564)); /* degres/s - nominal 300 */ + bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = (double)bsm.gyro_resolution / (2.*RadOfDeg( 395.01929)); /* degres/s - nominal 300 */ bsm.gyro_neutral = v_get(AXIS_NB); - bsm.gyro_neutral->ve[AXIS_P] = 65536. * 0.6238556; /* ratio of full scale - nominal 0.5 */ - bsm.gyro_neutral->ve[AXIS_Q] = 65536. * 0.6242371; /* ratio of full scale - nominal 0.5 */ - bsm.gyro_neutral->ve[AXIS_R] = 65536. * 0.6035156; /* ratio of full scale - nominal 0.5 */ + bsm.gyro_neutral->ve[AXIS_P] = (double)bsm.gyro_resolution * 0.6238556; /* ratio of full scale - nominal 0.5 */ + bsm.gyro_neutral->ve[AXIS_Q] = (double)bsm.gyro_resolution * 0.6242371; /* ratio of full scale - nominal 0.5 */ + bsm.gyro_neutral->ve[AXIS_R] = (double)bsm.gyro_resolution * 0.6035156; /* ratio of full scale - nominal 0.5 */ bsm.gyro_noise_std_dev = v_get(AXIS_NB); - bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; - bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; - bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; + bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.); + bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.); + bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.); bsm.gyro_bias_initial = v_get(AXIS_NB); - bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; - bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-1.0) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; - bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.7) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; + bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0); + bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-0.25); + bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5); bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB); - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; + bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1); + bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1); + bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1); bsm.gyro_bias_random_walk_value = v_get(AXIS_NB); bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P]; @@ -119,6 +126,33 @@ static void booz_sensors_model_gyro_init(void) { } + +static void booz_sensors_model_mag_init(void) { + + bsm.mag = v_get(AXIS_NB); + bsm.mag->ve[AXIS_X] = 0.; + bsm.mag->ve[AXIS_Y] = 0.; + bsm.mag->ve[AXIS_Z] = 0.; + bsm.mag_resolution = 4096; + + bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB); + m_zero(bsm.mag_sensitivity); + bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = -(double)bsm.mag_resolution / 6.; + bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = -(double)bsm.mag_resolution / 6.; + bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = (double)bsm.mag_resolution / 6.; + + bsm.mag_neutral = v_get(AXIS_NB); + bsm.mag_neutral->ve[AXIS_X] = 0.; + bsm.mag_neutral->ve[AXIS_Y] = 0.; + bsm.mag_neutral->ve[AXIS_Z] = 0.; + + bsm.mag_noise_std_dev = v_get(AXIS_NB); + bsm.mag_noise_std_dev->ve[AXIS_X] = 2e-1; + bsm.mag_noise_std_dev->ve[AXIS_Y] = 2e-1; + bsm.mag_noise_std_dev->ve[AXIS_Z] = 2e-1; + +} + static void booz_sensors_model_gps_init(void) { bsm.speed_sensor = v_get(AXIS_NB); @@ -153,6 +187,22 @@ static void booz_sensors_model_gps_init(void) { bsm.pos_bias_random_walk_value->ve[AXIS_Z] = bsm.pos_bias_initial->ve[AXIS_Z]; } + +#define RoundSensor(_sensor) { \ + _sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \ + _sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \ + _sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \ + } + +#define BoundSensor(_sensor, _min, _max) { \ + if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \ + if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \ + if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \ + if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \ + if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \ + if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \ + } + static void booz_sensors_model_accel_run( MAT* dcm ) { /* */ @@ -196,6 +246,7 @@ static void booz_sensors_model_accel_run( MAT* dcm ) { /* compute accel reading */ bsm.accel = mv_mlt(bsm.accel_sensitivity, accel_body, bsm.accel); bsm.accel = v_add(bsm.accel, bsm.accel_neutral, bsm.accel); + /* compute accel error readings */ static VEC *accel_error = VNULL; accel_error = v_resize(accel_error, AXIS_NB); @@ -204,18 +255,22 @@ static void booz_sensors_model_accel_run( MAT* dcm ) { accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error); /* constant bias */ accel_error = v_add(accel_error, bsm.accel_bias, accel_error); + /* scale to adc units FIXME : should use full adc gain ? sum ? */ + accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X]; + accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y]; + accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z]; /* add per accel error reading */ bsm.accel = v_add(bsm.accel, accel_error, bsm.accel); /* round signal to account for adc discretisation */ - bsm.accel->ve[AXIS_X] = rint( bsm.accel->ve[AXIS_X]); - bsm.accel->ve[AXIS_Y] = rint( bsm.accel->ve[AXIS_Y]); - bsm.accel->ve[AXIS_Z] = rint( bsm.accel->ve[AXIS_Z]); + RoundSensor(bsm.accel); + /* saturation */ + BoundSensor(bsm.accel, 0, bsm.accel_resolution); // printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]); } -static void booz_sensors_model_gyro_run( double dt) { +static void booz_sensors_model_gyro_run( double dt ) { /* extract rotational speed from flight model state */ static VEC *rate_body = VNULL; rate_body = v_resize(rate_body, AXIS_NB); @@ -227,7 +282,7 @@ static void booz_sensors_model_gyro_run( double dt) { bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro); bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro); - /* compute gyro error reading */ + /* compute gyro error readings */ static VEC *gyro_error = VNULL; gyro_error = v_resize(gyro_error, AXIS_NB); gyro_error = v_zero(gyro_error); @@ -239,16 +294,35 @@ static void booz_sensors_model_gyro_run( double dt) { bsm.gyro_bias_random_walk_std_dev, dt, bsm.gyro_bias_random_walk_value); gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error); - + /* scale to adc units FIXME : should use full adc gain ? sum ? */ + gyro_error->ve[AXIS_P] = gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; + gyro_error->ve[AXIS_Q] = gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; + gyro_error->ve[AXIS_R] = gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; /* add per gyro error reading */ bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro); /* round signal to account for adc discretisation */ - bsm.gyro->ve[AXIS_P] = rint( bsm.gyro->ve[AXIS_P]); - bsm.gyro->ve[AXIS_Q] = rint( bsm.gyro->ve[AXIS_Q]); - bsm.gyro->ve[AXIS_R] = rint( bsm.gyro->ve[AXIS_R]); - + RoundSensor(bsm.gyro); + /* saturation */ + BoundSensor(bsm.gyro, 0, bsm.gyro_resolution); } + +static void booz_sensors_model_mag_run( MAT* dcm ) { + /* rotate h to body frame */ + static VEC *h_body = VNULL; + h_body = v_resize(h_body, AXIS_NB); + h_body = mv_mlt(dcm, bfm.h_earth, h_body); + + bsm.mag = mv_mlt(bsm.mag_sensitivity, h_body, bsm.mag); + bsm.mag = v_add(bsm.mag, bsm.mag_neutral, bsm.mag); + + // printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]); + // printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]); + /* round signal to account for adc discretisation */ + RoundSensor(bsm.mag); +} + + static void booz_sensors_model_gps_run( double dt, MAT* dcm_t ) { /* simulate speed sensor */ diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h index 6cbb885e65..9fed66301f 100644 --- a/sw/simulator/booz_sensors_model.h +++ b/sw/simulator/booz_sensors_model.h @@ -10,12 +10,14 @@ extern void booz_sensors_model_run( double dt); struct BoozSensorsModel { VEC* accel; + unsigned int accel_resolution; MAT* accel_sensitivity; VEC* accel_neutral; VEC* accel_noise_std_dev; VEC* accel_bias; VEC* gyro; + unsigned int gyro_resolution; MAT* gyro_sensitivity; VEC* gyro_neutral; VEC* gyro_noise_std_dev; @@ -23,6 +25,13 @@ struct BoozSensorsModel { VEC* gyro_bias_random_walk_std_dev; VEC* gyro_bias_random_walk_value; + VEC* mag; + unsigned int mag_resolution; + MAT* mag_sensitivity; + VEC* mag_neutral; + VEC* mag_noise_std_dev; + + /* imaginary sensors - gps maybe */ VEC* speed_sensor; VEC* speed_noise_std_dev; diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index bc3d3e3217..49c592d3cb 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -14,8 +14,8 @@ //char* fg_host = "127.0.0.1"; -char* fg_host = "10.31.4.107"; -//char* fg_host = "192.168.1.191"; +//char* fg_host = "10.31.4.107"; +char* fg_host = "192.168.1.191"; unsigned int fg_port = 5501; char* joystick_dev = "/dev/input/js0"; @@ -63,6 +63,7 @@ static gboolean booz_sim_periodic(gpointer data) { booz_flight_model_run(DT, booz_sim_actuators_values); booz_sensors_model_run(DT); + sim_time += DT; /* call the filter periodic task to read sensors */ @@ -170,11 +171,93 @@ static inline void booz_sim_display(void) { (bfm.state->ve[BFMS_X]), (bfm.state->ve[BFMS_Y]), (bfm.state->ve[BFMS_Z])); + /* one good reason to store the bias in real unit !! */ + IvySendMsg("148 BOOZ_SIM_GYRO_BIAS %f %f %f", + DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_P]), + DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_Q]), + DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_R])); } } -//#define FAKE_JOYSTICK +#define BREAK_MTT() { \ + int foo = sim_time/10; \ + switch(foo%4) { \ + case 0: \ + ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + break; \ + case 1: \ + ppm_pulses[RADIO_PITCH] = 1500 + 0.0 * (2050-950); \ + ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \ + break; \ + case 2: \ + ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + break; \ + case 3: \ + ppm_pulses[RADIO_PITCH] = 1500 - 0.0 * (2050-950); \ + ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \ + break; \ + } \ +} + +#define WALK_OVAL() { \ + ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \ + int foo = sim_time/10; \ + switch(foo%4) { \ + case 0: \ + ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + break; \ + case 1: \ + ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \ + break; \ + case 2: \ + ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \ + break; \ + case 3: \ + ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \ + break; \ + } \ +} + +#define STATIONARY() { \ + ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \ + ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + } + + +#define TOUPIE() { \ + ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \ + ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \ + int foo = sim_time/20; \ + switch(foo%4) { \ + case 0: \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + break; \ + case 1: \ + ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \ + break; \ + case 2: \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + break; \ + case 3: \ + ppm_pulses[RADIO_YAW] = 1493 + -0.2 * (2050-950); \ + break; \ + } \ + } + + +#define CIRCLE() { \ + ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \ + double alpha = 0.01 * sim_time; \ + ppm_pulses[RADIO_PITCH] = 1498 + cos(alpha) * 250.; \ + ppm_pulses[RADIO_ROLL] = 1500 + sin(alpha) * 250.; \ + } + +#define FAKE_JOYSTICK static void booz_sim_set_ppm_from_joystick( void ) { #ifndef FAKE_JOYSTICK ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223); @@ -188,31 +271,12 @@ static void booz_sim_set_ppm_from_joystick( void ) { // printf("joystick pitch %f %d\n", booz_joystick_value[JS_PITCH], ppm_pulses[RADIO_PITCH]); // printf("joystick roll %f %d\n", booz_joystick_value[JS_ROLL], ppm_pulses[RADIO_ROLL]); #else - int foo = sim_time/10; - // ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); - // ppm_pulses[RADIO_MODE] = 1500 - 0.5 * (2050-950); - switch(foo%4) { - case 0: - // ppm_pulses[RADIO_THROTTLE] = 1223 + 0.25 * (2050-1223); - // ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950); - ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); - break; - case 1: - // ppm_pulses[RADIO_THROTTLE] = 1223 + 0.25 * (2050-1223); - // ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950); - ppm_pulses[RADIO_PITCH] = 1500 + 0.0 * (2050-950); - break; - case 2: - // ppm_pulses[RADIO_THROTTLE] = 1223 + 0.9 * (2050-1223); - // ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950); - ppm_pulses[RADIO_PITCH] = 1500 - 0.2 * (2050-950); - break; - case 3: - // ppm_pulses[RADIO_THROTTLE] = 1223 + 0.9 * (2050-1223); - // ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950); - ppm_pulses[RADIO_PITCH] = 1500 - 0.0 * (2050-950); - break; - } + ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223); + BREAK_MTT(); + WALK_OVAL(); + // CIRCLE(); + // STATIONARY(); + // TOUPIE(); #endif }