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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2006-12-05 19:31:56 +00:00
parent 8e627cf9d0
commit 1920c36429
9 changed files with 304 additions and 427 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/imu.xml
+1 -1
View File
@@ -20,7 +20,7 @@ imu.srcs = main_imu.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
#imu.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
imu.srcs += $(SRC_ARCH)/uart_hw.c
imu.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B9600 -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
imu.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
imu.srcs += downlink.c pprz_transport.c
imu.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
conf/messages.xml
+19 -22
View File
@@ -304,15 +304,25 @@
</message>
<message name="IMU_SENSORS" ID="200">
<field name="rates" type="float[]" unit="rad/s"/>
<message name="IMU_GYRO" ID="200">
<field name="gx" type="float" unit="rad/s"/>
<field name="gy" type="float" unit="rad/s"/>
<field name="gz" type="float" unit="rad/s"/>
</message>
<message name="IMU_FOO" ID="201">
<field name="rate" type="float" unit="rad/s"/>
<message name="IMU_MAG" ID="201">
<field name="ax" type="int16" />
<field name="ay" type="int16" />
<field name="az" type="int16" />
</message>
<message name="AHRS" ID="202">
<message name="IMU_ACCEL" ID="202">
<field name="ax" type="float" unit="rad/s"/>
<field name="ay" type="float" unit="rad/s"/>
<field name="az" type="float" unit="rad/s"/>
</message>
<message name="AHRS_STATE" ID="203">
<field name="q0" type="float"/>
<field name="q1" type="float"/>
<field name="q2" type="float"/>
@@ -322,25 +332,12 @@
<field name="bz" type="float"/>
</message>
<message name="AHRS2" ID="203">
<field name="roll" type="float"/>
<field name="pitch" type="float"/>
<field name="yaw" type="float"/>
<message name="AHRS_COV" ID="204">
<field name="p00" type="float"/>
<field name="p11" type="float"/>
</message>
<message name="AHRS_ACCEL" ID="204">
<field name="ax" type="float"/>
<field name="ay" type="float"/>
<field name="az" type="float"/>
</message>
<message name="AHRS_MAG" ID="205">
<field name="mx" type="int16"/>
<field name="my" type="int16"/>
<field name="mz" type="int16"/>
</message>
<message name="TIME" ID="206">
<message name="TIME" ID="206">
<field name="t" type="uint32"/>
</message>
sw/airborne/ahrs_new.c
+84 -306
View File
File diff suppressed because it is too large Load Diff
sw/airborne/ahrs_new.h
+21 -15
View File
@@ -4,24 +4,30 @@
#include <inttypes.h>
#define real_t float
#define index_t uint8_t
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 */
/* our state */
extern float q0, q1, q2, q3;
extern float bias_p, bias_q, bias_r;
/* we maintain eulers angles */
extern float ahrs_phi, ahrs_theta, ahrs_psi;
/* we maintain unbiased rates */
extern float ahrs_p, ahrs_q, ahrs_r;
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( const int16_t* mag);
extern float P[7][7]; /* covariance */
extern float A[4][7]; /* jacobian */
extern real_t ahrs_roll_of_accel( real_t* accel_cal );
extern real_t ahrs_pitch_of_accel( real_t* accel_cal);
extern real_t ahrs_heading_of_mag( const int16_t *mag);
extern void ahrs_init( const int16_t *mag, const float* accel, const float* gyro );
extern void ahrs_predict( const float* gyro );
extern void ahrs_roll_update( const float* accel);
extern void ahrs_pitch_update( const float* accel);
extern void ahrs_yaw_update( const int16_t* mag);
#if 0
extern float ahrs_roll_of_accel( const float* accel_cal );
extern float ahrs_pitch_of_accel( const float* accel_cal);
extern float ahrs_yaw_of_mag( const int16_t *mag);
#endif
#endif /* AHRS_H */
sw/airborne/link_imu.c
+24 -24
View File
@@ -90,30 +90,30 @@ void link_imu_init ( void ) {
}
void link_imu_send ( void ) {
if (!isnan(link_imu_state.eulers[AXIS_X]) &&
!isnan(link_imu_state.eulers[AXIS_X]) &&
!isnan(link_imu_state.eulers[AXIS_X])) {
link_imu_state.rates[AXIS_X] = ahrs_pqr[AXIS_X]*RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Y] = ahrs_pqr[AXIS_Y]*RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Z] = ahrs_pqr[AXIS_Z]*RATE_PI_S/M_PI;
link_imu_state.eulers[AXIS_X] = ahrs_euler[AXIS_X]*ANGLE_PI/M_PI;
link_imu_state.eulers[AXIS_Y] = ahrs_euler[AXIS_Y]*ANGLE_PI/M_PI;
link_imu_state.eulers[AXIS_Z] = ahrs_euler[AXIS_Z]*ANGLE_PI/M_PI;
link_imu_state.cos_theta = cos(link_imu_state.eulers[AXIS_Z]);
link_imu_state.sin_theta = sin(link_imu_state.eulers[AXIS_Z]);
link_imu_state.status = IMU_RUNNING;
}
else {
link_imu_state.rates[AXIS_X] = imu_gyro[AXIS_X]*RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Y] = imu_gyro[AXIS_Y]*RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Z] = imu_gyro[AXIS_Z]*RATE_PI_S/M_PI;
link_imu_state.eulers[AXIS_X] = 0;
link_imu_state.eulers[AXIS_Y] = 0;
link_imu_state.eulers[AXIS_Z] = 0;
link_imu_state.cos_theta = 1.;
link_imu_state.sin_theta = 0.;
link_imu_state.status = IMU_CRASHED;
}
if (!isnan(ahrs_phi) &&
!isnan(ahrs_theta) &&
!isnan(ahrs_psi)) {
link_imu_state.rates[AXIS_X] = ahrs_p * RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Y] = ahrs_q * RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Z] = ahrs_r * RATE_PI_S/M_PI;
link_imu_state.eulers[AXIS_X] = ahrs_phi * ANGLE_PI/M_PI;
link_imu_state.eulers[AXIS_Y] = ahrs_theta* ANGLE_PI/M_PI;
link_imu_state.eulers[AXIS_Z] = ahrs_psi * ANGLE_PI/M_PI;
link_imu_state.cos_theta = cos(link_imu_state.eulers[AXIS_Z]);
link_imu_state.sin_theta = sin(link_imu_state.eulers[AXIS_Z]);
link_imu_state.status = IMU_RUNNING;
}
else {
link_imu_state.rates[AXIS_X] = imu_gyro[AXIS_X]*RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Y] = imu_gyro[AXIS_Y]*RATE_PI_S/M_PI;
link_imu_state.rates[AXIS_Z] = imu_gyro[AXIS_Z]*RATE_PI_S/M_PI;
link_imu_state.eulers[AXIS_X] = 0;
link_imu_state.eulers[AXIS_Y] = 0;
link_imu_state.eulers[AXIS_Z] = 0;
link_imu_state.cos_theta = 1.;
link_imu_state.sin_theta = 0.;
link_imu_state.status = IMU_CRASHED;
}
Spi0InitBuf();
}
sw/airborne/main_imu.c
+21 -42
View File
@@ -21,17 +21,12 @@
#define SEND_MAG 1
#define SEND_GYRO 1
#define SEND_AHRS_STATE 1
#define SEND_AHRS_COV 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_az;
//struct adc_buf buf_bat;
static inline void ahrs_task( void );
static void main_init_spi1( void );
@@ -126,53 +121,37 @@ static inline void main_periodic_task( void ) {
static inline void ahrs_task( void ) {
ahrs_pqr[AXIS_X] = imu_gyro[AXIS_X];
ahrs_pqr[AXIS_Y] = imu_gyro[AXIS_Y];
ahrs_pqr[AXIS_Z] = imu_gyro[AXIS_Z];
switch (ahrs_step) {
case AHRS_STEP_UNINIT : {
/* discard first 100 measures */
if (ahrs_step == AHRS_STEP_UNINIT) {
static uint8_t init_count = 0;
init_count++;
if (init_count < 100) ahrs_step--;
else {
ahrs_euler[AXIS_X] = ahrs_roll_of_accel(imu_accel);
ahrs_euler[AXIS_Y] = ahrs_pitch_of_accel(imu_accel);
ahrs_euler[AXIS_Z] = 0.;
imu_mag[AXIS_X] = 100; imu_mag[AXIS_Y] = 0; imu_mag[AXIS_Z] = 0;
ahrs_init(imu_mag);
if (init_count > 100) {
ahrs_step = AHRS_STEP_ROLL;
ahrs_init(imu_mag, imu_accel, imu_gyro);
}
}
break;
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(imu_mag);
else {
ahrs_predict(imu_gyro);
switch (ahrs_step) {
case AHRS_STEP_ROLL:
ahrs_roll_update(imu_accel);
break;
case AHRS_STEP_PITCH:
ahrs_pitch_update(imu_accel);
break;
case AHRS_STEP_YAW:
ahrs_yaw_update(imu_mag);
break;
}
#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++;
if (ahrs_step == AHRS_STEP_NB) ahrs_step = AHRS_STEP_ROLL;
}
ahrs_step++;
if (ahrs_step == AHRS_STEP_NB) ahrs_step = AHRS_STEP_ROLL;
}
sw/logalizer/Makefile
+4 -2
View File
@@ -63,9 +63,11 @@ clean:
rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrsview imuview ahrs2fg
FGFS_ROOT = /home/poine/work/flightgear_cvs
FGFS = $(FGFS_ROOT)/bin/fgfs
#FGFS = $(FGFS_ROOT)/bin/fgfs
FGFS = fgfs
FGFS_ENV = LD_LIBRARY_PATH=/usr/local/lib:$(FGFS_ROOT)/lib
FGFS_COMMON_ARGS = --fg-root=$(FGFS_ROOT) --aircraft=A320 --timeofday=noon
#FGFS_COMMON_ARGS = --fg-root=$(FGFS_ROOT) --aircraft=A320 --timeofday=noon
FGFS_COMMON_ARGS = --aircraft=737-300 --timeofday=noon
FGFS_IN_FDM_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-fdm=socket,in,30,,5501,udp
FGFS_OUT_FDM_ARGS = $(FGFS_COMMON_ARGS) --native-fdm=socket,out,30,,5500,udp
FGFS_IN_GUI_ARGS = $(FGFS_COMMON_ARGS) --fdm=null --native-gui=socket,in,30,,5501,udp
sw/logalizer/ahrs_utils.h
+129 -14
View File
@@ -1,6 +1,9 @@
#ifndef AHRS_UTILS_H
#define AHRS_UTILS_H
#include "frames.h"
#ifdef DEBUG
#include <stdio.h>
#define PrintEuler() { \
@@ -21,6 +24,7 @@
#define PrintC() { \
printf("C %.2f %.2f %.2f %.2f\n\n", C[0], C[1], C[2], C[3]); \
}
#endif /* DEBUG */
extern float C[4];
@@ -30,15 +34,12 @@ 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;
/*
* Compute the five elements of the DCM that we use for our
* rotations and Jacobians. This is used by several other functions
* to speedup their computations.
*/
static inline void DCM_of_quat( void ) {
dcm00 = 1.0-2*(q2*q2 + q3*q3);
dcm01 = 2*(q1*q2 + q0*q3);
@@ -48,15 +49,19 @@ static inline void DCM_of_quat( void ) {
}
static inline void eulers_of_DCM ( void ) {
phi = atan2( dcm12, dcm22 );
theta = -asin( dcm02 );
psi = atan2( dcm01, dcm00 );
ahrs_phi = atan2( dcm12, dcm22 );
ahrs_theta = -asin( dcm02 );
ahrs_psi = atan2( dcm01, dcm00 );
}
/*
* initialise a quaternion from a set of eulers
*/
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 phi2 = ahrs_phi / 2.0;
const float theta2 = ahrs_theta / 2.0;
const float psi2 = ahrs_psi / 2.0;
const float sinphi2 = sin( phi2 );
const float cosphi2 = cos( phi2 );
@@ -73,6 +78,9 @@ static inline void quat_of_eulers ( void ) {
q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
}
/*
* normalize quaternion
*/
static inline void norm_quat( void ) {
float mag = q0*q0 + q1*q1 + q2*q2 + q3*q3;
mag = sqrt( mag );
@@ -82,6 +90,12 @@ static inline void norm_quat( void ) {
q3 /= mag;
}
/*
* Compute the Jacobian of the measurements to the system states.
* You must have already computed the DCM for the quaternion before
* calling this function.
*/
#if 1
static inline void compute_dphi_dq( void ) {
const float phi_err = 2 / (dcm22*dcm22 + dcm12*dcm12);
@@ -108,6 +122,107 @@ static inline void compute_dpsi_dq( void ) {
C[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err;
C[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err;
}
#else
/* paper JSchlep p85 */
static inline void compute_dphi_dq( void ) {
const float a = (q2+q1)*(q2+q1) + (q3+q0)*(q3+q0);
const float b = (q2-q1)*(q2-q1) + (q3-q0)*(q3-q0);
C[0] = -(q2+q1)/a - (q2-q1)/b;
C[1] = (q3+q0)/a + (q3-q0)/b;
C[2] = (q3+q0)/a - (q3-q0)/b;
C[3] = -(q2+q1)/a + (q2-q1)/b;
}
static inline void compute_dtheta_dq( void ) {
const float a = 2 / sqrt( 1 - 4*(q1*q2 + q0*q3)*(q1*q2 + q0*q3));
C[0] = q3 * a;
C[1] = q2 * a;
C[2] = q1 * a;
C[3] = q0 * a;
}
static inline void compute_dpsi_dq( void ) {
const float a = (q2+q1)*(q2+q1) + (q3+q0)*(q3+q0);
const float b = (q2-q1)*(q2-q1) + (q3-q0)*(q3-q0);
C[0] = -(q2+q1)/a + (q2-q1)/b;
C[1] = (q3+q0)/a - (q3-q0)/b;
C[2] = (q3+q0)/a + (q3-q0)/b;
C[3] = -(q2+q1)/a - (q2-q1)/b;
}
#endif
static inline float ahrs_roll_of_accel( const float* accel ) {
return atan2(accel[AXIS_Y], accel[AXIS_Z]);
}
static inline float ahrs_pitch_of_accel( const float* accel) {
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 ) );
}
/*
* The rotation matrix to rotate from NED frame to body frame without
* rotating in the yaw axis is:
*
* [ 1 0 0 ] [ cos(Theta) 0 -sin(Theta) ]
* [ 0 cos(Phi) sin(Phi) ] [ 0 1 0 ]
* [ 0 -sin(Phi) cos(Phi) ] [ sin(Theta) 0 cos(Theta) ]
*
* This expands to:
*
* [ cos(Theta) 0 -sin(Theta) ]
* [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)]
* [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)]
*
* However, to untilt the compass reading, we need to use the
* transpose of this matrix.
*
* [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ]
* [ 0 cos(Phi) -sin(Phi) ]
* [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ]
*
* Additionally,
* since we already have the DCM computed for our current attitude,
* we can short cut all of the trig. substituting
* in from the definition of euler2quat and quat2euler, we have:
*
* [ cos(Theta) -dcm12*dcm02 -dcm22*dcm02 ]
* [ 0 dcm22 -dcm12 ]
* [ dcm02 dcm12 dcm22 ]
*
*/
static inline float ahrs_yaw_of_mag( const int16_t* mag ) {
const float ctheta = cos( ahrs_theta );
#if 1
const float mn = ctheta * mag[0]
- (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta;
const float me =
(dcm22 * mag[1] - dcm12 * mag[2]) / ctheta;
#else
const float stheta = sin( ahrs_theta );
const float cphi = cos( ahrs_phi );
const float sphi = sin( ahrs_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];
#endif
const float yaw = -atan2( me, mn );
return yaw;
}
sw/logalizer/disp3d.c
+1 -1
View File
@@ -78,7 +78,7 @@ GTimer *gtimer = NULL;
double game_time;
double game_tick;
int draw_fast = 0;
int draw_fast = 1;