Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

some work for SECTION_IMU_ANALOG

Browse Source
This commit is contained in:
jpdumont
2005-08-28 20:58:01 +00:00
parent 2a6051f86b
commit 9712e740d7
8 changed files with 246 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/gorazoptere_brushless_ANALOG.xml
+2
View File
@@ -1,5 +1,7 @@
<airframe name="Gorazoptere_brushless_ANALOG" ctl_board="V1_2" gps="SAM_LS">
<section name="IMU_ANALOG" prefix="IMU_">
<define name="INIT_EULER_DOT_VARIANCE_MAX" value="3"/>
<define name="INIT_EULER_DOT_NB_SAMPLES_MIN" value ="10"/>
<define name="ADC_ROLL_DOT" value="1"/>
<define name="ADC_PITCH_DOT" value="2"/>
<define name="ADC_YAW_DOT" value="3"/>
sw/airborne/autopilot/ahrs.c
+178 -99
View File
@@ -63,6 +63,9 @@
//#define IMU_ADC_YAW_DOT 7
//#define IMU_ADC_YAW_DOT_SIGN -
//#define IMU_INIT_EULER_DOT_VARIANCE_MAX 2
//#define IMU_INIT_EULER_DOT_NB_SAMPLES_MIN 10
#define GYRO_SCALE (real_t) (0.9444 * 3.14159 / 180.0)
#define GYRO_ZERO 0x200
@@ -101,7 +104,7 @@ int16_t accel[3];
* The euler estimate will be updated less frequently than the
* quaternion, but some applications are ok with that.
*/
real_t euler[3];
real_t ahrs_euler[3];
/*
@@ -499,19 +502,19 @@ compute_dpsi_dq( void )
static inline void
compute_euler_roll( void )
{
euler[0] = atan2( dcm12, dcm22 );
ahrs_euler[0] = atan2( dcm12, dcm22 );
}
static inline void
compute_euler_pitch( void )
{
euler[1] = -asin( dcm02 );
ahrs_euler[1] = -asin( dcm02 );
}
static inline void
compute_euler_heading( void )
{
euler[2] = atan2( dcm01, dcm00 );
ahrs_euler[2] = atan2( dcm01, dcm00 );
}
@@ -613,7 +616,7 @@ ahrs_roll_update(
* Compute the error in our roll estimate and measurement.
* This can be between -180 and +180 degrees.
*/
roll -= euler[0];
roll -= ahrs_euler[0];
if( roll < -C_PI )
roll += 2 * C_PI;
if( roll > C_PI )
@@ -639,7 +642,7 @@ ahrs_pitch_update(
* Pitch can be between -90 and +90 degrees, so wrap it around
* for the shortest distance.
*/
pitch -= euler[1];
pitch -= ahrs_euler[1];
if( pitch < -C_PI / 2 )
pitch += C_PI;
if( pitch > C_PI / 2 )
@@ -686,7 +689,7 @@ untilt_compass(
const int16_t * mag
)
const real_t ctheta = cos( euler[1] );
const real_t ctheta = cos( ahrs_euler[1] );
const real_t mn = ctheta * mag[0]
- (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta;
@@ -728,7 +731,7 @@ ahrs_compass_update(
* Heading can be between -180 and +180 degrees, so we wrap
* to find the shortest turn between the two.
*/
heading -= euler[2];
heading -= ahrs_euler[2];
if( heading < -C_PI )
heading += 2 * C_PI;
if( heading > C_PI )
@@ -741,9 +744,9 @@ ahrs_compass_update(
static void
euler2quat( void )
{
const real_t phi = euler[0] / 2.0;
const real_t theta = euler[1] / 2.0;
const real_t psi = euler[2] / 2.0;
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 shphi0 = sin( phi );
const real_t chphi0 = cos( phi );
@@ -764,8 +767,8 @@ euler2quat( void )
/*
* Initialize the AHRS state data and covariance matrix. The user
* should have filled in the pqr and accel vectors before calling this.
* They should also have set euler[2] from an untilted compass reading
* and the euler[0], euler[1] from the accel2roll / accel2pitch values.
* They should also have set ahrs_euler[2] from an untilted compass reading
* and the ahrs_euler[0], ahrs_euler[1] from the accel2roll / accel2pitch values.
*/
void
_ahrs_init(
@@ -774,16 +777,10 @@ _ahrs_init(
{
index_t i;
//accel : paparazzi mean accel buffer init
adc_buf_channel(IMU_ADC_ACCELX, &buf_accelX, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(IMU_ADC_ACCELY, &buf_accelY, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(IMU_ADC_ACCELZ, &buf_accelZ, DEFAULT_AV_NB_SAMPLE);
euler2quat();
compute_DCM();
euler[2] = untilt_compass( mag );
ahrs_euler[2] = untilt_compass( mag );
euler2quat();
compute_DCM();
@@ -852,52 +849,111 @@ accel2pitch()
}
/** - Here start the Paparazzi englued code
* - TODO : move this code to another file will be good
*/
uint8_t ahrs_state = AHRS_NOT_INITIALIZED;
static inline void
imu_init( void )
accel_init( void )
{
//accel : paparazzi mean accel buffer init
adc_buf_channel(IMU_ADC_ACCELX, &buf_accelX, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(IMU_ADC_ACCELY, &buf_accelY, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(IMU_ADC_ACCELZ, &buf_accelZ, DEFAULT_AV_NB_SAMPLE);
}
static inline void
gyro_init( void)
{
//gyro : paparazzi initialization
//nothing todo
}
static inline void
imu_calibration( uint8_t reset )
{
static uint32_t dec = IMU_INIT_EULER_DOT_NB_SAMPLES_MIN;
static int16_t pqr_min[3],pqr_max[3];
int8_t i;
//assuming that pqr sample is new is new....
//init-algo
if (reset)
dec = IMU_INIT_EULER_DOT_NB_SAMPLES_MIN;
if (dec == IMU_INIT_EULER_DOT_NB_SAMPLES_MIN)
{
for(i=0;i<3;i++)
pqr_min[i] = pqr_max[i] = pqr[i];//<--from_fbw.euler_dot[i];
return;
}
if (dec-- > 0)
{
//Saving min and max
for(i=0;i<3;i++)
{
//pqr[i] = from_fbw.euler_dot[i];//already done
pqr_min[i] = (pqr_min[i] < pqr[i])? pqr_min[i] : pqr[i];
pqr_max[i] = (pqr_max[i] > pqr[i])? pqr_max[i] : pqr[i];
}
//testing variance
for(i=0;i<3;i++)
{
if ((pqr_max[i]- pqr_min[i]) > IMU_INIT_EULER_DOT_VARIANCE_MAX)
{
//re-init algo
dec = IMU_INIT_EULER_DOT_NB_SAMPLES_MIN;
return;
}
}
const int16_t ax_m = buf_accelX.sum/buf_accelX.av_nb_sample;
const int16_t ay_m = buf_accelY.sum/buf_accelY.av_nb_sample;
const int16_t az_m = buf_accelZ.sum/buf_accelZ.av_nb_sample;
}
else
{
//entering in the end of calibration ;-)
while (timer_periodic() == 0)
continue;
//Geeting ax ay az from this adc buffer mean
accel[0] = IMU_ADC_ACCELX_SIGN (ax_m - IMU_ADC_ACCELX_ZERO);
accel[1] = IMU_ADC_ACCELY_SIGN (ay_m - IMU_ADC_ACCELY_ZERO);
accel[2] = IMU_ADC_ACCELZ_SIGN (az_m - IMU_ADC_ACCELZ_ZERO);
//Geeting p q r from uc1
while (!link_fbw_receive_complete || !link_fbw_receive_valid)
continue;
//Actually this valuers have to be Normalized by mcu1
//with IMU_ADC_ROLL_DOT_SIGN GYRO_SCALE...
//So we don't do it here ;-)
pqr[0] = from_fbw.euler_dot[0];
pqr[1] = from_fbw.euler_dot[1];
pqr[2] = from_fbw.euler_dot[2];
//we take the middle for pqr
//pqr is already normalized and scaled by the fbw mcu
for(i=0;i<3;i++)
pqr[i] = (pqr_min[i] + pqr_max[i])/2;
/** - Time of Accel Now
*
*/
const int16_t ax_m = buf_accelX.sum/buf_accelX.av_nb_sample;
const int16_t ay_m = buf_accelY.sum/buf_accelY.av_nb_sample;
const int16_t az_m = buf_accelZ.sum/buf_accelZ.av_nb_sample;
//Geeting ax ay az from this adc buffer mean
accel[0] = IMU_ADC_ACCELX_SIGN (ax_m - IMU_ADC_ACCELX_ZERO);
accel[1] = IMU_ADC_ACCELY_SIGN (ay_m - IMU_ADC_ACCELY_ZERO);
accel[2] = IMU_ADC_ACCELZ_SIGN (az_m - IMU_ADC_ACCELZ_ZERO);
//Enf of ahrs_init ;-)
#ifdef PNI_MAG
pni_poll();
pni_poll();
_ahrs_init( pni_values );
#else
_ahrs_init( NULL );
#endif //PNI_MAG
ahrs_state = AHRS_RUNNING;
}
}
static inline void
pqr_update( void )
{
//have we got new datas to eat, tha the question ???????????
if (link_fbw_receive_complete && link_fbw_receive_valid)
{
//Actually this valuers have to be Normalized by mcu1
//with IMU_ADC_ROLL_DOT_SIGN GYRO_SCALE...
//So we don't do it here ;-)
pqr[0] = from_fbw.euler_dot[0];
pqr[1] = from_fbw.euler_dot[1];
pqr[2] = from_fbw.euler_dot[2];
}
//do nothing here in paparazzi
//pqr is filled "asymchronously" by the exported function ahrs_save_pqr_from_fbw
}
@@ -914,17 +970,8 @@ roll_update( void )
accel[1] = IMU_ADC_ACCELY_SIGN (ay_m - IMU_ADC_ACCELY_ZERO);
accel[2] = IMU_ADC_ACCELZ_SIGN (az_m - IMU_ADC_ACCELZ_ZERO);
euler[0] = accel2roll();
ahrs_euler[0] = accel2roll();
/*
putc( 'A' );
put_int16_t( accel[0] );
put_int16_t( accel[1] );
put_int16_t( accel[2] );
put_float( euler[0] );
putnl();
*/
}
@@ -941,17 +988,7 @@ pitch_update( void )
accel[1] = IMU_ADC_ACCELY_SIGN (ay_m - IMU_ADC_ACCELY_ZERO);
accel[2] = IMU_ADC_ACCELZ_SIGN (az_m - IMU_ADC_ACCELZ_ZERO);
euler[1] = accel2pitch();
/*
putc( 'A' );
put_int16_t( accel[0] );
put_int16_t( accel[1] );
put_int16_t( accel[2] );
put_float( euler[1] );
putnl();
*/
ahrs_euler[1] = accel2pitch();
}
@@ -971,10 +1008,10 @@ compass_update( void )
put_int16_t( pni_values[i] );
putnl();
*/
euler[2] = untilt_compass( pni_values );
ahrs_euler[2] = untilt_compass( pni_values );
#else
//faking the Compass
euler[2] = 0;
ahrs_euler[2] = 0;
#endif //PNI_MAG
}
@@ -983,35 +1020,77 @@ compass_update( void )
//global Value
index_t i;
uint8_t step = 0;
uint16_t sample = 0;
//Exported Function to paparazzi
void ahrs_init()
void ahrs_save_pqr_from_fbw( void )
{
#ifdef PNI_MAG
pni_init();
#endif //PNI_MAG
//we take the gyro data from the spi data
//No transformation is needed for pqr
pqr[0] = from_fbw.euler_dot[0];
pqr[1] = from_fbw.euler_dot[1];
pqr[2] = from_fbw.euler_dot[2];
}
imu_init();
euler[0] = accel2roll();
euler[1] = accel2pitch();
#ifdef PNI_MAG
_ahrs_init( pni_values );
#else
_ahrs_init( NULL );
#endif //PNI_MAG
//ahrs_init have to be call in the mainllop init part
//ahrs have to be call during the mainllop
void ahrs_init(uint8_t do_calibration)
{
if (ahrs_state == AHRS_IMU_CALIBRATION)
return;
if(ahrs_state == AHRS_NOT_INITIALIZED)
{
#ifdef PNI_MAG
pni_init();
#endif //PNI_MAG
accel_init();
gyro_init();
}
if(do_calibration)
{
ahrs_state = AHRS_IMU_CALIBRATION;
imu_calibration(TRUE);
//end of ahrs_init is done in imu_calibration when calib is done
}
else
{
roll_update();
pitch_update();
//ahrs_euler[0] = accel2roll();
//ahrs_euler[1] = accel2pitch();
//Enf of ahrs_init ;-)
#ifdef PNI_MAG
pni_poll();
_ahrs_init( pni_values );
#else
_ahrs_init( NULL );
#endif //PNI_MAG
ahrs_state = AHRS_RUNNING;
}
}
void ahrs_update()
{
if( isnan( q0 ) )
static uint8_t step = 0;
if (ahrs_state != AHRS_RUNNING)
{
if (ahrs_state == AHRS_IMU_CALIBRATION)
imu_calibration(FALSE);
return;
}
if( isnan( q0 ) )
{
//puts( "\r\nFilter NaN! Reset!\r\n" );
reset();
@@ -1026,13 +1105,13 @@ void ahrs_update()
if( step == 0 )
{
roll_update();
ahrs_roll_update( euler[0] );
ahrs_roll_update( ahrs_euler[0] );
step = 1;
} else
if( step == 1 )
{
pitch_update();
ahrs_pitch_update( euler[1] );
ahrs_pitch_update( ahrs_euler[1] );
step = 2;
} else
if( step == 2 )
@@ -1042,11 +1121,11 @@ void ahrs_update()
compass_update();
#else
//Fucking Compass
euler[2] = 0;
ahrs_euler[2] = 0;
#endif //PNI_MAG
ahrs_compass_update( euler[2] );
ahrs_compass_update( ahrs_euler[2] );
step = 0;
}
}
}
#endif //SECTION_IMU_ANALOG
sw/airborne/autopilot/ahrs.h
+20 -4
View File
@@ -23,6 +23,8 @@ typedef uint8_t index_t;
* Q_dot = Wxq(pqr) * Q
* Bias_dot = 0
*/
extern real_t X[7];
extern real_t quat[4];
extern real_t q0;
@@ -38,15 +40,29 @@ extern real_t pqr[3];
extern int16_t accel[3];
/*
* The euler estimate will be updated less frequently than the
* quaternion, but some applications are ok with that.
*/
extern real_t euler[3];
extern real_t ahrs_euler[3];
//exported functions
extern void ahrs_init( void );
extern void ahrs_update( void );
//haere are the ahrs_state possible values
#define AHRS_NOT_INITIALIZED 0
#define AHRS_IMU_CALIBRATION 1
#define AHRS_RUNNING 2
//export internal state
extern uint8_t ahrs_state;
//export int function
extern void ahrs_save_pqr_from_fbw( void );//needed by paparazzi achitecture
//exported mainloop functions
extern void ahrs_init( uint8_t do_imu_calibration );//will restart the ahrs with an initailsation phase
extern void ahrs_update( void );//will update state
#endif
sw/airborne/autopilot/estimator.c
+15 -9
View File
@@ -114,6 +114,21 @@ void estimator_init( void ) {
#define EstimatorIrGainIsCorrect() (TRUE)
#ifdef SECTION_IMU_3DMG
#include "link_fbw.h"
void estimator_update_state_3DMG( void ) {
estimator_phi = from_fbw.euler[0];
estimator_psi = from_fbw.euler[1];
estimator_theta = from_fbw.euler[2];
}
#elif defined SECTION_IMU_ANALOG
#include "ahrs.h"
void estimator_update_state_ANALOG( void ) {
estimator_phi = ahrs_euler[0];
estimator_theta = ahrs_euler[1];
estimator_psi = ahrs_euler[2];
}
#else //NO_IMU
void estimator_update_state_infrared( void ) {
float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ?
estimator_rad_of_ir : ir_rad_of_ir;
@@ -147,15 +162,6 @@ void estimator_update_state_infrared( void ) {
estimator_theta = rad_of_ir * ir_pitch;
}
#ifdef SECTION_IMU_3DMG
#include "link_fbw.h"
void estimator_update_state_3DMG ( void ) {
estimator_phi = from_fbw.euler[0];
estimator_psi = from_fbw.euler[1];
estimator_theta = from_fbw.euler[2];
}
#endif
#define INIT_WEIGHT 100. /* The number of times the initial value has to be taken */
sw/airborne/autopilot/estimator.h
+6
View File
@@ -58,7 +58,13 @@ extern float estimator_hspeed_mod;
extern float estimator_hspeed_dir;
void estimator_init( void );
#ifdef SECTION_IMU_3DMG
void estimator_update_state_3DMG( void );
#elif defined SECTION_IMU_ANALOG
void estimator_update_state_ANALOG( void );
#else //NO_IMU
void estimator_update_state_infrared( void );
#endif
void estimator_update_state_gps( void );
void estimator_propagate_state( void );
sw/airborne/autopilot/main.c
+4 -3
View File
@@ -522,14 +522,15 @@ inline void periodic_task( void ) {
}
case 2:
ir_update();//Perhaps ir_update should move into the NO_IMU SECTION
#if defined SECTION_IMU_3DMG
#elif defined SECTION_IMU_ANALOG
//20Hz/3 it could be possible because can run standalone at 60Hz/3
//20Hz/3 it could be possible since it can run standalone at 60Hz/3
ahrs_update();
estimator_update_state_ANALOG();
#else //NO IMU
ir_update();
estimator_update_state_infrared();
#endif
roll_pitch_pid_run(); /* Set desired_aileron & desired_elevator */
sw/airborne/autopilot/mainloop.c
+21 -12
View File
@@ -69,6 +69,9 @@ int main( void ) {
nav_init();
ir_init();
estimator_init();
#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
uart0_init();
#endif //SECTION_IMU
/** - start interrupt task */
sei();
@@ -80,8 +83,12 @@ int main( void ) {
}
#ifdef SECTION_IMU_ANALOG
/** - ahrs init */
ahrs_init();
/** - ahrs init(do_calibration)
* - Warning if do_calibration is TRUE this will provide an asynchronous
* - calibration process, and it will take some calls to ahrs_update() to
* - end. So Don't take off before ahrs_state == AHRS_RUNNING
*/
ahrs_init(TRUE);
#endif //SECTION_IMU_ANALOG
/** - enter mainloop:
@@ -111,20 +118,22 @@ int main( void ) {
#ifdef SECTION_IMU_3DMG
DOWNLINK_SEND_IMU_3DMG(&from_fbw.euler_dot[0], &from_fbw.euler_dot[1], &from_fbw.euler_dot[2], &from_fbw.euler[0], &from_fbw.euler[1], &from_fbw.euler[2]);
estimator_update_state_3DMG();
#endif
#ifdef SECTION_IMU_ANALOG
/** - ahrs update */
//moved into periodic_task() at 20Hz case 2 : ahrs_update();
//Rajouter en init: uart0_init();
uart0_transmit('E');
#elif defined SECTION_IMU_ANALOG
/** -Saving now the pqr values from the fbw struct since
* -it's not safe always
*/
ahrs_save_pqr_from_fbw();
uart0_transmit('E');
uart0_transmit(' ');
uart0_print_hex16(euler[0]);
uart0_print_hex16(ahrs_euler[0]);
uart0_transmit(',');
uart0_print_hex16(euler[1]);
uart0_print_hex16(ahrs_euler[1]);
uart0_transmit(',');
uart0_print_hex16(euler[2]);
uart0_print_hex16(ahrs_euler[2]);
uart0_transmit('\t');
#endif
#if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
uart0_transmit('G');
uart0_transmit(' ');
uart0_print_hex16(from_fbw.euler_dot[0]);
sw/airborne/fly_by_wire/main.c
-5
View File
@@ -165,11 +165,6 @@ int main( void )
#ifdef SECTION_IMU_3DMG
if (_3dmg_data_ready) {
imu_update();
RED_LED_TOGGLE();
if (roll>0) {GREEN_LED_ON();}
else {GREEN_LED_OFF();}
if (pitch>0) {YELLOW_LED_ON();}
else {YELLOW_LED_OFF();}
}
#endif
if (time_since_last_ppm >= STALLED_TIME) {