diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml index 9137ad366a..21d5c20097 100644 --- a/conf/airframes/booz.xml +++ b/conf/airframes/booz.xml @@ -77,6 +77,35 @@ imu.CFLAGS += -DEKF_UPDATE_CONTINUOUS imu.srcs += link_imu.c +# +# multitilt +# + +mtt.ARCHDIR = $(ARCHI) +mtt.ARCH = arm7tdmi +mtt.TARGET = mtt +mtt.TARGETDIR = mtt + +mtt.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)' +mtt.srcs = main_mtt.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c + +mtt.CFLAGS += -DLED + +mtt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 +mtt.srcs += $(SRC_ARCH)/uart_hw.c + +mtt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 +mtt.srcs += downlink.c pprz_transport.c + +mtt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4 +mtt.srcs += $(SRC_ARCH)/adc_hw.c + +mtt.srcs += $(SRC_ARCH)/max1167.c + +mtt.srcs += imu_v3.c + +mtt.srcs += multitilt.c + diff --git a/sw/airborne/imu_v3.c b/sw/airborne/imu_v3.c index c22832f111..1601b454d9 100644 --- a/sw/airborne/imu_v3.c +++ b/sw/airborne/imu_v3.c @@ -12,6 +12,9 @@ int16_t imu_mag[AXIS_NB]; /* battery in volts */ float imu_bat; +float imu_gyro_prev[AXIS_NB]; + + uint16_t imu_accel_raw[AXIS_NB]; uint16_t imu_gyro_raw[AXIS_NB]; int16_t imu_mag_raw[AXIS_NB]; @@ -43,7 +46,7 @@ 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. +#define IMU_VS_ACCEL_RAW_VAR_MAX 7000. void imu_init(void) { @@ -117,9 +120,11 @@ void imu_detect_vehicle_still(void) { imu_vs_accel_raw_var[i] /= (float)IMU_DETECT_STILL_LEN; imu_vs_mag_raw_var[i] /= (float)IMU_DETECT_STILL_LEN; } - if (imu_vs_accel_raw_var[0] < IMU_VS_ACCEL_VAR_MAX && - imu_vs_accel_raw_var[1] < IMU_VS_ACCEL_VAR_MAX && - imu_vs_accel_raw_var[2] < IMU_VS_ACCEL_VAR_MAX ) + // return; + /* check vehicle still */ + if (imu_vs_accel_raw_var[0] < IMU_VS_ACCEL_RAW_VAR_MAX && + imu_vs_accel_raw_var[1] < IMU_VS_ACCEL_RAW_VAR_MAX && + imu_vs_accel_raw_var[2] < IMU_VS_ACCEL_RAW_VAR_MAX ) imu_vehicle_still = TRUE; } diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h index 89e9d42a9d..98506a661f 100644 --- a/sw/airborne/imu_v3.h +++ b/sw/airborne/imu_v3.h @@ -9,6 +9,9 @@ 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 */ + +extern float imu_gyro_prev[AXIS_NB]; /* gyros in rad/s */ + /* raw sensors readings */ extern uint16_t imu_accel_raw[AXIS_NB]; extern uint16_t imu_gyro_raw[AXIS_NB]; @@ -31,42 +34,35 @@ extern struct adc_buf buf_ay; extern struct adc_buf buf_az; extern struct adc_buf buf_bat; -#define IMU_ACCEL_X_NEUTRAL 538 -#define IMU_ACCEL_Y_NEUTRAL 506 -#define IMU_ACCEL_Z_NEUTRAL 506 +#define IMU_ACCEL_X_NEUTRAL (538 * 32) +#define IMU_ACCEL_Y_NEUTRAL (506 * 32) +#define IMU_ACCEL_Z_NEUTRAL (506 * 32) -#if 0 -#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() { \ - 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; \ +#define IMU_ACCEL_X_GAIN -1. +#define IMU_ACCEL_Y_GAIN 1. +#define IMU_ACCEL_Z_GAIN 1. + +#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]= IMU_ACCEL_X_GAIN * (buf_ax.sum - IMU_ACCEL_X_NEUTRAL); \ + imu_accel[AXIS_Y]= IMU_ACCEL_Y_GAIN * (buf_ay.sum - IMU_ACCEL_Y_NEUTRAL); \ + imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (buf_az.sum - IMU_ACCEL_Z_NEUTRAL); \ } -#endif - #define IMU_GYRO_X_NEUTRAL 40885 #define IMU_GYRO_Y_NEUTRAL 40910 #define IMU_GYRO_Z_NEUTRAL 39552 -#define IMU_GYRO_X_GAIN -0.000220177991888642784 -#define IMU_GYRO_Y_GAIN -0.000214915108532129801 +#define IMU_GYRO_X_GAIN -0.0002202 +#define IMU_GYRO_Y_GAIN -0.0002160 #define IMU_GYRO_Z_GAIN 0.0002104 -#define ImuUpdateGyros() { \ +#define ImuUpdateGyros() { \ + imu_gyro_prev[AXIS_X] = imu_gyro[AXIS_X]; \ + imu_gyro_prev[AXIS_Y] = imu_gyro[AXIS_Y]; \ + imu_gyro_prev[AXIS_Z] = imu_gyro[AXIS_Z]; \ imu_gyro_raw[AXIS_X] = max1167_values[0]; \ imu_gyro_raw[AXIS_Y] = max1167_values[1]; \ imu_gyro_raw[AXIS_Z] = max1167_values[2]; \ @@ -96,4 +92,24 @@ extern struct adc_buf buf_bat; * IMU_BAT_GAIN; \ } + + +#define ImuUpdateFromAvg() { \ + imu_accel[AXIS_X] = IMU_ACCEL_X_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_X] - IMU_ACCEL_X_NEUTRAL); \ + imu_accel[AXIS_Y] = IMU_ACCEL_Y_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_Y] - IMU_ACCEL_Y_NEUTRAL); \ + imu_accel[AXIS_Z] = IMU_ACCEL_Z_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_Z] - IMU_ACCEL_Z_NEUTRAL); \ + imu_gyro[AXIS_X] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_X] - IMU_GYRO_X_NEUTRAL) \ + * IMU_GYRO_X_GAIN; \ + imu_gyro[AXIS_Y] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Y] - IMU_GYRO_Y_NEUTRAL) \ + * IMU_GYRO_Y_GAIN; \ + imu_gyro[AXIS_Z] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Z] - IMU_GYRO_Z_NEUTRAL) \ + * IMU_GYRO_Z_GAIN; \ + imu_gyro_prev[AXIS_X] = imu_gyro[AXIS_X]; \ + imu_gyro_prev[AXIS_Y] = imu_gyro[AXIS_Y]; \ + imu_gyro_prev[AXIS_Z] = imu_gyro[AXIS_Z]; \ +} + + + + #endif /* IMU_V3_H */ diff --git a/sw/airborne/multitilt.c b/sw/airborne/multitilt.c index e9a149875a..91ce274e94 100644 --- a/sw/airborne/multitilt.c +++ b/sw/airborne/multitilt.c @@ -2,153 +2,177 @@ #include #include -#include "imu_v3.h" -uint8_t mt_status; +#include "6dof.h" -/* FIXME */ -#define DT 0.01 +uint8_t mtt_status; + +#define DT 4e-3 /* attitude */ -float mt_phi; -float mt_theta; -float mt_psi; +float mtt_phi; +float mtt_theta; +float mtt_psi; /* unbiased rates */ -float mt_p; -float mt_q; -float mt_r; +float mtt_p; +float mtt_q; +float mtt_r; /* gyro biases */ -float mt_bp; -float mt_bq; -float mt_br; +float mtt_bp; +float mtt_bq; +float mtt_br; /* covariance matrix */ -float mt_P_phi[2][2]; -float mt_P_theta[2][2]; -float mt_P_psi[2][2]; +float mtt_P_phi[2][2]; +float mtt_P_theta[2][2]; +//float mtt_P_psi[2][2]; + /* process covariance noise */ -static const float Q_angle = 0.001; -static const float Q_gyro = 0.003; +static const float Q_angle = 0.0005; +static const float Q_bias = 0.0015; /* Measurement covariance */ static const float R_accel = 0.3; - -/* initialisation stuff */ -#define MT_INIT_NB_SAMPLES 32 -#define MT_INIT_MAX_VAR_PHI 0.01 -static float mt_phi_init_samples[MT_INIT_NB_SAMPLES]; -static float mt_phi_init_sum; -static float mt_p_init_samples[MT_INIT_NB_SAMPLES]; -static float mt_p_init_sum; -static uint8_t mt_init_head; -static bool_t mt_init_buf_filled; - -static void multitilt_initialise(void); -static void multitilt_propagate(void); -static void multitilt_update(void); - -void multitilt_reset(void) { - const float cov_init[2][2]; - memcpy(mt_P_phi, cov_init, sizeof(cov_init)); - memcpy(mt_P_theta, cov_init, sizeof(cov_init)); - memcpy(mt_P_psi, cov_init, sizeof(cov_init)); - - mt_status = MT_STATUS_UNINIT; - mt_init_buf_filled = FALSE; - mt_init_head = 0; - mt_phi_init_sum = 0.; - mt_p_init_sum = 0.; +void multitilt_init(void) { + mtt_status = MT_STATUS_UNINIT; } -void multitilt_run(void) { +void multitilt_start(const float* accel, const float* gyro) { + /* 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)); + + /* initialise state */ + mtt_p = 0.; + mtt_q = 0.; + mtt_r = 0.; + mtt_bp = gyro[AXIS_P]; + 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 ) ); + + mtt_phi = init_phi; + mtt_theta = init_theta; + + mtt_status = MT_STATUS_RUNNING; } -static void multitilt_propagate(void) { - /* unbias gyro */ - mt_p = imu_gyro[AXIS_P] - mt_bp; - /* update angle */ - mt_phi += mt_p * DT; +void multitilt_predict( const float* gyro ) { + /* unbias gyro */ + mtt_p = gyro[AXIS_P] - mtt_bp; + mtt_q = gyro[AXIS_Q] - mtt_bq; + mtt_r = gyro[AXIS_R] - mtt_br; + + /* update angles */ + float s_phi = sin(mtt_phi); + float c_phi = cos(mtt_phi); + 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; /* Pdot = A*P + P*A' + Q */ - const float Pdot00 = Q_angle - mt_P_phi[0][1] - mt_P_phi[1][0]; - const float Pdot01 = - mt_P_phi[1][1]; - const float Pdot10 = - mt_P_phi[1][1]; - const float Pdot11 = Q_gyro; + 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 */ - mt_P_phi[0][0] += Pdot00 * DT; - mt_P_phi[0][1] += Pdot01 * DT; - mt_P_phi[1][0] += Pdot10 * DT; - mt_P_phi[1][1] += Pdot11 * 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; } -static void multitilt_update(void) { +void multitilt_update( const float* accel ) { - const float measure_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); + const float measure_phi = atan2(accel[AXIS_Y], accel[AXIS_Z]); - const float err_phi = measure_phi - mt_phi; + const float err_phi = measure_phi - mtt_phi; - const float C_0 = 1.; - - const float PCt_0 = C_0 * mt_P_phi[0][0]; - const float PCt_1 = C_0 * mt_P_phi[1][0]; + 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 = R_accel + C_0 * PCt_0; + const float E_phi = R_accel + Pct_0_phi; /* K = P C' inv(E) */ - const float K_0 = PCt_0 / E; - const float K_1 = PCt_1 / 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 = PCt_0; - const float t_1 = C_0 * mt_P_phi[0][1]; + const float t_0_phi = Pct_0_phi; + const float t_1_phi = mtt_P_phi[0][1]; - mt_P_phi[0][0] -= K_0 * t_0; - mt_P_phi[0][1] -= K_0 * t_1; - mt_P_phi[1][0] -= K_1 * t_0; - mt_P_phi[1][1] -= K_1 * t_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 */ - mt_phi += K_0 * err_phi; - mt_bq += K_1 * err_phi; - -} + mtt_phi += K_0_phi * err_phi; + mtt_bp += K_1_phi * err_phi; + -static void multitilt_initialise(void) { + 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 ) ); - mt_init_head++; - if (mt_init_head >= MT_INIT_NB_SAMPLES) { - mt_init_buf_filled = TRUE; - mt_init_head = 0; - } + const float err_theta = measure_theta - mtt_theta; - const float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); - mt_phi_init_sum -= mt_phi_init_samples[mt_init_head]; - mt_phi_init_samples[mt_init_head] = m_phi; - mt_phi_init_sum += m_phi; - - mt_p_init_sum -= mt_p_init_samples[mt_init_head]; - mt_p_init_samples[mt_init_head] = imu_gyro[AXIS_P]; - mt_p_init_sum += imu_gyro[AXIS_P]; - - if (mt_init_buf_filled) { - float avg_phi = mt_phi_init_sum / MT_INIT_NB_SAMPLES; - float var_phi = 0.; - uint8_t i; - for (i=0; i < MT_INIT_NB_SAMPLES; i++) { - float diff = mt_phi_init_samples[i] - avg_phi; - var_phi += (diff * diff); - } + 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; - if (var_phi < MT_INIT_MAX_VAR_PHI) { - mt_phi = avg_phi; - mt_bp = mt_p_init_sum / MT_INIT_NB_SAMPLES; - mt_status = MT_STATUS_RUNNING; - } - } + /* X += K * err */ + mtt_theta += K_0_theta * err_theta; + mtt_bq += K_1_theta * err_theta; + + } + + + diff --git a/sw/airborne/multitilt.h b/sw/airborne/multitilt.h index 0d1fd55b04..909ea7867c 100644 --- a/sw/airborne/multitilt.h +++ b/sw/airborne/multitilt.h @@ -4,20 +4,28 @@ #include "std.h" #define MT_STATUS_UNINIT 0 -#define MT_STATUS_INITIALISING 1 #define MT_STATUS_RUNNING 2 #define MT_STATUS_CRASHED 3 -extern uint8_t mt_status; +extern uint8_t mtt_status; -extern float mt_phi; -extern float mt_p; -extern float mt_bp; -extern float mt_P_phi[][]; +extern float mtt_phi; +extern float mtt_p; +extern float mtt_bp; +extern float mtt_P_phi[2][2]; -extern void multitilt_reset(void); -extern void multitilt_run(void); +extern float mtt_theta; +extern float mtt_q; +extern float mtt_bq; +extern float mtt_P_theta[2][2]; +extern float mtt_r; +extern float mtt_br; + +extern void multitilt_init(void); +extern void multitilt_start( const float* accel, const float* gyro); +extern void multitilt_predict( const float* gyro ); +extern void multitilt_update( const float* accel ); #endif /* MULTITILT_H */