mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -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
|
||||
|
||||
</makefile>
|
||||
|
||||
</airframe>
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
+43
-27
@@ -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 */
|
||||
|
||||
+131
-107
@@ -2,153 +2,177 @@
|
||||
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#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;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
+16
-8
@@ -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 */
|
||||
|
||||
Reference in New Issue
Block a user