From 5545851d20e4c91195db2edec7c7356bb29ace27 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 20 Feb 2013 15:24:15 +0100 Subject: [PATCH 001/125] [ahrs] int_cmpl_quat: scale the correction in update_accel with AHRS_CORRECT_FREQUENCY Allow tuning of the rate and bias gains. Scaling correction by AHRS_CORRECT_FREQUENCY should also give you roughly correct tuning for 100Hz fixedwings, see #240. --- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 4 ++ conf/conf.xml.example | 2 +- .../estimation/ahrs_int_cmpl_quat.xml | 4 +- conf/tests_conf.xml | 2 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 65 ++++++++++++++----- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 4 +- 6 files changed, 59 insertions(+), 22 deletions(-) diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 1adbab32b2..766f780376 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -18,6 +18,7 @@ + @@ -25,6 +26,9 @@ + + + diff --git a/conf/conf.xml.example b/conf/conf.xml.example index 55150448ae..be94bcea2a 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -41,7 +41,7 @@ radio="radios/cockpitSX.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" + settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml" gui_color="white" /> - + + + diff --git a/conf/tests_conf.xml b/conf/tests_conf.xml index 422bc6bece..42c11cb6ba 100644 --- a/conf/tests_conf.xml +++ b/conf/tests_conf.xml @@ -41,7 +41,7 @@ radio="radios/cockpitSX.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" + settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml" gui_color="white" /> Date: Wed, 27 Feb 2013 13:18:34 +0100 Subject: [PATCH 002/125] [ahrs] int_cmpl_quat: lower correction from accel update double gain scale to allow for a nicer range of the gains --- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 58 +++++++++---------- 1 file changed, 26 insertions(+), 32 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 1d53525c83..bcbf397e59 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -28,6 +28,8 @@ * */ +#include "generated/airframe.h" + #include "subsystems/ahrs/ahrs_int_cmpl_quat.h" #include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_int_utils.h" @@ -41,12 +43,7 @@ #include "math/pprz_trig_int.h" #include "math/pprz_algebra_int.h" -#include "generated/airframe.h" -//#include "../../test/pprz_algebra_print.h" - -static inline void ahrs_update_mag_full(void); -static inline void ahrs_update_mag_2d(void); #ifdef AHRS_MAG_UPDATE_YAW_ONLY #warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw." @@ -60,18 +57,20 @@ static inline void ahrs_update_mag_2d(void); #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #endif PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + #ifndef AHRS_CORRECT_FREQUENCY -#define AHRS_CORRECT_FREQUENCY PERIODIC_FREQUENCY +#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY #endif PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) #ifndef AHRS_RATE_CORRECTION_GAIN -#define AHRS_RATE_CORRECTION_GAIN 25 +#define AHRS_RATE_CORRECTION_GAIN 32 #endif #ifndef AHRS_BIAS_CORRECTION_GAIN -#define AHRS_BIAS_CORRECTION_GAIN 16 +#define AHRS_BIAS_CORRECTION_GAIN 32 #endif + #ifdef AHRS_UPDATE_FW_ESTIMATOR // remotely settable #ifndef INS_ROLL_NEUTRAL_DEFAULT @@ -87,6 +86,8 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; struct AhrsIntCmpl ahrs_impl; static inline void set_body_state_from_quat(void); +static inline void ahrs_update_mag_full(void); +static inline void ahrs_update_mag_2d(void); void ahrs_init(void) { @@ -123,6 +124,7 @@ void ahrs_init(void) { } + void ahrs_align(void) { #if USE_MAGNETOMETER @@ -146,12 +148,6 @@ void ahrs_align(void) { } - -/* - * - * - * - */ void ahrs_propagate(void) { /* unbias gyro */ @@ -181,8 +177,6 @@ void ahrs_propagate(void) { } - - void ahrs_update_accel(void) { // c2 = ltp z-axis in imu-frame @@ -206,11 +200,12 @@ void ahrs_update_accel(void) { // FIXME: check overflows ! #define COMPUTATION_FRAC 16 +#define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body); - INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-COMPUTATION_FRAC); + INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; @@ -226,7 +221,7 @@ void ahrs_update_accel(void) { INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); - int32_t inv_weight; + int32_t inv_weight = 1; if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration norm */ @@ -238,21 +233,21 @@ void ahrs_update_accel(void) { int32_t acc_norm; INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement); - const int32_t acc_norm_d = ABS(ACCEL_BFP_OF_REAL(9.81)-acc_norm); - inv_weight = Chop(50*acc_norm_d/ACCEL_BFP_OF_REAL(9.81), 1, 50); - } - else { - inv_weight = 1; + const int32_t g_int = ACCEL_BFP_OF_REAL(9.81); + const int32_t acc_norm_d = ABS(g_int - acc_norm); + inv_weight = Chop(50 * acc_norm_d / g_int, 1, 50); } /* Correct the drift by adding a rate correction. * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 - * rate_correction FRAC = RATE_FRAC = 12 + * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 + * + * Scale residual with FRAC difference, update_accel freq and the gain. + * To allow convenient range for the correction gain, multiply by two again... */ - /* scale gain with the update_accel freq and */ - uint32_t inv_rate_scale = AHRS_CORRECT_FREQUENCY * (4096 / ahrs_impl.rate_correction_gain); - Bound(inv_rate_scale, 2000, 500000); + uint32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / ahrs_impl.rate_correction_gain; + Bound(inv_rate_scale, 8192, 1024000); ahrs_impl.rate_correction.p += -residual.x / inv_rate_scale / inv_weight; ahrs_impl.rate_correction.q += -residual.y / inv_rate_scale / inv_weight; ahrs_impl.rate_correction.r += -residual.z / inv_rate_scale / inv_weight; @@ -266,23 +261,24 @@ void ahrs_update_accel(void) { * high_rez_bias = RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^24 = 2^16 * + * Scale residual with FRAC difference, update_accel freq and the gain. * Use bias_correction_gain scale of 1/(2^11) compared to rate_correction_gain. * 2^16 / 2^11 = 32 * To allow a suitable range of the freq and bias gain, multily by 32 separately. */ /* scale gain with the update_accel freq */ - uint32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / ahrs_impl.bias_correction_gain; + uint32_t inv_bias_gain = 2 * AHRS_CORRECT_FREQUENCY / ahrs_impl.bias_correction_gain; Bound(inv_bias_gain, 1, 1000) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) * 32 / (2*inv_weight); ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) * 32 / (2*inv_weight); ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) * 32 / (2*inv_weight); - /* */ INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); } + void ahrs_update_mag(void) { #if AHRS_MAG_UPDATE_ALL_AXES ahrs_update_mag_full(); @@ -491,7 +487,7 @@ void ahrs_realign_heading(int32_t heading) { /* Rotate angles and rates from imu to body frame and set state */ -__attribute__ ((always_inline)) static inline void set_body_state_from_quat(void) { +static inline void set_body_state_from_quat(void) { /* Compute LTP to BODY quaternion */ struct Int32Quat ltp_to_body_quat; INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); @@ -516,5 +512,3 @@ __attribute__ ((always_inline)) static inline void set_body_state_from_quat(void /* Set state */ stateSetBodyRates_i(&body_rate); } - - From fa8c283d63282e597180bfc3446663eeeeeb20ee Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 27 Feb 2013 18:42:49 +0100 Subject: [PATCH 003/125] [ahrs] int_cmpl_quat: some more adjustable correction gains not tested in flight yet... --- .../estimation/ahrs_int_cmpl_quat.xml | 6 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 115 +++++++++++------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 19 +-- 3 files changed, 89 insertions(+), 51 deletions(-) diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index c0f3bc971a..f6448e8478 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -5,8 +5,10 @@ - - + + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index bcbf397e59..21fbc2551b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -44,15 +44,15 @@ #include "math/pprz_algebra_int.h" - #ifdef AHRS_MAG_UPDATE_YAW_ONLY -#warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw." +#error "The define AHRS_MAG_UPDATE_YAW_ONLY doesn't exist anymore, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw." #endif #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING -#warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." +#warning "Using both magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." #endif + #ifndef AHRS_PROPAGATE_FREQUENCY #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #endif @@ -63,11 +63,25 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) #endif PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) -#ifndef AHRS_RATE_CORRECTION_GAIN -#define AHRS_RATE_CORRECTION_GAIN 32 +#ifndef AHRS_MAG_CORRECT_FREQUENCY +#define AHRS_MAG_CORRECT_FREQUENCY 50 #endif -#ifndef AHRS_BIAS_CORRECTION_GAIN -#define AHRS_BIAS_CORRECTION_GAIN 32 +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + +/* + * default gains for correcting attitude and bias from accel/mag + */ +#ifndef AHRS_ACCEL_ATTITUDE_GAIN +#define AHRS_ACCEL_ATTITUDE_GAIN 32 +#endif +#ifndef AHRS_ACCEL_GYROBIAS_GAIN +#define AHRS_ACCEL_GYROBIAS_GAIN 32 +#endif +#ifndef AHRS_MAG_ATTITUDE_GAIN +#define AHRS_MAG_ATTITUDE_GAIN 32 +#endif +#ifndef AHRS_MAG_GYROBIAS_GAIN +#define AHRS_MAG_GYROBIAS_GAIN 32 #endif @@ -83,7 +97,7 @@ float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; #endif -struct AhrsIntCmpl ahrs_impl; +struct AhrsIntCmplQuat ahrs_impl; static inline void set_body_state_from_quat(void); static inline void ahrs_update_mag_full(void); @@ -105,8 +119,10 @@ void ahrs_init(void) { INT_RATES_ZERO(ahrs_impl.high_rez_bias); /* set default correction gains */ - ahrs_impl.rate_correction_gain = AHRS_RATE_CORRECTION_GAIN; - ahrs_impl.bias_correction_gain = AHRS_BIAS_CORRECTION_GAIN; + ahrs_impl.accel_attitude_gain = AHRS_ACCEL_ATTITUDE_GAIN; + ahrs_impl.accel_gyrobias_gain = AHRS_ACCEL_GYROBIAS_GAIN; + ahrs_impl.mag_attitude_gain = AHRS_MAG_ATTITUDE_GAIN; + ahrs_impl.mag_gyrobias_gain = AHRS_MAG_GYROBIAS_GAIN; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; @@ -246,7 +262,7 @@ void ahrs_update_accel(void) { * Scale residual with FRAC difference, update_accel freq and the gain. * To allow convenient range for the correction gain, multiply by two again... */ - uint32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / ahrs_impl.rate_correction_gain; + uint32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_attitude_gain; Bound(inv_rate_scale, 8192, 1024000); ahrs_impl.rate_correction.p += -residual.x / inv_rate_scale / inv_weight; ahrs_impl.rate_correction.q += -residual.y / inv_rate_scale / inv_weight; @@ -262,12 +278,12 @@ void ahrs_update_accel(void) { * FRAC conversion: 2^40 / 2^24 = 2^16 * * Scale residual with FRAC difference, update_accel freq and the gain. - * Use bias_correction_gain scale of 1/(2^11) compared to rate_correction_gain. + * Use accel_gyrobias_gain scale of 1/(2^11) compared to accel_attitude_gain. * 2^16 / 2^11 = 32 * To allow a suitable range of the freq and bias gain, multily by 32 separately. */ /* scale gain with the update_accel freq */ - uint32_t inv_bias_gain = 2 * AHRS_CORRECT_FREQUENCY / ahrs_impl.bias_correction_gain; + uint32_t inv_bias_gain = 2 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_gyrobias_gain; Bound(inv_bias_gain, 1, 1000) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) * 32 / (2*inv_weight); ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) * 32 / (2*inv_weight); @@ -299,14 +315,28 @@ static inline void ahrs_update_mag_full(void) { struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); - ahrs_impl.rate_correction.p += residual.x/32/16; - ahrs_impl.rate_correction.q += residual.y/32/16; - ahrs_impl.rate_correction.r += residual.z/32/16; + /* residual FRAC: 2 * MAG_FRAC = 22 + * rate_correction FRAC: RATE_FRAC = 12 + * FRAC conversion: 2^12 / 2^22 = 1/1024 + * + * Scale with mag_attitude_gain * 2 for convenient range, + * and with mag frequency. + */ + ahrs_impl.rate_correction.p += residual.x * ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.q += residual.y * ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.r += residual.z * ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - - ahrs_impl.high_rez_bias.p -= residual.x/32*1024; - ahrs_impl.high_rez_bias.q -= residual.y/32*1024; - ahrs_impl.high_rez_bias.r -= residual.z/32*1024; + /* residual FRAC: 2* MAG_FRAC = 22 + * high_rez_bias FRAC: RATE_FRAC+28 = 40 + * FRAC conversion: 2^40 / 2^22 = 2^18 + * + * bias correction gain scale with 1/2^13 compared to attitude correction: + * 2^18 / 2^11 = 32 + */ + uint32_t bias_scale = 32 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; + ahrs_impl.high_rez_bias.p -= residual.x * bias_scale; + ahrs_impl.high_rez_bias.q -= residual.y * bias_scale; + ahrs_impl.high_rez_bias.r -= residual.z * bias_scale; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); @@ -322,6 +352,7 @@ static inline void ahrs_update_mag_2d(void) { struct Int32Vect3 measured_ltp; INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag); + /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */ struct Int32Vect3 residual_ltp = { 0, 0, @@ -330,30 +361,30 @@ static inline void ahrs_update_mag_2d(void) { struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); - // residual_ltp FRAC = 2 * MAG_FRAC = 22 - // rate_correction FRAC = RATE_FRAC = 12 - // 2^12 / 2^22 * 2.5 = 1/410 - - // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410; - // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410; - // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410; - - ahrs_impl.rate_correction.p += residual_imu.x/16; - ahrs_impl.rate_correction.q += residual_imu.y/16; - ahrs_impl.rate_correction.r += residual_imu.z/16; + /* residual_imu FRAC = residual_ltp FRAC = 17 + * rate_correction FRAC: RATE_FRAC = 12 + * FRAC conversion: 2^12 / 2^17 = 1/32 + * + * Scale with mag_attitude_gain * 2 for convenient range, + * and with mag frequency. + */ + ahrs_impl.rate_correction.p += residual_imu.x * ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.q += residual_imu.y * ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.r += residual_imu.z * ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - // residual_ltp FRAC = 2 * MAG_FRAC = 22 - // high_rez_bias = RATE_FRAC+28 = 40 - // 2^40 / 2^22 * 2.5e-3 = 655 - - // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655; - // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655; - // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655; - - ahrs_impl.high_rez_bias.p -= residual_imu.x*1024; - ahrs_impl.high_rez_bias.q -= residual_imu.y*1024; - ahrs_impl.high_rez_bias.r -= residual_imu.z*1024; + /* residual_imu FRAC = residual_ltp FRAC = 17 + * high_rez_bias FRAC: RATE_FRAC+28 = 40 + * FRAC conversion: 2^40 / 2^17 = 2^23 + * + * bias correction with 1/2^11 compared to attitude correction: + * 2^23 / 2^11 = 4096 + * also divide mag_gyrobias_gain by 4 for convenience range + */ + uint32_t bias_scale = 1024 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; + ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_scale; + ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_scale; + ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_scale; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 28afe8c11e..4b98063687 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -28,14 +28,17 @@ * */ -#ifndef AHRS_INT_CMPL_H -#define AHRS_INT_CMPL_H +#ifndef AHRS_INT_CMPL_QUAT_H +#define AHRS_INT_CMPL_QUAT_H #include "subsystems/ahrs.h" #include "std.h" #include "math/pprz_algebra_int.h" -struct AhrsIntCmpl { +/** + * Ahrs implementation specifc values + */ +struct AhrsIntCmplQuat { struct Int32Rates gyro_bias; struct Int32Rates imu_rate; struct Int32Rates rate_correction; @@ -44,8 +47,10 @@ struct AhrsIntCmpl { struct Int32Quat ltp_to_imu_quat; struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry struct Int32Vect3 mag_h; - uint32_t rate_correction_gain; - uint32_t bias_correction_gain; + uint32_t accel_attitude_gain; ///< gain for correcting the attitude from accels (pseudo-gravity measurement) + uint32_t accel_gyrobias_gain; ///< gain for correcting the gyro-bias from accels (pseudo-gravity measurement) + uint32_t mag_attitude_gain; ///< gain for correcting the attitude (heading) from magnetometer + uint32_t mag_gyrobias_gain; ///< gain for correcting the gyro bias from magnetometer int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; bool_t correct_gravity; @@ -53,7 +58,7 @@ struct AhrsIntCmpl { bool_t heading_aligned; }; -extern struct AhrsIntCmpl ahrs_impl; +extern struct AhrsIntCmplQuat ahrs_impl; /** Update yaw based on a heading measurement. @@ -75,4 +80,4 @@ extern float ins_pitch_neutral; #endif -#endif /* AHRS_INT_CMPL_H */ +#endif /* AHRS_INT_CMPL_QUAT_H */ From 3375ffe4a9c9b12ab7efc795fa11dd21c926d339 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 5 Mar 2013 00:08:26 +0100 Subject: [PATCH 004/125] [ahrs] int_cmpl_quat: use signed integers for correction scaling beware of the usual arithmetic conversions!! --- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 21fbc2551b..dc60f1683e 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -262,11 +262,11 @@ void ahrs_update_accel(void) { * Scale residual with FRAC difference, update_accel freq and the gain. * To allow convenient range for the correction gain, multiply by two again... */ - uint32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_attitude_gain; + int32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_attitude_gain; Bound(inv_rate_scale, 8192, 1024000); - ahrs_impl.rate_correction.p += -residual.x / inv_rate_scale / inv_weight; - ahrs_impl.rate_correction.q += -residual.y / inv_rate_scale / inv_weight; - ahrs_impl.rate_correction.r += -residual.z / inv_rate_scale / inv_weight; + ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale / inv_weight; + ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale / inv_weight; + ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale / inv_weight; /* Correct the gyro bias. @@ -283,7 +283,7 @@ void ahrs_update_accel(void) { * To allow a suitable range of the freq and bias gain, multily by 32 separately. */ /* scale gain with the update_accel freq */ - uint32_t inv_bias_gain = 2 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_gyrobias_gain; + int32_t inv_bias_gain = 2 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_gyrobias_gain; Bound(inv_bias_gain, 1, 1000) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) * 32 / (2*inv_weight); ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) * 32 / (2*inv_weight); @@ -322,9 +322,9 @@ static inline void ahrs_update_mag_full(void) { * Scale with mag_attitude_gain * 2 for convenient range, * and with mag frequency. */ - ahrs_impl.rate_correction.p += residual.x * ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.q += residual.y * ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.r += residual.z * ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.p += residual.x * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.q += residual.y * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.r += residual.z * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); /* residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 @@ -333,7 +333,7 @@ static inline void ahrs_update_mag_full(void) { * bias correction gain scale with 1/2^13 compared to attitude correction: * 2^18 / 2^11 = 32 */ - uint32_t bias_scale = 32 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; + int32_t bias_scale = 32 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.high_rez_bias.p -= residual.x * bias_scale; ahrs_impl.high_rez_bias.q -= residual.y * bias_scale; ahrs_impl.high_rez_bias.r -= residual.z * bias_scale; @@ -368,9 +368,9 @@ static inline void ahrs_update_mag_2d(void) { * Scale with mag_attitude_gain * 2 for convenient range, * and with mag frequency. */ - ahrs_impl.rate_correction.p += residual_imu.x * ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.q += residual_imu.y * ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.r += residual_imu.z * ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.p += residual_imu.x * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.q += residual_imu.y * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + ahrs_impl.rate_correction.r += residual_imu.z * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); /* residual_imu FRAC = residual_ltp FRAC = 17 @@ -381,7 +381,7 @@ static inline void ahrs_update_mag_2d(void) { * 2^23 / 2^11 = 4096 * also divide mag_gyrobias_gain by 4 for convenience range */ - uint32_t bias_scale = 1024 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; + int32_t bias_scale = 1024 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_scale; ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_scale; ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_scale; From 6f33b8364c5e933176e46245bd4941646c49d527 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 5 Mar 2013 13:56:01 +0100 Subject: [PATCH 005/125] [imu] default settings for navgo imu --- conf/airframes/ENAC/quadrotor/hen1.xml | 1 - .../subsystems/shared/imu_navgo.makefile | 7 ++++++ sw/airborne/boards/navgo/imu_navgo.c | 23 +++++++++++++++++-- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 2fe0d7e579..9dfa2c74b8 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -113,7 +113,6 @@
- diff --git a/conf/firmwares/subsystems/shared/imu_navgo.makefile b/conf/firmwares/subsystems/shared/imu_navgo.makefile index 3ab4757bfe..15ca4c4b11 100644 --- a/conf/firmwares/subsystems/shared/imu_navgo.makefile +++ b/conf/firmwares/subsystems/shared/imu_navgo.makefile @@ -14,6 +14,13 @@ IMU_NAVGO_SRCS += peripherals/itg3200.c IMU_NAVGO_SRCS += peripherals/adxl345_i2c.c IMU_NAVGO_SRCS += peripherals/hmc58xx.c +# with default NAVGO_GYRO_SMPLRT_DIV (gyro output 500Hz) +# the AHRS_PROPAGATE_FREQUENCY needs to be adjusted accordingly +AHRS_PROPAGATE_FREQUENCY ?= 500 +AHRS_CORRECT_FREQUENCY ?= 500 +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) + ap.CFLAGS += $(IMU_NAVGO_CFLAGS) ap.srcs += $(IMU_NAVGO_SRCS) diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c index 8eba9bd9b3..00582cfb5e 100644 --- a/sw/airborne/boards/navgo/imu_navgo.c +++ b/sw/airborne/boards/navgo/imu_navgo.c @@ -49,8 +49,19 @@ #endif PRINT_CONFIG_VAR(NAVGO_ACCEL_RATE) +/* default gyro internal lowpass frequency and sample rate divider */ +#if !defined NAVGO_GYRO_LOWPASS && !defined NAVGO_GYRO_SMPLRT_DIV +#define NAVGO_GYRO_LOWPASS ITG3200_DLPF_10HZ +#define NAVGO_GYRO_SMPLRT_DIV 1 +PRINT_CONFIG_MSG("Gyro output rate is 500Hz") +#endif +PRINT_CONFIG_VAR(NAVGO_GYRO_LOWPASS) +PRINT_CONFIG_VAR(NAVGO_GYRO_SMPLRT_DIV) + +#if NAVGO_USE_MEDIAN_FILTER #include "filters/median_filter.h" struct MedianFilter3Int median_gyro, median_accel, median_mag; +#endif struct ImuNavgo imu_navgo; @@ -60,8 +71,8 @@ void imu_impl_init(void) // ITG3200 itg3200_init(&imu_navgo.itg, &(IMU_NAVGO_I2C_DEV), ITG3200_ADDR_ALT); // change the default configuration - imu_navgo.itg.config.smplrt_div = 1; // 500Hz sample rate since internal is 1kHz - imu_navgo.itg.config.dlpf_cfg = ITG3200_DLPF_10HZ; + imu_navgo.itg.config.smplrt_div = NAVGO_GYRO_SMPLRT_DIV; // 500Hz sample rate since internal is 1kHz + imu_navgo.itg.config.dlpf_cfg = NAVGO_GYRO_LOWPASS; ///////////////////////////////////////////////////////////////////// // ADXL345 @@ -73,10 +84,12 @@ void imu_impl_init(void) // HMC58XX hmc58xx_init(&imu_navgo.hmc, &(IMU_NAVGO_I2C_DEV), HMC58XX_ADDR); +#if NAVGO_USE_MEDIAN_FILTER // Init median filters InitMedianFilterRatesInt(median_gyro); InitMedianFilterVect3Int(median_accel); InitMedianFilterVect3Int(median_mag); +#endif imu_navgo.gyr_valid = FALSE; imu_navgo.acc_valid = FALSE; @@ -115,7 +128,9 @@ void imu_navgo_event( void ) itg3200_event(&imu_navgo.itg); if (imu_navgo.itg.data_available) { RATES_ASSIGN(imu.gyro_unscaled, -imu_navgo.itg.data.rates.q, imu_navgo.itg.data.rates.p, imu_navgo.itg.data.rates.r); +#if NAVGO_USE_MEDIAN_FILTER UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled); +#endif imu_navgo.itg.data_available = FALSE; imu_navgo.gyr_valid = TRUE; } @@ -124,7 +139,9 @@ void imu_navgo_event( void ) adxl345_i2c_event(&imu_navgo.adxl); if (imu_navgo.adxl.data_available) { VECT3_ASSIGN(imu.accel_unscaled, imu_navgo.adxl.data.vect.y, -imu_navgo.adxl.data.vect.x, imu_navgo.adxl.data.vect.z); +#if NAVGO_USE_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); +#endif imu_navgo.adxl.data_available = FALSE; imu_navgo.acc_valid = TRUE; } @@ -133,7 +150,9 @@ void imu_navgo_event( void ) hmc58xx_event(&imu_navgo.hmc); if (imu_navgo.hmc.data_available) { VECT3_COPY(imu.mag_unscaled, imu_navgo.hmc.data.vect); +#if NAVGO_USE_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled); +#endif imu_navgo.hmc.data_available = FALSE; imu_navgo.mag_valid = TRUE; } From b26a15799aaa667f7b4b9f9802ed4169e5cb11fc Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 5 Mar 2013 15:54:23 +0100 Subject: [PATCH 006/125] [ahrs] int_cmpl_quat: always run FIR on pseudo_gravity_measurement so it doesn't misbehave if you switch the gravity heuristic off in between --- sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index dc60f1683e..a10306095c 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -237,16 +237,15 @@ void ahrs_update_accel(void) { INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); + /* FIR filtered pseudo_gravity_measurement */ + static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7); + VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); + VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8); + int32_t inv_weight = 1; if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration norm */ - - /* FIR filtered pseudo_gravity_measurement */ - static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; - VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7); - VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); - VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8); - int32_t acc_norm; INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement); const int32_t g_int = ACCEL_BFP_OF_REAL(9.81); From 5fdbf28abbb9fbce58ef8aff01bb2b28c60c10d5 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 5 Mar 2013 17:46:44 +0100 Subject: [PATCH 007/125] [ahrs] int_cmpl_quat: use linear instead of hyperbolic weight in gravity heuristic --- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 48 +++++++++++-------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index a10306095c..19b9ef6c1b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -238,19 +238,28 @@ void ahrs_update_accel(void) { /* FIR filtered pseudo_gravity_measurement */ + #define FIR_FILTER_SIZE 8 static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; - VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, 7); + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); - VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, 8); + VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - int32_t inv_weight = 1; + + float weight = 1.0; if (ahrs_impl.use_gravity_heuristic) { - /* heuristic on acceleration norm */ - int32_t acc_norm; - INT32_VECT3_NORM(acc_norm, filtered_gravity_measurement); - const int32_t g_int = ACCEL_BFP_OF_REAL(9.81); - const int32_t acc_norm_d = ABS(g_int - acc_norm); - inv_weight = Chop(50 * acc_norm_d / g_int, 1, 50); + /* heuristic on acceleration (gravity estimate) norm */ + + /* Factor how strongly to change the weight. + * e.g.: 3 means that at 1/(2*3) g = 1.6m/s^2 + * the weight will half and zero at 3.27m/s^2 + */ + #define WEIGHT_FACTOR 3 + + struct FloatVect3 g_meas_f; + ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); + const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; + weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + Bound(weight, 0.02, 1.0); } /* Correct the drift by adding a rate correction. @@ -261,16 +270,14 @@ void ahrs_update_accel(void) { * Scale residual with FRAC difference, update_accel freq and the gain. * To allow convenient range for the correction gain, multiply by two again... */ - int32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_attitude_gain; + int32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_attitude_gain); Bound(inv_rate_scale, 8192, 1024000); - ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale / inv_weight; - ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale / inv_weight; - ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale / inv_weight; + ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; + ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; + ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale; /* Correct the gyro bias. - * Also make mitigating effect of gravity heuristic weight - * twice as large on bias as on rate. * * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 @@ -279,14 +286,13 @@ void ahrs_update_accel(void) { * Scale residual with FRAC difference, update_accel freq and the gain. * Use accel_gyrobias_gain scale of 1/(2^11) compared to accel_attitude_gain. * 2^16 / 2^11 = 32 - * To allow a suitable range of the freq and bias gain, multily by 32 separately. + * To allow a suitable range of the freq and bias gain, multily by 32 (<< 5) separately. */ - /* scale gain with the update_accel freq */ - int32_t inv_bias_gain = 2 * AHRS_CORRECT_FREQUENCY / ahrs_impl.accel_gyrobias_gain; + int32_t inv_bias_gain = 3 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_gyrobias_gain); Bound(inv_bias_gain, 1, 1000) - ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) * 32 / (2*inv_weight); - ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) * 32 / (2*inv_weight); - ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) * 32 / (2*inv_weight); + ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; + ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; + ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); From 970c3037ff55e7fd7faceba74f0ceadb263487ee Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 5 Mar 2013 19:58:49 +0100 Subject: [PATCH 008/125] [ahrs] int_cmpl_quat: turn on gravity heuristic by default --- sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 19b9ef6c1b..6f816a4878 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -85,6 +85,12 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #endif +/** by default use the gravity heursitic to reduce gain */ +#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC +#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE +#endif + + #ifdef AHRS_UPDATE_FW_ESTIMATOR // remotely settable #ifndef INS_ROLL_NEUTRAL_DEFAULT @@ -250,8 +256,8 @@ void ahrs_update_accel(void) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. - * e.g.: 3 means that at 1/(2*3) g = 1.6m/s^2 - * the weight will half and zero at 3.27m/s^2 + * e.g. for WEIGHT_FACTOR 3: + * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ #define WEIGHT_FACTOR 3 From f7dc61a7641d57345a716ec0571a6644577cb8e7 Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Fri, 8 Mar 2013 17:39:47 +0100 Subject: [PATCH 009/125] scaling rate with weight, scaling bias with weight*weight. accel filter parameters are now cut-off frequency and damping instead of gains. Defaults values set to 100 s response time. --- .../estimation/ahrs_int_cmpl_quat.xml | 4 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 43 ++++++++++--------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 4 +- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index f6448e8478..3f618e6882 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -5,8 +5,8 @@ - - + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 6f816a4878..6a6ad706b8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -71,11 +71,11 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) /* * default gains for correcting attitude and bias from accel/mag */ -#ifndef AHRS_ACCEL_ATTITUDE_GAIN -#define AHRS_ACCEL_ATTITUDE_GAIN 32 +#ifndef AHRS_ACCEL_INV_OMEGA +#define AHRS_ACCEL_INV_OMEGA 32 #endif -#ifndef AHRS_ACCEL_GYROBIAS_GAIN -#define AHRS_ACCEL_GYROBIAS_GAIN 32 +#ifndef AHRS_ACCEL_INV_ZETA +#define AHRS_ACCEL_INV_ZETA 64 #endif #ifndef AHRS_MAG_ATTITUDE_GAIN #define AHRS_MAG_ATTITUDE_GAIN 32 @@ -125,8 +125,8 @@ void ahrs_init(void) { INT_RATES_ZERO(ahrs_impl.high_rez_bias); /* set default correction gains */ - ahrs_impl.accel_attitude_gain = AHRS_ACCEL_ATTITUDE_GAIN; - ahrs_impl.accel_gyrobias_gain = AHRS_ACCEL_GYROBIAS_GAIN; + ahrs_impl.accel_inv_omega = AHRS_ACCEL_INV_OMEGA; + ahrs_impl.accel_inv_zeta = AHRS_ACCEL_INV_ZETA; ahrs_impl.mag_attitude_gain = AHRS_MAG_ATTITUDE_GAIN; ahrs_impl.mag_gyrobias_gain = AHRS_MAG_GYROBIAS_GAIN; @@ -255,7 +255,7 @@ void ahrs_update_accel(void) { if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration (gravity estimate) norm */ - /* Factor how strongly to change the weight. + /* Factor how strongly to change the weight. * e.g. for WEIGHT_FACTOR 3: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ @@ -265,37 +265,38 @@ void ahrs_update_accel(void) { ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); - Bound(weight, 0.02, 1.0); + Bound(weight, 0.15, 1.0); } - /* Correct the drift by adding a rate correction. + /* Complementary filter proportionnal gain. + * Kp = 1/(2 * zeta * omega) * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 * - * Scale residual with FRAC difference, update_accel freq and the gain. - * To allow convenient range for the correction gain, multiply by two again... + * Kp = 1 / inv_rate_scale * 4096 = 1/(2*45*32*500/32) * 4096 = 0.091 + * zeta = Kp/2/SQRT(Ki analog) = 0.73 */ - int32_t inv_rate_scale = 2 * 4096 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_attitude_gain); - Bound(inv_rate_scale, 8192, 1024000); + int32_t inv_rate_scale = 2 * ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_zeta * AHRS_CORRECT_FREQUENCY / (32 * weight); + Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale; - /* Correct the gyro bias. - * + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = 1/(omega*omega) * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^24 = 2^16 * - * Scale residual with FRAC difference, update_accel freq and the gain. - * Use accel_gyrobias_gain scale of 1/(2^11) compared to accel_attitude_gain. - * 2^16 / 2^11 = 32 - * To allow a suitable range of the freq and bias gain, multily by 32 (<< 5) separately. + * Ki = 1/2^16 / inv_bias_gain * 32 = 1/2^16 / (500*32*32/8192) * 32 = 7.8e-6 + * Ki analog = 7.8e-6 * 500 = 3.9e-3 + * omega = SQRT(Ki analog)/(2*pi) = 0.01 Hz - T = 100 s. */ - int32_t inv_bias_gain = 3 * AHRS_CORRECT_FREQUENCY / (weight * ahrs_impl.accel_gyrobias_gain); - Bound(inv_bias_gain, 1, 1000) + int32_t inv_bias_gain = ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_omega * AHRS_CORRECT_FREQUENCY / (weight * weight * 8192); + Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 4b98063687..555e418dc7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -47,8 +47,8 @@ struct AhrsIntCmplQuat { struct Int32Quat ltp_to_imu_quat; struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry struct Int32Vect3 mag_h; - uint32_t accel_attitude_gain; ///< gain for correcting the attitude from accels (pseudo-gravity measurement) - uint32_t accel_gyrobias_gain; ///< gain for correcting the gyro-bias from accels (pseudo-gravity measurement) + uint32_t accel_inv_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) + uint32_t accel_inv_zeta; ///< filter damping correcting the gyro-bias from accels (pseudo-gravity measurement) uint32_t mag_attitude_gain; ///< gain for correcting the attitude (heading) from magnetometer uint32_t mag_gyrobias_gain; ///< gain for correcting the gyro bias from magnetometer int32_t ltp_vel_norm; From 28eaceb2a0bb9ac36e26f8fffefadc249f2e184d Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Thu, 28 Mar 2013 16:17:57 +0100 Subject: [PATCH 010/125] complementary filter accel and mag cutt-off frequency and damping in physical units, accel weight sent to telemetry --- conf/messages.xml | 1 + .../estimation/ahrs_int_cmpl_quat.xml | 8 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 1 + .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 167 +++++++++++------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 8 +- 5 files changed, 112 insertions(+), 73 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 733ca40e93..a2d34073b4 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1295,6 +1295,7 @@ + diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 3f618e6882..9856758ec1 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -5,10 +5,10 @@ - - - - + + + + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 4e35e77dbc..9f17e3ccac 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -454,6 +454,7 @@ #define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ + &ahrs_impl.weight, \ &ahrs_impl.ltp_to_imu_quat.qi, \ &ahrs_impl.ltp_to_imu_quat.qx, \ &ahrs_impl.ltp_to_imu_quat.qy, \ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 6a6ad706b8..253e37d398 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2012 The Paparazzi Team + * Copyright (C) 2008-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -71,19 +71,19 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) /* * default gains for correcting attitude and bias from accel/mag */ -#ifndef AHRS_ACCEL_INV_OMEGA -#define AHRS_ACCEL_INV_OMEGA 32 +#ifndef AHRS_ACCEL_OMEGA +#define AHRS_ACCEL_OMEGA 0.063 #endif -#ifndef AHRS_ACCEL_INV_ZETA -#define AHRS_ACCEL_INV_ZETA 64 -#endif -#ifndef AHRS_MAG_ATTITUDE_GAIN -#define AHRS_MAG_ATTITUDE_GAIN 32 -#endif -#ifndef AHRS_MAG_GYROBIAS_GAIN -#define AHRS_MAG_GYROBIAS_GAIN 32 +#ifndef AHRS_ACCEL_ZETA +#define AHRS_ACCEL_ZETA 1. #endif +#ifndef AHRS_MAG_OMEGA +#define AHRS_MAG_OMEGA 0.063 +#endif +#ifndef AHRS_MAG_ZETA +#define AHRS_MAG_ZETA 1. +#endif /** by default use the gravity heursitic to reduce gain */ #ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC @@ -124,11 +124,14 @@ void ahrs_init(void) { INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); - /* set default correction gains */ - ahrs_impl.accel_inv_omega = AHRS_ACCEL_INV_OMEGA; - ahrs_impl.accel_inv_zeta = AHRS_ACCEL_INV_ZETA; - ahrs_impl.mag_attitude_gain = AHRS_MAG_ATTITUDE_GAIN; - ahrs_impl.mag_gyrobias_gain = AHRS_MAG_GYROBIAS_GAIN; + /* set default filter cut-off frequency and damping */ + ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_impl.mag_omega = AHRS_MAG_OMEGA; + ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + + /* set default gravity heuristic */ + ahrs_impl.weight = 1.0; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; @@ -163,7 +166,7 @@ void ahrs_align(void) { /* Use low passed gyro value as initial bias */ RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); - RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); + RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); ahrs.status = AHRS_RUNNING; @@ -233,7 +236,7 @@ void ahrs_update_accel(void) { struct Int32Vect3 acc_c_imu; INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); - /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ + /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu.accel); @@ -251,10 +254,9 @@ void ahrs_update_accel(void) { VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - float weight = 1.0; + if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration (gravity estimate) norm */ - /* Factor how strongly to change the weight. * e.g. for WEIGHT_FACTOR 3: * <0.66G = 0, 1G = 1.0, >1.33G = 0 @@ -264,20 +266,23 @@ void ahrs_update_accel(void) { struct FloatVect3 g_meas_f; ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; - weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); - Bound(weight, 0.15, 1.0); + ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + Bound(ahrs_impl.weight, 0.15, 1.0); } /* Complementary filter proportionnal gain. - * Kp = 1/(2 * zeta * omega) + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 - * FRAC conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 + * FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 + * Feedback_FRAC: 4 * - * Kp = 1 / inv_rate_scale * 4096 = 1/(2*45*32*500/32) * 4096 = 0.091 - * zeta = Kp/2/SQRT(Ki analog) = 0.73 + * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC + * inv_rate_scale= 4096 * 4 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 8192 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 2 * ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_zeta * AHRS_CORRECT_FREQUENCY / (32 * weight); + + int32_t inv_rate_scale = 8192 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -286,22 +291,22 @@ void ahrs_update_accel(void) { /* Complementary filter integral gain * Correct the gyro bias. - * Ki = 1/(omega*omega) + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 - * FRAC conversion: 2^40 / 2^24 = 2^16 - * - * Ki = 1/2^16 / inv_bias_gain * 32 = 1/2^16 / (500*32*32/8192) * 32 = 7.8e-6 - * Ki analog = 7.8e-6 * 500 = 3.9e-3 - * omega = SQRT(Ki analog)/(2*pi) = 0.01 Hz - T = 100 s. + * FRAC_conversion: 2^40 / 2^24 = 2^16 + * Feedback_FRAC: 4 + * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC + * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 4 + * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512) */ - int32_t inv_bias_gain = ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_omega * AHRS_CORRECT_FREQUENCY / (weight * weight * 8192); + + int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 512); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; - INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); } @@ -327,28 +332,41 @@ static inline void ahrs_update_mag_full(void) { struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); - /* residual FRAC: 2 * MAG_FRAC = 22 + /* Complementary filter proportionnal gain. + * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 + * Feedback_FRAC: 4 * - * Scale with mag_attitude_gain * 2 for convenient range, - * and with mag frequency. + * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 1024 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - ahrs_impl.rate_correction.p += residual.x * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.q += residual.y * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.r += residual.z * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); + + int32_t inv_rate_gain = 2048 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; - /* residual FRAC: 2* MAG_FRAC = 22 + ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; + ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; + ahrs_impl.rate_correction.r += residual.z / inv_rate_gain; + +/* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 - * - * bias correction gain scale with 1/2^13 compared to attitude correction: - * 2^18 / 2^11 = 32 - */ - int32_t bias_scale = 32 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.high_rez_bias.p -= residual.x * bias_scale; - ahrs_impl.high_rez_bias.q -= residual.y * bias_scale; - ahrs_impl.high_rez_bias.r -= residual.z * bias_scale; + * Feedback_FRAC: 4 + * + * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^16 + */ + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 65536; + ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; + ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; + ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); @@ -372,31 +390,44 @@ static inline void ahrs_update_mag_2d(void) { struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); - - /* residual_imu FRAC = residual_ltp FRAC = 17 + + /* Complementary filter proportionnal gain. + * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY/ AHRS_MAG_CORRECT_FREQUENCY + * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 - * - * Scale with mag_attitude_gain * 2 for convenient range, - * and with mag frequency. + * Feedback_FRAC: 4 + * + * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY + * / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * */ - ahrs_impl.rate_correction.p += residual_imu.x * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.q += residual_imu.y * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.r += residual_imu.z * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + + int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + + ahrs_impl.rate_correction.p += residual_imu.x / inv_rate_gain; + ahrs_impl.rate_correction.q += residual_imu.y / inv_rate_gain; + ahrs_impl.rate_correction.r += residual_imu.z / inv_rate_gain; - /* residual_imu FRAC = residual_ltp FRAC = 17 + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 - * - * bias correction with 1/2^11 compared to attitude correction: - * 2^23 / 2^11 = 4096 - * also divide mag_gyrobias_gain by 4 for convenience range - */ - int32_t bias_scale = 1024 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_scale; - ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_scale; - ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_scale; + * Feedback_FRAC: 4 + * + * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 + */ + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; + ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_gain; + ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain; + ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 290aa1afe0..64f2c0f2e7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -45,7 +45,13 @@ struct AhrsIntCmplQuat { struct Int64Quat high_rez_quat; struct Int64Rates high_rez_bias; struct Int32Quat ltp_to_imu_quat; - struct Int32Vect3 mag_h; + struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry + struct Int32Vect3 mag_h; + float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) + float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) + float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer + float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + float weight; int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; bool_t correct_gravity; From f39cc5c25b7e58c7cc939ad129fbfce14aafa770 Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Thu, 28 Mar 2013 16:17:57 +0100 Subject: [PATCH 011/125] [ahrs] int_cmpl_quat: physical units - accel and mag cutt-off frequency and damping in physical units - accel weight sent to telemetry --- conf/messages.xml | 1 + .../estimation/ahrs_int_cmpl_quat.xml | 8 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 1 + .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 157 +++++++++++------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 9 +- 5 files changed, 105 insertions(+), 71 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index b6ce932e59..473b25cecd 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1295,6 +1295,7 @@ + diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 3f618e6882..9856758ec1 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -5,10 +5,10 @@ - - - - + + + + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 208aae300c..4c88be0ad0 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -454,6 +454,7 @@ #define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ + &ahrs_impl.weight, \ &ahrs_impl.ltp_to_imu_quat.qi, \ &ahrs_impl.ltp_to_imu_quat.qx, \ &ahrs_impl.ltp_to_imu_quat.qy, \ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 6a6ad706b8..1947911e42 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2012 The Paparazzi Team + * Copyright (C) 2008-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -71,19 +71,19 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) /* * default gains for correcting attitude and bias from accel/mag */ -#ifndef AHRS_ACCEL_INV_OMEGA -#define AHRS_ACCEL_INV_OMEGA 32 +#ifndef AHRS_ACCEL_OMEGA +#define AHRS_ACCEL_OMEGA 0.063 #endif -#ifndef AHRS_ACCEL_INV_ZETA -#define AHRS_ACCEL_INV_ZETA 64 -#endif -#ifndef AHRS_MAG_ATTITUDE_GAIN -#define AHRS_MAG_ATTITUDE_GAIN 32 -#endif -#ifndef AHRS_MAG_GYROBIAS_GAIN -#define AHRS_MAG_GYROBIAS_GAIN 32 +#ifndef AHRS_ACCEL_ZETA +#define AHRS_ACCEL_ZETA 1. #endif +#ifndef AHRS_MAG_OMEGA +#define AHRS_MAG_OMEGA 0.063 +#endif +#ifndef AHRS_MAG_ZETA +#define AHRS_MAG_ZETA 1. +#endif /** by default use the gravity heursitic to reduce gain */ #ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC @@ -124,11 +124,14 @@ void ahrs_init(void) { INT_RATES_ZERO(ahrs_impl.rate_correction); INT_RATES_ZERO(ahrs_impl.high_rez_bias); - /* set default correction gains */ - ahrs_impl.accel_inv_omega = AHRS_ACCEL_INV_OMEGA; - ahrs_impl.accel_inv_zeta = AHRS_ACCEL_INV_ZETA; - ahrs_impl.mag_attitude_gain = AHRS_MAG_ATTITUDE_GAIN; - ahrs_impl.mag_gyrobias_gain = AHRS_MAG_GYROBIAS_GAIN; + /* set default filter cut-off frequency and damping */ + ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_impl.mag_omega = AHRS_MAG_OMEGA; + ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + + /* set default gravity heuristic */ + ahrs_impl.weight = 1.0; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; @@ -233,7 +236,7 @@ void ahrs_update_accel(void) { struct Int32Vect3 acc_c_imu; INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); - /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ + /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu.accel); @@ -251,11 +254,10 @@ void ahrs_update_accel(void) { VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - float weight = 1.0; + if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration (gravity estimate) norm */ - - /* Factor how strongly to change the weight. + /* Factor how strongly to change the weight. * e.g. for WEIGHT_FACTOR 3: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ @@ -264,20 +266,23 @@ void ahrs_update_accel(void) { struct FloatVect3 g_meas_f; ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; - weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); - Bound(weight, 0.15, 1.0); + ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + Bound(ahrs_impl.weight, 0.15, 1.0); } - /* Complementary filter proportionnal gain. - * Kp = 1/(2 * zeta * omega) + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 - * FRAC conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 + * FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 + * Feedback_FRAC: 4 * - * Kp = 1 / inv_rate_scale * 4096 = 1/(2*45*32*500/32) * 4096 = 0.091 - * zeta = Kp/2/SQRT(Ki analog) = 0.73 + * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC + * inv_rate_scale= 4096 * 4 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 8192 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 2 * ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_zeta * AHRS_CORRECT_FREQUENCY / (32 * weight); + + int32_t inv_rate_scale = 8192 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -286,22 +291,22 @@ void ahrs_update_accel(void) { /* Complementary filter integral gain * Correct the gyro bias. - * Ki = 1/(omega*omega) + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 - * FRAC conversion: 2^40 / 2^24 = 2^16 - * - * Ki = 1/2^16 / inv_bias_gain * 32 = 1/2^16 / (500*32*32/8192) * 32 = 7.8e-6 - * Ki analog = 7.8e-6 * 500 = 3.9e-3 - * omega = SQRT(Ki analog)/(2*pi) = 0.01 Hz - T = 100 s. + * FRAC_conversion: 2^40 / 2^24 = 2^16 + * Feedback_FRAC: 4 + * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC + * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 4 + * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512) */ - int32_t inv_bias_gain = ahrs_impl.accel_inv_omega * ahrs_impl.accel_inv_omega * AHRS_CORRECT_FREQUENCY / (weight * weight * 8192); + + int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 512); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.r += (residual.z / inv_bias_gain) << 5; - INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); } @@ -327,28 +332,41 @@ static inline void ahrs_update_mag_full(void) { struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); - /* residual FRAC: 2 * MAG_FRAC = 22 + /* Complementary filter proportionnal gain. + * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 + * Feedback_FRAC: 4 * - * Scale with mag_attitude_gain * 2 for convenient range, - * and with mag frequency. + * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 1024 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - ahrs_impl.rate_correction.p += residual.x * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.q += residual.y * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.r += residual.z * (int32_t)ahrs_impl.mag_attitude_gain / (512 * AHRS_MAG_CORRECT_FREQUENCY); - /* residual FRAC: 2* MAG_FRAC = 22 + int32_t inv_rate_gain = 2048 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + + ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; + ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; + ahrs_impl.rate_correction.r += residual.z / inv_rate_gain; + + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 + * Feedback_FRAC: 4 * - * bias correction gain scale with 1/2^13 compared to attitude correction: - * 2^18 / 2^11 = 32 + * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^16 */ - int32_t bias_scale = 32 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.high_rez_bias.p -= residual.x * bias_scale; - ahrs_impl.high_rez_bias.q -= residual.y * bias_scale; - ahrs_impl.high_rez_bias.r -= residual.z * bias_scale; + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 65536; + ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; + ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; + ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); @@ -373,30 +391,43 @@ static inline void ahrs_update_mag_2d(void) { struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); - /* residual_imu FRAC = residual_ltp FRAC = 17 + /* Complementary filter proportionnal gain. + * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY/ AHRS_MAG_CORRECT_FREQUENCY + * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 + * Feedback_FRAC: 4 + * + * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY + * / FRAC_conversion * Feedback_FRAC + * inv_rate_gain = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY * - * Scale with mag_attitude_gain * 2 for convenient range, - * and with mag frequency. */ - ahrs_impl.rate_correction.p += residual_imu.x * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.q += residual_imu.y * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); - ahrs_impl.rate_correction.r += residual_imu.z * (int32_t)ahrs_impl.mag_attitude_gain / (16 * AHRS_MAG_CORRECT_FREQUENCY); + + int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + + ahrs_impl.rate_correction.p += residual_imu.x / inv_rate_gain; + ahrs_impl.rate_correction.q += residual_imu.y / inv_rate_gain; + ahrs_impl.rate_correction.r += residual_imu.z / inv_rate_gain; - /* residual_imu FRAC = residual_ltp FRAC = 17 + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 + * Feedback_FRAC: 4 * - * bias correction with 1/2^11 compared to attitude correction: - * 2^23 / 2^11 = 4096 - * also divide mag_gyrobias_gain by 4 for convenience range + * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ - int32_t bias_scale = 1024 * ahrs_impl.mag_gyrobias_gain / AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_scale; - ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_scale; - ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_scale; + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; + ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_gain; + ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain; + ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 555e418dc7..4a59955717 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -47,10 +47,11 @@ struct AhrsIntCmplQuat { struct Int32Quat ltp_to_imu_quat; struct Int32Eulers ltp_to_imu_euler; // FIXME to compile telemetry struct Int32Vect3 mag_h; - uint32_t accel_inv_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) - uint32_t accel_inv_zeta; ///< filter damping correcting the gyro-bias from accels (pseudo-gravity measurement) - uint32_t mag_attitude_gain; ///< gain for correcting the attitude (heading) from magnetometer - uint32_t mag_gyrobias_gain; ///< gain for correcting the gyro bias from magnetometer + float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) + float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) + float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer + float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + float weight; int32_t ltp_vel_norm; bool_t ltp_vel_norm_valid; bool_t correct_gravity; From 80f1b19686c59072bbb192644ee12d98935c0fc5 Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Sat, 13 Apr 2013 10:35:07 +0200 Subject: [PATCH 012/125] feedback_frac corrected for both accel and mag correction, omega and zeta nominal and min/max settings adjusted --- .../estimation/ahrs_int_cmpl_quat.xml | 6 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 66 +++++++++---------- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 9856758ec1..b53f9e04e6 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -6,9 +6,9 @@ - - - + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 1947911e42..1d261502a8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -75,14 +75,14 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #define AHRS_ACCEL_OMEGA 0.063 #endif #ifndef AHRS_ACCEL_ZETA -#define AHRS_ACCEL_ZETA 1. +#define AHRS_ACCEL_ZETA 0.9 #endif #ifndef AHRS_MAG_OMEGA -#define AHRS_MAG_OMEGA 0.063 +#define AHRS_MAG_OMEGA 0.04 #endif #ifndef AHRS_MAG_ZETA -#define AHRS_MAG_ZETA 1. +#define AHRS_MAG_ZETA 0.9 #endif /** by default use the gravity heursitic to reduce gain */ @@ -275,14 +275,14 @@ void ahrs_update_accel(void) { * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 * FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 - * Feedback_FRAC: 4 + * Feedback_FRAC: 8 * * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC - * inv_rate_scale= 4096 * 4 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) - * inv_rate_scale= 8192 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) + * inv_rate_scale= 4096 * 8 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 16384 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 8192 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); + int32_t inv_rate_scale = 16384 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -295,13 +295,13 @@ void ahrs_update_accel(void) { * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC_conversion: 2^40 / 2^24 = 2^16 - * Feedback_FRAC: 4 + * Feedback_FRAC: 8 * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC - * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 4 - * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512) + * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 8 + * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 256) */ - int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 512); + int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 256); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -337,15 +337,15 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 1024 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 1024 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - int32_t inv_rate_gain = 2048 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -357,13 +357,13 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^16 + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 65536; + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; @@ -388,6 +388,7 @@ static inline void ahrs_update_mag_2d(void) { 0, (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)}; + struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); @@ -396,21 +397,19 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 - * Feedback_FRAC: 4 - * - * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * Feedback_FRAC: 1/8 + * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5 * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * + * / FRAC_conversion * Feedback_FRAC * 2^5 + * inv_rate_gain = 32 * 32 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.rate_correction.p += residual_imu.x / inv_rate_gain; - ahrs_impl.rate_correction.q += residual_imu.y / inv_rate_gain; - ahrs_impl.rate_correction.r += residual_imu.z / inv_rate_gain; - + ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain) << 5; + ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain) << 5; + ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain) << 5; /* Complementary filter integral gain * Correct the gyro bias. @@ -418,16 +417,17 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * - * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * Ki = bias_gain / FRAC_conversion * Feedback_frac * 2^5= ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion/Feedback_frac * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; - ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_gain; - ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain; - ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain; + ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain) << 5; + ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain) << 5; + ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain) << 5; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); From 347d69d63f7cd31ea79a4a54434722ffbfe2a760 Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Sat, 13 Apr 2013 10:35:07 +0200 Subject: [PATCH 013/125] [ahrs] int_cmpl_quat: feedback_frac corrected - feedback_frac for both accel and mag correction - omega and zeta nominal and min/max settings adjusted --- .../estimation/ahrs_int_cmpl_quat.xml | 6 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 66 +++++++++---------- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 9856758ec1..b53f9e04e6 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -6,9 +6,9 @@ - - - + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 1947911e42..1d261502a8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -75,14 +75,14 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #define AHRS_ACCEL_OMEGA 0.063 #endif #ifndef AHRS_ACCEL_ZETA -#define AHRS_ACCEL_ZETA 1. +#define AHRS_ACCEL_ZETA 0.9 #endif #ifndef AHRS_MAG_OMEGA -#define AHRS_MAG_OMEGA 0.063 +#define AHRS_MAG_OMEGA 0.04 #endif #ifndef AHRS_MAG_ZETA -#define AHRS_MAG_ZETA 1. +#define AHRS_MAG_ZETA 0.9 #endif /** by default use the gravity heursitic to reduce gain */ @@ -275,14 +275,14 @@ void ahrs_update_accel(void) { * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 * FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 - * Feedback_FRAC: 4 + * Feedback_FRAC: 8 * * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC - * inv_rate_scale= 4096 * 4 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) - * inv_rate_scale= 8192 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) + * inv_rate_scale= 4096 * 8 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 16384 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 8192 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); + int32_t inv_rate_scale = 16384 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -295,13 +295,13 @@ void ahrs_update_accel(void) { * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC_conversion: 2^40 / 2^24 = 2^16 - * Feedback_FRAC: 4 + * Feedback_FRAC: 8 * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC - * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 4 - * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 512) + * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 8 + * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 256) */ - int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 512); + int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 256); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -337,15 +337,15 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 1024 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 1024 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - int32_t inv_rate_gain = 2048 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -357,13 +357,13 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^16 + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 65536; + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; @@ -388,6 +388,7 @@ static inline void ahrs_update_mag_2d(void) { 0, (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)}; + struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); @@ -396,21 +397,19 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 - * Feedback_FRAC: 4 - * - * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * Feedback_FRAC: 1/8 + * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5 * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 32 * 4 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * + * / FRAC_conversion * Feedback_FRAC * 2^5 + * inv_rate_gain = 32 * 32 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.rate_correction.p += residual_imu.x / inv_rate_gain; - ahrs_impl.rate_correction.q += residual_imu.y / inv_rate_gain; - ahrs_impl.rate_correction.r += residual_imu.z / inv_rate_gain; - + ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain) << 5; + ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain) << 5; + ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain) << 5; /* Complementary filter integral gain * Correct the gyro bias. @@ -418,16 +417,17 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 - * Feedback_FRAC: 4 + * Feedback_FRAC: 1/8 * - * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC + * Ki = bias_gain / FRAC_conversion * Feedback_frac * 2^5= ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion/Feedback_frac * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 */ + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; - ahrs_impl.high_rez_bias.p -= residual_imu.x * bias_gain; - ahrs_impl.high_rez_bias.q -= residual_imu.y * bias_gain; - ahrs_impl.high_rez_bias.r -= residual_imu.z * bias_gain; + ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain) << 5; + ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain) << 5; + ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain) << 5; INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); From f45bfc29da5b52a0b62f260052588daf7fcb25ff Mon Sep 17 00:00:00 2001 From: Loic Drumettaz Date: Mon, 27 May 2013 16:46:22 +0200 Subject: [PATCH 014/125] [ahrs] Complementary filter gains calculated with cut-off frequency and damping settings in physical units, scaled with sensors update frequencies, for integer and float filters --- sw/airborne/math/pprz_algebra_float.h | 6 + sw/airborne/math/pprz_algebra_int.h | 15 +- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 137 ++++++++++++++---- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 98 +++++++------ 4 files changed, 182 insertions(+), 74 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index e48aca10dd..17bb1a4101 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -127,6 +127,12 @@ struct FloatRates { n = sqrtf((v).x*(v).x + (v).y*(v).y); \ } +#define FLOAT_VECT2_NORMALIZE(_v) { \ + float n; \ + FLOAT_VECT2_NORM(n, _v); \ + FLOAT_VECT2_SMUL(_v, _v, 1 / n); \ + } + /* * Dimension 3 Vectors diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index d482ad5565..d32d72c423 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -61,6 +61,8 @@ struct Int16Vect3 { #define INT32_ACCEL_FRAC 10 #define INT32_MAG_FRAC 11 +#define INT32_PERCENTAGE_FRAC 10 + struct Int32Vect2 { int32_t x; int32_t y; @@ -211,11 +213,18 @@ struct Int64Vect3 { #define INT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y) -#define INT32_VECT2_NORM(n, v) { \ - int32_t n2 = (v).x*(v).x + (v).y*(v).y; \ - INT32_SQRT(n, n2); \ +#define INT32_VECT2_NORM(_n, _v) { \ + int32_t n2 = (_v).x*(_v).x + (_v).y*(_v).y; \ + INT32_SQRT(_n, n2); \ } +#define INT32_VECT2_NORMALIZE(_v,_frac) { \ + int32_t n; \ + INT32_VECT2_NORM(n, _v); \ + INT32_VECT2_SCALE_2(_v, _v, BFP_OF_REAL((1.),_frac) , n); \ + } + + #define INT32_VECT2_RSHIFT(_o, _i, _r) { \ (_o).x = ((_i).x >> (_r)); \ (_o).y = ((_i).y >> (_r)); \ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 3ec76ada56..d3b08f32a4 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -55,12 +55,45 @@ #endif #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING -#warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." +#warning "Using magnetometer _and_ GPS course to update heading. Probably better to if you want to use GPS course." #endif #ifndef AHRS_PROPAGATE_FREQUENCY #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY #endif +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + +#ifndef AHRS_CORRECT_FREQUENCY +#define AHRS_CORRECT_FREQUENCY AHRS_PROPAGATE_FREQUENCY +#endif +PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + +#ifndef AHRS_MAG_CORRECT_FREQUENCY +#define AHRS_MAG_CORRECT_FREQUENCY 50 +#endif +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + +/* + * default gains for correcting attitude and bias from accel/mag + */ +#ifndef AHRS_ACCEL_OMEGA +#define AHRS_ACCEL_OMEGA 0.063 +#endif +#ifndef AHRS_ACCEL_ZETA +#define AHRS_ACCEL_ZETA 0.9 +#endif + +#ifndef AHRS_MAG_OMEGA +#define AHRS_MAG_OMEGA 0.04 +#endif +#ifndef AHRS_MAG_ZETA +#define AHRS_MAG_ZETA 0.9 +#endif + +/** by default use the gravity heursitic to reduce gain */ +#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC +#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE +#endif #ifdef AHRS_UPDATE_FW_ESTIMATOR // remotely settable @@ -101,12 +134,26 @@ void ahrs_init(void) { FLOAT_RATES_ZERO(ahrs_impl.imu_rate); + /* set default filter cut-off frequency and damping */ + ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; + ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_impl.mag_omega = AHRS_MAG_OMEGA; + ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; #else ahrs_impl.correct_gravity = FALSE; #endif +#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC + ahrs_impl.use_gravity_heuristic = TRUE; +#else + ahrs_impl.use_gravity_heuristic = FALSE; +#endif + +/* TO DO add local magnetic field + VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); */ } void ahrs_align(void) { @@ -131,7 +178,6 @@ void ahrs_align(void) { struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); - ahrs.status = AHRS_RUNNING; } @@ -184,6 +230,8 @@ void ahrs_update_accel(void) { struct FloatVect3 residual; + struct FloatVect3 pseudo_gravity_measurement; + if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame @@ -200,30 +248,43 @@ void ahrs_update_accel(void) { struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); - /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ - struct FloatVect3 corrected_gravity; - VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu); + /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ + VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu); - /* compute the residual of gravity vector in imu frame */ - FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2); } else { - FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2); + VECT3_COPY(pseudo_gravity_measurement, imu_accel_float); } -#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC - /* heuristic on acceleration norm */ - const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float); - const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.); -#else - const float weight = 1.; -#endif + FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); - /* compute correction */ - const float gravity_rate_update_gain = -5e-2; // -5e-2 - FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); + if (ahrs_impl.use_gravity_heuristic) { + /* heuristic on acceleration (gravity estimate) norm */ + /* Factor how strongly to change the weight. + * e.g. for WEIGHT_FACTOR 3: + * <0.66G = 0, 1G = 1.0, >1.33G = 0 + */ + #define WEIGHT_FACTOR 3 - const float gravity_bias_update_gain = 1e-5; // -5e-6 - FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); + /* TODO filter pseudo_gravity_measurement */ + /* g_meas_f=filter(pseudo_gravity_measurement) */ + + const float g_meas_norm = FLOAT_VECT3_NORM(pseudo_gravity_measurement) / 9.81; + ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + Bound(ahrs_impl.weight, 0.15, 1.0); + } + + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + */ + const float gravity_rate_update_gain = -2 / 9.81 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY; + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); + + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + */ + const float gravity_bias_update_gain = ( ahrs_impl.accel_omega * ahrs_impl.accel_omega * ahrs_impl.weight * ahrs_impl.weight) / (AHRS_CORRECT_FREQUENCY * 9.81); + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ @@ -253,39 +314,61 @@ void ahrs_update_mag_full(void) { // DISPLAY_FLOAT_VECT3("# measured", measured_imu); // DISPLAY_FLOAT_VECT3("# residual", residual); - const float mag_rate_update_gain = 2.5; + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + */ + + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); - const float mag_bias_update_gain = -2.5e-3; + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + */ + const float mag_bias_update_gain = -( ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); } void ahrs_update_mag_2d(void) { - const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y}; + struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y}; + // normalize expected ltp in 2D (x,y) + FLOAT_VECT2_NORMALIZE(expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu); + struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; + + // normalize measured ltp in 2D (x,y) + FLOAT_VECT2_NORMALIZE(measured_ltp_2d); + const struct FloatVect3 residual_ltp = { 0, 0, - measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x }; + measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); - const float mag_rate_update_gain = 2.5; + + /* Complementary filter proportional gain. + * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + */ + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); - const float mag_bias_update_gain = -2.5e-3; + /* Complementary filter integral gain + * Correct the gyro bias. + * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY + */ + const float mag_bias_update_gain = -( ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); - } diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 1d261502a8..810fef2b84 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -43,6 +43,9 @@ #include "math/pprz_trig_int.h" #include "math/pprz_algebra_int.h" +#ifdef AHRS_PROPAGATE_LOW_PASS_RATES +PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES") +#endif #ifdef AHRS_MAG_UPDATE_YAW_ONLY #error "The define AHRS_MAG_UPDATE_YAW_ONLY doesn't exist anymore, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw." @@ -77,6 +80,9 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #ifndef AHRS_ACCEL_ZETA #define AHRS_ACCEL_ZETA 0.9 #endif +PRINT_CONFIG_VAR(AHRS_ACCEL_OMEGA) +PRINT_CONFIG_VAR(AHRS_ACCEL_ZETA) + #ifndef AHRS_MAG_OMEGA #define AHRS_MAG_OMEGA 0.04 @@ -84,6 +90,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #ifndef AHRS_MAG_ZETA #define AHRS_MAG_ZETA 0.9 #endif +PRINT_CONFIG_VAR(AHRS_MAG_OMEGA) +PRINT_CONFIG_VAR(AHRS_MAG_ZETA) /** by default use the gravity heursitic to reduce gain */ #ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC @@ -246,7 +254,7 @@ void ahrs_update_accel(void) { INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); - /* FIR filtered pseudo_gravity_measurement */ + /* FIR filtered pseudo_gravity_measurement TO DO MOVE IN USE HEURISTIC*/ #define FIR_FILTER_SIZE 8 static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); @@ -254,14 +262,13 @@ void ahrs_update_accel(void) { VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. * e.g. for WEIGHT_FACTOR 3: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ - #define WEIGHT_FACTOR 3 + #define WEIGHT_FACTOR 5 struct FloatVect3 g_meas_f; ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); @@ -274,15 +281,14 @@ void ahrs_update_accel(void) { * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 - * FRAC_conversion: 2^12 / 2^24 = 1/(2^12) = 1/4096 - * Feedback_FRAC: 8 - * - * Kp = 1 / inv_rate_scale / FRAC_conversion * feedback_FRAC - * inv_rate_scale= 4096 * 8 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) - * inv_rate_scale= 16384 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) + * FRAC_conversion: 2^12 / 2^24 = 1 / 4096 + * cross_product_gain : 9.81 m/s2 + * Kp = 1 / inv_rate_scale / FRAC_conversion * cross_product_gain + * inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 16384 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); + int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -295,13 +301,13 @@ void ahrs_update_accel(void) { * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC_conversion: 2^40 / 2^24 = 2^16 - * Feedback_FRAC: 8 - * Ki = 1 / FRAC_conversion / inv_bias_gain * 2^5 * feedback_FRAC - * inv_bias_gain = 1/2^16/(omega*omega*weight*weight/AHRS_CORRECT_FREQUENCY) * 2^5 * 8 - * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 256) + * cross_product_gain : 9.81 m/s2 + * Ki = 1 / FRAC_conversion / inv_bias_gain * cross_product_gain * 2^5 + * inv_bias_gain = 9.81 / 2^16 / (omega * omega * weight * weight / AHRS_CORRECT_FREQUENCY) * 2^5 + * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048) */ - int32_t inv_bias_gain = AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 256); + int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -331,21 +337,21 @@ static inline void ahrs_update_mag_full(void) { struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); + //INT32_VECT3_RSHIFT(residual,residual,5); /* Complementary filter proportionnal gain. * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 - * Feedback_FRAC: 1/8 * - * Kp = 1/ inv_rate_gain / FRAC_conversion * Feedback_FRAC + * Kp = 1/ inv_rate_gain / FRAC_conversion * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion * Feedback_FRAC - * inv_rate_gain = 1024 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * / FRAC_conversion + * inv_rate_gain = 1024 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + int32_t inv_rate_gain = 512 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -357,13 +363,12 @@ static inline void ahrs_update_mag_full(void) { * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 - * Feedback_FRAC: 1/8 * - * Ki = bias_gain / FRAC_conversion * Feedback_FRAC = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion / Feedback_FRAC - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 + * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^18 */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * (1 << 18); ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; @@ -376,17 +381,25 @@ static inline void ahrs_update_mag_full(void) { static inline void ahrs_update_mag_2d(void) { + struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y}; + /* normalize expected ltp in 2D (x,y) */ + INT32_VECT2_NORMALIZE(expected_ltp, INT32_MAG_FRAC); + struct Int32RMat ltp_to_imu_rmat; INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); struct Int32Vect3 measured_ltp; INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag); + /* normalize measured ltp in 2D (x,y) */ + struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y}; + INT32_VECT2_NORMALIZE(measured_ltp_2d, INT32_MAG_FRAC); + /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */ struct Int32Vect3 residual_ltp = { 0, 0, - (measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x)/(1<<5)}; + (measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x)/(1<<5)}; struct Int32Vect3 residual_imu; @@ -397,19 +410,18 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 - * Feedback_FRAC: 1/8 + * * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5 * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion * Feedback_FRAC * 2^5 - * inv_rate_gain = 32 * 32 / 8 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * / FRAC_conversion * 2^5 + * inv_rate_gain = 32 * 32 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 512 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY */ - int32_t inv_rate_gain = 64 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; - - ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain) << 5; - ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain) << 5; - ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain) << 5; + int32_t inv_rate_gain = 16 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); + ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); + ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain); /* Complementary filter integral gain * Correct the gyro bias. @@ -417,18 +429,16 @@ static inline void ahrs_update_mag_2d(void) { * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 - * Feedback_FRAC: 1/8 * - * Ki = bias_gain / FRAC_conversion * Feedback_frac * 2^5= ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion/Feedback_frac - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^21 + * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23 */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2097152; - ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain) << 5; - ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain) << 5; - ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain) << 5; - + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * (1 << 23); + ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain); + ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain); + ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain); INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); From 62d0070eed7a2f4ae7eb9bc92aed1626650e0150 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 28 May 2013 17:48:01 +0200 Subject: [PATCH 015/125] [fix] compilation issue with float cmpl filter --- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index a40d888f08..ea40c65d05 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -40,9 +40,14 @@ struct AhrsFloatCmpl { struct FloatRMat ltp_to_imu_rmat; /* for gravity correction during coordinated turns */ float ltp_vel_norm; + float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) + float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) + float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer + float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + float weight; bool_t ltp_vel_norm_valid; bool_t correct_gravity; - + bool_t use_gravity_heuristic; bool_t heading_aligned; /* From 5a344232d9b40a00da3c0357912740f3bef1fdb1 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 25 Jun 2013 13:58:19 +0200 Subject: [PATCH 016/125] [mission] add mission messages --- conf/messages.xml | 55 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/conf/messages.xml b/conf/messages.xml index 2ddf6a8999..99700443f9 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2207,6 +2207,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From b361afdeaf3cf800bee0a8f20a2833ad23032ef2 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 25 Jun 2013 13:58:58 +0200 Subject: [PATCH 017/125] [mission] add mission module (only parsing messages for now) --- conf/modules/mission_msg.xml | 18 +++ sw/airborne/modules/mission/mission_msg.c | 138 ++++++++++++++++++++++ sw/airborne/modules/mission/mission_msg.h | 99 ++++++++++++++++ 3 files changed, 255 insertions(+) create mode 100644 conf/modules/mission_msg.xml create mode 100644 sw/airborne/modules/mission/mission_msg.c create mode 100644 sw/airborne/modules/mission/mission_msg.h diff --git a/conf/modules/mission_msg.xml b/conf/modules/mission_msg.xml new file mode 100644 index 0000000000..8812e48f81 --- /dev/null +++ b/conf/modules/mission_msg.xml @@ -0,0 +1,18 @@ + + + + + New messages for Mission Planner + +
+ +
+ + + + + + + + + diff --git a/sw/airborne/modules/mission/mission_msg.c b/sw/airborne/modules/mission/mission_msg.c new file mode 100644 index 0000000000..bdd528e736 --- /dev/null +++ b/sw/airborne/modules/mission/mission_msg.c @@ -0,0 +1,138 @@ +/** \file mission_msg.c + * \brief the new messages for mission planner library + * \ edited by Anh Truong + */ + +//#define MISSION_C + + + +#include + + +#include "subsystems/datalink/downlink.h" + +#include "mission/mission_msg.h" +//#include "estimator.h" +//#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" +//#include "firmwares/fixedwing/guidance/guidance_v.h" +#include "autopilot.h" +//#include "subsystems/ins.h" +//#include "subsystems/nav.h" +#include "subsystems/gps.h" +#include "generated/flight_plan.h" +#include "generated/airframe.h" +#include "dl_protocol.h" + +#include + + + + + +void mission_msg_init(void) { + //float x_to_go = 85.1; // I5 in FP file + //float y_to_go = 173.6; + //fly_to_xy(x, y); +} + +int mission_msg_GOTO_WP(float x, float y) { + //float x_to_go = 85.1; // I5 in FP file + //float y_to_go = 173.6; + fly_to_xy(x, y); + return FALSE; +} + +int mission_msg_SEGMENT(float x1, float y1, float x2, float y2) { + if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { + return FALSE; // end of block and go to the next block +// NextBlock(); + } + else { + //NextBlock() + + //nav_init_stage(); + nav_route_xy(x1, y1, x2, y2); + NavVerticalAutoThrottleMode(RadOfDeg(0.000000)); + NavVerticalAltitudeMode(5.0, 0.); + } + return TRUE; +} + + + +/** Status along the mission path */ +enum mission_msg_PATH_status { Path12, Path23, Path34, Path45 }; +static enum mission_msg_PATH_status mission_msg_PATH_status; + +int mission_msg_PATH_init( void ) { + // status of the first segment of the path + mission_msg_PATH_status = Path12; + + // path finder here to generate intermediate waypoints + // these five points are defined as global variables in mission_info.h + //struct tabWaypoint tabW; + //tabW = pathFinder(); + //pathFinder(); + + + /*AstarStartPoint_east = estimator_x; + AstarStartPoint_north = estimator_y; + + InterPoint_east_1 = + InterPoint_north_1 = + InterPoint_east_2 = + InterPoint_north_2 = + InterPoint_east_3 = + InterPoint_north_3 = + + + AstarEndPoint_east = estimator_x; + AstarEndPoint_north = estimator_y;*/ + + //PathFinder(int argc, const char * argv[]); -->> AstarPath_x1, AstarPath_y1... + + return FALSE; +} + +int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5) { + + switch(mission_msg_PATH_status) { + case Path12: /* path 1-2 */ + nav_route_xy(x1, y1, x2, y2); + if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { + mission_msg_PATH_status = Path23; + //nav_init_stage(); + } + break; + case Path23: /* path 2-3 */ + nav_route_xy(x2, y2, x3, y3); + if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) { + mission_msg_PATH_status = Path34; + //nav_init_stage(); + } + break; + case Path34: /* path 3-4 */ + nav_route_xy(x3, y3, x4, y4); + if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) { + mission_msg_PATH_status = Path45; + //nav_init_stage(); + } + break; + case Path45: /* path 4-5 */ + nav_route_xy(x4, y4, x5, y5); + if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) { + //mission_msg_PATH_status = Path12; + //nav_init_stage(); + return FALSE; /* This path ends when AC arrive to point 5 */ + } + break; + } + + + return TRUE; /* do the function */ +} + + + + diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h new file mode 100644 index 0000000000..0e40616015 --- /dev/null +++ b/sw/airborne/modules/mission/mission_msg.h @@ -0,0 +1,99 @@ +/* + * $Id$ + * + * Copyright (C) 2010 ENAC + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** \file mission_msg.h + * \brief the new messages for mission planner library + * \ edited by Anh Truong + */ + +#ifndef MISSION_H +#define MISSION_H + + + +#include "std.h" +#include "subsystems/nav.h" +#include "subsystems/navigation/traffic_info.h" + + +extern void mission_msg_init(void); + +extern int mission_msg_GOTO_WP(float x, float y); + +extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2); + +extern int mission_msg_PATH_init( void ); +extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5); + + + +#define ParseMissionGotoWp() { \ + if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) == AC_ID) { \ + uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \ + float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \ + float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \ + mission_msg_GOTO_WP(wp_east, wp_north); \ + } \ +} + +#define ParseMissionPath() { \ + if (DL_MISSION_PATH_ac_id(dl_buffer) == AC_ID) { \ + uint8_t ac_id = DL_MISSION_PATH_ac_id(dl_buffer); \ + float point_east_1 = DL_MISSION_PATH_point_east_1(dl_buffer); \ + float point_north_1 = DL_MISSION_PATH_point_north_1(dl_buffer); \ + float point_east_2 = DL_MISSION_PATH_point_east_2(dl_buffer); \ + float point_north_2 = DL_MISSION_PATH_point_north_2(dl_buffer); \ + float point_east_3 = DL_MISSION_PATH_point_east_3(dl_buffer); \ + float point_north_3 = DL_MISSION_PATH_point_north_3(dl_buffer); \ + float point_east_4 = DL_MISSION_PATH_point_east_4(dl_buffer); \ + float point_north_4 = DL_MISSION_PATH_point_north_4(dl_buffer); \ + float point_east_5 = DL_MISSION_PATH_point_east_5(dl_buffer); \ + float point_north_5 = DL_MISSION_PATH_point_north_5(dl_buffer); \ + mission_msg_PATH(point_east_1, point_north_1, point_east_2, point_north_2, point_east_3, point_north_3, point_east_4, point_north_4, point_east_5, point_north_5); \ + } \ +} + + +#define ParseMissionSegment() { \ + if (DL_MISSION_SEGMENT_ac_id(dl_buffer) == AC_ID) { \ + uint8_t ac_id = DL_MISSION_SEGMENT_ac_id(dl_buffer); \ + float segment_east_1 = DL_MISSION_SEGMENT_segment_east_1(dl_buffer); \ + float segment_north_1 = DL_MISSION_SEGMENT_segment_north_1(dl_buffer); \ + float segment_east_2 = DL_MISSION_SEGMENT_segment_east_2(dl_buffer); \ + float segment_north_2 = DL_MISSION_SEGMENT_segment_north_2(dl_buffer); \ + mission_msg_SEGMENT(segment_east_1, segment_north_1, segment_east_2, segment_north_2); \ + } \ +} + + +/* +#define ParseMissionGotoWp() { \ + uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \ + float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \ + float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \ + fly_to_xy(wp_east, wp_north); \ +} +*/ + +#endif // MISSION From 0c5e8e4807671268727412cf4e1ff7e71e30eb67 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 27 Jun 2013 12:11:32 +0200 Subject: [PATCH 018/125] [mission] define mission structures --- sw/airborne/modules/mission/mission_msg.c | 72 ++++++++--------------- sw/airborne/modules/mission/mission_msg.h | 64 ++++++++++++++++---- 2 files changed, 79 insertions(+), 57 deletions(-) diff --git a/sw/airborne/modules/mission/mission_msg.c b/sw/airborne/modules/mission/mission_msg.c index bdd528e736..95c084f56b 100644 --- a/sw/airborne/modules/mission/mission_msg.c +++ b/sw/airborne/modules/mission/mission_msg.c @@ -6,51 +6,46 @@ //#define MISSION_C +#include "modules/mission/mission_msg.h" #include - #include "subsystems/datalink/downlink.h" -#include "mission/mission_msg.h" -//#include "estimator.h" -//#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -//#include "firmwares/fixedwing/guidance/guidance_v.h" +#include "state.h" #include "autopilot.h" -//#include "subsystems/ins.h" -//#include "subsystems/nav.h" #include "subsystems/gps.h" -#include "generated/flight_plan.h" #include "generated/airframe.h" #include "dl_protocol.h" -#include + +struct _mission mission; - - - -void mission_msg_init(void) { - //float x_to_go = 85.1; // I5 in FP file - //float y_to_go = 173.6; - //fly_to_xy(x, y); +void mission_init(void) { + mission.mission_insert_idx = 0; + mission.mission_extract_idx = 0; } -int mission_msg_GOTO_WP(float x, float y) { - //float x_to_go = 85.1; // I5 in FP file - //float y_to_go = 173.6; - fly_to_xy(x, y); - return FALSE; +int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d) { + mission.elements[insert_idx].type = MissionWP; + mission.elements[insert_idx].element.mission_wp.wp.x = x; + mission.elements[insert_idx].element.mission_wp.wp.y = y; + mission.elements[insert_idx].element.mission_wp.wp.a = a; + mission.elements[insert_idx].element.duration = d; + + insert_idx++; + if (insert_idx == MISSION_ELEMENT_NB) insert_idx = 0; + return TRUE; } -int mission_msg_SEGMENT(float x1, float y1, float x2, float y2) { +int mission_msg_SEGMENT(float x1, float y1, float x2, float y2) { if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { return FALSE; // end of block and go to the next block // NextBlock(); - } + } else { //NextBlock() - //nav_init_stage(); nav_route_xy(x1, y1, x2, y2); NavVerticalAutoThrottleMode(RadOfDeg(0.000000)); @@ -75,28 +70,11 @@ int mission_msg_PATH_init( void ) { //tabW = pathFinder(); //pathFinder(); - - /*AstarStartPoint_east = estimator_x; - AstarStartPoint_north = estimator_y; - - InterPoint_east_1 = - InterPoint_north_1 = - InterPoint_east_2 = - InterPoint_north_2 = - InterPoint_east_3 = - InterPoint_north_3 = - - - AstarEndPoint_east = estimator_x; - AstarEndPoint_north = estimator_y;*/ - - //PathFinder(int argc, const char * argv[]); -->> AstarPath_x1, AstarPath_y1... - return FALSE; -} +} int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5) { - + switch(mission_msg_PATH_status) { case Path12: /* path 1-2 */ nav_route_xy(x1, y1, x2, y2); @@ -104,21 +82,21 @@ int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, mission_msg_PATH_status = Path23; //nav_init_stage(); } - break; + break; case Path23: /* path 2-3 */ nav_route_xy(x2, y2, x3, y3); if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) { mission_msg_PATH_status = Path34; //nav_init_stage(); } - break; + break; case Path34: /* path 3-4 */ nav_route_xy(x3, y3, x4, y4); if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) { mission_msg_PATH_status = Path45; //nav_init_stage(); } - break; + break; case Path45: /* path 4-5 */ nav_route_xy(x4, y4, x5, y5); if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) { @@ -126,7 +104,7 @@ int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, //nav_init_stage(); return FALSE; /* This path ends when AC arrive to point 5 */ } - break; + break; } diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h index 0e40616015..cd0ba7af1c 100644 --- a/sw/airborne/modules/mission/mission_msg.h +++ b/sw/airborne/modules/mission/mission_msg.h @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2010 ENAC + * Copyright (C) 2013 ENAC * * This file is part of paparazzi. * @@ -22,19 +20,65 @@ * */ -/** \file mission_msg.h - * \brief the new messages for mission planner library - * \ edited by Anh Truong +/** @file mission_msg.h + * @brief the new messages for mission planner library */ #ifndef MISSION_H #define MISSION_H - - #include "std.h" -#include "subsystems/nav.h" -#include "subsystems/navigation/traffic_info.h" +#include "subsystems/navigation/common_nav.h" + +enum MissionType { + MissionWP, + MissionCircle, + MissionSegment, + MissionPath, + MissionSurvey, + MissionEight, + MissionOval +}; + +struct _mission_wp { + struct point wp; +}; + +struct _mission_circle { + struct point center; + float radius; +}; + +struct _mission_segment { + struct point from; + struct point to; +}; + +#define MISSION_PATH_NB 5 +struct _mission_path { + struct point[MISSION_PATH_NB] path; +}; + +struct _mission_element { + enum MissionType type; + union { + struct _mission_wp mission_wp; + struct _mission_circle mission_circle; + struct _mission_segment mission_segment; + struct _mission_path mission_path; + } element; + uint16_t duration; +}; + +#define MISSION_ELEMENT_NB 20 +struct _mission { + struct _mission_element[MISSION_ELEMENT_NB] mission_tasks; + uint8_t mission_insert_idx; + uint8_t mission_extract_idx; + +}; + +extern struct _mission mission; extern void mission_msg_init(void); From c82262d5dc95ef2b2d2b7141d81cd25d95dfbc65 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 27 Jun 2013 13:38:13 +0200 Subject: [PATCH 019/125] [mission] forgot some files --- sw/airborne/modules/mission/mission_msg.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h index cd0ba7af1c..c35d5f42f6 100644 --- a/sw/airborne/modules/mission/mission_msg.h +++ b/sw/airborne/modules/mission/mission_msg.h @@ -72,7 +72,7 @@ struct _mission_element { #define MISSION_ELEMENT_NB 20 struct _mission { - struct _mission_element[MISSION_ELEMENT_NB] mission_tasks; + struct _mission_element[MISSION_ELEMENT_NB] elements; uint8_t mission_insert_idx; uint8_t mission_extract_idx; @@ -81,7 +81,7 @@ struct _mission { extern struct _mission mission; -extern void mission_msg_init(void); +extern void mission_init(void); extern int mission_msg_GOTO_WP(float x, float y); From 0362b747b31108b62657f9256aac42cbbbee3e7f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 15 Jul 2013 14:57:51 +0200 Subject: [PATCH 020/125] [flight_plan] add 'until' attribute to 'survey' and 'call' statement --- sw/tools/gen_flight_plan.ml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index cdcdad33ac..8c5a0cd249 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -481,6 +481,13 @@ let rec print_stage = fun index_of_waypoints x -> let statement = ExtXml.attrib x "fun" in lprintf "if (! (%s))\n" statement; lprintf " NextStageAndBreak();\n"; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStageAndBreak();\n" c + with + ExtXml.Error _ -> () + end; lprintf "break;\n" | "survey_rectangle" -> let grid = parsed_attrib x "grid" @@ -495,6 +502,13 @@ let rec print_stage = fun index_of_waypoints x -> left (); stage (); lprintf "NavSurveyRectangle(%s, %s);\n" wp1 wp2; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStageAndBreak();\n" c + with + ExtXml.Error _ -> () + end; lprintf "break;\n" | _s -> failwith "Unreachable" end; From cc10e482d76e926b623f5d64330b21ef7ba8dd83 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 15 Jul 2013 16:21:59 +0200 Subject: [PATCH 021/125] [flight_plan] update DTD for until attribute with call and survey --- conf/flight_plans/flight_plan.dtd | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 7d1fbdeb12..fb18d5ce0e 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -159,7 +159,8 @@ var CDATA #REQUIRED value CDATA #REQUIRED> +fun CDATA #REQUIRED +until CDATA #IMPLIED> grid CDATA #REQUIRED orientation CDATA #IMPLIED wp1 CDATA #REQUIRED -wp2 CDATA #REQUIRED> +wp2 CDATA #REQUIRED +until CDATA #IMPLIED> Date: Thu, 22 Aug 2013 18:33:17 +0200 Subject: [PATCH 022/125] [isa] conversion tools from pressure to meters --- sw/airborne/math/pprz_isa.h | 73 +++++++++++++++++++++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 sw/airborne/math/pprz_isa.h diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h new file mode 100644 index 0000000000..af1e133035 --- /dev/null +++ b/sw/airborne/math/pprz_isa.h @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** + * @file math/pprz_isa.h + * @brief Paparazzi atmospheric pressure convertion utilities + * + * Conversion functions are use to approximate altitude + * from atmospheric pressure based on the standard model + * and the International Standard Atmosphere (ISA) + * + * http://en.wikipedia.org/wiki/Atmospheric_pressure + * http://en.wikipedia.org/wiki/International_Standard_Atmosphere + * + */ + +#ifndef PPRZ_ISA_H +#define PPRZ_ISA_H + +#include "std.h" +#include + +// Standard Atmosphere constants +#define PPRZ_ISA_SEA_LEVEL_PRESSURE 101325.0 +#define PPRZ_ISA_SEA_LEVEL_TEMP 288.15 +#define PPRZ_ISA_TEMP_LAPS_RATE 0.0065 +#define PPRZ_ISA_GRAVITY 9.80665 +#define PPRZ_ISA_AIR_GAS_CONSTANT (8.31447/0.0289644) + +#define PPRZ_ISA_M_OF_P_CONST (PPRZ_ISA_AIR_GAS_CONSTANT*PPRZ_ISA_SEA_LEVEL_TEMP/PPRZ_ISA_GRAVITY) + +/** + * Get meters from pressure (using simplified equation) + * + * @param pressure pressure (float) in Pascal (Pa) + * @return altitude in pressure in ISA conditions + */ +static inline float pprz_isa_meters_of_pressure_f(float pressure) { + return (PPRZ_ISA_M_OF_P_CONST*logf(PPRZ_ISA_SEA_LEVEL_PRESSURE/pressure)); +} + +/** + * Get meters from pressure (using simplified equation) + * + * @param pressure pressure (int) in deci Pascal (10*Pa) + * @return altitude in pressure in ISA conditions + */ +static inline float pprz_isa_meters_of_pressure_i(int32_t pressure) { + float p = (float)pressure/10.; + return pprz_isa_meters_of_pressure_f(p); +} + + +#endif /* PPRZ_ISA_H */ From 4bb23aeb840aaceaef16ef481ea433f619cf4102 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 22 Aug 2013 18:34:33 +0200 Subject: [PATCH 023/125] [abi/baro] binding to ABI baro message is compiling --- conf/messages.xml | 10 +++- sw/airborne/firmwares/rotorcraft/main.c | 3 ++ sw/airborne/subsystems/abi_common.h | 4 +- sw/airborne/subsystems/ins/ins_int.c | 70 ++++++++++++++++++------- sw/airborne/subsystems/sensors/baro.h | 13 ++++- sw/tools/gen_abi.ml | 2 +- 6 files changed, 78 insertions(+), 24 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 8a610824c1..4d199623fb 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2548,10 +2548,18 @@ - + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 060367961d..e57f41b3a0 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -28,6 +28,8 @@ #define MODULES_C +#define ABI_C + #include #include "mcu.h" #include "mcu_periph/sys_time.h" @@ -72,6 +74,7 @@ #endif #include "generated/modules.h" +#include "subsystems/abi.h" /* if PRINT_CONFIG is defined, print some config options */ diff --git a/sw/airborne/subsystems/abi_common.h b/sw/airborne/subsystems/abi_common.h index 1e668b2e9c..bb3ebe335a 100644 --- a/sw/airborne/subsystems/abi_common.h +++ b/sw/airborne/subsystems/abi_common.h @@ -35,9 +35,9 @@ /* Some magic to avoid to compile C code, only headers */ #ifdef ABI_C -#define EXTERN +#define ABI_EXTERN #else -#define EXTERN extern +#define ABI_EXTERN extern #endif /** Generic callback definition */ diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 950397b7fa..e1d031b71f 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -28,6 +28,8 @@ #include "subsystems/ins/ins_int.h" +#include "subsystems/abi.h" + #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" #include "subsystems/gps.h" @@ -49,6 +51,7 @@ #include "math/pprz_geodetic_int.h" +#include "math/pprz_isa.h" #include "generated/flight_plan.h" @@ -73,6 +76,11 @@ struct FloatVect2 ins_gps_speed_m_s_ned; int32_t ins_qfe; bool_t ins_baro_initialised; int32_t ins_baro_alt; +#ifndef INS_BARO_ID +#define INS_BARO_ID ABI_BROADCAST +#endif +abi_event baro_ev; +static void baro_cb(const int32_t pressure); #endif /* output */ @@ -102,6 +110,8 @@ void ins_init() { ins_ltp_initialised = FALSE; #endif #if USE_VFF + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); ins_baro_initialised = FALSE; vff_init(0., 0., 0.); #endif @@ -168,29 +178,51 @@ void ins_propagate() { INS_NED_TO_STATE(); } -void ins_update_baro() { #if USE_VFF - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro.absolute; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = baro.absolute; - vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { /* not realigning, so normal update with baro measurement */ - ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; - float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); - vff_update(alt_float); - } +static void baro_cb(int32_t pressure) { + if (!ins_baro_initialised) { + ins_qfe = pressure; + ins_baro_initialised = TRUE; + } + if (ins.vf_realign) { + ins.vf_realign = FALSE; + ins_qfe = pressure; + vff_realign(0.); + ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + } + else { + float alt = pprz_isa_meters_of_pressure_i(pressure); + vff_update(alt); } INS_NED_TO_STATE(); +} #endif + +void ins_update_baro() { +//#if USE_VFF +// if (baro.status == BS_RUNNING) { +// if (!ins_baro_initialised) { +// ins_qfe = baro.absolute; +// ins_baro_initialised = TRUE; +// } +// if (ins.vf_realign) { +// ins.vf_realign = FALSE; +// ins_qfe = baro.absolute; +// vff_realign(0.); +// ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); +// ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); +// ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); +// } +// else { /* not realigning, so normal update with baro measurement */ +// ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; +// float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); +// vff_update(alt_float); +// } +// } +// INS_NED_TO_STATE(); +//#endif } diff --git a/sw/airborne/subsystems/sensors/baro.h b/sw/airborne/subsystems/sensors/baro.h index 76dd521dbc..11d46d041b 100644 --- a/sw/airborne/subsystems/sensors/baro.h +++ b/sw/airborne/subsystems/sensors/baro.h @@ -31,12 +31,23 @@ #include +/* + * Status of the baro + */ enum BaroStatus { BS_UNINITIALIZED, BS_RUNNING }; -/* pressure in which units ? */ +/* + * Baro structure + * + * pressures are in deciPa + * + * vertical resolution is around 1cm at sea level + * with standard atmospheric model + * + */ struct Baro { int32_t absolute; int32_t differential; diff --git a/sw/tools/gen_abi.ml b/sw/tools/gen_abi.ml index 2b2eb95a10..042f2c24b1 100644 --- a/sw/tools/gen_abi.ml +++ b/sw/tools/gen_abi.ml @@ -92,7 +92,7 @@ module Gen_onboard = struct let print_struct = fun h size -> Printf.fprintf h "\n/* Array and linked list structure */\n"; Printf.fprintf h "#define ABI_MESSAGE_NB %d\n\n" (size+1); - Printf.fprintf h "EXTERN abi_event* abi_queues[ABI_MESSAGE_NB];\n" + Printf.fprintf h "ABI_EXTERN abi_event* abi_queues[ABI_MESSAGE_NB];\n" (* Print arguments' function from fields *) let print_args = fun h fields starter -> From 3230e0a868ad7e4e28bc509c002006d068e63a84 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 23 Aug 2013 19:24:55 +0200 Subject: [PATCH 024/125] [baro] data are send and received with ABI (works on a navgo) --- sw/airborne/boards/navgo/baro_board.c | 38 ++++++++++++++++++--------- sw/airborne/boards/navgo/baro_board.h | 13 ++------- sw/airborne/subsystems/ins/ins_int.c | 28 ++++---------------- 3 files changed, 32 insertions(+), 47 deletions(-) diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index 898878070e..242d91069f 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -25,12 +25,20 @@ * Navarro & Gorraz & Hattenberger */ +#include "std.h" +#include "baro_board.h" #include "subsystems/sensors/baro.h" +#include "peripherals/mcp355x.h" +#include "subsystems/abi.h" #include "led.h" -#include "mcu_periph/spi.h" -/* Common Baro struct */ -struct Baro baro; +#ifndef NAVGO_BARO_SENS +#define NAVGO_BARO_SENS 0.0274181 +#endif + +#ifndef NAVGO_BARO_SENDER_ID +#define NAVGO_BARO_SENDER_ID 10 +#endif /* Counter to init mcp355x at startup */ #define BARO_STARTUP_COUNTER 200 @@ -38,9 +46,6 @@ uint16_t startup_cnt; void baro_init( void ) { mcp355x_init(); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; /* not handled on this board */ #ifdef ROTORCRAFT_BARO_LED LED_OFF(ROTORCRAFT_BARO_LED); #endif @@ -48,21 +53,28 @@ void baro_init( void ) { } void baro_periodic( void ) { - - if (baro.status == BS_UNINITIALIZED) { - // Run some loops to get correct readings from the adc + // Run some loops to get correct readings from the adc + if (startup_cnt > 0) { --startup_cnt; #ifdef ROTORCRAFT_BARO_LED LED_TOGGLE(ROTORCRAFT_BARO_LED); -#endif if (startup_cnt == 0) { - baro.status = BS_RUNNING; -#ifdef ROTORCRAFT_BARO_LED LED_ON(ROTORCRAFT_BARO_LED); -#endif } +#endif } // Read the ADC (at 50/4 Hz, conversion time is 68 ms) RunOnceEvery(4,mcp355x_read()); } +void navgo_baro_event(void) { + mcp355x_event(); + if (mcp355x_data_available) { + if (startup_cnt == 0) { + // Send data when init phase is done + uint32_t pressure = 10*NAVGO_BARO_SENS*mcp355x_data; + AbiSendMsgBARO_ABS(NAVGO_BARO_SENDER_ID, pressure); + } + mcp355x_data_available = FALSE; + } +} diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h index 56137a6806..c3bf00e475 100644 --- a/sw/airborne/boards/navgo/baro_board.h +++ b/sw/airborne/boards/navgo/baro_board.h @@ -29,17 +29,8 @@ #ifndef BOARDS_NAVGO_BARO_H #define BOARDS_NAVGO_BARO_H +extern void navgo_baro_event(void); -#include "std.h" -#include "peripherals/mcp355x.h" - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - mcp355x_event(); \ - if (mcp355x_data_available) { \ - baro.absolute = mcp355x_data; \ - _b_abs_handler(); \ - mcp355x_data_available = FALSE; \ - } \ -} +#define BaroEvent(_b_abs_handler, _b_diff_handler) navgo_baro_event() #endif // BOARDS_NAVGO_BARO_H diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index e1d031b71f..b62dcb7d7f 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -73,6 +73,9 @@ struct FloatVect2 ins_gps_speed_m_s_ned; /* barometer */ #if USE_VFF +/* Common Baro struct for telemetry*/ +struct Baro baro; + int32_t ins_qfe; bool_t ins_baro_initialised; int32_t ins_baro_alt; @@ -153,7 +156,7 @@ void ins_propagate() { #if USE_VFF float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (baro.status == BS_RUNNING && ins_baro_initialised) { + if (ins_baro_initialised) { vff_propagate(z_accel_meas_float); ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); @@ -180,6 +183,7 @@ void ins_propagate() { #if USE_VFF static void baro_cb(int32_t pressure) { + baro.absolute = pressure; // for BARO_RAW message if (!ins_baro_initialised) { ins_qfe = pressure; ins_baro_initialised = TRUE; @@ -201,28 +205,6 @@ static void baro_cb(int32_t pressure) { #endif void ins_update_baro() { -//#if USE_VFF -// if (baro.status == BS_RUNNING) { -// if (!ins_baro_initialised) { -// ins_qfe = baro.absolute; -// ins_baro_initialised = TRUE; -// } -// if (ins.vf_realign) { -// ins.vf_realign = FALSE; -// ins_qfe = baro.absolute; -// vff_realign(0.); -// ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); -// ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); -// ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); -// } -// else { /* not realigning, so normal update with baro measurement */ -// ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; -// float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); -// vff_update(alt_float); -// } -// } -// INS_NED_TO_STATE(); -//#endif } From 629dd68bad2b5be2db79689c4f8bad84714217c7 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 26 Aug 2013 16:06:27 +0200 Subject: [PATCH 025/125] [isa] atmosphere function all in float --- sw/airborne/math/pprz_isa.h | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h index af1e133035..eaa2514d4f 100644 --- a/sw/airborne/math/pprz_isa.h +++ b/sw/airborne/math/pprz_isa.h @@ -49,24 +49,33 @@ #define PPRZ_ISA_M_OF_P_CONST (PPRZ_ISA_AIR_GAS_CONSTANT*PPRZ_ISA_SEA_LEVEL_TEMP/PPRZ_ISA_GRAVITY) /** - * Get meters from pressure (using simplified equation) + * Get absolute altitude from pressure (using simplified equation). + * Referrence pressure is standard pressure at sea level * - * @param pressure pressure (float) in Pascal (Pa) + * @param pressure current pressure in Pascal (Pa) * @return altitude in pressure in ISA conditions */ -static inline float pprz_isa_meters_of_pressure_f(float pressure) { - return (PPRZ_ISA_M_OF_P_CONST*logf(PPRZ_ISA_SEA_LEVEL_PRESSURE/pressure)); +static inline float pprz_isa_altitude_of_pressure(float pressure) { + if (pressure > 0.) { + return (PPRZ_ISA_M_OF_P_CONST*logf(PPRZ_ISA_SEA_LEVEL_PRESSURE/pressure)); + } else { + return 0.; + } } /** - * Get meters from pressure (using simplified equation) + * Get relative altitude from pressure (using simplified equation). * - * @param pressure pressure (int) in deci Pascal (10*Pa) + * @param pressure current pressure in Pascal (Pa) + * @param ref reference pressure * @return altitude in pressure in ISA conditions */ -static inline float pprz_isa_meters_of_pressure_i(int32_t pressure) { - float p = (float)pressure/10.; - return pprz_isa_meters_of_pressure_f(p); +static inline float pprz_isa_height_of_pressure(float pressure, float ref) { + if (pressure > 0. && ref > 0.) { + return (PPRZ_ISA_M_OF_P_CONST*logf(ref/pressure)); + } else { + return 0.; + } } From 3604171f35b96f0883245c2781d9cae254cdf592 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 26 Aug 2013 16:07:13 +0200 Subject: [PATCH 026/125] [abi] pass sender id in the callback function --- sw/tools/gen_abi.ml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/sw/tools/gen_abi.ml b/sw/tools/gen_abi.ml index 042f2c24b1..d8b5010a58 100644 --- a/sw/tools/gen_abi.ml +++ b/sw/tools/gen_abi.ml @@ -95,14 +95,14 @@ module Gen_onboard = struct Printf.fprintf h "ABI_EXTERN abi_event* abi_queues[ABI_MESSAGE_NB];\n" (* Print arguments' function from fields *) - let print_args = fun h fields starter -> + let print_args = fun h fields -> let rec args = fun h l -> match l with [] -> Printf.fprintf h ")" - | [(n,t)] -> Printf.fprintf h "const %s %s)" t n - | (n,t)::l' -> Printf.fprintf h "const %s %s, " t n; args h l' + | [(n,t)] -> Printf.fprintf h ", const %s * %s)" t n + | (n,t)::l' -> Printf.fprintf h ", const %s * %s" t n; args h l' in - Printf.fprintf h "(%s" starter; + Printf.fprintf h "(uint8_t sender_id"; args h fields (* Print callbacks prototypes for all messages *) @@ -110,7 +110,7 @@ module Gen_onboard = struct Printf.fprintf h "\n/* Callbacks */\n"; List.iter (fun msg -> Printf.fprintf h "typedef void (*abi_callback%s)" (String.capitalize msg.name); - print_args h msg.fields ""; + print_args h msg.fields; Printf.fprintf h ";\n"; ) messages @@ -129,18 +129,18 @@ module Gen_onboard = struct let rec args = fun h l -> match l with [] -> Printf.fprintf h ");\n" - | [(n,_)] -> Printf.fprintf h "%s);\n" n - | (n,_)::l' -> Printf.fprintf h "%s, " n; args h l' + | [(n,_)] -> Printf.fprintf h ", %s);\n" n + | (n,_)::l' -> Printf.fprintf h ", %s" n; args h l' in let name = String.capitalize msg.name in Printf.fprintf h "\nstatic inline void AbiSendMsg%s" name; - print_args h msg.fields "uint8_t sender_id, "; + print_args h msg.fields; Printf.fprintf h " {\n"; Printf.fprintf h " abi_event* e;\n"; Printf.fprintf h " ABI_FOREACH(abi_queues[ABI_%s_ID],e) {\n" name; Printf.fprintf h " if (e->id == ABI_BROADCAST || e->id == sender_id) {\n"; Printf.fprintf h " abi_callback%s cb = (abi_callback%s)(e->cb);\n" name name; - Printf.fprintf h " cb("; + Printf.fprintf h " cb(sender_id"; args h msg.fields; Printf.fprintf h " };\n"; Printf.fprintf h " };\n"; From 82589c709ce73c71c6afa76b4621bb97cf4e2eb1 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 26 Aug 2013 16:09:23 +0200 Subject: [PATCH 027/125] [air_data] introduice air_data subsystem - split air data structure and baro interface - keep baro interface for integrated baro board - replace BOARD_HAS_BARO by USE_BARO_BOARD - add air data and baro_board to both firmwares --- conf/firmwares/rotorcraft.makefile | 6 +- .../subsystems/fixedwing/autopilot.makefile | 7 +++ .../subsystems/rotorcraft/fdm_jsbsim.makefile | 2 +- sw/airborne/firmwares/fixedwing/ap_downlink.h | 14 ++--- sw/airborne/firmwares/fixedwing/main_ap.c | 28 +++------- sw/airborne/firmwares/rotorcraft/main.c | 37 ++++++------ sw/airborne/firmwares/rotorcraft/telemetry.h | 10 ++-- sw/airborne/subsystems/air_data.c | 55 ++++++++++++++++++ sw/airborne/subsystems/air_data.h | 56 +++++++++++++++++++ sw/airborne/subsystems/sensors/baro.h | 32 +---------- 10 files changed, 162 insertions(+), 85 deletions(-) create mode 100644 sw/airborne/subsystems/air_data.c create mode 100644 sw/airborne/subsystems/air_data.h diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index bb42616a4b..c2810e09ea 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -147,8 +147,10 @@ ap.srcs += subsystems/actuators.c # # -# BARO +# AIR DATA and BARO (if needed) # +ap.srcs += subsystems/air_data.c + # booz baro ifeq ($(BOARD), booz) ap.srcs += $(SRC_BOARD)/baro_board.c @@ -215,7 +217,7 @@ ap.srcs += $(SRC_BOARD)/baro_board.c endif ifneq ($(BARO_LED),none) -ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) +ap.CFLAGS += -DBARO_LED=$(BARO_LED) endif # diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 3fef9171db..5136c28c4e 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -163,6 +163,9 @@ ap_srcs += state.c ap_srcs += subsystems/settings.c ap_srcs += $(SRC_ARCH)/subsystems/settings_arch.c +# AIR DATA +ap_srcs += subsystems/air_data.c + # BARO ifeq ($(BOARD), umarim) ifeq ($(BOARD_VERSION), 1.0) @@ -175,6 +178,10 @@ else ifeq ($(BOARD), lisa_l) ap_CFLAGS += -DUSE_I2C2 endif +ifneq ($(BARO_LED),none) +ap_CFLAGS += -DBARO_LED=$(BARO_LED) +endif + # ahrs frequencies if configured ifdef AHRS_PROPAGATE_FREQUENCY ap_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 6304c5a47e..6633e74466 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -92,7 +92,7 @@ nps.srcs += $(SRC_FIRMWARE)/datalink.c # nps.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c -nps.CFLAGS += -DROTORCRAFT_BARO_LED=2 +nps.CFLAGS += -DBARO_LED=2 nps.srcs += $(SRC_BOARD)/baro_board.c nps.CFLAGS += -DUSE_ADC diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index 76bdf38f2b..b78145d201 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -295,16 +295,12 @@ #define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {} #endif -#if USE_BAROMETER -#include "subsystems/sensors/baro.h" -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ - DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ - &baro.absolute, \ - &baro.differential); \ +#include "subsystems/air_data.h" +#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ + DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ + &air_data.pressure, \ + &air_data.differential); \ } -#else -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {} -#endif #ifdef MEASURE_AIRSPEED #define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 8c1e8bc7be..8f5266ef29 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -51,7 +51,8 @@ #if USE_AHRS_ALIGNER #include "subsystems/ahrs/ahrs_aligner.h" #endif -#if USE_BAROMETER +#include "subsystems/air_data.h" +#if USE_BARO_BOARD #include "subsystems/sensors/baro.h" #endif #include "subsystems/ins.h" @@ -135,11 +136,6 @@ volatile uint8_t ahrs_timeout_counter = 0; static inline void on_gps_solution( void ); #endif -#if USE_BAROMETER -static inline void on_baro_abs_event( void ); -static inline void on_baro_dif_event( void ); -#endif - // what version is this ???? static const uint16_t version = 1; @@ -192,7 +188,8 @@ void init_ap( void ) { ahrs_init(); #endif -#if USE_BAROMETER + air_data_init(); +#if USE_BARO_BOARD baro_init(); #endif @@ -579,7 +576,7 @@ void sensors_task( void ) { ahrs_propagate(); #endif -#if USE_BAROMETER +#if USE_BARO_BOARD baro_periodic(); #endif @@ -649,8 +646,8 @@ void event_task_ap( void ) { GpsEvent(on_gps_solution); #endif /* USE_GPS */ -#if USE_BAROMETER - BaroEvent(on_baro_abs_event, on_baro_dif_event); +#if USE_BARO_BOARD + BaroEvent(); #endif DatalinkEvent(); @@ -780,14 +777,3 @@ static inline void on_mag_event(void) #endif // USE_AHRS -#if USE_BAROMETER - -static inline void on_baro_abs_event( void ) { - ins_update_baro(); -} - -static inline void on_baro_dif_event( void ) { - -} - -#endif // USE_BAROMETER diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index e57f41b3a0..a7710e5f00 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -52,8 +52,11 @@ #include "subsystems/imu.h" #include "subsystems/gps.h" +#include "subsystems/air_data.h" +#if USE_BARO_BOARD #include "subsystems/sensors/baro.h" +#endif #include "subsystems/electrical.h" @@ -98,8 +101,6 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) static inline void on_gyro_event( void ); static inline void on_accel_event( void ); -static inline void on_baro_abs_event( void ); -static inline void on_baro_dif_event( void ); static inline void on_gps_event( void ); static inline void on_mag_event( void ); @@ -109,8 +110,10 @@ tid_t modules_tid; ///< id for modules_periodic_task() timer tid_t failsafe_tid; ///< id for failsafe_check() timer tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer tid_t electrical_tid; ///< id for electrical_periodic() timer -tid_t baro_tid; ///< id for baro_periodic() timer tid_t telemetry_tid; ///< id for telemetry_periodic() timer +#if USE_BARO_BOARD +tid_t baro_tid; ///< id for baro_periodic() timer +#endif #ifndef SITL int main( void ) { @@ -139,7 +142,10 @@ STATIC_INLINE void main_init( void ) { radio_control_init(); + air_data_init(); +#if USE_BARO_BOARD baro_init(); +#endif imu_init(); #if USE_IMU_FLOAT imu_float_init(); @@ -175,8 +181,10 @@ STATIC_INLINE void main_init( void ) { radio_control_tid = sys_time_register_timer((1./60.), NULL); failsafe_tid = sys_time_register_timer(0.05, NULL); electrical_tid = sys_time_register_timer(0.1, NULL); - baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL); +#if USE_BARO_BOARD + baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); +#endif } STATIC_INLINE void handle_periodic_tasks( void ) { @@ -190,10 +198,12 @@ STATIC_INLINE void handle_periodic_tasks( void ) { failsafe_check(); if (sys_time_check_and_ack_timer(electrical_tid)) electrical_periodic(); - if (sys_time_check_and_ack_timer(baro_tid)) - baro_periodic(); if (sys_time_check_and_ack_timer(telemetry_tid)) telemetry_periodic(); +#if USE_BARO_BOARD + if (sys_time_check_and_ack_timer(baro_tid)) + baro_periodic(); +#endif } STATIC_INLINE void main_periodic( void ) { @@ -250,7 +260,9 @@ STATIC_INLINE void main_event( void ) { ImuEvent(on_gyro_event, on_accel_event, on_mag_event); - BaroEvent(on_baro_abs_event, on_baro_dif_event); +#if USE_BARO_BOARD + BaroEvent(); +#endif #if USE_GPS GpsEvent(on_gps_event); @@ -293,17 +305,6 @@ static inline void on_gyro_event( void ) { #endif } -static inline void on_baro_abs_event( void ) { - ins_update_baro(); -#ifdef USE_VEHICLE_INTERFACE - vi_notify_baro_abs_available(); -#endif -} - -static inline void on_baro_dif_event( void ) { - -} - static inline void on_gps_event(void) { ahrs_update_gps(); ins_update_gps(); diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 4522d049b4..b81f212895 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -190,11 +190,11 @@ &electrical.current); \ } -#include "subsystems/sensors/baro.h" -#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ - DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ - &baro.absolute, \ - &baro.differential); \ +#include "subsystems/air_data.h" +#define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ + DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ + &air_data.pressure, \ + &air_data.differential); \ } diff --git a/sw/airborne/subsystems/air_data.c b/sw/airborne/subsystems/air_data.c new file mode 100644 index 0000000000..e7cd6b068f --- /dev/null +++ b/sw/airborne/subsystems/air_data.c @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/air_data.c + * Air Data interface + * - pressures + * - airspeed + * - angle of attack and sideslip + * - wind + */ + +#include "subsystems/air_data.h" +#include "subsystems/abi.h" + +/** global AirData state + */ +struct AirData air_data; + +/** ABI bindings + */ +#ifndef AIR_DATA_BARO_ABS_ID +#define AIR_DATA_BARO_ABS_ID ABI_BROADCAST +#endif +static abi_event pressure_abs_ev; + +static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float * pressure) { + air_data.pressure = *pressure; +} + +/** AirData initialization. Called at startup. + * Bind ABI messages + */ +void air_data_init( void ) { + AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb); +} + diff --git a/sw/airborne/subsystems/air_data.h b/sw/airborne/subsystems/air_data.h new file mode 100644 index 0000000000..b4963c4832 --- /dev/null +++ b/sw/airborne/subsystems/air_data.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/air_data.h + * Air Data interface + * - pressures + * - airspeed + * - angle of attack and sideslip + * - wind + */ + +#ifndef AIR_DATA_H +#define AIR_DATA_H + +#include "std.h" + +/** Air Data strucute */ +struct AirData { + float pressure; ///< Static atmospheric pressure (Pa) + float differential; ///< Differential pressure (dynamic - static pressure) (Pa) + float airspeed; ///< Conventional Air Speed (m/s) + float aoa; ///< angle of attack (rad) + float sideslip; ///< sideslip angle (rad) + float wind_speed; ///< wind speed (m/s) + float wind_dir; ///< wind direction (rad, 0 north, >0 clockwise) +}; + +/** global AirData state + */ +extern struct AirData air_data; + +/** AirData initialization. Called at startup. + */ +extern void air_data_init( void ); + +#endif /* AIR_DATA_H */ + diff --git a/sw/airborne/subsystems/sensors/baro.h b/sw/airborne/subsystems/sensors/baro.h index 11d46d041b..643da7bbef 100644 --- a/sw/airborne/subsystems/sensors/baro.h +++ b/sw/airborne/subsystems/sensors/baro.h @@ -23,42 +23,16 @@ * @file subsystems/sensors/baro.h * * Common barometric sensor implementation. + * Used with baro integrated to the autopilot board. + * Implementation is in boards//baro_board.[ch] * */ #ifndef SUBSYSTEMS_SENSORS_BARO_H #define SUBSYSTEMS_SENSORS_BARO_H -#include - -/* - * Status of the baro - */ -enum BaroStatus { - BS_UNINITIALIZED, - BS_RUNNING -}; - -/* - * Baro structure - * - * pressures are in deciPa - * - * vertical resolution is around 1cm at sea level - * with standard atmospheric model - * - */ -struct Baro { - int32_t absolute; - int32_t differential; - enum BaroStatus status; -}; - -extern struct Baro baro; - - #include BOARD_CONFIG -#if defined BOARD_HAS_BARO +#if USE_BARO_BOARD #include "baro_board.h" #endif From 046ce69f2b93c7991cacf39a2a92b3b7768e2ac9 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 26 Aug 2013 16:46:35 +0200 Subject: [PATCH 028/125] [ins/baro] pressure in float in ABI messages --- conf/messages.xml | 4 +- sw/airborne/subsystems/ins/ins_int.c | 24 +++--- sw/airborne/subsystems/ins/ins_int.h | 4 +- sw/airborne/subsystems/ins/ins_int_extended.c | 79 +++++++++++-------- sw/airborne/subsystems/ins/ins_int_extended.h | 4 +- 5 files changed, 60 insertions(+), 55 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 4d199623fb..deb8a2b98d 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2555,11 +2555,11 @@ - + - + diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index b62dcb7d7f..4e10c36e24 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -31,7 +31,6 @@ #include "subsystems/abi.h" #include "subsystems/imu.h" -#include "subsystems/sensors/baro.h" #include "subsystems/gps.h" #include "generated/airframe.h" @@ -49,7 +48,6 @@ #include #endif - #include "math/pprz_geodetic_int.h" #include "math/pprz_isa.h" @@ -73,17 +71,14 @@ struct FloatVect2 ins_gps_speed_m_s_ned; /* barometer */ #if USE_VFF -/* Common Baro struct for telemetry*/ -struct Baro baro; - -int32_t ins_qfe; -bool_t ins_baro_initialised; -int32_t ins_baro_alt; +bool_t ins_baro_initialised; +float ins_qfe; +float ins_baro_alt; #ifndef INS_BARO_ID #define INS_BARO_ID ABI_BROADCAST #endif abi_event baro_ev; -static void baro_cb(const int32_t pressure); +static void baro_cb(uint8_t sender_id, const float *pressure); #endif /* output */ @@ -182,23 +177,22 @@ void ins_propagate() { } #if USE_VFF -static void baro_cb(int32_t pressure) { - baro.absolute = pressure; // for BARO_RAW message +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { if (!ins_baro_initialised) { - ins_qfe = pressure; + ins_qfe = *pressure; ins_baro_initialised = TRUE; } if (ins.vf_realign) { ins.vf_realign = FALSE; - ins_qfe = pressure; + ins_qfe = *pressure; vff_realign(0.); ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { - float alt = pprz_isa_meters_of_pressure_i(pressure); - vff_update(alt); + ins_baro_alt = pprz_isa_height_of_pressure(*pressure, ins_qfe); + vff_update(ins_baro_alt); } INS_NED_TO_STATE(); } diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index c72c805c52..c9a861be8b 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -50,8 +50,8 @@ extern struct NedCoor_i ins_gps_speed_cm_s_ned; /* barometer */ #if USE_VFF -extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC -extern int32_t ins_qfe; +extern float ins_baro_alt; ///< altitude calculated from baro in meters +extern float ins_qfe; extern bool_t ins_baro_initialised; #endif diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c index aefa07f10f..9c6677bd12 100644 --- a/sw/airborne/subsystems/ins/ins_int_extended.c +++ b/sw/airborne/subsystems/ins/ins_int_extended.c @@ -29,17 +29,21 @@ #include "subsystems/ins/ins_int_extended.h" +#include "subsystems/abi.h" + #include "subsystems/imu.h" -#include "subsystems/sensors/baro.h" #include "subsystems/gps.h" #include "generated/airframe.h" #include "math/pprz_algebra_int.h" #include "math/pprz_algebra_float.h" +#include "math/pprz_geodetic_int.h" +#include "math/pprz_isa.h" #include "state.h" #include "subsystems/ins/vf_extended_float.h" +#include "filters/median_filter.h" #if USE_HFF #include "subsystems/ins/hf_float.h" @@ -54,8 +58,6 @@ #include "firmwares/rotorcraft/stabilization.h" #endif -#include "math/pprz_geodetic_int.h" - #include "generated/flight_plan.h" #ifndef USE_INS_NAV_INIT @@ -76,11 +78,17 @@ struct FloatVect2 ins_gps_speed_m_s_ned; #endif /* barometer */ -int32_t ins_qfe; -bool_t ins_baro_initialised; -int32_t ins_baro_alt; -#include "filters/median_filter.h" -struct MedianFilterInt baro_median; +bool_t ins_baro_initialised; +float ins_qfe; +float ins_baro_alt; +#ifndef INS_BARO_ID +#define INS_BARO_ID ABI_BROADCAST +#endif +abi_event baro_ev; +static void baro_cb(uint8_t sender_id, const float *pressure); + +// if median filter really needed, implement in float first +//struct MedianFilterInt baro_median; #if USE_SONAR /* sonar */ @@ -122,8 +130,10 @@ void ins_init() { ins_ltp_initialised = FALSE; #endif + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); ins_baro_initialised = FALSE; - init_median_filter(&baro_median); + //init_median_filter(&baro_median); #if USE_SONAR ins_update_on_agl = FALSE; @@ -166,7 +176,7 @@ void ins_propagate() { INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, *stateGetNedToBodyRMat_i(), accel_meas_body); float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (baro.status == BS_RUNNING && ins_baro_initialised) { + if (ins_baro_initialised) { vff_propagate(z_accel_meas_float); ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); @@ -188,30 +198,31 @@ void ins_propagate() { INS_NED_TO_STATE(); } -void ins_update_baro() { - int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute); - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro_pressure; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = baro_pressure; - vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { /* not realigning, so normal update with baro measurement */ - ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN; - float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt); - vff_update_baro(alt_float); - } +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { + // if median filter really needed, implement in float first + //int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute); + if (!ins_baro_initialised) { + ins_qfe = *pressure; + ins_baro_initialised = TRUE; + } + if (ins.vf_realign) { + ins.vf_realign = FALSE; + ins_qfe = *pressure; + vff_realign(0.); + ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + } + else { + ins_baro_alt = pprz_isa_height_of_pressure(*pressure, ins_qfe); + vff_update_baro(ins_baro_alt); } INS_NED_TO_STATE(); } +void ins_update_baro() { +} + void ins_update_gps(void) { #if USE_GPS @@ -279,8 +290,8 @@ void ins_update_sonar() { #ifdef INS_SONAR_VARIANCE_THRESHOLD /* compute variance of error between sonar and baro alt */ - int32_t err = POS_BFP_OF_REAL(sonar) + ins_baro_alt; // sonar positive up, baro positive down !!!! - var_err[var_idx] = POS_FLOAT_OF_BFP(err); + float err = sonar + ins_baro_alt; // sonar positive up, baro positive down !!!! + var_err[var_idx] = err; var_idx = (var_idx + 1) % VAR_ERR_MAX; float var = variance_float(var_err, VAR_ERR_MAX); DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); @@ -304,13 +315,13 @@ void ins_update_sonar() { && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD #endif #ifdef INS_SONAR_BARO_THRESHOLD - && ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */ + && ins_baro_alt > -INS_SONAR_BARO_THRESHOLD /* z down */ #endif #ifdef INS_SONAR_VARIANCE_THRESHOLD && var < INS_SONAR_VARIANCE_THRESHOLD #endif && ins_update_on_agl - && baro.status == BS_RUNNING) { + && ins_baro_initialised) { vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); last_offset = vff_offset; } diff --git a/sw/airborne/subsystems/ins/ins_int_extended.h b/sw/airborne/subsystems/ins/ins_int_extended.h index 6d0ae9b2ec..64d63cd419 100644 --- a/sw/airborne/subsystems/ins/ins_int_extended.h +++ b/sw/airborne/subsystems/ins/ins_int_extended.h @@ -54,8 +54,8 @@ extern struct NedCoor_i ins_gps_pos_cm_ned; extern struct NedCoor_i ins_gps_speed_cm_s_ned; /* barometer */ -extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC -extern int32_t ins_qfe; +extern float ins_baro_alt; ///< altitude calculated from baro in meters +extern float ins_qfe; extern bool_t ins_baro_initialised; #if USE_SONAR extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ From 4970368a2c2acf318ff97d08cbaaa48e31a92660 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 26 Aug 2013 16:58:42 +0200 Subject: [PATCH 029/125] [baro] update baro_board files for ABI - except most of the lisa boards since I'm waiting for another work to be merged into master --- sw/airborne/boards/apogee/baro_board.c | 59 +++++++++++++++------ sw/airborne/boards/apogee/baro_board.h | 29 +--------- sw/airborne/boards/apogee_0.99.h | 2 +- sw/airborne/boards/apogee_1.0.h | 2 +- sw/airborne/boards/ardrone/baro_board.h | 2 +- sw/airborne/boards/ardrone2_raw.h | 2 +- sw/airborne/boards/ardrone2_sdk.h | 2 +- sw/airborne/boards/booz/baro_board.c | 57 +++++++++++--------- sw/airborne/boards/booz/baro_board.h | 42 ++++++++++++--- sw/airborne/boards/booz_1.0.h | 2 +- sw/airborne/boards/krooz_sd.h | 2 +- sw/airborne/boards/lia_1.1.h | 2 +- sw/airborne/boards/lisa_l_1.0.h | 2 +- sw/airborne/boards/lisa_m/baro_ms5611_i2c.c | 4 +- sw/airborne/boards/lisa_m/baro_ms5611_spi.c | 4 +- sw/airborne/boards/lisa_m_1.0.h | 2 +- sw/airborne/boards/lisa_m_2.0.h | 2 +- sw/airborne/boards/navgo/baro_board.c | 15 +++--- sw/airborne/boards/navgo/baro_board.h | 5 +- sw/airborne/boards/navgo_1.0.h | 2 +- sw/airborne/boards/pc/baro_board.c | 42 ++++++++++++--- sw/airborne/boards/pc/baro_board.h | 34 ++++++++---- sw/airborne/boards/pc_sim.h | 2 +- sw/airborne/boards/umarim/baro_board.c | 48 +++++++++++++---- sw/airborne/boards/umarim/baro_board.h | 52 ++---------------- sw/airborne/boards/umarim_1.0.h | 2 +- 26 files changed, 241 insertions(+), 178 deletions(-) diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index cfabf3b2a5..7ee3a0adc8 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 ENAC + * Copyright (C) 2013 Gautier Hattenberger (ENAC) * * This file is part of paparazzi. * @@ -26,14 +26,25 @@ * integrated barometer for Apogee boards (mpl3115) */ +#include "std.h" #include "subsystems/sensors/baro.h" +#include "peripherals/mpl3115.h" // to get MPU status #include "boards/apogee/imu_apogee.h" +#include "subsystems/abi.h" +#include "led.h" -/* Common Baro struct */ -struct Baro baro; + +// FIXME +#ifndef APOGEE_BARO_SENS +#define APOGEE_BARO_SENS 0.0274181 +#endif + +#ifndef APOGEE_BARO_SENDER_ID +#define APOGEE_BARO_SENDER_ID 12 +#endif /** Counter to init ads1114 at startup */ #define BARO_STARTUP_COUNTER 200 @@ -41,25 +52,41 @@ uint16_t startup_cnt; void baro_init( void ) { mpl3115_init(); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 1; /* not handled on this board, use extra module */ +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { - if (baro.status == BS_UNINITIALIZED && mpl3115_data_available) { - // Run some loops to get correct readings from the adc - --startup_cnt; - mpl3115_data_available = FALSE; - if (startup_cnt == 0) { - baro.status = BS_RUNNING; - } - } - // Baro is slave of the MPU, only start reading it after MPU is configured - if (imu_apogee.mpu.config.initialized) + if (imu_apogee.mpu.config.initialized) { + + if (startup_cnt > 0 && mpl3115_data_available) { + // Run some loops to get correct readings from the adc + --startup_cnt; + mpl3115_data_available = FALSE; +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); + if (startup_cnt == 0) { + LED_ON(BARO_LED); + } +#endif + } + // Read the sensor Mpl3115Periodic(); + } +} + +void apogee_baro_event(void) { + mpl3115_event(); + if (mpl3115_data_available) { + if (startup_cnt == 0) { + float pressure = ((float)mpl3115_pressure/(1<<2)); + AbiSendMsgBARO_ABS(UMARIM_BARO_SENDER_ID, &pressure); + } + mpl3115_data_available = FALSE; + } } diff --git a/sw/airborne/boards/apogee/baro_board.h b/sw/airborne/boards/apogee/baro_board.h index c820fd1cb1..595a8a90cb 100644 --- a/sw/airborne/boards/apogee/baro_board.h +++ b/sw/airborne/boards/apogee/baro_board.h @@ -29,32 +29,7 @@ #ifndef BOARDS_APOGEE_BARO_H #define BOARDS_APOGEE_BARO_H -#include "std.h" -#include "peripherals/mpl3115.h" - -/* There is no differential pressure on the board but - * it can be available from an external sensor - * */ - -#define BaroAbs(_handler) { \ - mpl3115_event(); \ - if (mpl3115_data_available) { \ - baro.absolute = mpl3115_pressure; \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - mpl3115_data_available = FALSE; \ - } \ - } \ -} - -// TODO handle baro diff -#ifndef BaroDiff -#define BaroDiff(_h) {} -#endif - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - BaroAbs(_b_abs_handler); \ - BaroDiff(_b_diff_handler); \ -} +extern void apogee_baro_event(void); +#define BaroEvent apogee_baro_event #endif // BOARDS_APOGEE_BARO_H diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h index 8a79bdc24b..2ea2d92b4d 100644 --- a/sw/airborne/boards/apogee_0.99.h +++ b/sw/airborne/boards/apogee_0.99.h @@ -184,7 +184,7 @@ //#define SPI_SELECT_SLAVE5_PIN GPIO4 /* Activate onboard baro */ -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 /* PWM */ #define PWM_USE_TIM2 1 diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h index a9ca6af6c5..738abf446c 100644 --- a/sw/airborne/boards/apogee_1.0.h +++ b/sw/airborne/boards/apogee_1.0.h @@ -254,7 +254,7 @@ /* Activate onboard baro */ -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 /* PWM */ #define PWM_USE_TIM2 1 diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h index d98a771108..0b6a1b454e 100644 --- a/sw/airborne/boards/ardrone/baro_board.h +++ b/sw/airborne/boards/ardrone/baro_board.h @@ -30,7 +30,7 @@ #ifndef BOARDS_ARDRONE2_BARO_H #define BOARDS_ARDRONE2_BARO_H -#if BOARD_HAS_BARO +#if USE_BARO_BOARD #include "navdata.h" void process_ardrone_baro(void); diff --git a/sw/airborne/boards/ardrone2_raw.h b/sw/airborne/boards/ardrone2_raw.h index 6200042285..30024b58d5 100644 --- a/sw/airborne/boards/ardrone2_raw.h +++ b/sw/airborne/boards/ardrone2_raw.h @@ -9,6 +9,6 @@ #define ActuatorsDefaultInit() ActuatorsArdroneInit() #define ActuatorsDefaultCommit() ActuatorsArdroneCommit() -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_ARDRONE2_RAW */ diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h index 01bb97452d..923cc22037 100644 --- a/sw/airborne/boards/ardrone2_sdk.h +++ b/sw/airborne/boards/ardrone2_sdk.h @@ -15,6 +15,6 @@ #define ActuatorsDefaultInit() {} #define ActuatorsDefaultCommit() {} -#define BOARD_HAS_BARO 0 +#define USE_BARO_BOARD 0 #endif /* CONFIG_ARDRONE2_SDK */ diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index cfcbc8b5b2..9641f18144 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -1,5 +1,4 @@ -/* $Id$ - * +/* * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. @@ -20,48 +19,59 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file boards/booz/baro_board.c + * + */ + #include "subsystems/sensors/baro.h" #include "generated/airframe.h" +#include "subsystems/abi.h" #include "led.h" +#ifndef BOOZ_BARO_SENDER_ID +#define BOOZ_BARO_SENDER_ID 1 +#endif + /* threshold >0 && <1023 */ #ifndef BOOZ_ANALOG_BARO_THRESHOLD #define BOOZ_ANALOG_BARO_THRESHOLD 850 #endif -struct Baro baro; -struct BaroBoard baro_board; +// FIXME correct scale +#ifndef BOOZ_BARO_SENS +#define BOOZ_BARO_SENS 1.0 +#endif +struct BaroBoard baro_board; void baro_init( void ) { adc_buf_channel(ADC_CHANNEL_BARO, &baro_board.buf, DEFAULT_AV_NB_SAMPLE); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; /* not handled on this board */ - + baro_board.status = BB_UNINITIALIZED; + baro_board.absolute = 0; baro_board.offset = 1023; DACSet(baro_board.offset); - baro_board.value_filtered = 0; - baro_board.data_available = FALSE; -#ifdef ROTORCRAFT_BARO_LED - LED_OFF(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_OFF(BARO_LED); #endif } void baro_periodic(void) { - baro.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; - baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; - if (baro.status == BS_UNINITIALIZED) { + baro_board.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; + baro_board.value_filtered = (3*baro_board.value_filtered + baro_board.absolute)/4; + if (baro_board.status == BB_UNINITIALIZED) { RunOnceEvery(10, { baro_board_calibrate();}); } - /* else */ - baro_board.data_available = TRUE; + else { + float pressure = BOOZ_BARO_SENS*baro_board.absolute; + AbiSendMsgBARO_ABS(BOOZ_BARO_SENDER_ID, &pressure); + } } /* decrement offset until adc reading is over a threshold */ @@ -72,18 +82,15 @@ void baro_board_calibrate(void) { else baro_board.offset--; DACSet(baro_board.offset); -#ifdef ROTORCRAFT_BARO_LED - LED_TOGGLE(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); #endif } else { - baro.status = BS_RUNNING; -#ifdef ROTORCRAFT_BARO_LED - LED_ON(ROTORCRAFT_BARO_LED); + baro_board.status = BB_RUNNING; +#ifdef BARO_LED + LED_ON(BARO_LED); #endif } } - - - diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h index 902aef2db1..f47abe700b 100644 --- a/sw/airborne/boards/booz/baro_board.h +++ b/sw/airborne/boards/booz/baro_board.h @@ -1,3 +1,30 @@ + /* + * Copyright (C) 2010-2013 Antoine Drouin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** + * @file boards/booz/baro_board.h + * + */ + #ifndef BOARDS_BOOZ_BARO_H #define BOARDS_BOOZ_BARO_H @@ -7,24 +34,23 @@ #include "mcu_periph/adc.h" #include "mcu_periph/dac.h" +enum BaroBoardStatus { + BB_UNINITIALIZED, + BB_RUNNING +}; struct BaroBoard { + enum BaroBoardStatus status; + int32_t absolute; uint16_t offset; uint16_t value_filtered; - bool_t data_available; struct adc_buf buf; }; extern struct BaroBoard baro_board; extern void baro_board_calibrate(void); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.data_available) { \ - _b_abs_handler(); \ - baro_board.data_available = FALSE; \ - } \ - } +#define BaroEvent() {} static inline void baro_board_SetOffset(uint16_t _o) { baro_board.offset = _o; diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 1bb2007977..5dc6479522 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -182,6 +182,6 @@ #endif -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_BOOZ2_V1_0_H */ diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 1ecd270753..289d763230 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -225,7 +225,7 @@ #endif // USE_AD1 /* Activate onboard baro */ -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 /* PWM */ #define PWM_USE_TIM3 1 diff --git a/sw/airborne/boards/lia_1.1.h b/sw/airborne/boards/lia_1.1.h index 3b9c1bf419..31260f054d 100644 --- a/sw/airborne/boards/lia_1.1.h +++ b/sw/airborne/boards/lia_1.1.h @@ -166,7 +166,7 @@ // FIXME, using baro_board right now to include the appropriate header -#define BOARD_HAS_BARO 0 +#define USE_BARO_BOARD 0 #endif /* CONFIG_LIA_1_1_H */ diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index f18b7d34eb..1c109b84ce 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -156,7 +156,7 @@ #define BOARD_ADC_CHANNEL_3 0 #define BOARD_ADC_CHANNEL_4 15 -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 /* Default actuators driver */ diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c b/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c index 10994d5948..781e0853ae 100644 --- a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c +++ b/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c @@ -106,8 +106,8 @@ void baro_periodic(void) { void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){ if (ms5611_trans.status == I2CTransSuccess) { - #ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); + #ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); #endif switch (ms5611_status) { diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c b/sw/airborne/boards/lisa_m/baro_ms5611_spi.c index bd437cd318..4d7e6087fd 100644 --- a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c +++ b/sw/airborne/boards/lisa_m/baro_ms5611_spi.c @@ -55,8 +55,8 @@ static int8_t baro_ms5611_crc(uint16_t* prom) { } static void trans_cb_ms5611( struct spi_transaction *trans ) { -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); #endif } diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index a2b1b2e88d..e683048f29 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -120,6 +120,6 @@ } #endif // USE_AD1 -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_LISA_M_1_0_H */ diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h index 3f09b51a8d..1885f3eda9 100644 --- a/sw/airborne/boards/lisa_m_2.0.h +++ b/sw/airborne/boards/lisa_m_2.0.h @@ -165,6 +165,6 @@ #endif // USE_AD1 -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_LISA_M_2_0_H */ diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index 242d91069f..e1b03bad42 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -32,6 +32,7 @@ #include "subsystems/abi.h" #include "led.h" +// FIXME #ifndef NAVGO_BARO_SENS #define NAVGO_BARO_SENS 0.0274181 #endif @@ -46,8 +47,8 @@ uint16_t startup_cnt; void baro_init( void ) { mcp355x_init(); -#ifdef ROTORCRAFT_BARO_LED - LED_OFF(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_OFF(BARO_LED); #endif startup_cnt = BARO_STARTUP_COUNTER; } @@ -56,10 +57,10 @@ void baro_periodic( void ) { // Run some loops to get correct readings from the adc if (startup_cnt > 0) { --startup_cnt; -#ifdef ROTORCRAFT_BARO_LED - LED_TOGGLE(ROTORCRAFT_BARO_LED); +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); if (startup_cnt == 0) { - LED_ON(ROTORCRAFT_BARO_LED); + LED_ON(BARO_LED); } #endif } @@ -72,8 +73,8 @@ void navgo_baro_event(void) { if (mcp355x_data_available) { if (startup_cnt == 0) { // Send data when init phase is done - uint32_t pressure = 10*NAVGO_BARO_SENS*mcp355x_data; - AbiSendMsgBARO_ABS(NAVGO_BARO_SENDER_ID, pressure); + float pressure = NAVGO_BARO_SENS*mcp355x_data; + AbiSendMsgBARO_ABS(NAVGO_BARO_SENDER_ID, &pressure); } mcp355x_data_available = FALSE; } diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h index c3bf00e475..bbd56ba3f7 100644 --- a/sw/airborne/boards/navgo/baro_board.h +++ b/sw/airborne/boards/navgo/baro_board.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2012 Gautier Hattenberger + * Copyright (C) 2011-2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -31,6 +31,7 @@ extern void navgo_baro_event(void); -#define BaroEvent(_b_abs_handler, _b_diff_handler) navgo_baro_event() +// define BaroEvent macro +#define BaroEvent navgo_baro_event #endif // BOARDS_NAVGO_BARO_H diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h index 015b51d428..0f9c922d29 100644 --- a/sw/airborne/boards/navgo_1.0.h +++ b/sw/airborne/boards/navgo_1.0.h @@ -105,6 +105,6 @@ #define SERVO_REG_1 PWMMR5 #endif -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_NAVGO_V1_0_H */ diff --git a/sw/airborne/boards/pc/baro_board.c b/sw/airborne/boards/pc/baro_board.c index 52f3cb19fe..6c87012d77 100644 --- a/sw/airborne/boards/pc/baro_board.c +++ b/sw/airborne/boards/pc/baro_board.c @@ -1,17 +1,45 @@ +/* + * Copyright (C) 2010-2013 Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ +/** + * @file boards/pc/baro_board.h + * + * board specific fonction for the PC board ( simulator ) + * + */ #include "subsystems/sensors/baro.h" +#include "subsystems/abi.h" -struct Baro baro; +#ifndef SIM_BARO_SENDER_ID +#define SIM_BARO_SENDER_ID 4 +#endif -bool_t baro_pc_available; +void baro_init(void) {} -void baro_init(void) {baro_pc_available=FALSE;} - -void baro_periodic(void) {baro.status = BS_RUNNING;} +void baro_periodic(void) {} void baro_feed_value(double value) { - baro.absolute = (int32_t) value; - baro_pc_available = TRUE; + float pressure = (float) value; + AbiSendMsgBARO_ABS(SIM_BARO_SENDER_ID, &pressure); } diff --git a/sw/airborne/boards/pc/baro_board.h b/sw/airborne/boards/pc/baro_board.h index 54d902730b..e9ede28b60 100644 --- a/sw/airborne/boards/pc/baro_board.h +++ b/sw/airborne/boards/pc/baro_board.h @@ -1,4 +1,28 @@ /* + * Copyright (C) 2010-2013 Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/* + * @file boards/pc/baro_board.c + * * board specific fonction for the PC board ( simulator ) * */ @@ -6,15 +30,7 @@ #ifndef BOARDS_PC_BARO_H #define BOARDS_PC_BARO_H -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_pc_available) { \ - _b_abs_handler(); \ - baro_pc_available = FALSE; \ - } \ - } - - -extern bool_t baro_pc_available; +#define BaroEvent() {} extern void baro_feed_value(double value); diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index 2b80d2f632..c15970eb21 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -12,6 +12,6 @@ #define DefaultVoltageOfAdc(adc) (1.0*adc) -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_PC_SIM_H */ diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index c83675d17b..8790c2bc28 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 ENAC + * Copyright (C) 2010-2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -25,11 +25,26 @@ * Navarro & Gorraz & Hattenberger */ +#include "std.h" +#include "baro_board.h" #include "subsystems/sensors/baro.h" +#include "peripherals/ads1114.h" +#include "subsystems/abi.h" +#include "led.h" +// ADC for absolute pressure +#define BARO_ABS_ADS +#define BARO_ABS_ADS ads1114_1 +#endif -/* Common Baro struct */ -struct Baro baro; +// FIXME +#ifndef UMARIM_BARO_SENS +#define UMARIM_BARO_SENS 0.0274181 +#endif + +#ifndef UMARIM_BARO_SENDER_ID +#define UMARIM_BARO_SENDER_ID 11 +#endif /* Counter to init ads1114 at startup */ #define BARO_STARTUP_COUNTER 200 @@ -37,23 +52,36 @@ uint16_t startup_cnt; void baro_init( void ) { ads1114_init(); - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; /* not handled on this board, use extra module (ex: airspeed_ads1114) */ +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif startup_cnt = BARO_STARTUP_COUNTER; } void baro_periodic( void ) { - if (baro.status == BS_UNINITIALIZED && BARO_ABS_ADS.data_available) { - // Run some loops to get correct readings from the adc + // Run some loops to get correct readings from the adc + if (startup_cnt > 0) { --startup_cnt; - BARO_ABS_ADS.data_available = FALSE; +#ifdef BARO_LED + LED_TOGGLE(BARO_LED); if (startup_cnt == 0) { - baro.status = BS_RUNNING; + LED_ON(BARO_LED); } +#endif } // Read the ADC ads1114_read(&BARO_ABS_ADS); } +void umarim_baro_event(void) { + Ads1114Event(); + if (BARO_ABS_ADS.data_available) { + if (startup_cnt == 0) { + float pressure = UMARIM_BARO_SENS*Ads1114GetValue(BARO_ABS_ADS); + AbiSendMsgBARO_ABS(UMARIM_BARO_SENDER_ID, &pressure); + } + BARO_ABS_ADS.data_available = FALSE; + } +} + diff --git a/sw/airborne/boards/umarim/baro_board.h b/sw/airborne/boards/umarim/baro_board.h index a296e6a28c..b340417909 100644 --- a/sw/airborne/boards/umarim/baro_board.h +++ b/sw/airborne/boards/umarim/baro_board.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010-2012 Gautier Hattenberger + * Copyright (C) 2010-2013 Gautier Hattenberger * * This file is part of paparazzi. * @@ -29,53 +29,7 @@ #ifndef BOARDS_UMARIM_BARO_H #define BOARDS_UMARIM_BARO_H - -#include "std.h" -#include "peripherals/ads1114.h" - -/* There is no differential pressure on the board but - * it can be available from an external sensor - * */ - -#define BARO_ABS_ADS ads1114_1 - -#define BaroAbs(_ads, _handler) { \ - if (_ads.data_available) { \ - baro.absolute = Ads1114GetValue(_ads); \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - _ads.data_available = FALSE; \ - } \ - } \ -} - -#ifndef BaroDiff // Allow custom redefinition ? - -#if USE_BARO_DIFF - -#ifndef BARO_DIFF_ADS -#define BARO_DIFF_ADS ads1114_2 -#endif -#define BaroDiff(_ads, _handler) { \ - if (_ads.data_available) { \ - baro.differential = Ads1114GetValue(_ads); \ - if (baro.status == BS_RUNNING) { \ - _handler(); \ - _ads.data_available = FALSE; \ - } \ - } \ -} - -#else // Not using differential with ADS1114 -#define BaroDiff(_a, _h) {} -#endif - -#endif // ifndef BaroDiff - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - Ads1114Event(); \ - BaroAbs(BARO_ABS_ADS,_b_abs_handler); \ - BaroDiff(BARO_DIFF_ADS,_b_diff_handler); \ -} +extern void umarim_baro_event(void); +#define BaroEvent umarim_baro_event #endif // BOARDS_UMARIM_BARO_H diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h index d34ca18317..5b97725349 100644 --- a/sw/airborne/boards/umarim_1.0.h +++ b/sw/airborne/boards/umarim_1.0.h @@ -114,6 +114,6 @@ #define SPI1_DRDY_EINT 0 #define SPI1_DRDY_VIC_IT VIC_EINT0 -#define BOARD_HAS_BARO 1 +#define USE_BARO_BOARD 1 #endif /* CONFIG_UMARIM_V1_0_H */ From 815f7a9a0fe9e35ad36c7a835b17e0fe04a114f1 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 28 Aug 2013 17:59:30 +0200 Subject: [PATCH 030/125] [baro] convert remaining baro_board to ABI --- conf/firmwares/rotorcraft.makefile | 2 - sw/airborne/boards/apogee/baro_board.c | 2 +- sw/airborne/boards/ardrone/baro_board.c | 42 +++---- sw/airborne/boards/ardrone/baro_board.h | 23 +--- sw/airborne/boards/ardrone/baro_board_dummy.c | 40 ------- sw/airborne/boards/krooz/baro_board.h | 4 +- sw/airborne/boards/lia/baro_board.h | 16 --- sw/airborne/boards/lisa_l/baro_board.c | 105 ++++++++++++++++-- sw/airborne/boards/lisa_l/baro_board.h | 77 +++++-------- sw/airborne/boards/lisa_m/baro_board.c | 30 ++--- sw/airborne/boards/lisa_m/baro_board.h | 15 +-- 11 files changed, 170 insertions(+), 186 deletions(-) delete mode 100644 sw/airborne/boards/ardrone/baro_board_dummy.c delete mode 100644 sw/airborne/boards/lia/baro_board.h diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 3c740b0077..a49fb5e1c5 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -164,8 +164,6 @@ ap.srcs += $(SRC_BOARD)/baro_board.c # Ardrone baro else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) ap.srcs += $(SRC_BOARD)/baro_board.c -else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk) -ap.srcs += $(SRC_BOARD)/baro_board_dummy.c # Lisa/M baro else ifeq ($(BOARD), lisa_m) diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 7ee3a0adc8..53806f7fd7 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -84,7 +84,7 @@ void apogee_baro_event(void) { if (mpl3115_data_available) { if (startup_cnt == 0) { float pressure = ((float)mpl3115_pressure/(1<<2)); - AbiSendMsgBARO_ABS(UMARIM_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(APOGEE_BARO_SENDER_ID, &pressure); } mpl3115_data_available = FALSE; } diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index ce0a1591a6..e27df025a0 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -27,18 +27,25 @@ */ #include "subsystems/sensors/baro.h" +#include "subsystems/abi.h" #include "baro_board.h" #include "navdata.h" -struct Baro baro; +// FIXME +#ifndef ARDRONE2_BARO_SENS +#define ARDRONE2_BARO_SENS 1.0 +#endif + +#ifndef ARDRONE2_BARO_SENDER_ID +#define ARDRONE2_BARO_SENDER_ID 13 +#endif + #define BMP180_OSS 0 // Parrot ARDrone uses no oversampling -void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; -} +void baro_init(void) {} + +void baro_periodic(void) {} static inline int32_t baro_apply_calibration(int32_t raw) { @@ -69,20 +76,17 @@ static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw) return (baro_calibration.b5 + 8) >> 4; } -void baro_periodic(void) +void ardrone_baro_event(void) { -} - -void process_ardrone_baro(void) -{ - if(baro.status == BS_RUNNING) { - // first read temperature because pressure calibration depends on temperature - baro.differential = baro_apply_calibration_temp(navdata->temperature_pressure); // We store the temperature in Baro-Diff - baro.absolute = baro_apply_calibration(navdata->pressure); - } - else { - if (baro_calibrated == TRUE) { - baro.status = BS_RUNNING; + if (navdata_baro_available) { + if (baro_calibrated) { + // first read temperature because pressure calibration depends on temperature + // TODO send Temperature message + baro_apply_calibration_temp(navdata->temperature_pressure); + // FIXME apply correct sensitivity here + float pressure = ARDRONE2_BARO_SENS*(float)baro_apply_calibration(navdata->pressure); + AbiSendMsgBARO_ABS(ARDRONE2_BARO_SENDER_ID, &pressure); } + navdata_baro_available = FALSE; } } diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h index 0b6a1b454e..443e94b476 100644 --- a/sw/airborne/boards/ardrone/baro_board.h +++ b/sw/airborne/boards/ardrone/baro_board.h @@ -30,26 +30,7 @@ #ifndef BOARDS_ARDRONE2_BARO_H #define BOARDS_ARDRONE2_BARO_H -#if USE_BARO_BOARD -#include "navdata.h" - -void process_ardrone_baro(void); - - -static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){ - if (navdata_baro_available) - { - navdata_baro_available = 0; - process_ardrone_baro(); - b_abs_handler(); - } -} - -#define BaroEvent(_b_abs_handler, _b_diff_handler) {\ - baro_event(_b_abs_handler,_b_diff_handler);\ -} -#else -#define BaroEvent(_b_abs_handler, _b_diff_handler) {} -#endif +extern void ardrone_baro_event(void); +#define BaroEvent ardrone_baro_event #endif /* BOARDS_ARDRONE2_BARO_H */ diff --git a/sw/airborne/boards/ardrone/baro_board_dummy.c b/sw/airborne/boards/ardrone/baro_board_dummy.c deleted file mode 100644 index 8487675af7..0000000000 --- a/sw/airborne/boards/ardrone/baro_board_dummy.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2013 Dino Hensen - * - * This file is part of Paparazzi. - * - * Paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * Paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file boards/ardrone/baro_board_dummy.c - * Dummy Baro Board. - * - * These functions are mostly empty because this is a dummy. - */ - -#include "subsystems/sensors/baro.h" - -struct Baro baro; - -void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; -} - -void baro_periodic(void) { -} diff --git a/sw/airborne/boards/krooz/baro_board.h b/sw/airborne/boards/krooz/baro_board.h index 5f47ad650e..c12e63138c 100644 --- a/sw/airborne/boards/krooz/baro_board.h +++ b/sw/airborne/boards/krooz/baro_board.h @@ -8,7 +8,7 @@ #ifndef BOARDS_KROOZ_BARO_H #define BOARDS_KROOZ_BARO_H -extern void baro_event(void (*b_abs_handler)(void)); -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +extern void baro_event(void); +#define BaroEvent baro_event #endif /* BOARDS_KROOZ_SD_BARO_H */ diff --git a/sw/airborne/boards/lia/baro_board.h b/sw/airborne/boards/lia/baro_board.h deleted file mode 100644 index 59782132c8..0000000000 --- a/sw/airborne/boards/lia/baro_board.h +++ /dev/null @@ -1,16 +0,0 @@ - -/* - * FIXME, fake baro board header for BaroEvent - * - */ - -#ifndef BOARDS_LIA_BARO_H -#define BOARDS_LIA_BARO_H - -#include "std.h" - -extern void baro_event(void (*b_abs_handler)(void)); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) - -#endif /* BOARDS_LIA_BARO_H */ diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c index 9e0d825c5e..3a3f02ecd5 100644 --- a/sw/airborne/boards/lisa_l/baro_board.c +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -1,10 +1,57 @@ +/* + * Copyright (C) 2010-2013 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ +#include "std.h" #include "subsystems/sensors/baro.h" +#include "mcu_periph/i2c.h" +#include "subsystems/abi.h" +#include "led.h" + +enum LisaBaroStatus { + LBS_UNINITIALIZED, + LBS_RESETED, + LBS_INITIALIZING_ABS, + LBS_INITIALIZING_ABS_1, + LBS_INITIALIZING_DIFF, + LBS_INITIALIZING_DIFF_1, + LBS_IDLE, + LBS_READING_ABS, + LBS_READ_ABS, + LBS_READING_DIFF, + LBS_READ_DIFF +}; + +struct BaroBoard { + enum LisaBaroStatus status; + bool_t running; +}; + -struct Baro baro; struct BaroBoard baro_board; struct i2c_transaction baro_trans; +static inline void baro_board_send_reset(void); +static inline void baro_board_send_config_abs(void); +static inline void baro_board_send_config_diff(void); static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb); static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr); static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr); @@ -15,11 +62,25 @@ static inline void baro_board_read_from_current_register(uint8_t baro_addr); // differential #define BARO_DIFF_ADDR 0x92 +// FIXME +#ifndef LISA_L_BARO_SENS +#define LISA_L_BARO_SENS 1.0 +#endif + +#ifndef LISA_L_DIFF_SENS +#define LISA_L_DIFF_SENS 1.0 +#endif + +#ifndef LISA_L_BARO_SENDER_ID +#define LISA_L_BARO_SENDER_ID 2 +#endif + void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif baro_board.status = LBS_UNINITIALIZED; + baro_board.running = FALSE; } @@ -50,7 +111,7 @@ void baro_periodic(void) { // baro_board.status = LBS_UNINITIALIZED; break; case LBS_INITIALIZING_DIFF_1: - baro.status = BS_RUNNING; + baro_board.running = TRUE; case LBS_READ_DIFF: baro_board_read_from_current_register(BARO_ABS_ADDR); baro_board.status = LBS_READING_ABS; @@ -63,10 +124,38 @@ void baro_periodic(void) { break; } +#ifdef BARO_LED + if (baro_board.running == TRUE) { + LED_ON(BARO_LED); + } + else { + LED_TOGGLE(BARO_LED); + } +#endif } +void lisa_l_baro_event(void) { + if (baro_board.status == LBS_READING_ABS && + baro_trans.status != I2CTransPending) { + baro_board.status = LBS_READ_ABS; + if (baro_trans.status == I2CTransSuccess) { + int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; + float pressure = LISA_L_BARO_SENS*(float)tmp; + AbiSendMsgBARO_ABS(LISA_L_BARO_SENDER_ID, &pressure); + } + } + else if (baro_board.status == LBS_READING_DIFF && + baro_trans.status != I2CTransPending) { + baro_board.status = LBS_READ_DIFF; + if (baro_trans.status == I2CTransSuccess) { + int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; + float diff = LISA_L_DIFF_SENS*(float)tmp; + AbiSendMsgBARO_DIFF(LISA_L_BARO_SENDER_ID, &diff); + } + } +} -void baro_board_send_config_abs(void) { +static inline void baro_board_send_config_abs(void) { #ifndef BARO_LOW_GAIN INFO("Using High LisaL Baro Gain: Do not use below 1000hPa") baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); @@ -77,11 +166,11 @@ INFO("Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more") #endif } -void baro_board_send_config_diff(void) { +static inline void baro_board_send_config_diff(void) { baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); } -void baro_board_send_reset(void) { +static inline void baro_board_send_reset(void) { baro_trans.type = I2CTransTx; baro_trans.slave_addr = 0x00; baro_trans.len_w = 1; diff --git a/sw/airborne/boards/lisa_l/baro_board.h b/sw/airborne/boards/lisa_l/baro_board.h index 4eec5cfdee..5a39dc8ef8 100644 --- a/sw/airborne/boards/lisa_l/baro_board.h +++ b/sw/airborne/boards/lisa_l/baro_board.h @@ -1,5 +1,29 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger (ENAC) + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + /* + * @file boards/lisa_l/baro_board.h + * * board specific fonctions for the lisa_l board * */ @@ -7,56 +31,7 @@ #ifndef BOARDS_LISA_L_BARO_H #define BOARDS_LISA_L_BARO_H -#include "std.h" -#include "mcu_periph/i2c.h" - -enum LisaBaroStatus { - LBS_UNINITIALIZED, - LBS_RESETED, - LBS_INITIALIZING_ABS, - LBS_INITIALIZING_ABS_1, - LBS_INITIALIZING_DIFF, - LBS_INITIALIZING_DIFF_1, - LBS_IDLE, - LBS_READING_ABS, - LBS_READ_ABS, - LBS_READING_DIFF, - LBS_READ_DIFF -}; - -struct BaroBoard { - enum LisaBaroStatus status; -}; - -extern struct BaroBoard baro_board; -extern struct i2c_transaction baro_trans; - -extern void baro_board_send_reset(void); -extern void baro_board_send_config_abs(void); -extern void baro_board_send_config_diff(void); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.status == LBS_READING_ABS && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_ABS; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.absolute = tmp; \ - _b_abs_handler(); \ - } \ - } \ - else if (baro_board.status == LBS_READING_DIFF && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_DIFF; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.differential = tmp; \ - _b_diff_handler(); \ - } \ - } \ - } - - - +extern void lisa_l_baro_event(void); +#define BaroEvent lisa_l_baro_event #endif /* BOARDS_LISA_L_BARO_H */ diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 9c09b97d90..d8018a13b3 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -23,13 +23,19 @@ * Baro board interface for Bosch BMP085 on LisaM I2C2 with EOC check. */ +#include "std.h" + #include "subsystems/sensors/baro.h" #include "peripherals/bmp085_regs.h" #include +#include "subsystems/abi.h" #include "led.h" -struct Baro baro; +#ifndef LISA_M_BARO_SENDER_ID +#define LISA_M_BARO_SENDER_ID 6 +#endif + struct Bmp085 baro_bmp085; static bool_t baro_eoc(void) @@ -38,10 +44,6 @@ static bool_t baro_eoc(void) } void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; - bmp085_init(&baro_bmp085, &i2c2, BMP085_SLAVE_ADDR); /* setup eoc check function */ @@ -50,17 +52,20 @@ void baro_init(void) { gpio_clear(GPIOB, GPIO0); gpio_set_mode(GPIOB, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0); + +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif } void baro_periodic(void) { - if (baro_bmp085.initialized) { - baro.status = BS_RUNNING; bmp085_periodic(&baro_bmp085); } - else + else { bmp085_read_eeprom_calib(&baro_bmp085); + } } @@ -70,12 +75,11 @@ void baro_event(void (*b_abs_handler)(void)) bmp085_event(&baro_bmp085); if (baro_bmp085.data_available) { - baro.absolute = baro_bmp085.pressure; - b_abs_handler(); + float pressure = (float)baro_bmp085.pressure; + AbiSendMsgBARO_ABS(UMARIM_BARO_SENDER_ID, &pressure); baro_bmp085.data_available = FALSE; - -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); #endif } } diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h index b84a11a7da..bb080094ab 100644 --- a/sw/airborne/boards/lisa_m/baro_board.h +++ b/sw/airborne/boards/lisa_m/baro_board.h @@ -7,20 +7,9 @@ #ifndef BOARDS_LISA_M_BARO_H #define BOARDS_LISA_M_BARO_H -#include "std.h" - // for right now we abuse this file for the ms5611 baro on aspirin as well -#if !BARO_MS5611_I2C && !BARO_MS5611 -#include "peripherals/bmp085.h" - -extern struct Bmp085 baro_bmp085; - - -#endif // !BARO_MS5611_xx - -extern void baro_event(void (*b_abs_handler)(void)); - -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +extern void baro_event(void); +#define BaroEvent baro_event #endif /* BOARDS_LISA_M_BARO_H */ From 081188d11b14f33cf51ec16723fd8040d7e13856 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 18:31:02 +0200 Subject: [PATCH 031/125] [boards] fix baro for lisa_m --- sw/airborne/boards/lisa_m/baro_board.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index d8018a13b3..4cf8462778 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -26,6 +26,7 @@ #include "std.h" #include "subsystems/sensors/baro.h" +#include "peripherals/bmp085.h" #include "peripherals/bmp085_regs.h" #include #include "subsystems/abi.h" @@ -70,13 +71,13 @@ void baro_periodic(void) { -void baro_event(void (*b_abs_handler)(void)) +void baro_event(void) { bmp085_event(&baro_bmp085); if (baro_bmp085.data_available) { float pressure = (float)baro_bmp085.pressure; - AbiSendMsgBARO_ABS(UMARIM_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(LISA_M_BARO_SENDER_ID, &pressure); baro_bmp085.data_available = FALSE; #ifdef BARO_LED RunOnceEvery(10,LED_TOGGLE(BARO_LED)); From e867282eb0b7926c1689d022425be98a6c1076b1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Aug 2013 19:29:59 +0200 Subject: [PATCH 032/125] [dox] doxygen headers for abi --- sw/airborne/subsystems/abi.h | 9 +++++++-- sw/airborne/subsystems/abi_common.h | 10 +++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/sw/airborne/subsystems/abi.h b/sw/airborne/subsystems/abi.h index 1e950e4e9f..d0289b13b4 100644 --- a/sw/airborne/subsystems/abi.h +++ b/sw/airborne/subsystems/abi.h @@ -19,11 +19,16 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file subsystems/abi.h + * + * Main include for ABI (AirBorneInterface). + * @todo explain how to use ABI + */ + #ifndef ABI_H #define ABI_H -/* TODO Explain how to use ABI */ - #include "abi_messages.h" #endif /* ABI_H */ diff --git a/sw/airborne/subsystems/abi_common.h b/sw/airborne/subsystems/abi_common.h index bb3ebe335a..cfa41b592a 100644 --- a/sw/airborne/subsystems/abi_common.h +++ b/sw/airborne/subsystems/abi_common.h @@ -19,7 +19,11 @@ * Boston, MA 02111-1307, USA. */ -/* Common tools for ABI middelware */ +/** + * @file subsystems/abi_common.h + * + * Common tools for ABI middelware. + */ #ifndef ABI_COMMON_H #define ABI_COMMON_H @@ -44,8 +48,8 @@ typedef void (*abi_callback)(void); /** Broadcast address. - * When passing broadcast address has a sender id, - * messages from all sender are received + * When passing broadcast address as a sender id, + * messages from all senders are received */ #define ABI_BROADCAST 0 From 983407997a20b0bc2a96670bd8b0bfa09612de3e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Aug 2013 11:54:58 +0200 Subject: [PATCH 033/125] [ahrs] style fixes - break overly long lines - add FIR filter for pseudo_gravity_measurement in float version --- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 45 +++++---- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 92 ++++++++++++------- 2 files changed, 85 insertions(+), 52 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index bd50d47e68..0ff2abbc07 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 The Paparazzi Team + * Copyright (C) 2010-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -90,7 +90,7 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #define AHRS_MAG_ZETA 0.9 #endif -/** by default use the gravity heursitic to reduce gain */ +/** by default use the gravity heuristic to reduce gain */ #ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC #define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE #endif @@ -254,7 +254,14 @@ void ahrs_update_accel(void) { VECT3_COPY(pseudo_gravity_measurement, imu_accel_float); } - FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); + FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); + + /* FIR filtered pseudo_gravity_measurement */ + #define FIR_FILTER_SIZE 8 + static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.}; + VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); + VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); + VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); if (ahrs_impl.use_gravity_heuristic) { /* heuristic on acceleration (gravity estimate) norm */ @@ -262,12 +269,9 @@ void ahrs_update_accel(void) { * e.g. for WEIGHT_FACTOR 3: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ - #define WEIGHT_FACTOR 3 +#define WEIGHT_FACTOR 3 - /* TODO filter pseudo_gravity_measurement */ - /* g_meas_f=filter(pseudo_gravity_measurement) */ - - const float g_meas_norm = FLOAT_VECT3_NORM(pseudo_gravity_measurement) / 9.81; + const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81; ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); Bound(ahrs_impl.weight, 0.15, 1.0); } @@ -275,14 +279,16 @@ void ahrs_update_accel(void) { /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY */ - const float gravity_rate_update_gain = -2 / 9.81 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY; + const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * + ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY */ - const float gravity_bias_update_gain = ( ahrs_impl.accel_omega * ahrs_impl.accel_omega * ahrs_impl.weight * ahrs_impl.weight) / (AHRS_CORRECT_FREQUENCY * 9.81); + const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega * + ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ @@ -316,14 +322,16 @@ void ahrs_update_mag_full(void) { * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY */ - const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * + AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY */ - const float mag_bias_update_gain = -( ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY; + const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / + AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); } @@ -341,7 +349,7 @@ void ahrs_update_mag_2d(void) { FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu); struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; - + // normalize measured ltp in 2D (x,y) FLOAT_VECT2_NORMALIZE(measured_ltp_2d); @@ -359,14 +367,16 @@ void ahrs_update_mag_2d(void) { /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY */ - const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * + AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY */ - const float mag_bias_update_gain = -( ahrs_impl.mag_omega * ahrs_impl.mag_omega) / AHRS_MAG_CORRECT_FREQUENCY; + const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) / + AHRS_MAG_CORRECT_FREQUENCY; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); } @@ -385,7 +395,8 @@ void ahrs_update_mag_2d_dumb(void) { const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; - const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0)*mn; + const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0) * me + + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0) * mn; const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,1), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; @@ -527,5 +538,3 @@ static inline void compute_body_orientation_and_rates(void) { FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); } - - diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 810fef2b84..f250ceebe4 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -93,7 +93,7 @@ PRINT_CONFIG_VAR(AHRS_ACCEL_ZETA) PRINT_CONFIG_VAR(AHRS_MAG_OMEGA) PRINT_CONFIG_VAR(AHRS_MAG_ZETA) -/** by default use the gravity heursitic to reduce gain */ +/** by default use the gravity heuristic to reduce gain */ #ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC #define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE #endif @@ -153,7 +153,8 @@ void ahrs_init(void) { ahrs_impl.use_gravity_heuristic = FALSE; #endif - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); + VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), + MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); } @@ -162,7 +163,8 @@ void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, + &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ @@ -202,7 +204,8 @@ void ahrs_propagate(void) { INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ - INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); + INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, + omega, AHRS_PROPAGATE_FREQUENCY); INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); @@ -235,7 +238,8 @@ void ahrs_update_accel(void) { #define COMPUTATION_FRAC 16 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC - const struct Int32Vect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; + const struct Int32Vect3 vel_tangential_body = + {ahrs_impl.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); @@ -244,7 +248,8 @@ void ahrs_update_accel(void) { struct Int32Vect3 acc_c_imu; INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); - /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ + /* and subtract it from imu measurement to get a corrected measurement + * of the gravity vector */ INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu.accel); @@ -254,7 +259,7 @@ void ahrs_update_accel(void) { INT32_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); - /* FIR filtered pseudo_gravity_measurement TO DO MOVE IN USE HEURISTIC*/ + /* FIR filtered pseudo_gravity_measurement */ #define FIR_FILTER_SIZE 8 static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0}; VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); @@ -279,16 +284,21 @@ void ahrs_update_accel(void) { /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 * FRAC_conversion: 2^12 / 2^24 = 1 / 4096 * cross_product_gain : 9.81 m/s2 + * * Kp = 1 / inv_rate_scale / FRAC_conversion * cross_product_gain - * inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) - * inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) + * inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight * + * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) + * inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / + * (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) */ - int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); + int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / + (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); Bound(inv_rate_scale, 8192, 4194304); ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; @@ -297,17 +307,20 @@ void ahrs_update_accel(void) { /* Complementary filter integral gain * Correct the gyro bias. + * * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC_conversion: 2^40 / 2^24 = 2^16 * cross_product_gain : 9.81 m/s2 + * * Ki = 1 / FRAC_conversion / inv_bias_gain * cross_product_gain * 2^5 * inv_bias_gain = 9.81 / 2^16 / (omega * omega * weight * weight / AHRS_CORRECT_FREQUENCY) * 2^5 - * inv_bias_gain = AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048) + * inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048) */ - int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048); + int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / + (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -340,18 +353,21 @@ static inline void ahrs_update_mag_full(void) { //INT32_VECT3_RSHIFT(residual,residual,5); /* Complementary filter proportionnal gain. - * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * * residual FRAC: 2 * MAG_FRAC = 22 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 * * Kp = 1/ inv_rate_gain / FRAC_conversion - * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion - * inv_rate_gain = 1024 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) * + * AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion + * inv_rate_gain = 1024 * AHRS_MAG_CORRECT_FREQUENCY / + * (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) */ - int32_t inv_rate_gain = 512 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; + const int32_t inv_rate_gain = 512 * AHRS_MAG_CORRECT_FREQUENCY / + (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY); ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -364,11 +380,15 @@ static inline void ahrs_update_mag_full(void) { * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 * - * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^18 + * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / + * AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / + * AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion + * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * 2^18 / + * AHRS_MAG_CORRECT_FREQUENCY */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * (1 << 18); + const int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1<<18) / + AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; @@ -406,22 +426,25 @@ static inline void ahrs_update_mag_2d(void) { INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); /* Complementary filter proportionnal gain. - * Kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY/ AHRS_MAG_CORRECT_FREQUENCY + * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 * * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5 - * inv_rate_gain = 1 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY )* AHRS_MAG_CORRECT_FREQUENCY - * / FRAC_conversion * 2^5 - * inv_rate_gain = 32 * 32 / (2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY - * inv_rate_gain = 512 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY + * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) * + * AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion * 2^5 + * inv_rate_gain = 32 * 32 * AHRS_MAG_CORRECT_FREQUENCY / + * (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) + * inv_rate_gain = 512 * AHRS_MAG_CORRECT_FREQUENCY / + * (mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) */ - int32_t inv_rate_gain = 16 / (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY) * AHRS_MAG_CORRECT_FREQUENCY; - ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); - ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); - ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain); + int32_t inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY / + (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY); + ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); + ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); + ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain); /* Complementary filter integral gain * Correct the gyro bias. @@ -430,12 +453,13 @@ static inline void ahrs_update_mag_2d(void) { * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 * - * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23 + * Ki = bias_gain / FRAC_conversion = omega * omega / AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion + * bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23 */ - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / AHRS_MAG_CORRECT_FREQUENCY * (1 << 23); + int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1 << 23) / + AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain); ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain); ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain); From d1d1a7e57dbfed2c7c5cb4a3b359a39a518ab118 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sun, 1 Sep 2013 23:25:08 +0200 Subject: [PATCH 034/125] [baro] convert baro modules to ABI --- sw/airborne/modules/sensors/baro_MS5534A.c | 12 ++-- sw/airborne/modules/sensors/baro_MS5534A.h | 2 - sw/airborne/modules/sensors/baro_amsys.c | 12 ++-- sw/airborne/modules/sensors/baro_amsys.h | 2 - sw/airborne/modules/sensors/baro_bmp.c | 7 ++ sw/airborne/modules/sensors/baro_bmp.h | 2 - .../modules/sensors/baro_board_module.c | 59 ---------------- .../modules/sensors/baro_board_module.h | 70 ------------------- sw/airborne/modules/sensors/baro_ets.c | 13 ++++ sw/airborne/modules/sensors/baro_ets.h | 2 - sw/airborne/modules/sensors/baro_hca.c | 17 +++++ sw/airborne/modules/sensors/baro_mpl3115.c | 18 +++-- sw/airborne/modules/sensors/baro_ms5611_i2c.c | 7 ++ sw/airborne/modules/sensors/baro_ms5611_i2c.h | 4 -- sw/airborne/modules/sensors/baro_ms5611_spi.c | 7 ++ sw/airborne/modules/sensors/baro_ms5611_spi.h | 4 -- sw/airborne/modules/sensors/baro_scp.c | 7 ++ sw/airborne/modules/sensors/baro_scp.h | 2 - sw/airborne/modules/sensors/baro_scp_i2c.c | 7 ++ .../modules/sensors/pressure_board_navarro.c | 12 ++++ .../modules/sensors/pressure_board_navarro.h | 2 - 21 files changed, 106 insertions(+), 162 deletions(-) delete mode 100644 sw/airborne/modules/sensors/baro_board_module.c delete mode 100644 sw/airborne/modules/sensors/baro_board_module.h diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index 6b605df2d3..0553ca8b83 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -33,9 +33,14 @@ #ifndef BARO_NO_DOWNLINK #include "ap_downlink.h" #endif +#include "subsystems/abi.h" #include "subsystems/nav.h" #include "state.h" +#ifndef BARO_MS5534A_SENDER_ID +#define BARO_MS5534A_SENDER_ID 15 +#endif + bool_t baro_MS5534A_do_reset; uint32_t baro_MS5534A_pressure; uint16_t baro_MS5534A_temp; @@ -257,11 +262,10 @@ void baro_MS5534A_event( void ) { spi_message_received = FALSE; baro_MS5534A_event_task(); if (baro_MS5534A_available) { - // baro_MS5534A_available = FALSE; // Checked by INS + baro_MS5534A_available = FALSE; baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; - // if (alt_baro_enabled) { - // EstimatorSetAlt(baro_MS5534A_z); // Updated by INS - // } + float pressure = (float)baro_MS5534A_pressure; + AbiSendMsgBARO_ABS(BARO_MS5534A_SENDER_ID, &pressure); } } } diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h index 7e98fa1c34..0b7b0f6373 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.h +++ b/sw/airborne/modules/sensors/baro_MS5534A.h @@ -54,8 +54,6 @@ void baro_MS5534A_event_task( void ); void baro_MS5534A_event( void ); -#define BaroMS5534AUpdate(_b, _h) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; _h(); baro_MS5534A_available = FALSE; } } - #endif // USE_BARO_MS5534A #endif // BARO_MS5534A_H diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index faa014ff94..962e351ad1 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -25,6 +25,7 @@ #include "sensors/baro_amsys.h" #include "mcu_periph/i2c.h" +#include "subsystems/abi.h" #include "state.h" #include #include "generated/flight_plan.h" // for ground alt @@ -43,10 +44,6 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#ifdef SITL -#include "subsystems/gps.h" -#endif - #define BARO_AMSYS_ADDR 0xE4 #define BARO_AMSYS_REG 0x07 #ifndef BARO_AMSYS_SCALE @@ -77,6 +74,10 @@ #define BARO_AMSYS_I2C_DEV i2c0 #endif +#ifndef BARO_AMSYS_SENDER_ID +#define BARO_AMSYS_SENDER_ID 16 +#endif + // Global variables uint16_t pBaroRaw; uint16_t tBaroRaw; @@ -180,6 +181,9 @@ void baro_amsys_read_event( void ) { //Convert to pressure baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); + // Send pressure over ABI + AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); + // compute altitude localy if (!baro_amsys_offset_init) { --baro_amsys_cnt; // Check if averaging completed diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h index a6852dab58..5fda78c154 100644 --- a/sw/airborne/modules/sensors/baro_amsys.h +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -48,6 +48,4 @@ extern void baro_amsys_read_event( void ); #define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); } -#define BaroAmsysUpdate(_b, _h) { if (baro_amsys_valid) { _b = baro_amsys_adc; _h(); baro_amsys_valid = FALSE; } } - #endif // BARO_AMSYS_H diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 5011b7d075..e39a9b299d 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -36,6 +36,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" +#include "subsystems/abi.h" #include "messages.h" #include "subsystems/datalink/downlink.h" @@ -51,6 +52,9 @@ #define BMP_I2C_DEV i2c0 #endif +#ifndef BARO_BMP_SENDER_ID +#define BARO_BMP_SENDER_ID 17 +#endif #define BARO_BMP_R 0.5 #define BARO_BMP_SIGMA2 0.1 @@ -92,6 +96,9 @@ void baro_bmp_event(void) { tmp = pow(tmp, 0.190295); baro_bmp_alt = 44330 * (1.0 - tmp); + float pressure = (float)baro_bmp.pressure; + AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, &pressure); + #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp.up, &baro_bmp.ut, &baro_bmp.pressure, diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h index db1039e817..b36d292f6d 100644 --- a/sw/airborne/modules/sensors/baro_bmp.h +++ b/sw/airborne/modules/sensors/baro_bmp.h @@ -51,6 +51,4 @@ void baro_bmp_init(void); void baro_bmp_periodic(void); void baro_bmp_event(void); -#define BaroBmpUpdate(_b, _h) { if (baro_bmp.data_available) { _b = baro_bmp.pressure; _h(); baro_bmp.data_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_board_module.c b/sw/airborne/modules/sensors/baro_board_module.c deleted file mode 100644 index f5271f0df9..0000000000 --- a/sw/airborne/modules/sensors/baro_board_module.c +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2012 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/* - * Wrapper for the board specific barometer - * Allows to use external baro sensor to feed the general baro interface - */ - -#include "modules/sensors/baro_board_module.h" - -/* Common Baro struct */ -struct Baro baro; - -/* Counter to init custom baro at startup */ -#define BARO_STARTUP_COUNTER 200 -uint16_t startup_cnt; - -/** Implementation of the generic baro interface initialization. - * No need to call this functions from the modules, already done by main. - */ -void baro_init( void ) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; - startup_cnt = BARO_STARTUP_COUNTER; -} - -/** Implementation of the generic baro interface periodic task. - * No need to call this functions from the modules, already done by main. - */ -void baro_periodic( void ) { - if (baro.status == BS_UNINITIALIZED) { - // Run some loops to get correct readings from the adc - --startup_cnt; - if (startup_cnt == 0) { - baro.status = BS_RUNNING; - } - } -} - diff --git a/sw/airborne/modules/sensors/baro_board_module.h b/sw/airborne/modules/sensors/baro_board_module.h deleted file mode 100644 index bc5f48d23b..0000000000 --- a/sw/airborne/modules/sensors/baro_board_module.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright (C) 2011 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file modules/sensors/baro_board_module.h - * - * Wrapper for the board specific barometer. - */ - -#ifndef BARO_BOARD_MODULE_H -#define BARO_BOARD_MODULE_H - -#include "subsystems/sensors/baro.h" - -/** Absolute baro macro mapping. - * Select the baro module you want to use to feed the common baro interface - * in your airframe file when configuring baro_board module - * ex: - * for module baro_ets - * @verbatim - * - * @endverbatim - */ -#ifndef BARO_ABS_EVENT -#define BARO_ABS_EVENT NoBaro -#endif - -/** Differential baro macro mapping. - * TODO - */ -#ifndef BARO_DIFF_EVENT -#define BARO_DIFF_EVENT NoBaro -#endif - -#define NoBaro(_b, _h) {} - -/** BaroEvent macro. - * Need to be maped to one the external baro running has a module - * - * Undef if necessary (already defined in a baro_board.h file) - */ -#ifdef BaroEvent -#undef BaroEvent -#endif - -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - BARO_ABS_EVENT(baro.absolute, _b_abs_handler); \ - BARO_DIFF_EVENT(baro.differential, _b_diff_handler); \ -} - - -#endif diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index e545e6d2a2..3e4c9f2cd5 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -42,6 +42,7 @@ #include "sensors/baro_ets.h" #include "mcu_periph/i2c.h" #include "state.h" +#include "subsystems/abi.h" #include #include "mcu_periph/sys_time.h" @@ -73,11 +74,21 @@ #define BARO_ETS_R 0.5 #define BARO_ETS_SIGMA2 0.1 +// Pressure offset to convert raw adc to real pressure (FIXME find real value) +#ifndef BARO_ETS_PRESSURE_OFFSET +#define BARO_ETS_PRESSURE_OFFSET 101325.0 +#endif + #ifndef BARO_ETS_I2C_DEV #define BARO_ETS_I2C_DEV i2c0 #endif PRINT_CONFIG_VAR(BARO_ETS_I2C_DEV) +#ifndef BARO_ETS_SENDER_ID +#define BARO_ETS_SENDER_ID 18 +#endif +PRINT_CONFIG_VAR(BARO_ETS_SENDER_ID) + /** delay in seconds until sensor is read after startup */ #ifndef BARO_ETS_START_DELAY #define BARO_ETS_START_DELAY 0.2 @@ -178,6 +189,8 @@ void baro_ets_read_event( void ) { if (baro_ets_offset_init) { baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); // New value available + float pressure = BARO_ETS_SCALE * (float) baro_ets_adc + BARO_ETS_PRESSURE_OFFSET; + AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, &pressure); #ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h index 33f7bbd592..90d02ab4c6 100644 --- a/sw/airborne/modules/sensors/baro_ets.h +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -64,6 +64,4 @@ extern void baro_ets_read_event( void ); #define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); } -#define BaroEtsUpdate(_b, _h) { if (baro_ets_valid) { _b = baro_ets_adc; _h(); baro_ets_valid = FALSE; } } - #endif // BARO_ETS_H diff --git a/sw/airborne/modules/sensors/baro_hca.c b/sw/airborne/modules/sensors/baro_hca.c index b8c257c848..99f6a23ff1 100644 --- a/sw/airborne/modules/sensors/baro_hca.c +++ b/sw/airborne/modules/sensors/baro_hca.c @@ -22,6 +22,7 @@ #include "sensors/baro_hca.h" #include "mcu_periph/i2c.h" +#include "subsystems/abi.h" #include //Messages @@ -39,10 +40,24 @@ #define BARO_HCA_MAX_OUT 27852 //dec #define BARO_HCA_MIN_OUT 1638 //dec +// FIXME +#ifndef BARO_HCA_SCALE +#define BARO_HCA_SCALE 1.0 +#endif + +// FIXME +#ifndef BARO_HCA_PRESSURE_OFFSET +#define BARO_HCA_PRESSURE_OFFSET 101325.0 +#endif + #ifndef BARO_HCA_I2C_DEV #define BARO_HCA_I2C_DEV i2c0 #endif +#ifndef BARO_HCA_SENDER_ID +#define BARO_HCA_SENDER_ID 19 +#endif + // Global variables uint16_t pBaroRaw; bool_t baro_hca_valid; @@ -83,6 +98,8 @@ void baro_hca_read_event( void ) { if (pBaroRaw > BARO_HCA_MAX_OUT) pBaroRaw = BARO_HCA_MAX_OUT; + float pressure = BARO_HCA_SCALE*(float)pBaroRaw + BARO_HCA_PRESSURE_OFFSET; + AbiSendMsgBARO_ABS(BARO_HCA_SENDER_ID, &pressure); } baro_hca_i2c_trans.status = I2CTransDone; diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index f44e248c64..431674388b 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -21,6 +21,7 @@ */ #include "modules/sensors/baro_mpl3115.h" +#include "subsystems/abi.h" //Messages #include "mcu_periph/uart.h" @@ -31,23 +32,30 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif +#ifndef BARO_MPL3115_SENDER_ID +#define BARO_MPL3115_SENDER_ID 20 +#endif + void baro_mpl3115_init( void ) { mpl3115_init(); } void baro_mpl3115_read_periodic( void ) { -#ifdef SENSOR_SYNC_SEND - if (mpl3115_data_available) { - DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &mpl3115_pressure, &mpl3115_temperature, &mpl3115_alt); - } -#endif Mpl3115Periodic(); } void baro_mpl3115_read_event( void ) { mpl3115_event(); + if (mpl3115_data_available) { + float pressure = (float)mpl3115_pressure; + AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, &pressure); +#ifdef SENSOR_SYNC_SEND + DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &mpl3115_pressure, &mpl3115_temperature, &mpl3115_alt); +#endif + mpl3115_data_available = FALSE; + } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index d3b3a3d88c..20f61d69ca 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -30,6 +30,7 @@ #include "modules/sensors/baro_ms5611_i2c.h" #include "mcu_periph/sys_time.h" +#include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" @@ -38,6 +39,10 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif +#ifndef BARO_MS5611_SENDER_ID +#define BARO_MS5611_SENDER_ID 20 +#endif + #ifndef MS5611_I2C_DEV #define MS5611_I2C_DEV i2c0 #endif @@ -97,6 +102,8 @@ void baro_ms5611_event( void ) { ms5611_i2c_event(&baro_ms5611); if (baro_ms5611.data_available) { + float pressure = (float)baro_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level tmp_float = pow(tmp_float, 0.190295); baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h index f30b28debc..c601849c1d 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -22,8 +22,4 @@ extern void baro_ms5611_read(void); extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); -#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } } - -#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index c91d713153..a261eabf19 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -30,6 +30,7 @@ #include "modules/sensors/baro_ms5611_spi.h" #include "mcu_periph/sys_time.h" +#include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" @@ -38,6 +39,10 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif +#ifndef BARO_MS5611_SENDER_ID +#define BARO_MS5611_SENDER_ID 20 +#endif + #ifndef MS5611_SPI_DEV #define MS5611_SPI_DEV spi1 #endif @@ -97,6 +102,8 @@ void baro_ms5611_event( void ) { ms5611_spi_event(&baro_ms5611); if (baro_ms5611.data_available) { + float pressure = (float)baro_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level tmp_float = pow(tmp_float, 0.190295); baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h index 7bab9d92e8..be521c5ada 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.h +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h @@ -50,8 +50,4 @@ extern void baro_ms5611_read(void); extern void baro_ms5611_periodic_check(void); extern void baro_ms5611_event(void); -#define BaroMs5611UpdatePressure(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; baro_ms5611.data_available = FALSE; } } - -#define BaroMs5611UpdateAlt(_b) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; baro_ms5611.data_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c index 94a2f67807..580f3d36b6 100644 --- a/sw/airborne/modules/sensors/baro_scp.c +++ b/sw/airborne/modules/sensors/baro_scp.c @@ -2,6 +2,7 @@ #include "mcu_periph/sys_time.h" #include "led.h" #include "mcu.h" +#include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "messages.h" @@ -15,6 +16,10 @@ #warning set SENSOR_SYNC_SEND to use baro_scp #endif +#ifndef BARO_SCP_SENDER_ID +#define BARO_SCP_SENDER_ID 21 +#endif + #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif @@ -184,6 +189,8 @@ static void baro_scp_read(void) { void baro_scp_event( void ) { if (baro_scp_available == TRUE) { + float pressure = (float)baro_scp_pressure; + AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature); #endif diff --git a/sw/airborne/modules/sensors/baro_scp.h b/sw/airborne/modules/sensors/baro_scp.h index 77aeb504d1..c69534ec0f 100644 --- a/sw/airborne/modules/sensors/baro_scp.h +++ b/sw/airborne/modules/sensors/baro_scp.h @@ -20,6 +20,4 @@ void baro_scp_init(void); void baro_scp_periodic(void); void baro_scp_event(void); -#define BaroScpUpdate(_b, _h) { if (baro_scp_available) { _b = baro_scp_pressure; _h(); baro_scp_available = FALSE; } } - #endif diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c index a3c1f24d16..331549ffcf 100644 --- a/sw/airborne/modules/sensors/baro_scp_i2c.c +++ b/sw/airborne/modules/sensors/baro_scp_i2c.c @@ -9,6 +9,7 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/i2c.h" +#include "subsystems/abi.h" #include "led.h" #include "mcu_periph/uart.h" @@ -19,6 +20,10 @@ #warning set SENSOR_SYNC_SEND to use baro_scp_i2c #endif +#ifndef BARO_SCP_SENDER_ID +#define BARO_SCP_SENDER_ID 21 +#endif + #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif @@ -99,6 +104,8 @@ void baro_scp_event( void ) { baro_scp_pressure |= scp_trans.buf[1]; baro_scp_pressure *= 25; + float pressure = (float) baro_scp_pressure; + AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature); #endif diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index ebe33e997a..0c26f41e6c 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -27,6 +27,7 @@ #include "pressure_board_navarro.h" #include "state.h" +#include "subsystems/abi.h" /* Default I2C device on tiny is i2c0 */ @@ -56,6 +57,14 @@ #define PBN_ALTITUDE_SCALE 0.32 #endif +#ifndef PBN_PRESSURE_OFFSET +#define PBN_PRESSURE_OFFSET 101325.0 +#endif + +#ifndef BARO_PBN_SENDER_ID +#define BARO_PBN_SENDER_ID 22 +#endif + // Global variables uint16_t altitude_adc; uint16_t airspeed_adc; @@ -132,6 +141,9 @@ void pbn_read_event( void ) { --offset_cnt; } else { + // Compute pressure + float pressure = PBN_ALTITUDE_SCALE * (float) altitude_adc + PBN_PRESSURE_OFFSET; + AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, &pressure); // Compute airspeed and altitude //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; uint16_t diff = Max(airspeed_adc-airspeed_offset, 0); diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h index 3ba8b2d208..f5e78f2a2f 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.h +++ b/sw/airborne/modules/sensors/pressure_board_navarro.h @@ -54,8 +54,6 @@ extern void pbn_read_event( void ); #define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); } -#define BaroPbnUpdate(_b) { if (data_valid) { _b = altitude_adc; data_valid = FALSE; } } - #define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel, DefaultDevice,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset); #endif // PRESSURE_BOARD_NAVARRO_H From 5665b37a51b74b8bda9d21f3fd68bd9ab4d4fdae Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Sep 2013 11:36:04 +0200 Subject: [PATCH 035/125] [ahrs] int_cmpl_quat: fix comment --- sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index f250ceebe4..0d7be100a1 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -431,12 +431,12 @@ static inline void ahrs_update_mag_2d(void) { * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 * - * Kp = 1/ inv_rate_gain / FRAC_conversion * 2^5 + * Kp = 1/ inv_rate_gain / FRAC_conversion * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) * - * AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion * 2^5 - * inv_rate_gain = 32 * 32 * AHRS_MAG_CORRECT_FREQUENCY / + * AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion + * inv_rate_gain = 32 * AHRS_MAG_CORRECT_FREQUENCY / * (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) - * inv_rate_gain = 512 * AHRS_MAG_CORRECT_FREQUENCY / + * inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY / * (mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) */ From 175e51a7e3ce38a2493aa67f18f14d6e41571345 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 2 Sep 2013 11:47:39 +0200 Subject: [PATCH 036/125] [baro] convert new boards to ABI for baro --- sw/airborne/boards/lisa_s/baro_board.h | 10 +++++----- sw/airborne/boards/px4fmu/baro_board.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/sw/airborne/boards/lisa_s/baro_board.h b/sw/airborne/boards/lisa_s/baro_board.h index 7581ef4de3..8f41c8e3ca 100644 --- a/sw/airborne/boards/lisa_s/baro_board.h +++ b/sw/airborne/boards/lisa_s/baro_board.h @@ -4,13 +4,13 @@ * */ -#ifndef BOARDS_LISA_M_BARO_H -#define BOARDS_LISA_M_BARO_H +#ifndef BOARDS_LISA_S_BARO_H +#define BOARDS_LISA_S_BARO_H #include "std.h" -extern void baro_event(void (*b_abs_handler)(void)); +extern void baro_event(void); -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +#define BaroEvent baro_event -#endif /* BOARDS_LISA_M_BARO_H */ +#endif /* BOARDS_LISA_S_BARO_H */ diff --git a/sw/airborne/boards/px4fmu/baro_board.h b/sw/airborne/boards/px4fmu/baro_board.h index 4aff1cc1c7..8ef2dacaea 100644 --- a/sw/airborne/boards/px4fmu/baro_board.h +++ b/sw/airborne/boards/px4fmu/baro_board.h @@ -7,8 +7,8 @@ #ifndef BOARDS_PX4FMU_BARO_H #define BOARDS_PX4FMU_BARO_H -extern void baro_event(void (*b_abs_handler)(void)); +extern void baro_event(void); -#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler) +#define BaroEvent baro_event #endif /* BOARDS_PX4FMU_BARO_H */ From 4dd6772565b6c00d1ee03baa7ad70247085eed05 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 2 Sep 2013 11:48:00 +0200 Subject: [PATCH 037/125] [ins] conver alt_float ins to ABI for baro --- sw/airborne/subsystems/ins/ins_alt_float.c | 72 +++++++++++++--------- sw/airborne/subsystems/ins/ins_alt_float.h | 6 +- 2 files changed, 46 insertions(+), 32 deletions(-) diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 8f0004ffd1..29265a034c 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -24,7 +24,9 @@ * Filters altitude and climb rate for fixedwings. */ -#include "subsystems/ins.h" +#include "subsystems/ins/ins_alt_float.h" + +#include "subsystems/abi.h" #include #include @@ -46,9 +48,18 @@ float ins_alt_dot; #if USE_BAROMETER #include "subsystems/sensors/baro.h" -int32_t ins_qfe; +#include "math/pprz_isa.h" +float ins_qfe; bool_t ins_baro_initialised; float ins_baro_alt; + +// Baro event on ABI +#ifndef INS_BARO_ID +#define INS_BARO_ID ABI_BROADCAST +#endif +abi_event baro_ev; +static void baro_cb(uint8_t sender_id, const float *pressure); + #endif void ins_init() { @@ -64,6 +75,8 @@ void ins_init() { ins_qfe = 0;; ins_baro_initialised = FALSE; ins_baro_alt = 0.; + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); #endif ins.vf_realign = FALSE; @@ -83,36 +96,37 @@ void ins_realign_v(float z __attribute__ ((unused))) { void ins_propagate() { } -void ins_update_baro() { +void ins_update_baro() {} + #if USE_BAROMETER - // TODO update kalman filter with baro struct - if (baro.status == BS_RUNNING) { - if (!ins_baro_initialised) { - ins_qfe = baro.absolute; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = baro.absolute; - } - else { /* not realigning, so normal update with baro measurement */ - /* altitude decreases with increasing baro.absolute pressure */ - ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS; - /* run the filter */ - EstimatorSetAlt(ins_baro_alt); - /* set new altitude, just copy old horizontal position */ - struct UtmCoor_f utm; - UTM_COPY(utm, *stateGetPositionUtm_f()); - utm.alt = ins_alt; - stateSetPositionUtm_f(&utm); - struct NedCoor_f ned_vel; - memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); - ned_vel.z = -ins_alt_dot; - stateSetSpeedNed_f(&ned_vel); - } +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { + if (!ins_baro_initialised) { + ins_qfe = *pressure; + ins_baro_initialised = TRUE; + } + if (ins.vf_realign) { + ins.vf_realign = FALSE; + ins_alt = ground_alt; + ins_alt_dot = 0.; + ins_qfe = *pressure; + alt_kalman_reset(); + } + else { /* not realigning, so normal update with baro measurement */ + ins_baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_qfe); + /* run the filter */ + EstimatorSetAlt(ins_baro_alt); + /* set new altitude, just copy old horizontal position */ + struct UtmCoor_f utm; + UTM_COPY(utm, *stateGetPositionUtm_f()); + utm.alt = ins_alt; + stateSetPositionUtm_f(&utm); + struct NedCoor_f ned_vel; + memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); + ned_vel.z = -ins_alt_dot; + stateSetSpeedNed_f(&ned_vel); } -#endif } +#endif void ins_update_gps(void) { diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 613ebc5281..785ba95d24 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -56,9 +56,9 @@ #include "modules/sensors/baro_amsys.h" #endif -extern int32_t ins_qfe; -extern float ins_baro_alt; -extern bool_t ins_baro_initialised; +extern float ins_qfe; +extern float ins_baro_alt; +extern bool_t ins_baro_initialised; #endif //USE_BAROMETER extern float ins_alt; ///< estimated altitude above MSL in meters From 93cf55b07f3402c491dd478411d6d520234704f5 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 2 Sep 2013 17:38:17 +0200 Subject: [PATCH 038/125] [baro] fix baro ABI for sim target with amsys sensor --- .../subsystems/fixedwing/autopilot.makefile | 17 +++++++++++++- sw/airborne/firmwares/fixedwing/main_ap.c | 3 +++ sw/airborne/math/pprz_isa.h | 22 ++++++++++++++++++- sw/airborne/modules/sensors/baro_amsys.c | 3 +++ .../subsystems/navigation/common_nav.c | 5 +++++ 5 files changed, 48 insertions(+), 2 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 5136c28c4e..f6acdfe815 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -167,6 +167,8 @@ ap_srcs += $(SRC_ARCH)/subsystems/settings_arch.c ap_srcs += subsystems/air_data.c # BARO + +# Umarim ifeq ($(BOARD), umarim) ifeq ($(BOARD_VERSION), 1.0) ap_srcs += boards/umarim/baro_board.c @@ -174,9 +176,20 @@ ap_CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 ap_CFLAGS += -DADS1114_I2C_DEV=i2c1 ap_srcs += peripherals/ads1114.c endif + +# Lisa/L else ifeq ($(BOARD), lisa_l) ap_CFLAGS += -DUSE_I2C2 -endif + +# Apogee +else ifeq ($(BOARD), apogee) +ap_CFLAGS += -DUSE_I2C1 +ap_CFLAGS += -DMPL3115_I2C_DEV=i2c1 +ap_CFLAGS += -DMPL3115_ALT_MODE=0 +ap_srcs += peripherals/mpl3115.c +ap_srcs += $(SRC_BOARD)/baro_board.c + +endif # End baro ifneq ($(BARO_LED),none) ap_CFLAGS += -DBARO_LED=$(BARO_LED) @@ -211,6 +224,8 @@ sim.srcs += $(SRC_ARCH)/sim_ap.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c +sim.srcs += $(SRC_BOARD)/baro_board.c + sim.srcs += subsystems/settings.c sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 8f5266ef29..c0de8a9fe1 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -30,6 +30,8 @@ #define MODULES_C +#define ABI_C + #include #include "firmwares/fixedwing/main_ap.h" @@ -82,6 +84,7 @@ #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 #include "rc_settings.h" #endif +#include "subsystems/abi.h" #include "led.h" diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h index eaa2514d4f..a5b6f0e0e7 100644 --- a/sw/airborne/math/pprz_isa.h +++ b/sw/airborne/math/pprz_isa.h @@ -67,7 +67,7 @@ static inline float pprz_isa_altitude_of_pressure(float pressure) { * Get relative altitude from pressure (using simplified equation). * * @param pressure current pressure in Pascal (Pa) - * @param ref reference pressure + * @param ref reference pressure (QFE) when height = 0 * @return altitude in pressure in ISA conditions */ static inline float pprz_isa_height_of_pressure(float pressure, float ref) { @@ -78,5 +78,25 @@ static inline float pprz_isa_height_of_pressure(float pressure, float ref) { } } +/** + * Get pressure in Pa from absolute altitude (using simplified equation). + * + * @param altitude current absolute altitude in meters + * @return static pressure in Pa in ISA conditions + */ +static inline float pprz_isa_pressure_of_altitude(float altitude) { + return (PPRZ_ISA_SEA_LEVEL_PRESSURE*expf((-1./PPRZ_ISA_M_OF_P_CONST)*altitude)); +} + +/** + * Get pressure in Pa from height (using simplified equation). + * + * @param altitude current relative altitude in meters + * @param ref reference pressure (QFE) when height = 0 + * @return static pressure in Pa in ISA conditions + */ +static inline float pprz_isa_pressure_of_height(float altitude, float ref) { + return (ref*expf((-1./PPRZ_ISA_M_OF_P_CONST)*altitude)); +} #endif /* PPRZ_ISA_H */ diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index 962e351ad1..c5dd55502f 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -33,6 +33,7 @@ #ifdef SITL #include "subsystems/gps.h" #include "subsystems/navigation/common_nav.h" +#include "math/pprz_isa.h" #endif //Messages @@ -144,6 +145,8 @@ void baro_amsys_read_periodic( void ) { pBaroRaw = 0; baro_amsys_altitude = gps.hmsl / 1000.0; baro_amsys_adc = baro_amsys_offset - ((baro_amsys_altitude - ground_alt) / INS_BARO_SENS); + baro_amsys_p = pprz_isa_pressure_of_altitude(baro_amsys_altitude); + AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); baro_amsys_valid = TRUE; #endif diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 7e641c05d6..9e2623e3fe 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -26,6 +26,7 @@ #include "subsystems/navigation/common_nav.h" #include "generated/flight_plan.h" +#include "subsystems/ins.h" #include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" @@ -116,6 +117,10 @@ unit_t nav_reset_reference( void ) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); + // realign INS if needed + ins.hf_realign = TRUE; + ins.vf_realign = TRUE; + previous_ground_alt = ground_alt; ground_alt = gps.hmsl/1000.; return 0; From 6f15edbb101c22b0553ff369a11f04cac7ae899f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 2 Sep 2013 17:44:27 +0200 Subject: [PATCH 039/125] [message] fix message format for BARO_RAW --- conf/messages.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 3ff5912545..3a9323b475 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1873,8 +1873,8 @@ - - + + From 61b45079170bfe048466b67e50286b68ef9f2332 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 4 Sep 2013 13:01:17 +0200 Subject: [PATCH 040/125] [ahrs] int_cmpl_quat: precompute some gains for efficiency --- .../estimation/ahrs_int_cmpl_quat.xml | 8 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 118 ++++++++++-------- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 50 ++++++-- 3 files changed, 112 insertions(+), 64 deletions(-) diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index b53f9e04e6..7c18312931 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -5,10 +5,10 @@ - - - - + + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 0d7be100a1..7f3e17ac42 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -135,8 +135,10 @@ void ahrs_init(void) { /* set default filter cut-off frequency and damping */ ahrs_impl.accel_omega = AHRS_ACCEL_OMEGA; ahrs_impl.accel_zeta = AHRS_ACCEL_ZETA; + ahrs_set_accel_gains(); ahrs_impl.mag_omega = AHRS_MAG_OMEGA; ahrs_impl.mag_zeta = AHRS_MAG_ZETA; + ahrs_set_mag_gains(); /* set default gravity heuristic */ ahrs_impl.weight = 1.0; @@ -147,11 +149,7 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif -#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC - ahrs_impl.use_gravity_heuristic = TRUE; -#else - ahrs_impl.use_gravity_heuristic = FALSE; -#endif + ahrs_impl.use_gravity_heuristic = AHRS_GRAVITY_UPDATE_NORM_HEURISTIC; VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); @@ -175,8 +173,8 @@ void ahrs_align(void) { set_body_state_from_quat(); /* Use low passed gyro value as initial bias */ - RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); - RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); + RATES_COPY(ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); + RATES_COPY(ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); ahrs.status = AHRS_RUNNING; @@ -213,6 +211,28 @@ void ahrs_propagate(void) { } +void ahrs_set_accel_gains(void) { + /* Complementary filter proportionnal gain + * Kp = 2 * omega * zeta * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * + * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain) + * accel_inv_kp = 4096 * 9.81 / Kp + */ + ahrs_impl.accel_inv_kp = 4096 * 9.81 / + (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * + AHRS_PROPAGATE_FREQUENCY / AHRS_PROPAGATE_FREQUENCY); + + /* Complementary filter integral gain + * Ki = omega^2 / AHRS_CORRECT_FREQUENCY + * + * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain) + * accel_inv_ki = 2^5 / 2^16 * 9.81 / Ki + * accel_inv_ki = 9.81 / 2048 / Ki + */ + ahrs_impl.accel_inv_ki = AHRS_CORRECT_FREQUENCY * 9.81 / 2048 / + (ahrs_impl.accel_omega * ahrs_impl.accel_omega); +} + void ahrs_update_accel(void) { // c2 = ltp z-axis in imu-frame @@ -283,23 +303,22 @@ void ahrs_update_accel(void) { } /* Complementary filter proportional gain. - * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY + * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY * * residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * rate_correction FRAC: RATE_FRAC = 12 * FRAC_conversion: 2^12 / 2^24 = 1 / 4096 * cross_product_gain : 9.81 m/s2 * - * Kp = 1 / inv_rate_scale / FRAC_conversion * cross_product_gain - * inv_rate_scale= 4096 * 9.81 / (2 * zeta * omega * weight * - * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY ) - * inv_rate_scale= 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / - * (omega * zeta * weight * AHRS_PROPAGATE_FREQUENCY ) + * accel_inv_kp = 1 / (Kp * FRAC_conversion / cross_product_gain) + * accel_inv_kp = 4096 * 9.81 / Kp + * + * inv_rate_scale = 1 / (weight * Kp * FRAC_conversion / cross_product_gain) + * inv_rate_scale = inv_kp / weight */ - - int32_t inv_rate_scale = 2048 * 9.81 * AHRS_CORRECT_FREQUENCY / - (ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * AHRS_PROPAGATE_FREQUENCY); + int32_t inv_rate_scale = (int32_t)(ahrs_impl.accel_inv_kp / ahrs_impl.weight); Bound(inv_rate_scale, 8192, 4194304); + ahrs_impl.rate_correction.p -= residual.x / inv_rate_scale; ahrs_impl.rate_correction.q -= residual.y / inv_rate_scale; ahrs_impl.rate_correction.r -= residual.z / inv_rate_scale; @@ -307,20 +326,22 @@ void ahrs_update_accel(void) { /* Complementary filter integral gain * Correct the gyro bias. + * Ki = omega^2 / AHRS_CORRECT_FREQUENCY * - * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY * residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 * high_rez_bias = RATE_FRAC+28 = 40 * FRAC_conversion: 2^40 / 2^24 = 2^16 * cross_product_gain : 9.81 m/s2 * - * Ki = 1 / FRAC_conversion / inv_bias_gain * cross_product_gain * 2^5 - * inv_bias_gain = 9.81 / 2^16 / (omega * omega * weight * weight / AHRS_CORRECT_FREQUENCY) * 2^5 - * inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / (omega * omega * weight * weight * 2048) + * accel_inv_ki = 2^5 / (Ki * FRAC_conversion / cross_product_gain) + * accel_inv_ki = 2^5 / 2^16 * 9.81 * Ki = 9.81 / 2^11 * Ki + * + * inv_bias_gain = 2^5 / (weight^2 * Ki * FRAC_conversion / cross_product_gain) + * inv_bias_gain = accel_inv_ki / weight^2 */ - int32_t inv_bias_gain = 9.81 * AHRS_CORRECT_FREQUENCY / - (ahrs_impl.weight * ahrs_impl.weight * ahrs_impl.accel_omega * ahrs_impl.accel_omega * 2048); + int32_t inv_bias_gain = (int32_t)(ahrs_impl.accel_inv_ki / + (ahrs_impl.weight * ahrs_impl.weight)); Bound(inv_bias_gain, 8, 65536) ahrs_impl.high_rez_bias.p += (residual.x / inv_bias_gain) << 5; ahrs_impl.high_rez_bias.q += (residual.y / inv_bias_gain) << 5; @@ -339,6 +360,15 @@ void ahrs_update_mag(void) { #endif } +void ahrs_set_mag_gains(void) { + /* Complementary filter proportionnal gain = 2*omega*zeta */ + ahrs_impl.mag_kp = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega * + AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY; + /* Complementary filter integral gain = omega^2 */ + ahrs_impl.mag_ki = ahrs_impl.mag_omega * ahrs_impl.mag_omega / + AHRS_MAG_CORRECT_FREQUENCY; +} + static inline void ahrs_update_mag_full(void) { @@ -350,7 +380,6 @@ static inline void ahrs_update_mag_full(void) { struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); - //INT32_VECT3_RSHIFT(residual,residual,5); /* Complementary filter proportionnal gain. * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY @@ -359,15 +388,11 @@ static inline void ahrs_update_mag_full(void) { * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^22 = 1/1024 * - * Kp = 1/ inv_rate_gain / FRAC_conversion - * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) * - * AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion - * inv_rate_gain = 1024 * AHRS_MAG_CORRECT_FREQUENCY / - * (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) + * inv_rate_gain = 1 / Kp / FRAC_conversion + * inv_rate_gain = 1024 / Kp */ - const int32_t inv_rate_gain = 512 * AHRS_MAG_CORRECT_FREQUENCY / - (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY); + const int32_t inv_rate_gain = (int32_t)(1024.0 / ahrs_impl.mag_kp); ahrs_impl.rate_correction.p += residual.x / inv_rate_gain; ahrs_impl.rate_correction.q += residual.y / inv_rate_gain; @@ -376,19 +401,15 @@ static inline void ahrs_update_mag_full(void) { /* Complementary filter integral gain * Correct the gyro bias. * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * * residual FRAC: 2* MAG_FRAC = 22 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^22 = 2^18 * - * Ki = bias_gain / FRAC_conversion = ahrs_impl.omega * ahrs_impl.omega / - * AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega / - * AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion - * bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * 2^18 / - * AHRS_MAG_CORRECT_FREQUENCY + * bias_gain = Ki * FRAC_conversion = Ki * 2^18 */ - const int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1<<18) / - AHRS_MAG_CORRECT_FREQUENCY; + const int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1<<18)); + ahrs_impl.high_rez_bias.p -= residual.x * bias_gain; ahrs_impl.high_rez_bias.q -= residual.y * bias_gain; ahrs_impl.high_rez_bias.r -= residual.z * bias_gain; @@ -427,21 +448,16 @@ static inline void ahrs_update_mag_2d(void) { /* Complementary filter proportionnal gain. * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY + * * residual_imu FRAC = residual_ltp FRAC = 17 * rate_correction FRAC: RATE_FRAC = 12 * FRAC conversion: 2^12 / 2^17 = 1/32 * - * Kp = 1/ inv_rate_gain / FRAC_conversion - * inv_rate_gain = 1 / (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) * - * AHRS_MAG_CORRECT_FREQUENCY / FRAC_conversion - * inv_rate_gain = 32 * AHRS_MAG_CORRECT_FREQUENCY / - * (2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) - * inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY / - * (mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY) + * inv_rate_gain = 1 / Kp / FRAC_conversion + * inv_rate_gain = 32 / Kp */ + int32_t inv_rate_gain = (int32_t)(32.0 / ahrs_impl.mag_kp); - int32_t inv_rate_gain = 16 * AHRS_MAG_CORRECT_FREQUENCY / - (ahrs_impl.mag_zeta * ahrs_impl.mag_omega * AHRS_PROPAGATE_FREQUENCY); ahrs_impl.rate_correction.p += (residual_imu.x / inv_rate_gain); ahrs_impl.rate_correction.q += (residual_imu.y / inv_rate_gain); ahrs_impl.rate_correction.r += (residual_imu.z / inv_rate_gain); @@ -449,17 +465,15 @@ static inline void ahrs_update_mag_2d(void) { /* Complementary filter integral gain * Correct the gyro bias. * Ki = omega^2 / AHRS_MAG_CORRECT_FREQUENCY + * * residual_imu FRAC = residual_ltp FRAC = 17 * high_rez_bias FRAC: RATE_FRAC+28 = 40 * FRAC conversion: 2^40 / 2^17 = 2^23 * - * Ki = bias_gain / FRAC_conversion = omega * omega / AHRS_MAG_CORRECT_FREQUENCY - * bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * FRAC_conversion - * bias_gain = mag_omega * mag_omega / AHRS_MAG_CORRECT_FREQUENCY * 2^23 + * bias_gain = Ki * FRAC_conversion = Ki * 2^23 */ + int32_t bias_gain = (int32_t)(ahrs_impl.mag_ki * (1 << 23)); - int32_t bias_gain = ahrs_impl.mag_omega * ahrs_impl.mag_omega * (1 << 23) / - AHRS_MAG_CORRECT_FREQUENCY; ahrs_impl.high_rez_bias.p -= (residual_imu.x * bias_gain); ahrs_impl.high_rez_bias.q -= (residual_imu.y * bias_gain); ahrs_impl.high_rez_bias.r -= (residual_imu.z * bias_gain); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 194f15d9fa..56d7ad1131 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011 The Paparazzi Team + * Copyright (C) 2008-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -46,16 +46,23 @@ struct AhrsIntCmplQuat { struct Int64Rates high_rez_bias; struct Int32Quat ltp_to_imu_quat; struct Int32Vect3 mag_h; + + int32_t ltp_vel_norm; + bool_t ltp_vel_norm_valid; + bool_t heading_aligned; + float weight; + float accel_inv_kp; + float accel_inv_ki; + float mag_kp; + float mag_ki; + + /* parameters/options that can be changed */ + bool_t correct_gravity; + bool_t use_gravity_heuristic; float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer - float weight; - int32_t ltp_vel_norm; - bool_t ltp_vel_norm_valid; - bool_t correct_gravity; - bool_t use_gravity_heuristic; - bool_t heading_aligned; }; extern struct AhrsIntCmplQuat ahrs_impl; @@ -74,10 +81,37 @@ void ahrs_update_heading(int32_t heading); */ void ahrs_realign_heading(int32_t heading); + +/// update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta +extern void ahrs_set_accel_gains(void); + +static inline void ahrs_int_cmpl_quat_SetAccelOmega(float omega) { + ahrs_impl.accel_omega = omega; + ahrs_set_accel_gains(); +} + +static inline void ahrs_int_cmpl_quat_SetAccelZeta(float zeta) { + ahrs_impl.accel_zeta = zeta; + ahrs_set_accel_gains(); +} + +/// update pre-computed kp and ki gains from mag_omega and mag_zeta +extern void ahrs_set_mag_gains(void); + +static inline void ahrs_int_cmpl_quat_SetMagOmega(float omega) { + ahrs_impl.mag_omega = omega; + ahrs_set_mag_gains(); +} + +static inline void ahrs_int_cmpl_quat_SetMagZeta(float zeta) { + ahrs_impl.mag_zeta = zeta; + ahrs_set_mag_gains(); +} + + #ifdef AHRS_UPDATE_FW_ESTIMATOR extern float ins_roll_neutral; extern float ins_pitch_neutral; #endif - #endif /* AHRS_INT_CMPL_QUAT_H */ From a7eeb488108277058c53d4de032f7dd7ac35c558 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 4 Sep 2013 18:13:04 +0200 Subject: [PATCH 041/125] [nps] sim proper baro pressure in pascal --- .../subsystems/rotorcraft/fdm_jsbsim.makefile | 2 +- .../simulator/nps/nps_sensors_params_aspirin_1.5.h | 11 ++++------- conf/simulator/nps/nps_sensors_params_booz2_a1.h | 11 ++++------- conf/simulator/nps/nps_sensors_params_booz2_a1p.h | 11 ++++------- conf/simulator/nps/nps_sensors_params_default.h | 10 +++------- sw/simulator/nps/nps_sensor_baro.c | 14 +++++--------- sw/simulator/nps/nps_sensor_baro.h | 2 +- 7 files changed, 22 insertions(+), 39 deletions(-) diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 6633e74466..42ee416ca0 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -92,7 +92,7 @@ nps.srcs += $(SRC_FIRMWARE)/datalink.c # nps.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c -nps.CFLAGS += -DBARO_LED=2 +nps.srcs += subsystems/air_data.c nps.srcs += $(SRC_BOARD)/baro_board.c nps.CFLAGS += -DUSE_ADC diff --git a/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h b/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h index 678a33833a..d9ae3d501f 100644 --- a/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h +++ b/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h @@ -131,14 +131,11 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 + /* * GPS diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1.h b/conf/simulator/nps/nps_sensors_params_booz2_a1.h index 3464326aa8..f815d77033 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1.h @@ -130,14 +130,11 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 + /* * GPS diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1p.h b/conf/simulator/nps/nps_sensors_params_booz2_a1p.h index 3edb61d98f..7e4e88fa38 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1p.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1p.h @@ -130,14 +130,11 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 + /* * GPS diff --git a/conf/simulator/nps/nps_sensors_params_default.h b/conf/simulator/nps/nps_sensors_params_default.h index 1874378859..d0be213c4e 100644 --- a/conf/simulator/nps/nps_sensors_params_default.h +++ b/conf/simulator/nps/nps_sensors_params_default.h @@ -126,14 +126,10 @@ /* - * Barometer + * Barometer (pressure and std dev in Pascal) */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#define NPS_BARO_QNH 900. -#define NPS_BARO_SENSITIVITY 17.066667 -#define NPS_BARO_DT (1./100.) -#define NPS_BARO_NOISE_STD_DEV 5.e-2 +#define NPS_BARO_DT (1./50.) +#define NPS_BARO_NOISE_STD_DEV 2 /* * GPS diff --git a/sw/simulator/nps/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c index f0fdce2686..22ca9f9a4c 100644 --- a/sw/simulator/nps/nps_sensor_baro.c +++ b/sw/simulator/nps/nps_sensor_baro.c @@ -1,6 +1,7 @@ #include "nps_sensor_baro.h" #include "generated/airframe.h" +#include "math/pprz_isa.h" #include "std.h" #include "nps_fdm.h" @@ -21,15 +22,10 @@ void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) { if (time < baro->next_update) return; - /*if (time < 10.) - baro->value = rint(time*90); - else {*/ - double z = fdm.ltpprz_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV; - double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY; - baro_reading = rint(baro_reading); - baro->value = baro_reading; - Bound(baro->value, 0, 1024); - //} + /* pressure in Pascal */ + baro->value = pprz_isa_pressure_of_altitude(fdm.hmsl); + /* add noise with std dev Pascal */ + baro->value += get_gaussian_noise() * NPS_BARO_NOISE_STD_DEV; baro->next_update += NPS_BARO_DT; baro->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_baro.h b/sw/simulator/nps/nps_sensor_baro.h index 06630b066d..0a07c4a04c 100644 --- a/sw/simulator/nps/nps_sensor_baro.h +++ b/sw/simulator/nps/nps_sensor_baro.h @@ -7,7 +7,7 @@ #include "std.h" struct NpsSensorBaro { - double value; + double value; ///< pressure in Pascal double next_update; bool_t data_available; }; From dadd73eb7977b673bfe625aaff30b7c3c7e7586e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 4 Sep 2013 19:01:52 +0200 Subject: [PATCH 042/125] [messages] INS_REF: baro_qfe in float --- conf/messages.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/messages.xml b/conf/messages.xml index 3a9323b475..4489b858dc 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1334,7 +1334,7 @@ - + From c459bb0f3055b3db8e4bac3c8801d4cbefc2e933 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 4 Sep 2013 19:13:22 +0200 Subject: [PATCH 043/125] [ins] fix ins_baro_alt, ned frame, z-down! --- sw/airborne/subsystems/ins/ins_int.c | 2 +- sw/airborne/subsystems/ins/ins_int.h | 2 +- sw/airborne/subsystems/ins/ins_int_extended.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 4e10c36e24..a1334f3874 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -191,7 +191,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { - ins_baro_alt = pprz_isa_height_of_pressure(*pressure, ins_qfe); + ins_baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); vff_update(ins_baro_alt); } INS_NED_TO_STATE(); diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index c9a861be8b..9de003ca96 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -50,7 +50,7 @@ extern struct NedCoor_i ins_gps_speed_cm_s_ned; /* barometer */ #if USE_VFF -extern float ins_baro_alt; ///< altitude calculated from baro in meters +extern float ins_baro_alt; ///< altitude calculated from baro in meters (z-down) extern float ins_qfe; extern bool_t ins_baro_initialised; #endif diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c index 9c6677bd12..3470095461 100644 --- a/sw/airborne/subsystems/ins/ins_int_extended.c +++ b/sw/airborne/subsystems/ins/ins_int_extended.c @@ -214,7 +214,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { - ins_baro_alt = pprz_isa_height_of_pressure(*pressure, ins_qfe); + ins_baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); vff_update_baro(ins_baro_alt); } INS_NED_TO_STATE(); From 2d87a6a5f06a8542585edd17d1e8bd447239e810 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 4 Sep 2013 22:55:09 +0200 Subject: [PATCH 044/125] [ins] some cleanup and refactoring - ins_int and ins_int_extended combinded to avoid code duplication - ins_impl struct - american spelling: x_initialised -> x_initialzed - vff is mandatory for ins_int (if needed rather make a new simple gps_passthrough ins with ned coordinates) --- .../subsystems/rotorcraft/ins.makefile | 1 - .../rotorcraft/ins_extended.makefile | 4 +- .../subsystems/rotorcraft/ins_hff.makefile | 1 - sw/airborne/firmwares/rotorcraft/datalink.c | 4 +- sw/airborne/firmwares/rotorcraft/navigation.c | 10 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 64 ++-- sw/airborne/modules/cam_control/cam_track.c | 24 +- .../vehicle_interface/vi_test_signal.c | 2 +- sw/airborne/subsystems/ins/hf_float.c | 34 +- sw/airborne/subsystems/ins/hf_float.h | 2 +- sw/airborne/subsystems/ins/ins_alt_float.c | 8 +- sw/airborne/subsystems/ins/ins_alt_float.h | 2 +- sw/airborne/subsystems/ins/ins_ardrone2.c | 10 +- sw/airborne/subsystems/ins/ins_ardrone2.h | 2 +- sw/airborne/subsystems/ins/ins_int.c | 347 ++++++++++++------ sw/airborne/subsystems/ins/ins_int.h | 60 ++- sw/airborne/subsystems/ins/ins_int_extended.c | 334 ----------------- sw/airborne/subsystems/ins/ins_int_extended.h | 82 ----- sw/simulator/nps/nps_ivy.c | 4 +- 19 files changed, 331 insertions(+), 664 deletions(-) delete mode 100644 sw/airborne/subsystems/ins/ins_int_extended.c delete mode 100644 sw/airborne/subsystems/ins/ins_int_extended.h diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile index fd4878813c..e7c485cbe4 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile @@ -8,5 +8,4 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile index 6ce7f94af9..7f1809dd83 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile @@ -2,9 +2,9 @@ # extended INS with vertical filter using sonar in a better way (flap ground) # -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int_extended.h\" +$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile index fd0ca65228..f14ac11350 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile @@ -8,7 +8,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF # horizontal filter float version $(TARGET).CFLAGS += -DUSE_HFF diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index c463b3218d..2f23e27e0a 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -103,8 +103,8 @@ void dl_parse_msg(void) { lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); /* WP_alt is in cm, lla.alt in mm */ - lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); + lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_impl.ltp_def.hmsl + ins_impl.ltp_def.lla.alt; + enu_of_lla_point_i(&enu,&ins_impl.ltp_def,&lla); enu.x = POS_BFP_OF_REAL(enu.x)/100; enu.y = POS_BFP_OF_REAL(enu.y)/100; enu.z = POS_BFP_OF_REAL(enu.z)/100; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index f78723abbf..3f87aaef37 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -267,7 +267,7 @@ static inline void nav_set_altitude( void ) { /** Reset the geographic reference to the current GPS fix */ unit_t nav_reset_reference( void ) { - ins_ltp_initialised = FALSE; + ins_impl.ltp_initialized = FALSE; ins.hf_realign = TRUE; ins.vf_realign = TRUE; return 0; @@ -277,9 +277,9 @@ unit_t nav_reset_alt( void ) { ins.vf_realign = TRUE; #if USE_GPS - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_ltp_def); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_impl.ltp_def); #endif return 0; @@ -302,7 +302,7 @@ void nav_periodic_task() { /* run carrot loop */ nav_run(); - ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.); + ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.); } #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 00eab3c9b2..be41696b7c 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -589,51 +589,51 @@ &guidance_h_pos_sp.y, \ &guidance_h_pos_ref.x, \ &guidance_h_pos_ref.y, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y); \ + &ins_impl.ltp_pos.x, \ + &ins_impl.ltp_pos.y); \ } #define PERIODIC_SEND_INS_Z(_trans, _dev) { \ DOWNLINK_SEND_INS_Z(_trans, _dev, \ &ins_baro_alt, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.z); \ + &ins_impl.ltp_pos.z, \ + &ins_impl.ltp_speed.z, \ + &ins_impl.ltp_accel.z); \ } #define PERIODIC_SEND_INS(_trans, _dev) { \ DOWNLINK_SEND_INS(_trans, _dev, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.x, \ - &ins_ltp_speed.y, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.x, \ - &ins_ltp_accel.y, \ - &ins_ltp_accel.z); \ + &ins_impl.ltp_pos.x, \ + &ins_impl.ltp_pos.y, \ + &ins_impl.ltp_pos.z, \ + &ins_impl.ltp_speed.x, \ + &ins_impl.ltp_speed.y, \ + &ins_impl.ltp_speed.z, \ + &ins_impl.ltp_accel.x, \ + &ins_impl.ltp_accel.y, \ + &ins_impl.ltp_accel.z); \ } #define PERIODIC_SEND_INS_REF(_trans, _dev) { \ - if (ins_ltp_initialised) \ + if (ins_impl.ltp_initialized) \ DOWNLINK_SEND_INS_REF(_trans, _dev, \ - &ins_ltp_def.ecef.x, \ - &ins_ltp_def.ecef.y, \ - &ins_ltp_def.ecef.z, \ - &ins_ltp_def.lla.lat, \ - &ins_ltp_def.lla.lon, \ - &ins_ltp_def.lla.alt, \ - &ins_ltp_def.hmsl, \ - &ins_qfe); \ + &ins_impl.ltp_def.ecef.x, \ + &ins_impl.ltp_def.ecef.y, \ + &ins_impl.ltp_def.ecef.z, \ + &ins_impl.ltp_def.lla.lat, \ + &ins_impl.ltp_def.lla.lon, \ + &ins_impl.ltp_def.lla.alt, \ + &ins_impl.ltp_def.hmsl, \ + &ins_impl.qfe); \ } #define PERIODIC_SEND_VERT_LOOP(_trans, _dev) { \ DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \ &guidance_v_z_sp, \ &guidance_v_zd_sp, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.z, \ + &ins_impl.ltp_pos.z, \ + &ins_impl.ltp_speed.z, \ + &ins_impl.ltp_accel.z, \ &guidance_v_z_ref, \ &guidance_v_zd_ref, \ &guidance_v_zdd_ref, \ @@ -650,12 +650,12 @@ DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ &guidance_h_pos_sp.x, \ &guidance_h_pos_sp.y, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y, \ - &ins_ltp_speed.x, \ - &ins_ltp_speed.y, \ - &ins_ltp_accel.x, \ - &ins_ltp_accel.y, \ + &ins_impl.ltp_pos.x, \ + &ins_impl.ltp_pos.y, \ + &ins_impl.ltp_speed.x, \ + &ins_impl.ltp_speed.y, \ + &ins_impl.ltp_accel.x, \ + &ins_impl.ltp_accel.y, \ &guidance_h_pos_err.x, \ &guidance_h_pos_err.y, \ &guidance_h_speed_err.x, \ diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c index ae7a29abda..e933f6f62c 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -55,7 +55,7 @@ uint8_t cam_status; uint8_t cam_data_len; void track_init(void) { - ins_ltp_initialised = TRUE; // ltp is initialized and centered on the target + ins_impl.ltp_initialized = TRUE; // ltp is initialized and centered on the target ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground) cam_status = UNINIT; @@ -119,8 +119,8 @@ void track_periodic_task(void) { } void track_event(void) { - if (!ins_ltp_initialised) { - ins_ltp_initialised = TRUE; + if (!ins_impl.ltp_initialized) { + ins_impl.ltp_initialized = TRUE; ins.hf_realign = TRUE; } @@ -134,24 +134,24 @@ void track_event(void) { } const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 }; b2_hff_update_pos(-target_pos_ned, measurement_noise); - ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); + ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); INS_NED_TO_STATE(); #else // store pos in ins - ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x)); - ins_ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y)); + ins_impl.ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x)); + ins_impl.ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y)); // compute speed from last pos // TODO get delta T // store last pos VECT3_COPY(last_pos_ned, target_pos_ned); - stateSetPositionNed_i(&ins_ltp_pos); + stateSetPositionNed_i(&ins_impl.ltp_pos); #endif b2_hff_lost_counter = 0; diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.c b/sw/airborne/modules/vehicle_interface/vi_test_signal.c index 35edb58220..de29c7d25e 100644 --- a/sw/airborne/modules/vehicle_interface/vi_test_signal.c +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.c @@ -67,7 +67,7 @@ void booz_fms_impl_periodic(void) { #if 0 case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { if (guidance_v_mode < GUIDANCE_V_MODE_HOVER) - booz_fms_test_signal_start_z = ins_ltp_pos.z; + booz_fms_test_signal_start_z = ins_impl.ltp_pos.z; else { booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? booz_fms_test_signal_start_z : diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 0af2ea4569..4190cfe069 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -28,7 +28,6 @@ */ #include "subsystems/ins/hf_float.h" -#include "subsystems/ins.h" #include "subsystems/imu.h" #include "state.h" #include "subsystems/gps.h" @@ -459,14 +458,6 @@ void b2_hff_propagate(void) { b2_hff_propagate_x(&b2_hff_state); b2_hff_propagate_y(&b2_hff_state); - /* update ins state from horizontal filter */ - ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); - #ifdef GPS_LAG /* increase lag counter on last saved state */ if (b2_hff_rb_n > 0) @@ -490,7 +481,7 @@ void b2_hff_propagate(void) { -void b2_hff_update_gps(void) { +void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) { b2_hff_lost_counter = 0; #if USE_GPS_ACC4R @@ -508,20 +499,13 @@ void b2_hff_update_gps(void) { #endif /* update filter state with measurement */ - b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos); - b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos); + b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos); + b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos); #ifdef HFF_UPDATE_SPEED - b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel); - b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel); + b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel); + b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel); #endif - /* update ins state */ - ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); #ifdef GPS_LAG } else if (b2_hff_rb_n > 0) { @@ -530,11 +514,11 @@ void b2_hff_update_gps(void) { PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err)); if (abs(lag_counter_err) <= GPS_LAG_TOL_N) { b2_hff_rb_last->rollback = TRUE; - b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos); - b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos); + b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos); + b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos); #ifdef HFF_UPDATE_SPEED - b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel); - b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel); + b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel); + b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel); #endif past_save_counter = GPS_DT_N-1;// + lag_counter_err; PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter)); diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h index 2d1c08dbc9..c53ddb6b8b 100644 --- a/sw/airborne/subsystems/ins/hf_float.h +++ b/sw/airborne/subsystems/ins/hf_float.h @@ -81,7 +81,7 @@ extern float b2_hff_ydd_meas; extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot); extern void b2_hff_propagate(void); -extern void b2_hff_update_gps(void); +extern void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned); extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos); extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel); extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 29265a034c..0488c44dc7 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -50,7 +50,7 @@ float ins_alt_dot; #include "subsystems/sensors/baro.h" #include "math/pprz_isa.h" float ins_qfe; -bool_t ins_baro_initialised; +bool_t ins_baro_initialized; float ins_baro_alt; // Baro event on ABI @@ -73,7 +73,7 @@ void ins_init() { #if USE_BAROMETER ins_qfe = 0;; - ins_baro_initialised = FALSE; + ins_baro_initialized = FALSE; ins_baro_alt = 0.; // Bind to BARO_ABS message AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); @@ -100,9 +100,9 @@ void ins_update_baro() {} #if USE_BAROMETER static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { - if (!ins_baro_initialised) { + if (!ins_baro_initialized) { ins_qfe = *pressure; - ins_baro_initialised = TRUE; + ins_baro_initialized = TRUE; } if (ins.vf_realign) { ins.vf_realign = FALSE; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 785ba95d24..837ca01c35 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -58,7 +58,7 @@ extern float ins_qfe; extern float ins_baro_alt; -extern bool_t ins_baro_initialised; +extern bool_t ins_baro_initialized; #endif //USE_BAROMETER extern float ins_alt; ///< estimated altitude above MSL in meters diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index a50f0ed908..4ff1d671ac 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -54,7 +54,7 @@ struct LtpDef_i ins_ltp_def; struct NedCoor_f ins_ltp_accel; struct NedCoor_f ins_ltp_speed; -bool_t ins_ltp_initialised; +bool_t ins_ltp_initialized; void ins_init() { #if USE_INS_NAV_INIT @@ -72,9 +72,9 @@ void ins_init() { //Set the ltp stateSetLocalOrigin_i(&ins_ltp_def); - ins_ltp_initialised = TRUE; + ins_ltp_initialized = TRUE; #else - ins_ltp_initialised = FALSE; + ins_ltp_initialized = FALSE; #endif ins.vf_realign = FALSE; @@ -128,11 +128,11 @@ void ins_update_gps(void) { //Check for GPS fix if (gps.fix == GPS_FIX_3D) { //Set the initial coordinates - if(!ins_ltp_initialised) { + if(!ins_ltp_initialized) { ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); ins_ltp_def.lla.alt = gps.lla_pos.alt; ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; + ins_ltp_initialized = TRUE; stateSetLocalOrigin_i(&ins_ltp_def); } diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h index c14e362eea..7cbd6a528b 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.h +++ b/sw/airborne/subsystems/ins/ins_ardrone2.h @@ -40,6 +40,6 @@ extern struct NedCoor_i ins_ltp_pos; extern struct LtpDef_i ins_ltp_def; extern struct NedCoor_f ins_ltp_speed; extern struct NedCoor_f ins_ltp_accel; -extern bool_t ins_ltp_initialised; +extern bool_t ins_ltp_initialized; #endif /* INS_INT_H */ diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index a1334f3874..f43a24c9f9 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -35,7 +35,9 @@ #include "generated/airframe.h" -#if USE_VFF +#if USE_VFF_EXTENDED +#include "subsystems/ins/vf_extended_float.h" +#else #include "subsystems/ins/vf_float.h" #endif @@ -53,93 +55,103 @@ #include "generated/flight_plan.h" + +#if USE_SONAR + +#if !USE_VFF_EXTENDED +#error USE_SONAR needs USE_VFF_EXTENDED +#endif + +#ifdef INS_SONAR_THROTTLE_THRESHOLD +#include "firmwares/rotorcraft/stabilization.h" +#endif + +#ifndef INS_SONAR_OFFSET +#define INS_SONAR_OFFSET 0 +#endif +#define VFF_R_SONAR_0 0.1 +#define VFF_R_SONAR_OF_M 0.2 + +#endif // USE_SONAR + #ifndef USE_INS_NAV_INIT #define USE_INS_NAV_INIT TRUE PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif -/* gps transformed to LTP-NED */ -struct LtpDef_i ins_ltp_def; - bool_t ins_ltp_initialised; -struct NedCoor_i ins_gps_pos_cm_ned; -struct NedCoor_i ins_gps_speed_cm_s_ned; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -struct FloatVect2 ins_gps_pos_m_ned; -struct FloatVect2 ins_gps_speed_m_s_ned; -#endif /* barometer */ -#if USE_VFF -bool_t ins_baro_initialised; -float ins_qfe; -float ins_baro_alt; #ifndef INS_BARO_ID #define INS_BARO_ID ABI_BROADCAST #endif abi_event baro_ev; static void baro_cb(uint8_t sender_id, const float *pressure); + +struct InsInt ins_impl; + + +static void ins_init_origin_from_flightplan(void); +static void ins_ned_to_state(void); +#if USE_HFF +static void ins_update_from_hff(void); #endif -/* output */ -struct NedCoor_i ins_ltp_pos; -struct NedCoor_i ins_ltp_speed; -struct NedCoor_i ins_ltp_accel; +void ins_init(void) { -void ins_init() { #if USE_INS_NAV_INIT - ins_ltp_initialised = TRUE; - - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ - llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); - /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ - llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - - struct EcefCoor_i ecef_nav0; - ecef_of_lla_i(&ecef_nav0, &llh_nav0); - - ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); - ins_ltp_def.hmsl = NAV_ALT0; - stateSetLocalOrigin_i(&ins_ltp_def); + ins_init_origin_from_flightplan(); + ins_impl.ltp_initialized = TRUE; #else - ins_ltp_initialised = FALSE; + ins_impl.ltp_initialized = FALSE; #endif -#if USE_VFF + // Bind to BARO_ABS message AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); - ins_baro_initialised = FALSE; - vff_init(0., 0., 0.); + ins_impl.baro_initialized = FALSE; + +#if USE_SONAR + ins_impl.update_on_agl = FALSE; + init_median_filter(&ins_impl.sonar_median); + ins_impl.sonar_offset = INS_SONAR_OFFSET; #endif + ins.vf_realign = FALSE; ins.hf_realign = FALSE; + + /* init vertical and horizontal filters */ +#if USE_VFF_EXTENDED + vff_init(0., 0., 0., 0.); +#else + vff_init(0., 0., 0.); +#endif #if USE_HFF b2_hff_init(0., 0., 0., 0.); #endif - INT32_VECT3_ZERO(ins_ltp_pos); - INT32_VECT3_ZERO(ins_ltp_speed); - INT32_VECT3_ZERO(ins_ltp_accel); - // TODO correct init - ins.status = INS_RUNNING; + INT32_VECT3_ZERO(ins_impl.ltp_pos); + INT32_VECT3_ZERO(ins_impl.ltp_speed); + INT32_VECT3_ZERO(ins_impl.ltp_accel); } -void ins_periodic( void ) { +void ins_periodic(void) { + if (ins_impl.ltp_initialized) + ins.status = INS_RUNNING; } -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { #if USE_HFF +void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { b2_hff_realign(pos, speed); -#endif /* USE_HFF */ } +#else +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), + struct FloatVect2 speed __attribute__ ((unused))) {} +#endif /* USE_HFF */ -void ins_realign_v(float z __attribute__ ((unused))) { -#if USE_VFF + +void ins_realign_v(float z) { vff_realign(z); -#endif } void ins_propagate() { @@ -149,54 +161,56 @@ void ins_propagate() { struct Int32Vect3 accel_meas_ltp; INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body); -#if USE_VFF float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (ins_baro_initialised) { + if (ins_impl.baro_initialized) { vff_propagate(z_accel_meas_float); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { // feed accel from the sensors - // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); + // subtract -9.81m/s2 (acceleration measured due to gravity, + // but vehicle not accelerating in ltp) + ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); } -#else - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); -#endif /* USE_VFF */ #if USE_HFF /* propagate horizontal filter */ b2_hff_propagate(); + /* convert and copy result to ins_impl */ + ins_update_from_hff(); #else - ins_ltp_accel.x = accel_meas_ltp.x; - ins_ltp_accel.y = accel_meas_ltp.y; + ins_impl.ltp_accel.x = accel_meas_ltp.x; + ins_impl.ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ - INS_NED_TO_STATE(); + ins_ned_to_state(); } -#if USE_VFF static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { - if (!ins_baro_initialised) { - ins_qfe = *pressure; - ins_baro_initialised = TRUE; + if (!ins_impl.baro_initialized) { + ins_impl.qfe = *pressure; + ins_impl.baro_initialized = TRUE; } if (ins.vf_realign) { ins.vf_realign = FALSE; - ins_qfe = *pressure; + ins_impl.qfe = *pressure; vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); + ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); + ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); + ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff_z); } else { - ins_baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); - vff_update(ins_baro_alt); - } - INS_NED_TO_STATE(); -} + ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); +#if USE_VFF_EXTENDED + vff_update_baro(ins_impl.baro_z); +#else + vff_update(ins_impl.baro_z); #endif + } + ins_ned_to_state(); +} + void ins_update_baro() { } @@ -205,60 +219,155 @@ void ins_update_baro() { void ins_update_gps(void) { #if USE_GPS if (gps.fix == GPS_FIX_3D) { - if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); + if (!ins_impl.ltp_initialized) { + ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_impl.ltp_def); } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); + + struct NedCoor_i gps_pos_cm_ned; + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); + /// @todo maybe use gps.ned_vel directly?? + struct NedCoor_i gps_speed_cm_s_ned; + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel); + #if USE_HFF - VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); - VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); - VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); - VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); + /* horizontal gps transformed to NED in meters as float */ + struct FloatVect2 gps_pos_m_ned; + VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y); + VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.); + + struct FloatVect2 gps_speed_m_s_ned; + VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y); + VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.); + if (ins.hf_realign) { ins.hf_realign = FALSE; -#ifdef SITL - struct FloatVect2 true_pos, true_speed; - VECT2_COPY(true_pos, fdm.ltpprz_pos); - VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); - b2_hff_realign(true_pos, true_speed); -#else const struct FloatVect2 zero = {0.0, 0.0}; - b2_hff_realign(ins_gps_pos_m_ned, zero); -#endif + b2_hff_realign(gps_pos_m_ned, zero); } - b2_hff_update_gps(); -#if !USE_VFF /* vff not used */ - ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN; -#endif /* vff not used */ -#endif /* hff used */ + // run horizontal filter + b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned); + // convert and copy result to ins_impl + ins_update_from_hff(); +#else /* hff not used */ + /* simply copy horizontal pos/speed from gps */ + INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned, + INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned, + INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); +#endif /* USE_HFF */ -#if !USE_HFF /* hff not used */ -#if !USE_VFF /* neither hf nor vf used */ - INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#else /* only vff used */ - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#endif - -#if USE_GPS_LAG_HACK - VECT2_COPY(d_pos, ins_ltp_speed); - INT32_VECT2_RSHIFT(d_pos, d_pos, 11); - VECT2_ADD(ins_ltp_pos, d_pos); -#endif -#endif /* hff not used */ - - INS_NED_TO_STATE(); + ins_ned_to_state(); } #endif /* USE_GPS */ } + +//#define INS_SONAR_VARIANCE_THRESHOLD 0.01 + +#ifdef INS_SONAR_VARIANCE_THRESHOLD + +#include "messages.h" +#include "mcu_periph/uart.h" +#include "subsystems/datalink/downlink.h" + +#include "math/pprz_stat.h" +#define VAR_ERR_MAX 10 +float var_err[VAR_ERR_MAX]; +uint8_t var_idx = 0; +#endif + + void ins_update_sonar() { +#if USE_SONAR + static float last_offset = 0.; + // new value filtered with median_filter + ins_impl.sonar_alt = update_median_filter(&ins_impl.sonar_median, sonar_meas); + float sonar = (ins_impl.sonar_alt - ins_impl.sonar_offset) * INS_SONAR_SENS; + +#ifdef INS_SONAR_VARIANCE_THRESHOLD + /* compute variance of error between sonar and baro alt */ + float err = sonar + ins_impl.baro_z; // sonar positive up, baro positive down !!!! + var_err[var_idx] = err; + var_idx = (var_idx + 1) % VAR_ERR_MAX; + float var = variance_float(var_err, VAR_ERR_MAX); + DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); + //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_impl.sonar_alt, &sonar, &var); +#endif + + /* update filter assuming a flat ground */ + if (sonar < INS_SONAR_MAX_RANGE +#ifdef INS_SONAR_MIN_RANGE + && sonar > INS_SONAR_MIN_RANGE +#endif +#ifdef INS_SONAR_THROTTLE_THRESHOLD + && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD +#endif +#ifdef INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD + && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD +#endif +#ifdef INS_SONAR_BARO_THRESHOLD + && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ +#endif +#ifdef INS_SONAR_VARIANCE_THRESHOLD + && var < INS_SONAR_VARIANCE_THRESHOLD +#endif + && ins_impl.update_on_agl + && ins_impl.baro_initialized) { + vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); + last_offset = vff_offset; + } + else { + /* update offset with last value to avoid divergence */ + vff_update_offset(last_offset); + } +#endif // USE_SONAR } + + +/** initialize the local origin (ltp_def) from flight plan position */ +static void ins_init_origin_from_flightplan(void) { + + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; + + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0); + ins_impl.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_impl.ltp_def); + +} + +/** copy position and speed to state interface */ +static void ins_ned_to_state(void) { + stateSetPositionNed_i(&ins_impl.ltp_pos); + stateSetSpeedNed_i(&ins_impl.ltp_speed); + stateSetAccelNed_i(&ins_impl.ltp_accel); +} + + +#if USE_HFF +/** update ins state from horizontal filter */ +static void ins_update_from_hff(void) { + ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); +} +#endif diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index 9de003ca96..71171c5fbf 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -34,42 +34,34 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" -// TODO integrate all internal state to the structure -///** Ins implementation state (fixed point) */ -//struct InsInt { -//}; -// -///** global INS state */ -//extern struct InsInt ins_impl; - -/* gps transformed to LTP-NED */ -extern struct LtpDef_i ins_ltp_def; -extern bool_t ins_ltp_initialised; -extern struct NedCoor_i ins_gps_pos_cm_ned; -extern struct NedCoor_i ins_gps_speed_cm_s_ned; - -/* barometer */ -#if USE_VFF -extern float ins_baro_alt; ///< altitude calculated from baro in meters (z-down) -extern float ins_qfe; -extern bool_t ins_baro_initialised; +#if USE_SONAR +#include "filters/median_filter.h" #endif -/* output LTP NED */ -extern struct NedCoor_i ins_ltp_pos; -extern struct NedCoor_i ins_ltp_speed; -extern struct NedCoor_i ins_ltp_accel; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -extern struct FloatVect2 ins_gps_pos_m_ned; -extern struct FloatVect2 ins_gps_speed_m_s_ned; -#endif +/** Ins implementation state (fixed point) */ +struct InsInt { + struct LtpDef_i ltp_def; + bool_t ltp_initialized; -/* copy position and speed to state interface */ -#define INS_NED_TO_STATE() { \ - stateSetPositionNed_i(&ins_ltp_pos); \ - stateSetSpeedNed_i(&ins_ltp_speed); \ - stateSetAccelNed_i(&ins_ltp_accel); \ -} + /* output LTP NED */ + struct NedCoor_i ltp_pos; + struct NedCoor_i ltp_speed; + struct NedCoor_i ltp_accel; + + /* baro */ + float baro_z; ///< z-position calculated from baro in meters (z-down) + float qfe; + bool_t baro_initialized; + +#if USE_SONAR + bool_t update_on_agl; /* use sonar to update agl if available */ + int32_t sonar_alt; + int32_t sonar_offset; + struct MedianFilterInt sonar_median; +#endif +}; + +/** global INS state */ +extern struct InsInt ins_impl; #endif /* INS_INT_H */ diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c deleted file mode 100644 index 3470095461..0000000000 --- a/sw/airborne/subsystems/ins/ins_int_extended.c +++ /dev/null @@ -1,334 +0,0 @@ -/* - * Copyright (C) 2008-2010 The Paparazzi Team - * Copyright (C) 2012 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file subsystems/ins/ins_int_extended.c - * - * INS for rotorcrafts combining vertical and horizontal filters. - * - */ - -#include "subsystems/ins/ins_int_extended.h" - -#include "subsystems/abi.h" - -#include "subsystems/imu.h" -#include "subsystems/gps.h" - -#include "generated/airframe.h" -#include "math/pprz_algebra_int.h" -#include "math/pprz_algebra_float.h" -#include "math/pprz_geodetic_int.h" -#include "math/pprz_isa.h" - -#include "state.h" - -#include "subsystems/ins/vf_extended_float.h" -#include "filters/median_filter.h" - -#if USE_HFF -#include "subsystems/ins/hf_float.h" -#endif - -#ifdef SITL -#include "nps_fdm.h" -#include -#endif - -#ifdef INS_SONAR_THROTTLE_THRESHOLD -#include "firmwares/rotorcraft/stabilization.h" -#endif - -#include "generated/flight_plan.h" - -#ifndef USE_INS_NAV_INIT -#define USE_INS_NAV_INIT TRUE -PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") -#endif - - -/* gps transformed to LTP-NED */ -struct LtpDef_i ins_ltp_def; - bool_t ins_ltp_initialised; -struct NedCoor_i ins_gps_pos_cm_ned; -struct NedCoor_i ins_gps_speed_cm_s_ned; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -struct FloatVect2 ins_gps_pos_m_ned; -struct FloatVect2 ins_gps_speed_m_s_ned; -#endif - -/* barometer */ -bool_t ins_baro_initialised; -float ins_qfe; -float ins_baro_alt; -#ifndef INS_BARO_ID -#define INS_BARO_ID ABI_BROADCAST -#endif -abi_event baro_ev; -static void baro_cb(uint8_t sender_id, const float *pressure); - -// if median filter really needed, implement in float first -//struct MedianFilterInt baro_median; - -#if USE_SONAR -/* sonar */ -bool_t ins_update_on_agl; -int32_t ins_sonar_alt; -int32_t ins_sonar_offset; -struct MedianFilterInt sonar_median; -#ifndef INS_SONAR_OFFSET -#define INS_SONAR_OFFSET 0 -#endif -#define VFF_R_SONAR_0 0.1 -#define VFF_R_SONAR_OF_M 0.2 -#endif // USE_SONAR - -/* output */ -struct NedCoor_i ins_ltp_pos; -struct NedCoor_i ins_ltp_speed; -struct NedCoor_i ins_ltp_accel; - - -void ins_init() { -#if USE_INS_NAV_INIT - ins_ltp_initialised = TRUE; - - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ - llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); - /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ - llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - - struct EcefCoor_i ecef_nav0; - ecef_of_lla_i(&ecef_nav0, &llh_nav0); - - ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0); - ins_ltp_def.hmsl = NAV_ALT0; - stateSetLocalOrigin_i(&ins_ltp_def); -#else - ins_ltp_initialised = FALSE; -#endif - - // Bind to BARO_ABS message - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); - ins_baro_initialised = FALSE; - //init_median_filter(&baro_median); - -#if USE_SONAR - ins_update_on_agl = FALSE; - init_median_filter(&sonar_median); - ins_sonar_offset = INS_SONAR_OFFSET; -#endif - - vff_init(0., 0., 0., 0.); - ins.vf_realign = FALSE; - ins.hf_realign = FALSE; -#if USE_HFF - b2_hff_init(0., 0., 0., 0.); -#endif - INT32_VECT3_ZERO(ins_ltp_pos); - INT32_VECT3_ZERO(ins_ltp_speed); - INT32_VECT3_ZERO(ins_ltp_accel); -} - -void ins_periodic( void ) { -} - -#if USE_HFF -void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { - b2_hff_realign(pos, speed); -} -#else -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {} -#endif /* USE_HFF */ - - -void ins_realign_v(float z) { - vff_realign(z); -} - -void ins_propagate() { - /* untilt accels */ - struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); - struct Int32Vect3 accel_meas_ltp; - INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, *stateGetNedToBodyRMat_i(), accel_meas_body); - - float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); - if (ins_baro_initialised) { - vff_propagate(z_accel_meas_float); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { // feed accel from the sensors - // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp) - ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); - } - -#if USE_HFF - /* propagate horizontal filter */ - b2_hff_propagate(); -#else - ins_ltp_accel.x = accel_meas_ltp.x; - ins_ltp_accel.y = accel_meas_ltp.y; -#endif /* USE_HFF */ - - INS_NED_TO_STATE(); -} - -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { - // if median filter really needed, implement in float first - //int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute); - if (!ins_baro_initialised) { - ins_qfe = *pressure; - ins_baro_initialised = TRUE; - } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_qfe = *pressure; - vff_realign(0.); - ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot); - ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot); - ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z); - } - else { - ins_baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); - vff_update_baro(ins_baro_alt); - } - INS_NED_TO_STATE(); -} - -void ins_update_baro() { -} - - -void ins_update_gps(void) { -#if USE_GPS - if (gps.fix == GPS_FIX_3D) { - if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialised = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); - } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); -#if USE_HFF - VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); - VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); - VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y); - VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.); - if (ins.hf_realign) { - ins.hf_realign = FALSE; -#ifdef SITL - struct FloatVect2 true_pos, true_speed; - VECT2_COPY(true_pos, fdm.ltpprz_pos); - VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel); - b2_hff_realign(true_pos, true_speed); -#else - const struct FloatVect2 zero = {0.0, 0.0}; - b2_hff_realign(ins_gps_pos_m_ned, zero); -#endif - } - b2_hff_update_gps(); -#endif /* hff used */ - -#if !USE_HFF /* hff not used */ - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); -#endif /* hff not used */ - - } - INS_NED_TO_STATE(); -#endif /* USE_GPS */ -} - -//#define INS_SONAR_VARIANCE_THRESHOLD 0.01 - -#ifdef INS_SONAR_VARIANCE_THRESHOLD - -#include "messages.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" - -#include "math/pprz_stat.h" -#define VAR_ERR_MAX 10 -float var_err[VAR_ERR_MAX]; -uint8_t var_idx = 0; -#endif - - -void ins_update_sonar() { -#if USE_SONAR - static float last_offset = 0.; - // new value filtered with median_filter - ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas); - float sonar = (ins_sonar_alt - ins_sonar_offset) * INS_SONAR_SENS; - -#ifdef INS_SONAR_VARIANCE_THRESHOLD - /* compute variance of error between sonar and baro alt */ - float err = sonar + ins_baro_alt; // sonar positive up, baro positive down !!!! - var_err[var_idx] = err; - var_idx = (var_idx + 1) % VAR_ERR_MAX; - float var = variance_float(var_err, VAR_ERR_MAX); - DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); - //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_sonar_alt, &sonar, &var); -#endif - - /* update filter assuming a flat ground */ - if (sonar < INS_SONAR_MAX_RANGE -#ifdef INS_SONAR_MIN_RANGE - && sonar > INS_SONAR_MIN_RANGE -#endif -#ifdef INS_SONAR_THROTTLE_THRESHOLD - && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD -#endif -#ifdef INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD -#endif -#ifdef INS_SONAR_BARO_THRESHOLD - && ins_baro_alt > -INS_SONAR_BARO_THRESHOLD /* z down */ -#endif -#ifdef INS_SONAR_VARIANCE_THRESHOLD - && var < INS_SONAR_VARIANCE_THRESHOLD -#endif - && ins_update_on_agl - && ins_baro_initialised) { - vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); - last_offset = vff_offset; - } - else { - /* update offset with last value to avoid divergence */ - vff_update_offset(last_offset); - } -#endif // USE_SONAR -} - diff --git a/sw/airborne/subsystems/ins/ins_int_extended.h b/sw/airborne/subsystems/ins/ins_int_extended.h deleted file mode 100644 index 64d63cd419..0000000000 --- a/sw/airborne/subsystems/ins/ins_int_extended.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright (C) 2008-2012 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file subsystems/ins/ins_int_extended.h - * - * INS for rotorcrafts combining vertical and horizontal filters. - * This filter includes estimation of the baro drift based on sonar. - * - * Vertical filter is always enabled - * - * TODO: use GPS alt if good enough, especially when sonar readings are not available - * - */ - -#ifndef INS_INT_EXTENDED_H -#define INS_INT_EXTENDED_H - -#include "subsystems/ins.h" -#include "std.h" -#include "math/pprz_geodetic_int.h" -#include "math/pprz_algebra_float.h" - -// TODO integrate all internal state to the structure -///** Ins extended implementation state (fixed point) */ -//struct InsIntExtended { -//}; -// -///** global INS state */ -//extern struct InsInt ins_impl; - -/* gps transformed to LTP-NED */ -extern struct LtpDef_i ins_ltp_def; -extern bool_t ins_ltp_initialised; -extern struct NedCoor_i ins_gps_pos_cm_ned; -extern struct NedCoor_i ins_gps_speed_cm_s_ned; - -/* barometer */ -extern float ins_baro_alt; ///< altitude calculated from baro in meters -extern float ins_qfe; -extern bool_t ins_baro_initialised; -#if USE_SONAR -extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ -extern int32_t ins_sonar_offset; -#endif - -/* output LTP NED */ -extern struct NedCoor_i ins_ltp_pos; -extern struct NedCoor_i ins_ltp_speed; -extern struct NedCoor_i ins_ltp_accel; -#if USE_HFF -/* horizontal gps transformed to NED in meters as float */ -extern struct FloatVect2 ins_gps_pos_m_ned; -extern struct FloatVect2 ins_gps_speed_m_s_ned; -#endif - -/* copy position and speed to state interface */ -#define INS_NED_TO_STATE() { \ - stateSetPositionNed_i(&ins_ltp_pos); \ - stateSetSpeedNed_i(&ins_ltp_speed); \ - stateSetAccelNed_i(&ins_ltp_accel); \ -} - -#endif /* INS_INT_EXTENDED_H */ diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c index 93847c425c..6104766d07 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -126,8 +126,8 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), struct EnuCoor_i enu; lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); - lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); + lla.alt = atoi(argv[5])*10 - ins_impl.ltp_def.hmsl + ins_impl.ltp_def.lla.alt; + enu_of_lla_point_i(&enu,&ins_impl.ltp_def,&lla); enu.x = POS_BFP_OF_REAL(enu.x)/100; enu.y = POS_BFP_OF_REAL(enu.y)/100; enu.z = POS_BFP_OF_REAL(enu.z)/100; From 3aada5265d8f2cf1df9286d3a2030adca3c2381c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 16:47:02 +0200 Subject: [PATCH 045/125] [nps] sim_overwrite_ins updates --- sw/airborne/firmwares/fixedwing/main_ap.c | 2 +- sw/airborne/firmwares/rotorcraft/main.c | 5 +--- sw/airborne/subsystems/ins/ins_int.c | 10 +++++-- sw/simulator/nps/nps_autopilot.h | 7 ++++- sw/simulator/nps/nps_autopilot_fixedwing.c | 33 ++++++++++++++++++--- sw/simulator/nps/nps_autopilot_fixedwing.h | 10 ------- sw/simulator/nps/nps_autopilot_rotorcraft.c | 2 +- sw/simulator/nps/nps_autopilot_rotorcraft.h | 12 -------- 8 files changed, 46 insertions(+), 35 deletions(-) delete mode 100644 sw/simulator/nps/nps_autopilot_fixedwing.h delete mode 100644 sw/simulator/nps/nps_autopilot_rotorcraft.h diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 796c89c206..008346035d 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -89,7 +89,7 @@ #include "led.h" #ifdef USE_NPS -#include "nps_autopilot_fixedwing.h" +#include "nps_autopilot.h" #endif /* Default trim commands for roll, pitch and yaw */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 4ee0f227b7..8ce0ca30ca 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -73,7 +73,7 @@ #include "firmwares/rotorcraft/main.h" #ifdef SITL -#include "nps_autopilot_rotorcraft.h" +#include "nps_autopilot.h" #endif #include "generated/modules.h" @@ -309,9 +309,6 @@ static inline void on_gyro_event( void ) { if (nps_bypass_ahrs) sim_overwrite_ahrs(); #endif ins_propagate(); -#ifdef SITL - if (nps_bypass_ins) sim_overwrite_ins(); -#endif } #ifdef USE_VEHICLE_INTERFACE vi_notify_imu_available(); diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index f43a24c9f9..bbf65f8410 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -45,8 +45,9 @@ #include "subsystems/ins/hf_float.h" #endif -#ifdef SITL -#include "nps_fdm.h" +#if defined SITL && USE_NPS +//#include "nps_fdm.h" +#include "nps_autopilot.h" #include #endif @@ -357,6 +358,11 @@ static void ins_ned_to_state(void) { stateSetPositionNed_i(&ins_impl.ltp_pos); stateSetSpeedNed_i(&ins_impl.ltp_speed); stateSetAccelNed_i(&ins_impl.ltp_accel); + +#if defined SITL && USE_NPS + if (nps_bypass_ins) + sim_overwrite_ins(); +#endif } diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index 730c46cce0..04d8304d0b 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -11,10 +11,15 @@ struct NpsAutopilot { extern struct NpsAutopilot autopilot; +extern bool_t nps_bypass_ahrs; +extern bool_t nps_bypass_ins; +extern void sim_overwrite_ahrs(void); +extern void sim_overwrite_ins(void); + extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char* js_dev); extern void nps_autopilot_run_step(double time); extern void nps_autopilot_run_systime_step(void); + #endif /* NPS_AUTOPILOT_H */ - diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 5cf695e12c..dd0490e723 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -20,7 +20,7 @@ * Boston, MA 02111-1307, USA. */ -#include "nps_autopilot_fixedwing.h" +#include "nps_autopilot.h" #ifdef FBW #include "firmwares/fixedwing/main_fbw.h" @@ -50,14 +50,19 @@ #include "subsystems/commands.h" - struct NpsAutopilot autopilot; bool_t nps_bypass_ahrs; +bool_t nps_bypass_ins; #ifndef NPS_BYPASS_AHRS #define NPS_BYPASS_AHRS FALSE #endif +#ifndef NPS_BYPASS_INS +#define NPS_BYPASS_INS FALSE +#endif + + #if !defined (FBW) || !defined (AP) #error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! #endif @@ -66,6 +71,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha nps_radio_control_init(type_rc, num_rc_script, rc_dev); nps_bypass_ahrs = NPS_BYPASS_AHRS; + nps_bypass_ins = NPS_BYPASS_INS; Fbw(init); Ap(init); @@ -108,8 +114,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { } if (nps_sensors_baro_available()) { - /** @todo feed baro values */ - //baro_feed_value(sensors.baro.value); + baro_feed_value(sensors.baro.value); Fbw(event_task); Ap(event_task); } @@ -124,6 +129,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { sim_overwrite_ahrs(); } + if (nps_bypass_ins) { + sim_overwrite_ins(); + } + Fbw(handle_periodic_tasks); Ap(handle_periodic_tasks); @@ -144,3 +153,19 @@ void sim_overwrite_ahrs(void) { stateSetBodyRates_f(&rates_f); } + +void sim_overwrite_ins(void) { + + struct NedCoor_f ltp_pos; + VECT3_COPY(ltp_pos, fdm.ltpprz_pos); + stateSetPositionNed_f(<p_pos); + + struct NedCoor_f ltp_speed; + VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel); + stateSetSpeedNed_f(<p_speed); + + struct NedCoor_f ltp_accel; + VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel); + stateSetAccelNed_f(<p_accel); + +} diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.h b/sw/simulator/nps/nps_autopilot_fixedwing.h deleted file mode 100644 index a4d05a199b..0000000000 --- a/sw/simulator/nps/nps_autopilot_fixedwing.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef NPS_AUTOPILOT_FIXEDWING_H -#define NPS_AUTOPILOT_FIXEDWING_H - -#include "nps_autopilot.h" - - -extern bool_t nps_bypass_ahrs; -extern void sim_overwrite_ahrs(void); - -#endif /* NPS_AUTOPILOT_FIXEDWING_H */ diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 0064c2f897..aca82f52ea 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -19,7 +19,7 @@ * Boston, MA 02111-1307, USA. */ -#include "nps_autopilot_rotorcraft.h" +#include "nps_autopilot.h" #include "firmwares/rotorcraft/main.h" #include "nps_sensors.h" diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.h b/sw/simulator/nps/nps_autopilot_rotorcraft.h deleted file mode 100644 index b71ffd7eb8..0000000000 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef NPS_AUTOPILOT_ROTORCRAFT_H -#define NPS_AUTOPILOT_ROTORCRAFT_H - -#include "nps_autopilot.h" - - -extern bool_t nps_bypass_ahrs; -extern bool_t nps_bypass_ins; -extern void sim_overwrite_ahrs(void); -extern void sim_overwrite_ins(void); - -#endif /* NPS_AUTOPILOT_ROTORCRAFT_H */ From ddb2e04cb64ef12437bcde125258007737fcff19 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 17:38:45 +0200 Subject: [PATCH 046/125] [conf] baro_board makefile for both fw and rotorcraft --- conf/firmwares/rotorcraft.makefile | 97 +-------------- .../subsystems/fixedwing/autopilot.makefile | 28 +---- .../subsystems/shared/baro_board.makefile | 114 ++++++++++++++++++ 3 files changed, 116 insertions(+), 123 deletions(-) create mode 100644 conf/firmwares/subsystems/shared/baro_board.makefile diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index bf809b3a8f..1dd8959ae4 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -154,102 +154,7 @@ ap.srcs += subsystems/actuators.c # ap.srcs += subsystems/air_data.c -# booz baro -ifeq ($(BOARD), booz) -ap.srcs += $(SRC_BOARD)/baro_board.c -else ifeq ($(BOARD), lisa_l) -ap.CFLAGS += -DUSE_I2C2 -ap.srcs += $(SRC_BOARD)/baro_board.c - -# Ardrone baro -else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) -ap.srcs += $(SRC_BOARD)/baro_board.c - -# Lisa/M baro -else ifeq ($(BOARD), lisa_m) -# defaults to i2c baro bmp085 on the board -LISA_M_BARO ?= BARO_BOARD_BMP085 - ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) - include $(CFG_SHARED)/spi_master.makefile - ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) - ap.CFLAGS += -DUSE_I2C2 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) - ap.srcs += peripherals/bmp085.c - ap.srcs += $(SRC_BOARD)/baro_board.c - ap.CFLAGS += -DUSE_I2C2 - endif - ap.CFLAGS += -D$(LISA_M_BARO) - -# Lisa/S baro -else ifeq ($(BOARD), lisa_s) -# defaults to SPI baro MS5611 on the board - include $(CFG_SHARED)/spi_master.makefile - ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 - ap.CFLAGS += -DMS5611_SPI_DEV=spi1 - ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - ap.CFLAGS += -DBARO_MS5611_SPI - -# Lia baro (no bmp onboard) -else ifeq ($(BOARD), lia) -# fixme, reuse the baro drivers in lisa_m dir -LIA_BARO ?= BARO_MS5611_SPI - ifeq ($(LIA_BARO), BARO_MS5611_SPI) - include $(CFG_SHARED)/spi_master.makefile - ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - else ifeq ($(LIA_BARO), BARO_MS5611_I2C) - ap.CFLAGS += -DUSE_I2C2 - ap.srcs += peripherals/ms5611.c - ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - endif - ap.CFLAGS += -D$(LIA_BARO) - -# navgo baro -else ifeq ($(BOARD), navgo) -include $(CFG_SHARED)/spi_master.makefile -ap.CFLAGS += -DUSE_SPI_SLAVE0 -ap.CFLAGS += -DUSE_SPI1 -ap.srcs += peripherals/mcp355x.c -ap.srcs += $(SRC_BOARD)/baro_board.c - -# krooz baro -else ifeq ($(BOARD), krooz) -ap.CFLAGS += -DMS5611_I2C_DEV=i2c2 -DMS5611_SLAVE_ADDR=0xEC -ap.srcs += peripherals/ms5611.c -ap.srcs += peripherals/ms5611_i2c.c -ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - -else ifeq ($(BOARD), px4fmu) -ap.CFLAGS += -DUSE_I2C2 -DMS5611_I2C_DEV=i2c2 -ap.srcs += peripherals/ms5611.c -ap.srcs += peripherals/ms5611_i2c.c -ap.srcs += subsystems/sensors/baro_ms5611_i2c.c - -# apogee baro -else ifeq ($(BOARD), apogee) -ap.CFLAGS += -DUSE_I2C1 -ap.CFLAGS += -DMPL3115_I2C_DEV=i2c1 -ap.CFLAGS += -DMPL3115_ALT_MODE=0 -ap.srcs += peripherals/mpl3115.c -ap.srcs += $(SRC_BOARD)/baro_board.c -endif - -ifneq ($(BARO_LED),none) -ap.CFLAGS += -DBARO_LED=$(BARO_LED) -endif +include $(CFG_SHARED)/baro_board.makefile # # Analog Backend diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index d9f33a408c..8de067522a 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -160,33 +160,7 @@ ap_srcs += $(SRC_ARCH)/subsystems/settings_arch.c ap_srcs += subsystems/air_data.c # BARO - -# Umarim -ifeq ($(BOARD), umarim) -ifeq ($(BOARD_VERSION), 1.0) -ap_srcs += boards/umarim/baro_board.c -ap_CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 -ap_CFLAGS += -DADS1114_I2C_DEV=i2c1 -ap_srcs += peripherals/ads1114.c -endif - -# Lisa/L -else ifeq ($(BOARD), lisa_l) -ap_CFLAGS += -DUSE_I2C2 - -# Apogee -else ifeq ($(BOARD), apogee) -ap_CFLAGS += -DUSE_I2C1 -ap_CFLAGS += -DMPL3115_I2C_DEV=i2c1 -ap_CFLAGS += -DMPL3115_ALT_MODE=0 -ap_srcs += peripherals/mpl3115.c -ap_srcs += $(SRC_BOARD)/baro_board.c - -endif # End baro - -ifneq ($(BARO_LED),none) -ap_CFLAGS += -DBARO_LED=$(BARO_LED) -endif +include $(CFG_SHARED)/baro_board.makefile # ahrs frequencies if configured ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile new file mode 100644 index 0000000000..5c4bd92e2c --- /dev/null +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -0,0 +1,114 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Drivers for baros already on an autopilot board + + +# booz baro +ifeq ($(BOARD), booz) + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Lisa/L +else ifeq ($(BOARD), lisa_l) + ap.CFLAGS += -DUSE_I2C2 + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Ardrone baro +else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Lisa/M baro +else ifeq ($(BOARD), lisa_m) +# defaults to i2c baro bmp085 on the board +LISA_M_BARO ?= BARO_BOARD_BMP085 + ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c + else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) + ap.CFLAGS += -DUSE_I2C2 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) + ap.srcs += peripherals/bmp085.c + ap.srcs += $(SRC_BOARD)/baro_board.c + ap.CFLAGS += -DUSE_I2C2 + endif + ap.CFLAGS += -D$(LISA_M_BARO) + +# Lisa/S baro +else ifeq ($(BOARD), lisa_s) +# defaults to SPI baro MS5611 on the board + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 + ap.CFLAGS += -DMS5611_SPI_DEV=spi1 + ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c + ap.CFLAGS += -DBARO_MS5611_SPI + +# Lia baro (no bmp onboard) +else ifeq ($(BOARD), lia) +# fixme, reuse the baro drivers in lisa_m dir +LIA_BARO ?= BARO_MS5611_SPI + ifeq ($(LIA_BARO), BARO_MS5611_SPI) + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c + else ifeq ($(LIA_BARO), BARO_MS5611_I2C) + ap.CFLAGS += -DUSE_I2C2 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + endif + ap.CFLAGS += -D$(LIA_BARO) + +# navgo baro +else ifeq ($(BOARD), navgo) + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI_SLAVE0 + ap.CFLAGS += -DUSE_SPI1 + ap.srcs += peripherals/mcp355x.c + ap.srcs += $(SRC_BOARD)/baro_board.c + +# krooz baro +else ifeq ($(BOARD), krooz) + ap.CFLAGS += -DMS5611_I2C_DEV=i2c2 -DMS5611_SLAVE_ADDR=0xEC + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + +# PX4FMU +else ifeq ($(BOARD), px4fmu) + ap.CFLAGS += -DUSE_I2C2 -DMS5611_I2C_DEV=i2c2 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + +# apogee baro +else ifeq ($(BOARD), apogee) + ap.CFLAGS += -DUSE_I2C1 + ap.CFLAGS += -DMPL3115_I2C_DEV=i2c1 + ap.CFLAGS += -DMPL3115_ALT_MODE=0 + ap.srcs += peripherals/mpl3115.c + ap.srcs += $(SRC_BOARD)/baro_board.c + +# Umarim +else ifeq ($(BOARD), umarim) + ifeq ($(BOARD_VERSION), 1.0) + ap.srcs += boards/umarim/baro_board.c + ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 + ap.CFLAGS += -DADS1114_I2C_DEV=i2c1 + ap.srcs += peripherals/ads1114.c + endif + +endif # End baro + +ifneq ($(BARO_LED),none) +ap.CFLAGS += -DBARO_LED=$(BARO_LED) +endif From 9deae5e6733b0d7cbfe95787d8114e8f53f651b4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 17:39:52 +0200 Subject: [PATCH 047/125] [baro] fix baro_board ms5611 wrappers --- .../subsystems/sensors/baro_ms5611_i2c.c | 77 +++++++++--------- .../subsystems/sensors/baro_ms5611_spi.c | 78 +++++++++---------- 2 files changed, 76 insertions(+), 79 deletions(-) diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c index a3c446e4bd..1679daf816 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c @@ -33,6 +33,15 @@ #include "mcu_periph/sys_time.h" #include "led.h" #include "std.h" +#include "subsystems/abi.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + #ifndef MS5611_I2C_DEV #define MS5611_I2C_DEV i2c2 @@ -48,71 +57,61 @@ #define MS5611_SLAVE_ADDR 0xEE #endif -#ifdef DEBUG -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -float fbaroms, ftempms; +#ifndef BARO_BOARD_MS5611_SENDER_ID +#define BARO_BOARD_MS5611_SENDER_ID 7 #endif -struct Baro baro; -struct Ms5611_I2c baro_ms5611; +struct Ms5611_I2c bb_ms5611; void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; + ms5611_i2c_init(&bb_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR); - ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR); +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif } void baro_periodic(void) { if (sys_time.nb_sec > 1) { /* call the convenience periodic that initializes the sensor and starts reading*/ - ms5611_i2c_periodic(&baro_ms5611); + ms5611_i2c_periodic(&bb_ms5611); - if (baro_ms5611.initialized) { - baro.status = BS_RUNNING; #if DEBUG - RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &baro_ms5611.data.c[0], - &baro_ms5611.data.c[1], - &baro_ms5611.data.c[2], - &baro_ms5611.data.c[3], - &baro_ms5611.data.c[4], - &baro_ms5611.data.c[5], - &baro_ms5611.data.c[6], - &baro_ms5611.data.c[7])); + if (bb_ms5611.initialized) + RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &bb_ms5611.data.c[0], + &bb_ms5611.data.c[1], + &bb_ms5611.data.c[2], + &bb_ms5611.data.c[3], + &bb_ms5611.data.c[4], + &bb_ms5611.data.c[5], + &bb_ms5611.data.c[6], + &bb_ms5611.data.c[7]); #endif - } } } -void baro_event(void (*b_abs_handler)(void)){ +void baro_event(void){ if (sys_time.nb_sec > 1) { - ms5611_i2c_event(&baro_ms5611); + ms5611_i2c_event(&bb_ms5611); - if (baro_ms5611.data_available) { - baro.absolute = baro_ms5611.data.pressure; - b_abs_handler(); - baro_ms5611.data_available = FALSE; + if (bb_ms5611.data_available) { + float pressure = (float)bb_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_BOARD_MS5611_SENDER_ID, &pressure); + bb_ms5611.data_available = FALSE; -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); #endif #if DEBUG - ftempms = baro_ms5611.data.temperature / 100.; - fbaroms = baro_ms5611.data.pressure / 100.; + float ftempms = bb_ms5611.data.temperature / 100.; + float fbaroms = bb_ms5611.data.pressure / 100.; DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, + &bb_ms5611.data.d1, &bb_ms5611.data.d2, &fbaroms, &ftempms); #endif } diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c index bd8603ea43..09c8b0008b 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c +++ b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c @@ -33,6 +33,15 @@ #include "mcu_periph/sys_time.h" #include "led.h" #include "std.h" +#include "subsystems/abi.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + #ifndef MS5611_SPI_DEV #define MS5611_SPI_DEV spi2 @@ -46,71 +55,60 @@ #endif -#ifdef DEBUG - -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -float fbaroms, ftempms; +#ifndef BARO_BOARD_MS5611_SENDER_ID +#define BARO_BOARD_MS5611_SENDER_ID 7 #endif -struct Baro baro; -struct Ms5611_Spi baro_ms5611; +struct Ms5611_Spi bb_ms5611; void baro_init(void) { - baro.status = BS_UNINITIALIZED; - baro.absolute = 0; - baro.differential = 0; + ms5611_spi_init(&bb_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV); - ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV); +#ifdef BARO_LED + LED_OFF(BARO_LED); +#endif } void baro_periodic(void) { if (sys_time.nb_sec > 1) { /* call the convenience periodic that initializes the sensor and starts reading*/ - ms5611_spi_periodic(&baro_ms5611); + ms5611_spi_periodic(&bb_ms5611); - if (baro_ms5611.initialized) { - baro.status = BS_RUNNING; #if DEBUG - RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &baro_ms5611.data.c[0], - &baro_ms5611.data.c[1], - &baro_ms5611.data.c[2], - &baro_ms5611.data.c[3], - &baro_ms5611.data.c[4], - &baro_ms5611.data.c[5], - &baro_ms5611.data.c[6], - &baro_ms5611.data.c[7])); + if (bb_ms5611.initialized) + RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &bb_ms5611.data.c[0], + &bb_ms5611.data.c[1], + &bb_ms5611.data.c[2], + &bb_ms5611.data.c[3], + &bb_ms5611.data.c[4], + &bb_ms5611.data.c[5], + &bb_ms5611.data.c[6], + &bb_ms5611.data.c[7]); #endif - } } } -void baro_event(void (*b_abs_handler)(void)){ +void baro_event(){ if (sys_time.nb_sec > 1) { - ms5611_spi_event(&baro_ms5611); + ms5611_spi_event(&bb_ms5611); - if (baro_ms5611.data_available) { - baro.absolute = baro_ms5611.data.pressure; - b_abs_handler(); - baro_ms5611.data_available = FALSE; + if (bb_ms5611.data_available) { + float pressure = (float)bb_ms5611.data.pressure; + AbiSendMsgBARO_ABS(BARO_BOARD_MS5611_SENDER_ID, &pressure); + bb_ms5611.data_available = FALSE; -#ifdef ROTORCRAFT_BARO_LED - RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED)); +#ifdef BARO_LED + RunOnceEvery(10,LED_TOGGLE(BARO_LED)); #endif #if DEBUG - ftempms = baro_ms5611.data.temperature / 100.; - fbaroms = baro_ms5611.data.pressure / 100.; + float ftempms = bb_ms5611.data.temperature / 100.; + float fbaroms = bb_ms5611.data.pressure / 100.; DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, + &bb_ms5611.data.d1, &bb_ms5611.data.d2, &fbaroms, &ftempms); #endif } From 0845b6f53349b88ad42cbfcfe2e74bebd1eb8cb9 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 19:15:50 +0200 Subject: [PATCH 048/125] [conf] fix nps settings file --- conf/settings/nps.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml index 8fb6ed6513..5c2b502593 100644 --- a/conf/settings/nps.xml +++ b/conf/settings/nps.xml @@ -4,8 +4,8 @@ - - + + From 4eced1b096867bf7c36eb4d356cd0ad50e3e9be8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 19:17:10 +0200 Subject: [PATCH 049/125] [ins] clean ins_ardrone2 --- sw/airborne/subsystems/ins/ins_ardrone2.c | 84 ++++++++++------------- sw/airborne/subsystems/ins/ins_ardrone2.h | 26 ++++--- 2 files changed, 50 insertions(+), 60 deletions(-) diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index 4ff1d671ac..cdce78e65d 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -41,53 +41,39 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif - -/* TODO: implement in state */ -int32_t ins_qfe; -int32_t ins_baro_alt; - -//Keep track of gps pos and the init pos -struct NedCoor_i ins_ltp_pos; -struct LtpDef_i ins_ltp_def; - -// Keep track of INS LTP accel and speed -struct NedCoor_f ins_ltp_accel; -struct NedCoor_f ins_ltp_speed; - -bool_t ins_ltp_initialized; +struct InsArdrone2 ins_impl; void ins_init() { #if USE_INS_NAV_INIT - struct LlaCoor_i llh_nav; + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */ - llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0); - llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0); - llh_nav.alt = NAV_ALT0 + NAV_MSL0; + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); - //Convert ltp - ltp_def_from_lla_i(&ins_ltp_def, &llh_nav); - ins_ltp_def.hmsl = NAV_ALT0; + ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0); + ins_impl.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_impl.ltp_def); - //Set the ltp - stateSetLocalOrigin_i(&ins_ltp_def); - - ins_ltp_initialized = TRUE; + ins_impl.ltp_initialized = TRUE; #else - ins_ltp_initialized = FALSE; + ins_impl.ltp_initialized = FALSE; #endif ins.vf_realign = FALSE; ins.hf_realign = FALSE; - INT32_VECT3_ZERO(ins_ltp_pos); - - // TODO correct init - ins.status = INS_RUNNING; + INT32_VECT3_ZERO(ins_impl.ltp_pos); + INT32_VECT3_ZERO(ins_impl.ltp_speed); + INT32_VECT3_ZERO(ins_impl.ltp_accel); } void ins_periodic( void ) { - + if (ins_impl.ltp_initialized) + ins.status = INS_RUNNING; } void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { @@ -100,21 +86,21 @@ void ins_realign_v(float z __attribute__ ((unused))) { void ins_propagate() { /* untilt accels and speeds */ - FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel); - FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed); + FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel); + FLOAT_RMAT_VECT3_TRANSP_MUL(ins_impl.ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed); //Add g to the accelerations - ins_ltp_accel.z += 9.81; + ins_impl.ltp_accel.z += 9.81; //Save the accelerations and speeds - stateSetAccelNed_f(&ins_ltp_accel); - stateSetSpeedNed_f(&ins_ltp_speed); + stateSetAccelNed_f(&ins_impl.ltp_accel); + stateSetSpeedNed_f(&ins_impl.ltp_speed); //Don't set the height if we use the one from the gps #if !USE_GPS_HEIGHT //Set the height and save the position - ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - stateSetPositionNed_i(&ins_ltp_pos); + ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; + stateSetPositionNed_i(&ins_impl.ltp_pos); #endif } @@ -128,27 +114,27 @@ void ins_update_gps(void) { //Check for GPS fix if (gps.fix == GPS_FIX_3D) { //Set the initial coordinates - if(!ins_ltp_initialized) { - ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); - ins_ltp_def.lla.alt = gps.lla_pos.alt; - ins_ltp_def.hmsl = gps.hmsl; - ins_ltp_initialized = TRUE; - stateSetLocalOrigin_i(&ins_ltp_def); + if(!ins_impl.ltp_initialized) { + ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_impl.ltp_def); } //Set the x and y and maybe z position in ltp and save struct NedCoor_i ins_gps_pos_cm_ned; - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); //When we don't want to use the height of the navdata we can use the gps height #if USE_GPS_HEIGHT - INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT3_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #else - INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #endif //Set the local origin - stateSetPositionNed_i(&ins_ltp_pos); + stateSetPositionNed_i(&ins_impl.ltp_pos); } #endif /* USE_GPS */ } diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h index 7cbd6a528b..617c2845f3 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.h +++ b/sw/airborne/subsystems/ins/ins_ardrone2.h @@ -24,22 +24,26 @@ * INS implementation for ardrone2-sdk. */ -#ifndef INS_INT_H -#define INS_INT_H +#ifndef INS_ARDRONE2_SDK_H +#define INS_ARDRONE2_SDK_H #include "subsystems/ins.h" #include "std.h" #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" -//TODO: implement in state -extern int32_t ins_qfe; -extern int32_t ins_baro_alt; +struct InsArdrone2 { + struct LtpDef_i ltp_def; + bool_t ltp_initialized; -extern struct NedCoor_i ins_ltp_pos; -extern struct LtpDef_i ins_ltp_def; -extern struct NedCoor_f ins_ltp_speed; -extern struct NedCoor_f ins_ltp_accel; -extern bool_t ins_ltp_initialized; + float qfe; ///< not used, only dummy for INS_REF message -#endif /* INS_INT_H */ + /* output LTP NED */ + struct NedCoor_i ltp_pos; + struct NedCoor_i ltp_speed; + struct NedCoor_i ltp_accel; +}; + +extern struct InsArdrone2 ins_impl; + +#endif /* INS_ARDRONE2_SDK_H */ From 1e50aacf20175d96005ecc2ccadffab9d2c3d2cd Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 19:17:15 +0200 Subject: [PATCH 050/125] [rotorcraft] add nav_move_waypoint_lla function and use it in MOVE_WP in nps and datalink --- sw/airborne/firmwares/rotorcraft/datalink.c | 27 +++++++++---------- sw/airborne/firmwares/rotorcraft/navigation.c | 21 ++++++++++++--- sw/airborne/firmwares/rotorcraft/navigation.h | 1 + sw/simulator/nps/nps_ivy_rotorcraft.c | 18 +++++-------- 4 files changed, 38 insertions(+), 29 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index 2f23e27e0a..96001f4198 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -49,7 +49,7 @@ #include "firmwares/rotorcraft/navigation.h" #include "math/pprz_geodetic_int.h" -#include "subsystems/ins.h" +#include "state.h" #define IdOfMsg(x) (x[1]) @@ -97,19 +97,18 @@ void dl_parse_msg(void) { { uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer); if (ac_id != AC_ID) break; - uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); - struct LlaCoor_i lla; - struct EnuCoor_i enu; - lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); - lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); - /* WP_alt is in cm, lla.alt in mm */ - lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_impl.ltp_def.hmsl + ins_impl.ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_impl.ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); + if (stateIsLocalCoordinateValid()) { + uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); + struct LlaCoor_i lla; + lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); + lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); + /* WP_alt from message is alt above MSL in cm + * lla.alt is above ellipsoid in mm + */ + lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - state.ned_origin_i.hmsl + + state.ned_origin_i.lla.alt; + nav_move_waypoint_lla(wp_id, &lla); + } } break; #endif /* USE_NAVIGATION */ diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 62fb47b6ab..ceef6c7ded 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -44,6 +44,10 @@ #include "math/pprz_algebra_int.h" +#include "subsystems/datalink/downlink.h" +#include "messages.h" +#include "mcu_periph/uart.h" + const uint8_t nb_waypoint = NB_WAYPOINT; struct EnuCoor_i waypoints[NB_WAYPOINT]; @@ -305,13 +309,22 @@ void nav_periodic_task() { ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.); } -#include "subsystems/datalink/downlink.h" -#include "messages.h" -#include "mcu_periph/uart.h" +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) { + if (stateIsLocalCoordinateValid()) { + struct EnuCoor_i enu; + enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos); + enu.x = POS_BFP_OF_REAL(enu.x)/100; + enu.y = POS_BFP_OF_REAL(enu.y)/100; + enu.z = POS_BFP_OF_REAL(enu.z)/100; + nav_move_waypoint(wp_id, &enu); + } +} + void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) { if (wp_id < nb_waypoint) { INT32_VECT3_COPY(waypoints[wp_id],(*new_pos)); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z)); + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), + &(new_pos->y), &(new_pos->z)); } } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index b8df4ee553..120e574de0 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -79,6 +79,7 @@ void compute_dist2_to_home(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_reset_alt( void ) __attribute__ ((unused)); void nav_periodic_task(void); +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos); void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos); bool_t nav_detect_ground(void); bool_t nav_is_in_flight(void); diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c index bf0ef5562b..afb4764d7d 100644 --- a/sw/simulator/nps/nps_ivy_rotorcraft.c +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -8,8 +8,8 @@ #include "nps_autopilot.h" #include "nps_fdm.h" #include "nps_sensors.h" -#include "subsystems/ins.h" #include "firmwares/rotorcraft/navigation.h" +#include "state.h" #ifdef RADIO_CONTROL_TYPE_DATALINK #include "subsystems/radio_control.h" @@ -31,7 +31,6 @@ void nps_ivy_init(char* ivy_bus) { } -//TODO use datalink parsing from rotorcraft instead of doing it here explicitly #include "generated/settings.h" #include "dl_protocol.h" @@ -43,16 +42,13 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), uint8_t wp_id = atoi(argv[1]); struct LlaCoor_i lla; - struct EnuCoor_i enu; lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); - lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); - printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z); + /* WP_alt from message is alt above MSL in cm + * lla.alt is above ellipsoid in mm + */ + lla.alt = atoi(argv[5]) *10 - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; + nav_move_waypoint_lla(wp_id, &lla); + printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, waypoints[wp_id].x, waypoints[wp_id].y, waypoints[wp_id].z); } } From 5e4f494806314991b59842d7970f770e737f530b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Sep 2013 21:32:40 +0200 Subject: [PATCH 051/125] [baro] some ms5611 cleanup - move ms5611 baro board wrappers to boards dir - no defaults devices for baro_board_ms5611_x wrappers - rename MS5611_SLAVE_DEV to MS5611_SLAVE_IDX --- .../subsystems/shared/baro_board.makefile | 41 +++++++++++-------- conf/modules/baro_ms5611_spi.xml | 2 +- .../baro_board_ms5611_i2c.c} | 14 +++---- .../baro_board_ms5611_spi.c} | 18 ++------ sw/airborne/modules/sensors/baro_ms5611_spi.c | 6 +-- 5 files changed, 35 insertions(+), 46 deletions(-) rename sw/airborne/{subsystems/sensors/baro_ms5611_i2c.c => boards/baro_board_ms5611_i2c.c} (92%) rename sw/airborne/{subsystems/sensors/baro_ms5611_spi.c => boards/baro_board_ms5611_spi.c} (89%) diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index 5c4bd92e2c..ac93af396a 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -23,32 +23,33 @@ LISA_M_BARO ?= BARO_BOARD_BMP085 ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) include $(CFG_SHARED)/spi_master.makefile ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 + ap.CFLAGS += -DBB_MS5611_SPI_DEV=spi2 + ap.CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE3 ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c + ap.srcs += boards/baro_board_ms5611_spi.c else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) - ap.srcs += peripherals/bmp085.c - ap.srcs += $(SRC_BOARD)/baro_board.c ap.CFLAGS += -DUSE_I2C2 + ap.srcs += peripherals/bmp085.c + ap.srcs += $(SRC_BOARD)/baro_board.c endif - ap.CFLAGS += -D$(LISA_M_BARO) # Lisa/S baro else ifeq ($(BOARD), lisa_s) # defaults to SPI baro MS5611 on the board include $(CFG_SHARED)/spi_master.makefile ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 - ap.CFLAGS += -DMS5611_SPI_DEV=spi1 - ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 + ap.CFLAGS += -DBB_MS5611_SPI_DEV=spi1 + ap.CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE1 ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c - ap.CFLAGS += -DBARO_MS5611_SPI + ap.srcs += boards/baro_board_ms5611_spi.c # Lia baro (no bmp onboard) else ifeq ($(BOARD), lia) @@ -57,16 +58,18 @@ LIA_BARO ?= BARO_MS5611_SPI ifeq ($(LIA_BARO), BARO_MS5611_SPI) include $(CFG_SHARED)/spi_master.makefile ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 + ap.CFLAGS += -DBB_MS5611_SPI_DEV=spi2 + ap.CFLAGS += -DBB_MS5611_SLAVE_IDX=SPI_SLAVE3 ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_spi.c - ap.srcs += subsystems/sensors/baro_ms5611_spi.c + ap.srcs += boards/baro_board_ms5611_spi.c else ifeq ($(LIA_BARO), BARO_MS5611_I2C) ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c endif - ap.CFLAGS += -D$(LIA_BARO) # navgo baro else ifeq ($(BOARD), navgo) @@ -78,17 +81,19 @@ else ifeq ($(BOARD), navgo) # krooz baro else ifeq ($(BOARD), krooz) - ap.CFLAGS += -DMS5611_I2C_DEV=i2c2 -DMS5611_SLAVE_ADDR=0xEC + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + ap.CFLAGS += -DBB_MS5611_SLAVE_ADDR=0xEC ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c # PX4FMU else ifeq ($(BOARD), px4fmu) - ap.CFLAGS += -DUSE_I2C2 -DMS5611_I2C_DEV=i2c2 + ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 ap.srcs += peripherals/ms5611.c ap.srcs += peripherals/ms5611_i2c.c - ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + ap.srcs += boards/baro_board_ms5611_i2c.c # apogee baro else ifeq ($(BOARD), apogee) @@ -101,10 +106,10 @@ else ifeq ($(BOARD), apogee) # Umarim else ifeq ($(BOARD), umarim) ifeq ($(BOARD_VERSION), 1.0) - ap.srcs += boards/umarim/baro_board.c ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1 ap.CFLAGS += -DADS1114_I2C_DEV=i2c1 - ap.srcs += peripherals/ads1114.c + ap.srcs += peripherals/ads1114.c + ap.srcs += boards/umarim/baro_board.c endif endif # End baro diff --git a/conf/modules/baro_ms5611_spi.xml b/conf/modules/baro_ms5611_spi.xml index 2ec153e554..281bc6f94f 100644 --- a/conf/modules/baro_ms5611_spi.xml +++ b/conf/modules/baro_ms5611_spi.xml @@ -7,7 +7,7 @@ Measurement Specialties MS5611-01BA pressure sensor (SPI) - +
diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c similarity index 92% rename from sw/airborne/subsystems/sensors/baro_ms5611_i2c.c rename to sw/airborne/boards/baro_board_ms5611_i2c.c index 1679daf816..c5e6158e30 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -21,9 +21,9 @@ */ /** - * @file subsystems/sensors/baro_ms5611_i2c.c + * @file boards/baro_board_ms5611_i2c.c * - * Driver for MS5611 baro via I2C. + * Driver for onboard MS5611 baro via I2C. * */ @@ -43,18 +43,14 @@ #include "subsystems/datalink/downlink.h" -#ifndef MS5611_I2C_DEV -#define MS5611_I2C_DEV i2c2 -#endif - /* default i2c address * when CSB is set to GND addr is 0xEE * when CSB is set to VCC addr is 0xEC * * Note: Aspirin 2.1 has CSB bound to GND. */ -#ifndef MS5611_SLAVE_ADDR -#define MS5611_SLAVE_ADDR 0xEE +#ifndef BB_MS5611_SLAVE_ADDR +#define BB_MS5611_SLAVE_ADDR 0xEE #endif @@ -66,7 +62,7 @@ struct Ms5611_I2c bb_ms5611; void baro_init(void) { - ms5611_i2c_init(&bb_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR); + ms5611_i2c_init(&bb_ms5611, &BB_MS5611_I2C_DEV, BB_MS5611_SLAVE_ADDR); #ifdef BARO_LED LED_OFF(BARO_LED); diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c similarity index 89% rename from sw/airborne/subsystems/sensors/baro_ms5611_spi.c rename to sw/airborne/boards/baro_board_ms5611_spi.c index 09c8b0008b..3de17c1cca 100644 --- a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -21,9 +21,9 @@ */ /** - * @file subsystems/sensors/baro_ms5611_spi.c + * @file boards/baro_board_ms5611_spi.c * - * Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI. + * Driver for onboard MS5611 baro via SPI. * */ @@ -43,18 +43,6 @@ #include "subsystems/datalink/downlink.h" -#ifndef MS5611_SPI_DEV -#define MS5611_SPI_DEV spi2 -#endif - -/* SPI SLAVE3 is on pin PC13 - * Aspirin 2.2 has ms5611 on SPI bus - */ -#ifndef MS5611_SLAVE_DEV -#define MS5611_SLAVE_DEV SPI_SLAVE3 -#endif - - #ifndef BARO_BOARD_MS5611_SENDER_ID #define BARO_BOARD_MS5611_SENDER_ID 7 #endif @@ -63,7 +51,7 @@ struct Ms5611_Spi bb_ms5611; void baro_init(void) { - ms5611_spi_init(&bb_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV); + ms5611_spi_init(&bb_ms5611, &BB_MS5611_SPI_DEV, BB_MS5611_SLAVE_IDX); #ifdef BARO_LED LED_OFF(BARO_LED); diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index 826cab9ec1..e15fcec945 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -47,8 +47,8 @@ #define MS5611_SPI_DEV spi1 #endif -#ifndef MS5611_SLAVE_DEV -#define MS5611_SLAVE_DEV SPI_SLAVE0 +#ifndef MS5611_SLAVE_IDX +#define MS5611_SLAVE_IDX SPI_SLAVE0 #endif @@ -64,7 +64,7 @@ float baro_ms5611_sigma2; void baro_ms5611_init(void) { - ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV); + ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_IDX); baro_ms5611_enabled = TRUE; baro_ms5611_alt_valid = FALSE; From 54c14aef3b57aaa6d2b40fee6b8c7fcfe7fcd22b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 16:12:37 +0200 Subject: [PATCH 052/125] [tests] fix test_baro --- conf/firmwares/lisa_test_progs.makefile | 33 +++++++++++++++---------- sw/airborne/lisa/test_baro_i2c.c | 8 ++++-- 2 files changed, 26 insertions(+), 15 deletions(-) diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile index 5a18345359..3fdac78122 100644 --- a/conf/firmwares/lisa_test_progs.makefile +++ b/conf/firmwares/lisa_test_progs.makefile @@ -190,6 +190,8 @@ test_baro.srcs += $(COMMON_TELEMETRY_SRCS) test_baro.CFLAGS += -I$(SRC_LISA) +test_baro.srcs += subsystems/air_data.c + ifeq ($(BOARD), lisa_l) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c @@ -200,32 +202,37 @@ test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c else ifeq ($(BOARD), lisa_m) # defaults to i2c baro bmp085 on the board LISA_M_BARO ?= BARO_BOARD_BMP085 - ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) - include $(CFG_SHARED)/spi_master.makefile - test_baro.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - test_baro.srcs += peripherals/ms5611.c - test_baro.srcs += peripherals/ms5611_spi.c - test_baro.srcs += subsystems/sensors/baro_ms5611_spi.c - test_baro.srcs += $(SRC_LISA)/test_baro_spi.c - else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) + ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + test_baro.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 test_baro.srcs += peripherals/ms5611.c test_baro.srcs += peripherals/ms5611_i2c.c - test_baro.srcs += subsystems/sensors/baro_ms5611_i2c.c + test_baro.srcs += boards/baro_board_ms5611_i2c.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) - test_baro.CFLAGS += -DUSE_I2C2 + test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c test_baro.srcs += peripherals/bmp085.c test_baro.srcs += $(SRC_BOARD)/baro_board.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c endif - test_baro.CFLAGS += -D$(LISA_M_BARO) + +# Lia baro (no bmp onboard) +else ifeq ($(BOARD), lia) +LIA_BARO ?= BARO_MS5611_SPI + ifeq ($(LIA_BARO), BARO_MS5611_I2C) + test_baro.CFLAGS += -DUSE_I2C2 + test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + test_baro.CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + test_baro.srcs += peripherals/ms5611.c + test_baro.srcs += peripherals/ms5611_i2c.c + test_baro.srcs += boards/baro_board_ms5611_i2c.c + test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c + endif + endif - - # # test_rc_spektrum : sends RADIO_CONTROL messages on telemetry # diff --git a/sw/airborne/lisa/test_baro_i2c.c b/sw/airborne/lisa/test_baro_i2c.c index 250bf89c33..5d5b4e13fa 100644 --- a/sw/airborne/lisa/test_baro_i2c.c +++ b/sw/airborne/lisa/test_baro_i2c.c @@ -36,8 +36,12 @@ #include "subsystems/datalink/downlink.h" #include "subsystems/sensors/baro.h" +#include "subsystems/air_data.h" //#include "my_debug_servo.h" +#define ABI_C +#include "subsystems/abi.h" + static inline void main_init( void ); static inline void main_periodic_task( void ); static inline void main_event_task( void ); @@ -107,7 +111,7 @@ static inline void main_periodic_task( void ) { static inline void main_event_task( void ) { - BaroEvent(main_on_baro_abs, main_on_baro_diff); + baro_event(); } @@ -117,5 +121,5 @@ static inline void main_on_baro_diff(void) { } static inline void main_on_baro_abs(void) { - RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);}); + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &air_data.pressure, &air_data.differential);}); } From ee86ce1ad4259659dd7597044e7ff6c241e8c8c7 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 16:15:25 +0200 Subject: [PATCH 053/125] [jsbsim] fix baro --- conf/firmwares/subsystems/fixedwing/autopilot.makefile | 2 ++ 1 file changed, 2 insertions(+) diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 8de067522a..e0368401d4 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -223,6 +223,8 @@ endif jsbsim.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS) jsbsim.srcs += $(fbw_srcs) $(ap_srcs) +jsbsim.srcs += $(SRC_BOARD)/baro_board.c + jsbsim.CFLAGS += -DSITL -DUSE_JSBSIM jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_ac_flightgear.c From 8e3a7b7a3191d9b59eb1e16726b7789b41bfacc8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 16:16:16 +0200 Subject: [PATCH 054/125] [conf] remove obsolete baro_board module from airframes --- conf/airframes/ENAC/fixed-wing/funjet2.xml | 3 --- conf/airframes/ENAC/fixed-wing/mythe.xml | 1 - conf/airframes/examples/Twinstar_energyadaptive.xml | 3 --- conf/airframes/examples/easystar_ets.xml | 3 --- conf/airframes/examples/quadrotor_navgo.xml | 3 ++- conf/airframes/examples/stm32f4_discovery_test.xml | 3 --- 6 files changed, 2 insertions(+), 14 deletions(-) diff --git a/conf/airframes/ENAC/fixed-wing/funjet2.xml b/conf/airframes/ENAC/fixed-wing/funjet2.xml index 56455152a5..670c51f99a 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet2.xml @@ -15,9 +15,6 @@ - diff --git a/conf/airframes/ENAC/fixed-wing/mythe.xml b/conf/airframes/ENAC/fixed-wing/mythe.xml index f6c226b11b..58d0beaac0 100644 --- a/conf/airframes/ENAC/fixed-wing/mythe.xml +++ b/conf/airframes/ENAC/fixed-wing/mythe.xml @@ -9,7 +9,6 @@ - diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index d58dbd1fb1..66e236d673 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -53,9 +53,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed - - - diff --git a/conf/airframes/examples/easystar_ets.xml b/conf/airframes/examples/easystar_ets.xml index a66406c0c7..ea1d6b70d2 100644 --- a/conf/airframes/examples/easystar_ets.xml +++ b/conf/airframes/examples/easystar_ets.xml @@ -37,9 +37,6 @@ - - - diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 98df961abd..e7f5a5ad05 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -36,6 +36,8 @@ + +
@@ -117,7 +119,6 @@
- diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index 2f10e1c38b..d5da1a6a6c 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -35,9 +35,6 @@ - - - From 1bb10aceae4ff0a637b4d57cae40bf3fe72119cf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 16:17:03 +0200 Subject: [PATCH 055/125] [conf] fix some fixedwing ins makefiles --- .../fixedwing/ins_gps_passthrough.makefile | 14 ++++-- .../subsystems/fixedwing/ins_xsens.makefile | 2 + .../fixedwing/ins_xsens700.makefile | 43 +++++++++++-------- 3 files changed, 37 insertions(+), 22 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile index f6f1c628f9..0ab3fc9cca 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile @@ -1,6 +1,14 @@ +# Hey Emacs, this is a -*- makefile -*- -ap.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +ins_srcs += $(SRC_SUBSYSTEMS)/ins.c +ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c -sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c -nps.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +ap.CFLAGS += $(ins_CFLAGS) +ap.srcs += $(ins_srcs) + +sim.CFLAGS += $(ins_CFLAGS) +sim.srcs += $(ins_srcs) + +nps.CFLAGS += $(ins_CFLAGS) +nps.srcs += $(ins_srcs) diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index 4440f92bfd..b9589d04c5 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -28,6 +28,7 @@ ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR) ap.CFLAGS += -DINS_LINK=UART$(XSENS_UART_NR) ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD) ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 +ap.srcs += $(SRC_SUBSYSTEMS)/ins.c ap.srcs += $(SRC_MODULES)/ins/ins_xsens.c ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP @@ -56,6 +57,7 @@ $(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c $(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile index a28dbe30dc..eb064c36a9 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile @@ -50,9 +50,8 @@ else ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=B230400 endif -ap.CFLAGS += -DUSE_GPS_XSENS -ap.CFLAGS += -DGPS_NB_CHANNELS=50 ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 +ap.srcs += $(SRC_SUBSYSTEMS)/ins.c ap.srcs += $(SRC_MODULES)/ins/ins_xsens700.c ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP @@ -68,28 +67,34 @@ fbw.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\" endif -ifeq ($(TARGET), sim) - -sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR - -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -endif - ######################################### ## GPS # ap.CFLAGS += -DGPS +ap.CFLAGS += -DUSE_GPS_XSENS +ap.CFLAGS += -DGPS_NB_CHANNELS=50 +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c + +######################################### +## Simulator +SIM_TARGETS = sim jsbsim nps + +ifneq (,$(findstring $(TARGET),$(SIM_TARGETS))) + +$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" +$(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c + +$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c -sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c - - - - +endif From 49ab4df3aa362c4d5c20f36d6265365fd2682270 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 16:45:17 +0200 Subject: [PATCH 056/125] [ins] ins.status to running for fw ins --- sw/airborne/subsystems/ins/ins_alt_float.c | 1 + sw/airborne/subsystems/ins/ins_gps_passthrough.c | 2 ++ 2 files changed, 3 insertions(+) diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 0488c44dc7..b12c50b66f 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -82,6 +82,7 @@ void ins_init() { EstimatorSetAlt(0.); + ins.status = INS_RUNNING; } void ins_periodic( void ) { diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index c274b32024..ab1d5356f1 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -38,6 +38,8 @@ void ins_init() { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); + + ins.status = INS_RUNNING; } void ins_periodic( void ) { From 6ab91ace9b1e8c3a64e884162a6fe42f04959e00 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 16:45:52 +0200 Subject: [PATCH 057/125] [baro_board] disable onboard baro with --- .../subsystems/shared/baro_board.makefile | 14 +++++++++++++- sw/airborne/boards/apogee_0.99.h | 4 +++- sw/airborne/boards/apogee_1.0.h | 5 ++++- sw/airborne/boards/ardrone2_raw.h | 4 ++++ sw/airborne/boards/booz_1.0.h | 3 +++ sw/airborne/boards/krooz_sd.h | 6 +++++- sw/airborne/boards/lisa_l_1.0.h | 4 ++++ sw/airborne/boards/lisa_m_1.0.h | 6 +++++- sw/airborne/boards/lisa_m_2.0.h | 3 +++ sw/airborne/boards/navgo_1.0.h | 4 ++++ sw/airborne/boards/umarim_1.0.h | 4 ++++ 11 files changed, 52 insertions(+), 5 deletions(-) diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index ac93af396a..e9ffc56381 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -3,6 +3,12 @@ # Drivers for baros already on an autopilot board +# check that onboard baro was not explicitly disabled with +# + +USE_BARO_BOARD ?= TRUE +ifeq ($(USE_BARO_BOARD), TRUE) + # booz baro ifeq ($(BOARD), booz) ap.srcs += $(SRC_BOARD)/baro_board.c @@ -112,8 +118,14 @@ else ifeq ($(BOARD), umarim) ap.srcs += boards/umarim/baro_board.c endif -endif # End baro +endif # check board ifneq ($(BARO_LED),none) ap.CFLAGS += -DBARO_LED=$(BARO_LED) endif + +else # USE_BARO_BOARD is not TRUE, was explicitly disabled + +ap.CFLAGS += -DUSE_BARO_BOARD=FALSE + +endif # check USE_BARO_BOARD diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h index 2ea2d92b4d..6e22c73192 100644 --- a/sw/airborne/boards/apogee_0.99.h +++ b/sw/airborne/boards/apogee_0.99.h @@ -183,8 +183,10 @@ //#define SPI_SELECT_SLAVE5_PORT GPIOC //#define SPI_SELECT_SLAVE5_PIN GPIO4 -/* Activate onboard baro */ +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif /* PWM */ #define PWM_USE_TIM2 1 diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h index bf77163d64..7fc32b63d0 100644 --- a/sw/airborne/boards/apogee_1.0.h +++ b/sw/airborne/boards/apogee_1.0.h @@ -253,8 +253,11 @@ #define I2C2_GPIO_SDA GPIO11 -/* Activate onboard baro */ +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif + /* PWM */ #define PWM_USE_TIM2 1 diff --git a/sw/airborne/boards/ardrone2_raw.h b/sw/airborne/boards/ardrone2_raw.h index 30024b58d5..b60b41efc4 100644 --- a/sw/airborne/boards/ardrone2_raw.h +++ b/sw/airborne/boards/ardrone2_raw.h @@ -9,6 +9,10 @@ #define ActuatorsDefaultInit() ActuatorsArdroneInit() #define ActuatorsDefaultCommit() ActuatorsArdroneCommit() + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_ARDRONE2_RAW */ diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 5dc6479522..70fc889bc3 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -182,6 +182,9 @@ #endif +/* by default enable onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_BOOZ2_V1_0_H */ diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 289d763230..5c12ecef7e 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -224,8 +224,12 @@ } #endif // USE_AD1 -/* Activate onboard baro */ + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif + /* PWM */ #define PWM_USE_TIM3 1 diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index 1c109b84ce..bb21346971 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -156,7 +156,11 @@ #define BOARD_ADC_CHANNEL_3 0 #define BOARD_ADC_CHANNEL_4 15 + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif /* Default actuators driver */ diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index e683048f29..e4f48c3ab2 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -111,7 +111,7 @@ #endif /* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */ -// FIXME, this is not very nice, is also stm lib specific +// FIXME, this is not very nice, is also libopencm3 lib specific #ifdef USE_AD1 #define ADC1_GPIO_INIT(gpio) { \ gpio_set_mode(GPIOC, GPIO_MODE_INPUT, \ @@ -120,6 +120,10 @@ } #endif // USE_AD1 + +/* by default enable onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_LISA_M_1_0_H */ diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h index 1885f3eda9..de0664a950 100644 --- a/sw/airborne/boards/lisa_m_2.0.h +++ b/sw/airborne/boards/lisa_m_2.0.h @@ -165,6 +165,9 @@ #endif // USE_AD1 +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_LISA_M_2_0_H */ diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h index 0f9c922d29..8cdfd75591 100644 --- a/sw/airborne/boards/navgo_1.0.h +++ b/sw/airborne/boards/navgo_1.0.h @@ -105,6 +105,10 @@ #define SERVO_REG_1 PWMMR5 #endif + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_NAVGO_V1_0_H */ diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h index 5b97725349..b3859e2897 100644 --- a/sw/airborne/boards/umarim_1.0.h +++ b/sw/airborne/boards/umarim_1.0.h @@ -114,6 +114,10 @@ #define SPI1_DRDY_EINT 0 #define SPI1_DRDY_VIC_IT VIC_EINT0 + +/* by default enable onboard baro */ +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_UMARIM_V1_0_H */ From f106f62596617f22ead8d4fbd532728b86d821d0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 17:04:29 +0200 Subject: [PATCH 058/125] [tests] finally fix test_baro --- sw/airborne/lisa/test_baro_i2c.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/lisa/test_baro_i2c.c b/sw/airborne/lisa/test_baro_i2c.c index 5d5b4e13fa..1bcff1dde1 100644 --- a/sw/airborne/lisa/test_baro_i2c.c +++ b/sw/airborne/lisa/test_baro_i2c.c @@ -111,7 +111,7 @@ static inline void main_periodic_task( void ) { static inline void main_event_task( void ) { - baro_event(); + BaroEvent(); } From 37a59a425e5a1c1cc057269e7ca05bf6bc7898f1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 17:13:42 +0200 Subject: [PATCH 059/125] [math] convert PPRZ_ISA_M_OF_P_CONST from define to static const float so it doesn't have to be recomputed every time --- sw/airborne/math/pprz_isa.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h index a5b6f0e0e7..28c397975b 100644 --- a/sw/airborne/math/pprz_isa.h +++ b/sw/airborne/math/pprz_isa.h @@ -46,7 +46,7 @@ #define PPRZ_ISA_GRAVITY 9.80665 #define PPRZ_ISA_AIR_GAS_CONSTANT (8.31447/0.0289644) -#define PPRZ_ISA_M_OF_P_CONST (PPRZ_ISA_AIR_GAS_CONSTANT*PPRZ_ISA_SEA_LEVEL_TEMP/PPRZ_ISA_GRAVITY) +static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT*PPRZ_ISA_SEA_LEVEL_TEMP/PPRZ_ISA_GRAVITY); /** * Get absolute altitude from pressure (using simplified equation). From aa4bb5c28302f55519eaa6d9bedfe0cb60777d08 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 17:21:07 +0200 Subject: [PATCH 060/125] [conf] remove all INS_BARO_SENS this is not needed anymore. Instead every baro sensor has to output pressure in Pascal. --- conf/airframes/CDW/asctec_cdw.xml | 1 - conf/airframes/CDW/tricopter_cdw.xml | 1 - conf/airframes/ENAC/fixed-wing/firestorm.xml | 1 - conf/airframes/ENAC/fixed-wing/weasel.xml | 1 - conf/airframes/ENAC/quadrotor/blender.xml | 1 - conf/airframes/ENAC/quadrotor/booz2_g1.xml | 1 - conf/airframes/ENAC/quadrotor/hen1.xml | 1 - conf/airframes/NoVa_L.xml | 1 - conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml | 1 - conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml | 1 - conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml | 1 - conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml | 1 - conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml | 1 - conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml | 1 - conf/airframes/ardrone2_raw.xml | 1 - conf/airframes/booz2_flixr.xml | 1 - conf/airframes/booz2_ppzuav.xml | 1 - conf/airframes/esden/cocto_lm2a2.xml | 1 - conf/airframes/esden/gain_scheduling_example.xml | 1 - conf/airframes/esden/hexy_ll11a2pwm.xml | 1 - conf/airframes/esden/hexy_lm2a2pwm.xml | 1 - conf/airframes/esden/lisa2_hex.xml | 1 - conf/airframes/esden/qs_asp22.xml | 1 - conf/airframes/esden/quady_ll11a2pwm.xml | 1 - conf/airframes/esden/quady_lm1a1pwm.xml | 1 - conf/airframes/esden/quady_lm2a2pwm.xml | 1 - conf/airframes/esden/quady_lm2a2pwmppm.xml | 1 - conf/airframes/esden/quady_ls01pwm.xml | 1 - conf/airframes/examples/Twinstar_energyadaptive.xml | 1 - conf/airframes/examples/booz2.xml | 1 - conf/airframes/examples/easystar_ets.xml | 1 - conf/airframes/examples/h_hex.xml | 1 - conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml | 1 - conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml | 1 - conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml | 1 - conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml | 1 - conf/airframes/examples/lisa_asctec.xml | 1 - conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml | 1 - .../examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml | 1 - conf/airframes/examples/quadrotor_lisa_m_mkk.xml | 1 - conf/airframes/examples/quadrotor_lisa_s.xml | 1 - conf/airframes/examples/quadrotor_mlkf.xml | 1 - conf/airframes/examples/quadrotor_navgo.xml | 1 - conf/airframes/examples/quadrotor_px4fmu.xml | 1 - conf/airframes/examples/quadshot_asp21_spektrum.xml | 1 - conf/airframes/examples/stm32f4_discovery_test.xml | 1 - conf/airframes/fraser_lisa_m_rotorcraft.xml | 1 - conf/airframes/obsolete/ENAC/g1_vision.xml | 1 - conf/airframes/obsolete/ENAC/mkk1-vision.xml | 1 - conf/airframes/obsolete/ENAC/mkk1.xml | 1 - conf/airframes/obsolete/ENAC/nova1.xml | 1 - .../obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml | 1 - .../obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml | 1 - conf/airframes/obsolete/Poine/booz2_a1.xml | 1 - conf/airframes/obsolete/Poine/booz2_a7.xml | 1 - conf/airframes/obsolete/Poine/h_hex.xml | 1 - conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml | 1 - conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml | 1 - conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml | 1 - conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml | 1 - conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml | 1 - conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml | 1 - conf/airframes/obsolete/booz2_Aron.xml | 1 - conf/airframes/obsolete/booz2_NoVa.xml | 1 - conf/airframes/obsolete/booz2_NoVa_001.xml | 1 - conf/airframes/obsolete/booz2_NoVa_002.xml | 1 - conf/airframes/obsolete/booz2_a1p.xml | 1 - conf/airframes/obsolete/booz2_a2.xml | 1 - conf/airframes/obsolete/booz2_a3.xml | 1 - conf/airframes/obsolete/booz2_a4.xml | 1 - conf/airframes/obsolete/booz2_a5.xml | 1 - conf/airframes/obsolete/booz2_s1.xml | 1 - conf/airframes/obsolete/booz2_x1.xml | 1 - conf/airframes/obsolete/example_heli_lisam.xml | 1 - conf/airframes/obsolete/mm/rotor/qmk1.xml | 2 -- conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml | 1 - conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml | 1 - conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml | 1 - sw/airborne/modules/sensors/baro_amsys.c | 2 +- 79 files changed, 1 insertion(+), 80 deletions(-) diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index 830c176d2f..b366bcb90a 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 86946f11f3..ca48ac1ec1 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -121,7 +121,6 @@ LiPo/LiIo-Zellen: 2-3
-
diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml index 222712c259..4f9abc4b4c 100644 --- a/conf/airframes/ENAC/fixed-wing/firestorm.xml +++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml index f15a4b5c41..91d220785e 100644 --- a/conf/airframes/ENAC/fixed-wing/weasel.xml +++ b/conf/airframes/ENAC/fixed-wing/weasel.xml @@ -120,7 +120,6 @@
-
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 2dce3cc671..7136bc411d 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -142,7 +142,6 @@
- diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 834132e720..ee1b52add3 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -177,7 +177,6 @@
- diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 2c0bdb49ae..16a2e39f82 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -127,7 +127,6 @@
-
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml index aaf45e3e27..bc7e33c41c 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/NoVa_L.xml @@ -189,7 +189,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index ace2179575..b9bb7d2ab2 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -145,7 +145,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 064c01ae5a..989341a4ad 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -147,7 +147,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index a11d9f889b..4817a01d36 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -131,7 +131,6 @@
-
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index 4e7dda0a2e..c843d8dd41 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -134,7 +134,6 @@
-
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml index ee9e0848ac..1766366e50 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml @@ -221,7 +221,6 @@
-
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml index 06f2f97307..17a36534f7 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml @@ -221,7 +221,6 @@
-
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 99265a1638..7b66b4e517 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -104,7 +104,6 @@
-
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index a62ace8aee..379e112587 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -190,7 +190,6 @@
-
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml index af532c20f2..a34b282fd0 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/booz2_ppzuav.xml @@ -188,7 +188,6 @@
-
diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 609e7295bd..3745d83a5b 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -170,7 +170,6 @@ B2L -> CW
-
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index 02f574bd9f..b731662fba 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -153,7 +153,6 @@
-
diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index f796f42c68..f011c0e02f 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -167,7 +167,6 @@
-
diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 73554f3209..6078dbf311 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -128,7 +128,6 @@
-
diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index b21dddffcb..36bed33636 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -135,7 +135,6 @@
-
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index c4ef62b938..b6d90d7b42 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -161,7 +161,6 @@
-
diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index ca80e5a5d2..b33c992432 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -163,7 +163,6 @@
-
diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index 406734e757..82e8505f01 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index e87c817b3e..9d6fd8ff2e 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -127,7 +127,6 @@
-
diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index 31fd65c39d..ed6df64b95 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -124,7 +124,6 @@
-
diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 4a3a194c39..ef54cfdd47 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -127,7 +127,6 @@
-
diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index 66e236d673..b65867a8df 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -121,7 +121,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
-
diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index e93ccf9d2c..82d8b1b746 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -148,7 +148,6 @@
-
diff --git a/conf/airframes/examples/easystar_ets.xml b/conf/airframes/examples/easystar_ets.xml index ea1d6b70d2..4f23f35360 100644 --- a/conf/airframes/examples/easystar_ets.xml +++ b/conf/airframes/examples/easystar_ets.xml @@ -19,7 +19,6 @@ - diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml index 5faaedebe3..893b038012 100644 --- a/conf/airframes/examples/h_hex.xml +++ b/conf/airframes/examples/h_hex.xml @@ -147,7 +147,6 @@
-
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 44f2681826..b571a508e7 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -119,7 +119,6 @@
-
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index 4bf55eeb9f..a75893dd26 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -122,7 +122,6 @@
-
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index ebdda35837..53ebf044a0 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -112,7 +112,6 @@
-
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index cf85a484c0..ca6118751c 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -108,7 +108,6 @@
-
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 5e4869cb87..d6ae4f8628 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -146,7 +146,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index ea03629dd7..53bbf56cce 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -102,7 +102,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml index 06a69c6cf2..33619c4a57 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -104,7 +104,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 4746f8f78d..654d30fc8c 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -94,7 +94,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index 6cee5b9cfa..db0f89502a 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -145,7 +145,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 8778f766e9..83866d79ab 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -100,7 +100,6 @@
-
diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index e7f5a5ad05..d36f1925c1 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -128,7 +128,6 @@
- diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index a67d1738fd..301cae338f 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -98,7 +98,6 @@
-
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index d5fc8439fd..69386a3d99 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -184,7 +184,6 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index d5da1a6a6c..0e191f57f9 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -97,7 +97,6 @@
-
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 7fec69072c..0fdfd315c1 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -138,7 +138,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index d24b588c2e..3ffcf3ec2c 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -130,7 +130,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/mkk1-vision.xml b/conf/airframes/obsolete/ENAC/mkk1-vision.xml index 4d78dbd80b..eccb54ea99 100644 --- a/conf/airframes/obsolete/ENAC/mkk1-vision.xml +++ b/conf/airframes/obsolete/ENAC/mkk1-vision.xml @@ -163,7 +163,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml index 18a9742bc7..99f0d73b7d 100644 --- a/conf/airframes/obsolete/ENAC/mkk1.xml +++ b/conf/airframes/obsolete/ENAC/mkk1.xml @@ -193,7 +193,6 @@
-
diff --git a/conf/airframes/obsolete/ENAC/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml index 0ef106ba39..1bd3135b95 100644 --- a/conf/airframes/obsolete/ENAC/nova1.xml +++ b/conf/airframes/obsolete/ENAC/nova1.xml @@ -164,7 +164,6 @@
-
diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index 0c2cfe06c0..c8f68b580e 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -149,7 +149,6 @@
-
diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index 6d60135e55..b5b958a8ed 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -166,7 +166,6 @@
-
diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 5823e29e03..6a66e8dfe2 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -142,7 +142,6 @@
-
diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index d3f23c31fd..d7fe9909c6 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -161,7 +161,6 @@
-
diff --git a/conf/airframes/obsolete/Poine/h_hex.xml b/conf/airframes/obsolete/Poine/h_hex.xml index 252cdacf33..7eb7fe8fee 100644 --- a/conf/airframes/obsolete/Poine/h_hex.xml +++ b/conf/airframes/obsolete/Poine/h_hex.xml @@ -128,7 +128,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 10b61e41a0..9276b95eff 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -216,7 +216,6 @@ second attempt
-
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml index 5f3ee2a945..57642ca562 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml @@ -153,7 +153,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index 406d6de2c0..f22da3d19a 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml index 3328cc4b18..c350ad5ef2 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 82f499852b..4202753b78 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -217,7 +217,6 @@ second attempt
-
diff --git a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml index 0cecb4fdb2..01c4ed126f 100644 --- a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml @@ -132,7 +132,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml index a5a48f5295..754f24d400 100644 --- a/conf/airframes/obsolete/booz2_Aron.xml +++ b/conf/airframes/obsolete/booz2_Aron.xml @@ -134,7 +134,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml index aa12a6dfdd..928ac58841 100644 --- a/conf/airframes/obsolete/booz2_NoVa.xml +++ b/conf/airframes/obsolete/booz2_NoVa.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml index 81c9021e83..088bf5a7ec 100644 --- a/conf/airframes/obsolete/booz2_NoVa_001.xml +++ b/conf/airframes/obsolete/booz2_NoVa_001.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml index ed879fb742..1f268ba327 100644 --- a/conf/airframes/obsolete/booz2_NoVa_002.xml +++ b/conf/airframes/obsolete/booz2_NoVa_002.xml @@ -171,7 +171,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index 18ff3d98f8..22d5124d79 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -146,7 +146,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml index d0f9361638..df65d7bb25 100644 --- a/conf/airframes/obsolete/booz2_a2.xml +++ b/conf/airframes/obsolete/booz2_a2.xml @@ -134,7 +134,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml index 0635e3ce89..086855cb34 100644 --- a/conf/airframes/obsolete/booz2_a3.xml +++ b/conf/airframes/obsolete/booz2_a3.xml @@ -121,7 +121,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml index e9cc7ede69..883dd43ef9 100644 --- a/conf/airframes/obsolete/booz2_a4.xml +++ b/conf/airframes/obsolete/booz2_a4.xml @@ -99,7 +99,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml index 6fc2fe07eb..e0c43ad6ee 100644 --- a/conf/airframes/obsolete/booz2_a5.xml +++ b/conf/airframes/obsolete/booz2_a5.xml @@ -138,7 +138,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 4cf6bb4bbe..c4a37b4cf7 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -130,7 +130,6 @@
-
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml index 370920cd85..ee626f8deb 100644 --- a/conf/airframes/obsolete/booz2_x1.xml +++ b/conf/airframes/obsolete/booz2_x1.xml @@ -141,7 +141,6 @@
-
diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index 79e8d519ac..c0d13704d0 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -181,7 +181,6 @@ AP_MODE_NAV
-
diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 37b3b6c07a..3d05729aa2 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -141,8 +141,6 @@
- -
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml index 5d1b3e397d..3e43cd0eda 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml @@ -119,7 +119,6 @@
-
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml index 5343031357..0bac6a6e4e 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml @@ -82,7 +82,6 @@
-
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml index 2ba487a784..f2e2ee5a09 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml @@ -89,7 +89,6 @@
-
diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index c5dd55502f..c92edb3f1e 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -144,8 +144,8 @@ void baro_amsys_read_periodic( void ) { } pBaroRaw = 0; baro_amsys_altitude = gps.hmsl / 1000.0; - baro_amsys_adc = baro_amsys_offset - ((baro_amsys_altitude - ground_alt) / INS_BARO_SENS); baro_amsys_p = pprz_isa_pressure_of_altitude(baro_amsys_altitude); + baro_amsys_adc = baro_amsys_p; AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); baro_amsys_valid = TRUE; #endif From e31901900f25dec7bb9c99fe92bcedfcb2a496ee Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 18:16:27 +0200 Subject: [PATCH 061/125] [ahrs] configurable gravity_heuristic_factor if set to zero, gravity heuristic is disabled - removed AHRS_USE_GRAVITY_UPDATE_NORM_HEURISTIC --- .../examples/krooz_sd/krooz_sd_hexa_mkk.xml | 2 +- .../examples/krooz_sd/krooz_sd_okto_mkk.xml | 2 +- .../examples/krooz_sd/krooz_sd_quad_mkk.xml | 2 +- .../examples/krooz_sd/krooz_sd_quad_pwm.xml | 2 +- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 2 +- conf/airframes/examples/quadrotor_mlkf.xml | 4 +-- .../examples/stm32f4_discovery_test.xml | 2 +- conf/airframes/fraser_lisa_m_rotorcraft.xml | 2 +- .../estimation/ahrs_int_cmpl_quat.xml | 2 +- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 17 ++++------ sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h | 16 ++++++--- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 18 +++++----- .../subsystems/ahrs/ahrs_int_cmpl_quat.h | 33 ++++++++++++++++--- 13 files changed, 63 insertions(+), 41 deletions(-) diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 428fa2fe44..9b36d7a0f2 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -112,7 +112,7 @@
- + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index ff2a2c3510..0fde4a1238 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -115,7 +115,7 @@
- + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index 9166f503b2..115801cade 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -105,7 +105,7 @@
- + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index df4334d751..571700196d 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -101,7 +101,7 @@
- + diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 8881af6298..0fb7f99e9d 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -35,7 +35,7 @@ - + diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 151464b016..17208c6594 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -27,9 +27,7 @@ - - - + diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index 2f10e1c38b..98acc16d07 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -24,7 +24,7 @@ - + diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index d57a70036b..e1662703b4 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -30,7 +30,7 @@ - + diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml index 7c18312931..600de04e88 100644 --- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml +++ b/conf/settings/estimation/ahrs_int_cmpl_quat.xml @@ -4,7 +4,7 @@ - + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 0ff2abbc07..b4ab80b7cd 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -91,8 +91,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #endif /** by default use the gravity heuristic to reduce gain */ -#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC -#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE +#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR +#define AHRS_GRAVITY_HEURISTIC_FACTOR 30 #endif #ifdef AHRS_UPDATE_FW_ESTIMATOR @@ -146,11 +146,7 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif -#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC - ahrs_impl.use_gravity_heuristic = TRUE; -#else - ahrs_impl.use_gravity_heuristic = FALSE; -#endif + ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); } @@ -263,16 +259,15 @@ void ahrs_update_accel(void) { VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.use_gravity_heuristic) { + if (ahrs_impl.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. - * e.g. for WEIGHT_FACTOR 3: + * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ -#define WEIGHT_FACTOR 3 const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81; - ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_impl.weight, 0.15, 1.0); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index 5c22c81331..a3b83ba6b7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -38,16 +38,22 @@ struct AhrsFloatCmpl { struct FloatRates imu_rate; struct FloatQuat ltp_to_imu_quat; struct FloatRMat ltp_to_imu_rmat; - /* for gravity correction during coordinated turns */ - float ltp_vel_norm; + + bool_t correct_gravity; ///< enable gravity correction during coordinated turns + float ltp_vel_norm; ///< velocity norm for gravity correction during coordinated turns + bool_t ltp_vel_norm_valid; + float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + + /** sets how strongly the gravity heuristic reduces accel correction. + * Set to zero in order to disable gravity heuristic. + */ + uint8_t gravity_heuristic_factor; float weight; - bool_t ltp_vel_norm_valid; - bool_t correct_gravity; - bool_t use_gravity_heuristic; + bool_t heading_aligned; struct FloatVect3 mag_h; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 7f3e17ac42..5f89196323 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -94,8 +94,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_OMEGA) PRINT_CONFIG_VAR(AHRS_MAG_ZETA) /** by default use the gravity heuristic to reduce gain */ -#ifndef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC -#define AHRS_GRAVITY_UPDATE_NORM_HEURISTIC TRUE +#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR +#define AHRS_GRAVITY_HEURISTIC_FACTOR 30 #endif @@ -141,7 +141,7 @@ void ahrs_init(void) { ahrs_set_mag_gains(); /* set default gravity heuristic */ - ahrs_impl.weight = 1.0; + ahrs_impl.gravity_heuristic_factor = AHRS_GRAVITY_HEURISTIC_FACTOR; #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN ahrs_impl.correct_gravity = TRUE; @@ -149,8 +149,6 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif - ahrs_impl.use_gravity_heuristic = AHRS_GRAVITY_UPDATE_NORM_HEURISTIC; - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); @@ -287,20 +285,22 @@ void ahrs_update_accel(void) { VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); - if (ahrs_impl.use_gravity_heuristic) { + if (ahrs_impl.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. - * e.g. for WEIGHT_FACTOR 3: + * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ - #define WEIGHT_FACTOR 5 struct FloatVect3 g_meas_f; ACCELS_FLOAT_OF_BFP(g_meas_f, filtered_gravity_measurement); const float g_meas_norm = FLOAT_VECT3_NORM(g_meas_f)/9.81; - ahrs_impl.weight = 1.0 - WEIGHT_FACTOR * fabs(1.0 - g_meas_norm); + ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10; Bound(ahrs_impl.weight, 0.15, 1.0); } + else { + ahrs_impl.weight = 1.0; + } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 56d7ad1131..e7c5fbce6d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -57,12 +57,35 @@ struct AhrsIntCmplQuat { float mag_ki; /* parameters/options that can be changed */ + /** enable gravity vector correction by removing centrifugal acceleration */ bool_t correct_gravity; - bool_t use_gravity_heuristic; - float accel_omega; ///< filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement) - float accel_zeta; ///< filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement) - float mag_omega; ///< filter cut-off frequency for correcting the attitude (heading) from magnetometer - float mag_zeta; ///< filter damping for correcting the gyro bias from magnetometer + + /** sets how strongly the gravity heuristic reduces accel correction. + * Set to zero in order to disable gravity heuristic. + */ + uint8_t gravity_heuristic_factor; + + /** filter cut-off frequency for correcting the attitude from accels. + * (pseudo-gravity measurement) + * only update through #ahrs_int_cmpl_quat_SetAccelOmega(omega) + */ + float accel_omega; + + /** filter damping for correcting the gyro-bias from accels. + * (pseudo-gravity measurement) + * only update through #ahrs_int_cmpl_quat_SetAccelZeta(zeta) + */ + float accel_zeta; + + /** filter cut-off frequency for correcting the attitude (heading) from magnetometer. + * only update through #ahrs_int_cmpl_quat_SetMagOmega(omega) + */ + float mag_omega; + + /** filter damping for correcting the gyro bias from magnetometer. + * only update through #ahrs_int_cmpl_quat_SetMagZeta(zeta) + */ + float mag_zeta; }; extern struct AhrsIntCmplQuat ahrs_impl; From d2addc1033c841cd09225fac0de20d77a1a2b8df Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 19:08:28 +0200 Subject: [PATCH 062/125] [ahrs] float_cmpl: set weight to 1.0 if heuristic is disabled --- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index b4ab80b7cd..65a3e3cc99 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -270,6 +270,9 @@ void ahrs_update_accel(void) { ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_impl.weight, 0.15, 1.0); } + else { + ahrs_impl.weight = 1.0; + } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY From 1235c7d9829e50294e366de6154ffb5ba8054b82 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 Sep 2013 19:46:16 +0200 Subject: [PATCH 063/125] [conf] add ahrs_float_cmpl settings file --- conf/settings/estimation/ahrs_float_cmpl.xml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 conf/settings/estimation/ahrs_float_cmpl.xml diff --git a/conf/settings/estimation/ahrs_float_cmpl.xml b/conf/settings/estimation/ahrs_float_cmpl.xml new file mode 100644 index 0000000000..950b651e70 --- /dev/null +++ b/conf/settings/estimation/ahrs_float_cmpl.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + From 4c8e57fe07bd8a814d396ad25ea590ebda8ac649 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 9 Sep 2013 18:15:12 +0200 Subject: [PATCH 064/125] [state] fix local/global coordinate validity checks --- sw/airborne/state.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 9818c0fafd..cb5d101555 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -487,12 +487,12 @@ extern void stateCalcPositionLla_f(void); /// Test if local coordinates are valid. static inline bool_t stateIsLocalCoordinateValid(void) { - return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status & ~(POS_LOCAL_COORD))); + return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status & (POS_LOCAL_COORD))); } /// Test if global coordinates are valid. static inline bool_t stateIsGlobalCoordinateValid(void) { - return ((state.pos_status & ~(POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid()); + return ((state.pos_status & (POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid()); } /************************ Set functions ****************************/ From cf178364913288c7d80050850d6b3fca519307f3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 10 Sep 2013 10:58:00 +0200 Subject: [PATCH 065/125] [rotorcraft] telemetry: use state interface instead of ins_impl --- sw/airborne/firmwares/rotorcraft/telemetry.h | 150 ++++++++++--------- 1 file changed, 77 insertions(+), 73 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index be41696b7c..2f0d7b4acd 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -583,92 +583,96 @@ #define PERIODIC_SEND_HFF_GPS(_trans, _dev) {} #endif -#define PERIODIC_SEND_GUIDANCE_H_INT(_trans, _dev) { \ - DOWNLINK_SEND_GUIDANCE_H_INT(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_ref.x, \ - &guidance_h_pos_ref.y, \ - &ins_impl.ltp_pos.x, \ - &ins_impl.ltp_pos.y); \ +#define PERIODIC_SEND_GUIDANCE_H_INT(_trans, _dev) { \ + DOWNLINK_SEND_GUIDANCE_H_INT(_trans, _dev, \ + &guidance_h_pos_sp.x, \ + &guidance_h_pos_sp.y, \ + &guidance_h_pos_ref.x, \ + &guidance_h_pos_ref.y, \ + &(stateGetPositionNed_i()->x), \ + &(stateGetPositionNed_i()->y)); \ } #define PERIODIC_SEND_INS_Z(_trans, _dev) { \ - DOWNLINK_SEND_INS_Z(_trans, _dev, \ - &ins_baro_alt, \ - &ins_impl.ltp_pos.z, \ - &ins_impl.ltp_speed.z, \ - &ins_impl.ltp_accel.z); \ + DOWNLINK_SEND_INS_Z(_trans, _dev, \ + &ins_baro_alt, \ + &(stateGetPositionNed_i()->z), \ + &(stateGetSpeedNed_i()->z), \ + &(stateGetAccelNed_i()->z)); \ } -#define PERIODIC_SEND_INS(_trans, _dev) { \ - DOWNLINK_SEND_INS(_trans, _dev, \ - &ins_impl.ltp_pos.x, \ - &ins_impl.ltp_pos.y, \ - &ins_impl.ltp_pos.z, \ - &ins_impl.ltp_speed.x, \ - &ins_impl.ltp_speed.y, \ - &ins_impl.ltp_speed.z, \ - &ins_impl.ltp_accel.x, \ - &ins_impl.ltp_accel.y, \ - &ins_impl.ltp_accel.z); \ +#define PERIODIC_SEND_INS(_trans, _dev) { \ + struct NedCoor_i* ltp_pos = stateGetPositionNed_i(); \ + struct NedCoor_i* ltp_speed = stateGetSpeedNed_i(); \ + struct NedCoor_i* ltp_accel = stateGetAccelNed_i(); \ + DOWNLINK_SEND_INS(_trans, _dev, \ + &(ltp_pos->x), \ + &(ltp_pos->y), \ + &(ltp_pos->z), \ + &(ltp_speed->x), \ + &(ltp_speed->y), \ + &(ltp_speed->z), \ + &(ltp_accel->x), \ + &(ltp_accel->y), \ + &(ltp_accel->z)); \ } -#define PERIODIC_SEND_INS_REF(_trans, _dev) { \ - if (ins_impl.ltp_initialized) \ - DOWNLINK_SEND_INS_REF(_trans, _dev, \ - &ins_impl.ltp_def.ecef.x, \ - &ins_impl.ltp_def.ecef.y, \ - &ins_impl.ltp_def.ecef.z, \ - &ins_impl.ltp_def.lla.lat, \ - &ins_impl.ltp_def.lla.lon, \ - &ins_impl.ltp_def.lla.alt, \ - &ins_impl.ltp_def.hmsl, \ - &ins_impl.qfe); \ +#define PERIODIC_SEND_INS_REF(_trans, _dev) { \ + if (state.ned_initialized_i) { \ + DOWNLINK_SEND_INS_REF(_trans, _dev, \ + &state.ned_origin_i.ecef.x, \ + &state.ned_origin_i.ecef.y, \ + &state.ned_origin_i.ecef.z, \ + &state.ned_origin_i.lla.lat, \ + &state.ned_origin_i.lla.lon, \ + &state.ned_origin_i.lla.alt, \ + &state.ned_origin_i.hmsl, \ + &ins_impl.qfe); \ + } \ } #define PERIODIC_SEND_VERT_LOOP(_trans, _dev) { \ - DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \ - &guidance_v_z_sp, \ - &guidance_v_zd_sp, \ - &ins_impl.ltp_pos.z, \ - &ins_impl.ltp_speed.z, \ - &ins_impl.ltp_accel.z, \ - &guidance_v_z_ref, \ - &guidance_v_zd_ref, \ - &guidance_v_zdd_ref, \ - &gv_adapt_X, \ - &gv_adapt_P, \ - &gv_adapt_Xmeas, \ - &guidance_v_z_sum_err, \ - &guidance_v_ff_cmd, \ - &guidance_v_fb_cmd, \ - &guidance_v_delta_t); \ + DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \ + &guidance_v_z_sp, \ + &guidance_v_zd_sp, \ + &(stateGetPositionNed_i()->z), \ + &(stateGetSpeedNed_i()->z), \ + &(stateGetAccelNed_i()->z), \ + &guidance_v_z_ref, \ + &guidance_v_zd_ref, \ + &guidance_v_zdd_ref, \ + &gv_adapt_X, \ + &gv_adapt_P, \ + &gv_adapt_Xmeas, \ + &guidance_v_z_sum_err, \ + &guidance_v_ff_cmd, \ + &guidance_v_fb_cmd, \ + &guidance_v_delta_t); \ } #define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) { \ - DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_sp.y, \ - &ins_impl.ltp_pos.x, \ - &ins_impl.ltp_pos.y, \ - &ins_impl.ltp_speed.x, \ - &ins_impl.ltp_speed.y, \ - &ins_impl.ltp_accel.x, \ - &ins_impl.ltp_accel.y, \ - &guidance_h_pos_err.x, \ - &guidance_h_pos_err.y, \ - &guidance_h_speed_err.x, \ - &guidance_h_speed_err.y, \ - &guidance_h_pos_err_sum.x, \ - &guidance_h_pos_err_sum.y, \ - &guidance_h_nav_err.x, \ - &guidance_h_nav_err.y, \ - &guidance_h_command_earth.x, \ - &guidance_h_command_earth.y, \ - &guidance_h_command_body.phi, \ - &guidance_h_command_body.theta, \ - &guidance_h_command_body.psi); \ + DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ + &guidance_h_pos_sp.x, \ + &guidance_h_pos_sp.y, \ + &(stateGetPositionNed_i()->x), \ + &(stateGetPositionNed_i()->y), \ + &(stateGetSpeedNed_i()->x), \ + &(stateGetSpeedNed_i()->y), \ + &(stateGetAccelNed_i()->x), \ + &(stateGetAccelNed_i()->y), \ + &guidance_h_pos_err.x, \ + &guidance_h_pos_err.y, \ + &guidance_h_speed_err.x, \ + &guidance_h_speed_err.y, \ + &guidance_h_pos_err_sum.x, \ + &guidance_h_pos_err_sum.y, \ + &guidance_h_nav_err.x, \ + &guidance_h_nav_err.y, \ + &guidance_h_command_earth.x, \ + &guidance_h_command_earth.y, \ + &guidance_h_command_body.phi, \ + &guidance_h_command_body.theta, \ + &guidance_h_command_body.psi); \ } #define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \ From 381eb343d96f4d31f01258b873a19390e6450199 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 10 Sep 2013 17:48:54 +0200 Subject: [PATCH 066/125] [fixedwing] fix nps autopilot include --- sw/airborne/firmwares/fixedwing/main_fbw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 701a644b82..ddee0741bf 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -58,7 +58,7 @@ uint8_t fbw_mode; #include "inter_mcu.h" #ifdef USE_NPS -#include "nps_autopilot_fixedwing.h" +#include "nps_autopilot.h" #endif /** Trim commands for roll, pitch and yaw. From 02080119c63eca4e405744ab681c3a74648317ea Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Sep 2013 19:05:27 +0200 Subject: [PATCH 067/125] [rotorcraft] add rpy/earth commands to quaternion transformations --- .../stabilization_float_quat.makefile | 1 + .../stabilization_int_quat.makefile | 1 + .../stabilization_attitude_quat_float.c | 68 +--------- .../stabilization_attitude_quat_int.c | 77 +---------- ...bilization_attitude_quat_transformations.c | 127 ++++++++++++++++++ ...bilization_attitude_quat_transformations.h | 38 ++++++ 6 files changed, 171 insertions(+), 141 deletions(-) create mode 100644 sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c create mode 100644 sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile index d79598d8eb..448148fedc 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile @@ -3,6 +3,7 @@ STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_float.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile index 317172085a..50e15da7b1 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile @@ -3,6 +3,7 @@ STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_int.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index a5764fa859..00fa397b66 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -145,72 +146,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) { void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = stab_att_sp_euler.phi; - ov.y = stab_att_sp_euler.theta; - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); - - /// @todo optimize yaw angle calculation - - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(stab_att_sp_euler.psi); - psi_vect.y = sinf(stab_att_sp_euler.psi); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(stab_att_sp_quat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + quat_from_rpy_cmd_f(&stab_att_sp_quat, &stab_att_sp_euler); } #ifndef GAIN_PRESCALER_FF diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index f709f9f5c0..7ec25903d9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -96,81 +97,7 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { // copy euler setpoint for debugging memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); - /// @todo calc sp_quat in fixed-point - - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi); - ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta); - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); - - const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi); - - /// @todo optimize yaw angle calculation - - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(psi_sp); - psi_vect.y = sinf(psi_sp); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - struct FloatQuat q_sp; - FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(q_sp); - FLOAT_QUAT_WRAP_SHORTEST(q_sp); - - /* convert to fixed point */ - QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp); + quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c new file mode 100644 index 0000000000..bcac72e9cb --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -0,0 +1,127 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.c + * Quaternion transformation functions. + */ + +#include "stabilization_attitude_quat_transformations.h" + +void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) { + struct FloatEulers rpy_f; + EULERS_FLOAT_OF_BFP(rpy_f, *rpy); + struct FloatQuat quat_f; + quat_from_rpy_cmd_f(&quat_f, &rpy_f); + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) { + // only a plug for now... doesn't apply roll/pitch wrt. current yaw angle + + /* orientation vector describing simultaneous rotation of roll/pitch/yaw */ + const struct FloatVect3 ov = {rpy->phi, rpy->theta, rpy->psi}; + /* quaternion from that orientation vector */ + FLOAT_QUAT_OF_ORIENTATION_VECT(*quat, ov); + +} + +void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) { + // use float conversion for now... + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + float heading_f = ANGLE_FLOAT_OF_BFP(heading); + + struct FloatQuat quat_f; + quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f); + + // convert back to fixed point + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { + + /* orientation vector describing simultaneous rotation of roll/pitch */ + const struct FloatVect3 ov = {cmd->x, cmd->y, 0.0}; + /* quaternion from that orientation vector */ + struct FloatQuat q_rp; + FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + + /* as rotation matrix */ + struct FloatRMat R_rp; + FLOAT_RMAT_OF_QUAT(R_rp, q_rp); + /* body x-axis (before heading command) is first column */ + struct FloatVect3 b_x; + VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]); + /* body z-axis (thrust vect) is last column */ + struct FloatVect3 thrust_vect; + VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]); + + /// @todo optimize yaw angle calculation + + /* + * Instead of using the psi setpoint angle to rotate around the body z-axis, + * calculate the real angle needed to align the projection of the body x-axis + * onto the horizontal plane with the psi setpoint. + * + * angle between two vectors a and b: + * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) + * where the normal n is the thrust vector (i.e. both a and b lie in that plane) + */ + + // desired heading vect in earth x-y plane + const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0}; + + /* projection of desired heading onto body x-y plane + * b = v - dot(v,n)*n + */ + float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, thrust_vect); + struct FloatVect3 dotn; + FLOAT_VECT3_SMUL(dotn, thrust_vect, dot); + + // b = v - dot(v,n)*n + struct FloatVect3 b; + FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(b_x, b); + struct FloatVect3 cross; + VECT3_CROSS_PRODUCT(cross, b_x, b); + // norm of the cross product + float nc = FLOAT_VECT3_NORM(cross); + // angle = atan2(norm(cross(a,b)), dot(a,b)) + float yaw2 = atan2(nc, dot) / 2.0; + + // negative angle if needed + // sign(dot(cross(a,b), n) + float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, thrust_vect); + if (dot_cross_ab < 0) { + yaw2 = -yaw2; + } + + /* quaternion with yaw command */ + struct FloatQuat q_yaw; + QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); + + /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ + FLOAT_QUAT_COMP(*quat, q_rp, q_yaw); + FLOAT_QUAT_NORMALIZE(*quat); + FLOAT_QUAT_WRAP_SHORTEST(*quat); + +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h new file mode 100644 index 0000000000..1c57038b3f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.h + * Quaternion transformation functions. + */ + +#ifndef STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H +#define STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" + +extern void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy); +extern void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy); + +extern void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading); +extern void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading); + +#endif From f0e5a200001f93741ffefebcaee5ccc3b8eaf5be Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 12:11:40 +0200 Subject: [PATCH 068/125] [rotorcraft] guidance_h commands in earth frame --- .../rotorcraft/guidance/guidance_h.c | 52 +++++++------------ .../rotorcraft/guidance/guidance_h.h | 9 +++- .../stabilization/stabilization_attitude.h | 5 +- .../stabilization_attitude_quat_int.c | 8 ++- ...bilization_attitude_quat_transformations.c | 8 ++- sw/airborne/firmwares/rotorcraft/telemetry.h | 12 ++--- 6 files changed, 46 insertions(+), 48 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 842f3ccf21..35609fc3d4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -69,10 +69,9 @@ struct Int32Vect2 guidance_h_speed_err; struct Int32Vect2 guidance_h_pos_err_sum; struct Int32Vect2 guidance_h_nav_err; +struct Int32Vect2 guidance_h_cmd_earth; struct Int32Eulers guidance_h_rc_sp; -struct Int32Vect2 guidance_h_command_earth; -struct Int32Vect2 guidance_h_stick_earth_sp; -struct Int32Eulers guidance_h_command_body; +int32_t guidance_h_heading_sp; int32_t guidance_h_pgain; int32_t guidance_h_dgain; @@ -98,7 +97,7 @@ void guidance_h_init(void) { INT_VECT2_ZERO(guidance_h_pos_sp); INT_VECT2_ZERO(guidance_h_pos_err_sum); INT_EULERS_ZERO(guidance_h_rc_sp); - INT_EULERS_ZERO(guidance_h_command_body); + guidance_h_heading_sp = 0; guidance_h_pgain = GUIDANCE_H_PGAIN; guidance_h_igain = GUIDANCE_H_IGAIN; guidance_h_dgain = GUIDANCE_H_DGAIN; @@ -247,10 +246,12 @@ void guidance_h_run(bool_t in_flight) { guidance_h_update_reference(); /* set psi command */ - guidance_h_command_body.psi = guidance_h_rc_sp.psi; - /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_heading_sp = guidance_h_rc_sp.psi; + /* compute x,y earth commands */ guidance_h_traj_run(in_flight); - + /* set final attitude setpoint */ + stabilization_attitude_set_earth_cmd_i(guidance_h_cmd_earth, + guidance_h_heading_sp); stabilization_attitude_run(in_flight); break; @@ -264,7 +265,7 @@ void guidance_h_run(bool_t in_flight) { sp_cmd_i.theta = nav_pitch; /* FIXME: heading can't be set via attitude block yet, use current heading for now */ sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi; - stabilization_attitude_set_cmd_i(&sp_cmd_i); + stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -272,10 +273,13 @@ void guidance_h_run(bool_t in_flight) { guidance_h_update_reference(); /* set psi command */ - guidance_h_command_body.psi = nav_heading; - INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi); - /* compute roll and pitch commands and set final attitude setpoint */ + guidance_h_heading_sp = nav_heading; + INT32_ANGLE_NORMALIZE(guidance_h_heading_sp); + /* compute x,y earth commands */ guidance_h_traj_run(in_flight); + /* set final attitude setpoint */ + stabilization_attitude_set_earth_cmd_i(guidance_h_cmd_earth, + guidance_h_heading_sp); } stabilization_attitude_run(in_flight); break; @@ -339,38 +343,18 @@ static void guidance_h_traj_run(bool_t in_flight) { } /* run PID */ - guidance_h_command_earth.x = + guidance_h_cmd_earth.x = ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + ((guidance_h_igain * (guidance_h_pos_err_sum.x >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */ - guidance_h_command_earth.y = + guidance_h_cmd_earth.y = ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ - VECT2_STRIM(guidance_h_command_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK); - - /* Rotate to body frame */ - int32_t s_psi, c_psi; - int32_t psi = stateGetNedToBodyEulers_i()->psi; - PPRZ_ITRIG_SIN(s_psi, psi); - PPRZ_ITRIG_COS(c_psi, psi); - - // Restore angle ref resolution after rotation - guidance_h_command_body.phi = - ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; - guidance_h_command_body.theta = - - ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> INT32_TRIG_FRAC; - - - /* Add RC roll and pitch setpoints for emergency corrections */ - guidance_h_command_body.phi += guidance_h_rc_sp.phi; - guidance_h_command_body.theta += guidance_h_rc_sp.theta; - - /* Set attitude setpoint from pseudo-euler commands */ - stabilization_attitude_set_cmd_i(&guidance_h_command_body); + VECT2_STRIM(guidance_h_cmd_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK); } static void guidance_h_hover_enter(void) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 5a136317d5..3d634401ca 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -69,9 +69,14 @@ extern struct Int32Vect2 guidance_h_speed_err; extern struct Int32Vect2 guidance_h_pos_err_sum; extern struct Int32Vect2 guidance_h_nav_err; + +/** horizontal guidance command. + * In north/east with #INT32_ANGLE_FRAC + * @todo convert to real force command + */ +extern struct Int32Vect2 guidance_h_cmd_earth; extern struct Int32Eulers guidance_h_rc_sp; ///< with #INT32_ANGLE_FRAC -extern struct Int32Vect2 guidance_h_command_earth; -extern struct Int32Eulers guidance_h_command_body; ///< with #INT32_ANGLE_FRAC +extern int32_t guidance_h_heading_sp; ///< with #INT32_ANGLE_FRAC extern int32_t guidance_h_pgain; extern int32_t guidance_h_dgain; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 94b47466e4..8917a2d93c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -34,8 +34,9 @@ extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); extern void stabilization_attitude_enter(void); extern void stabilization_attitude_set_failsafe_setpoint(void); -extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd); -extern void stabilization_attitude_run(bool_t in_flight); +extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy); +extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); +extern void stabilization_attitude_run(bool_t in_flight); #endif /* STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 7ec25903d9..c2255b87e1 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -93,13 +93,17 @@ void stabilization_attitude_set_failsafe_setpoint(void) { PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { // copy euler setpoint for debugging - memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); + memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading); +} + #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c index bcac72e9cb..12ca369d62 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -59,8 +59,12 @@ void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32 void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { - /* orientation vector describing simultaneous rotation of roll/pitch */ - const struct FloatVect3 ov = {cmd->x, cmd->y, 0.0}; + /* cmd_x is positive to north = negative pitch + * cmd_y is positive to east = positive roll + * + * orientation vector describing simultaneous rotation of roll/pitch + */ + const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0}; /* quaternion from that orientation vector */ struct FloatQuat q_rp; FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 4891f0a0df..d668b58990 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -664,11 +664,11 @@ &guidance_h_pos_err_sum.y, \ &guidance_h_nav_err.x, \ &guidance_h_nav_err.y, \ - &guidance_h_command_earth.x, \ - &guidance_h_command_earth.y, \ - &guidance_h_command_body.phi, \ - &guidance_h_command_body.theta, \ - &guidance_h_command_body.psi); \ + &guidance_h_cmd_earth.x, \ + &guidance_h_cmd_earth.y, \ + &guidance_h_cmd_earth.x, \ + &guidance_h_cmd_earth.y, \ + &guidance_h_heading_sp); \ } #define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \ @@ -699,7 +699,7 @@ &guidance_h_pos_sp.y, \ &guidance_h_pos_sp.x, \ &carrot_up, \ - &guidance_h_command_body.psi, \ + &guidance_h_heading_sp, \ &stabilization_cmd[COMMAND_THRUST], \ &autopilot_flight_time); \ } From 3b2a5740ff7bc93bfe62446566bbfc0a2cfcd853 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 15:02:36 +0200 Subject: [PATCH 069/125] [rotorcraft] quat_int: still compute "euler" setpoints --- .../stabilization_attitude_quat_int.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index c2255b87e1..0f49833b4a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -94,13 +94,25 @@ void stabilization_attitude_set_failsafe_setpoint(void) { } void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { - // copy euler setpoint for debugging + // stab_att_sp_euler.psi still used in ref.. memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); } void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + // stab_att_sp_euler.psi still used in ref.. + stab_att_sp_euler.psi = heading; + + // compute sp_euler phi/theta for debugging/telemetry + /* Rotate horizontal commands to body frame by psi */ + int32_t psi = stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; + quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading); } From d1a180de02b07784a81c478725d9c50e734587cf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 13:05:32 +0200 Subject: [PATCH 070/125] [rotorcraft] update read_rc_roll_pitch_quat --- .../firmwares/rotorcraft/guidance/guidance_h.c | 4 +++- .../stabilization_attitude_rc_setpoint.c | 15 +++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 35609fc3d4..c882734b37 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -263,7 +263,9 @@ void guidance_h_run(bool_t in_flight) { struct Int32Eulers sp_cmd_i; sp_cmd_i.phi = nav_roll; sp_cmd_i.theta = nav_pitch; - /* FIXME: heading can't be set via attitude block yet, use current heading for now */ + /** @todo: heading can't be set via attitude block yet. + * use current euler psi for now, should be real heading + */ sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi; stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 7054d1824b..0affbcd21b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -221,15 +221,14 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo * @param[out] q quaternion representing the RC roll/pitch input */ void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { - q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; - q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; - q->qz = 0.0; + /* orientation vector describing simultaneous rotation of roll/pitch */ + struct FloatVect3 ov; + ov.x = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2; + ov.y = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2; + ov.z = 0.0; - /* normalize */ - float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy)); - q->qi = 1.0 / norm; - q->qx /= norm; - q->qy /= norm; + /* quaternion from that orientation vector */ + FLOAT_QUAT_OF_ORIENTATION_VECT(*q, ov); } /** Read roll/pitch command from RC as quaternion. From 03b2a86203f8cfb6eb6428c5334d996ca24a22e3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 13:31:05 +0200 Subject: [PATCH 071/125] [rotorcraft][stabilization] set_rpy|earth_cmd for quat_float --- .../stabilization_attitude_quat_float.c | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 00fa397b66..ee7f42f067 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -143,12 +143,23 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_quat.qz = sinf(heading2); } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + // copy euler setpoint for debugging + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); quat_from_rpy_cmd_f(&stab_att_sp_quat, &stab_att_sp_euler); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + float heading_f; + heading_f = ANGLE_FLOAT_OF_BFP(heading); + + quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f); +} + #ifndef GAIN_PRESCALER_FF #define GAIN_PRESCALER_FF 1 #endif From adfeb50c241d2feb94d3a86355b20b3614353e67 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 14:20:17 +0200 Subject: [PATCH 072/125] [rotorcraft][stabilization] set_rpy|earth_cmd for euler --- .../stabilization_attitude_euler_float.c | 17 +++++++++++++++-- .../stabilization_attitude_euler_int.c | 14 ++++++++++++-- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index 3a5df19417..3e77aac20c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -86,10 +86,23 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + struct FloatVect2 cmd_f; + cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); + cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); + + /* Rotate horizontal commands to body frame by psi */ + float psi = stateGetNedToBodyEulers_f()->psi; + float s_psi = sinf(psi); + float c_psi = cosf(psi); + stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y; + stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y; + stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading); +} #define MAX_SUM_ERR RadOfDeg(56000) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index a86f9c4c80..27285f4403 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -102,10 +102,20 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { - memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { + memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); } +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { + /* Rotate horizontal commands to body frame by psi */ + int32_t psi = stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; + stab_att_sp_euler.psi = heading; +} #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) From 1a26a6fb11ee190de57ef8a48b3ec2cac51897be Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 16:29:08 +0200 Subject: [PATCH 073/125] [conf] add baro_board for simulators from baro_board.makefile --- conf/firmwares/subsystems/fixedwing/autopilot.makefile | 4 ---- .../subsystems/rotorcraft/fdm_jsbsim.makefile | 1 - conf/firmwares/subsystems/shared/baro_board.makefile | 10 +++++++++- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index e0368401d4..a38e3cbef0 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -191,8 +191,6 @@ sim.srcs += $(SRC_ARCH)/sim_ap.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c -sim.srcs += $(SRC_BOARD)/baro_board.c - sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c # hack: always compile some of the sim functions, so ocaml sim does not complain about no-existing functions @@ -223,8 +221,6 @@ endif jsbsim.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS) jsbsim.srcs += $(fbw_srcs) $(ap_srcs) -jsbsim.srcs += $(SRC_BOARD)/baro_board.c - jsbsim.CFLAGS += -DSITL -DUSE_JSBSIM jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_ac_flightgear.c diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 9d63f232ae..92e4b6b169 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -94,7 +94,6 @@ nps.srcs += $(SRC_FIRMWARE)/datalink.c nps.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c nps.srcs += subsystems/air_data.c -nps.srcs += $(SRC_BOARD)/baro_board.c nps.CFLAGS += -DUSE_ADC nps.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index e9ffc56381..360e19e9d0 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -124,8 +124,16 @@ ifneq ($(BARO_LED),none) ap.CFLAGS += -DBARO_LED=$(BARO_LED) endif +# +# add it for simulators +# +sim.srcs += $(SRC_BOARD)/baro_board.c +jsbsim.srcs += $(SRC_BOARD)/baro_board.c +nps.srcs += $(SRC_BOARD)/baro_board.c + + else # USE_BARO_BOARD is not TRUE, was explicitly disabled -ap.CFLAGS += -DUSE_BARO_BOARD=FALSE +$(TARGET).CFLAGS += -DUSE_BARO_BOARD=FALSE endif # check USE_BARO_BOARD From 969705306b48d556007117cf8ef76ed6105da152 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 17:43:11 +0200 Subject: [PATCH 074/125] [rotorcraft] ups, fix setting commands from guidance_h --- sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index c882734b37..9a19561096 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -250,7 +250,7 @@ void guidance_h_run(bool_t in_flight) { /* compute x,y earth commands */ guidance_h_traj_run(in_flight); /* set final attitude setpoint */ - stabilization_attitude_set_earth_cmd_i(guidance_h_cmd_earth, + stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h_heading_sp); stabilization_attitude_run(in_flight); break; @@ -280,7 +280,7 @@ void guidance_h_run(bool_t in_flight) { /* compute x,y earth commands */ guidance_h_traj_run(in_flight); /* set final attitude setpoint */ - stabilization_attitude_set_earth_cmd_i(guidance_h_cmd_earth, + stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h_heading_sp); } stabilization_attitude_run(in_flight); From 4d8d2a907d2c6ac68a158ebd84e9ef35b13d0837 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 11 Sep 2013 22:06:51 +0200 Subject: [PATCH 075/125] [rotorcraft] add GUIDANCE_H_APPROX_FORCE_BY_THRUST based on the idea of Sergei from pull request #532 Still need to use only the vertical thrust instead of total thrust. Will be corrected later using the get_vertical_thrust_coeff from pull request #528 --- .../rotorcraft/guidance/guidance_h.c | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 9a19561096..418fc33275 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -316,14 +316,18 @@ static void guidance_h_update_reference(void) { #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.) #define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12) +#ifndef GUIDANCE_H_THRUST_CMD_FILTER +#define GUIDANCE_H_THRUST_CMD_FILTER 10 +#endif + /* with a pgain of 100 and a scale of 2, * you get an angle of 5.6 degrees for 1m pos error */ #define GH_GAIN_SCALE 2 -/** maximum bank angle: default 20 deg */ -#define TRAJ_MAX_BANK BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC) - static void guidance_h_traj_run(bool_t in_flight) { + /* maximum bank angle: default 20 deg, max 40 deg*/ + static const int32_t traj_max_bank = Max(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC), + BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC)); /* compute position error */ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i()); @@ -356,7 +360,17 @@ static void guidance_h_traj_run(bool_t in_flight) { ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ - VECT2_STRIM(guidance_h_cmd_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK); + /* compute a better approximation of force commands by taking thrust into account */ +#if GUIDANCE_H_APPROX_FORCE_BY_THRUST + if (in_flight) { + static int32_t thrust_cmd_filt; + thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + stabilization_cmd[COMMAND_THRUST]) / (GUIDANCE_H_THRUST_CMD_FILTER + 1); + guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + } +#endif + + VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); } static void guidance_h_hover_enter(void) { From 03234de546c2f94f5be2dcddf1966d160f8f8134 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 13 Sep 2013 16:09:32 +0200 Subject: [PATCH 076/125] [rotorcraft][guidance_h] guidance_h_approx_force_by_thrust - enable/disable via settings - gets a better approximation of vertical thrust using guidance_v_thrust_coeff - vertical thrust is recomputed in guidance_h to take the latest thrust command after guidance_v was run --- conf/airframes/fraser_lisa_m_rotorcraft.xml | 3 ++- conf/settings/control/rotorcraft_guidance.xml | 1 + .../firmwares/rotorcraft/guidance/guidance_h.c | 16 ++++++++++++---- .../firmwares/rotorcraft/guidance/guidance_h.h | 1 + .../firmwares/rotorcraft/guidance/guidance_v.c | 2 +- .../firmwares/rotorcraft/guidance/guidance_v.h | 3 +++ sw/airborne/firmwares/rotorcraft/navigation.c | 10 ++++------ 7 files changed, 24 insertions(+), 12 deletions(-) diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index a347658ee8..451e682961 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -229,7 +229,8 @@
- + + diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index 45c73a958b..c44ecb0069 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -15,6 +15,7 @@ + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 418fc33275..d38140e3c1 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -30,6 +30,9 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "firmwares/rotorcraft/navigation.h" +/* for guidance_v_thrust_coeff */ +#include "firmwares/rotorcraft/guidance/guidance_v.h" + #include "state.h" #include "generated/airframe.h" @@ -55,9 +58,14 @@ PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF) +#ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST +#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE +#endif + uint8_t guidance_h_mode; bool_t guidance_h_use_ref; +bool_t guidance_h_approx_force_by_thrust; struct Int32Vect2 guidance_h_pos_sp; struct Int32Vect2 guidance_h_pos_ref; @@ -93,6 +101,7 @@ void guidance_h_init(void) { guidance_h_mode = GUIDANCE_H_MODE_KILL; guidance_h_use_ref = GUIDANCE_H_USE_REF; + guidance_h_approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST; INT_VECT2_ZERO(guidance_h_pos_sp); INT_VECT2_ZERO(guidance_h_pos_err_sum); @@ -361,14 +370,13 @@ static void guidance_h_traj_run(bool_t in_flight) { ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ /* compute a better approximation of force commands by taking thrust into account */ -#if GUIDANCE_H_APPROX_FORCE_BY_THRUST - if (in_flight) { + if (guidance_h_approx_force_by_thrust && in_flight) { static int32_t thrust_cmd_filt; - thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + stabilization_cmd[COMMAND_THRUST]) / (GUIDANCE_H_THRUST_CMD_FILTER + 1); + int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; + thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) / (GUIDANCE_H_THRUST_CMD_FILTER + 1); guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); } -#endif VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 3d634401ca..e8ba961fc1 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -53,6 +53,7 @@ extern uint8_t guidance_h_mode; extern bool_t guidance_h_use_ref; +extern bool_t guidance_h_approx_force_by_thrust; /** horizontal position setpoint in NED. * fixed point representation: Q23.8 diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 23f625b442..23f6d5b11b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -94,7 +94,7 @@ int32_t guidance_v_ki; int32_t guidance_v_z_sum_err; -static int32_t guidance_v_thrust_coeff; +int32_t guidance_v_thrust_coeff; #define GuidanceVSetRef(_pos, _speed, _accel) { \ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 4a94b4cfa8..93e0aeaa4d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -91,6 +91,9 @@ extern float guidance_v_nominal_throttle; */ extern bool_t guidance_v_adapt_throttle_enabled; + +extern int32_t guidance_v_thrust_coeff; + extern int32_t guidance_v_kp; ///< vertical control P-gain extern int32_t guidance_v_kd; ///< vertical control D-gain extern int32_t guidance_v_ki; ///< vertical control I-gain diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 366ff9d010..249497e680 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -141,13 +141,11 @@ static inline void nav_advance_carrot(void) { void nav_run(void) { -#if !GUIDANCE_H_USE_REF - PRINT_CONFIG_MSG("NOT using horizontal guidance reference :-(") - nav_advance_carrot(); -#else - PRINT_CONFIG_MSG("Using horizontal guidance reference :-)") - // if H_REF is used, CARROT_DIST is not used +#if GUIDANCE_H_USE_REF + // if GUIDANCE_H_USE_REF, CARROT_DIST is not used VECT2_COPY(navigation_carrot, navigation_target); +#else + nav_advance_carrot(); #endif nav_set_altitude(); From dcd89a12bb8acfca733952514587e115461479e2 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 13 Sep 2013 19:00:08 +0200 Subject: [PATCH 077/125] [boards][apogee] remove unneeded APOGEE_BARO_SENS pressure is already in Pascal --- sw/airborne/boards/apogee/baro_board.c | 6 ------ sw/airborne/peripherals/mpl3115.h | 2 +- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 53806f7fd7..55e94e7171 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -36,12 +36,6 @@ #include "subsystems/abi.h" #include "led.h" - -// FIXME -#ifndef APOGEE_BARO_SENS -#define APOGEE_BARO_SENS 0.0274181 -#endif - #ifndef APOGEE_BARO_SENDER_ID #define APOGEE_BARO_SENDER_ID 12 #endif diff --git a/sw/airborne/peripherals/mpl3115.h b/sw/airborne/peripherals/mpl3115.h index 5c723ed379..9b0977499e 100644 --- a/sw/airborne/peripherals/mpl3115.h +++ b/sw/airborne/peripherals/mpl3115.h @@ -77,7 +77,7 @@ extern bool_t mpl3115_initialized; // Data ready flag extern volatile bool_t mpl3115_data_available; // Data -extern uint32_t mpl3115_pressure; +extern uint32_t mpl3115_pressure; ///< absolute pressure in Pascal extern int16_t mpl3115_temperature; extern float mpl3115_alt; // I2C transaction structure From b424bbac4986947df30dc982012d4fce923c8349 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 13 Sep 2013 21:39:12 +0200 Subject: [PATCH 078/125] [peripherals] refactor baro mpl3115 --- .../subsystems/shared/baro_board.makefile | 2 - conf/modules/baro_mpl3115.xml | 7 +- sw/airborne/boards/apogee/baro_board.c | 18 +- sw/airborne/modules/sensors/baro_mpl3115.c | 31 ++- sw/airborne/modules/sensors/baro_mpl3115.h | 8 +- sw/airborne/peripherals/mpl3115.c | 179 +++++++++--------- sw/airborne/peripherals/mpl3115.h | 57 +++--- 7 files changed, 160 insertions(+), 142 deletions(-) diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index 360e19e9d0..012f3326b8 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -104,8 +104,6 @@ else ifeq ($(BOARD), px4fmu) # apogee baro else ifeq ($(BOARD), apogee) ap.CFLAGS += -DUSE_I2C1 - ap.CFLAGS += -DMPL3115_I2C_DEV=i2c1 - ap.CFLAGS += -DMPL3115_ALT_MODE=0 ap.srcs += peripherals/mpl3115.c ap.srcs += $(SRC_BOARD)/baro_board.c diff --git a/conf/modules/baro_mpl3115.xml b/conf/modules/baro_mpl3115.xml index 81151291fb..13225e56c0 100644 --- a/conf/modules/baro_mpl3115.xml +++ b/conf/modules/baro_mpl3115.xml @@ -3,7 +3,10 @@ Baro MPL3115A2 (I2C) - + + + +
@@ -13,7 +16,7 @@ - + diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 55e94e7171..a4d335f686 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -44,8 +44,10 @@ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; +struct Mpl3115 apogee_baro; + void baro_init( void ) { - mpl3115_init(); + mpl3115_init(&apogee_baro, &i2c1, MPL3115_I2C_ADDR); #ifdef BARO_LED LED_OFF(BARO_LED); #endif @@ -57,10 +59,10 @@ void baro_periodic( void ) { // Baro is slave of the MPU, only start reading it after MPU is configured if (imu_apogee.mpu.config.initialized) { - if (startup_cnt > 0 && mpl3115_data_available) { + if (startup_cnt > 0 && apogee_baro.data_available) { // Run some loops to get correct readings from the adc --startup_cnt; - mpl3115_data_available = FALSE; + apogee_baro.data_available = FALSE; #ifdef BARO_LED LED_TOGGLE(BARO_LED); if (startup_cnt == 0) { @@ -69,18 +71,18 @@ void baro_periodic( void ) { #endif } // Read the sensor - Mpl3115Periodic(); + mpl3115_periodic(&apogee_baro); } } void apogee_baro_event(void) { - mpl3115_event(); - if (mpl3115_data_available) { + mpl3115_event(&apogee_baro); + if (apogee_baro.data_available) { if (startup_cnt == 0) { - float pressure = ((float)mpl3115_pressure/(1<<2)); + float pressure = ((float)apogee_baro.pressure/(1<<4)); AbiSendMsgBARO_ABS(APOGEE_BARO_SENDER_ID, &pressure); } - mpl3115_data_available = FALSE; + apogee_baro.data_available = FALSE; } } diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index 431674388b..91dbfefedc 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -17,10 +17,17 @@ * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. + */ + +/** + * @file modules/sensors/baro_mpl3155.c + * + * Module for the baro MPL3115A2 from Freescale (i2c) * */ #include "modules/sensors/baro_mpl3115.h" +#include "peripherals/mpl3115.h" #include "subsystems/abi.h" //Messages @@ -32,29 +39,39 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif +#ifndef BARO_MPL3115_I2C_DEV +#define BARO_MPL3115_I2C_DEV i2c0 +#endif + +#ifndef BARO_MPL3115_I2C_SLAVE_ADDR +#define BARO_MPL3115_I2C_SLAVE_ADDR MPL3115_I2C_ADDR +#endif + #ifndef BARO_MPL3115_SENDER_ID #define BARO_MPL3115_SENDER_ID 20 #endif +struct Mpl3115 baro_mpl; + void baro_mpl3115_init( void ) { - mpl3115_init(); + mpl3115_init(&baro_mpl, &BARO_MPL3115_I2C_DEV, BARO_MPL3115_I2C_SLAVE_ADDR); } void baro_mpl3115_read_periodic( void ) { - Mpl3115Periodic(); + mpl3115_periodic(&baro_mpl); } void baro_mpl3115_read_event( void ) { - mpl3115_event(); - if (mpl3115_data_available) { - float pressure = (float)mpl3115_pressure; + mpl3115_event(&baro_mpl); + if (baro_mpl.data_available) { + float pressure = (float)baro_mpl.pressure/(1<<4); AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, &pressure); #ifdef SENSOR_SYNC_SEND - DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &mpl3115_pressure, &mpl3115_temperature, &mpl3115_alt); + DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &baro_mpl.pressure, &baro_mpl.temperature, &baro_mpl.alt); #endif - mpl3115_data_available = FALSE; + baro_mpl.data_available = FALSE; } } diff --git a/sw/airborne/modules/sensors/baro_mpl3115.h b/sw/airborne/modules/sensors/baro_mpl3115.h index ef9f8b1124..fd6bd274db 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.h +++ b/sw/airborne/modules/sensors/baro_mpl3115.h @@ -17,14 +17,18 @@ * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. + */ + +/** + * @file modules/sensors/baro_mpl3155.h + * + * Module for the baro MPL3115A2 from Freescale (i2c) * */ #ifndef BARO_MPL3115_H #define BARO_MPL3115_H -#include "peripherals/mpl3115.h" - extern void baro_mpl3115_init( void ); extern void baro_mpl3115_read_periodic( void ); extern void baro_mpl3115_read_event( void ); diff --git a/sw/airborne/peripherals/mpl3115.c b/sw/airborne/peripherals/mpl3115.c index 24a3bab2a1..bfac2abad1 100644 --- a/sw/airborne/peripherals/mpl3115.c +++ b/sw/airborne/peripherals/mpl3115.c @@ -27,57 +27,50 @@ #include "peripherals/mpl3115.h" #include "std.h" -#define MPL_CONF_UNINIT 0 -#define MPL_CONF_PT_DATA 1 -#define MPL_CONF_CTRL1 2 -#define MPL_CONF_DONE 3 - -// Data ready flag -volatile bool_t mpl3115_data_available; -// Data -uint32_t mpl3115_pressure; -int16_t mpl3115_temperature; -float mpl3115_alt; -// I2C transaction for reading and configuring -struct i2c_transaction mpl3115_trans; -// I2C transaction for conversion request -struct i2c_transaction mpl3115_req_trans; -// Init flag -bool_t mpl3115_initialized; -uint8_t mpl3115_init_status; - -void mpl3115_init(void) +void mpl3115_init(struct Mpl3115 *mpl, struct i2c_periph *i2c_p, uint8_t addr) { - mpl3115_trans.status = I2CTransDone; - mpl3115_req_trans.status = I2CTransDone; - mpl3115_initialized = FALSE; - mpl3115_init_status = MPL_CONF_UNINIT; - mpl3115_pressure = 0; - mpl3115_temperature = 0; - mpl3115_alt = 0.; + /* set i2c_peripheral */ + mpl->i2c_p = i2c_p; + + /* slave address */ + mpl->trans.slave_addr = addr; + + mpl->trans.status = I2CTransDone; + mpl->req_trans.status = I2CTransDone; + mpl->initialized = FALSE; + mpl->init_status = MPL_CONF_UNINIT; + + /* by default disable raw mode and set pressure mode */ + mpl->raw_mode = FALSE; + mpl->alt_mode = FALSE; + + mpl->pressure = 0; + mpl->temperature = 0; + mpl->altitude = 0.; } // Configuration function called once before normal use -static void mpl3115_send_config(void) +static void mpl3115_send_config(struct Mpl3115 *mpl) { - switch (mpl3115_init_status) { + switch (mpl->init_status) { case MPL_CONF_PT_DATA: - mpl3115_trans.buf[0] = MPL3115_REG_PT_DATA_CFG; - mpl3115_trans.buf[1] = MPL3115_PT_DATA_CFG; - i2c_transmit(&MPL3115_I2C_DEV, &mpl3115_trans, MPL3115_I2C_ADDR, 2); - mpl3115_init_status++; + mpl->trans.buf[0] = MPL3115_REG_PT_DATA_CFG; + mpl->trans.buf[1] = MPL3115_PT_DATA_CFG; + i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); + mpl->init_status++; break; case MPL_CONF_CTRL1: - mpl3115_trans.buf[0] = MPL3115_REG_CTRL_REG1; - mpl3115_trans.buf[1] = MPL3115_CTRL_REG1; - i2c_transmit(&MPL3115_I2C_DEV, &mpl3115_trans, MPL3115_I2C_ADDR, 2); - mpl3115_init_status++; + mpl->trans.buf[0] = MPL3115_REG_CTRL_REG1; + mpl->trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | + (mpl->alt_mode<<7)); + i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); + mpl->init_status++; break; case MPL_CONF_DONE: - mpl3115_initialized = TRUE; - mpl3115_trans.status = I2CTransDone; + mpl->initialized = TRUE; + mpl->trans.status = I2CTransDone; break; default: break; @@ -85,77 +78,85 @@ static void mpl3115_send_config(void) } // Configure -void mpl3115_configure(void) +void mpl3115_configure(struct Mpl3115 *mpl) { - if (mpl3115_init_status == MPL_CONF_UNINIT) { - mpl3115_init_status++; - if (mpl3115_trans.status == I2CTransSuccess || mpl3115_trans.status == I2CTransDone) { - mpl3115_send_config(); + if (mpl->init_status == MPL_CONF_UNINIT) { + mpl->init_status++; + if (mpl->trans.status == I2CTransSuccess || mpl->trans.status == I2CTransDone) { + mpl3115_send_config(mpl); } } } // Normal reading -void mpl3115_read(void) +void mpl3115_read(struct Mpl3115 *mpl) { // ask for a reading and then prepare next conversion - if (mpl3115_initialized && mpl3115_trans.status == I2CTransDone) { - mpl3115_trans.buf[0] = MPL3115_REG_STATUS; - i2c_transceive(&MPL3115_I2C_DEV, &mpl3115_trans, MPL3115_I2C_ADDR, 1, 6); - if (mpl3115_req_trans.status == I2CTransDone) { - mpl3115_req_trans.buf[0] = MPL3115_REG_CTRL_REG1; - mpl3115_req_trans.buf[1] = MPL3115_CTRL_REG1 | MPL3115_OST_BIT; - i2c_transmit(&MPL3115_I2C_DEV, &mpl3115_req_trans, MPL3115_I2C_ADDR, 2); + if (mpl->initialized && mpl->trans.status == I2CTransDone) { + mpl->trans.buf[0] = MPL3115_REG_STATUS; + i2c_transceive(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 1, 6); + if (mpl->req_trans.status == I2CTransDone) { + mpl->req_trans.buf[0] = MPL3115_REG_CTRL_REG1; + mpl->req_trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | + (mpl->alt_mode<<7) | MPL3115_OST_BIT); + i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); } } } -void mpl3115_event(void) +void mpl3115_event(struct Mpl3115 *mpl) { - if (mpl3115_initialized) { - if (mpl3115_trans.status == I2CTransFailed) { - mpl3115_trans.status = I2CTransDone; + if (mpl->initialized) { + if (mpl->trans.status == I2CTransFailed) { + mpl->trans.status = I2CTransDone; } - else if (mpl3115_trans.status == I2CTransSuccess) { + else if (mpl->trans.status == I2CTransSuccess) { // Successfull reading and new pressure data available - if (mpl3115_trans.buf[0] & (1<<2)) { -#if MPL3115_RAW_OUTPUT - // New data available - mpl3115_pressure = ((uint32_t)mpl3115_trans.buf[1]<<16)|((uint16_t)mpl3115_trans.buf[2]<<8)|mpl3115_trans.buf[3]; - mpl3115_temperature = ((int16_t)mpl3115_trans.buf[4]<<8)|mpl3115_trans.buf[5]; - mpl3115_data_available = TRUE; -#else // Not in raw mode -#if MPL3115_ALT_MODE - uint32_t tmp = ((uint32_t)mpl3115_trans.buf[1]<<16)|((uint16_t)mpl3115_trans.buf[2]<<8)|mpl3115_trans.buf[3]; - mpl3115_alt = (float)(tmp>>4)/(1<<4); - tmp = ((int16_t)mpl3115_trans.buf[4]<<8)|mpl3115_trans.buf[5]; - mpl3115_temperature = (tmp>>4); - mpl3115_data_available = TRUE; -#else // Pressure mode - uint32_t tmp = ((uint32_t)mpl3115_trans.buf[1]<<16)|((uint16_t)mpl3115_trans.buf[2]<<8)|mpl3115_trans.buf[3]; - mpl3115_pressure = (tmp>>4); - tmp = ((int16_t)mpl3115_trans.buf[4]<<8)|mpl3115_trans.buf[5]; - mpl3115_temperature = (tmp>>4); - mpl3115_data_available = TRUE; -#endif // end alt mode -#endif // end raw mode + if (mpl->trans.buf[0] & (1<<2)) { + if (mpl->raw_mode) { + // New data available + mpl->pressure = (((uint32_t)mpl->trans.buf[1]<<16) | + ((uint16_t)mpl->trans.buf[2]<<8) | + mpl->trans.buf[3]); + mpl->temperature = ((int16_t)mpl->trans.buf[4]<<8) | mpl->trans.buf[5]; + } + else { // not in raw mode + uint32_t tmp = (((uint32_t)mpl->trans.buf[1]<<16) | + ((uint16_t)mpl->trans.buf[2]<<8) | + mpl->trans.buf[3]); + if (mpl->alt_mode) { + mpl->altitude = (float)(tmp>>4) / (1<<4); + } + else { // Pressure mode + mpl->pressure = (tmp>>4); + } + tmp = ((int16_t)mpl->trans.buf[4]<<8) | mpl->trans.buf[5]; + mpl->temperature = (tmp>>4); + } + mpl->data_available = TRUE; } - mpl3115_trans.status = I2CTransDone; + mpl->trans.status = I2CTransDone; } } - else if (!mpl3115_initialized && mpl3115_init_status != MPL_CONF_UNINIT) { // Configuring - if (mpl3115_trans.status == I2CTransSuccess || mpl3115_trans.status == I2CTransDone) { - mpl3115_trans.status = I2CTransDone; - mpl3115_send_config(); + else if (!mpl->initialized && mpl->init_status != MPL_CONF_UNINIT) { // Configuring + if (mpl->trans.status == I2CTransSuccess || mpl->trans.status == I2CTransDone) { + mpl->trans.status = I2CTransDone; + mpl3115_send_config(mpl); } - if (mpl3115_trans.status == I2CTransFailed) { - mpl3115_init_status--; - mpl3115_trans.status = I2CTransDone; - mpl3115_send_config(); // Retry config (TODO max retry) + if (mpl->trans.status == I2CTransFailed) { + mpl->init_status--; + mpl->trans.status = I2CTransDone; + mpl3115_send_config(mpl); // Retry config (TODO max retry) } } - if (mpl3115_req_trans.status == I2CTransSuccess || mpl3115_req_trans.status == I2CTransFailed) { - mpl3115_req_trans.status = I2CTransDone; + if (mpl->req_trans.status == I2CTransSuccess || mpl->req_trans.status == I2CTransFailed) { + mpl->req_trans.status = I2CTransDone; } } +void mpl3115_periodic(struct Mpl3115 *mpl) { + if (mpl->initialized) + mpl3115_read(mpl); + else + mpl3115_configure(mpl); +} diff --git a/sw/airborne/peripherals/mpl3115.h b/sw/airborne/peripherals/mpl3115.h index 9b0977499e..71a9da8a3e 100644 --- a/sw/airborne/peripherals/mpl3115.h +++ b/sw/airborne/peripherals/mpl3115.h @@ -34,11 +34,6 @@ /* Device address (8 bits) */ #define MPL3115_I2C_ADDR 0xC0 -/* Default I2C device */ -#ifndef MPL3115_I2C_DEV -#define MPL3115_I2C_DEV i2c0 -#endif - /* Registers */ #define MPL3115_REG_STATUS 0x00 #define MPL3115_REG_OUT_P_MSB 0x01 @@ -63,36 +58,34 @@ #ifndef MPL3115_OVERSAMPLING #define MPL3115_OVERSAMPLING 0x5 // Oversample ratio (0x5: 130ms between data sample) #endif -#ifndef MPL3115_RAW_OUTPUT -#define MPL3115_RAW_OUTPUT 0x0 // Raw output (disable alt mode if true) -#endif -#ifndef MPL3115_ALT_MODE -#define MPL3115_ALT_MODE 0x1 // 0: baro, 1: alti (disable by raw mode) -#endif -#define MPL3115_CTRL_REG1 ((MPL3115_OVERSAMPLING<<3)|(MPL3115_RAW_OUTPUT<<6)|(MPL3115_ALT_MODE<<7)) -// Config done flag -extern bool_t mpl3115_initialized; -// Data ready flag -extern volatile bool_t mpl3115_data_available; -// Data -extern uint32_t mpl3115_pressure; ///< absolute pressure in Pascal -extern int16_t mpl3115_temperature; -extern float mpl3115_alt; -// I2C transaction structure -//extern struct i2c_transaction mpl3115_trans; +enum Mpl3115Status { + MPL_CONF_UNINIT, + MPL_CONF_PT_DATA, + MPL_CONF_CTRL1, + MPL_CONF_DONE +}; + +struct Mpl3115 { + struct i2c_periph *i2c_p; + struct i2c_transaction trans; ///< I2C transaction for reading and configuring + struct i2c_transaction req_trans; ///< I2C transaction for conversion request + enum Mpl3115Status init_status; + bool_t initialized; ///< config done flag + volatile bool_t data_available; ///< data ready flag + bool_t raw_mode; ///< set to TRUE to enable raw output + bool_t alt_mode; ///< set to TRUE to enable altitude output (otherwise pressure) + int16_t temperature; ///< temperature in 1/16 degrees Celcius + uint32_t pressure; ///< pressure in 1/16 Pascal + float altitude; ///< altitude in meters +}; // Functions -extern void mpl3115_init(void); -extern void mpl3115_configure(void); -extern void mpl3115_read(void); -extern void mpl3115_event(void); - -// Macro for using MPL3115 in periodic function -#define Mpl3115Periodic() { \ - if (mpl3115_initialized) mpl3115_read(); \ - else mpl3115_configure(); \ -} +extern void mpl3115_init(struct Mpl3115 *mpl, struct i2c_periph *i2c_p, uint8_t addr); +extern void mpl3115_configure(struct Mpl3115 *mpl); +extern void mpl3115_read(struct Mpl3115 *mpl); +extern void mpl3115_event(struct Mpl3115 *mpl); +extern void mpl3115_periodic(struct Mpl3115 *mpl); #endif // MPL3115_H From 6e4d2020b9bd3d05efac46f9a3dcc539b373f684 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 14 Sep 2013 23:52:44 +0200 Subject: [PATCH 079/125] [peripherals] baro mpl3115 pressure is in 1/4 pascal --- sw/airborne/boards/apogee/baro_board.c | 2 +- sw/airborne/modules/sensors/baro_mpl3115.c | 2 +- sw/airborne/peripherals/mpl3115.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index a4d335f686..2862bfe449 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -79,7 +79,7 @@ void apogee_baro_event(void) { mpl3115_event(&apogee_baro); if (apogee_baro.data_available) { if (startup_cnt == 0) { - float pressure = ((float)apogee_baro.pressure/(1<<4)); + float pressure = ((float)apogee_baro.pressure/(1<<2)); AbiSendMsgBARO_ABS(APOGEE_BARO_SENDER_ID, &pressure); } apogee_baro.data_available = FALSE; diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index 91dbfefedc..9de0f8346c 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -66,7 +66,7 @@ void baro_mpl3115_read_periodic( void ) { void baro_mpl3115_read_event( void ) { mpl3115_event(&baro_mpl); if (baro_mpl.data_available) { - float pressure = (float)baro_mpl.pressure/(1<<4); + float pressure = (float)baro_mpl.pressure/(1<<2); AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, &pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &baro_mpl.pressure, &baro_mpl.temperature, &baro_mpl.alt); diff --git a/sw/airborne/peripherals/mpl3115.h b/sw/airborne/peripherals/mpl3115.h index 71a9da8a3e..096f91c4ce 100644 --- a/sw/airborne/peripherals/mpl3115.h +++ b/sw/airborne/peripherals/mpl3115.h @@ -77,7 +77,7 @@ struct Mpl3115 { bool_t raw_mode; ///< set to TRUE to enable raw output bool_t alt_mode; ///< set to TRUE to enable altitude output (otherwise pressure) int16_t temperature; ///< temperature in 1/16 degrees Celcius - uint32_t pressure; ///< pressure in 1/16 Pascal + uint32_t pressure; ///< pressure in 1/4 Pascal float altitude; ///< altitude in meters }; From 4775fc5244dab5698e1b8c7adbe325287d3b87be Mon Sep 17 00:00:00 2001 From: softsr Date: Sun, 15 Sep 2013 04:02:46 -0700 Subject: [PATCH 080/125] Added RC setpoint control in H-H mode --- .../examples/krooz_sd/krooz_sd_quad_mkk.xml | 67 ++++++++++--------- .../rotorcraft/guidance/guidance_h.c | 35 +++++++++- 2 files changed, 70 insertions(+), 32 deletions(-) diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index ebdda35837..673fe19937 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -25,7 +25,6 @@ - @@ -101,11 +100,14 @@ + +
+ @@ -145,35 +147,35 @@ - + - - - - + + + + - - - - + + + + - + - - + + - - - + + + - - - + + + @@ -182,7 +184,7 @@ - +
@@ -191,13 +193,14 @@ - - - - - - + + + + + + + @@ -206,12 +209,14 @@
+ - + + + - - +
@@ -229,8 +234,8 @@
- - + +
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index d38140e3c1..ab83aefdfb 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -71,7 +71,9 @@ struct Int32Vect2 guidance_h_pos_sp; struct Int32Vect2 guidance_h_pos_ref; struct Int32Vect2 guidance_h_speed_ref; struct Int32Vect2 guidance_h_accel_ref; - +#ifdef GUIDANCE_H_USE_SPEED_REF +struct Int32Vect2 guidance_h_speed_sp; +#endif struct Int32Vect2 guidance_h_pos_err; struct Int32Vect2 guidance_h_speed_err; struct Int32Vect2 guidance_h_pos_err_sum; @@ -211,6 +213,31 @@ void guidance_h_read_rc(bool_t in_flight) { case GUIDANCE_H_MODE_HOVER: stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); +#ifdef GUIDANCE_H_USE_SPEED_REF + if(in_flight) { + int32_t psi, s_psi, c_psi, rc_norm, max_pprz; + int64_t rc_x, rc_y; + int64_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED); + rc_x = (int64_t)radio_control.values[RADIO_PITCH]; + rc_y = (int64_t)radio_control.values[RADIO_ROLL]; + DeadBand(rc_x, MAX_PPRZ/20); + DeadBand(rc_y, MAX_PPRZ/20); + rc_norm = sqrt(pow(rc_x, 2) + pow(rc_y, 2)); + rc_x = abs(rc_x); + rc_y = abs(rc_y); + max_pprz = rc_norm * MAX_PPRZ / Max(rc_x, rc_y); + rc_x = rc_x * max_speed / max_pprz; + rc_y = -rc_y * max_speed / max_pprz; + /* Rotate to body frame */ + psi = stateGetNedToBodyEulers_i()->psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + guidance_h_speed_sp.x = (int32_t)(((int64_t)-c_psi * rc_x + (int64_t)s_psi * rc_y) / (1 << INT32_TRIG_FRAC)); + guidance_h_speed_sp.y = (int32_t)(((int64_t)-s_psi * rc_x - (int64_t)c_psi * rc_y) / (1 << INT32_TRIG_FRAC)); + } + else + stabilization_attitude_enter(); +#endif break; case GUIDANCE_H_MODE_NAV: @@ -304,12 +331,18 @@ void guidance_h_run(bool_t in_flight) { static void guidance_h_update_reference(void) { /* compute reference even if usage temporarily disabled via guidance_h_use_ref */ #if GUIDANCE_H_USE_REF +#if GUIDANCE_H_USE_SPEED_REF + if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) + gh_update_ref_from_speed_sp(guidance_h_speed_sp); + else +#endif gh_update_ref_from_pos_sp(guidance_h_pos_sp); #endif /* either use the reference or simply copy the pos setpoint */ if (guidance_h_use_ref) { /* convert our reference to generic representation */ + VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC)); INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_speed_ref, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_accel_ref, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); From 7dae549f314c1edea60b3d943fe8b065191eca6c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Sep 2013 16:05:05 +0200 Subject: [PATCH 081/125] [modules] fix baro_m5611 - set data_available to false after it is read - use pprz_isa_altitude_of_pressure --- sw/airborne/modules/sensors/baro_ms5611_i2c.c | 7 ++++--- sw/airborne/modules/sensors/baro_ms5611_spi.c | 7 ++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index 67e0b661ac..635c50616c 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -29,6 +29,7 @@ #include "modules/sensors/baro_ms5611_i2c.h" +#include "math/pprz_isa.h" #include "mcu_periph/sys_time.h" #include "subsystems/abi.h" #include "mcu_periph/uart.h" @@ -97,9 +98,9 @@ void baro_ms5611_event( void ) { if (baro_ms5611.data_available) { float pressure = (float)baro_ms5611.data.pressure; AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); - float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level - tmp_float = pow(tmp_float, 0.190295); - baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL + baro_ms5611.data_available = FALSE; + + baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure); baro_ms5611_alt_valid = TRUE; #ifdef SENSOR_SYNC_SEND diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index e15fcec945..16a3f81012 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -29,6 +29,7 @@ #include "modules/sensors/baro_ms5611_spi.h" +#include "math/pprz_isa.h" #include "mcu_periph/sys_time.h" #include "subsystems/abi.h" #include "mcu_periph/uart.h" @@ -98,9 +99,9 @@ void baro_ms5611_event( void ) { if (baro_ms5611.data_available) { float pressure = (float)baro_ms5611.data.pressure; AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); - float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level - tmp_float = pow(tmp_float, 0.190295); - baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL + baro_ms5611.data_available = FALSE; + + baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure); baro_ms5611_alt_valid = TRUE; #ifdef SENSOR_SYNC_SEND From fc2a471d6fc842b0eb4b2e745b574405bdeead5c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Sep 2013 18:48:25 +0200 Subject: [PATCH 082/125] [rotorcraft] fix hover speed RC setpoint - fix rc_x and rc_y values - still needs to be scaled to max speed using circle instead of box bounding, but while making sure a division by zero can't occur! - only copy pos_ref to pos_sp in hover mode - move GUIDANCE_H_REF_MAX_SPEED|ACCEL to ref header file so it can be used - refactor rc speed setpoint reading into separate function closes #543 --- .../quadrotor_lisa_m_2_pwm_spektrum.xml | 1 + .../rotorcraft/guidance/guidance_h.c | 67 ++++++++++++------- .../rotorcraft/guidance/guidance_h_ref.c | 11 +-- .../rotorcraft/guidance/guidance_h_ref.h | 12 ++++ 4 files changed, 55 insertions(+), 36 deletions(-) diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 4d68ce1534..d8481b8a1d 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -194,6 +194,7 @@
+ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index ab83aefdfb..ec09756d55 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -71,7 +71,7 @@ struct Int32Vect2 guidance_h_pos_sp; struct Int32Vect2 guidance_h_pos_ref; struct Int32Vect2 guidance_h_speed_ref; struct Int32Vect2 guidance_h_accel_ref; -#ifdef GUIDANCE_H_USE_SPEED_REF +#if GUIDANCE_H_USE_SPEED_REF struct Int32Vect2 guidance_h_speed_sp; #endif struct Int32Vect2 guidance_h_pos_err; @@ -97,6 +97,7 @@ static void guidance_h_traj_run(bool_t in_flight); static void guidance_h_hover_enter(void); static void guidance_h_nav_enter(void); static inline void transition_run(void); +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight); void guidance_h_init(void) { @@ -213,30 +214,8 @@ void guidance_h_read_rc(bool_t in_flight) { case GUIDANCE_H_MODE_HOVER: stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight); -#ifdef GUIDANCE_H_USE_SPEED_REF - if(in_flight) { - int32_t psi, s_psi, c_psi, rc_norm, max_pprz; - int64_t rc_x, rc_y; - int64_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED); - rc_x = (int64_t)radio_control.values[RADIO_PITCH]; - rc_y = (int64_t)radio_control.values[RADIO_ROLL]; - DeadBand(rc_x, MAX_PPRZ/20); - DeadBand(rc_y, MAX_PPRZ/20); - rc_norm = sqrt(pow(rc_x, 2) + pow(rc_y, 2)); - rc_x = abs(rc_x); - rc_y = abs(rc_y); - max_pprz = rc_norm * MAX_PPRZ / Max(rc_x, rc_y); - rc_x = rc_x * max_speed / max_pprz; - rc_y = -rc_y * max_speed / max_pprz; - /* Rotate to body frame */ - psi = stateGetNedToBodyEulers_i()->psi; - PPRZ_ITRIG_SIN(s_psi, psi); - PPRZ_ITRIG_COS(c_psi, psi); - guidance_h_speed_sp.x = (int32_t)(((int64_t)-c_psi * rc_x + (int64_t)s_psi * rc_y) / (1 << INT32_TRIG_FRAC)); - guidance_h_speed_sp.y = (int32_t)(((int64_t)-s_psi * rc_x - (int64_t)c_psi * rc_y) / (1 << INT32_TRIG_FRAC)); - } - else - stabilization_attitude_enter(); +#if GUIDANCE_H_USE_SPEED_REF + read_rc_setpoint_speed_i(&guidance_h_speed_sp, in_flight); #endif break; @@ -342,7 +321,6 @@ static void guidance_h_update_reference(void) { /* either use the reference or simply copy the pos setpoint */ if (guidance_h_use_ref) { /* convert our reference to generic representation */ - VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC)); INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_speed_ref, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_accel_ref, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); @@ -351,6 +329,12 @@ static void guidance_h_update_reference(void) { INT_VECT2_ZERO(guidance_h_speed_ref); INT_VECT2_ZERO(guidance_h_accel_ref); } + +#if GUIDANCE_H_USE_SPEED_REF + if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) { + VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only + } +#endif } @@ -443,3 +427,34 @@ static inline void transition_run(void) { transition_theta_offset = INT_MULT_RSHIFT((transition_percentage<<(INT32_ANGLE_FRAC-INT32_PERCENTAGE_FRAC))/100, max_offset, INT32_ANGLE_FRAC); #endif } + +/// read speed setpoint from RC +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) { + if(in_flight) { + // negative pitch is forward + int64_t rc_x = -radio_control.values[RADIO_PITCH]; + int64_t rc_y = radio_control.values[RADIO_ROLL]; + DeadBand(rc_x, MAX_PPRZ/20); + DeadBand(rc_y, MAX_PPRZ/20); + + // convert input from MAX_PPRZ range to SPEED_BFP + int32_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED); + /// @todo calc proper scale while making sure a division by zero can't occur + //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y); + //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y); + rc_x = rc_x * max_speed / MAX_PPRZ; + rc_y = rc_y * max_speed / MAX_PPRZ; + + /* Rotate from body to NED frame by negative psi angle */ + int32_t psi = -stateGetNedToBodyEulers_i()->psi; + int32_t s_psi, c_psi; + PPRZ_ITRIG_SIN(s_psi, psi); + PPRZ_ITRIG_COS(c_psi, psi); + speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x - (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC); + speed_sp->y = (int32_t)(((int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC); + } + else { + speed_sp->x = 0; + speed_sp->y = 0; + } +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index 34b23d212c..fb8be1fa67 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -47,18 +47,9 @@ struct Int32Vect2 gh_speed_ref; */ struct Int64Vect2 gh_pos_ref; -/** Accel saturation. - * tanf(RadOfDeg(30.))*9.81 = 5.66 - */ -#ifndef GUIDANCE_H_REF_MAX_ACCEL -#define GUIDANCE_H_REF_MAX_ACCEL 5.66 -#endif + static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC); -/** Speed saturation */ -#ifndef GUIDANCE_H_REF_MAX_SPEED -#define GUIDANCE_H_REF_MAX_SPEED 5. -#endif /** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ #define GH_MAX_SPEED_REF_FRAC 7 static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index ab285baa11..179097db55 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -31,6 +31,18 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" +/** Speed saturation */ +#ifndef GUIDANCE_H_REF_MAX_SPEED +#define GUIDANCE_H_REF_MAX_SPEED 5. +#endif + +/** Accel saturation. + * tanf(RadOfDeg(30.))*9.81 = 5.66 + */ +#ifndef GUIDANCE_H_REF_MAX_ACCEL +#define GUIDANCE_H_REF_MAX_ACCEL 5.66 +#endif + /** Update frequency */ #define GH_FREQ_FRAC 9 From a0b81eac001bbb57d48fab2a9390b756b685fd94 Mon Sep 17 00:00:00 2001 From: softsr Date: Sun, 15 Sep 2013 04:15:53 -0700 Subject: [PATCH 083/125] Proper vertical climb/descend speed calculation --- conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml | 5 +---- sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c | 9 ++++++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index 673fe19937..9e0655d7ef 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -197,13 +197,10 @@ + - - - -
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 23f6d5b11b..36bcf836c5 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -131,9 +131,12 @@ void guidance_v_read_rc(void) { guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ - guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF; - DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND); - + guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]); + DeadBand(guidance_v_rc_zd_sp, MAX_PPRZ/10); + if(guidance_v_rc_zd_sp > 0) + guidance_v_rc_zd_sp = SPEED_BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD) / (MAX_PPRZ/2 - MAX_PPRZ/10) * guidance_v_rc_zd_sp; + else + guidance_v_rc_zd_sp = -SPEED_BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD) / (MAX_PPRZ/2 - MAX_PPRZ/10) * guidance_v_rc_zd_sp; } void guidance_v_mode_changed(uint8_t new_mode) { From 91678391d0c52b6210a279d155cfad6d7b2d5934 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Sep 2013 21:39:57 +0200 Subject: [PATCH 084/125] [rotorcraft] RC_CLIMB cleanup - add GUIDANCE_V_CLIMB_RC_DEADBAND (in pprz units, default MAX_PPRZ/10) - GUIDANCE_V_RC_CLIMB_DEAD_BAND is obsolete - GUIDANCE_V_MAX_RC_CLIMB_SPEED defaults to GUIDANCE_V_REF_MIN_ZD - GUIDANCE_V_MAX_RC_DESCENT_SPEED defaults to GUIDANCE_V_REF_MAX_ZD --- .../rotorcraft/guidance/guidance_v.c | 37 ++++++++++++++----- .../rotorcraft/guidance/guidance_v_ref.c | 7 ---- .../rotorcraft/guidance/guidance_v_ref.h | 10 +++++ 3 files changed, 38 insertions(+), 16 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 36bcf836c5..7ec75d152d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -62,6 +62,19 @@ PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE) PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED) +#ifndef GUIDANCE_V_CLIMB_RC_DEADBAND +#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10 +#endif + +#ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED +#define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD +#endif + +#ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED +#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD +#endif + + uint8_t guidance_v_mode; int32_t guidance_v_ff_cmd; int32_t guidance_v_fb_cmd; @@ -98,10 +111,10 @@ int32_t guidance_v_thrust_coeff; #define GuidanceVSetRef(_pos, _speed, _accel) { \ - gv_set_ref(_pos, _speed, _accel); \ - guidance_v_z_ref = _pos; \ - guidance_v_zd_ref = _speed; \ - guidance_v_zdd_ref = _accel; \ + gv_set_ref(_pos, _speed, _accel); \ + guidance_v_z_ref = _pos; \ + guidance_v_zd_ref = _speed; \ + guidance_v_zdd_ref = _accel; \ } static int32_t get_vertical_thrust_coeff(void); @@ -131,12 +144,18 @@ void guidance_v_read_rc(void) { guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ - guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]); - DeadBand(guidance_v_rc_zd_sp, MAX_PPRZ/10); + guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]; + DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND); + + static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + if(guidance_v_rc_zd_sp > 0) - guidance_v_rc_zd_sp = SPEED_BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD) / (MAX_PPRZ/2 - MAX_PPRZ/10) * guidance_v_rc_zd_sp; - else - guidance_v_rc_zd_sp = -SPEED_BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD) / (MAX_PPRZ/2 - MAX_PPRZ/10) * guidance_v_rc_zd_sp; + guidance_v_rc_zd_sp *= descent_scale; + else + guidance_v_rc_zd_sp *= climb_scale; } void guidance_v_mode_changed(uint8_t new_mode) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c index 281827bb0b..31d1e2c0e4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c @@ -58,14 +58,7 @@ int64_t gv_z_ref; #endif #define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC) -#ifndef GUIDANCE_V_REF_MIN_ZD -#define GUIDANCE_V_REF_MIN_ZD (-3.) -#endif #define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC) - -#ifndef GUIDANCE_V_REF_MAX_ZD -#define GUIDANCE_V_REF_MAX_ZD ( 3.) -#endif #define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC) /* second order model natural frequency and damping */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index f462a07a2b..3816a9031b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -32,6 +32,16 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" + +#ifndef GUIDANCE_V_REF_MIN_ZD +#define GUIDANCE_V_REF_MIN_ZD (-3.) +#endif + +#ifndef GUIDANCE_V_REF_MAX_ZD +#define GUIDANCE_V_REF_MAX_ZD ( 3.) +#endif + + /** Update frequency */ #define GV_FREQ_FRAC 9 From 6b7fdd25a71f22a782342a0073398f62f63b8d83 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Sep 2013 21:43:07 +0200 Subject: [PATCH 085/125] [conf][airframes] remove obsolete RC_CLIMB_COEF and RC_CLIMB_DEAD_BAND --- conf/airframes/CDW/asctec_cdw.xml | 4 ---- conf/airframes/CDW/tricopter_cdw.xml | 4 ---- conf/airframes/ENAC/quadrotor/blender.xml | 2 -- conf/airframes/ENAC/quadrotor/booz2_g1.xml | 2 -- conf/airframes/ENAC/quadrotor/hen1.xml | 2 -- conf/airframes/NoVa_L.xml | 4 ---- conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml | 4 ---- conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml | 4 ---- conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml | 4 ---- conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml | 4 ---- conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml | 4 ---- conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml | 4 ---- conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml | 4 ---- conf/airframes/ardrone2_raw.xml | 4 ---- conf/airframes/ardrone2_sdk.xml | 4 ---- conf/airframes/booz2_flixr.xml | 2 -- conf/airframes/booz2_ppzuav.xml | 2 -- conf/airframes/esden/cocto_lm2a2.xml | 4 ---- conf/airframes/esden/gain_scheduling_example.xml | 2 -- conf/airframes/esden/hexy_ll11a2pwm.xml | 4 ---- conf/airframes/esden/hexy_lm2a2pwm.xml | 4 ---- conf/airframes/esden/lisa2_hex.xml | 4 ---- conf/airframes/esden/qs_asp22.xml | 2 -- conf/airframes/esden/quady_ll11a2pwm.xml | 4 ---- conf/airframes/esden/quady_lm1a1pwm.xml | 4 ---- conf/airframes/esden/quady_lm2a2pwm.xml | 4 ---- conf/airframes/esden/quady_lm2a2pwmppm.xml | 4 ---- conf/airframes/esden/quady_ls01pwm.xml | 4 ---- conf/airframes/examples/booz2.xml | 4 ---- conf/airframes/examples/h_hex.xml | 4 ---- conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml | 2 -- conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml | 2 -- conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml | 2 -- conf/airframes/examples/lisa_asctec.xml | 4 ---- conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml | 4 ---- .../examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml | 4 ---- conf/airframes/examples/quadrotor_lisa_m_mkk.xml | 4 ---- conf/airframes/examples/quadrotor_lisa_s.xml | 4 ---- conf/airframes/examples/quadrotor_mlkf.xml | 4 ---- conf/airframes/examples/quadrotor_navgo.xml | 2 -- conf/airframes/examples/quadrotor_px4fmu.xml | 4 ---- conf/airframes/examples/quadshot_asp21_spektrum.xml | 2 -- conf/airframes/examples/stm32f4_discovery_test.xml | 4 ---- conf/airframes/fraser_lisa_m_rotorcraft.xml | 4 ---- conf/airframes/obsolete/ENAC/g1_vision.xml | 2 -- conf/airframes/obsolete/ENAC/mkk1-vision.xml | 4 ---- conf/airframes/obsolete/ENAC/mkk1.xml | 4 ---- conf/airframes/obsolete/ENAC/nova1.xml | 4 ---- .../obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml | 2 -- .../obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml | 2 -- conf/airframes/obsolete/Poine/booz2_a1.xml | 4 ---- conf/airframes/obsolete/Poine/booz2_a7.xml | 4 ---- conf/airframes/obsolete/Poine/h_hex.xml | 4 ---- conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml | 4 ---- conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml | 4 ---- conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml | 2 -- conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml | 2 -- conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml | 4 ---- conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml | 4 ---- conf/airframes/obsolete/booz2_Aron.xml | 2 -- conf/airframes/obsolete/booz2_NoVa.xml | 2 -- conf/airframes/obsolete/booz2_NoVa_001.xml | 2 -- conf/airframes/obsolete/booz2_NoVa_002.xml | 2 -- conf/airframes/obsolete/booz2_a1p.xml | 4 ---- conf/airframes/obsolete/booz2_a2.xml | 2 -- conf/airframes/obsolete/booz2_a3.xml | 2 -- conf/airframes/obsolete/booz2_a4.xml | 2 -- conf/airframes/obsolete/booz2_a5.xml | 4 ---- conf/airframes/obsolete/booz2_s1.xml | 2 -- conf/airframes/obsolete/booz2_x1.xml | 2 -- conf/airframes/obsolete/example_heli_lisam.xml | 4 ---- conf/airframes/obsolete/mm/rotor/qmk1.xml | 4 ---- conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml | 4 ---- conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml | 4 ---- conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml | 4 ---- 75 files changed, 248 deletions(-) diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index 830c176d2f..48bf3d250b 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -182,10 +182,6 @@ - - - -
diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 86946f11f3..51282ed794 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -179,10 +179,6 @@ LiPo/LiIo-Zellen: 2-3 - - - -
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 2dce3cc671..0e30d8b98e 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -225,9 +225,7 @@ - -
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 834132e720..7bc59e2ff9 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -199,9 +199,7 @@ - -
diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 2c0bdb49ae..82bbdad77c 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -205,9 +205,7 @@ - -
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml index aaf45e3e27..d0834d9e57 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/NoVa_L.xml @@ -208,10 +208,6 @@ --> - - - -
diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index c67a98311e..8743fa786c 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -176,10 +176,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index ace2179575..35c2f7e1bb 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -157,10 +157,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 064c01ae5a..f6f649785d 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -159,10 +159,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml index a11d9f889b..31e1e248c8 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml @@ -143,10 +143,6 @@ - - - - diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index 4e7dda0a2e..e974354f94 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -146,10 +146,6 @@ - - - - diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml index ee9e0848ac..2abbffbe35 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml @@ -234,10 +234,6 @@ - - - - diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml index 06f2f97307..ecf1a1c44f 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml @@ -234,10 +234,6 @@ - - - - diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 99265a1638..851a85abfe 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -186,10 +186,6 @@ - - - - diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 73c21af476..792c0bc74f 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -81,10 +81,6 @@ - - - - diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index a62ace8aee..1caaad3f40 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -204,9 +204,7 @@ - - diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml index af532c20f2..8dd9a4a01c 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/booz2_ppzuav.xml @@ -164,9 +164,7 @@ - -
diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 609e7295bd..0378866152 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -182,10 +182,6 @@ B2L -> CW - - - -
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index 02f574bd9f..af9bf85d86 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -165,8 +165,6 @@ - - diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index f796f42c68..0d90506b29 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -179,10 +179,6 @@ - - - - diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 73554f3209..3ae9b86194 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -140,10 +140,6 @@ - - - - diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index b21dddffcb..03c2cf1fa1 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -147,10 +147,6 @@ - - - - diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index c4ef62b938..767074fd1a 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -173,8 +173,6 @@ - - diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index ca80e5a5d2..96374827a9 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -175,10 +175,6 @@ - - - - diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index 406734e757..9b53ad433b 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -136,10 +136,6 @@ - - - - diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index e87c817b3e..696ebcaea9 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -139,10 +139,6 @@ - - - - diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index 31fd65c39d..b4de081a06 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -136,10 +136,6 @@ - - - - diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 4a3a194c39..0e1ca2d7e1 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -139,10 +139,6 @@ - - - - diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index e93ccf9d2c..8d55914c9f 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -160,10 +160,6 @@ - - - - diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml index 5faaedebe3..0f9c50c31c 100644 --- a/conf/airframes/examples/h_hex.xml +++ b/conf/airframes/examples/h_hex.xml @@ -159,10 +159,6 @@ - - - - diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 44f2681826..51f8b24ec9 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -206,9 +206,7 @@ - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index 4bf55eeb9f..505eb445e7 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -209,9 +209,7 @@ - -
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index cf85a484c0..02f161cbe8 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -194,9 +194,7 @@ - -
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 5e4869cb87..3023bdf628 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -158,10 +158,6 @@ - - - -
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index d8481b8a1d..4a5bb02b9f 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -184,10 +184,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml index 9f7210bd3e..df3fb54495 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -187,10 +187,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 4746f8f78d..a5552bdc51 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -176,10 +176,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index 877026c43b..450b8469b1 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -158,10 +158,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 781cbd771a..f3dc3f729c 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -186,10 +186,6 @@ - - - - diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 98df961abd..5a0952f747 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -208,9 +208,7 @@ - - diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index fb02773791..e43771927f 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -180,10 +180,6 @@ - - - - diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index d5fc8439fd..36dce08da3 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -197,8 +197,6 @@ More information on the Quadshot can be found at transition-robotics.com --> - - diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index ed339e5a4a..f5555ae309 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -182,10 +182,6 @@ - - - - diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 451e682961..f8929f63ff 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -220,10 +220,6 @@ - - - - diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index d24b588c2e..72dca9815e 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -156,9 +156,7 @@ - - diff --git a/conf/airframes/obsolete/ENAC/mkk1-vision.xml b/conf/airframes/obsolete/ENAC/mkk1-vision.xml index 4d78dbd80b..23c7008d8a 100644 --- a/conf/airframes/obsolete/ENAC/mkk1-vision.xml +++ b/conf/airframes/obsolete/ENAC/mkk1-vision.xml @@ -189,10 +189,6 @@ - - - - diff --git a/conf/airframes/obsolete/ENAC/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml index 18a9742bc7..7f776cac2f 100644 --- a/conf/airframes/obsolete/ENAC/mkk1.xml +++ b/conf/airframes/obsolete/ENAC/mkk1.xml @@ -209,10 +209,6 @@ - - - - diff --git a/conf/airframes/obsolete/ENAC/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml index 0ef106ba39..d4f1fcf1e0 100644 --- a/conf/airframes/obsolete/ENAC/nova1.xml +++ b/conf/airframes/obsolete/ENAC/nova1.xml @@ -180,10 +180,6 @@ - - - - diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index 0c2cfe06c0..e854e832f9 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -164,9 +164,7 @@ - - diff --git a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index 6d60135e55..210bb076cc 100644 --- a/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/obsolete/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -182,9 +182,7 @@ - - diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 5823e29e03..2b6e8d4477 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -154,10 +154,6 @@ - - - - diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index d3f23c31fd..6700f2db08 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -180,10 +180,6 @@ - - - - diff --git a/conf/airframes/obsolete/Poine/h_hex.xml b/conf/airframes/obsolete/Poine/h_hex.xml index 252cdacf33..68b5909458 100644 --- a/conf/airframes/obsolete/Poine/h_hex.xml +++ b/conf/airframes/obsolete/Poine/h_hex.xml @@ -140,10 +140,6 @@ - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 10b61e41a0..133609d264 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -228,10 +228,6 @@ second attempt - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml index 5f3ee2a945..b3ffc90742 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml @@ -165,10 +165,6 @@ - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index 406d6de2c0..2d79f20d0c 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -189,9 +189,7 @@ - - diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml index 3328cc4b18..a49c75280d 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_NoVa_001_1000.xml @@ -189,9 +189,7 @@ - - diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 82f499852b..d696663c45 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -229,10 +229,6 @@ second attempt - - - - diff --git a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml index 0cecb4fdb2..a3c44a1e7f 100644 --- a/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/lisa_a1000.xml @@ -144,10 +144,6 @@ - - - - diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml index a5a48f5295..416dab39d9 100644 --- a/conf/airframes/obsolete/booz2_Aron.xml +++ b/conf/airframes/obsolete/booz2_Aron.xml @@ -149,9 +149,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml index aa12a6dfdd..c34c091e2e 100644 --- a/conf/airframes/obsolete/booz2_NoVa.xml +++ b/conf/airframes/obsolete/booz2_NoVa.xml @@ -189,9 +189,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml index 81c9021e83..1a5409d1bf 100644 --- a/conf/airframes/obsolete/booz2_NoVa_001.xml +++ b/conf/airframes/obsolete/booz2_NoVa_001.xml @@ -189,9 +189,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml index ed879fb742..c09fd3073e 100644 --- a/conf/airframes/obsolete/booz2_NoVa_002.xml +++ b/conf/airframes/obsolete/booz2_NoVa_002.xml @@ -189,9 +189,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index 18ff3d98f8..d9e902c05c 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -158,10 +158,6 @@ - - - - diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml index d0f9361638..2a92ca9e34 100644 --- a/conf/airframes/obsolete/booz2_a2.xml +++ b/conf/airframes/obsolete/booz2_a2.xml @@ -149,9 +149,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml index 0635e3ce89..0cf7baf229 100644 --- a/conf/airframes/obsolete/booz2_a3.xml +++ b/conf/airframes/obsolete/booz2_a3.xml @@ -134,9 +134,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml index e9cc7ede69..85b6b0030f 100644 --- a/conf/airframes/obsolete/booz2_a4.xml +++ b/conf/airframes/obsolete/booz2_a4.xml @@ -112,9 +112,7 @@ - -
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml index 6fc2fe07eb..28bfac3b35 100644 --- a/conf/airframes/obsolete/booz2_a5.xml +++ b/conf/airframes/obsolete/booz2_a5.xml @@ -150,10 +150,6 @@ - - - -
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 4cf6bb4bbe..3b27bd401a 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -142,9 +142,7 @@ - - diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml index 370920cd85..c8e5d7374a 100644 --- a/conf/airframes/obsolete/booz2_x1.xml +++ b/conf/airframes/obsolete/booz2_x1.xml @@ -154,9 +154,7 @@ - - diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index 79e8d519ac..6ca655ad60 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -193,10 +193,6 @@ AP_MODE_NAV - - - - diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 37b3b6c07a..4093f1a9a9 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -154,10 +154,6 @@ - - - - diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml index 5d1b3e397d..f52ddd2f1f 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml @@ -201,10 +201,6 @@ - - - - diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml index 5343031357..724162a770 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml @@ -164,10 +164,6 @@ - - - -
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml index 2ba487a784..12694aaf9c 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml @@ -171,10 +171,6 @@ - - - -
From 413cd51ffbae80304a0f63259ab70bd06c2028ff Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 16 Sep 2013 11:27:53 +0200 Subject: [PATCH 086/125] [modules] baro_ms611 SENSOR_SYNC_SEND for sending coeff every 30s --- sw/airborne/modules/sensors/baro_ms5611_i2c.c | 9 +++++---- sw/airborne/modules/sensors/baro_ms5611_spi.c | 9 +++++---- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index 635c50616c..690d7e212b 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -78,9 +78,9 @@ void baro_ms5611_periodic_check( void ) { ms5611_i2c_periodic_check(&baro_ms5611); -#if BARO_MS5611_SEND_COEFF - // send coeff every 5s - RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#if SENSOR_SYNC_SEND + // send coeff every 30s + RunOnceEvery((30*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); #endif } @@ -107,7 +107,8 @@ void baro_ms5611_event( void ) { ftempms = baro_ms5611.data.temperature / 100.; fbaroms = baro_ms5611.data.pressure / 100.; DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms); + &baro_ms5611.data.d1, &baro_ms5611.data.d2, + &fbaroms, &ftempms); #endif } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index 16a3f81012..ce4f2f5307 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -78,9 +78,9 @@ void baro_ms5611_periodic_check( void ) { ms5611_spi_periodic_check(&baro_ms5611); -#if BARO_MS5611_SEND_COEFF - // send coeff every 5s - RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff()); +#if SENSOR_SYNC_SEND + // send coeff every 30s + RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); #endif } @@ -108,7 +108,8 @@ void baro_ms5611_event( void ) { ftempms = baro_ms5611.data.temperature / 100.; fbaroms = baro_ms5611.data.pressure / 100.; DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice, - &baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms); + &baro_ms5611.data.d1, &baro_ms5611.data.d2, + &fbaroms, &ftempms); #endif } } From 4e7975cea974d36ec1df90a9021cbb551e513793 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Tue, 24 Sep 2013 17:06:01 +0200 Subject: [PATCH 087/125] [ArDrone] Fix breaking autoconfig --- conf/Makefile.omap | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/conf/Makefile.omap b/conf/Makefile.omap index e567d0c314..9cc359d7cd 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -81,6 +81,38 @@ load upload program: $(OBJDIR)/$(TARGET).elf # Kill the application -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + # Make the target dir and edit the config + -{ \ + echo "mkdir -p $(TARGET_DIR)"; \ + } | telnet $(HOST) + + # Upload the drivers and new application + { \ + echo "binary"; \ + echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko"; \ + echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \ + echo "quit"; \ + } | ftp -n $(HOST) + + # Upload the modules and start the application + -{ \ + echo "insmod $(TARGET_DIR)/cdc-acm.ko"; \ + echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \ + echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \ + } | telnet $(HOST) + +ifeq ($(ARDRONE2_REBOOT),1) + -{ \ + echo "reboot"; \ + } | telnet $(HOST) +endif + +# Program the device and start it. +load2 upload2 program2: $(OBJDIR)/$(TARGET).elf + + # Kill the application + -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + # Make the target dir and edit the config -{ \ echo "mkdir -p $(TARGET_DIR)"; \ @@ -111,7 +143,7 @@ load upload program: $(OBJDIR)/$(TARGET).elf echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \ echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \ } | telnet $(HOST) - + ifeq ($(ARDRONE2_REBOOT),1) -{ \ echo "reboot"; \ @@ -119,6 +151,8 @@ ifeq ($(ARDRONE2_REBOOT),1) endif + + # Link: create ELF output file from object files. .SECONDARY : $(OBJDIR)/$(TARGET).elf .PRECIOUS : $(OBJ_C_OMAP) $(OBJ_CPP_OMAP) @@ -140,6 +174,7 @@ $(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac $(Q)test -d $(dir $@) || mkdir -p $(dir $@) $(Q)$(CXX) -c $(CXXFLAGS) $< -o $@ + # Listing of phony targets. .PHONY : all build elf clean clean_list From a1f48373fa5eeeadf85d8a30481445596e3781de Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Sep 2013 10:26:46 +0200 Subject: [PATCH 088/125] [mission] update mission interface --- conf/messages.xml | 10 ++ conf/modules/mission_msg.xml | 7 +- sw/airborne/modules/mission/mission_msg.c | 169 ++++++++++++---------- sw/airborne/modules/mission/mission_msg.h | 71 ++++++--- 4 files changed, 158 insertions(+), 99 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 99700443f9..ce098db290 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2214,6 +2214,7 @@ + @@ -2223,6 +2224,7 @@ + @@ -2233,6 +2235,7 @@ + @@ -2249,6 +2252,7 @@ + @@ -2259,6 +2263,12 @@ + + + + + + diff --git a/conf/modules/mission_msg.xml b/conf/modules/mission_msg.xml index 8812e48f81..1dee0bd4a7 100644 --- a/conf/modules/mission_msg.xml +++ b/conf/modules/mission_msg.xml @@ -6,13 +6,18 @@
+ +
- + + + + diff --git a/sw/airborne/modules/mission/mission_msg.c b/sw/airborne/modules/mission/mission_msg.c index 95c084f56b..c897805794 100644 --- a/sw/airborne/modules/mission/mission_msg.c +++ b/sw/airborne/modules/mission/mission_msg.c @@ -5,18 +5,18 @@ //#define MISSION_C - +#include #include "modules/mission/mission_msg.h" #include #include "subsystems/datalink/downlink.h" -#include "state.h" +/*#include "state.h" #include "autopilot.h" #include "subsystems/gps.h" #include "generated/airframe.h" -#include "dl_protocol.h" +#include "dl_protocol.h"*/ struct _mission mission; @@ -25,92 +25,103 @@ struct _mission mission; void mission_init(void) { mission.mission_insert_idx = 0; mission.mission_extract_idx = 0; + //mission_nav_finish = FALSE; } int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d) { - mission.elements[insert_idx].type = MissionWP; - mission.elements[insert_idx].element.mission_wp.wp.x = x; - mission.elements[insert_idx].element.mission_wp.wp.y = y; - mission.elements[insert_idx].element.mission_wp.wp.a = a; - mission.elements[insert_idx].element.duration = d; + //int insert_idx = mission.mission_insert_idx; + mission.elements[mission.mission_insert_idx].type = MissionWP; + mission.elements[mission.mission_insert_idx].element.mission_wp.wp.x = x; + mission.elements[mission.mission_insert_idx].element.mission_wp.wp.y = y; + mission.elements[mission.mission_insert_idx].element.mission_wp.wp.a = a; + mission.elements[mission.mission_insert_idx].duration = d; - insert_idx++; - if (insert_idx == MISSION_ELEMENT_NB) insert_idx = 0; + //printf("stocking mission GOTO_WP...\n");fflush(stdout); + mission.mission_insert_idx++; + if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; return TRUE; } -int mission_msg_SEGMENT(float x1, float y1, float x2, float y2) { - if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { - return FALSE; // end of block and go to the next block -// NextBlock(); - } - else { - //NextBlock() - //nav_init_stage(); - nav_route_xy(x1, y1, x2, y2); - NavVerticalAutoThrottleMode(RadOfDeg(0.000000)); - NavVerticalAltitudeMode(5.0, 0.); - } +int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d) { + //int insert_idx = mission.mission_insert_idx; + mission.elements[mission.mission_insert_idx].type = MissionSegment; + mission.elements[mission.mission_insert_idx].element.mission_segment.from.x = x1; + mission.elements[mission.mission_insert_idx].element.mission_segment.from.y = y1; + mission.elements[mission.mission_insert_idx].element.mission_segment.from.a = a; + mission.elements[mission.mission_insert_idx].element.mission_segment.to.x = x2; + mission.elements[mission.mission_insert_idx].element.mission_segment.to.y = y2; + mission.elements[mission.mission_insert_idx].element.mission_segment.to.a = a; + mission.elements[mission.mission_insert_idx].duration = d; + + //printf("stocking mission SEGMENT...\n");fflush(stdout); + //printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout); + mission.mission_insert_idx++; + if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; + return TRUE; +} + + +int mission_msg_CIRCLE(float x, float y, float radius, float a, uint16_t d) { + //int insert_idx = mission.mission_insert_idx; + mission.elements[mission.mission_insert_idx].type = MissionCircle; + mission.elements[mission.mission_insert_idx].element.mission_circle.center.x = x; + mission.elements[mission.mission_insert_idx].element.mission_circle.center.y = y; + mission.elements[mission.mission_insert_idx].element.mission_circle.center.a = a; + mission.elements[mission.mission_insert_idx].element.mission_circle.radius = radius; + mission.elements[mission.mission_insert_idx].duration = d; + + //printf("stocking mission CIRCLE...\n");fflush(stdout); + //printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout); + mission.mission_insert_idx++; + if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; + return TRUE; +} + +//int i_path, i_point; +int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5, float a, uint16_t d) { + //int insert_idx = mission.mission_insert_idx; + mission.elements[mission.mission_insert_idx].type = MissionPath; + + int i_path; + i_path = 0; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x1; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y1; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; + + + i_path++; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x2; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y2; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; + + + i_path++; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x3; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y3; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; + + + i_path++; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x4; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y4; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; + + + i_path++; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x5; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y5; + mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; + + + mission.elements[mission.mission_insert_idx].duration = d; + + //printf("stocking mission PATH...\n");fflush(stdout); + //printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout); + mission.mission_insert_idx++; + if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; return TRUE; } -/** Status along the mission path */ -enum mission_msg_PATH_status { Path12, Path23, Path34, Path45 }; -static enum mission_msg_PATH_status mission_msg_PATH_status; - -int mission_msg_PATH_init( void ) { - // status of the first segment of the path - mission_msg_PATH_status = Path12; - - // path finder here to generate intermediate waypoints - // these five points are defined as global variables in mission_info.h - //struct tabWaypoint tabW; - //tabW = pathFinder(); - //pathFinder(); - - return FALSE; -} - -int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5) { - - switch(mission_msg_PATH_status) { - case Path12: /* path 1-2 */ - nav_route_xy(x1, y1, x2, y2); - if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { - mission_msg_PATH_status = Path23; - //nav_init_stage(); - } - break; - case Path23: /* path 2-3 */ - nav_route_xy(x2, y2, x3, y3); - if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) { - mission_msg_PATH_status = Path34; - //nav_init_stage(); - } - break; - case Path34: /* path 3-4 */ - nav_route_xy(x3, y3, x4, y4); - if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) { - mission_msg_PATH_status = Path45; - //nav_init_stage(); - } - break; - case Path45: /* path 4-5 */ - nav_route_xy(x4, y4, x5, y5); - if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) { - //mission_msg_PATH_status = Path12; - //nav_init_stage(); - return FALSE; /* This path ends when AC arrive to point 5 */ - } - break; - } - - - return TRUE; /* do the function */ -} - - - diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h index c35d5f42f6..e6436491be 100644 --- a/sw/airborne/modules/mission/mission_msg.h +++ b/sw/airborne/modules/mission/mission_msg.h @@ -30,6 +30,16 @@ #include "std.h" #include "subsystems/navigation/common_nav.h" +#include + +#include "subsystems/datalink/downlink.h" + +#include "state.h" +#include "autopilot.h" +#include "subsystems/gps.h" +#include "generated/airframe.h" +#include "dl_protocol.h" + enum MissionType { MissionWP, MissionCircle, @@ -56,10 +66,12 @@ struct _mission_segment { #define MISSION_PATH_NB 5 struct _mission_path { - struct point[MISSION_PATH_NB] path; + //struct point[MISSION_PATH_NB] path; + struct point path[MISSION_PATH_NB]; }; struct _mission_element { + //char *mission_name; enum MissionType type; union { struct _mission_wp mission_wp; @@ -68,27 +80,34 @@ struct _mission_element { struct _mission_path mission_path; } element; uint16_t duration; + //bool_t mission_finish; }; #define MISSION_ELEMENT_NB 20 struct _mission { - struct _mission_element[MISSION_ELEMENT_NB] elements; + //struct _mission_element[MISSION_ELEMENT_NB] elements; + struct _mission_element elements[MISSION_ELEMENT_NB]; uint8_t mission_insert_idx; uint8_t mission_extract_idx; - + //bool_t mission_nav_finish = FALSE; }; extern struct _mission mission; - +//extern bool_t mission_nav_finish; extern void mission_init(void); -extern int mission_msg_GOTO_WP(float x, float y); +extern int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d); -extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2); +extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d); -extern int mission_msg_PATH_init( void ); -extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5); +extern int mission_msg_CIRCLE(float x, float y, float radius, float a, uint16_t d); + +//extern int mission_msg_PATH_init( void ); +extern int i_path, i_point; +extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5, float a, uint16_t d); + +extern int goto_mission_msg(uint8_t ac_id, uint8_t mission_id); @@ -97,7 +116,9 @@ extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, fl uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \ float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \ float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \ - mission_msg_GOTO_WP(wp_east, wp_north); \ + float wp_alt = DL_MISSION_GOTO_WP_wp_alt(dl_buffer); \ + float duration = DL_MISSION_GOTO_WP_duration(dl_buffer); \ + mission_msg_GOTO_WP(wp_east, wp_north, wp_alt, duration); \ } \ } @@ -114,7 +135,9 @@ extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, fl float point_north_4 = DL_MISSION_PATH_point_north_4(dl_buffer); \ float point_east_5 = DL_MISSION_PATH_point_east_5(dl_buffer); \ float point_north_5 = DL_MISSION_PATH_point_north_5(dl_buffer); \ - mission_msg_PATH(point_east_1, point_north_1, point_east_2, point_north_2, point_east_3, point_north_3, point_east_4, point_north_4, point_east_5, point_north_5); \ + float path_alt = DL_MISSION_PATH_path_alt(dl_buffer); \ + float duration = DL_MISSION_PATH_duration(dl_buffer); \ + mission_msg_PATH(point_east_1, point_north_1, point_east_2, point_north_2, point_east_3, point_north_3, point_east_4, point_north_4, point_east_5, point_north_5, path_alt, duration); \ } \ } @@ -126,18 +149,28 @@ extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, fl float segment_north_1 = DL_MISSION_SEGMENT_segment_north_1(dl_buffer); \ float segment_east_2 = DL_MISSION_SEGMENT_segment_east_2(dl_buffer); \ float segment_north_2 = DL_MISSION_SEGMENT_segment_north_2(dl_buffer); \ - mission_msg_SEGMENT(segment_east_1, segment_north_1, segment_east_2, segment_north_2); \ + float segment_alt = DL_MISSION_SEGMENT_segment_alt(dl_buffer); \ + float duration = DL_MISSION_SEGMENT_duration(dl_buffer); \ + mission_msg_SEGMENT(segment_east_1, segment_north_1, segment_east_2, segment_north_2, segment_alt, duration); \ } \ } - -/* -#define ParseMissionGotoWp() { \ - uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \ - float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \ - float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \ - fly_to_xy(wp_east, wp_north); \ +#define ParseMissionCircle() { \ + if (DL_MISSION_CIRCLE_ac_id(dl_buffer) == AC_ID) { \ + uint8_t ac_id = DL_MISSION_CIRCLE_ac_id(dl_buffer); \ + float center_east = DL_MISSION_CIRCLE_center_east(dl_buffer); \ + float center_north = DL_MISSION_CIRCLE_center_north(dl_buffer); \ + float radius = DL_MISSION_CIRCLE_radius(dl_buffer); \ + float center_alt = DL_MISSION_CIRCLE_center_alt(dl_buffer); \ + float duration = DL_MISSION_CIRCLE_duration(dl_buffer); \ + mission_msg_CIRCLE(center_east, center_north, radius, center_alt, duration); \ + } \ +} + +#define ParseGotoMission() { \ + uint8_t ac_id = DL_GOTO_MISSION_ac_id(dl_buffer); \ + uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer); \ + goto_mission_msg(ac_id, mission_id); \ } -*/ #endif // MISSION From a22c015801287d7bd4a00d55cf65f5834e2dd68f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 Sep 2013 10:29:26 +0200 Subject: [PATCH 089/125] [mission] add missing files --- sw/airborne/modules/mission/mission_nav.c | 180 ++++++++++++++++++++++ sw/airborne/modules/mission/mission_nav.h | 49 ++++++ 2 files changed, 229 insertions(+) create mode 100644 sw/airborne/modules/mission/mission_nav.c create mode 100644 sw/airborne/modules/mission/mission_nav.h diff --git a/sw/airborne/modules/mission/mission_nav.c b/sw/airborne/modules/mission/mission_nav.c new file mode 100644 index 0000000000..419a8fe1b5 --- /dev/null +++ b/sw/airborne/modules/mission/mission_nav.c @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2013 ENAC + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file mission_nav.h + * @brief mission navigation + */ + +#include +#include "modules/mission/mission_nav.h" +//#include "std.h" +//#include "subsystems/nav.h" + + +int mission_nav_GOTO_WP(float x, float y) { + fly_to_xy(x, y); + return FALSE; +} + +//int mission_nav_SEGMENT(float x1, float y1, float x2, float y2) { +bool_t mission_nav_SEGMENT(float x1, float y1, float x2, float y2) { + + if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { + //mission_nav_finish = 1; + return FALSE; // end of block and go to the next block + } + else { + nav_route_xy(x1, y1, x2, y2); + printf("doing mission SEGMENT ...\n\n");fflush(stdout); + } + return TRUE; +} + +//int mission_nav_CIRCLE(float x, float y, float radius){ +bool_t mission_nav_CIRCLE(float x, float y, float radius){ + nav_circle_XY(x, y, radius); + printf("doing mission CIRCLE ...\n\n");fflush(stdout); + return TRUE; +} + + + +int mission_path_seg_idx; +mission_path_seg_idx = 1; +//int mission_nav_PATH(struct _mission_path mission_path_nav) { +bool_t mission_nav_PATH(struct _mission_path mission_path_nav) { + float x1, y1, x2, y2, x3, y3, x4, y4, x5, y5; + switch (mission_path_seg_idx) { + case 1: // path 1-2 + x1 = mission_path_nav.path[0].x; + y1 = mission_path_nav.path[0].y; + x2 = mission_path_nav.path[1].x; + y2 = mission_path_nav.path[1].y; + nav_route_xy(x1, y1, x2, y2); + printf("doing mission PATH: path 1-2...\n\n");fflush(stdout); + if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { + mission_path_seg_idx = 2; + } + break; + case 2: // path 2-3 + x2 = mission_path_nav.path[1].x; + y2 = mission_path_nav.path[1].y; + x3 = mission_path_nav.path[2].x; + y3 = mission_path_nav.path[2].y; + nav_route_xy(x2, y2, x3, y3); + printf("doing mission PATH: path 2-3...\n\n");fflush(stdout); + if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) { + mission_path_seg_idx = 3; + } + break; + case 3: // path 3-4 + x3 = mission_path_nav.path[2].x; + y3 = mission_path_nav.path[2].y; + x4 = mission_path_nav.path[3].x; + y4 = mission_path_nav.path[3].y; + nav_route_xy(x3, y3, x4, y4); + printf("doing mission PATH: path 3-4...\n\n");fflush(stdout); + if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) { + mission_path_seg_idx = 4; + } + break; + case 4: // path 4-5 + x4 = mission_path_nav.path[3].x; + y4 = mission_path_nav.path[3].y; + x5 = mission_path_nav.path[4].x; + y5 = mission_path_nav.path[4].y; + nav_route_xy(x4, y4, x5, y5); + printf("doing mission PATH: path 4-5...\n\n");fflush(stdout); + if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) { + //mission_nav_finish = 1; + return FALSE; // This path ends when AC arrive to point 5 + } + break; + } + + return TRUE; // do the function +} + + +bool_t mission_nav_return; +bool_t mission_nav_finish; +int i_mission; +int mission_call(struct _mission mission) { +//void mission_nav() { + //int i_mission; + // detect where the mission list is (by detecting duration non NULL)?????????????????????????????????????????????????????????????????????? + + float wp_mission_x, wp_mission_y; + float wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y; + float wp_center_x, wp_center_y, radius; + struct _mission_path mission_path_nav; + + switch (mission.elements[i_mission].type){ + case MissionWP: + wp_mission_x = mission.elements[i_mission].element.mission_wp.wp.x; + wp_mission_y = mission.elements[i_mission].element.mission_wp.wp.y; + //mission_nav_GOTO_WP(wp_mission_x, wp_mission_y); + mission_nav_GOTO_WP(wp_mission_x, wp_mission_y); + break; + + case MissionSegment: + wp_mission_from_x = mission.elements[i_mission].element.mission_segment.from.x; + wp_mission_from_y = mission.elements[i_mission].element.mission_segment.from.y; + wp_mission_to_x = mission.elements[i_mission].element.mission_segment.to.x; + wp_mission_to_y = mission.elements[i_mission].element.mission_segment.to.y; + //mission_nav_SEGMENT(wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y); + mission_nav_return = mission_nav_SEGMENT(wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y); + break; + + case MissionCircle: + wp_center_x = mission.elements[i_mission].element.mission_circle.center.x; + wp_center_y = mission.elements[i_mission].element.mission_circle.center.y; + radius = mission.elements[i_mission].element.mission_circle.radius; + //mission_nav_CIRCLE(wp_center_x, wp_center_y, radius); + mission_nav_return = mission_nav_CIRCLE(wp_center_x, wp_center_y, radius); + break; + + case MissionPath: + mission_path_nav = mission.elements[i_mission].element.mission_path; + //mission_nav_PATH(mission_path_nav); + mission_nav_return = mission_nav_PATH(mission_path_nav); + break; + } + + if (!(mission_nav_return)){ //when mission.elements is finished + printf("finish mission.elements %d\n\n\n", i_mission);fflush(stdout); + i_mission++; + printf("goto mission.elements %d\n\n\n", i_mission);fflush(stdout); + } + + + if (i_mission == MISSION_ELEMENT_NB) i_mission = 0; + return TRUE; /* do the function */ + +} + + + +int goto_mission_msg(uint8_t ac_id, uint8_t mission_id){ + i_mission = mission_id; + mission_call(mission); +} diff --git a/sw/airborne/modules/mission/mission_nav.h b/sw/airborne/modules/mission/mission_nav.h new file mode 100644 index 0000000000..63509f8bc8 --- /dev/null +++ b/sw/airborne/modules/mission/mission_nav.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2013 ENAC + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file mission_nav.h + * @brief mission navigation + */ + + +#include "modules/mission/mission_msg.h" +#include "std.h" +#include "subsystems/nav.h" + + +extern int mission_nav_GOTO_WP(float x, float y); +//extern int mission_nav_SEGMENT(float x1, float y1, float x2, float y2); +extern bool_t mission_nav_SEGMENT(float x1, float y1, float x2, float y2); + +//extern int mission_path_seg_idx; +//extern int i_seg; + +//extern int mission_nav_PATH_init( void ); +extern bool_t mission_nav_PATH(struct _mission_path mission_path_nav); + +extern bool_t mission_nav_return, mission_nav_finish; +//extern int mission_nav_return; +//extern int mission_nav_finish; +extern int i_mission; +extern int mission_call(struct _mission mission); + +//extern int goto_mission_msg(uint8_t ac_id, uint8_t mission_id); From f9ce4aed2c1d94f7ebafbfc257a38dbe7330e283 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 25 Sep 2013 18:27:02 +0200 Subject: [PATCH 090/125] [math] geodetic_int: add extra ecef <-> enu/ned position functions - ecef_of_enu_point_i has coordinates in cm - ecef_of_enu_pos_i has ned in meter with INT32_POS_FRAC and ecef with cm --- sw/airborne/math/pprz_geodetic_int.c | 74 +++++++++- sw/airborne/math/pprz_geodetic_int.h | 4 + sw/airborne/test/Makefile | 3 +- sw/airborne/test/test_geodetic.c | 195 +++++++++++++++++++-------- 4 files changed, 215 insertions(+), 61 deletions(-) diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index d66e3cfe2a..0562689d24 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -92,11 +92,40 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) { - struct EnuCoor_i enu; enu_of_ecef_point_i(&enu, def, ecef); ENU_OF_TO_NED(*ned, enu); +} + +/** Convert a ECEF position to local ENU. + * @param[out] enu ENU position in meter << #INT32_POS_FRAC + * @param[in] def local coordinate system definition + * @param[in] ecef ECEF position in cm + */ +void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) { + struct EnuCoor_i enu_cm; + enu_of_ecef_point_i(&enu_cm, def, ecef); + + /* enu = (enu_cm / 100) << INT32_POS_FRAC + * to loose less range: + * enu_cm = enu << (INT32_POS_FRAC-2) / 25 + * which puts max enu output Q23.8 range to 8388km / 25 = 335km + */ + INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2); + VECT3_SDIV(*enu, *enu, 25); +} + + +/** Convert a ECEF position to local NED. + * @param[out] ned NED position in meter << #INT32_POS_FRAC + * @param[in] def local coordinate system definition + * @param[in] ecef ECEF position in cm + */ +void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) { + struct EnuCoor_i enu; + enu_of_ecef_pos_i(&enu, def, ecef); + ENU_OF_TO_NED(*ned, enu); } void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) { @@ -149,11 +178,23 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne ecef_of_enu_vect_i(ecef, def, &enu); } + +/** Convert a point in local ENU to ECEF. + * @param[out] ecef ECEF point in cm + * @param[in] def local coordinate system definition + * @param[in] enu ENU point in cm + */ void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { ecef_of_enu_vect_i(ecef, def, enu); INT32_VECT3_ADD(*ecef, def->ecef); } + +/** Convert a point in local NED to ECEF. + * @param[out] ecef ECEF point in cm + * @param[in] def local coordinate system definition + * @param[in] ned NED point in cm + */ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { struct EnuCoor_i enu; ENU_OF_TO_NED(enu, *ned); @@ -161,6 +202,37 @@ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct N } +/** Convert a local ENU position to ECEF. + * @param[out] ecef ECEF position in cm + * @param[in] def local coordinate system definition + * @param[in] enu ENU position in meter << #INT32_POS_FRAC + */ +void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { + /* enu_cm = (enu * 100) >> INT32_POS_FRAC + * to loose less range: + * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2) + * which puts max enu input Q23.8 range to 8388km / 25 = 335km + */ + struct EnuCoor_i enu_cm; + VECT3_SMUL(enu_cm, *enu, 25); + INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2); + ecef_of_enu_vect_i(ecef, def, &enu_cm); + INT32_VECT3_ADD(*ecef, def->ecef); +} + + +/** Convert a local NED position to ECEF. + * @param[out] ecef ECEF position in cm + * @param[in] def local coordinate system definition + * @param[in] ned NED position in meter << #INT32_POS_FRAC + */ +void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { + struct EnuCoor_i enu; + ENU_OF_TO_NED(enu, *ned); + ecef_of_enu_pos_i(ecef, def, &enu); +} + + void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef,lla); diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index f17ff2dc5a..cbeac4ac5f 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -104,6 +104,8 @@ extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in); extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in); extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); +extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); +extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla); @@ -112,6 +114,8 @@ extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struc extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla); extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); +extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); +extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); diff --git a/sw/airborne/test/Makefile b/sw/airborne/test/Makefile index f8e05d1151..5938ed851a 100644 --- a/sw/airborne/test/Makefile +++ b/sw/airborne/test/Makefile @@ -4,7 +4,8 @@ Q=@ CC = gcc -CFLAGS = -std=c99 -I.. -I../../include -I../booz -I../../booz -Wall +CFLAGS = -std=c99 -I.. -I../../include -Wall +#CFLAGS += -DDEBUG LDFLAGS = -lm diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c index e4c24a983e..51197828f5 100644 --- a/sw/airborne/test/test_geodetic.c +++ b/sw/airborne/test/test_geodetic.c @@ -20,6 +20,7 @@ static void test_lla_of_utm(void); static void test_floats(void); static void test_doubles(void); static void test_enu_of_ecef_int(void); +static void test_ecef_of_ned_int(void); static void test_ned_to_ecef_to_ned(void); static void test_enu_to_ecef_to_enu( void ); @@ -32,12 +33,16 @@ int main(int argc, char** argv) { //test_floats(); //test_doubles(); - test_lla_of_utm(); + //test_lla_of_utm(); + + //test_ned_to_ecef_to_ned(); + + //test_enu_to_ecef_to_enu(); + + test_ecef_of_ned_int(); //test_enu_of_ecef_int(); - // test_ned_to_ecef_to_ned(); - // test_enu_to_ecef_to_enu(); return 0; } @@ -48,7 +53,7 @@ static void test_lla_of_utm(void) { struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 }; struct LlaCoor_d lla_d; struct LlaCoor_d l_ref_d = {.lat=0.749999999392454875, - .lon=0.019999999054505127}; + .lon=0.019999999054505127}; lla_of_utm_d(&lla_d, &utm_d); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon); @@ -58,7 +63,7 @@ static void test_lla_of_utm(void) { struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 }; struct LlaCoor_f lla_f; struct LlaCoor_f l_ref_f = {.lat=0.749999999392454875, - .lon=0.019999999054505127}; + .lon=0.019999999054505127}; lla_of_utm_f(&lla_f, &utm_f); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon); @@ -78,15 +83,16 @@ static void test_floats(void) { struct LtpDef_f ltp_def; ltp_def_from_ecef_f(<p_def, &ref_coor); - printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt); + printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), + ltp_def.lla.alt); struct EcefCoor_f my_ecef_point = ref_coor; struct EnuCoor_f my_enu_point; enu_of_ecef_point_f(&my_enu_point, <p_def, &my_ecef_point); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, - my_enu_point.x, my_enu_point.y, my_enu_point.z ); + my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, + my_enu_point.x, my_enu_point.y, my_enu_point.z ); printf("\n"); } @@ -105,18 +111,91 @@ static void test_doubles(void) { struct LtpDef_d ltp_def; ltp_def_from_ecef_d(<p_def, &ref_coor); - printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt); + printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), + ltp_def.lla.alt); struct EcefCoor_d my_ecef_point = ref_coor; struct EnuCoor_d my_enu_point; enu_of_ecef_point_d(&my_enu_point, <p_def, &my_ecef_point); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, - my_enu_point.x, my_enu_point.y, my_enu_point.z ); + my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, + my_enu_point.x, my_enu_point.y, my_enu_point.z ); printf("\n"); } +static void test_ecef_of_ned_int(void) { + + printf("\n--- ecef_of_ned int ---\n"); + struct EcefCoor_d ref_coor_d = { 4624497.0 , 116475.0, 4376563.0}; + struct LtpDef_d ltp_def_d; + ltp_def_from_ecef_d(<p_def_d, &ref_coor_d); + + struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_d.x)), + rint(CM_OF_M(ref_coor_d.y)), + rint(CM_OF_M(ref_coor_d.z))}; + printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); + struct LtpDef_i ltp_def_i; + ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); + + +#define STEP_NED 100.0 +#define RANGE_NED 10000. + double sum_err = 0; + struct FloatVect3 max_err; + FLOAT_VECT3_ZERO(max_err); + struct FloatVect3 offset; + for (offset.x=-RANGE_NED; offset.x<=RANGE_NED; offset.x+=STEP_NED) { + for (offset.y=-RANGE_NED; offset.y<=RANGE_NED; offset.y+=STEP_NED) { + for (offset.z=-RANGE_NED; offset.z<=RANGE_NED; offset.z+=STEP_NED) { + struct NedCoor_d my_ned_point_d; + VECT3_COPY(my_ned_point_d, offset); + struct EcefCoor_d my_ecef_point_d; + ecef_of_ned_point_d(&my_ecef_point_d, <p_def_d, &my_ned_point_d); +#if DEBUG + printf("ned to ecef double : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", + my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z, + my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z); +#endif + + struct NedCoor_i my_ned_point_i; + POSITIONS_BFP_OF_REAL(my_ned_point_i, my_ned_point_d); + struct EcefCoor_i my_ecef_point_i; + ecef_of_ned_pos_i(&my_ecef_point_i, <p_def_i, &my_ned_point_i); + +#if DEBUG + //printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); + printf("ned to ecef int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", + POS_BFP_OF_REAL(my_ned_point_i.x), + POS_BFP_OF_REAL(my_ned_point_i.y), + POS_BFP_OF_REAL(my_ned_point_i.z), + M_OF_CM((double)my_ecef_point_i.x), + M_OF_CM((double)my_ecef_point_i.y), + M_OF_CM((double)my_ecef_point_i.z)); +#endif + + float ex = my_ecef_point_d.x - M_OF_CM((double)my_ecef_point_i.x); + if (fabs(ex) > max_err.x) max_err.x = fabs(ex); + float ey = my_ecef_point_d.y - M_OF_CM((double)my_ecef_point_i.y); + if (fabs(ey) > max_err.y) max_err.y = fabs(ey); + float ez = my_ecef_point_d.z - M_OF_CM((double)my_ecef_point_i.z); + if (fabs(ez) > max_err.z) max_err.z = fabs(ez); + sum_err += ex*ex + ey*ey + ez*ez; + } + } + } + + double nb_samples = (2*RANGE_NED / STEP_NED + 1) * (2*RANGE_NED / STEP_NED + 1) * + (2*RANGE_NED / STEP_NED + 1); + + + printf("ecef_of_ned_pos int/float comparison:\n"); + printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z ); + printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); + printf("\n"); + +} + static void test_enu_of_ecef_int(void) { @@ -126,15 +205,16 @@ static void test_enu_of_ecef_int(void) { ltp_def_from_ecef_f(<p_def_f, &ref_coor_f); struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)), - rint(CM_OF_M(ref_coor_f.y)), - rint(CM_OF_M(ref_coor_f.z))}; + rint(CM_OF_M(ref_coor_f.y)), + rint(CM_OF_M(ref_coor_f.z))}; printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); struct LtpDef_i ltp_def_i; ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); - printf("lla0 : (%f deg, %f deg, %f m) (%f,%f,%f)\n", DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, - DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), - DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), - M_OF_MM((double)ltp_def_i.lla.alt)); + printf("lla0 : float (%f deg, %f deg, %f m) , int (%f, %f, %f)\n", + DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, + DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), + DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), + M_OF_MM((double)ltp_def_i.lla.alt)); #define STEP 1000. #define RANGE 100000. @@ -145,40 +225,40 @@ static void test_enu_of_ecef_int(void) { for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) { for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) { for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) { - struct EcefCoor_f my_ecef_point_f = ref_coor_f; - VECT3_ADD(my_ecef_point_f, offset); - struct EnuCoor_f my_enu_point_f; - enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); + struct EcefCoor_f my_ecef_point_f = ref_coor_f; + VECT3_ADD(my_ecef_point_f, offset); + struct EnuCoor_f my_enu_point_f; + enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); #if DEBUG - printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", - my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, - my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); + printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", + my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, + my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); #endif - struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), - rint(CM_OF_M(my_ecef_point_f.y)), - rint(CM_OF_M(my_ecef_point_f.z))};; - struct EnuCoor_i my_enu_point_i; - enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); + struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), + rint(CM_OF_M(my_ecef_point_f.y)), + rint(CM_OF_M(my_ecef_point_f.z))};; + struct EnuCoor_i my_enu_point_i; + enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); #if DEBUG - // printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); - printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", - M_OF_CM((double)my_ecef_point_i.x), - M_OF_CM((double)my_ecef_point_i.y), - M_OF_CM((double)my_ecef_point_i.z), - M_OF_CM((double)my_enu_point_i.x), - M_OF_CM((double)my_enu_point_i.y), - M_OF_CM((double)my_enu_point_i.z)); + //printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); + printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", + M_OF_CM((double)my_ecef_point_i.x), + M_OF_CM((double)my_ecef_point_i.y), + M_OF_CM((double)my_ecef_point_i.z), + M_OF_CM((double)my_enu_point_i.x), + M_OF_CM((double)my_enu_point_i.y), + M_OF_CM((double)my_enu_point_i.z)); #endif - float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); - if (fabs(ex) > max_err.x) max_err.x = fabs(ex); - float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); - if (fabs(ey) > max_err.y) max_err.y = fabs(ey); - float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); - if (fabs(ez) > max_err.z) max_err.z = fabs(ez); - sum_err += ex*ex + ey*ey + ez*ez; + float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); + if (fabs(ex) > max_err.x) max_err.x = fabs(ex); + float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); + if (fabs(ey) > max_err.y) max_err.y = fabs(ey); + float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); + if (fabs(ez) > max_err.z) max_err.z = fabs(ez); + sum_err += ex*ex + ey*ey + ez*ez; } } } @@ -191,7 +271,6 @@ static void test_enu_of_ecef_int(void) { printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); printf("\n"); - } @@ -200,7 +279,8 @@ static void test_enu_of_ecef_int(void) { void test_ned_to_ecef_to_ned( void ) { -#if 0 + printf("\n--- NED -> ECEF -> NED in double ---\n"); + struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0}; printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z); @@ -211,18 +291,16 @@ void test_ned_to_ecef_to_ned( void ) { struct NedCoor_d ned_p1; ned_of_ecef_point_d(&ned_p1, <p_def, &ecef_p1); printf("ecef to ned : (%f,%f,%f) -> (%f,%f,%f)\n", - ecef_p1.x, ecef_p1.y, ecef_p1.z, - ned_p1.x, ned_p1.y, ned_p1.z ); + ecef_p1.x, ecef_p1.y, ecef_p1.z, + ned_p1.x, ned_p1.y, ned_p1.z ); struct EcefCoor_d ecef_p2; ecef_of_ned_point_d(&ecef_p2, <p_def, &ned_p1); printf("ned to ecef : (%f,%f,%f) -> (%f,%f,%f)\n", - ned_p1.x, ned_p1.y, ned_p1.z, - ecef_p2.x, ecef_p2.y, ecef_p2.z); + ned_p1.x, ned_p1.y, ned_p1.z, + ecef_p2.x, ecef_p2.y, ecef_p2.z); printf("\n"); -#endif - } @@ -233,6 +311,8 @@ void test_ned_to_ecef_to_ned( void ) { void test_enu_to_ecef_to_enu( void ) { + printf("\n--- ENU -> ECEF -> ENU in float ---\n"); + struct EcefCoor_f ref_coor = { 4624497.0 , 116475.0, 4376563.0}; printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z); @@ -243,19 +323,16 @@ void test_enu_to_ecef_to_enu( void ) { struct EnuCoor_f enu_p1; enu_of_ecef_point_f(&enu_p1, <p_def, &ecef_p1); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - ecef_p1.x, ecef_p1.y, ecef_p1.z, - enu_p1.x, enu_p1.y, enu_p1.z ); + ecef_p1.x, ecef_p1.y, ecef_p1.z, + enu_p1.x, enu_p1.y, enu_p1.z ); -#if 0 struct EcefCoor_f ecef_p2; ecef_of_enu_point_f(&ecef_p2, <p_def, &enu_p1); printf("enu to ecef : (%f,%f,%f) -> (%f,%f,%f)\n", - enu_p1.x, enu_p1.y, enu_p1.z, - ecef_p2.x, ecef_p2.y, ecef_p2.z); + enu_p1.x, enu_p1.y, enu_p1.z, + ecef_p2.x, ecef_p2.y, ecef_p2.z); printf("\n"); -#endif - } From e84e292f3e874ffbd6fa62a6be5c9a3540d49e90 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 25 Sep 2013 18:31:14 +0200 Subject: [PATCH 091/125] [state] fix local ned/enu to ecef conversion by using new ecef_of_enu_pos_i functions which have enu in meter with INT32_POS_FRAC and ecef with cm instead of everything in cm --- sw/airborne/state.c | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 0b92f2fe3a..f72bc48438 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -69,8 +69,7 @@ void stateCalcPositionEcef_i(void) { ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { - /// @todo check if resolution is good enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f to ecef_f, set status bit, then convert to int */ @@ -114,7 +113,7 @@ void stateCalcPositionNed_i(void) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ @@ -206,7 +205,7 @@ void stateCalcPositionEnu_i(void) { INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> enu_f, set status bit, then convert to int */ @@ -306,8 +305,7 @@ void stateCalcPositionLla_i(void) { } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ - /// @todo check if resolution is enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ } @@ -360,8 +358,7 @@ void stateCalcPositionEcef_f(void) { } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> ecef_f, set status bits */ - /// @todo check if resolution is good enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_F); ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); } @@ -396,7 +393,7 @@ void stateCalcPositionNed_f(void) { } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> ned_i -> ned_f, set status bits */ - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); } @@ -407,7 +404,7 @@ void stateCalcPositionNed_f(void) { /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); } @@ -481,7 +478,7 @@ void stateCalcPositionEnu_f(void) { } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> enu_i -> enu_f, set status bits */ - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } @@ -492,7 +489,7 @@ void stateCalcPositionEnu_f(void) { /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } From 9add5d1c316b251f064197ee65ca7c7ddb928f47 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 26 Sep 2013 09:05:01 +0200 Subject: [PATCH 092/125] [Ardrone][Raw] More general airframe --- conf/airframes/ardrone2_raw.xml | 70 +++++++++++++++++---------------- 1 file changed, 36 insertions(+), 34 deletions(-) diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 99265a1638..98ec28a287 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -5,7 +5,6 @@ - @@ -20,15 +19,14 @@ + - - @@ -39,7 +37,7 @@ - + @@ -55,11 +53,12 @@ + - - - - + + + +
@@ -71,36 +70,42 @@
- - - + + + + - - - - - - - - - + + + + + + + + + + + +
- - +
- -
- +
- + + + + + +
@@ -132,7 +137,6 @@
-
@@ -186,19 +190,18 @@ - - - -
+ + + - +
@@ -221,5 +224,4 @@
- From 45ff6dbc1167b4f0d35d66d0bd3b25ec18dc4434 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 26 Sep 2013 09:13:16 +0200 Subject: [PATCH 093/125] [rotorcraft] guidance_h MAX_BANK In case of wind, the -20 to 20 max bank should become -40 deg in the wind direction and +0 in the opposite direction. The i-gain should be sufficiently SLOW but with HIGH authority. This commit suggests to split the max-bank from PD an I with separate saturation and have the I-gain before the integrator so better insight is gained in its authority. --- .../firmwares/rotorcraft/guidance/guidance_h.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index ec09756d55..aca6673794 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -367,9 +367,11 @@ static void guidance_h_traj_run(bool_t in_flight) { /* update pos error integral, zero it if not in_flight */ if (in_flight) { - VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); + guidance_h_pos_err_sum.x += ((guidance_h_igain * guidance_h_pos_err.x) >> 4); + guidance_h_pos_err_sum.y += ((guidance_h_igain * guidance_h_pos_err.y) >> 4); + // VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); /* saturate it */ - VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); + VECT2_STRIM(guidance_h_pos_err_sum, -traj_max_bank , traj_max_bank); } else { INT_VECT2_ZERO(guidance_h_pos_err_sum); } @@ -378,12 +380,10 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_cmd_earth.x = ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_igain * (guidance_h_pos_err_sum.x >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */ guidance_h_cmd_earth.y = ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ /* compute a better approximation of force commands by taking thrust into account */ @@ -395,6 +395,9 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); } + guidance_h_cmd_earth.x += guidance_h_pos_err_sum.x; + guidance_h_cmd_earth.y += guidance_h_pos_err_sum.y; + VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); } From cad1ca2ad271c3d21feb5f171c7504003b51a3c1 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 26 Sep 2013 09:19:44 +0200 Subject: [PATCH 094/125] [rotorcraft] guidance_h: Rename sum err for clarity --- .../rotorcraft/guidance/guidance_h.c | 20 +++++++++---------- .../rotorcraft/guidance/guidance_h.h | 4 ++-- sw/airborne/firmwares/rotorcraft/telemetry.h | 4 ++-- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index aca6673794..b3c34ec1e4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -76,7 +76,7 @@ struct Int32Vect2 guidance_h_speed_sp; #endif struct Int32Vect2 guidance_h_pos_err; struct Int32Vect2 guidance_h_speed_err; -struct Int32Vect2 guidance_h_pos_err_sum; +struct Int32Vect2 guidance_h_trim_att_integrator; struct Int32Vect2 guidance_h_nav_err; struct Int32Vect2 guidance_h_cmd_earth; @@ -107,7 +107,7 @@ void guidance_h_init(void) { guidance_h_approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST; INT_VECT2_ZERO(guidance_h_pos_sp); - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); INT_EULERS_ZERO(guidance_h_rc_sp); guidance_h_heading_sp = 0; guidance_h_pgain = GUIDANCE_H_PGAIN; @@ -125,7 +125,7 @@ static inline void reset_guidance_reference_from_current_position(void) { INT_VECT2_ZERO(guidance_h_accel_ref); gh_set_ref(guidance_h_pos_ref, guidance_h_speed_ref, guidance_h_accel_ref); - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); } void guidance_h_mode_changed(uint8_t new_mode) { @@ -367,13 +367,13 @@ static void guidance_h_traj_run(bool_t in_flight) { /* update pos error integral, zero it if not in_flight */ if (in_flight) { - guidance_h_pos_err_sum.x += ((guidance_h_igain * guidance_h_pos_err.x) >> 4); - guidance_h_pos_err_sum.y += ((guidance_h_igain * guidance_h_pos_err.y) >> 4); - // VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); + guidance_h_trim_att_integrator.x += ((guidance_h_igain * guidance_h_pos_err.x) >> 4); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * guidance_h_pos_err.y) >> 4); + // VECT2_ADD(guidance_h_trim_att_integrator, guidance_h_pos_err); /* saturate it */ - VECT2_STRIM(guidance_h_pos_err_sum, -traj_max_bank , traj_max_bank); + VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); } else { - INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_trim_att_integrator); } /* run PID */ @@ -395,8 +395,8 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); } - guidance_h_cmd_earth.x += guidance_h_pos_err_sum.x; - guidance_h_cmd_earth.y += guidance_h_pos_err_sum.y; + guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x; + guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y; VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index e8ba961fc1..f8a934ce43 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -67,7 +67,7 @@ extern struct Int32Vect2 guidance_h_accel_ref; ///< with #INT32_ACCEL_FRAC extern struct Int32Vect2 guidance_h_pos_err; extern struct Int32Vect2 guidance_h_speed_err; -extern struct Int32Vect2 guidance_h_pos_err_sum; +extern struct Int32Vect2 guidance_h_trim_att_integrator; extern struct Int32Vect2 guidance_h_nav_err; @@ -95,7 +95,7 @@ extern void guidance_h_run(bool_t in_flight); #define guidance_h_SetKi(_val) { \ guidance_h_igain = _val; \ - INT_VECT2_ZERO(guidance_h_pos_err_sum); \ + INT_VECT2_ZERO(guidance_h_trim_att_integrator); \ } /* Make sure that ref can only be temporarily disabled for testing, diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 04bf83cf7a..412421cd60 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -660,8 +660,8 @@ &guidance_h_pos_err.y, \ &guidance_h_speed_err.x, \ &guidance_h_speed_err.y, \ - &guidance_h_pos_err_sum.x, \ - &guidance_h_pos_err_sum.y, \ + &guidance_h_trim_att_integrator.x, \ + &guidance_h_trim_att_integrator.y, \ &guidance_h_nav_err.x, \ &guidance_h_nav_err.y, \ &guidance_h_cmd_earth.x, \ From 8a3b8b4991a928510d9941d7d6eb85ed9e529789 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 26 Sep 2013 09:28:20 +0200 Subject: [PATCH 095/125] [rotorcraft] guidance_h: Updated integrator dynamics Integrate twice as fast when not only POS but also SPEED are wrong, but do not integrate POS errors when the SPEED is already catching up. --- .../rotorcraft/guidance/guidance_h.c | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index b3c34ec1e4..301a62acf8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -365,17 +365,6 @@ static void guidance_h_traj_run(bool_t in_flight) { /* saturate it */ VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); - /* update pos error integral, zero it if not in_flight */ - if (in_flight) { - guidance_h_trim_att_integrator.x += ((guidance_h_igain * guidance_h_pos_err.x) >> 4); - guidance_h_trim_att_integrator.y += ((guidance_h_igain * guidance_h_pos_err.y) >> 4); - // VECT2_ADD(guidance_h_trim_att_integrator, guidance_h_pos_err); - /* saturate it */ - VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); - } else { - INT_VECT2_ZERO(guidance_h_trim_att_integrator); - } - /* run PID */ guidance_h_cmd_earth.x = ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + @@ -395,6 +384,16 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); } + /* update pos & speed error integral, zero it if not in_flight */ + if (in_flight) { + guidance_h_trim_att_integrator.x += ((guidance_h_igain * guidance_h_cmd_earth.x) >> 10); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * guidance_h_cmd_earth.y) >> 10); + /* saturate it */ + VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); + } else { + INT_VECT2_ZERO(guidance_h_trim_att_integrator); + } + guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x; guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y; From de12d2f4bbd0153cf575a41e696a672e99049cf3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 26 Sep 2013 11:41:40 +0200 Subject: [PATCH 096/125] [rotorcraft] guidance_h total_max_bank --- .../rotorcraft/guidance/guidance_h.c | 36 +++++++++++-------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 301a62acf8..18bed49d72 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -340,7 +340,6 @@ static void guidance_h_update_reference(void) { #define MAX_POS_ERR POS_BFP_OF_REAL(16.) #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.) -#define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12) #ifndef GUIDANCE_H_THRUST_CMD_FILTER #define GUIDANCE_H_THRUST_CMD_FILTER 10 @@ -354,6 +353,7 @@ static void guidance_h_traj_run(bool_t in_flight) { /* maximum bank angle: default 20 deg, max 40 deg*/ static const int32_t traj_max_bank = Max(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC), BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC)); + static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC); /* compute position error */ VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i()); @@ -375,6 +375,25 @@ static void guidance_h_traj_run(bool_t in_flight) { ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ + /* trim max bank angle from PD */ + VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); + + /* Update pos & speed error integral, zero it if not in_flight. + * Integrate twice as fast when not only POS but also SPEED are wrong, + * but do not integrate POS errors when the SPEED is already catching up. + */ + if (in_flight) { + guidance_h_trim_att_integrator.x += ((guidance_h_igain * guidance_h_cmd_earth.x) >> 10); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * guidance_h_cmd_earth.y) >> 10); + /* saturate it */ + VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); + /* add it to the command */ + guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x; + guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y; + } else { + INT_VECT2_ZERO(guidance_h_trim_att_integrator); + } + /* compute a better approximation of force commands by taking thrust into account */ if (guidance_h_approx_force_by_thrust && in_flight) { static int32_t thrust_cmd_filt; @@ -384,20 +403,7 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); } - /* update pos & speed error integral, zero it if not in_flight */ - if (in_flight) { - guidance_h_trim_att_integrator.x += ((guidance_h_igain * guidance_h_cmd_earth.x) >> 10); - guidance_h_trim_att_integrator.y += ((guidance_h_igain * guidance_h_cmd_earth.y) >> 10); - /* saturate it */ - VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); - } else { - INT_VECT2_ZERO(guidance_h_trim_att_integrator); - } - - guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x; - guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y; - - VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); + VECT2_STRIM(guidance_h_cmd_earth, total_max_bank, total_max_bank); } static void guidance_h_hover_enter(void) { From b6c2a8fef44a8405abab8ba74f35bce43cf3923a Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 27 Sep 2013 08:56:08 +0200 Subject: [PATCH 097/125] [rotorcraft] guidance_h Resolution of integrator increased, AGain not in integration. --- .../rotorcraft/guidance/guidance_h.c | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 18bed49d72..2d184a09f4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -366,14 +366,16 @@ static void guidance_h_traj_run(bool_t in_flight) { VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); /* run PID */ - guidance_h_cmd_earth.x = - ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */ - guidance_h_cmd_earth.y = - ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) + - ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */ + int32_t pd_x = + ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + int32_t pd_y = + ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + guidance_h_cmd_earth.x = pd_x + + ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* acceleration feedforward gain */ + guidance_h_cmd_earth.y = pd_y + + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* acceleration feedforward gain */ /* trim max bank angle from PD */ VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank); @@ -383,8 +385,9 @@ static void guidance_h_traj_run(bool_t in_flight) { * but do not integrate POS errors when the SPEED is already catching up. */ if (in_flight) { - guidance_h_trim_att_integrator.x += ((guidance_h_igain * guidance_h_cmd_earth.x) >> 10); - guidance_h_trim_att_integrator.y += ((guidance_h_igain * guidance_h_cmd_earth.y) >> 10); + /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (10) -> ANGLE_FRAX (12) */ + guidance_h_trim_att_integrator.x += ((guidance_h_igain * (pd_x >> 6)) >> 12); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * (pd_y >> 6)) >> 12); /* saturate it */ VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); /* add it to the command */ From 90778612108538a099a2f1652a48438e05dc504e Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 27 Sep 2013 13:25:38 +0200 Subject: [PATCH 098/125] [rotorcraft] guidance_h typo in saturation and better resolution of the integrator --- .../rotorcraft/guidance/guidance_h.c | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 2d184a09f4..4f48097d9f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -367,14 +367,16 @@ static void guidance_h_traj_run(bool_t in_flight) { /* run PID */ int32_t pd_x = - ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); int32_t pd_y = - ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + - ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); - guidance_h_cmd_earth.x = pd_x + + ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) + + ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)); + guidance_h_cmd_earth.x = + pd_x + ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* acceleration feedforward gain */ - guidance_h_cmd_earth.y = pd_y + + guidance_h_cmd_earth.y = + pd_y + ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* acceleration feedforward gain */ /* trim max bank angle from PD */ @@ -385,14 +387,14 @@ static void guidance_h_traj_run(bool_t in_flight) { * but do not integrate POS errors when the SPEED is already catching up. */ if (in_flight) { - /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (10) -> ANGLE_FRAX (12) */ - guidance_h_trim_att_integrator.x += ((guidance_h_igain * (pd_x >> 6)) >> 12); - guidance_h_trim_att_integrator.y += ((guidance_h_igain * (pd_y >> 6)) >> 12); + /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> ANGLE_FRAX (12) */ + guidance_h_trim_att_integrator.x += ((guidance_h_igain * pd_x) >> (8)); + guidance_h_trim_att_integrator.y += ((guidance_h_igain * pd_y) >> (8)); /* saturate it */ - VECT2_STRIM(guidance_h_trim_att_integrator, -traj_max_bank , traj_max_bank); + VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 12) , (traj_max_bank << 12)); /* add it to the command */ - guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x; - guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y; + guidance_h_cmd_earth.x += guidance_h_trim_att_integrator.x >> 12; + guidance_h_cmd_earth.y += guidance_h_trim_att_integrator.y >> 12; } else { INT_VECT2_ZERO(guidance_h_trim_att_integrator); } @@ -406,7 +408,7 @@ static void guidance_h_traj_run(bool_t in_flight) { guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); } - VECT2_STRIM(guidance_h_cmd_earth, total_max_bank, total_max_bank); + VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank); } static void guidance_h_hover_enter(void) { From d3ac39c6fc1414cf1dbb11b392a4e2fd4d53d4cf Mon Sep 17 00:00:00 2001 From: fvantienen Date: Fri, 27 Sep 2013 23:53:20 +0200 Subject: [PATCH 099/125] [generator] Fix special characters in flight plan and settings --- sw/tools/gen_flight_plan.ml | 2 +- sw/tools/gen_settings.ml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 972704966b..62449a3be6 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -800,7 +800,7 @@ let () = List.iter (fun s -> let v = ExtXml.attrib s "name" in - lprintf "#define BLOCK_%s %d\n" (Str.global_replace (Str.regexp "[\\. ]") "_" v) !idx; incr idx) blocks; + lprintf "#define BLOCK_%s %d\n" (Str.global_replace (Str.regexp "[^A-Za-z0-9]") "_" v) !idx; incr idx) blocks; lprintf "\n"; let index_of_waypoints = diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 087d477f22..2413805309 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -74,7 +74,7 @@ let print_dl_settings = fun settings -> List.iter (fun s -> let v = ExtXml.attrib s "var" in - lprintf "#define SETTINGS_%s %d\n" (Str.global_replace (Str.regexp "\\.") "_" v) !idx; incr idx) + lprintf "#define SETTINGS_%s %d\n" (Str.global_replace (Str.regexp "[^A-Za-z0-9]") "_" v) !idx; incr idx) settings; lprintf "\n"; From df7e47e9a2f361c12e9517fa158e80c7b0f08bff Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 26 Sep 2013 23:36:23 +0200 Subject: [PATCH 100/125] [rotorcraft] stabilization attitude ref include cleanup --- .../stabilization_attitude_quat_float.h | 7 ------- .../stabilization_attitude_ref_euler_float.c | 3 ++- .../stabilization_attitude_ref_euler_float.h | 3 --- .../stabilization_attitude_ref_euler_int.c | 3 ++- .../stabilization_attitude_ref_euler_int.h | 2 -- .../stabilization_attitude_ref_float.h | 4 +--- .../stabilization/stabilization_attitude_ref_int.h | 2 -- .../stabilization_attitude_ref_quat_float.c | 5 ++--- .../stabilization_attitude_ref_quat_float.h | 13 +++++++------ .../stabilization_attitude_ref_quat_int.c | 5 ++--- .../stabilization_attitude_ref_quat_int.h | 1 - ..._ref.h => stabilization_attitude_ref_saturate.h} | 10 +++++----- 12 files changed, 21 insertions(+), 37 deletions(-) rename sw/airborne/firmwares/rotorcraft/stabilization/{stabilization_attitude_ref.h => stabilization_attitude_ref_saturate.h} (92%) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h index 9077bbd8cb..8655fc23ac 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.h @@ -31,13 +31,6 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_float.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" -#ifndef STABILIZATION_ATTITUDE_GAIN_NB -#define STABILIZATION_ATTITUDE_GAIN_NB 1 -#endif - -#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT -#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 -#endif extern struct FloatAttitudeGains stabilization_gains[]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index a70e780560..04e3e60b10 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -1,4 +1,5 @@ -#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #include "generated/airframe.h" struct FloatEulers stab_att_sp_euler; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 762e66ed96..ba55615872 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -22,10 +22,7 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_EULER_FLOAT_H -#include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" -#include "stabilization_attitude_ref.h" void stabilization_attitude_ref_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index d7e8566a8b..14472fbb03 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -24,7 +24,8 @@ * */ -#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #include "generated/airframe.h" struct Int32Eulers stab_att_sp_euler; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index bf3aacf19c..08579a412a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -28,7 +28,5 @@ #define STABILIZATION_ATTITUDE_REF_EULER_INT_H #include "stabilization_attitude_ref_int.h" -#include "stabilization_attitude_ref.h" - #endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h index 2faf9e0bc9..fb62bcc457 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h @@ -27,9 +27,7 @@ #ifndef STABILIZATION_ATTITUDE_REF_FLOAT_H #define STABILIZATION_ATTITUDE_REF_FLOAT_H -#include "generated/airframe.h" - -#include "state.h" +#include "math/pprz_algebra_float.h" extern struct FloatEulers stab_att_sp_euler; extern struct FloatQuat stab_att_sp_quat; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index c0ef65a2b8..24175b5fc2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -29,8 +29,6 @@ #include "math/pprz_algebra_int.h" -#include "state.h" - extern struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index da50880f15..ff15cf95a8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -28,10 +28,9 @@ */ #include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization.h" -#include "state.h" -#include "stabilization_attitude_ref_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index 0424fbf820..b5543ed82b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -30,14 +30,15 @@ #ifndef STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H #define STABILIZATION_ATTITUDE_FLOAT_REF_QUAT_FLOAT_H -#include "subsystems/radio_control.h" -#include "math/pprz_algebra_float.h" - #include "stabilization_attitude_ref_float.h" -#include "stabilization_attitude_ref.h" -#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE)) -#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0) +#ifndef STABILIZATION_ATTITUDE_GAIN_NB +#define STABILIZATION_ATTITUDE_GAIN_NB 1 +#endif + +#ifndef STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT +#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT 0 +#endif void stabilization_attitude_ref_enter(void); void stabilization_attitude_ref_schedule(uint8_t idx); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index b0202847dd..aacd33d198 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -28,9 +28,8 @@ */ #include "generated/airframe.h" -#include "firmwares/rotorcraft/stabilization.h" - -#include "stabilization_attitude_ref_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h" #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h index 3b1cbaa3e8..4fe03f3218 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -31,7 +31,6 @@ #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H #include "stabilization_attitude_ref_int.h" -#include "stabilization_attitude_ref.h" void stabilization_attitude_ref_enter(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h similarity index 92% rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h index 11d41dfb39..5bb4efb06d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h @@ -19,12 +19,12 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h - * Common rotorcraft attitude reference generation include. +/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h + * Common rotorcraft attitude reference saturation include. */ -#ifndef STABILIZATION_ATTITUDE_REF_H -#define STABILIZATION_ATTITUDE_REF_H +#ifndef STABILIZATION_ATTITUDE_REF_SATURATE_H +#define STABILIZATION_ATTITUDE_REF_SATURATE_H #define SATURATE_SPEED_TRIM_ACCEL() { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ @@ -60,4 +60,4 @@ } -#endif /* STABILIZATION_ATTITUDE_REF_H */ +#endif /* STABILIZATION_ATTITUDE_REF_SATURATE_H */ From 50a8ae9634282f9f21c86b7da1ff976727098105 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 28 Sep 2013 20:17:43 +0200 Subject: [PATCH 101/125] [airframes] ardrone_raw: RC_CLIMB defines still needed here Can only be removed once pull request #539 is merged --- conf/airframes/ardrone2_raw.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 98ec28a287..8025779613 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -190,6 +190,10 @@ + + + + From 06c95d8e8f616332507f421d4de399d622caa7a6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 30 Sep 2013 22:45:47 +0200 Subject: [PATCH 102/125] [tools] minor code style updates --- sw/tools/calibration/calibrate.py | 13 ++-- sw/tools/calibration/calibrate_gyro.py | 14 ++-- sw/tools/calibration/calibrate_mag_current.py | 8 +-- sw/tools/calibration/calibration_utils.py | 69 +++++++++---------- 4 files changed, 50 insertions(+), 54 deletions(-) diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index c67aa7df0b..082545f995 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -30,6 +30,7 @@ from scipy import optimize import calibration_utils + def main(): usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options." parser = OptionParser(usage) @@ -58,7 +59,7 @@ def main(): print(args[0] + " not found") sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) - if options.ac_id == None: + if options.ac_id is None: if len(ac_ids) == 1: options.ac_id = ac_ids[0] else: @@ -69,13 +70,13 @@ def main(): if options.sensor == "ACCEL": sensor_ref = 9.81 sensor_res = 10 - noise_window = 20; - noise_threshold = 40; + noise_window = 20 + noise_threshold = 40 elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 - noise_window = 10; - noise_threshold = 1000; + noise_window = 10 + noise_threshold = 1000 if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -88,7 +89,7 @@ def main(): print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!") sys.exit(1) if options.verbose: - print("found "+str(len(measurements))+" records") + print("found "+str(len(measurements))+" records") # estimate the noise threshold # find the median of measurement vector lenght diff --git a/sw/tools/calibration/calibrate_gyro.py b/sw/tools/calibration/calibrate_gyro.py index 7f2970e0fd..77aac9f076 100755 --- a/sw/tools/calibration/calibrate_gyro.py +++ b/sw/tools/calibration/calibrate_gyro.py @@ -87,7 +87,7 @@ def main(): if options.verbose: print("reading file "+filename+" for aircraft "+str(options.ac_id)+" and turntable "+str(options.tt_id)) - samples = calibration_utils.read_turntable_log(options.ac_id, options.tt_id, filename, 1, 7) + samples = calibration_utils.read_turntable_log(options.ac_id, options.tt_id, filename, 1, 7) if len(samples) == 0: print("Error: found zero matching messages in log file!") @@ -106,13 +106,13 @@ def main(): parser.error("Specify a valid axis!") #Linear regression using stats.linregress - t = samples[:, 0] + t = samples[:, 0] xn = samples[:, axis_idx] - (a_s, b_s, r, tt, stderr)=stats.linregress(t, xn) + (a_s, b_s, r, tt, stderr) = stats.linregress(t, xn) print('Linear regression using stats.linregress') print(('regression: a=%.2f b=%.2f, std error= %.3f' % (a_s, b_s, stderr))) - print(('' % (b_s))); - print(('' % (pow(2, 12)/a_s))); + print(('' % (b_s))) + print(('' % (pow(2, 12)/a_s))) # # overlay fited value @@ -125,7 +125,7 @@ def main(): plot(samples[:, 1]) plot(samples[:, 2]) plot(samples[:, 3]) - legend(['p', 'q', 'r']); + legend(['p', 'q', 'r']) subplot(3, 1, 2) plot(samples[:, 0]) @@ -134,7 +134,7 @@ def main(): plot(samples[:, 0], samples[:, axis_idx], 'b.') plot(ovl_omega, ovl_adc, 'r') - show(); + show() if __name__ == "__main__": diff --git a/sw/tools/calibration/calibrate_mag_current.py b/sw/tools/calibration/calibrate_mag_current.py index d93f6cfc64..6779f0f9a1 100755 --- a/sw/tools/calibration/calibrate_mag_current.py +++ b/sw/tools/calibration/calibrate_mag_current.py @@ -49,7 +49,7 @@ def main(): print(args[0] + " not found") sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) - if options.ac_id == None: + if options.ac_id is None: if len(ac_ids) == 1: options.ac_id = ac_ids[0] else: @@ -57,10 +57,6 @@ def main(): if options.verbose: print("Using aircraft id "+options.ac_id) - sensor_ref = 1. - sensor_res = 11 - noise_window = 10; - noise_threshold = 1000; if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -73,7 +69,7 @@ def main(): print("Error: found zero IMU_MAG_CURRENT_CALIBRATION measurements for aircraft with id "+options.ac_id+" in log file!") sys.exit(1) if options.verbose: - print("found "+str(len(measurements))+" records") + print("found "+str(len(measurements))+" records") coefficient = calibration_utils.estimate_mag_current_relation(measurements) diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py index 0cc8f54b75..4fc13aab6c 100644 --- a/sw/tools/calibration/calibration_utils.py +++ b/sw/tools/calibration/calibration_utils.py @@ -38,7 +38,7 @@ def get_ids_in_log(filename): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: id = m.group(1) if not id in ids: @@ -56,7 +56,7 @@ def read_log(ac_id, filename, sensor): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4))]) return scipy.array(list_meas) @@ -72,7 +72,7 @@ def read_log_mag_current(ac_id, filename): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4)), float(m.group(5))]) return scipy.array(list_meas) @@ -106,8 +106,8 @@ def get_min_max_guess(meas, scale): # scale the set of measurements # def scale_measurements(meas, p): - l_comp = []; - l_norm = []; + l_comp = [] + l_norm = [] for m in meas[:,]: sm = (m - p[0:3])*p[3:6] l_comp.append(sm) @@ -120,7 +120,7 @@ def scale_measurements(meas, p): def estimate_mag_current_relation(meas): coefficient = [] for i in range(0,3): - gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3],meas[:,i]) + gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3], meas[:,i]) coefficient.append(gradient) return coefficient @@ -154,26 +154,26 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen title('Raw sensors') subplot(3, 2, 3) - plot(cp0[:, 0]); - plot(cp0[:, 1]); - plot(cp0[:, 2]); - plot(-sensor_ref*scipy.ones(len(flt_meas))); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(cp0[:, 0]) + plot(cp0[:, 1]) + plot(cp0[:, 2]) + plot(-sensor_ref*scipy.ones(len(flt_meas))) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 4) - plot(np0); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(np0) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 5) - plot(cp1[:, 0]); - plot(cp1[:, 1]); - plot(cp1[:, 2]); - plot(-sensor_ref*scipy.ones(len(flt_meas))); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(cp1[:, 0]) + plot(cp1[:, 1]) + plot(cp1[:, 2]) + plot(-sensor_ref*scipy.ones(len(flt_meas))) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 6) - plot(np1); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(np1) + plot(sensor_ref*scipy.ones(len(flt_meas))) # if we want to have another plot we only draw the figure (non-blocking) # also in matplotlib before 1.0.0 there is only one call to show possible @@ -187,14 +187,14 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen # def plot_mag_3d(measured, calibrated, p): # set up points for sphere and ellipsoid wireframes - u=r_[0:2*pi:20j] - v=r_[0:pi:20j] - wx=outer(cos(u),sin(v)) - wy=outer(sin(u),sin(v)) - wz=outer(ones(size(u)),cos(v)) - ex=p[0]*ones(size(u)) + outer(cos(u),sin(v))/p[3] - ey=p[1]*ones(size(u)) + outer(sin(u),sin(v))/p[4] - ez=p[2]*ones(size(u)) + outer(ones(size(u)),cos(v))/p[5] + u = r_[0:2 * pi:20j] + v = r_[0:pi:20j] + wx = outer(cos(u), sin(v)) + wy = outer(sin(u), sin(v)) + wz = outer(ones(size(u)), cos(v)) + ex = p[0] * ones(size(u)) + outer(cos(u), sin(v)) / p[3] + ey = p[1] * ones(size(u)) + outer(sin(u), sin(v)) / p[4] + ez = p[2] * ones(size(u)) + outer(ones(size(u)), cos(v)) / p[5] # measurements mx = measured[:, 0] @@ -228,10 +228,10 @@ def plot_mag_3d(measured, calibrated, p): ax.plot_wireframe(ex, ey, ez, color='grey', alpha=0.5) # Create cubic bounding box to simulate equal aspect ratio - max_range = np.array([mx.max()-mx.min(), my.max()-my.min(), mz.max()-mz.min()]).max() - Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(mx.max()+mx.min()) - Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(my.max()+my.min()) - Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(mz.max()+mz.min()) + max_range = np.array([mx.max() - mx.min(), my.max() - my.min(), mz.max() - mz.min()]).max() + Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][0].flatten() + 0.5 * (mx.max() + mx.min()) + Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][1].flatten() + 0.5 * (my.max() + my.min()) + Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -1:2:2][2].flatten() + 0.5 * (mz.max() + mz.min()) # uncomment following both lines to test the fake bounding box: #for xb, yb, zb in zip(Xb, Yb, Zb): # ax.plot([xb], [yb], [zb], 'r*') @@ -241,7 +241,6 @@ def plot_mag_3d(measured, calibrated, p): ax.set_ylabel('y') ax.set_zlabel('z') - if matplotlib.__version__.startswith('0'): ax = Axes3D(fig, rect=rect_r) else: @@ -273,10 +272,10 @@ def read_turntable_log(ac_id, tt_id, filename, _min, _max): line = f.readline().strip() if line == '': break - m=re.match(pattern_t, line) + m = re.match(pattern_t, line) if m: last_tt = float(m.group(2)) - m=re.match(pattern_g, line) + m = re.match(pattern_g, line) if m and last_tt and last_tt > _min and last_tt < _max: list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), float(m.group(4))]) return scipy.array(list_tt) From 1499a6f7beccb2f2ce3caca200cebcc183128ab7 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 1 Oct 2013 15:23:20 +0200 Subject: [PATCH 103/125] [tools] attitude_viz pep8 --- sw/in_progress/python/attitude_viz.py | 627 +++++++++++++------------- 1 file changed, 318 insertions(+), 309 deletions(-) diff --git a/sw/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py index ebc5074440..325c5d3b7f 100755 --- a/sw/in_progress/python/attitude_viz.py +++ b/sw/in_progress/python/attitude_viz.py @@ -18,353 +18,362 @@ import os _NAME = 'attitude_viz' + class TelemetryQuat: - def __init__(self, message_name, index, name, integer): - self.message_name = message_name - self.index = index - self.name = name - self.qi = 1 - self.qx = 0 - self.qy = 0 - self.qz = 0 - # optional scaling for fixed point telemetry - if integer: - self.scale = 0.00003051757812 - else: - self.scale = 1.0 + def __init__(self, message_name, index, name, integer): + self.message_name = message_name + self.index = index + self.name = name + self.qi = 1 + self.qx = 0 + self.qy = 0 + self.qz = 0 + # optional scaling for fixed point telemetry + if integer: + self.scale = 0.00003051757812 + else: + self.scale = 1.0 + class TelemetryValue: - def __init__(self, message_name, index, name, offset, scale, max): - self.message_name = message_name - self.index = index - self.name = name - self.offset = offset - self.scale = scale - self.max = max - self.value = 0 + def __init__(self, message_name, index, name, offset, scale, max): + self.message_name = message_name + self.index = index + self.name = name + self.offset = offset + self.scale = scale + self.max = max + self.value = 0 + class Visualization: - def __init__(self, parent): - self.quats = [] - self.graph_values = [] - self.throttle = 0.0 - self.mode = 0.0 - self.airspeed = 0.0 - self.display_list = None - self.display_dirty = True - self.rotate_theta = parent.rotate_theta - - for message_name, index, name, bfp in VEHICLE_QUATS: - self.quats.append(TelemetryQuat(message_name, index, name, bfp)) - for message_name, index, name, offset, scale, max in BAR_VALUES: - self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) - - def onmsgproc(self, agent, *larg): - data = str(larg[0]).split(' ') - for telemetry_quat in self.quats: - if (telemetry_quat.message_name == data[1]): + def __init__(self, parent): + self.quats = [] + self.graph_values = [] + self.throttle = 0.0 + self.mode = 0.0 + self.airspeed = 0.0 + self.display_list = None self.display_dirty = True - telemetry_quat.qi = float(data[telemetry_quat.index + 0]) - telemetry_quat.qx = float(data[telemetry_quat.index + 1]) - telemetry_quat.qy = float(data[telemetry_quat.index + 2]) - telemetry_quat.qz = float(data[telemetry_quat.index + 3]) + self.rotate_theta = parent.rotate_theta - for graph_value in self.graph_values: - if (graph_value.message_name == data[1]): - self.display_dirty = True - graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale + for message_name, index, name, bfp in VEHICLE_QUATS: + self.quats.append(TelemetryQuat(message_name, index, name, bfp)) + for message_name, index, name, offset, scale, max in BAR_VALUES: + self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) - def DrawCircle(self, radius): - glBegin(GL_TRIANGLE_FAN) - glVertex3f(0, 0, 0) - for angle in range (0, 361, 12): - glVertex3f( math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) - glEnd() + def onmsgproc(self, agent, *larg): + data = str(larg[0]).split(' ') + for telemetry_quat in self.quats: + if telemetry_quat.message_name == data[1]: + self.display_dirty = True + telemetry_quat.qi = float(data[telemetry_quat.index + 0]) + telemetry_quat.qx = float(data[telemetry_quat.index + 1]) + telemetry_quat.qy = float(data[telemetry_quat.index + 2]) + telemetry_quat.qz = float(data[telemetry_quat.index + 3]) - # draw quad centered at origin, z = 0 - def DrawQuad(self, width, height): - glBegin (GL_QUADS) - glVertex3f( width, height, 0) - glVertex3f( -width, height, 0) - glVertex3f( -width, -height, 0) - glVertex3f( width, -height, 0) - glEnd() + for graph_value in self.graph_values: + if graph_value.message_name == data[1]: + self.display_dirty = True + graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale - def DrawBox(self, width, height, depth): - glPushMatrix() - glTranslate(0, 0, depth) - self.DrawQuad(width, height) - glTranslate(0, 0, -2 * depth) - self.DrawQuad(width, height) - glPopMatrix() + def DrawCircle(self, radius): + glBegin(GL_TRIANGLE_FAN) + glVertex3f(0, 0, 0) + for angle in range(0, 361, 12): + glVertex3f(math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) + glEnd() - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(0, 0, height) - self.DrawQuad(width, depth) - glTranslate(0, 0, -2 * height) - self.DrawQuad(width, depth) - glPopMatrix() + # draw quad centered at origin, z = 0 + def DrawQuad(self, width, height): + glBegin(GL_QUADS) + glVertex3f(width, height, 0) + glVertex3f(-width, height, 0) + glVertex3f(-width, -height, 0) + glVertex3f(width, -height, 0) + glEnd() - glPushMatrix() - glRotate(90, 0, 1, 0) - glTranslate(0, 0, width) - self.DrawQuad(depth, height) - glTranslate(0, 0, -2 * width) - self.DrawQuad(depth, height) - glPopMatrix() - - def DrawVehicle(self, name): - wingspan = 2.7 - separation = 0.7 - chord = 0.35 - thickness = 0.08 - strutcount = 3 - discradius = 0.45 - discseparation = 0.01 - - #wings - glColor3f(0.1, 0.1, 0.9) - glPushMatrix() - glTranslate(0, 0, 0.05) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(-wingspan, -0.2, thickness + 0.01) - glScale(0.004, 0.004, 0.004) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - - glPushMatrix() - glTranslate(0, 0, -0.05) - glColor3f(0.6, 0.6, 0.2) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(wingspan, -0.2, -0.01 - thickness) - glScale(0.004, 0.004, 0.004) - glRotate(180, 0, 1, 0) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - - if self.display_list is None: - self.display_list = glGenLists(1) - glNewList(self.display_list, GL_COMPILE) - # struts - glColor3f(0.4, 0.4, 0.4) - glPushMatrix() - glTranslate(-wingspan/2, 0, separation/2) - glRotate(90, 0, 1, 0) - for x in range (0, strutcount-1): - self.DrawBox(separation/2, chord - .01, thickness) - glTranslate(0, 0, wingspan) - glTranslate(separation, 0, -5*wingspan/2) - for x in range (0, strutcount-1): - self.DrawBox(separation/2, chord - .01, thickness) - glTranslate(0, 0, 2*wingspan) - glPopMatrix() - - #rotors - glColor3f(0.9, 0.1, 0.1) - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan/2, separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount + 1), 0, 0) - glPopMatrix() - - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan, -separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount - 1), 0, 0) - glPopMatrix() - glEndList() - - glCallList(self.display_list) - - def DrawBar(self, name, value): - bar_height = 0.12 - bar_length = 3 - glPushMatrix() - glColor3f(0, 0, 0) - glTranslate(-bar_length, -0.09, 0.02) - glScale(0.0015, 0.0015, 0.0015) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - glColor3f(0.92, 0.92, 0.92) - glPushMatrix() - glTranslate(0, 0, 0) - self.DrawQuad(bar_length, bar_height) - glPopMatrix() - glPushMatrix() - glTranslate(bar_length * value - bar_length, 0, 0.01) - glColor3f(0.6, 0.6, 0.6) - self.DrawQuad(bar_length * value, bar_height) - glPopMatrix() - - def Draw(self): - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - - glPushMatrix() - - height = 5 - - glDisable(GL_LIGHTING) - glPushMatrix() - for graph_value in self.graph_values: - self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) - glTranslate(0, 0.35, 0) - glPopMatrix() - glEnable(GL_LIGHTING) - - glTranslate(0, -height + (height / len(self.quats) + 1), 0) - for telemetry_quat in self.quats: - glPushMatrix() - try: - scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] - glRotate(360 * math.acos(scaled_quat[0] ) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) - glRotate(self.rotate_theta, 1, 0, 0) - - self.DrawVehicle(telemetry_quat.name) - except Exception: - raise Exception - finally: + def DrawBox(self, width, height, depth): + glPushMatrix() + glTranslate(0, 0, depth) + self.DrawQuad(width, height) + glTranslate(0, 0, -2 * depth) + self.DrawQuad(width, height) glPopMatrix() - glTranslate(0, 2 * height / (len(self.quats)), 0) - glPopMatrix() + + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(0, 0, height) + self.DrawQuad(width, depth) + glTranslate(0, 0, -2 * height) + self.DrawQuad(width, depth) + glPopMatrix() + + glPushMatrix() + glRotate(90, 0, 1, 0) + glTranslate(0, 0, width) + self.DrawQuad(depth, height) + glTranslate(0, 0, -2 * width) + self.DrawQuad(depth, height) + glPopMatrix() + + def DrawVehicle(self, name): + wingspan = 2.7 + separation = 0.7 + chord = 0.35 + thickness = 0.08 + strutcount = 3 + discradius = 0.45 + discseparation = 0.01 + + #wings + glColor3f(0.1, 0.1, 0.9) + glPushMatrix() + glTranslate(0, 0, 0.05) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(-wingspan, -0.2, thickness + 0.01) + glScale(0.004, 0.004, 0.004) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + + glPushMatrix() + glTranslate(0, 0, -0.05) + glColor3f(0.6, 0.6, 0.2) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(wingspan, -0.2, -0.01 - thickness) + glScale(0.004, 0.004, 0.004) + glRotate(180, 0, 1, 0) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + + if self.display_list is None: + self.display_list = glGenLists(1) + glNewList(self.display_list, GL_COMPILE) + # struts + glColor3f(0.4, 0.4, 0.4) + glPushMatrix() + glTranslate(-wingspan / 2, 0, separation / 2) + glRotate(90, 0, 1, 0) + for x in range(0, strutcount - 1): + self.DrawBox(separation / 2, chord - .01, thickness) + glTranslate(0, 0, wingspan) + glTranslate(separation, 0, -5 * wingspan / 2) + for x in range(0, strutcount - 1): + self.DrawBox(separation / 2, chord - .01, thickness) + glTranslate(0, 0, 2 * wingspan) + glPopMatrix() + + #rotors + glColor3f(0.9, 0.1, 0.1) + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan / 2, separation, -(chord + .01)) + for x in range(0, strutcount): + if (x != strutcount / 2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan / (strutcount + 1), 0, 0) + glPopMatrix() + + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan, -separation, -(chord + .01)) + for x in range(0, strutcount): + if (x != strutcount / 2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan / (strutcount - 1), 0, 0) + glPopMatrix() + glEndList() + + glCallList(self.display_list) + + def DrawBar(self, name, value): + bar_height = 0.12 + bar_length = 3 + glPushMatrix() + glColor3f(0, 0, 0) + glTranslate(-bar_length, -0.09, 0.02) + glScale(0.0015, 0.0015, 0.0015) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + glColor3f(0.92, 0.92, 0.92) + glPushMatrix() + glTranslate(0, 0, 0) + self.DrawQuad(bar_length, bar_height) + glPopMatrix() + glPushMatrix() + glTranslate(bar_length * value - bar_length, 0, 0.01) + glColor3f(0.6, 0.6, 0.6) + self.DrawQuad(bar_length * value, bar_height) + glPopMatrix() + + def Draw(self): + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + + glPushMatrix() + + height = 5 + + glDisable(GL_LIGHTING) + glPushMatrix() + for graph_value in self.graph_values: + self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) + glTranslate(0, 0.35, 0) + glPopMatrix() + glEnable(GL_LIGHTING) + + glTranslate(0, -height + (height / len(self.quats) + 1), 0) + for telemetry_quat in self.quats: + glPushMatrix() + try: + scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, + telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] + glRotate(360 * math.acos(scaled_quat[0]) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) + glRotate(self.rotate_theta, 1, 0, 0) + + self.DrawVehicle(telemetry_quat.name) + except Exception: + raise Exception + finally: + glPopMatrix() + glTranslate(0, 2 * height / (len(self.quats)), 0) + glPopMatrix() + class Visualizer: - def __init__(self, rotate_theta): - self.rotate_theta = rotate_theta - self.visualization = Visualization(self) + def __init__(self, rotate_theta): + self.rotate_theta = rotate_theta + self.visualization = Visualization(self) - # listen to Ivy - logging.getLogger('Ivy').setLevel(logging.WARN) - IvyInit(_NAME, - "", - 0, - lambda x, y: y, - lambda x, z: z - ) + # listen to Ivy + logging.getLogger('Ivy').setLevel(logging.WARN) + IvyInit(_NAME, + "", + 0, + lambda x, y: y, + lambda x, z: z) - if os.getenv('IVY_BUS') != None: - IvyStart(os.getenv('IVY_BUS')) - else: - if platform.system() == 'Darwin': - IvyStart("224.255.255.255:2010") - else: - IvyStart() + if os.getenv('IVY_BUS') is not None: + IvyStart(os.getenv('IVY_BUS')) + else: + if platform.system() == 'Darwin': + IvyStart("224.255.255.255:2010") + else: + IvyStart() - # list of all message names - messages = [] + # list of all message names + messages = [] - # append all message names - for vehicle_quat in VEHICLE_QUATS: - messages.append(vehicle_quat[0]) - for bar_value in BAR_VALUES: - messages.append(bar_value[0]) + # append all message names + for vehicle_quat in VEHICLE_QUATS: + messages.append(vehicle_quat[0]) + for bar_value in BAR_VALUES: + messages.append(bar_value[0]) - # bind to set of messages (ie, only bind each message once) - for message_name in set(messages): - bind_string = "(^.*" + message_name + ".*$)" - IvyBindMsg(self.visualization.onmsgproc, bind_string) + # bind to set of messages (ie, only bind each message once) + for message_name in set(messages): + bind_string = "(^.*" + message_name + ".*$)" + IvyBindMsg(self.visualization.onmsgproc, bind_string) - def Draw(self): - if self.visualization.display_dirty: - self.visualization.Draw() - self.visualization.display_dirty = False + def Draw(self): + if self.visualization.display_dirty: + self.visualization.Draw() + self.visualization.display_dirty = False + + def OnClose(self): + IvyStop() - def OnClose(self): - IvyStop() SCREEN_SIZE = (800, 800) + def resize(width, height): - glViewport(0, 0, width, height) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(60.0, float(width/height), .1, 100.) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() + glViewport(0, 0, width, height) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(60.0, float(width / height), .1, 100.) + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + def init(): - glutInit() - glEnable(GL_LINE_SMOOTH) - glEnable(GL_DEPTH_TEST) - glEnable(GL_LIGHTING) - glEnable(GL_LIGHT0) - glEnable(GL_BLEND) - glShadeModel (GL_SMOOTH) - glClearColor(1.0, 1.0, 1.0, 1.0) - glClearDepth(1.0) + glutInit() + glEnable(GL_LINE_SMOOTH) + glEnable(GL_DEPTH_TEST) + glEnable(GL_LIGHTING) + glEnable(GL_LIGHT0) + glEnable(GL_BLEND) + glShadeModel(GL_SMOOTH) + glClearColor(1.0, 1.0, 1.0, 1.0) + glClearDepth(1.0) - glPointSize(3.0) + glPointSize(3.0) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(7.0, 1.0, 95.0, 105.0) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(7.0, 1.0, 95.0, 105.0) - glMatrixMode(GL_MODELVIEW) + glMatrixMode(GL_MODELVIEW) - glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) - glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) - glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) - glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) - glEnable(GL_COLOR_MATERIAL) - glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) + glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) + glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) + glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) + glEnable(GL_COLOR_MATERIAL) + glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + gluLookAt(0.0, 0.0, 100.0, + 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() - gluLookAt(0.0, 0.0, 100.0, - 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0) def run(): - global VEHICLE_QUATS, BAR_VALUES - VEHICLE_QUATS = [ ["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] - BAR_VALUES = [ ["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100] ] - window_title = "Attitude_Viz" - rotate_theta = -90 - try: - opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) - for o, a in opts: - if o in ("-t", "--title"): - window_title = a - if o in ("-r", "--rotate_theta"): - rotate_theta = int(a) - except getopt.error as msg: - print(msg) - print("""usage: + global VEHICLE_QUATS, BAR_VALUES + VEHICLE_QUATS = [["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] + BAR_VALUES = [["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100]] + window_title = "Attitude_Viz" + rotate_theta = -90 + try: + opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) + for o, a in opts: + if o in ("-t", "--title"): + window_title = a + if o in ("-r", "--rotate_theta"): + rotate_theta = int(a) + except getopt.error as msg: + print(msg) + print("""usage: -t, --title set window title -r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: -90) """) - pygame.init() - screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL|pygame.DOUBLEBUF) - #resize(*SCREEN_SIZE) - init() - visualizer = Visualizer(rotate_theta) + pygame.init() + screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL | pygame.DOUBLEBUF) + #resize(*SCREEN_SIZE) + init() + visualizer = Visualizer(rotate_theta) + + try: + while True: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + visualizer.OnClose() + return + if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: + visualizer.OnClose() + return + visualizer.Draw() + pygame.display.flip() + time.sleep(.02) + except KeyboardInterrupt: + visualizer.OnClose() + return - try: - while True: - for event in pygame.event.get(): - if event.type == pygame.QUIT: - visualizer.OnClose() - return - if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: - visualizer.OnClose() - return - visualizer.Draw() - pygame.display.flip() - time.sleep(.02) - except KeyboardInterrupt: - visualizer.OnClose() - return if __name__ == "__main__": - run() + run() From 7b8f824fbdba670e496237d3b805bf228bd60540 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 2 Oct 2013 15:14:21 +0200 Subject: [PATCH 104/125] [mission] better mission interface - cleaning interface - better parsing - better handling of navigation routine - implement several insertion modes - implement duration - conversions from LLA to local are missing in parsing function - some patterns are missing - only fixedwing so far --- conf/messages.xml | 98 +++++-- conf/modules/mission_fw.xml | 32 +++ conf/modules/mission_msg.xml | 23 -- sw/airborne/modules/mission/mission.c | 253 +++++++++++++++++++ sw/airborne/modules/mission/mission.h | 143 +++++++++++ sw/airborne/modules/mission/mission_fw_nav.c | 146 +++++++++++ sw/airborne/modules/mission/mission_msg.c | 127 ---------- sw/airborne/modules/mission/mission_msg.h | 176 ------------- sw/airborne/modules/mission/mission_nav.c | 180 ------------- sw/airborne/modules/mission/mission_nav.h | 49 ---- 10 files changed, 657 insertions(+), 570 deletions(-) create mode 100644 conf/modules/mission_fw.xml delete mode 100644 conf/modules/mission_msg.xml create mode 100644 sw/airborne/modules/mission/mission.c create mode 100644 sw/airborne/modules/mission/mission.h create mode 100644 sw/airborne/modules/mission/mission_fw_nav.c delete mode 100644 sw/airborne/modules/mission/mission_msg.c delete mode 100644 sw/airborne/modules/mission/mission_msg.h delete mode 100644 sw/airborne/modules/mission/mission_nav.c delete mode 100644 sw/airborne/modules/mission/mission_nav.h diff --git a/conf/messages.xml b/conf/messages.xml index ce098db290..919b48adec 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2210,37 +2210,67 @@ - + - + - + - + + + + + + + + + + - + - + - + + + + + + + + + + + - + - + - + + + + + + + + + + + + @@ -2252,25 +2282,63 @@ - + + - + - + + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/mission_fw.xml b/conf/modules/mission_fw.xml new file mode 100644 index 0000000000..096e896394 --- /dev/null +++ b/conf/modules/mission_fw.xml @@ -0,0 +1,32 @@ + + + + + + Interface for mission control of fixed wing aircraft. + This module parse datalink commands for basic navigation routines + and store them in a queue. + + +
+ +
+ + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/mission_msg.xml b/conf/modules/mission_msg.xml deleted file mode 100644 index 1dee0bd4a7..0000000000 --- a/conf/modules/mission_msg.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - New messages for Mission Planner - -
- - - -
- - - - - - - - - - - - diff --git a/sw/airborne/modules/mission/mission.c b/sw/airborne/modules/mission/mission.c new file mode 100644 index 0000000000..dc20909f2f --- /dev/null +++ b/sw/airborne/modules/mission/mission.c @@ -0,0 +1,253 @@ +/* + * Copyright (C) 2013 ENAC + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/mission/mission.c + * @brief messages parser for mission interface + */ + +#include "modules/mission/mission.h" + +#include +#include "generated/airframe.h" +#include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" + + +struct _mission mission; + + +void mission_init(void) { + mission.insert_idx = 0; + mission.current_idx = 0; + mission.element_time = 0.; +} + +bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element) { + uint8_t tmp; + + switch (insert) { + case Append: + tmp = (mission.insert_idx + 1) % MISSION_ELEMENT_NB; + if (tmp == mission.current_idx) return FALSE; // no room to insert element + memcpy(&mission.elements[mission.insert_idx], element, sizeof(struct _mission_element)); // add element + mission.insert_idx = tmp; // move insert index + break; + case Prepend: + if (mission.current_idx == 0) tmp = MISSION_ELEMENT_NB-1; + else tmp = mission.current_idx - 1; + if (tmp == mission.insert_idx) return FALSE; // no room to inser element + memcpy(&mission.elements[tmp], element, sizeof(struct _mission_element)); // add element + mission.current_idx = tmp; // move current index + break; + case ReplaceCurrent: + // current element can always be modified, index are not changed + memcpy(&mission.elements[mission.current_idx], element, sizeof(struct _mission_element)); + break; + case ReplaceAll: + // reset queue and index + memcpy(&mission.elements[0], element, sizeof(struct _mission_element)); + mission.insert_idx = 0; + mission.current_idx = 0; + break; + default: + // unknown insertion mode + return FALSE; + } + return TRUE; + +} + +bool_t mission_get(struct _mission_element * element __attribute__((unused))) { + if (mission.current_idx == mission.insert_idx) { + element = NULL; + return FALSE; + } + element = &(mission.elements[mission.current_idx]); + return TRUE; +} + + + +/////////////////////// +// Parsing functions // +/////////////////////// + +int mission_parse_GOTO_WP(void) { + if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionWP; + me.element.mission_wp.wp.x = DL_MISSION_GOTO_WP_wp_east(dl_buffer); + me.element.mission_wp.wp.y = DL_MISSION_GOTO_WP_wp_north(dl_buffer); + me.element.mission_wp.wp.z = DL_MISSION_GOTO_WP_wp_alt(dl_buffer); + me.duration = DL_MISSION_GOTO_WP_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_GOTO_WP_LLA(void) { + if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionWP; + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_CIRCLE(void) { + if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionCircle; + me.element.mission_circle.center.x = DL_MISSION_CIRCLE_center_east(dl_buffer); + me.element.mission_circle.center.y = DL_MISSION_CIRCLE_center_north(dl_buffer); + me.element.mission_circle.center.z = DL_MISSION_CIRCLE_center_alt(dl_buffer); + me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(dl_buffer); + me.duration = DL_MISSION_CIRCLE_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_CIRCLE_LLA(void) { + if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionCircle; + me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(dl_buffer); + me.duration = DL_MISSION_CIRCLE_LLA_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_SEGMENT(void) { + if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionSegment; + me.element.mission_segment.from.x = DL_MISSION_SEGMENT_segment_east_1(dl_buffer); + me.element.mission_segment.from.y = DL_MISSION_SEGMENT_segment_north_1(dl_buffer); + me.element.mission_segment.from.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer); + me.element.mission_segment.to.x = DL_MISSION_SEGMENT_segment_east_2(dl_buffer); + me.element.mission_segment.to.y = DL_MISSION_SEGMENT_segment_north_2(dl_buffer); + me.element.mission_segment.to.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer); + me.duration = DL_MISSION_SEGMENT_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_SEGMENT_LLA(void) { + if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionSegment; + me.duration = DL_MISSION_SEGMENT_LLA_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_PATH(void) { + if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionPath; + me.element.mission_path.path[0].x = DL_MISSION_PATH_point_east_1(dl_buffer); + me.element.mission_path.path[0].y = DL_MISSION_PATH_point_north_1(dl_buffer); + me.element.mission_path.path[0].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[1].x = DL_MISSION_PATH_point_east_2(dl_buffer); + me.element.mission_path.path[1].y = DL_MISSION_PATH_point_north_2(dl_buffer); + me.element.mission_path.path[1].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[2].x = DL_MISSION_PATH_point_east_3(dl_buffer); + me.element.mission_path.path[2].y = DL_MISSION_PATH_point_north_3(dl_buffer); + me.element.mission_path.path[2].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[3].x = DL_MISSION_PATH_point_east_4(dl_buffer); + me.element.mission_path.path[3].y = DL_MISSION_PATH_point_north_4(dl_buffer); + me.element.mission_path.path[3].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.path[4].x = DL_MISSION_PATH_point_east_5(dl_buffer); + me.element.mission_path.path[4].y = DL_MISSION_PATH_point_north_5(dl_buffer); + me.element.mission_path.path[4].z = DL_MISSION_PATH_path_alt(dl_buffer); + me.element.mission_path.nb = DL_MISSION_PATH_nb(dl_buffer); + if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB; + me.element.mission_path.path_idx = 0; + me.duration = DL_MISSION_PATH_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_PATH_LLA(void) { + if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + struct _mission_element me; + me.type = MissionPath; + me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(dl_buffer); + if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB; + me.element.mission_path.path_idx = 0; + me.duration = DL_MISSION_PATH_LLA_duration(dl_buffer); + + enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_LLA_insert(dl_buffer)); + + return mission_insert(insert, &me); +} + +int mission_parse_GOTO_MISSION(void) { + if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer); + if (mission_id < MISSION_ELEMENT_NB) { + mission.current_idx = mission_id; + } + else return FALSE; + + return TRUE; +} + +int mission_parse_NEXT_MISSION(void) { + if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + if (mission.current_idx == mission.insert_idx) return FALSE; // already at the last position + + // increment current index + mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB; + return TRUE; +} + +int mission_parse_END_MISSION(void) { + if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft + + // set current index to insert index (last position) + mission.current_idx = mission.insert_idx; + return TRUE; +} + diff --git a/sw/airborne/modules/mission/mission.h b/sw/airborne/modules/mission/mission.h new file mode 100644 index 0000000000..14cd363ea4 --- /dev/null +++ b/sw/airborne/modules/mission/mission.h @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/mission/mission.h + * @brief mission planner library + * + * Provide the generic interface for the mission control + * Handle the parsing of datalink messages + */ + +#ifndef MISSION_H +#define MISSION_H + +#include "std.h" +#include "math/pprz_geodetic_float.h" + +enum MissionType { + MissionWP, + MissionCircle, + MissionSegment, + MissionPath, + MissionSurvey, + MissionEight, + MissionOval +}; + +enum MissionInsertMode { + Append, ///< add at the last position + Prepend, ///< add before the current element + ReplaceCurrent, ///< replace current element + ReplaceAll ///< remove all elements and add the new one +}; + +struct _mission_wp { + struct EnuCoor_f wp; +}; + +struct _mission_circle { + struct EnuCoor_f center; + float radius; +}; + +struct _mission_segment { + struct EnuCoor_f from; + struct EnuCoor_f to; +}; + +#define MISSION_PATH_NB 5 +struct _mission_path { + struct EnuCoor_f path[MISSION_PATH_NB]; + uint8_t path_idx; + uint8_t nb; +}; + +struct _mission_element { + enum MissionType type; + union { + struct _mission_wp mission_wp; + struct _mission_circle mission_circle; + struct _mission_segment mission_segment; + struct _mission_path mission_path; + } element; + float duration; ///< time to spend in the element (<= 0 to disable) +}; + +#define MISSION_ELEMENT_NB 20 +struct _mission { + struct _mission_element elements[MISSION_ELEMENT_NB]; + float element_time; ///< time in second spend in the current element + uint8_t insert_idx; ///< inserstion index + uint8_t current_idx; ///< current mission element index +}; + +extern struct _mission mission; + +/** Init mission structure + */ +extern void mission_init(void); + +/** Insert a mission element according to the insertion mode + * @param insert insertion mode + * @param element mission element structure + * @return return TRUE if insertion is succesful, FALSE otherwise + */ +extern bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element); + +/** Get current mission element + * @param element pointer to the current mission element + * @return TRUE if valid element, FALSE if mission queue is empty (add element is set to NULL) + */ +extern bool_t mission_get(struct _mission_element * element); + +/** Run mission + * + * This function should be implemented into a dedicated file since + * navigation functions are different for different firmwares + * + * Currently, this function should be called from the flight plan + * + * @return return TRUE when the mission is running, FALSE when it is finished + */ +extern int mission_run(); + +/** Parsing functions called when a mission message is received + */ +extern int mission_parse_GOTO_WP(void); +extern int mission_parse_GOTO_WP_LLA(void); +extern int mission_parse_CIRCLE(void); +extern int mission_parse_CIRCLE_LLA(void); +extern int mission_parse_SEGMENT(void); +extern int mission_parse_SEGMENT_LLA(void); +extern int mission_parse_PATH(void); +extern int mission_parse_PATH_LLA(void); +extern int mission_parse_SURVEY(void); +extern int mission_parse_SURVEY_LLA(void); +extern int mission_parse_GOTO_MISSION(void); +extern int mission_parse_NEXT_MISSION(void); +extern int mission_parse_END_MISSION(void); + +/** Status report messages + * @todo + */ + +#endif // MISSION diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c new file mode 100644 index 0000000000..7bc3494a0d --- /dev/null +++ b/sw/airborne/modules/mission/mission_fw_nav.c @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/mission/mission_fw_nav.c + * @brief mission navigation for fixedwing aircraft + * + * Implement specific navigation routines for the mission control + * of a fixedwing aircraft + */ + +#include +#include "modules/mission/mission.h" +#include "firmwares/fixedwing/autopilot.h" +#include "subsystems/nav.h" + +// navigation time step +const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY); + +// dirty hack to comply with nav_approaching_xy function +struct EnuCoor_f last_wp = { 0., 0., 0. }; + +/** Navigation function to a single waypoint + */ +static inline bool_t mission_nav_wp(struct _mission_wp * wp) { + if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp.x, last_wp.y, CARROT)) { + last_wp = wp->wp; // store last wp + return FALSE; // end of mission element + } + // set navigation command + fly_to_xy(wp->wp.x, wp->wp.y); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(wp->wp.z, 0.); + return TRUE; +} + +/** Navigation function on a circle + */ +static inline bool_t mission_nav_circle(struct _mission_circle * circle) { + nav_circle_XY(circle->center.x, circle->center.y, circle->radius); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(circle->center.z, 0.); + return TRUE; +} + +/** Navigation function along a segment + */ +static inline bool_t mission_nav_segment(struct _mission_segment * segment) { + if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) { + last_wp = segment->to; + return FALSE; // end of mission element + } + nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(segment->to.z, 0.); // both altitude should be the same anyway + return TRUE; +} + +/** Navigation function along a path + */ +static inline bool_t mission_nav_path(struct _mission_path * path) { + if (path->nb == 0) { + return FALSE; // nothing to do + } + if (path->nb == 1) { + // handle as a single waypoint + struct _mission_wp wp = { path->path[0] }; + return mission_nav_wp(&wp); + } + if (path->path_idx == path->nb-1) { + last_wp = path->path[path->path_idx]; // store last wp + return FALSE; // end of path + } + // normal case + struct EnuCoor_f from = path->path[path->path_idx]; + struct EnuCoor_f to = path->path[path->path_idx+1]; + nav_route_xy(from.x, from.y, to.x, to.y); + NavVerticalAutoThrottleMode(0.); + NavVerticalAltitudeMode(to.z, 0.); // both altitude should be the same anyway + if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)) { + path->path_idx++; // go to next segment + } + return TRUE; +} + + +int mission_run() { + // current element + struct _mission_element * el = NULL; + if (!mission_get(el)) { + // TODO do something special like a waiting circle before ending the mission ? + return FALSE; // end of mission + } + + bool_t el_running = FALSE; + switch (el->type){ + case MissionWP: + el_running = mission_nav_wp(&(el->element.mission_wp)); + break; + case MissionCircle: + el_running = mission_nav_circle(&(el->element.mission_circle)); + break; + case MissionSegment: + el_running = mission_nav_segment(&(el->element.mission_segment)); + break; + case MissionPath: + el_running = mission_nav_path(&(el->element.mission_path)); + break; + default: + // invalid type or pattern not yet handled + break; + } + + // increment element_time + mission.element_time += dt_navigation; + + // test if element is finished or element time is elapsed + if (!el_running || (el->duration > 0. && mission.element_time >= el->duration)) { + // reset timer + mission.element_time = 0.; + // go to next element + mission.current_idx++; + } + + return TRUE; +} + + diff --git a/sw/airborne/modules/mission/mission_msg.c b/sw/airborne/modules/mission/mission_msg.c deleted file mode 100644 index c897805794..0000000000 --- a/sw/airborne/modules/mission/mission_msg.c +++ /dev/null @@ -1,127 +0,0 @@ -/** \file mission_msg.c - * \brief the new messages for mission planner library - * \ edited by Anh Truong - */ - -//#define MISSION_C - -#include -#include "modules/mission/mission_msg.h" - -#include - -#include "subsystems/datalink/downlink.h" - -/*#include "state.h" -#include "autopilot.h" -#include "subsystems/gps.h" -#include "generated/airframe.h" -#include "dl_protocol.h"*/ - - -struct _mission mission; - - -void mission_init(void) { - mission.mission_insert_idx = 0; - mission.mission_extract_idx = 0; - //mission_nav_finish = FALSE; -} - -int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d) { - //int insert_idx = mission.mission_insert_idx; - mission.elements[mission.mission_insert_idx].type = MissionWP; - mission.elements[mission.mission_insert_idx].element.mission_wp.wp.x = x; - mission.elements[mission.mission_insert_idx].element.mission_wp.wp.y = y; - mission.elements[mission.mission_insert_idx].element.mission_wp.wp.a = a; - mission.elements[mission.mission_insert_idx].duration = d; - - //printf("stocking mission GOTO_WP...\n");fflush(stdout); - mission.mission_insert_idx++; - if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; - return TRUE; -} - -int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d) { - //int insert_idx = mission.mission_insert_idx; - mission.elements[mission.mission_insert_idx].type = MissionSegment; - mission.elements[mission.mission_insert_idx].element.mission_segment.from.x = x1; - mission.elements[mission.mission_insert_idx].element.mission_segment.from.y = y1; - mission.elements[mission.mission_insert_idx].element.mission_segment.from.a = a; - mission.elements[mission.mission_insert_idx].element.mission_segment.to.x = x2; - mission.elements[mission.mission_insert_idx].element.mission_segment.to.y = y2; - mission.elements[mission.mission_insert_idx].element.mission_segment.to.a = a; - mission.elements[mission.mission_insert_idx].duration = d; - - //printf("stocking mission SEGMENT...\n");fflush(stdout); - //printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout); - mission.mission_insert_idx++; - if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; - return TRUE; -} - - -int mission_msg_CIRCLE(float x, float y, float radius, float a, uint16_t d) { - //int insert_idx = mission.mission_insert_idx; - mission.elements[mission.mission_insert_idx].type = MissionCircle; - mission.elements[mission.mission_insert_idx].element.mission_circle.center.x = x; - mission.elements[mission.mission_insert_idx].element.mission_circle.center.y = y; - mission.elements[mission.mission_insert_idx].element.mission_circle.center.a = a; - mission.elements[mission.mission_insert_idx].element.mission_circle.radius = radius; - mission.elements[mission.mission_insert_idx].duration = d; - - //printf("stocking mission CIRCLE...\n");fflush(stdout); - //printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout); - mission.mission_insert_idx++; - if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; - return TRUE; -} - -//int i_path, i_point; -int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5, float a, uint16_t d) { - //int insert_idx = mission.mission_insert_idx; - mission.elements[mission.mission_insert_idx].type = MissionPath; - - int i_path; - i_path = 0; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x1; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y1; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; - - - i_path++; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x2; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y2; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; - - - i_path++; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x3; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y3; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; - - - i_path++; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x4; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y4; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; - - - i_path++; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x5; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y5; - mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a; - - - mission.elements[mission.mission_insert_idx].duration = d; - - //printf("stocking mission PATH...\n");fflush(stdout); - //printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout); - mission.mission_insert_idx++; - if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0; - return TRUE; -} - - - - diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h deleted file mode 100644 index e6436491be..0000000000 --- a/sw/airborne/modules/mission/mission_msg.h +++ /dev/null @@ -1,176 +0,0 @@ -/* - * Copyright (C) 2013 ENAC - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file mission_msg.h - * @brief the new messages for mission planner library - */ - -#ifndef MISSION_H -#define MISSION_H - -#include "std.h" -#include "subsystems/navigation/common_nav.h" - -#include - -#include "subsystems/datalink/downlink.h" - -#include "state.h" -#include "autopilot.h" -#include "subsystems/gps.h" -#include "generated/airframe.h" -#include "dl_protocol.h" - -enum MissionType { - MissionWP, - MissionCircle, - MissionSegment, - MissionPath, - MissionSurvey, - MissionEight, - MissionOval -}; - -struct _mission_wp { - struct point wp; -}; - -struct _mission_circle { - struct point center; - float radius; -}; - -struct _mission_segment { - struct point from; - struct point to; -}; - -#define MISSION_PATH_NB 5 -struct _mission_path { - //struct point[MISSION_PATH_NB] path; - struct point path[MISSION_PATH_NB]; -}; - -struct _mission_element { - //char *mission_name; - enum MissionType type; - union { - struct _mission_wp mission_wp; - struct _mission_circle mission_circle; - struct _mission_segment mission_segment; - struct _mission_path mission_path; - } element; - uint16_t duration; - //bool_t mission_finish; -}; - -#define MISSION_ELEMENT_NB 20 -struct _mission { - //struct _mission_element[MISSION_ELEMENT_NB] elements; - struct _mission_element elements[MISSION_ELEMENT_NB]; - uint8_t mission_insert_idx; - uint8_t mission_extract_idx; - //bool_t mission_nav_finish = FALSE; -}; - -extern struct _mission mission; - -//extern bool_t mission_nav_finish; -extern void mission_init(void); - -extern int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d); - -extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d); - -extern int mission_msg_CIRCLE(float x, float y, float radius, float a, uint16_t d); - -//extern int mission_msg_PATH_init( void ); -extern int i_path, i_point; -extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5, float a, uint16_t d); - -extern int goto_mission_msg(uint8_t ac_id, uint8_t mission_id); - - - -#define ParseMissionGotoWp() { \ - if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) == AC_ID) { \ - uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \ - float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \ - float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \ - float wp_alt = DL_MISSION_GOTO_WP_wp_alt(dl_buffer); \ - float duration = DL_MISSION_GOTO_WP_duration(dl_buffer); \ - mission_msg_GOTO_WP(wp_east, wp_north, wp_alt, duration); \ - } \ -} - -#define ParseMissionPath() { \ - if (DL_MISSION_PATH_ac_id(dl_buffer) == AC_ID) { \ - uint8_t ac_id = DL_MISSION_PATH_ac_id(dl_buffer); \ - float point_east_1 = DL_MISSION_PATH_point_east_1(dl_buffer); \ - float point_north_1 = DL_MISSION_PATH_point_north_1(dl_buffer); \ - float point_east_2 = DL_MISSION_PATH_point_east_2(dl_buffer); \ - float point_north_2 = DL_MISSION_PATH_point_north_2(dl_buffer); \ - float point_east_3 = DL_MISSION_PATH_point_east_3(dl_buffer); \ - float point_north_3 = DL_MISSION_PATH_point_north_3(dl_buffer); \ - float point_east_4 = DL_MISSION_PATH_point_east_4(dl_buffer); \ - float point_north_4 = DL_MISSION_PATH_point_north_4(dl_buffer); \ - float point_east_5 = DL_MISSION_PATH_point_east_5(dl_buffer); \ - float point_north_5 = DL_MISSION_PATH_point_north_5(dl_buffer); \ - float path_alt = DL_MISSION_PATH_path_alt(dl_buffer); \ - float duration = DL_MISSION_PATH_duration(dl_buffer); \ - mission_msg_PATH(point_east_1, point_north_1, point_east_2, point_north_2, point_east_3, point_north_3, point_east_4, point_north_4, point_east_5, point_north_5, path_alt, duration); \ - } \ -} - - -#define ParseMissionSegment() { \ - if (DL_MISSION_SEGMENT_ac_id(dl_buffer) == AC_ID) { \ - uint8_t ac_id = DL_MISSION_SEGMENT_ac_id(dl_buffer); \ - float segment_east_1 = DL_MISSION_SEGMENT_segment_east_1(dl_buffer); \ - float segment_north_1 = DL_MISSION_SEGMENT_segment_north_1(dl_buffer); \ - float segment_east_2 = DL_MISSION_SEGMENT_segment_east_2(dl_buffer); \ - float segment_north_2 = DL_MISSION_SEGMENT_segment_north_2(dl_buffer); \ - float segment_alt = DL_MISSION_SEGMENT_segment_alt(dl_buffer); \ - float duration = DL_MISSION_SEGMENT_duration(dl_buffer); \ - mission_msg_SEGMENT(segment_east_1, segment_north_1, segment_east_2, segment_north_2, segment_alt, duration); \ - } \ -} - -#define ParseMissionCircle() { \ - if (DL_MISSION_CIRCLE_ac_id(dl_buffer) == AC_ID) { \ - uint8_t ac_id = DL_MISSION_CIRCLE_ac_id(dl_buffer); \ - float center_east = DL_MISSION_CIRCLE_center_east(dl_buffer); \ - float center_north = DL_MISSION_CIRCLE_center_north(dl_buffer); \ - float radius = DL_MISSION_CIRCLE_radius(dl_buffer); \ - float center_alt = DL_MISSION_CIRCLE_center_alt(dl_buffer); \ - float duration = DL_MISSION_CIRCLE_duration(dl_buffer); \ - mission_msg_CIRCLE(center_east, center_north, radius, center_alt, duration); \ - } \ -} - -#define ParseGotoMission() { \ - uint8_t ac_id = DL_GOTO_MISSION_ac_id(dl_buffer); \ - uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer); \ - goto_mission_msg(ac_id, mission_id); \ -} - -#endif // MISSION diff --git a/sw/airborne/modules/mission/mission_nav.c b/sw/airborne/modules/mission/mission_nav.c deleted file mode 100644 index 419a8fe1b5..0000000000 --- a/sw/airborne/modules/mission/mission_nav.c +++ /dev/null @@ -1,180 +0,0 @@ -/* - * Copyright (C) 2013 ENAC - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file mission_nav.h - * @brief mission navigation - */ - -#include -#include "modules/mission/mission_nav.h" -//#include "std.h" -//#include "subsystems/nav.h" - - -int mission_nav_GOTO_WP(float x, float y) { - fly_to_xy(x, y); - return FALSE; -} - -//int mission_nav_SEGMENT(float x1, float y1, float x2, float y2) { -bool_t mission_nav_SEGMENT(float x1, float y1, float x2, float y2) { - - if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { - //mission_nav_finish = 1; - return FALSE; // end of block and go to the next block - } - else { - nav_route_xy(x1, y1, x2, y2); - printf("doing mission SEGMENT ...\n\n");fflush(stdout); - } - return TRUE; -} - -//int mission_nav_CIRCLE(float x, float y, float radius){ -bool_t mission_nav_CIRCLE(float x, float y, float radius){ - nav_circle_XY(x, y, radius); - printf("doing mission CIRCLE ...\n\n");fflush(stdout); - return TRUE; -} - - - -int mission_path_seg_idx; -mission_path_seg_idx = 1; -//int mission_nav_PATH(struct _mission_path mission_path_nav) { -bool_t mission_nav_PATH(struct _mission_path mission_path_nav) { - float x1, y1, x2, y2, x3, y3, x4, y4, x5, y5; - switch (mission_path_seg_idx) { - case 1: // path 1-2 - x1 = mission_path_nav.path[0].x; - y1 = mission_path_nav.path[0].y; - x2 = mission_path_nav.path[1].x; - y2 = mission_path_nav.path[1].y; - nav_route_xy(x1, y1, x2, y2); - printf("doing mission PATH: path 1-2...\n\n");fflush(stdout); - if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) { - mission_path_seg_idx = 2; - } - break; - case 2: // path 2-3 - x2 = mission_path_nav.path[1].x; - y2 = mission_path_nav.path[1].y; - x3 = mission_path_nav.path[2].x; - y3 = mission_path_nav.path[2].y; - nav_route_xy(x2, y2, x3, y3); - printf("doing mission PATH: path 2-3...\n\n");fflush(stdout); - if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) { - mission_path_seg_idx = 3; - } - break; - case 3: // path 3-4 - x3 = mission_path_nav.path[2].x; - y3 = mission_path_nav.path[2].y; - x4 = mission_path_nav.path[3].x; - y4 = mission_path_nav.path[3].y; - nav_route_xy(x3, y3, x4, y4); - printf("doing mission PATH: path 3-4...\n\n");fflush(stdout); - if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) { - mission_path_seg_idx = 4; - } - break; - case 4: // path 4-5 - x4 = mission_path_nav.path[3].x; - y4 = mission_path_nav.path[3].y; - x5 = mission_path_nav.path[4].x; - y5 = mission_path_nav.path[4].y; - nav_route_xy(x4, y4, x5, y5); - printf("doing mission PATH: path 4-5...\n\n");fflush(stdout); - if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) { - //mission_nav_finish = 1; - return FALSE; // This path ends when AC arrive to point 5 - } - break; - } - - return TRUE; // do the function -} - - -bool_t mission_nav_return; -bool_t mission_nav_finish; -int i_mission; -int mission_call(struct _mission mission) { -//void mission_nav() { - //int i_mission; - // detect where the mission list is (by detecting duration non NULL)?????????????????????????????????????????????????????????????????????? - - float wp_mission_x, wp_mission_y; - float wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y; - float wp_center_x, wp_center_y, radius; - struct _mission_path mission_path_nav; - - switch (mission.elements[i_mission].type){ - case MissionWP: - wp_mission_x = mission.elements[i_mission].element.mission_wp.wp.x; - wp_mission_y = mission.elements[i_mission].element.mission_wp.wp.y; - //mission_nav_GOTO_WP(wp_mission_x, wp_mission_y); - mission_nav_GOTO_WP(wp_mission_x, wp_mission_y); - break; - - case MissionSegment: - wp_mission_from_x = mission.elements[i_mission].element.mission_segment.from.x; - wp_mission_from_y = mission.elements[i_mission].element.mission_segment.from.y; - wp_mission_to_x = mission.elements[i_mission].element.mission_segment.to.x; - wp_mission_to_y = mission.elements[i_mission].element.mission_segment.to.y; - //mission_nav_SEGMENT(wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y); - mission_nav_return = mission_nav_SEGMENT(wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y); - break; - - case MissionCircle: - wp_center_x = mission.elements[i_mission].element.mission_circle.center.x; - wp_center_y = mission.elements[i_mission].element.mission_circle.center.y; - radius = mission.elements[i_mission].element.mission_circle.radius; - //mission_nav_CIRCLE(wp_center_x, wp_center_y, radius); - mission_nav_return = mission_nav_CIRCLE(wp_center_x, wp_center_y, radius); - break; - - case MissionPath: - mission_path_nav = mission.elements[i_mission].element.mission_path; - //mission_nav_PATH(mission_path_nav); - mission_nav_return = mission_nav_PATH(mission_path_nav); - break; - } - - if (!(mission_nav_return)){ //when mission.elements is finished - printf("finish mission.elements %d\n\n\n", i_mission);fflush(stdout); - i_mission++; - printf("goto mission.elements %d\n\n\n", i_mission);fflush(stdout); - } - - - if (i_mission == MISSION_ELEMENT_NB) i_mission = 0; - return TRUE; /* do the function */ - -} - - - -int goto_mission_msg(uint8_t ac_id, uint8_t mission_id){ - i_mission = mission_id; - mission_call(mission); -} diff --git a/sw/airborne/modules/mission/mission_nav.h b/sw/airborne/modules/mission/mission_nav.h deleted file mode 100644 index 63509f8bc8..0000000000 --- a/sw/airborne/modules/mission/mission_nav.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2013 ENAC - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** @file mission_nav.h - * @brief mission navigation - */ - - -#include "modules/mission/mission_msg.h" -#include "std.h" -#include "subsystems/nav.h" - - -extern int mission_nav_GOTO_WP(float x, float y); -//extern int mission_nav_SEGMENT(float x1, float y1, float x2, float y2); -extern bool_t mission_nav_SEGMENT(float x1, float y1, float x2, float y2); - -//extern int mission_path_seg_idx; -//extern int i_seg; - -//extern int mission_nav_PATH_init( void ); -extern bool_t mission_nav_PATH(struct _mission_path mission_path_nav); - -extern bool_t mission_nav_return, mission_nav_finish; -//extern int mission_nav_return; -//extern int mission_nav_finish; -extern int i_mission; -extern int mission_call(struct _mission mission); - -//extern int goto_mission_msg(uint8_t ac_id, uint8_t mission_id); From d7b84baf60d9bb4ee8cb7835a64632a9c4ff46da Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 3 Oct 2013 14:31:44 +0200 Subject: [PATCH 105/125] [flightplan] add IndexOfBlock function to FP parser --- sw/lib/ocaml/expr_syntax.ml | 42 ++++++++++++++++++++++-------------- sw/lib/ocaml/expr_syntax.mli | 3 ++- sw/tools/gen_flight_plan.ml | 6 +++--- 3 files changed, 31 insertions(+), 20 deletions(-) diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml index d94a1cfe71..269f959b85 100644 --- a/sw/lib/ocaml/expr_syntax.ml +++ b/sw/lib/ocaml/expr_syntax.ml @@ -39,22 +39,32 @@ type expression = let c_var_of_ident = fun x -> "_var_" ^ x -let rec sprint = function -Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1))) - | Ident i -> sprintf "%s" i - | Int i -> sprintf "%d" i - | Float i -> sprintf "%f" i - | CallOperator (op, [e1;e2]) -> - sprintf "(%s%s%s)" (sprint e1) op (sprint e2) - | CallOperator (op, [e1]) -> - sprintf "%s(%s)" op (sprint e1) - | CallOperator (_,_) -> failwith "Operator should be binary or unary" - | Call (i, es) -> - let ses = List.map sprint es in - sprintf "%s(%s)" i (String.concat "," ses) - | Index (i,e) -> sprintf "%s[%s]" i (sprint e) - | Field (i,f) -> sprintf "%s.%s" i f - | Deref (e,f) -> sprintf "(%s)->%s" (sprint e) f +let sprint = fun ?call_assoc expr -> + let n, l = match call_assoc with + | None -> None, [] + | Some (n, l) -> Some n, l + in + let rec eval = function + | Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1))) + | Ident i -> sprintf "%s" i + | Int i -> sprintf "%d" i + | Float i -> sprintf "%f" i + | CallOperator (op, [e1;e2]) -> + sprintf "(%s%s%s)" (eval e1) op (eval e2) + | CallOperator (op, [e1]) -> + sprintf "%s(%s)" op (eval e1) + | CallOperator (_,_) -> failwith "Operator should be binary or unary" + | Call (i, [Ident s]) when Some i = n -> + let index = try List.assoc s l with Not_found -> failwith (sprintf "Unknown block: '%s'" s) in + sprintf "%d" index + | Call (i, es) -> + let ses = List.map eval es in + sprintf "%s(%s)" i (String.concat "," ses) + | Index (i,e) -> sprintf "%s[%s]" i (eval e) + | Field (i,f) -> sprintf "%s.%s" i f + | Deref (e,f) -> sprintf "(%s)->%s" (eval e) f + in + eval expr (* Valid functions : FIXME *) let functions = [ diff --git a/sw/lib/ocaml/expr_syntax.mli b/sw/lib/ocaml/expr_syntax.mli index ee57fbe537..46c6f30c23 100644 --- a/sw/lib/ocaml/expr_syntax.mli +++ b/sw/lib/ocaml/expr_syntax.mli @@ -37,7 +37,8 @@ type expression = val c_var_of_ident : ident -> string (** Encapsulate a user ident into a C variable *) -val sprint : expression -> string +val sprint : ?call_assoc:(ident * (ident * int) list) -> expression -> string +(** call_assoc: call an optional association list function *) exception Unknown_ident of string exception Unknown_operator of string diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 62449a3be6..f283e4786f 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -36,6 +36,8 @@ let georef_of_xml = fun xml -> let sof = string_of_float let soi = string_of_int +let index_of_blocks = ref [] + let check_expressions = ref false let cm = fun x -> 100. *. x @@ -55,7 +57,7 @@ let parse = fun s -> | Expr_syntax.Unknown_function x -> unexpected "function" x end end; - Expr_syntax.sprint e + Expr_syntax.sprint ~call_assoc:("IndexOfBlock", !index_of_blocks) e let parsed_attrib = fun xml a -> parse (ExtXml.attrib xml a) @@ -126,8 +128,6 @@ let print_waypoint_lla = fun utm0 default_alt waypoint -> printf " {%d, %d, %.0f}, /* 1e7deg, 1e7deg, cm (hmsl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (100. *. float_of_string alt) (Egm96.of_wgs84 wgs84) -let index_of_blocks = ref [] - let get_index_block = fun x -> try List.assoc x !index_of_blocks From 780c03c05acf93e687f7627b8af8f4a92b919fc4 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 3 Oct 2013 14:53:20 +0200 Subject: [PATCH 106/125] [generator] generate arrays of the settings and fp blocks names not used yet, but it can be later to send the settings and block names to the ground of to implement to associate a name to an index at runtime --- sw/tools/gen_flight_plan.ml | 12 +++--------- sw/tools/gen_settings.ml | 12 ++++-------- 2 files changed, 7 insertions(+), 17 deletions(-) diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index f283e4786f..ade969b63c 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -785,6 +785,9 @@ let () = lprintf "};\n"; Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); + Xml2h.define "FP_BLOCKS" "{ \\"; + List.iter (fun b -> printf " { \"%s\" }, \\\n" (ExtXml.attrib b "name")) blocks; + lprintf "};\n"; Xml2h.define "NB_BLOCK" (string_of_int (List.length blocks)); Xml2h.define "GROUND_ALT" (sof !ground_alt); @@ -794,15 +797,6 @@ let () = Xml2h.define "HOME_MODE_HEIGHT" (sof home_mode_height); Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh); - (** Print defines for blocks **) - lprintf "\n"; - let idx = ref 0 in - List.iter - (fun s -> - let v = ExtXml.attrib s "name" in - lprintf "#define BLOCK_%s %d\n" (Str.global_replace (Str.regexp "[^A-Za-z0-9]") "_" v) !idx; incr idx) blocks; - lprintf "\n"; - let index_of_waypoints = let i = ref (-1) in List.map (fun w -> incr i; (name_of w, !i)) waypoints in diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 2413805309..e959b8e400 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -69,14 +69,10 @@ let print_dl_settings = fun settings -> lprintf "\n"; (** Datalink knowing what settings mean **) - let idx = ref 0 in - lprintf "\n"; - List.iter - (fun s -> - let v = ExtXml.attrib s "var" in - lprintf "#define SETTINGS_%s %d\n" (Str.global_replace (Str.regexp "[^A-Za-z0-9]") "_" v) !idx; incr idx) - settings; - lprintf "\n"; + Xml2h.define "SETTINGS" "{ \\"; + List.iter (fun b -> printf " { \"%s\" }, \\\n" (ExtXml.attrib b "var")) settings; + lprintf "};\n"; + Xml2h.define "NB_SETTING" (string_of_int (List.length settings)); (** Macro to call to set one variable *) lprintf "#define DlSetting(_idx, _value) { \\\n"; From ddab784efb846887fb1dff643dd56c31b00d3f84 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 3 Oct 2013 22:24:23 +0200 Subject: [PATCH 107/125] [mission] fix segmentation fault and add generic mission FP --- conf/flight_plans/mission_fw.xml | 63 ++++++++++++++++++++ sw/airborne/modules/mission/mission.c | 8 +-- sw/airborne/modules/mission/mission.h | 5 +- sw/airborne/modules/mission/mission_fw_nav.c | 2 +- 4 files changed, 69 insertions(+), 9 deletions(-) create mode 100644 conf/flight_plans/mission_fw.xml diff --git a/conf/flight_plans/mission_fw.xml b/conf/flight_plans/mission_fw.xml new file mode 100644 index 0000000000..daef0aaf3d --- /dev/null +++ b/conf/flight_plans/mission_fw.xml @@ -0,0 +1,63 @@ + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/mission/mission.c b/sw/airborne/modules/mission/mission.c index dc20909f2f..d3794bbca9 100644 --- a/sw/airborne/modules/mission/mission.c +++ b/sw/airborne/modules/mission/mission.c @@ -76,13 +76,11 @@ bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * e } -bool_t mission_get(struct _mission_element * element __attribute__((unused))) { +struct _mission_element * mission_get(void) { if (mission.current_idx == mission.insert_idx) { - element = NULL; - return FALSE; + return NULL; } - element = &(mission.elements[mission.current_idx]); - return TRUE; + return &(mission.elements[mission.current_idx]); } diff --git a/sw/airborne/modules/mission/mission.h b/sw/airborne/modules/mission/mission.h index 14cd363ea4..d423c48c12 100644 --- a/sw/airborne/modules/mission/mission.h +++ b/sw/airborne/modules/mission/mission.h @@ -104,10 +104,9 @@ extern void mission_init(void); extern bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element); /** Get current mission element - * @param element pointer to the current mission element - * @return TRUE if valid element, FALSE if mission queue is empty (add element is set to NULL) + * @return return a pointer to the next mission element or NULL if no more elements */ -extern bool_t mission_get(struct _mission_element * element); +extern struct _mission_element * mission_get(void); /** Run mission * diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c index 7bc3494a0d..f0b24e8094 100644 --- a/sw/airborne/modules/mission/mission_fw_nav.c +++ b/sw/airborne/modules/mission/mission_fw_nav.c @@ -105,7 +105,7 @@ static inline bool_t mission_nav_path(struct _mission_path * path) { int mission_run() { // current element struct _mission_element * el = NULL; - if (!mission_get(el)) { + if ((el = mission_get()) == NULL) { // TODO do something special like a waiting circle before ending the mission ? return FALSE; // end of mission } From 1008f946d82154579cd65726633afca3da079cc4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 5 Oct 2013 13:19:08 +0200 Subject: [PATCH 108/125] [lib/ocaml] update leap_seconds most recent leap second was inserted on June 30, 2012 at 23:59:60 UTC So since the gps epoch 1980 we have 16 leap seconds. closes #549 --- sw/lib/ocaml/latlong.ml | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml index 1e7e3d689a..5862626830 100644 --- a/sw/lib/ocaml/latlong.ml +++ b/sw/lib/ocaml/latlong.ml @@ -469,9 +469,16 @@ let bearing = fun geo1 geo2 -> ((Rad>>Deg)(atan2 dx dy), sqrt(dx*.dx+.dy*.dy)) -let leap_seconds = 15 (* http://www.leapsecond.com/java/gpsclock.htm *) +(** Offset between GPS and UTC times in seconds. + * Update when a new leap second is inserted and be careful about times in the + * past when this offset was different. + * Last leap second was inserted on June 30, 2012 at 23:59:60 UTC + * http://www.leapsecond.com/java/gpsclock.htm + *) +let leap_seconds = 16 -let gps_epoch = 315964800. (* In seconds, in the unix reference *) +(** Unix timestamp of the GPS epoch 1980-01-06 00:00:00 UTC *) +let gps_epoch = 315964800. let gps_tow_of_utc = fun ?wday hour min sec -> let wday = From a22eb864312ac40f342c3d217864a7e7c77481bd Mon Sep 17 00:00:00 2001 From: hendrixgr Date: Mon, 7 Oct 2013 11:14:09 +0300 Subject: [PATCH 109/125] [boards] update stm32f4_discovery --- conf/boards/stm32f4_discovery.makefile | 6 +- sw/airborne/boards/stm32f4_discovery.h | 503 +++++++++++++++++++++---- 2 files changed, 441 insertions(+), 68 deletions(-) diff --git a/conf/boards/stm32f4_discovery.makefile b/conf/boards/stm32f4_discovery.makefile index f00700c783..1961c9c387 100644 --- a/conf/boards/stm32f4_discovery.makefile +++ b/conf/boards/stm32f4_discovery.makefile @@ -28,8 +28,8 @@ DFU_UTIL ?= y # RADIO_CONTROL_LED ?= 4 BARO_LED ?= none -AHRS_ALIGNER_LED ?= none -GPS_LED ?= none +AHRS_ALIGNER_LED ?= 5 +GPS_LED ?= 6 SYS_TIME_LED ?= 3 # @@ -39,7 +39,7 @@ SYS_TIME_LED ?= 3 MODEM_PORT ?= UART6 MODEM_BAUD ?= B57600 -GPS_PORT ?= UART4 +GPS_PORT ?= UART3 GPS_BAUD ?= B38400 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h index 288385d132..4884eb932d 100755 --- a/sw/airborne/boards/stm32f4_discovery.h +++ b/sw/airborne/boards/stm32f4_discovery.h @@ -1,3 +1,96 @@ +/* + * STM32F4 DISCOVERY BOARD DEFINITIONS FOR THE PAPARAZZI AUTOPILOT. + * + * DISCOVERY USED FOR THE AUTOPILOT PINS + * PA0 = PWM9 + user-wake up button switch b1 normaly open. + * PA1 = PWM8 + * PA2 = PWM7 + * PA3 = PWM6 + * PA4 = ADC_4 (ADC12 IN 4)+CS43L22 DAC out, capacitively coupled+100Kohm, should not interfere. + * PA5 = FREE + * PA6 = CANNOT BE USED + * PA7 = FREE + * PA8 = SPECTRUM BIND + * PA9 = FREE (ONLY if usb is not active during runtime, PC0 must be high or input ) + * PA10 = UART2 + * PA11 = FREE if usb is not active during runtime + * PA12 = FREE if usb is not active during runtime + * PA13 = FREE + * PA14 = FREE + * PA15 = FREE + * + * PB0 = FREE + * PB1 = ADC_1 (ADC12 IN 9) + * PB2 = FREE + * PB3 = SPI1 SKL + * PB4 = SPI1 MISO + * PB5 = SPI1 MOSI + * PB6 = UART1 TX + 4K7 Ohm pull up resistor normaly used for i2c + * PB7 = UART1 RX + * PB8 = I2C1 SCL + * PB9 = I2C1 SDA + 4K7 Ohm pull up resistor normaly used for i2c + * PB10 = I2C2 SCL + MP45DT02 MEMS MIC clock in + * PB11 = I2C2 SDA + * PB12 = FREE + * PB13 = SPI2 SCL + * PB14 = PWM 10 OR SPI2 MISO + * PB15 = PWM 11 OR SPI2 MOSI + * + * PC0 = CANNOT BE USED, pulled up to 3.3v (10Kohm), low = usb power on directly applied to PA9) + * PC1 = ADC_5 (ADC123 IN 11) + * PC2 = ADC_6 (ADC123 IN 12) + * PC3 = CANNOT BE USED (MEMS microphone output) + * PC4 = ADC_3 (ADC12 IN 14) + * PC5 = ADC_2 (ADC12 IN 15) + * PC6 = UART6 TX + * PC7 = UART6 RX + * PC8 = FREE + * PC9 = PPM INPUT FROM RC RECEIVER + * PC10 = UART4 TX OR SPI3 SCL + * PC11 = UART4 RX OR SPI3 MISO + * PC12 = UART5 TX OR SPI3 MOSI + * PC13 = FREE max 3ma sink + * PC14 = FREE max 3ma sink, short circuit the relevant PCB bridge with solder to activate + * PC15 = FREE max 3ma sink, short circuit the relevant PCB bridge with solder to activate + * + * PD0 = FREE + * PD1 = FREE + * PD2 = UART5 RX + * PD3 = FREE + * PD4 = FREE + * PD5 = UART2 TX (usb power management fault output open drain) + * PD6 = UART2 RX + * PD7 = FREE + * PD8 = UART3 TX + * PD9 = UART3 RX + * PD10 = FREE + * PD11 = FREE + * PD12 = LED_3 + * PD13 = LED_4 + * PD14 = LED_5 + * PD15 = LED_6 + * + * PE0 = CANNOT BE USED (Accel int output) + * PE1 = CANNOT BE USED (Accel int output) + * PE2 = SPI SLAVE 0 + * PE3 = FREE (Needs some testing as it is used for the accel spi/i2c select pin) + * PE4 = FREE + * PE5 = PWM4 + * PE6 = PWM5 + * PE7 = SPI SLAVE 1 + * PE8 = FREE + * PE9 = PWM0 + * PE10 = FREE + * PE11 = PWM1 + * PE12 = FREE + * PE13 = PWM2 + * PE14 = PWM3 + * PE15 = FREE + * + * PH0 = CRYSTAL - OSC IN + * PH1 = CRYSTAL - OSC OUT + */ + #ifndef CONFIG_STM32F4_DISCOVERY_H #define CONFIG_STM32F4_DISCOVERY_H @@ -17,7 +110,7 @@ #endif #define LED_3_GPIO GPIOD #define LED_3_GPIO_CLK RCC_AHB1ENR_IOPDEN -#define LED_3_GPIO_PIN GPIO13 +#define LED_3_GPIO_PIN GPIO12 #define LED_3_AFIO_REMAP ((void)0) #define LED_3_GPIO_ON gpio_set #define LED_3_GPIO_OFF gpio_clear @@ -28,7 +121,7 @@ #endif #define LED_4_GPIO GPIOD #define LED_4_GPIO_CLK RCC_AHB1ENR_IOPDEN -#define LED_4_GPIO_PIN GPIO12 +#define LED_4_GPIO_PIN GPIO13 #define LED_4_AFIO_REMAP ((void)0) #define LED_4_GPIO_ON gpio_set #define LED_4_GPIO_OFF gpio_clear @@ -56,46 +149,107 @@ #define LED_6_GPIO_OFF gpio_clear -/* UART */ + +/* +// LED 9 (Green) of the STM32F4 discovery board same as USB power (VBUS). +#ifndef USE_LED_9 +#define USE_LED_9 1 +#endif +#define LED_9_GPIO GPIOA +#define LED_9_GPIO_CLK RCC_AHB1ENR_IOPAEN +#define LED_9_GPIO_PIN GPIO9 +#define LED_9_AFIO_REMAP ((void)0) +#define LED_9_GPIO_ON gpio_set +#define LED_9_GPIO_OFF gpio_clear +*/ + + +/***************************************************************************************************/ +/************************************** UART *************************************************/ +/***************************************************************************************************/ + +// If you don't need all those uarts comment a uart block out to free pins. #define UART1_GPIO_AF GPIO_AF7 -#define UART1_GPIO_PORT_RX GPIOA -#define UART1_GPIO_RX GPIO10 #define UART1_GPIO_PORT_TX GPIOB #define UART1_GPIO_TX GPIO6 +#define UART1_GPIO_PORT_RX GPIOB +#define UART1_GPIO_RX GPIO7 +// UART2 is used for the Spectrum rx input also #define UART2_GPIO_AF GPIO_AF7 -#define UART2_GPIO_PORT_RX GPIOA -#define UART2_GPIO_RX GPIO3 -#define UART2_GPIO_PORT_TX GPIOA -#define UART2_GPIO_TX GPIO2 +#define UART2_GPIO_PORT_TX GPIOD +#define UART2_GPIO_TX GPIO5 +#define UART2_GPIO_PORT_RX GPIOD +#define UART2_GPIO_RX GPIO6 +#define UART3_GPIO_AF GPIO_AF7 +#define UART3_GPIO_PORT_TX GPIOD +#define UART3_GPIO_TX GPIO8 +#define UART3_GPIO_PORT_RX GPIOD +#define UART3_GPIO_RX GPIO9 + +// UARTS 4 AND 5 CANT BE USED IF SPI3 IS NEEDED +#if !USE_SPI3 #define UART4_GPIO_AF GPIO_AF8 -#define UART4_GPIO_PORT_RX GPIOA -#define UART4_GPIO_RX GPIO1 -#define UART4_GPIO_PORT_TX GPIOA -#define UART4_GPIO_TX GPIO0 +#define UART4_GPIO_PORT_TX GPIOC +#define UART4_GPIO_TX GPIO10 +#define UART4_GPIO_PORT_RX GPIOC +#define UART4_GPIO_RX GPIO11 + +#define UART5_GPIO_AF GPIO_AF8 +#define UART5_GPIO_PORT_TX GPIOC +#define UART5_GPIO_TX GPIO12 +#define UART5_GPIO_PORT_RX GPIOD +#define UART5_GPIO_RX GPIO2 +#endif #define UART6_GPIO_AF GPIO_AF8 -#define UART6_GPIO_PORT_RX GPIOC -#define UART6_GPIO_RX GPIO7 #define UART6_GPIO_PORT_TX GPIOC #define UART6_GPIO_TX GPIO6 +#define UART6_GPIO_PORT_RX GPIOC +#define UART6_GPIO_RX GPIO7 -/* SPI */ +/***************************************************************************************************/ +/************************************** SPI *************************************************/ +/***************************************************************************************************/ + #define SPI1_GPIO_AF GPIO_AF5 -#define SPI1_GPIO_PORT_MISO GPIOA -#define SPI1_GPIO_MISO GPIO6 -#define SPI1_GPIO_PORT_MOSI GPIOA -#define SPI1_GPIO_MOSI GPIO7 -#define SPI1_GPIO_PORT_SCK GPIOA -#define SPI1_GPIO_SCK GPIO5 - -#define SPI_SELECT_SLAVE0_PORT GPIOB -#define SPI_SELECT_SLAVE0_PIN GPIO9 +#define SPI1_GPIO_PORT_SCK GPIOB +#define SPI1_GPIO_SCK GPIO3 +#define SPI1_GPIO_PORT_MISO GPIOB +#define SPI1_GPIO_MISO GPIO4 +#define SPI1_GPIO_PORT_MOSI GPIOB +#define SPI1_GPIO_MOSI GPIO5 -/* I2C mapping */ +/* CANNOT BE USED IF PWM CHANNELS 10 & 11 ARE ACTIVE !!! */ +#define SPI2_GPIO_AF GPIO_AF5 +#define SPI2_GPIO_PORT_SCK GPIOB +#define SPI2_GPIO_SCK GPIO13 +#define SPI2_GPIO_PORT_MISO GPIOB +#define SPI2_GPIO_MISO GPIO14 +#define SPI2_GPIO_PORT_MOSI GPIOB +#define SPI2_GPIO_MOSI GPIO15 + +/* CANNOT BE USED IF UARTS 4 & 5 ARE ACTIVE !!! */ +#define SPI3_GPIO_AF GPIO_AF6 +#define SPI3_GPIO_PORT_SCK GPIOC +#define SPI3_GPIO_SCK GPIO10 +#define SPI3_GPIO_PORT_MISO GPIOC +#define SPI3_GPIO_MISO GPIO11 +#define SPI3_GPIO_PORT_MOSI GPIOC +#define SPI3_GPIO_MOSI GPIO12 + +#define SPI_SELECT_SLAVE0_PORT GPIOE +#define SPI_SELECT_SLAVE0_PIN GPIO2 +#define SPI_SELECT_SLAVE1_PORT GPIOE +#define SPI_SELECT_SLAVE1_PIN GPIO7 + + +/***************************************************************************************************/ +/************************************** I2C *************************************************/ +/***************************************************************************************************/ #define I2C1_GPIO_PORT GPIOB #define I2C1_GPIO_SCL GPIO8 #define I2C1_GPIO_SDA GPIO9 @@ -104,47 +258,16 @@ #define I2C2_GPIO_SCL GPIO10 #define I2C2_GPIO_SDA GPIO11 -#define I2C3_GPIO_PORT_SCL GPIOA -#define I2C3_GPIO_PORT_SDA GPIOC -#define I2C3_GPIO_SCL GPIO8 -#define I2C3_GPIO_SDA GPIO9 +//#define I2C3_GPIO_PORT_SCL GPIOA +//#define I2C3_GPIO_SCL GPIO8 +//#define I2C3_GPIO_PORT_SDA GPIOC +//#define I2C3_GPIO_SDA GPIO9 -/* - * PPM - */ -#define USE_PPM_TIM1 1 +/***************************************************************************************************/ +/************************************** ADC *************************************************/ +/***************************************************************************************************/ -#define PPM_CHANNEL TIM_IC1 -#define PPM_TIMER_INPUT TIM_IC_IN_TI1 -#define PPM_IRQ NVIC_TIM1_CC_IRQ -#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ -// Capture/Compare InteruptEnable and InterruptFlag -#define PPM_CC_IE TIM_DIER_CC1IE -#define PPM_CC_IF TIM_SR_CC1IF -#define PPM_GPIO_PORT GPIOA -#define PPM_GPIO_PIN GPIO8 -#define PPM_GPIO_AF GPIO_AF1 - - -/* - * Spektrum - */ -/* The line that is pulled low at power up to initiate the bind process */ -#define SPEKTRUM_BIND_PIN GPIO8 -#define SPEKTRUM_BIND_PIN_PORT GPIOA - -#define SPEKTRUM_UART2_RCC_REG &RCC_APB2ENR -#define SPEKTRUM_UART2_RCC_DEV RCC_APB2ENR_USART1EN -#define SPEKTRUM_UART2_BANK GPIOA -#define SPEKTRUM_UART2_PIN GPIO10 -#define SPEKTRUM_UART2_AF GPIO_AF7 -#define SPEKTRUM_UART2_IRQ NVIC_USART1_IRQ -#define SPEKTRUM_UART2_ISR usart1_isr -#define SPEKTRUM_UART2_DEV USART1 - - -/* Onboard ADCs */ #define USE_AD_TIM4 1 #define BOARD_ADC_CHANNEL_1 9 @@ -155,10 +278,12 @@ #ifndef USE_AD1 #define USE_AD1 1 #endif + /* provide defines that can be used to access the ADC_x in the code or airframe file * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ + // AUX 1 #define ADC_1 ADC1_C1 #ifdef USE_ADC_1 @@ -206,7 +331,7 @@ #endif #define USE_AD1_4 1 -/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file */ #ifndef ADC_CHANNEL_VSUPPLY #define ADC_CHANNEL_VSUPPLY ADC_4 #endif @@ -226,6 +351,10 @@ #define DefaultVoltageOfAdc(adc) (0.00485*adc) +/***************************************************************************************************/ +/************************************ ACTUATORS ************************************************/ +/***************************************************************************************************/ + /* Default actuators driver */ #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" #define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) @@ -233,4 +362,248 @@ #define ActuatorsDefaultCommit() ActuatorsPwmCommit() +/***************************************************************************************************/ +/********************************** SERVO PWM *************************************************/ +/***************************************************************************************************/ + +#define PWM_USE_TIM1 1 +//#define PWM_USE_TIM3 1 +#define PWM_USE_TIM5 1 +#define PWM_USE_TIM9 1 +#define PWM_USE_TIM12 1 + +#define USE_PWM0 1 +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 +#define USE_PWM5 1 +#define USE_PWM6 1 +#define USE_PWM7 1 +#define USE_PWM8 1 +#define USE_PWM9 1 +#if USE_SPI2 +#define USE_PWM10 0 +#define USE_PWM11 0 +#else +#define USE_PWM10 1 +#define USE_PWM11 1 +#endif + +#define ACTUATORS_PWM_NB 12 + +// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array +#if USE_PWM0 +#define PWM_SERVO_0 0 +#define PWM_SERVO_0_TIMER TIM1 +#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_0_GPIO GPIOE +#define PWM_SERVO_0_PIN GPIO9 +#define PWM_SERVO_0_AF GPIO_AF1 +#define PWM_SERVO_0_OC TIM_OC1 +#define PWM_SERVO_0_OC_BIT (1<<0) +#else +#define PWM_SERVO_0_OC_BIT 0 +#endif + +#if USE_PWM1 +#define PWM_SERVO_1 1 +#define PWM_SERVO_1_TIMER TIM1 +#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_1_GPIO GPIOE +#define PWM_SERVO_1_PIN GPIO11 +#define PWM_SERVO_1_AF GPIO_AF1 +#define PWM_SERVO_1_OC TIM_OC2 +#define PWM_SERVO_1_OC_BIT (1<<1) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 2 +#define PWM_SERVO_2_TIMER TIM1 +#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_2_GPIO GPIOE +#define PWM_SERVO_2_PIN GPIO13 +#define PWM_SERVO_2_AF GPIO_AF1 +#define PWM_SERVO_2_OC TIM_OC3 +#define PWM_SERVO_2_OC_BIT (1<<2) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 3 +#define PWM_SERVO_3_TIMER TIM1 +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_3_GPIO GPIOE +#define PWM_SERVO_3_PIN GPIO14 +#define PWM_SERVO_3_AF GPIO_AF1 +#define PWM_SERVO_3_OC TIM_OC4 +#define PWM_SERVO_3_OC_BIT (1<<3) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 4 +#define PWM_SERVO_4_TIMER TIM9 +#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_4_GPIO GPIOE +#define PWM_SERVO_4_PIN GPIO5 +#define PWM_SERVO_4_AF GPIO_AF3 +#define PWM_SERVO_4_OC TIM_OC1 +#define PWM_SERVO_4_OC_BIT (1<<0) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 5 +#define PWM_SERVO_5_TIMER TIM9 +#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPEEN +#define PWM_SERVO_5_GPIO GPIOE +#define PWM_SERVO_5_PIN GPIO6 +#define PWM_SERVO_5_AF GPIO_AF3 +#define PWM_SERVO_5_OC TIM_OC2 +#define PWM_SERVO_5_OC_BIT (1<<1) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + + +#if USE_PWM6 +#define PWM_SERVO_6 6 +#define PWM_SERVO_6_TIMER TIM5 +#define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO3 +#define PWM_SERVO_6_AF GPIO_AF2 +#define PWM_SERVO_6_OC TIM_OC4 +#define PWM_SERVO_6_OC_BIT (1<<3) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + +#if USE_PWM7 +#define PWM_SERVO_7 7 +#define PWM_SERVO_7_TIMER TIM5 +#define PWM_SERVO_7_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_7_GPIO GPIOA +#define PWM_SERVO_7_PIN GPIO2 +#define PWM_SERVO_7_AF GPIO_AF2 +#define PWM_SERVO_7_OC TIM_OC3 +#define PWM_SERVO_7_OC_BIT (1<<2) +#else +#define PWM_SERVO_7_OC_BIT 0 +#endif + +#if USE_PWM8 +#define PWM_SERVO_8 8 +#define PWM_SERVO_8_TIMER TIM5 +#define PWM_SERVO_8_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_8_GPIO GPIOA +#define PWM_SERVO_8_PIN GPIO1 +#define PWM_SERVO_8_AF GPIO_AF2 +#define PWM_SERVO_8_OC TIM_OC2 +#define PWM_SERVO_8_OC_BIT (1<<1) +#else +#define PWM_SERVO_8_OC_BIT 0 +#endif + +#if USE_PWM9 +#define PWM_SERVO_9 9 +#define PWM_SERVO_9_TIMER TIM5 +#define PWM_SERVO_9_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_9_GPIO GPIOA +#define PWM_SERVO_9_PIN GPIO0 +#define PWM_SERVO_9_AF GPIO_AF2 +#define PWM_SERVO_9_OC TIM_OC1 +#define PWM_SERVO_9_OC_BIT (1<<0) +#else +#define PWM_SERVO_9_OC_BIT 0 +#endif + +/* PWM10 AND PWM11 cannot be used if SPI2 is active. */ +#if USE_PWM10 +#define PWM_SERVO_10 10 +#define PWM_SERVO_10_TIMER TIM12 +#define PWM_SERVO_10_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_10_GPIO GPIOB +#define PWM_SERVO_10_PIN GPIO14 +#define PWM_SERVO_10_AF GPIO_AF9 +#define PWM_SERVO_10_OC TIM_OC1 +#define PWM_SERVO_10_OC_BIT (1<<0) +#else +#define PWM_SERVO_10_OC_BIT 0 +#endif + +#if USE_PWM11 +#define PWM_SERVO_11 11 +#define PWM_SERVO_11_TIMER TIM12 +#define PWM_SERVO_11_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_11_GPIO GPIOB +#define PWM_SERVO_11_PIN GPIO15 +#define PWM_SERVO_11_AF GPIO_AF9 +#define PWM_SERVO_11_OC TIM_OC2 +#define PWM_SERVO_11_OC_BIT (1<<1) +#else +#define PWM_SERVO_11_OC_BIT 0 +#endif + +#define PWM_TIM1_CHAN_MASK (PWM_SERVO_0_OC_BIT|PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT) +#define PWM_TIM9_CHAN_MASK (PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT) +#define PWM_TIM5_CHAN_MASK (PWM_SERVO_6_OC_BIT|PWM_SERVO_7_OC_BIT|PWM_SERVO_8_OC_BIT|PWM_SERVO_9_OC_BIT) +#define PWM_TIM12_CHAN_MASK (PWM_SERVO_10_OC_BIT|PWM_SERVO_11_OC_BIT) + +/***************************************************************************************************/ +/*********************************** PPM INPUT *************************************************/ +/***************************************************************************************************/ + +/* +#define USE_PPM_TIM1 1 + +#define PPM_CHANNEL TIM_IC1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_IRQ NVIC_TIM1_CC_IRQ +#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC1IE +#define PPM_CC_IF TIM_SR_CC1IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO8 +#define PPM_GPIO_AF GPIO_AF1 +*/ + +#define USE_PPM_TIM8 1 + +#define PPM_CHANNEL TIM_IC4 +#define PPM_TIMER_INPUT TIM_IC_IN_TI4 +#define PPM_IRQ NVIC_TIM8_CC_IRQ +#define PPM_IRQ2 NVIC_TIM8_UP_TIM13_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC4IE +#define PPM_CC_IF TIM_SR_CC4IF +#define PPM_GPIO_PORT GPIOC +#define PPM_GPIO_PIN GPIO9 +#define PPM_GPIO_AF GPIO_AF3 + +/***************************************************************************************************/ +/********************************* SPECTRUM UART *************************************************/ +/***************************************************************************************************/ + +/* The line that is pulled low at power up to initiate the bind process */ +#define SPEKTRUM_BIND_PIN GPIO8 +#define SPEKTRUM_BIND_PIN_PORT GPIOA + +#define SPEKTRUM_UART2_RCC_REG &RCC_APB2ENR +#define SPEKTRUM_UART2_RCC_DEV RCC_APB2ENR_USART1EN +#define SPEKTRUM_UART2_BANK GPIOA +#define SPEKTRUM_UART2_PIN GPIO10 +#define SPEKTRUM_UART2_AF GPIO_AF7 +#define SPEKTRUM_UART2_IRQ NVIC_USART1_IRQ +#define SPEKTRUM_UART2_ISR usart1_isr +#define SPEKTRUM_UART2_DEV USART1 + + #endif /* CONFIG_STM32F4_DISCOVERY_H */ From 49ab9baa7b51a33170aa180c858852b482e577a7 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 7 Oct 2013 18:44:24 +0200 Subject: [PATCH 110/125] [conf] remove baro_board.xml module file --- conf/modules/baro_board.xml | 18 ------------------ 1 file changed, 18 deletions(-) delete mode 100644 conf/modules/baro_board.xml diff --git a/conf/modules/baro_board.xml b/conf/modules/baro_board.xml deleted file mode 100644 index b5c56db26f..0000000000 --- a/conf/modules/baro_board.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - Baro board wrapper. - Allows to use baro interface on fixedwing with external barometers. - - -
- -
- - - - - - From bfe0072f99fde8161608ce66f499ec8dcaca7a9c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 7 Oct 2013 18:44:49 +0200 Subject: [PATCH 111/125] [modules] baro modules for real sensors only for ap target, not sim --- conf/modules/baro_amsys.xml | 2 +- conf/modules/baro_ets.xml | 2 +- conf/modules/baro_hca.xml | 2 +- sw/airborne/modules/sensors/baro_amsys.c | 20 -------------------- sw/airborne/modules/sensors/baro_bmp.h | 4 ---- sw/airborne/modules/sensors/baro_ets.c | 16 +--------------- 6 files changed, 4 insertions(+), 42 deletions(-) diff --git a/conf/modules/baro_amsys.xml b/conf/modules/baro_amsys.xml index 2b94bb226c..7b7f72f3d4 100644 --- a/conf/modules/baro_amsys.xml +++ b/conf/modules/baro_amsys.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/modules/baro_ets.xml b/conf/modules/baro_ets.xml index 0b59c2a45b..9323145872 100644 --- a/conf/modules/baro_ets.xml +++ b/conf/modules/baro_ets.xml @@ -31,7 +31,7 @@ - + diff --git a/conf/modules/baro_hca.xml b/conf/modules/baro_hca.xml index d874c6388c..3a368f48d8 100644 --- a/conf/modules/baro_hca.xml +++ b/conf/modules/baro_hca.xml @@ -13,7 +13,7 @@ - + diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index c92edb3f1e..be9946b94d 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -30,12 +30,6 @@ #include #include "generated/flight_plan.h" // for ground alt -#ifdef SITL -#include "subsystems/gps.h" -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_isa.h" -#endif - //Messages #include "mcu_periph/uart.h" #include "messages.h" @@ -128,7 +122,6 @@ void baro_amsys_init( void ) { void baro_amsys_read_periodic( void ) { // Initiate next read -#ifndef SITL if (baro_amsys_i2c_trans.status == I2CTransDone){ #ifndef MEASURE_AMSYS_TEMPERATURE i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); @@ -136,19 +129,6 @@ void baro_amsys_read_periodic( void ) { i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); #endif } -#else // SITL - /* fake an offset so sim works for under hmsl as well */ - if (!baro_amsys_offset_init) { - baro_amsys_offset = BARO_AMSYS_OFFSET_MAX; - baro_amsys_offset_init = TRUE; - } - pBaroRaw = 0; - baro_amsys_altitude = gps.hmsl / 1000.0; - baro_amsys_p = pprz_isa_pressure_of_altitude(baro_amsys_altitude); - baro_amsys_adc = baro_amsys_p; - AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); - baro_amsys_valid = TRUE; -#endif #ifdef BARO_AMSYS_SYNC_SEND DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp); diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h index b36d292f6d..aa56e10266 100644 --- a/sw/airborne/modules/sensors/baro_bmp.h +++ b/sw/airborne/modules/sensors/baro_bmp.h @@ -36,11 +36,7 @@ extern struct Bmp085 baro_bmp; /// new measurement every 3rd baro_bmp_periodic -#ifndef SITL #define BARO_BMP_DT (BARO_BMP_PERIODIC_PERIOD / 3) -#else -#define BARO_BMP_DT BARO_BMP_PERIODIC_PERIOID -#endif extern bool_t baro_bmp_enabled; extern float baro_bmp_r; diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 3e4c9f2cd5..98333d4767 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -48,10 +48,6 @@ #include "subsystems/nav.h" -#ifdef SITL -#include "subsystems/gps.h" -#endif - #ifdef BARO_ETS_SYNC_SEND #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -133,23 +129,13 @@ void baro_ets_init( void ) { void baro_ets_read_periodic( void ) { // Initiate next read -#ifndef SITL if (!baro_ets_delay_done) { if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return; else baro_ets_delay_done = TRUE; } - if (baro_ets_i2c_trans.status == I2CTransDone) + if (baro_ets_i2c_trans.status == I2CTransDone) { i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2); -#else // SITL - /* fake an offset so sim works as well */ - if (!baro_ets_offset_init) { - baro_ets_offset = 12400; - baro_ets_offset_init = TRUE; } - baro_ets_altitude = gps.hmsl / 1000.0; - baro_ets_adc = baro_ets_offset - ((baro_ets_altitude - ground_alt) / BARO_ETS_SCALE); - baro_ets_valid = TRUE; -#endif #ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); From c81d770fd9c2d212131174f45616c53195f00164 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 7 Oct 2013 19:13:57 +0200 Subject: [PATCH 112/125] [modules] baro_ets: BARO_ETS_SCALE Conversion of ADC to Pascal should be roughly correct now, not tested though. --- .../examples/Twinstar_energyadaptive.xml | 3 ++- .../examples/stm32f4_discovery_test.xml | 3 ++- conf/modules/baro_ets.xml | 4 ++- sw/airborne/modules/sensors/baro_ets.c | 27 ++++++++++++------- 4 files changed, 25 insertions(+), 12 deletions(-) diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index b65867a8df..d0f83f5147 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -50,7 +50,8 @@ twog_1.0 + aspirin + ETS baro + ETS speed - + + diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml index 0ec68e55f3..9bb579254b 100644 --- a/conf/airframes/examples/stm32f4_discovery_test.xml +++ b/conf/airframes/examples/stm32f4_discovery_test.xml @@ -31,7 +31,8 @@ - + + diff --git a/conf/modules/baro_ets.xml b/conf/modules/baro_ets.xml index 9323145872..ec53f74a56 100644 --- a/conf/modules/baro_ets.xml +++ b/conf/modules/baro_ets.xml @@ -19,7 +19,9 @@ - + + + diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 98333d4767..6da9cb8e80 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -60,9 +60,6 @@ #define BARO_ETS_ADDR 0xE8 #define BARO_ETS_REG 0x07 -#ifndef BARO_ETS_SCALE -#define BARO_ETS_SCALE 0.32 -#endif #define BARO_ETS_OFFSET_MAX 30000 #define BARO_ETS_OFFSET_MIN 10 #define BARO_ETS_OFFSET_NBSAMPLES_INIT 20 @@ -70,7 +67,23 @@ #define BARO_ETS_R 0.5 #define BARO_ETS_SIGMA2 0.1 -// Pressure offset to convert raw adc to real pressure (FIXME find real value) +/** scale factor to convert raw ADC measurement to pressure in Pascal. + * @todo check value + * At low altitudes pressure change is ~1.2 kPa for every 100 meters. + * So with a scale ADC->meters of 0.32 we get: + * 12 Pascal = 0.32 * ADC + * -> SCALE = ~ 12 / 0.32 = 37.5 + */ +#ifndef BARO_ETS_SCALE +#define BARO_ETS_SCALE 37.5 +#endif + +/** scale factor to convert raw ADC measurement to altitude change in meters */ +#ifndef BARO_ETS_ALT_SCALE +#define BARO_ETS_ALT_SCALE 0.32 +#endif + +/** Pressure offset in Pascal when converting raw adc to real pressure (@todo find real value) */ #ifndef BARO_ETS_PRESSURE_OFFSET #define BARO_ETS_PRESSURE_OFFSET 101325.0 #endif @@ -136,10 +149,6 @@ void baro_ets_read_periodic( void ) { if (baro_ets_i2c_trans.status == I2CTransDone) { i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2); } - -#ifdef BARO_ETS_SYNC_SEND - DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); -#endif } void baro_ets_read_event( void ) { @@ -173,7 +182,7 @@ void baro_ets_read_event( void ) { } // Convert raw to m/s if (baro_ets_offset_init) { - baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); + baro_ets_altitude = ground_alt + BARO_ETS_ALT_SCALE * (float)(baro_ets_offset-baro_ets_adc); // New value available float pressure = BARO_ETS_SCALE * (float) baro_ets_adc + BARO_ETS_PRESSURE_OFFSET; AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, &pressure); From 86bea39098c8349c6f44df4d8ab6b8d6ba67b989 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 7 Oct 2013 19:43:57 +0200 Subject: [PATCH 113/125] [boards] roughly correct BOOZ_BARO_SENS --- sw/airborne/boards/booz/baro_board.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index 9641f18144..5b40358571 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -35,14 +35,20 @@ #define BOOZ_BARO_SENDER_ID 1 #endif -/* threshold >0 && <1023 */ +/** threshold >0 && <1023 */ #ifndef BOOZ_ANALOG_BARO_THRESHOLD #define BOOZ_ANALOG_BARO_THRESHOLD 850 #endif -// FIXME correct scale +/** scale factor to convert raw ADC measurement to pressure in Pascal. + * @todo check value + * At low altitudes pressure change is ~1.2 kPa for every 100 meters. + * So with previous scale of 15 for ADC -> meters with INT32_POS_FRAC we get: + * 12 Pascal = (15 * ADC) << 8 + * -> SENS = ~ 12 / (15 * 256) = 0.003125 + */ #ifndef BOOZ_BARO_SENS -#define BOOZ_BARO_SENS 1.0 +#define BOOZ_BARO_SENS 0.003125 #endif struct BaroBoard baro_board; From c88a0b775c7db06ce6a0e68ccab70d6eac3969ee Mon Sep 17 00:00:00 2001 From: NeoFromMatrix Date: Mon, 7 Oct 2013 22:56:18 +0200 Subject: [PATCH 114/125] [boards] stm32f4_discovery typo in description typo at line 25: SPI1 SKL -> SPI1 SCL added info line 15: UART2 -> UART2 (Spektrum input) closes #552 --- sw/airborne/boards/stm32f4_discovery.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h index 4884eb932d..49c2fdc26f 100755 --- a/sw/airborne/boards/stm32f4_discovery.h +++ b/sw/airborne/boards/stm32f4_discovery.h @@ -12,7 +12,7 @@ * PA7 = FREE * PA8 = SPECTRUM BIND * PA9 = FREE (ONLY if usb is not active during runtime, PC0 must be high or input ) - * PA10 = UART2 + * PA10 = UART2 (Spektrum input) * PA11 = FREE if usb is not active during runtime * PA12 = FREE if usb is not active during runtime * PA13 = FREE @@ -22,7 +22,7 @@ * PB0 = FREE * PB1 = ADC_1 (ADC12 IN 9) * PB2 = FREE - * PB3 = SPI1 SKL + * PB3 = SPI1 SCL * PB4 = SPI1 MISO * PB5 = SPI1 MOSI * PB6 = UART1 TX + 4K7 Ohm pull up resistor normaly used for i2c From d401dd83df609d1c9696711858d62367ad867a33 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 8 Oct 2013 15:03:54 +0200 Subject: [PATCH 115/125] Update README for recommended gcc-arm-embedded toolchain --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 86ced4a95d..4cd013f77b 100644 --- a/README.md +++ b/README.md @@ -18,11 +18,12 @@ Installation is described in the wiki (http://paparazzi.enac.fr/wiki/Installatio For Ubuntu users, required packages are available in the [paparazzi-uav PPA] (https://launchpad.net/~paparazzi-uav/+archive/ppa), Debian users can use http://paparazzi.enac.fr/debian - +Debian/Ubuntu packages: - **paparazzi-dev** is the meta-package that depends on everything needed to compile and run the ground segment and the simulator. -- **paparazzi-arm-multilib** ARM cross-compiling toolchain for LPC21 and STM32 based boards. - **paparazzi-jsbsim** is needed for using JSBSim as flight dynamic model for the simulator. +Recommended cross compiling toolchain: https://launchpad.net/gcc-arm-embedded + Directories quick and dirty description: ---------------------------------------- From 934e4838b08860d9af9895f2b22265cce2b73e09 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 8 Oct 2013 16:17:47 +0200 Subject: [PATCH 116/125] [apogee] fix baro mpl3115 init on apogee --- sw/airborne/boards/apogee/imu_apogee.c | 11 +++++++++++ sw/airborne/modules/sensors/baro_mpl3115.h | 2 ++ 2 files changed, 13 insertions(+) diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 886afa1559..40b663f421 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -61,6 +61,13 @@ PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) struct ImuApogee imu_apogee; +// baro config will be done later in bypass mode +bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void* mpu); + +bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))) { + return TRUE; +} + void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// @@ -71,6 +78,10 @@ void imu_impl_init(void) imu_apogee.mpu.config.dlpf_cfg = APOGEE_LOWPASS_FILTER; imu_apogee.mpu.config.gyro_range = APOGEE_GYRO_RANGE; imu_apogee.mpu.config.accel_range = APOGEE_ACCEL_RANGE; + // set MPU in bypass mode for the baro + imu_apogee.mpu.config.nb_slaves = 1; + imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave; + imu_apogee.mpu.config.i2c_bypass = TRUE; imu_apogee.gyr_valid = FALSE; imu_apogee.acc_valid = FALSE; diff --git a/sw/airborne/modules/sensors/baro_mpl3115.h b/sw/airborne/modules/sensors/baro_mpl3115.h index ef9f8b1124..099e86bdee 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.h +++ b/sw/airborne/modules/sensors/baro_mpl3115.h @@ -29,4 +29,6 @@ extern void baro_mpl3115_init( void ); extern void baro_mpl3115_read_periodic( void ); extern void baro_mpl3115_read_event( void ); +#define BaroMpl3115Update(_b, _h) { if (mpl3115_data_available) { _b = mpl3115_pressure; _h(); mpl3115_data_available = FALSE; } } + #endif From 1734b1d004b5569416d563c45bdbf7cb76e2760a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 8 Oct 2013 18:41:33 +0200 Subject: [PATCH 117/125] [sim] remove unused sim_baro --- sw/airborne/arch/sim/sim_baro.c | 37 --------------------------------- 1 file changed, 37 deletions(-) delete mode 100644 sw/airborne/arch/sim/sim_baro.c diff --git a/sw/airborne/arch/sim/sim_baro.c b/sw/airborne/arch/sim/sim_baro.c deleted file mode 100644 index cf6221bb29..0000000000 --- a/sw/airborne/arch/sim/sim_baro.c +++ /dev/null @@ -1,37 +0,0 @@ -#include -#include "state.h" -#include "subsystems/nav.h" -#include "subsystems/gps.h" -#include "baro_MS5534A.h" - -bool_t alt_baro_enabled; -bool_t spi_message_received; -bool_t baro_MS5534A_available; -uint32_t baro_MS5534A_pressure; -uint32_t baro_MS5534A_ground_pressure; -uint16_t baro_MS5534A_temp; -float baro_MS5534A_r; -float baro_MS5534A_sigma2; -float baro_MS5534A_z; - -void baro_MS5534A_init(void) { - baro_MS5534A_ground_pressure = 100000; - - baro_MS5534A_r = 10.; - baro_MS5534A_sigma2 = 1; -} - -// void baro_MS5534A_reset(void); - -void baro_MS5534A_send(void) { - static bool_t even = FALSE; - even = !even; - - spi_message_received = even; -} - -void baro_MS5534A_event_task( void ) { - baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); - baro_MS5534A_temp = 10 + stateGetPositionUtm_f()->alt; - baro_MS5534A_available = TRUE; -} From 3c4f223366da4a0052650118bdfd168938d00c40 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 7 Oct 2013 20:18:51 +0200 Subject: [PATCH 118/125] [modules] add baro_sim as a temporary workaround to get baro simulation in ocaml sim --- .../examples/Twinstar_energyadaptive.xml | 1 + .../subsystems/shared/baro_board.makefile | 5 ++- conf/modules/baro_sim.xml | 22 ++++++++++ sw/airborne/boards/pc_sim.h | 3 +- sw/airborne/modules/sensors/baro_sim.c | 44 +++++++++++++++++++ sw/airborne/modules/sensors/baro_sim.h | 37 ++++++++++++++++ sw/airborne/subsystems/ins/ins_alt_float.c | 6 ++- 7 files changed, 114 insertions(+), 4 deletions(-) create mode 100644 conf/modules/baro_sim.xml create mode 100644 sw/airborne/modules/sensors/baro_sim.c create mode 100644 sw/airborne/modules/sensors/baro_sim.h diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index d0f83f5147..972093d111 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -54,6 +54,7 @@ twog_1.0 + aspirin + ETS baro + ETS speed + diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index 012f3326b8..7bea12cf0b 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -125,8 +125,9 @@ endif # # add it for simulators # -sim.srcs += $(SRC_BOARD)/baro_board.c -jsbsim.srcs += $(SRC_BOARD)/baro_board.c +# only NPS for now... +#sim.srcs += $(SRC_BOARD)/baro_board.c +#jsbsim.srcs += $(SRC_BOARD)/baro_board.c nps.srcs += $(SRC_BOARD)/baro_board.c diff --git a/conf/modules/baro_sim.xml b/conf/modules/baro_sim.xml new file mode 100644 index 0000000000..ca694bb582 --- /dev/null +++ b/conf/modules/baro_sim.xml @@ -0,0 +1,22 @@ + + + + + + Simulated barometer. + Sends the BARO_ABS ABI message with gps.hmsl converted to absolute pressure. + + + +
+ +
+ + + + + + + + + diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index c15970eb21..c3583ebabd 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -11,7 +11,8 @@ #define DefaultVoltageOfAdc(adc) (1.0*adc) - +#ifndef USE_BARO_BOARD #define USE_BARO_BOARD 1 +#endif #endif /* CONFIG_PC_SIM_H */ diff --git a/sw/airborne/modules/sensors/baro_sim.c b/sw/airborne/modules/sensors/baro_sim.c new file mode 100644 index 0000000000..0b58664a2d --- /dev/null +++ b/sw/airborne/modules/sensors/baro_sim.c @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file baro_sim.c + * + * Simulate barometer pressure measurement using gps.hmsl + */ + +#include "math/pprz_isa.h" +#include "subsystems/gps.h" +#include "subsystems/abi.h" + +#ifndef BARO_SIM_SENDER_ID +#define BARO_SIM_SENDER_ID 1 +#endif +PRINT_CONFIG_VAR(BARO_SIM_SENDER_ID) + +void baro_sim_init(void) { + +} + +void baro_sim_periodic(void) { + float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); +} diff --git a/sw/airborne/modules/sensors/baro_sim.h b/sw/airborne/modules/sensors/baro_sim.h new file mode 100644 index 0000000000..a78d3218fa --- /dev/null +++ b/sw/airborne/modules/sensors/baro_sim.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file baro_sim.h + * + * Simulate barometer pressure measurement using gps.hmsl + */ + +#ifndef BARO_SIM_H +#define BARO_SIM_H + +/// new measurement every baro_sim_periodic +#define BARO_SIM_DT BARO_SIM_PERIODIC_PERIOD + +extern void baro_sim_init(void); +extern void baro_sim_periodic(void); + +#endif diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index b12c50b66f..6e0afeb916 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -193,7 +193,11 @@ void alt_kalman(float z_meas) { float SIGMA2; #if USE_BAROMETER -#if USE_BARO_MS5534A +#ifdef SITL + DT = BARO_SIM_DT; + R = 0.5; + SIGMA2 = 0.1; +#elif USE_BARO_MS5534A if (alt_baro_enabled) { DT = BARO_DT; R = baro_MS5534A_r; From ff42b37d6f106cddb52f279f1f6a93227a0eb0ac Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 8 Oct 2013 22:41:12 +0200 Subject: [PATCH 119/125] [conf] move some airframes to obsolete --- conf/airframes/{ => obsolete}/NoVa_L.xml | 0 conf/airframes/{ => obsolete}/booz2_flixr.xml | 0 conf/airframes/{ => obsolete}/booz2_ppzuav.xml | 0 conf/airframes/{ => obsolete}/mentor_tum.xml | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename conf/airframes/{ => obsolete}/NoVa_L.xml (100%) rename conf/airframes/{ => obsolete}/booz2_flixr.xml (100%) rename conf/airframes/{ => obsolete}/booz2_ppzuav.xml (100%) rename conf/airframes/{ => obsolete}/mentor_tum.xml (100%) diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/obsolete/NoVa_L.xml similarity index 100% rename from conf/airframes/NoVa_L.xml rename to conf/airframes/obsolete/NoVa_L.xml diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/obsolete/booz2_flixr.xml similarity index 100% rename from conf/airframes/booz2_flixr.xml rename to conf/airframes/obsolete/booz2_flixr.xml diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/obsolete/booz2_ppzuav.xml similarity index 100% rename from conf/airframes/booz2_ppzuav.xml rename to conf/airframes/obsolete/booz2_ppzuav.xml diff --git a/conf/airframes/mentor_tum.xml b/conf/airframes/obsolete/mentor_tum.xml similarity index 100% rename from conf/airframes/mentor_tum.xml rename to conf/airframes/obsolete/mentor_tum.xml From 4dd2226fff78fab215fb4a4369b4c2b56fc70ed3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 8 Oct 2013 22:45:06 +0200 Subject: [PATCH 120/125] [boards] sim pc: only USE_BARO_BOARD for nps --- sw/airborne/boards/pc_sim.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sw/airborne/boards/pc_sim.h b/sw/airborne/boards/pc_sim.h index c3583ebabd..f2ac35ef1f 100644 --- a/sw/airborne/boards/pc_sim.h +++ b/sw/airborne/boards/pc_sim.h @@ -12,7 +12,11 @@ #define DefaultVoltageOfAdc(adc) (1.0*adc) #ifndef USE_BARO_BOARD +#if USE_NPS #define USE_BARO_BOARD 1 +#else +#define USE_BARO_BOARD 0 +#endif #endif #endif /* CONFIG_PC_SIM_H */ From c0b9260c23cf11b784a3c6e0f8bc301dbf7009c9 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 9 Oct 2013 15:51:24 +0200 Subject: [PATCH 121/125] [baro] fix baro scaling for booz and navgo --- sw/airborne/boards/booz/baro_board.c | 26 +++++++++++++++++++------- sw/airborne/boards/navgo/baro_board.c | 19 ++++++++++++++++--- 2 files changed, 35 insertions(+), 10 deletions(-) diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index 5b40358571..fbedde6f72 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -41,14 +41,26 @@ #endif /** scale factor to convert raw ADC measurement to pressure in Pascal. - * @todo check value - * At low altitudes pressure change is ~1.2 kPa for every 100 meters. - * So with previous scale of 15 for ADC -> meters with INT32_POS_FRAC we get: - * 12 Pascal = (15 * ADC) << 8 - * -> SENS = ~ 12 / (15 * 256) = 0.003125 + * + * Sensor Sensitivity -> SS = 0.045 mv / Pa + * Sensor Gain -> G = 94.25 + * Sensitivity -> S = SS*G = 4.24125 mV / Pa + * 10 bit ADC -> A = 3.3 V / 1024 = 3.223 mV / LSB + * Total Sensitivity SENS = A / S = 0.759837 + * + * For the real pressure you also need to take into account the (variable) offset + * + * supply voltage Vs = 5V + * real sensor sensitivity Vout = Vs * (0.009 P - 0.095) + * voltage variable offset Voff(DAC) = Vs / 69.23 + (DAC * 3.3 / 1024) / 21.77 + * ADC voltage at init Vadc = 3.3*BARO_THRESHOLD/1024 = Vout - Voff + * + * => Inverting these formulas can give the 'real' pressure + * + * since we don't care that much in this case, we can take a fixed offset of 101325 Pa */ #ifndef BOOZ_BARO_SENS -#define BOOZ_BARO_SENS 0.003125 +#define BOOZ_BARO_SENS 0.759837 #endif struct BaroBoard baro_board; @@ -75,7 +87,7 @@ void baro_periodic(void) { RunOnceEvery(10, { baro_board_calibrate();}); } else { - float pressure = BOOZ_BARO_SENS*baro_board.absolute; + float pressure = 101325.0 - BOOZ_BARO_SENS*(BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); AbiSendMsgBARO_ABS(BOOZ_BARO_SENDER_ID, &pressure); } } diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index e1b03bad42..5a9c9e9abd 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -32,9 +32,22 @@ #include "subsystems/abi.h" #include "led.h" -// FIXME +/** scale factor to convert raw ADC measurement to pressure in Pascal. + * + * supply voltage Vs = 5V + * real sensor sensitivity Vout = Vs * (0.009 P - 0.095) with P in kPa + * 22 bit signed ADC -> only 21 useful bits ADC = 5/2^21 = 2.384e-6 V / LSB + * + * offset = 5*0.095/2.384e-6 = 199229 (LSB) + * sensitivity = (1000/0.009)*2.384e-6/5 = 0.05298 Pa/LSB + * + */ #ifndef NAVGO_BARO_SENS -#define NAVGO_BARO_SENS 0.0274181 +#define NAVGO_BARO_SENS 0.05298 +#endif + +#ifndef NAVGO_BARO_OFFSET +#define NAVGO_BARO_OFFSET 199229 #endif #ifndef NAVGO_BARO_SENDER_ID @@ -73,7 +86,7 @@ void navgo_baro_event(void) { if (mcp355x_data_available) { if (startup_cnt == 0) { // Send data when init phase is done - float pressure = NAVGO_BARO_SENS*mcp355x_data; + float pressure = NAVGO_BARO_SENS*(mcp355x_data+NAVGO_BARO_OFFSET); AbiSendMsgBARO_ABS(NAVGO_BARO_SENDER_ID, &pressure); } mcp355x_data_available = FALSE; From 007120aa9af3b71a77447c8f8f15d7a79802e980 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 9 Oct 2013 15:53:51 +0200 Subject: [PATCH 122/125] [fix] fix typo in actuators asctec v2 --- conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile index 94bdf097cd..4442ed91f6 100644 --- a/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile +++ b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile @@ -7,7 +7,7 @@ # $(TARGET).CFLAGS += -DACTUATORS -ACTUATORS_ASCTEC_SRCS = subsystems/actuators/actuators_asctec_v2.c +ACTUATORS_ASCTEC_V2_SRCS = subsystems/actuators/actuators_asctec_v2.c # set default i2c device if not already configured From b2814e7169863845fbe60af962e13f3eeb8f86db Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 10 Oct 2013 17:48:09 +0200 Subject: [PATCH 123/125] [abi] split abi messages from telemetry messages currently, the dtd is the same, but it can be changed in the future to be more adapted to ABI messages --- Makefile | 3 ++- conf/abi.xml | 22 ++++++++++++++++++++++ conf/messages.xml | 17 ----------------- 3 files changed, 24 insertions(+), 18 deletions(-) create mode 100644 conf/abi.xml diff --git a/Makefile b/Makefile index b845818f72..132917c452 100644 --- a/Makefile +++ b/Makefile @@ -82,6 +82,7 @@ SUBDIRS = $(PPRZCENTER) $(MISC) $(LOGALIZER) # xml files used as input for header generation # MESSAGES_XML = $(CONF)/messages.xml +ABI_XML = $(CONF)/abi.xml UBX_XML = $(CONF)/ubx.xml MTK_XML = $(CONF)/mtk.xml XSENS_XML = $(CONF)/xsens_MTi-G.xml @@ -218,7 +219,7 @@ $(DL_PROTOCOL2_H) : $(MESSAGES_XML) tools $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(ABI_MESSAGES_H) : $(MESSAGES_XML) tools +$(ABI_MESSAGES_H) : $(ABI_XML) tools @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_abi.out $< airborne > $($@_TMP) diff --git a/conf/abi.xml b/conf/abi.xml new file mode 100644 index 0000000000..896761d545 --- /dev/null +++ b/conf/abi.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index 82d69ed0c6..bf1ef19874 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2573,23 +2573,6 @@ - - - - - - - - - - - - - From 754e20a0ac66a6e306cf9fa8b30ec96f7f006915 Mon Sep 17 00:00:00 2001 From: TU Delft developer Date: Thu, 10 Oct 2013 20:33:00 +0200 Subject: [PATCH 124/125] [sim] flight gear viz: daytime on all spots on the planet, not only France closes #555 --- sw/simulator/fg.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/simulator/fg.c b/sw/simulator/fg.c index 63493b2a30..19bd9c7a91 100644 --- a/sw/simulator/fg.c +++ b/sw/simulator/fg.c @@ -41,7 +41,7 @@ value fg_msg_native(value s, value lat, value lon, value z, value phi, value the msg.num_tanks = 1; msg.fuel_quantity[0] = 10.; - msg.cur_time = 3213092700ul;//time(NULL); + msg.cur_time = 3213092700ul+((uint32_t)((msg.longitude)*13578)); //time(NULL); msg.warp = 0; msg.ground_elev = 0.; From 78ede475d4c5af0361a6126398bb1608088cf62a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 10 Oct 2013 19:17:50 +0200 Subject: [PATCH 125/125] [abi] define ABI x_SENDER_IDs in abi_sender_ids.h Same BARO_BOARD_SENDER_ID for all baro_board implementations. Since there can only be one baro_board at the same time and this provides a good default for INS_BARO_ID. The abi_sender_ids.h can be considered a temporary solution, we might come up with a different scheme later... --- .../examples/Twinstar_energyadaptive.xml | 4 +- sw/airborne/boards/apogee/baro_board.c | 6 +- sw/airborne/boards/ardrone/baro_board.c | 7 +- sw/airborne/boards/baro_board_ms5611_i2c.c | 6 +- sw/airborne/boards/baro_board_ms5611_spi.c | 6 +- sw/airborne/boards/booz/baro_board.c | 5 +- sw/airborne/boards/lisa_l/baro_board.c | 8 +- sw/airborne/boards/lisa_m/baro_board.c | 5 +- sw/airborne/boards/navgo/baro_board.c | 6 +- sw/airborne/boards/pc/baro_board.c | 6 +- sw/airborne/boards/umarim/baro_board.c | 6 +- sw/airborne/modules/sensors/baro_MS5534A.c | 3 - sw/airborne/modules/sensors/baro_amsys.c | 3 - sw/airborne/modules/sensors/baro_bmp.c | 4 - sw/airborne/modules/sensors/baro_ets.c | 4 +- sw/airborne/modules/sensors/baro_hca.c | 4 - sw/airborne/modules/sensors/baro_mpl3115.c | 3 - sw/airborne/modules/sensors/baro_ms5611_i2c.c | 4 - sw/airborne/modules/sensors/baro_ms5611_spi.c | 4 - sw/airborne/modules/sensors/baro_scp.c | 4 - sw/airborne/modules/sensors/baro_scp_i2c.c | 4 - sw/airborne/modules/sensors/baro_sim.c | 3 - .../modules/sensors/pressure_board_navarro.c | 3 - sw/airborne/subsystems/abi_common.h | 1 + sw/airborne/subsystems/abi_sender_ids.h | 79 +++++++++++++++++++ sw/airborne/subsystems/ins/ins_alt_float.c | 2 +- sw/airborne/subsystems/ins/ins_int.c | 4 +- 27 files changed, 98 insertions(+), 96 deletions(-) create mode 100644 sw/airborne/subsystems/abi_sender_ids.h diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index 972093d111..c5afbe19cf 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -12,7 +12,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed - @@ -22,6 +21,9 @@ twog_1.0 + aspirin + ETS baro + ETS speed + + + diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 2862bfe449..27aabe4bfe 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -36,10 +36,6 @@ #include "subsystems/abi.h" #include "led.h" -#ifndef APOGEE_BARO_SENDER_ID -#define APOGEE_BARO_SENDER_ID 12 -#endif - /** Counter to init ads1114 at startup */ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; @@ -80,7 +76,7 @@ void apogee_baro_event(void) { if (apogee_baro.data_available) { if (startup_cnt == 0) { float pressure = ((float)apogee_baro.pressure/(1<<2)); - AbiSendMsgBARO_ABS(APOGEE_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } apogee_baro.data_available = FALSE; } diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 95c3c496ed..73c33868f3 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -32,11 +32,6 @@ #include "navdata.h" -#ifndef ARDRONE2_BARO_SENDER_ID -#define ARDRONE2_BARO_SENDER_ID 13 -#endif - - #define BMP180_OSS 0 // Parrot ARDrone uses no oversampling void baro_init(void) {} @@ -80,7 +75,7 @@ void ardrone_baro_event(void) // TODO send Temperature message baro_apply_calibration_temp(navdata.temperature_pressure); float pressure = (float)baro_apply_calibration(navdata.pressure); - AbiSendMsgBARO_ABS(ARDRONE2_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } navdata_baro_available = FALSE; } diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c index c5e6158e30..3cd631019b 100644 --- a/sw/airborne/boards/baro_board_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -54,10 +54,6 @@ #endif -#ifndef BARO_BOARD_MS5611_SENDER_ID -#define BARO_BOARD_MS5611_SENDER_ID 7 -#endif - struct Ms5611_I2c bb_ms5611; @@ -96,7 +92,7 @@ void baro_event(void){ if (bb_ms5611.data_available) { float pressure = (float)bb_ms5611.data.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_MS5611_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); bb_ms5611.data_available = FALSE; #ifdef BARO_LED diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 3de17c1cca..61b0396d78 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -43,10 +43,6 @@ #include "subsystems/datalink/downlink.h" -#ifndef BARO_BOARD_MS5611_SENDER_ID -#define BARO_BOARD_MS5611_SENDER_ID 7 -#endif - struct Ms5611_Spi bb_ms5611; @@ -85,7 +81,7 @@ void baro_event(){ if (bb_ms5611.data_available) { float pressure = (float)bb_ms5611.data.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_MS5611_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); bb_ms5611.data_available = FALSE; #ifdef BARO_LED diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index fbedde6f72..e7d955ecdb 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -31,9 +31,6 @@ #include "subsystems/abi.h" #include "led.h" -#ifndef BOOZ_BARO_SENDER_ID -#define BOOZ_BARO_SENDER_ID 1 -#endif /** threshold >0 && <1023 */ #ifndef BOOZ_ANALOG_BARO_THRESHOLD @@ -88,7 +85,7 @@ void baro_periodic(void) { } else { float pressure = 101325.0 - BOOZ_BARO_SENS*(BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); - AbiSendMsgBARO_ABS(BOOZ_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } } diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c index 3a3f02ecd5..a7a6a7834b 100644 --- a/sw/airborne/boards/lisa_l/baro_board.c +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -71,10 +71,6 @@ static inline void baro_board_read_from_current_register(uint8_t baro_addr); #define LISA_L_DIFF_SENS 1.0 #endif -#ifndef LISA_L_BARO_SENDER_ID -#define LISA_L_BARO_SENDER_ID 2 -#endif - void baro_init(void) { #ifdef BARO_LED LED_OFF(BARO_LED); @@ -141,7 +137,7 @@ void lisa_l_baro_event(void) { if (baro_trans.status == I2CTransSuccess) { int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; float pressure = LISA_L_BARO_SENS*(float)tmp; - AbiSendMsgBARO_ABS(LISA_L_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } } else if (baro_board.status == LBS_READING_DIFF && @@ -150,7 +146,7 @@ void lisa_l_baro_event(void) { if (baro_trans.status == I2CTransSuccess) { int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; float diff = LISA_L_DIFF_SENS*(float)tmp; - AbiSendMsgBARO_DIFF(LISA_L_BARO_SENDER_ID, &diff); + AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, &diff); } } } diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 4cf8462778..5a6aa9a489 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -33,9 +33,6 @@ #include "led.h" -#ifndef LISA_M_BARO_SENDER_ID -#define LISA_M_BARO_SENDER_ID 6 -#endif struct Bmp085 baro_bmp085; @@ -77,7 +74,7 @@ void baro_event(void) if (baro_bmp085.data_available) { float pressure = (float)baro_bmp085.pressure; - AbiSendMsgBARO_ABS(LISA_M_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); baro_bmp085.data_available = FALSE; #ifdef BARO_LED RunOnceEvery(10,LED_TOGGLE(BARO_LED)); diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index 5a9c9e9abd..fd99a5c6ca 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -50,10 +50,6 @@ #define NAVGO_BARO_OFFSET 199229 #endif -#ifndef NAVGO_BARO_SENDER_ID -#define NAVGO_BARO_SENDER_ID 10 -#endif - /* Counter to init mcp355x at startup */ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; @@ -87,7 +83,7 @@ void navgo_baro_event(void) { if (startup_cnt == 0) { // Send data when init phase is done float pressure = NAVGO_BARO_SENS*(mcp355x_data+NAVGO_BARO_OFFSET); - AbiSendMsgBARO_ABS(NAVGO_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } mcp355x_data_available = FALSE; } diff --git a/sw/airborne/boards/pc/baro_board.c b/sw/airborne/boards/pc/baro_board.c index 6c87012d77..d65be929b8 100644 --- a/sw/airborne/boards/pc/baro_board.c +++ b/sw/airborne/boards/pc/baro_board.c @@ -31,15 +31,11 @@ #include "subsystems/abi.h" -#ifndef SIM_BARO_SENDER_ID -#define SIM_BARO_SENDER_ID 4 -#endif - void baro_init(void) {} void baro_periodic(void) {} void baro_feed_value(double value) { float pressure = (float) value; - AbiSendMsgBARO_ABS(SIM_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index 8790c2bc28..69f5866403 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -42,10 +42,6 @@ #define UMARIM_BARO_SENS 0.0274181 #endif -#ifndef UMARIM_BARO_SENDER_ID -#define UMARIM_BARO_SENDER_ID 11 -#endif - /* Counter to init ads1114 at startup */ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; @@ -79,7 +75,7 @@ void umarim_baro_event(void) { if (BARO_ABS_ADS.data_available) { if (startup_cnt == 0) { float pressure = UMARIM_BARO_SENS*Ads1114GetValue(BARO_ABS_ADS); - AbiSendMsgBARO_ABS(UMARIM_BARO_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } BARO_ABS_ADS.data_available = FALSE; } diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index 0553ca8b83..8442e22e56 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -37,9 +37,6 @@ #include "subsystems/nav.h" #include "state.h" -#ifndef BARO_MS5534A_SENDER_ID -#define BARO_MS5534A_SENDER_ID 15 -#endif bool_t baro_MS5534A_do_reset; uint32_t baro_MS5534A_pressure; diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index be9946b94d..66bdab29c5 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -69,9 +69,6 @@ #define BARO_AMSYS_I2C_DEV i2c0 #endif -#ifndef BARO_AMSYS_SENDER_ID -#define BARO_AMSYS_SENDER_ID 16 -#endif // Global variables uint16_t pBaroRaw; diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index e39a9b299d..a0dbade0af 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -52,10 +52,6 @@ #define BMP_I2C_DEV i2c0 #endif -#ifndef BARO_BMP_SENDER_ID -#define BARO_BMP_SENDER_ID 17 -#endif - #define BARO_BMP_R 0.5 #define BARO_BMP_SIGMA2 0.1 diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 6da9cb8e80..f6f0f80a02 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -93,9 +93,7 @@ #endif PRINT_CONFIG_VAR(BARO_ETS_I2C_DEV) -#ifndef BARO_ETS_SENDER_ID -#define BARO_ETS_SENDER_ID 18 -#endif + PRINT_CONFIG_VAR(BARO_ETS_SENDER_ID) /** delay in seconds until sensor is read after startup */ diff --git a/sw/airborne/modules/sensors/baro_hca.c b/sw/airborne/modules/sensors/baro_hca.c index 99f6a23ff1..d1b15cfd9e 100644 --- a/sw/airborne/modules/sensors/baro_hca.c +++ b/sw/airborne/modules/sensors/baro_hca.c @@ -54,10 +54,6 @@ #define BARO_HCA_I2C_DEV i2c0 #endif -#ifndef BARO_HCA_SENDER_ID -#define BARO_HCA_SENDER_ID 19 -#endif - // Global variables uint16_t pBaroRaw; bool_t baro_hca_valid; diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index 9de0f8346c..af58bc2cd7 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -47,9 +47,6 @@ #define BARO_MPL3115_I2C_SLAVE_ADDR MPL3115_I2C_ADDR #endif -#ifndef BARO_MPL3115_SENDER_ID -#define BARO_MPL3115_SENDER_ID 20 -#endif struct Mpl3115 baro_mpl; diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index 690d7e212b..5ee80bf457 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -40,10 +40,6 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#ifndef BARO_MS5611_SENDER_ID -#define BARO_MS5611_SENDER_ID 20 -#endif - #ifndef MS5611_I2C_DEV #define MS5611_I2C_DEV i2c0 #endif diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index ce4f2f5307..9a457b78e5 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -40,10 +40,6 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#ifndef BARO_MS5611_SENDER_ID -#define BARO_MS5611_SENDER_ID 20 -#endif - #ifndef MS5611_SPI_DEV #define MS5611_SPI_DEV spi1 #endif diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c index 580f3d36b6..278fe3e4a1 100644 --- a/sw/airborne/modules/sensors/baro_scp.c +++ b/sw/airborne/modules/sensors/baro_scp.c @@ -16,10 +16,6 @@ #warning set SENSOR_SYNC_SEND to use baro_scp #endif -#ifndef BARO_SCP_SENDER_ID -#define BARO_SCP_SENDER_ID 21 -#endif - #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c index 331549ffcf..eb9ff82b96 100644 --- a/sw/airborne/modules/sensors/baro_scp_i2c.c +++ b/sw/airborne/modules/sensors/baro_scp_i2c.c @@ -20,10 +20,6 @@ #warning set SENSOR_SYNC_SEND to use baro_scp_i2c #endif -#ifndef BARO_SCP_SENDER_ID -#define BARO_SCP_SENDER_ID 21 -#endif - #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif diff --git a/sw/airborne/modules/sensors/baro_sim.c b/sw/airborne/modules/sensors/baro_sim.c index 0b58664a2d..b78e6995c4 100644 --- a/sw/airborne/modules/sensors/baro_sim.c +++ b/sw/airborne/modules/sensors/baro_sim.c @@ -29,9 +29,6 @@ #include "subsystems/gps.h" #include "subsystems/abi.h" -#ifndef BARO_SIM_SENDER_ID -#define BARO_SIM_SENDER_ID 1 -#endif PRINT_CONFIG_VAR(BARO_SIM_SENDER_ID) void baro_sim_init(void) { diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index 0c26f41e6c..d832fad9a0 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -61,9 +61,6 @@ #define PBN_PRESSURE_OFFSET 101325.0 #endif -#ifndef BARO_PBN_SENDER_ID -#define BARO_PBN_SENDER_ID 22 -#endif // Global variables uint16_t altitude_adc; diff --git a/sw/airborne/subsystems/abi_common.h b/sw/airborne/subsystems/abi_common.h index cfa41b592a..f4eac21514 100644 --- a/sw/airborne/subsystems/abi_common.h +++ b/sw/airborne/subsystems/abi_common.h @@ -36,6 +36,7 @@ * Ex: '#include "subsystems/gps.h"' in order to use the GpsState structure */ +#include "subsystems/abi_sender_ids.h" /* Some magic to avoid to compile C code, only headers */ #ifdef ABI_C diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h new file mode 100644 index 0000000000..f87b5298b8 --- /dev/null +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file subsystems/abi_sender_ids.h + * + * Convenience defines for ABI sender IDs. + */ + +#ifndef ABI_SENDER_IDS_H +#define ABI_SENDER_IDS_H + +/** default onboard baro */ +#ifndef BARO_BOARD_SENDER_ID +#define BARO_BOARD_SENDER_ID 1 +#endif + +/* + * IDs of baro modules that can be loaded + */ +#ifndef BARO_MS5611_SENDER_ID +#define BARO_MS5611_SENDER_ID 10 +#endif + +#ifndef BARO_AMSYS_SENDER_ID +#define BARO_AMSYS_SENDER_ID 11 +#endif + +#ifndef BARO_BMP_SENDER_ID +#define BARO_BMP_SENDER_ID 12 +#endif + +#ifndef BARO_ETS_SENDER_ID +#define BARO_ETS_SENDER_ID 13 +#endif + +#ifndef BARO_MS5534A_SENDER_ID +#define BARO_MS5534A_SENDER_ID 14 +#endif + +#ifndef BARO_HCA_SENDER_ID +#define BARO_HCA_SENDER_ID 15 +#endif + +#ifndef BARO_MPL3115_SENDER_ID +#define BARO_MPL3115_SENDER_ID 16 +#endif + +#ifndef BARO_SCP_SENDER_ID +#define BARO_SCP_SENDER_ID 17 +#endif + +#ifndef BARO_PBN_SENDER_ID +#define BARO_PBN_SENDER_ID 18 +#endif + +#ifndef BARO_SIM_SENDER_ID +#define BARO_SIM_SENDER_ID 19 +#endif + +#endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 6e0afeb916..791a20fc7d 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -55,7 +55,7 @@ float ins_baro_alt; // Baro event on ABI #ifndef INS_BARO_ID -#define INS_BARO_ID ABI_BROADCAST +#define INS_BARO_ID BARO_BOARD_SENDER_ID #endif abi_event baro_ev; static void baro_cb(uint8_t sender_id, const float *pressure); diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index bbf65f8410..b11a8d85c9 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -81,9 +81,9 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif -/* barometer */ +/** default barometer to use in INS */ #ifndef INS_BARO_ID -#define INS_BARO_ID ABI_BROADCAST +#define INS_BARO_ID BARO_BOARD_SENDER_ID #endif abi_event baro_ev; static void baro_cb(uint8_t sender_id, const float *pressure);