Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2006-12-04 03:07:34 +00:00
parent fe85403ba1
commit 71b4392ea7
8 changed files with 307 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/ahrs_new.c
+40 -88
View File
@@ -15,9 +15,6 @@
* frequency with which ahrs_state_update() is called with the body
* angular rates.
*/
//#define dt 0.04
//#define dt 0.0625
//#define dt 0.0117185
#define dt 0.015625
//#define CONFIG_SPLIT_COVARIANCE
@@ -75,11 +72,8 @@ static real_t dcm22;
* every other state step. This is because the covariance should change
* at a rate somewhat slower than the dynamics of the system.
*/
static real_t P[7][7];
real_t P[7][7];
static real_t Pdot[7][7];
#ifdef CONFIG_SPLIT_COVARIANCE
static index_t covariance_state;
#endif
/*
* A represents the Jacobian of the derivative of the system with respect
@@ -89,8 +83,7 @@ static index_t covariance_state;
real_t A[4][7];
/*
* These Kalman filter variables share space with A when the filter
* is being updated.
* Kalman filter variables.
*/
real_t PCt[7];
real_t K[7];
@@ -104,7 +97,7 @@ real_t E;
*
* Since we compute each axis independently, we only allocate one
* column out of the C matrix at a time. This allows us to reuse the
* matrix. Additionally, this space is shared by Qdot.
* matrix.
*/
real_t C[4];
real_t Qdot[4];
@@ -140,28 +133,17 @@ real_t Qdot[4];
/*
* Simple helper to normalize our quaternion attitude estimate. We could
* probably do this at a lower time step to save on calls to sqrt().
* Simple helper to normalize our quaternion attitude estimate.
*/
void
norm_quat( void )
{
// index_t i;
real_t mag = 0;
// for( i=0 ; i<4 ; i++ )
// mag += quat[i] * quat[i];
mag = q0*q0 + q1*q1 + q2*q2 + q3*q3;
real_t mag = q0*q0 + q1*q1 + q2*q2 + q3*q3;
mag = sqrt( mag );
// for( i=0 ; i<4 ; i++ )
// quat[i] /= mag;
q0 /= mag;
q1 /= mag;
q2 /= mag;
q3 /= mag;
}
/*
@@ -231,16 +213,17 @@ compute_A_bias( void )
*
* P_dot = A*P + P*A_transpose + Q
*
* However, this takes a very long time to compute. So we split
* the multiplications and additions into two separate routines.
*
* The first of these zeros P_dot, computes part of (A*P + P*A_tranpose)
* and adds in the parts of Q that are non-zero. Note that we know that
* A has three zero rows at the bottom, so we do not include those in our
* math.
*
* The second part of the covariance update computes the inner portion
* of Pdot, the 4x4 region that corresponds to the quaternion. It also
* updates the covariance matrix P.
*/
static void
covariance_update_0( void )
covariance_update( void )
{
index_t i;
index_t j;
@@ -270,20 +253,6 @@ covariance_update_0( void )
*/
for( i=4 ; i<7 ; i++ )
Pdot[i][i] += Q_gyro;
}
/*
* The second part of the covariance update computes the inner portion
* of Pdot, the 4x4 region that corresponds to the quaternion. It also
* updates the covariance matrix P.
*/
static void
covariance_update_1( void )
{
index_t i;
index_t j;
index_t k;
/*
* Compute A*P + P*At for the region [0..3][0..3]
@@ -373,20 +342,7 @@ ahrs_state_update( void )
*/
//norm_quat();
#ifdef CONFIG_SPLIT_COVARIANCE
/* Compute our split covariance update */
if( covariance_state == 0 ) {
covariance_state = 1;
covariance_update_0();
}
else {
covariance_state = 0;
covariance_update_1();
}
#else
covariance_update_0();
covariance_update_1();
#endif
covariance_update();
}
/*
@@ -413,17 +369,14 @@ compute_DCM( void )
static inline void
compute_dphi_dq( void )
{
index_t i;
const real_t phi_err = 2 / (dcm22*dcm22 + dcm12*dcm12);
C[0] = (q1 * dcm22);
C[1] = (q0 * dcm22 + 2 * q1 * dcm12);
C[2] = (q3 * dcm22 + 2 * q2 * dcm12);
C[3] = (q2 * dcm22);
for( i=0 ; i<4 ; i++ )
C[i] *= phi_err;
C[0] = (q1 * dcm22) * phi_err;
C[1] = (q0 * dcm22 + 2 * q1 * dcm12) * phi_err;
//C[1] = (q0 * dcm22 ) * phi_err;
C[2] = (q3 * dcm22 + 2 * q2 * dcm12) * phi_err;
//C[2] = (q3 * dcm22) * phi_err;
C[3] = (q2 * dcm22) * phi_err;
}
static inline void
@@ -440,17 +393,14 @@ compute_dtheta_dq( void )
static inline void
compute_dpsi_dq( void )
{
index_t i;
const float psi_err = 2 / (dcm00*dcm00 + dcm01*dcm01);
const real_t psi_err = 2 / (dcm00*dcm00 + dcm01*dcm01);
C[0] = (q3 * dcm00);
C[1] = (q2 * dcm00);
C[2] = (q1 * dcm00 + 2 * q2 * dcm01);
C[3] = (q0 * dcm00 + 2 * q3 * dcm01);
for( i=0 ; i<4 ; i++ )
C[i] *= psi_err;
C[0] = (q3 * dcm00) * psi_err;
C[1] = (q2 * dcm00) * psi_err;
C[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err;
//C[2] = (q1 * dcm00) * psi_err;
C[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err;
//C[3] = (q0 * dcm00) * psi_err;
}
@@ -711,9 +661,11 @@ real_t ahrs_pitch_of_accel( real_t* accel_cal) {
*/
void
ahrs_compass_update(
real_t heading
const int16_t* mag
)
{
real_t heading = ahrs_heading_of_mag(mag);
// Update the DCM since this will require
compute_DCM();
compute_euler_heading();
@@ -738,23 +690,23 @@ ahrs_compass_update(
static void
euler2quat( void )
{
const real_t phi = ahrs_euler[0] / 2.0;
const real_t theta = ahrs_euler[1] / 2.0;
const real_t psi = ahrs_euler[2] / 2.0;
const real_t phi0 = ahrs_euler[0] / 2.0;
const real_t theta0 = ahrs_euler[1] / 2.0;
const real_t psi0 = ahrs_euler[2] / 2.0;
const real_t shphi0 = sin( phi );
const real_t chphi0 = cos( phi );
const real_t sinphi0 = sin( phi0 );
const real_t cosphi0 = cos( phi0 );
const real_t shtheta0 = sin( theta );
const real_t chtheta0 = cos( theta );
const real_t sintheta0 = sin( theta0 );
const real_t costheta0 = cos( theta0 );
const real_t shpsi0 = sin( psi );
const real_t chpsi0 = cos( psi );
const real_t sinpsi0 = sin( psi0 );
const real_t cospsi0 = cos( psi0 );
q0 = chphi0 * chtheta0 * chpsi0 + shphi0 * shtheta0 * shpsi0;
q1 = -chphi0 * shtheta0 * shpsi0 + shphi0 * chtheta0 * chpsi0;
q2 = chphi0 * shtheta0 * chpsi0 + shphi0 * chtheta0 * shpsi0;
q3 = chphi0 * chtheta0 * shpsi0 - shphi0 * shtheta0 * chpsi0;
q0 = cosphi0 * costheta0 * cospsi0 + sinphi0 * sintheta0 * sinpsi0;
q1 = -cosphi0 * sintheta0 * sinpsi0 + sinphi0 * costheta0 * cospsi0;
q2 = cosphi0 * sintheta0 * cospsi0 + sinphi0 * costheta0 * sinpsi0;
q3 = cosphi0 * costheta0 * sinpsi0 - sinphi0 * sintheta0 * cospsi0;
}
/*
sw/airborne/ahrs_new.h
+3 -2
View File
@@ -11,13 +11,14 @@ extern real_t ahrs_pqr[3];
extern real_t q0, q1, q2, q3;
extern real_t ahrs_euler[3];
extern real_t bias_p, bias_q, bias_r;
extern real_t P[7][7]; /* covariance */
extern real_t A[4][7]; /* jacobian */
extern void ahrs_init( const int16_t *mag );
extern void ahrs_state_update( void );
extern void ahrs_pitch_update( real_t pitch);
extern void ahrs_roll_update( real_t roll);
extern void ahrs_compass_update( real_t heading );
extern void ahrs_compass_update( const int16_t* mag);
extern real_t ahrs_roll_of_accel( real_t* accel_cal );
extern real_t ahrs_pitch_of_accel( real_t* accel_cal);
sw/airborne/imu_v3.c
+1 -1
View File
@@ -13,7 +13,7 @@ int16_t imu_mag[AXIS_NB];
float imu_bat;
struct adc_buf buf_ax;
/* struct adc_buf buf_ay; */
struct adc_buf buf_ay;
struct adc_buf buf_az;
struct adc_buf buf_bat;
sw/airborne/imu_v3.h
+6 -7
View File
@@ -8,7 +8,7 @@
/* accelerometers in arbitrary unit */
extern float imu_accel[AXIS_NB];
/* gyros in rad/s */
extern float imu_gyro[AXIS_NB];
extern float imu_gyro[AXIS_NB];
/* magnetometer in arbitrary unit */
extern int16_t imu_mag[AXIS_NB];
/* battery in volts */
@@ -16,7 +16,6 @@ extern float imu_bat;
extern void imu_init(void);
extern struct adc_buf buf_ax;
extern struct adc_buf buf_ay;
extern struct adc_buf buf_az;
@@ -34,14 +33,14 @@ extern struct adc_buf buf_bat;
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; \
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; \
}
#else
#define ImuUpdateAccels() { \
#define ImuUpdateAccels() { \
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
sw/airborne/main_imu.c
+29 -8
View File
@@ -17,13 +17,18 @@
#include "messages.h"
#include "downlink.h"
#define SEND_ACCEL 1
#define SEND_MAG 1
#define SEND_GYRO 1
#define SEND_AHRS_STATE 1
#define SEND_AHRS_COV 1
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void);
//struct adc_buf buf_ax;
struct adc_buf buf_ay;
//struct adc_buf buf_ay;
//struct adc_buf buf_az;
//struct adc_buf buf_bat;
@@ -66,7 +71,6 @@ static inline void main_init( void ) {
main_init_spi1();
max1167_init();
micromag_init();
// spi0_init();
link_imu_init();
int_enable();
}
@@ -79,7 +83,9 @@ static inline void main_event_task( void ) {
micromag_data_available = FALSE;
spi_cur_slave = SPI_SLAVE_NONE;
ImuUpdateMag();
DOWNLINK_SEND_AHRS_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
#ifdef SEND_MAG
DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
#endif
spi_cur_slave = SPI_SLAVE_MAX;
max1167_read();
}
@@ -88,11 +94,17 @@ static inline void main_event_task( void ) {
max1167_data_available = FALSE;
spi_cur_slave = SPI_SLAVE_NONE;
ImuUpdateGyros();
#ifdef SEND_GYRO
DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]);
#endif
ImuUpdateAccels();
#ifdef SEND_ACCEL
DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]);
#endif
ahrs_task();
t1=T0TC;
uint32_t dt = t1 - t0;
// uint32_t dt = t1 - t0;
// DOWNLINK_SEND_TIME(&dt);
link_imu_send();
@@ -137,17 +149,26 @@ static inline void ahrs_task( void ) {
case AHRS_STEP_ROLL:
ahrs_state_update();
ahrs_roll_update(ahrs_roll_of_accel(imu_accel));
#ifdef SEND_AHRS_STATE
DOWNLINK_SEND_AHRS_STATE(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r);
#endif
break;
case AHRS_STEP_PITCH:
ahrs_state_update();
ahrs_pitch_update(ahrs_pitch_of_accel(imu_accel));
#ifdef SEND_AHRS_STATE
DOWNLINK_SEND_AHRS_STATE(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r);
#endif
break;
case AHRS_STEP_YAW:
ahrs_state_update();
ahrs_compass_update(ahrs_heading_of_mag(imu_mag));
DOWNLINK_SEND_AHRS(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r);
// DOWNLINK_SEND_AHRS2(&ahrs_euler[AXIS_X], &ahrs_euler[AXIS_Y], &ahrs_euler[AXIS_Z]);
// DOWNLINK_SEND_AHRS2(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]);
ahrs_compass_update(imu_mag);
#ifdef SEND_AHRS_STATE
DOWNLINK_SEND_AHRS_STATE(&q0, &q1, &q2, &q3, &bias_p, &bias_q, &bias_r);
#endif
#ifdef SEND_AHRS_COV
DOWNLINK_SEND_AHRS_COV(&P[0][0], &P[1][1]);
#endif
break;
}
ahrs_step++;
sw/logalizer/Makefile
+5
View File
@@ -29,6 +29,11 @@ all: play
play : play.ml
$(OCAMLC) $(INCLUDES) -custom -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma -I +lablgtk2 -I ../lib/ocaml lablgtk.cma lib-pprz.cma gtkInit.cmo $^
pt : ahrsview imuview ahrs2fg
CC = gcc
CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags`
LDFLAGS=`pkg-config gtk+-2.0 --libs` -s -lgtkdatabox `pcre-config --libs` -lglibivy
sw/logalizer/ahrs_utils.h
+114
View File
@@ -0,0 +1,114 @@
#ifndef AHRS_UTILS_H
#define AHRS_UTILS_H
#include <stdio.h>
#define PrintEuler() { \
printf("euler %f %f %f\n\n", phi, theta, psi); \
}
#define PrintQuat() { \
float norm = sqrt ( q0*q0 + q1*q1 + q2*q2 + q3*q3); \
printf("quat %f %f %f %f (%f)\n\n", q0, q1, q2, q3, norm); \
}
#define PrintDCM() { \
printf("DCM %.2f %.2f %.2f\n", dcm00, dcm01, dcm02); \
printf(" XXXX XXXX %.2f\n", dcm12); \
printf(" XXXX XXXX %.2f\n\n", dcm22); \
}
#define PrintC() { \
printf("C %.2f %.2f %.2f %.2f\n\n", C[0], C[1], C[2], C[3]); \
}
extern float C[4];
extern float dcm00;
extern float dcm01;
extern float dcm02;
extern float dcm12;
extern float dcm22;
extern float phi;
extern float theta;
extern float psi;
extern float q0;
extern float q1;
extern float q2;
extern float q3;
static inline void DCM_of_quat( void ) {
dcm00 = 1.0-2*(q2*q2 + q3*q3);
dcm01 = 2*(q1*q2 + q0*q3);
dcm02 = 2*(q1*q3 - q0*q2);
dcm12 = 2*(q2*q3 + q0*q1);
dcm22 = 1.0-2*(q1*q1 + q2*q2);
}
static inline void eulers_of_DCM ( void ) {
phi = atan2( dcm12, dcm22 );
theta = -asin( dcm02 );
psi = atan2( dcm01, dcm00 );
}
static inline void quat_of_eulers ( void ) {
const float phi2 = phi / 2.0;
const float theta2 = theta / 2.0;
const float psi2 = psi / 2.0;
const float sinphi2 = sin( phi2 );
const float cosphi2 = cos( phi2 );
const float sintheta2 = sin( theta2 );
const float costheta2 = cos( theta2 );
const float sinpsi2 = sin( psi2 );
const float cospsi2 = cos( psi2 );
q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
}
static inline void norm_quat( void ) {
float mag = q0*q0 + q1*q1 + q2*q2 + q3*q3;
mag = sqrt( mag );
q0 /= mag;
q1 /= mag;
q2 /= mag;
q3 /= mag;
}
static inline void compute_dphi_dq( void ) {
const float phi_err = 2 / (dcm22*dcm22 + dcm12*dcm12);
C[0] = (q1 * dcm22) * phi_err;
C[1] = (q0 * dcm22 + 2 * q1 * dcm12) * phi_err;
C[2] = (q3 * dcm22 + 2 * q2 * dcm12) * phi_err;
C[3] = (q2 * dcm22) * phi_err;
}
static inline void compute_dtheta_dq( void ) {
const float theta_err = -2 / sqrt( 1 - dcm02*dcm02 );
C[0] = -q2 * theta_err;
C[1] = q3 * theta_err;
C[2] = -q0 * theta_err;
C[3] = q1 * theta_err;
}
static inline void compute_dpsi_dq( void ) {
const float psi_err = 2 / (dcm00*dcm00 + dcm01*dcm01);
C[0] = (q3 * dcm00) * psi_err;
C[1] = (q2 * dcm00) * psi_err;
C[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err;
C[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err;
}
#endif /* AHRS_UTILS_H */
sw/logalizer/test_2.c
+109
View File
@@ -0,0 +1,109 @@
#include <math.h>
#include "ahrs_utils.h"
float C[4];
float dcm00;
float dcm01;
float dcm02;
float dcm12;
float dcm22;
float phi;
float theta;
float psi;
float q0;
float q1;
float q2;
float q3;
/* d_euler / dq */
/*
phi = atan ( 2(q2q3 + q0q1) / (q0^2 - q1^2 - q2^2 + q3^2))
*/
void test_dphi_dq ( void ) {
float my_dcm22 = q0*q0 - q1*q1 - q2*q2 + q3*q3;
float dcm22_sq = my_dcm22 * my_dcm22;
float my_dcm12 = 2*(q2*q3 + q0*q1);
float dcm12_sq = my_dcm12 * my_dcm12;
C[0] = 2 * q1 * dcm22 / (dcm22_sq + dcm12_sq);
C[1] = 2 * q0 * dcm22 / (dcm22_sq + dcm12_sq);
C[2] = 2 * q3 * dcm22 / (dcm22_sq + dcm12_sq);
C[3] = 2 * q2 * dcm22 / (dcm22_sq + dcm12_sq);
}
/*
theta = asin(-2(q1q3 - q0q2))
dasin = 1/sqrt(1-x^2)
*/
void test_dtheta_dq ( void ) {
float my_dcm02 = 2 * (q1*q3 - q0*q2);
float dcm02_sq = my_dcm02 * my_dcm02;
C[0] = 2 * q2 / (sqrt(1 - dcm02_sq));
C[1] = -2 * q3 / (sqrt(1 - dcm02_sq));
C[2] = 2 * q0 / (sqrt(1 - dcm02_sq));
C[3] = -2 * q1 / (sqrt(1 - dcm02_sq));
}
/*
psi = atan ( 2(q1q2 + q0q3) / (q0^2 + q1^2 - q2^2 - q3^2))
datan = 1/(1+x^2)
*/
void test_dpsi_dq ( void ) {
float my_dcm00 = 1 - 2. * q2 * q2 - 2. * q3 * q3;
float my_dcm01 = 2*(q1*q2 +q0*q3);
float dcm00_sq = my_dcm00 * my_dcm00;
float dcm01_sq = my_dcm01 * my_dcm01;
C[0] = 2. * q3 * dcm00 / (dcm00_sq + dcm01_sq);
C[1] = 2. * q2 * dcm00 / (dcm00_sq + dcm01_sq);
C[2] = 2. * q1 * dcm00 / (dcm00_sq + dcm01_sq);
C[3] = 2. * q0 * dcm00 / (dcm00_sq + dcm01_sq);
}
int main (int argc, char** argv) {
phi = 0.3;
theta = 0.5;
psi = 0.5;
PrintEuler()
quat_of_eulers();
PrintQuat();
DCM_of_quat();
PrintDCM();
#if 1
printf("phi\n");
compute_dphi_dq();
PrintC();
test_dphi_dq();
PrintC();
#endif
#if 1
printf("theta\n");
compute_dtheta_dq();
PrintC();
test_dtheta_dq();
PrintC();
#endif
#if 1
printf("psi\n");
compute_dpsi_dq();
PrintC();
test_dpsi_dq();
PrintC();
#endif
return 0;
}