diff --git a/conf/abi.xml b/conf/abi.xml index 45918ac3a0..5704993f98 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -46,7 +46,15 @@ + + + + + + + + + - diff --git a/conf/airframes/bebop.xml b/conf/airframes/bebop.xml index 4d89a43048..74e3e517c7 100644 --- a/conf/airframes/bebop.xml +++ b/conf/airframes/bebop.xml @@ -21,11 +21,15 @@ - + + + + + diff --git a/conf/settings/estimation/ahrs_secondary.xml b/conf/settings/estimation/ahrs_secondary.xml new file mode 100644 index 0000000000..7418f257e8 --- /dev/null +++ b/conf/settings/estimation/ahrs_secondary.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/conf/settings/estimation/ins_float_invariant.xml b/conf/settings/estimation/ins_float_invariant.xml index 30ac06cd03..bb7da6c8c0 100644 --- a/conf/settings/estimation/ins_float_invariant.xml +++ b/conf/settings/estimation/ins_float_invariant.xml @@ -3,20 +3,20 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 408910ef63..55c329beb2 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -65,6 +65,9 @@ static bool_t navdata_available = FALSE; static pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER; static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER; +#ifndef NAVDATA_FILTER_ID +#define NAVDATA_FILTER_ID 2 +#endif /** Sonar offset. * Offset value in ADC @@ -158,16 +161,6 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev) &navdata.measure.chksum, &navdata.checksum_errors); } - -static void send_filter_status(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t mde = 3; - if (!DefaultAhrsImpl.is_aligned) { mde = 2; } - if (navdata.imu_lost) { mde = 5; } - uint16_t val = navdata.lost_imu_frames; - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); -} - #endif /** @@ -245,7 +238,6 @@ bool_t navdata_init() #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); - register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif // Set to initialized diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 8c5998d3e8..6069588737 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -127,11 +127,7 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) #endif -#define __DefaultAhrsRegister(_x) _x ## _register() -#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) -#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) - -#if USE_AHRS && USE_IMU +#if USE_IMU #ifdef AHRS_PROPAGATE_FREQUENCY #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY) @@ -143,19 +139,7 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_mag_event(void); -volatile uint8_t ahrs_timeout_counter = 0; - -//FIXME not the correct place -static void send_filter_status(struct transport_tx *trans, struct link_device *dev) -{ - uint8_t mde = 3; - if (!DefaultAhrsImpl.is_aligned) { mde = 2; } - if (ahrs_timeout_counter > 10) { mde = 5; } - uint16_t val = 0; - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); -} - -#endif // USE_AHRS && USE_IMU +#endif // USE_IMU #if USE_GPS static inline void on_gps_solution(void); @@ -200,20 +184,11 @@ void init_ap(void) #endif #if USE_AHRS -#if defined SITL && !USE_NPS - ahrs_sim_init(); -#else ahrs_init(); - DefaultAhrsRegister(); -#endif #endif ins_init(); -#if USE_AHRS && USE_IMU - register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); -#endif - #if USE_BARO_BOARD baro_init(); #endif @@ -617,12 +592,6 @@ void sensors_task(void) { #if USE_IMU imu_periodic(); - -#if USE_AHRS - if (ahrs_timeout_counter < 255) { - ahrs_timeout_counter ++; - } -#endif // USE_AHRS #endif // USE_IMU //FIXME: this is just a kludge @@ -634,7 +603,10 @@ void sensors_task(void) gps_periodic_check(); #endif - ins_periodic(); + //FIXME: temporary hack, remove me +#ifdef InsPeriodic + InsPeriodic(); +#endif } @@ -697,7 +669,7 @@ void event_task_ap(void) mcu_event(); #endif /* SINGLE_MCU */ -#if USE_AHRS && USE_IMU +#if USE_IMU ImuEvent(on_gyro_event, on_accel_event, on_mag_event); #endif @@ -742,10 +714,11 @@ void event_task_ap(void) #if USE_GPS static inline void on_gps_solution(void) { -#if USE_AHRS - ahrs_update_gps(); -#endif - ins_update_gps(); + // current timestamp + uint32_t now_ts = get_sys_time_usec(); + + AbiSendMsgGPS(1, now_ts, &gps); + #ifdef GPS_TRIGGERED_FUNCTION GPS_TRIGGERED_FUNCTION(); #endif @@ -769,10 +742,6 @@ static inline void on_gyro_event(void) // current timestamp uint32_t now_ts = get_sys_time_usec(); -#if USE_AHRS - ahrs_timeout_counter = 0; -#endif - imu_scale_gyro(&imu); AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev); @@ -788,20 +757,6 @@ static inline void on_gyro_event(void) if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } #endif -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for INS propagation.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); -#endif - ins_propagate(dt); - #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index b23a9803a0..99ef9bc4e5 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -89,10 +89,9 @@ bool_t autopilot_detect_ground_once; #endif #ifndef AUTOPILOT_DISABLE_AHRS_KILL -#include "subsystems/ahrs.h" static inline int ahrs_is_aligned(void) { - return DefaultAhrsImpl.is_aligned; + return stateIsAttitudeValid(); } #else PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL") diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index a315c834e6..eaf8fedbcc 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -105,10 +105,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t #endif #endif -#define __DefaultAhrsRegister(_x) _x ## _register() -#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) -#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) - static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_gps_event(void); @@ -160,10 +156,12 @@ STATIC_INLINE void main_init(void) #if USE_AHRS_ALIGNER ahrs_aligner_init(); #endif - ahrs_init(); - ins_init(); - DefaultAhrsRegister(); +#if USE_AHRS + ahrs_init(); +#endif + + ins_init(); #if USE_GPS gps_init(); @@ -228,6 +226,11 @@ STATIC_INLINE void main_periodic(void) imu_periodic(); + //FIXME: temporary hack, remove me +#ifdef InsPeriodic + InsPeriodic(); +#endif + /* run control loops */ autopilot_periodic(); /* set actuators */ @@ -363,20 +366,6 @@ static inline void on_gyro_event( void ) { if (nps_bypass_ahrs) sim_overwrite_ahrs(); #endif -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for INS propagation.") - // timestamp in usec when last callback was received - static uint32_t last_ts = 0; - // dt between this and last callback in seconds - float dt = (float)(now_ts - last_ts) / 1e6; - last_ts = now_ts; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); -#endif - ins_propagate(dt); - #ifdef USE_VEHICLE_INTERFACE vi_notify_imu_available(); #endif @@ -384,8 +373,11 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) static inline void on_gps_event(void) { - ahrs_update_gps(); - ins_update_gps(); + // current timestamp + uint32_t now_ts = get_sys_time_usec(); + + AbiSendMsgGPS(1, now_ts, &gps); + #ifdef USE_VEHICLE_INTERFACE if (gps.fix == GPS_FIX_3D) { vi_notify_gps_available(); diff --git a/sw/airborne/modules/ahrs/ahrs_infrared.c b/sw/airborne/modules/ahrs/ahrs_infrared.c index dcb4598b2e..5971f7ba29 100644 --- a/sw/airborne/modules/ahrs/ahrs_infrared.c +++ b/sw/airborne/modules/ahrs/ahrs_infrared.c @@ -38,6 +38,10 @@ #include "state.h" #include "subsystems/abi.h" +#ifndef INFRARED_FILTER_ID +#define INFRARED_FILTER_ID 2 +#endif + static float heading; /** ABI binding for gyro data. @@ -69,8 +73,9 @@ static void send_status(struct transport_tx *trans, struct link_device *dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; + uint8_t id = INFRARED_FILTER_ID; if (contrast < 50) { mde = 7; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &contrast); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &id, &contrast); } #endif diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index 9e329b9753..7531f91c07 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -38,7 +38,6 @@ #include "mcu_periph/sys_time.h" #include "subsystems/electrical.h" -#include "subsystems/ahrs.h" #include "state.h" #include "pprz_version.h" @@ -296,7 +295,7 @@ static inline void mavlink_send_heartbeat(void) #else uint8_t mav_type = MAV_TYPE_QUADROTOR; #endif - if (DefaultAhrsImpl.is_aligned) { + if (stateIsAttitudeValid()) { if (kill_throttle) { mav_state = MAV_STATE_STANDBY; } diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c index a9d3a95d59..87fcde9879 100644 --- a/sw/airborne/modules/geo_mag/geo_mag.c +++ b/sw/airborne/modules/geo_mag/geo_mag.c @@ -29,9 +29,14 @@ #include "math/pprz_geodetic_wmm2010.h" #include "math/pprz_algebra_double.h" #include "subsystems/gps.h" +#include "subsystems/abi.h" + +// for kill_throttle check #include "autopilot.h" -#include "subsystems/ahrs.h" +#ifndef GEO_MAG_SENDER_ID +#define GEO_MAG_SENDER_ID 1 +#endif bool_t geo_mag_calc_flag; struct GeoMag geo_mag; @@ -72,16 +77,13 @@ void geo_mag_event(void) mag_calc(1, latitude, longitude, alt, nmax, gha, &geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z, IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3); - double_vect3_normalize(&geo_mag.vect); - // copy to ahrs -#ifdef AHRS_FLOAT - VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect); -#else - // convert to MAG_BFP and copy to ahrs - VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), - MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z)); -#endif + // send as normalized float vector via ABI + struct FloatVect3 h = { .x = geo_mag.vect.x, + .y = geo_mag.vect.y, + .z = geo_mag.vect.z }; + float_vect3_normalize(&h); + AbiSendMsgGEO_MAG(GEO_MAG_SENDER_ID, &h); geo_mag.ready = TRUE; } diff --git a/sw/airborne/modules/ins/ahrs_chimu.h b/sw/airborne/modules/ins/ahrs_chimu.h index 6ab50e450a..2032113646 100644 --- a/sw/airborne/modules/ins/ahrs_chimu.h +++ b/sw/airborne/modules/ins/ahrs_chimu.h @@ -35,7 +35,9 @@ struct AhrsChimu { extern struct AhrsChimu ahrs_chimu; -#define DefaultAhrsImpl ahrs_chimu +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_chimu +#endif extern void ahrs_chimu_register(void); extern void ahrs_chimu_init(void); diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 161312858e..7c735849a9 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -37,11 +37,22 @@ INS_FORMAT ins_pitch_neutral; struct AhrsChimu ahrs_chimu; -void ahrs_chimu_update_gps(void); +void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d); +#include "subsystems/abi.h" +static abi_event gps_ev; +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ahrs_chimu_update_gps(gps_s->fix, gps_s->speed_3d); +} void ahrs_chimu_register(void) { - ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps); + ahrs_chimu_init(); + /// @TODO: provide enable function + ahrs_register_impl(NULL); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); } void ahrs_chimu_init(void) @@ -118,15 +129,15 @@ void parse_ins_msg(void) } -void ahrs_chimu_update_gps(void) +void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d) { // Send SW Centripetal Corrections uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 }; float gps_speed = 0; - if (gps.fix == GPS_FIX_3D) { - gps_speed = gps.speed_3d / 100.; + if (gps_fix == GPS_FIX_3D) { + gps_speed = gps_speed_3d / 100.; } gps_speed = FloatSwap(gps_speed); diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c index 8bd7045279..30674a839b 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_uart.c +++ b/sw/airborne/modules/ins/ahrs_chimu_uart.c @@ -34,7 +34,9 @@ struct AhrsChimu ahrs_chimu; void ahrs_chimu_register(void) { - ahrs_register_impl(ahrs_chimu_init, NULL); + ahrs_chimu_init(); + /// @TODO: provide enable function + ahrs_register_impl(NULL); } void ahrs_chimu_init(void) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index e0c48687d6..ffc9de1ece 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -210,7 +210,6 @@ uint8_t send_ck; volatile int xsens_configured = 0; void xsens_init(void); -void xsens_periodic(void); void xsens_init(void) { @@ -247,7 +246,9 @@ void imu_periodic(void) #endif /* USE_IMU */ #if USE_INS_MODULE -void ins_init(void) +void ins_xsens_update_gps(struct GpsState *gps_s); + +void ins_xsens_init(void) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); @@ -256,26 +257,36 @@ void ins_init(void) xsens_init(); } -void ins_periodic(void) +#include "subsystems/abi.h" +static abi_event gps_ev; +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) { - xsens_periodic(); + ins_xsens_update_gps(gps_s); } -void ins_update_gps(void) +void ins_xsens_register(void) +{ + ins_register_impl(ins_xsens_init); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); +} + +void ins_xsens_update_gps(struct GpsState *gps_s) { struct UtmCoor_f utm; - utm.east = gps.utm_pos.east / 100.; - utm.north = gps.utm_pos.north / 100.; + utm.east = gps_s->utm_pos.east / 100.; + utm.north = gps_s->utm_pos.north / 100.; utm.zone = nav_utm_zone0; - utm.alt = gps.hmsl / 1000.; + utm.alt = gps_s->hmsl / 1000.; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { - gps.ned_vel.x / 100., - gps.ned_vel.y / 100., - gps.ned_vel.z / 100. + gps_s->ned_vel.x / 100., + gps_s->ned_vel.y / 100., + gps_s->ned_vel.z / 100. }; // set velocity stateSetSpeedNed_f(&ned_vel); @@ -352,7 +363,7 @@ void xsens_periodic(void) #if USE_INS_MODULE #include "state.h" -static inline void update_fw_estimator(void) +static inline void update_state_interface(void) { // Send to Estimator (Control) #ifdef XSENS_BACKWARDS @@ -387,7 +398,7 @@ void handle_ins_msg(void) { #if USE_INS_MODULE - update_fw_estimator(); + update_state_interface(); #endif #if USE_IMU diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index ca0c46595f..57bf14c404 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -46,6 +46,7 @@ extern struct XsensTime xsens_time; extern uint8_t xsens_msg_status; extern uint16_t xsens_time_stamp; +extern void xsens_periodic(void); /* To use Xsens to just provide IMU measurements * for use with an external AHRS algorithm @@ -82,6 +83,10 @@ extern struct ImuXsens imu_xsens; #define InsEvent(_ins_handler) { \ ins_event_check_and_handle(handle_ins_msg); \ } +#define DefaultInsImpl ins_xsens +#define InsPeriodic xsens_periodic +extern void ins_xsens_init(void); +extern void ins_xsens_register(void); #endif diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index ffe462ded9..7cfe86b832 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -188,11 +188,6 @@ void ins_init(void) xsens_init(); } -void ins_periodic(void) -{ - xsens_periodic(); -} - void ins_update_gps(void) { struct UtmCoor_f utm; @@ -304,7 +299,7 @@ void xsens_periodic(void) #if USE_INS_MODULE #include "state.h" -static inline void update_fw_estimator(void) +static inline void update_state_interface(void) { // Send to Estimator (Control) #if XSENS_BACKWARDS @@ -339,7 +334,7 @@ void handle_ins_msg(void) { #if USE_INS_MODULE - update_fw_estimator(); + update_state_interface(); #endif #if USE_GPS_XSENS diff --git a/sw/airborne/subsystems/abi_common.h b/sw/airborne/subsystems/abi_common.h index 90de6f1b05..603a05afbd 100644 --- a/sw/airborne/subsystems/abi_common.h +++ b/sw/airborne/subsystems/abi_common.h @@ -32,6 +32,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" #include "math/pprz_algebra_float.h" +#include "subsystems/gps.h" /* Include here headers with structure definition you may want to use with ABI * Ex: '#include "subsystems/gps.h"' in order to use the GpsState structure */ diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c index cb05e487a1..5fa1726fb0 100644 --- a/sw/airborne/subsystems/ahrs.c +++ b/sw/airborne/subsystems/ahrs.c @@ -21,31 +21,79 @@ /** * @file subsystems/ahrs.c - * Attitude and Heading Reference System interface. + * Dispatcher to register actual AHRS implementations. */ #include "subsystems/ahrs.h" -struct Ahrs ahrs; +#ifndef PRIMARY_AHRS +#error "PRIMARY_AHRS not set!" +#else +PRINT_CONFIG_VAR(PRIMARY_AHRS) +#endif -void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps) +#ifdef SECONDARY_AHRS +PRINT_CONFIG_VAR(SECONDARY_AHRS) +#endif + +#define __RegisterAhrs(_x) _x ## _register() +#define _RegisterAhrs(_x) __RegisterAhrs(_x) +#define RegisterAhrs(_x) _RegisterAhrs(_x) + +/** maximum number of AHRS implementations that can register */ +#ifndef AHRS_NB_IMPL +#define AHRS_NB_IMPL 2 +#endif + +/** references a registered AHRS implementation */ +struct AhrsImpl { + AhrsEnableOutput enable; +}; + +struct AhrsImpl ahrs_impls[AHRS_NB_IMPL]; +uint8_t ahrs_output_idx; + +void ahrs_register_impl(AhrsEnableOutput enable) { - ahrs.init = init; - ahrs.update_gps = update_gps; - - ahrs.init(); + int i; + for (i=0; i < AHRS_NB_IMPL; i++) { + if (ahrs_impls[i].enable == NULL) { + ahrs_impls[i].enable = enable; + break; + } + } } void ahrs_init(void) { - ahrs.init = NULL; - ahrs.update_gps = NULL; + int i; + for (i=0; i < AHRS_NB_IMPL; i++) { + ahrs_impls[i].enable = NULL; + } + + RegisterAhrs(PRIMARY_AHRS); +#ifdef SECONDARY_AHRS + RegisterAhrs(SECONDARY_AHRS); +#endif + + // enable primary AHRS by default + ahrs_switch(0); } -void ahrs_update_gps(void) +int ahrs_switch(uint8_t idx) { - if (ahrs.update_gps != NULL) { - ahrs.update_gps(); + if (idx >= AHRS_NB_IMPL) { return -1; } + if (ahrs_impls[idx].enable == NULL) { return -1; } + /* first disable other AHRS output */ + int i; + for (i=0; i < AHRS_NB_IMPL; i++) { + if (ahrs_impls[i].enable != NULL) { + ahrs_impls[i].enable(FALSE); + } } + /* enable requested AHRS */ + ahrs_impls[idx].enable(TRUE); + ahrs_output_idx = idx; + return ahrs_output_idx; } diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 8d192febf5..1675c4a512 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -21,7 +21,7 @@ /** * @file subsystems/ahrs.h - * Attitude and Heading Reference System interface. + * Dispatcher to register actual AHRS implementations. */ #ifndef AHRS_H @@ -29,35 +29,37 @@ #include "std.h" -/* underlying includes (needed for parameters) */ +/* include actual (primary) implementation header */ #ifdef AHRS_TYPE_H #include AHRS_TYPE_H #endif -typedef void (*AhrsInit)(void); -typedef void (*AhrsUpdateGps)(void); +/* include secondary implementation header */ +#ifdef AHRS_SECONDARY_TYPE_H +#include AHRS_SECONDARY_TYPE_H +#endif -/** Attitude and Heading Reference System state */ -struct Ahrs { - /* function pointers to actual implementation, set by ahrs_register_impl */ - AhrsInit init; - AhrsUpdateGps update_gps; -}; +typedef bool_t (*AhrsEnableOutput)(bool_t); -/** global AHRS state */ -extern struct Ahrs ahrs; +/* for settings when using secondary AHRS */ +extern uint8_t ahrs_output_idx; -extern void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps); +/** + * Register an AHRS implementation. + * Adds it to an internal list. + * @param enable pointer to function to enable/disable the output of registering AHRS + */ +extern void ahrs_register_impl(AhrsEnableOutput enable); /** AHRS initialization. Called at startup. - * Initialized the global AHRS struct. + * Registers/initializes the default AHRS. */ extern void ahrs_init(void); -/** Update AHRS state with GPS measurements. - * Calls implementation if registered. - * Reads the global #gps data struct. +/** + * Switch to the output of another AHRS impl. + * @param idx index of the AHRS impl (0 = PRIMARY_AHRS, 1 = SECONDARY_AHRS). */ -extern void ahrs_update_gps(void); +extern int ahrs_switch(uint8_t idx); #endif /* AHRS_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index 5b567457b7..4f3a9e21ed 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -70,7 +70,9 @@ static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) void ahrs_ardrone2_register(void) { - ahrs_register_impl(ahrs_ardrone2_init, NULL); + ahrs_ardrone2_init(); + /// @TODO: provide enable function + ahrs_register_impl(NULL); } void ahrs_ardrone2_init(void) diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h index 7aa81ece64..ee1f668f6c 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.h @@ -47,7 +47,9 @@ struct AhrsARDrone { }; extern struct AhrsARDrone ahrs_ardrone2; -#define DefaultAhrsImpl ahrs_ardrone2 +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_ardrone2 +#endif extern void ahrs_ardrone2_register(void); extern void ahrs_ardrone2_init(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 0db6c94fb3..2f563b1795 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -36,7 +36,6 @@ #if USE_GPS #include "subsystems/gps.h" #endif -#include "state.h" //#include "../../test/pprz_algebra_print.h" @@ -55,7 +54,6 @@ #warning "Using magnetometer _and_ GPS course to update heading. Probably better to if you want to use GPS course." #endif - /* * default gains for correcting attitude and bias from accel/mag */ @@ -83,9 +81,6 @@ void ahrs_fc_update_mag_full(struct Int32Vect3 *mag, float dt); void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt); void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag); -static inline void compute_body_orientation_and_rates(void); - - struct AhrsFloatCmpl ahrs_fc; void ahrs_fc_init(void) @@ -138,9 +133,6 @@ bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, /* Convert initial orientation from quat to rotation matrix representations. */ float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); - /* Compute initial body orientation */ - compute_body_orientation_and_rates(); - /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, *lp_gyro); @@ -185,7 +177,6 @@ void ahrs_fc_propagate(struct Int32Rates *gyro, float dt) float_quat_normalize(&ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); #endif - compute_body_orientation_and_rates(); // increase accel and mag propagation counters ahrs_fc.accel_cnt++; @@ -221,12 +212,14 @@ void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt) * assumption: tangential velocity only along body x-axis */ const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0}; + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); + struct FloatRates body_rate; + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); struct FloatVect3 acc_c_body; - VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); + VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ @@ -403,11 +396,11 @@ void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag) } -void ahrs_fc_update_gps(void) +void ahrs_fc_update_gps(struct GpsState *gps_s) { #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS - if (gps.fix == GPS_FIX_3D) { - ahrs_fc.ltp_vel_norm = gps.speed_3d / 100.; + if (gps_s->fix == GPS_FIX_3D) { + ahrs_fc.ltp_vel_norm = gps_s->speed_3d / 100.; ahrs_fc.ltp_vel_norm_valid = TRUE; } else { ahrs_fc.ltp_vel_norm_valid = FALSE; @@ -416,12 +409,11 @@ void ahrs_fc_update_gps(void) #if AHRS_USE_GPS_HEADING && USE_GPS //got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg - if (gps.fix == GPS_FIX_3D && - gps.gspeed >= 500 && - gps.cacc <= RadOfDeg(10 * 1e7)) { + if (3dfix && gps_s->gspeed >= 500 && + gps_s->cacc <= RadOfDeg(10 * 1e7)) { - // gps.course is in rad * 1e7, we need it in rad - float course = gps.course / 1e7; + // gps_s->course is in rad * 1e7, we need it in rad + float course = gps_s->course / 1e7; if (ahrs_fc.heading_aligned) { /* the assumption here is that there is no side-slip, so heading=course */ @@ -440,12 +432,16 @@ void ahrs_fc_update_heading(float heading) FLOAT_ANGLE_NORMALIZE(heading); - struct FloatRMat *ltp_to_body_rmat = stateGetNedToBodyRMat_f(); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); + struct FloatQuat ltp_to_body_quat; + float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); + struct FloatRMat ltp_to_body_rmat; + float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat); // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y struct FloatVect2 expected_ltp = { - RMAT_ELMT(*ltp_to_body_rmat, 0, 0), - RMAT_ELMT(*ltp_to_body_rmat, 0, 1) + RMAT_ELMT(ltp_to_body_rmat, 0, 0), + RMAT_ELMT(ltp_to_body_rmat, 0, 1) }; // expected_heading cross measured_heading @@ -488,10 +484,12 @@ void ahrs_fc_realign_heading(float heading) q_h_new.qz = sinf(heading / 2.0); q_h_new.qi = cosf(heading / 2.0); - struct FloatQuat *ltp_to_body_quat = stateGetNedToBodyQuat_f(); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); + struct FloatQuat ltp_to_body_quat; + float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); /* quaternion representing current heading only */ struct FloatQuat q_h; - QUAT_COPY(q_h, *ltp_to_body_quat); + QUAT_COPY(q_h, ltp_to_body_quat); q_h.qx = 0.; q_h.qy = 0.; float_quat_normalize(&q_h); @@ -502,39 +500,15 @@ void ahrs_fc_realign_heading(float heading) /* correct current heading in body frame */ struct FloatQuat q; - float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); + float_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); - /* set state */ - stateSetNedToBodyQuat_f(&q); - ahrs_fc.heading_aligned = TRUE; } - -/** - * Compute body orientation and rates from imu orientation and rates - */ -static inline void compute_body_orientation_and_rates(void) -{ - /* Compute LTP to BODY quaternion */ - struct FloatQuat ltp_to_body_quat; - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); - float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); - /* Set state */ - stateSetNedToBodyQuat_f(<p_to_body_quat); - - /* compute body rates */ - struct FloatRates body_rate; - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); - float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); - stateSetBodyRates_f(&body_rate); -} - void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu) { ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index 38d507238a..ba271bbf3b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -33,6 +33,7 @@ #include "std.h" #include "math/pprz_algebra_float.h" #include "math/pprz_orientation_conversion.h" +#include "subsystems/gps.h" enum AhrsFCStatus { AHRS_FC_UNINIT, @@ -84,7 +85,7 @@ extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_ac extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt); extern void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt); extern void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt); -extern void ahrs_fc_update_gps(void); +extern void ahrs_fc_update_gps(struct GpsState *gps_s); /** Update yaw based on a heading measurement. * e.g. from GPS course diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c index ebc4c45442..5935f64373 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -29,8 +29,21 @@ #include "subsystems/abi.h" #include "state.h" +#ifndef AHRS_FC_OUTPUT_ENABLED +#define AHRS_FC_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_FC_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_fc_output_enabled; +static uint32_t ahrs_fc_last_stamp; + +static void compute_body_orientation_and_rates(void); + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" +#include "mcu_periph/sys_time.h" +#include "state.h" static void send_att(struct transport_tx *trans, struct link_device *dev) { @@ -53,6 +66,22 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) pprz_msg_send_GEO_MAG(trans, dev, AC_ID, &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); } + +#ifndef AHRS_FC_FILTER_ID +#define AHRS_FC_FILTER_ID 5 +#endif + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = AHRS_FC_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_fc.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_fc_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} #endif @@ -67,12 +96,14 @@ static abi_event accel_ev; static abi_event mag_ev; static abi_event aligner_ev; static abi_event body_to_imu_ev; +static abi_event geo_mag_ev; +static abi_event gps_ev; static void gyro_cb(uint8_t __attribute__((unused)) sender_id, - uint32_t __attribute__((unused)) stamp, - struct Int32Rates *gyro) + uint32_t stamp, struct Int32Rates *gyro) { + ahrs_fc_last_stamp = stamp; #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.") /* timestamp in usec when last callback was received */ @@ -81,6 +112,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_fc.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_fc_propagate(gyro, dt); + compute_body_orientation_and_rates(); } last_stamp = stamp; #else @@ -89,6 +121,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_fc.status == AHRS_FC_RUNNING) { const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); ahrs_fc_propagate(gyro, dt); + compute_body_orientation_and_rates(); } #endif } @@ -143,7 +176,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Vect3 *lp_mag) { if (!ahrs_fc.is_aligned) { - ahrs_fc_align(lp_gyro, lp_accel, lp_mag); + if (ahrs_fc_align(lp_gyro, lp_accel, lp_mag)) { + compute_body_orientation_and_rates(); + } } } @@ -153,9 +188,51 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), ahrs_fc_set_body_to_imu_quat(q_b2i_f); } +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) +{ + memcpy(&ahrs_fc.mag_h, h, sizeof(struct FloatVect3)); +} + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ahrs_fc_update_gps(gps_s); + compute_body_orientation_and_rates(); +} + +static bool_t ahrs_fc_enable_output(bool_t enable) +{ + ahrs_fc_output_enabled = enable; + return ahrs_fc_output_enabled; +} + +/** + * Compute body orientation and rates from imu orientation and rates + */ +static void compute_body_orientation_and_rates(void) +{ + if (ahrs_fc_output_enabled) { + /* Compute LTP to BODY quaternion */ + struct FloatQuat ltp_to_body_quat; + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); + float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); + /* Set state */ + stateSetNedToBodyQuat_f(<p_to_body_quat); + + /* compute body rates */ + struct FloatRates body_rate; + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); + stateSetBodyRates_f(&body_rate); + } +} + void ahrs_fc_register(void) { - ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps); + ahrs_fc_output_enabled = AHRS_FC_OUTPUT_ENABLED; + ahrs_fc_init(); + ahrs_register_impl(ahrs_fc_enable_output); /* * Subscribe to scaled IMU measurements and attach callbacks @@ -165,9 +242,12 @@ void ahrs_fc_register(void) AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h index b6ae7ed973..0f37b12ef0 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h @@ -29,7 +29,9 @@ #include "subsystems/ahrs/ahrs_float_cmpl.h" -#define DefaultAhrsImpl ahrs_fc +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_fc +#endif extern void ahrs_fc_register(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 6d1717a6ff..17e50cf965 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -34,8 +34,6 @@ #include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #include "math/pprz_algebra_float.h" -#include "state.h" - #if USE_GPS #include "subsystems/gps.h" #endif @@ -51,7 +49,6 @@ #include "subsystems/datalink/downlink.h" #endif - struct AhrsFloatDCM ahrs_dcm; // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. @@ -76,8 +73,7 @@ float MAG_Heading_X = 1; float MAG_Heading_Y = 0; #endif -static inline void compute_ahrs_representations(void); -static inline void set_body_orientation_and_rates(void); +static void compute_ahrs_representations(void); static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat); void Normalize(void); @@ -133,9 +129,6 @@ bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, /* set filter dcm */ set_dcm_matrix_from_rmat(<p_to_imu_rmat); - /* Set initial body orientation */ - set_body_orientation_and_rates(); - /* use averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, *lp_gyro); @@ -179,17 +172,17 @@ void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt) compute_ahrs_representations(); } -void ahrs_dcm_update_gps(void) +void ahrs_dcm_update_gps(struct GpsState *gps_s) { static float last_gps_speed_3d = 0; #if USE_GPS - if (gps.fix == GPS_FIX_3D) { + if (gps_s->fix == GPS_FIX_3D) { ahrs_dcm.gps_age = 0; - ahrs_dcm.gps_speed = gps.speed_3d / 100.; + ahrs_dcm.gps_speed = gps_s->speed_3d / 100.; - if (gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s - ahrs_dcm.gps_course = ((float)gps.course) / 1.e7; + if (gps_s->gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s + ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7; ahrs_dcm.gps_course_valid = TRUE; } else { ahrs_dcm.gps_course_valid = FALSE; @@ -512,27 +505,7 @@ void Matrix_update(float dt) } } -/* - * Compute body orientation and rates from imu orientation and rates - */ -static inline void set_body_orientation_and_rates(void) -{ - - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu); - - struct FloatRates body_rate; - float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate); - stateSetBodyRates_f(&body_rate); - - struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; - float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler); - float_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); - - stateSetNedToBodyRMat_f(<p_to_body_rmat); - -} - -static inline void compute_ahrs_representations(void) +static void compute_ahrs_representations(void) { #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y, accel_float.z); // atan2(acc_y,acc_z) @@ -544,25 +517,6 @@ static inline void compute_ahrs_representations(void) ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]); ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif - - set_body_orientation_and_rates(); - - /* - RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice, - &(DCM_Matrix[0][0]), - &(DCM_Matrix[0][1]), - &(DCM_Matrix[0][2]), - - &(DCM_Matrix[1][0]), - &(DCM_Matrix[1][1]), - &(DCM_Matrix[1][2]), - - &(DCM_Matrix[2][0]), - &(DCM_Matrix[2][1]), - &(DCM_Matrix[2][2]) - - )); - */ } void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index 00398af3bc..7b22e9fa8b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -27,6 +27,7 @@ #include #include "math/pprz_algebra_float.h" #include "math/pprz_orientation_conversion.h" +#include "subsystems/gps.h" enum AhrsDCMStatus { AHRS_DCM_UNINIT, @@ -87,6 +88,6 @@ extern bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_a extern void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt); extern void ahrs_dcm_update_accel(struct Int32Vect3 *accel); extern void ahrs_dcm_update_mag(struct Int32Vect3 *mag); -extern void ahrs_dcm_update_gps(void); +extern void ahrs_dcm_update_gps(struct GpsState *gps_s); #endif // AHRS_FLOAT_DCM_H diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c index 6c99c90aa0..00b2961a14 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c @@ -29,6 +29,38 @@ #include "subsystems/abi.h" #include "state.h" +#ifndef AHRS_DCM_OUTPUT_ENABLED +#define AHRS_DCM_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_DCM_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_dcm_output_enabled; +static uint32_t ahrs_dcm_last_stamp; + +static void set_body_orientation_and_rates(void); + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +#include "mcu_periph/sys_time.h" + +#ifndef AHRS_DCM_FILTER_ID +#define AHRS_DCM_FILTER_ID 6 +#endif + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = AHRS_DCM_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_dcm.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_dcm_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} +#endif + /** ABI binding for IMU data. * Used for gyro, accel and mag ABI messages. */ @@ -40,12 +72,13 @@ static abi_event accel_ev; static abi_event mag_ev; static abi_event aligner_ev; static abi_event body_to_imu_ev; +static abi_event gps_ev; static void gyro_cb(uint8_t __attribute__((unused)) sender_id, - uint32_t __attribute__((unused)) stamp, - struct Int32Rates *gyro) + uint32_t stamp, struct Int32Rates *gyro) { + ahrs_dcm_last_stamp = stamp; #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.") /* timestamp in usec when last callback was received */ @@ -53,6 +86,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_dcm.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_dcm_propagate(gyro, dt); + set_body_orientation_and_rates(); } last_stamp = stamp; #else @@ -61,6 +95,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_dcm.is_aligned) { const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); ahrs_dcm_propagate(gyro, dt); + set_body_orientation_and_rates(); } #endif } @@ -89,7 +124,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Vect3 *lp_mag) { if (!ahrs_dcm.is_aligned) { - ahrs_dcm_align(lp_gyro, lp_accel, lp_mag); + if (ahrs_dcm_align(lp_gyro, lp_accel, lp_mag)) { + set_body_orientation_and_rates(); + } } } @@ -99,9 +136,44 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), ahrs_dcm_set_body_to_imu_quat(q_b2i_f); } +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ahrs_dcm_update_gps(gps_s); +} + +static bool_t ahrs_dcm_enable_output(bool_t enable) +{ + ahrs_dcm_output_enabled = enable; + return ahrs_dcm_output_enabled; +} + +/** + * Compute body orientation and rates from imu orientation and rates + */ +static void set_body_orientation_and_rates(void) +{ + if (ahrs_dcm_output_enabled) { + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu); + + struct FloatRates body_rate; + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate); + stateSetBodyRates_f(&body_rate); + + struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; + float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler); + float_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); + + stateSetNedToBodyRMat_f(<p_to_body_rmat); + } +} + void ahrs_dcm_register(void) { - ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps); + ahrs_dcm_output_enabled = AHRS_DCM_OUTPUT_ENABLED; + ahrs_dcm_init(); + ahrs_register_impl(ahrs_dcm_enable_output); /* * Subscribe to scaled IMU measurements and attach callbacks @@ -111,4 +183,9 @@ void ahrs_dcm_register(void) AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); +#endif } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h index 8c9ba605be..7120118ec2 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h @@ -29,7 +29,9 @@ #include "subsystems/ahrs/ahrs_float_dcm.h" -#define DefaultAhrsImpl ahrs_dcm +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_dcm +#endif extern void ahrs_dcm_register(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 7d7f0fbcd3..2b5e0e8d05 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -33,8 +33,6 @@ #include /* for memcpy */ -#include "state.h" - #include "math/pprz_algebra_float.h" #include "math/pprz_algebra_int.h" #include "math/pprz_simple_matrix.h" @@ -57,7 +55,6 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured, struct FloatVect3 *noise); static inline void reset_state(void); -static inline void set_body_state_from_quat(void); struct AhrsMlkf ahrs_mlkf; @@ -115,9 +112,6 @@ bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag); - /* set initial body orientation */ - set_body_state_from_quat(); - /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, *lp_gyro); @@ -132,7 +126,6 @@ void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt) { propagate_ref(gyro, dt); propagate_state(dt); - set_body_state_from_quat(); } void ahrs_mlkf_update_accel(struct Int32Vect3 *accel) @@ -382,25 +375,3 @@ static inline void reset_state(void) float_quat_identity(&ahrs_mlkf.gibbs_cor); } - -/** - * Compute body orientation and rates from imu orientation and rates - */ -static inline void set_body_state_from_quat(void) -{ - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu); - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu); - - /* Compute LTP to BODY quaternion */ - struct FloatQuat ltp_to_body_quat; - float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat); - /* Set in state interface */ - stateSetNedToBodyQuat_f(<p_to_body_quat); - - /* compute body rates */ - struct FloatRates body_rate; - float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate); - /* Set state */ - stateSetBodyRates_f(&body_rate); - -} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c index 2bee55615b..17e4363bd9 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c @@ -28,15 +28,44 @@ #include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h" #include "subsystems/ahrs.h" #include "subsystems/abi.h" +#include "state.h" + +#ifndef AHRS_MLKF_OUTPUT_ENABLED +#define AHRS_MLKF_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_mlkf_output_enabled; +static uint32_t ahrs_mlkf_last_stamp; + +static void set_body_state_from_quat(void); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" +#include "mcu_periph/sys_time.h" static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_GEO_MAG(trans, dev, AC_ID, &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z); } + +#ifndef AHRS_MLKF_FILTER_ID +#define AHRS_MLKF_FILTER_ID 6 +#endif + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = AHRS_MLKF_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_mlkf.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_mlkf_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} #endif @@ -51,12 +80,13 @@ static abi_event accel_ev; static abi_event mag_ev; static abi_event aligner_ev; static abi_event body_to_imu_ev; +static abi_event geo_mag_ev; static void gyro_cb(uint8_t __attribute__((unused)) sender_id, - uint32_t __attribute__((unused)) stamp, - struct Int32Rates *gyro) + uint32_t stamp, struct Int32Rates *gyro) { + ahrs_mlkf_last_stamp = stamp; #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.") /* timestamp in usec when last callback was received */ @@ -65,6 +95,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_mlkf.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_mlkf_propagate(gyro, dt); + set_body_state_from_quat(); } last_stamp = stamp; #else @@ -73,6 +104,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); ahrs_mlkf_propagate(gyro, dt); + set_body_state_from_quat(); } #endif } @@ -83,6 +115,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)), { if (ahrs_mlkf.is_aligned) { ahrs_mlkf_update_accel(accel); + set_body_state_from_quat(); } } @@ -92,6 +125,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)), { if (ahrs_mlkf.is_aligned) { ahrs_mlkf_update_mag(mag); + set_body_state_from_quat(); } } @@ -101,7 +135,10 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Vect3 *lp_mag) { if (!ahrs_mlkf.is_aligned) { - ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag); + /* set initial body orientation in state interface if alignment was successful */ + if (ahrs_mlkf_align(lp_gyro, lp_accel, lp_mag)) { + set_body_state_from_quat(); + } } } @@ -111,9 +148,45 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), ahrs_mlkf_set_body_to_imu_quat(q_b2i_f); } +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) +{ + memcpy(&ahrs_mlkf.mag_h, h, sizeof(struct FloatVect3)); +} + +static bool_t ahrs_mlkf_enable_output(bool_t enable) +{ + ahrs_mlkf_output_enabled = enable; + return ahrs_mlkf_output_enabled; +} + +/** + * Compute body orientation and rates from imu orientation and rates + */ +static void set_body_state_from_quat(void) +{ + if (ahrs_mlkf_output_enabled) { + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu); + + /* Compute LTP to BODY quaternion */ + struct FloatQuat ltp_to_body_quat; + float_quat_comp_inv(<p_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat); + /* Set in state interface */ + stateSetNedToBodyQuat_f(<p_to_body_quat); + + /* compute body rates */ + struct FloatRates body_rate; + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate); + /* Set state */ + stateSetBodyRates_f(&body_rate); + } +} + void ahrs_mlkf_register(void) { - ahrs_register_impl(ahrs_mlkf_init, NULL); + ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED; + ahrs_mlkf_init(); + ahrs_register_impl(ahrs_mlkf_enable_output); /* * Subscribe to scaled IMU measurements and attach callbacks @@ -123,8 +196,11 @@ void ahrs_mlkf_register(void) AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif } + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h index 4c623a75e3..cb394a23e7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h @@ -29,7 +29,9 @@ #include "subsystems/ahrs/ahrs_float_mlkf.h" -#define DefaultAhrsImpl ahrs_mlkf +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_mlkf +#endif extern void ahrs_mlkf_register(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c index d72bf0c17b..ced34bd764 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c @@ -339,7 +339,9 @@ void ahrs_gx3_init(void) void ahrs_gx3_register(void) { - ahrs_register_impl(ahrs_gx3_init, NULL); + ahrs_gx3_init(); + /// @TODO: provide enable function + ahrs_register_impl(NULL); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.h b/sw/airborne/subsystems/ahrs/ahrs_gx3.h index 34680e04ac..2c82cc21a2 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.h +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.h @@ -83,7 +83,10 @@ struct AhrsGX3 { }; extern struct AhrsGX3 ahrs_gx3; -#define DefaultAhrsImpl ahrs_gx3 + +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_gx3 +#endif extern void ahrs_gx3_init(void); extern void ahrs_gx3_align(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index ba6f6899af..9a1c5c0f75 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -30,8 +30,6 @@ #include "ahrs_int_cmpl_euler.h" -#include "state.h" -#include "subsystems/abi.h" #include "math/pprz_trig_int.h" #include "math/pprz_algebra_int.h" @@ -51,7 +49,6 @@ static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_ struct Int32Vect3 *accel); static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 *mag); -static inline void set_body_state_from_euler(void); #define F_UPDATE 512 @@ -94,8 +91,6 @@ bool_t ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE); - set_body_state_from_euler(); - RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro); ahrs_ice.status = AHRS_ICE_RUNNING; @@ -202,9 +197,6 @@ void ahrs_ice_propagate(struct Int32Rates *gyro) /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE); - - set_body_state_from_euler(); - } void ahrs_ice_update_accel(struct Int32Vect3 *accel) @@ -282,26 +274,6 @@ __attribute__((always_inline)) static inline void get_psi_measurement_from_mag(i } -/* Rotate angles and rates from imu to body frame and set state */ -static void set_body_state_from_euler(void) -{ - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_ice.body_to_imu); - struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; - /* Compute LTP to IMU rotation matrix */ - int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_ice.ltp_to_imu_euler); - /* Compute LTP to BODY rotation matrix */ - int32_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); - /* Set state */ - stateSetNedToBodyRMat_i(<p_to_body_rmat); - - struct Int32Rates body_rate; - /* compute body rates */ - int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_ice.imu_rate); - /* Set state */ - stateSetBodyRates_i(&body_rate); - -} - void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu) { ahrs_ice_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c index 9937e44db8..ff34ca10ea 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c @@ -29,8 +29,21 @@ #include "subsystems/abi.h" #include "state.h" +#ifndef AHRS_ICE_OUTPUT_ENABLED +#define AHRS_ICE_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_ICE_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_ice_output_enabled; +static uint32_t ahrs_ice_last_stamp; + +static void set_body_state_from_euler(void); + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" +#include "mcu_periph/sys_time.h" +#include "state.h" static void send_filter(struct transport_tx *trans, struct link_device *dev) { @@ -69,6 +82,22 @@ static void send_bias(struct transport_tx *trans, struct link_device *dev) pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, &ahrs_ice.gyro_bias.r); } + +#ifndef AHRS_ICE_FILTER_ID +#define AHRS_ICE_FILTER_ID 4 +#endif + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = AHRS_ICE_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_ice.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_ice_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} #endif /** ABI binding for IMU data. @@ -85,13 +114,13 @@ static abi_event body_to_imu_ev; static void gyro_cb(uint8_t sender_id __attribute__((unused)), - uint32_t stamp __attribute__((unused)), - struct Int32Rates *gyro) + uint32_t stamp, struct Int32Rates *gyro) { + ahrs_ice_last_stamp = stamp; if (ahrs_ice.is_aligned) { ahrs_ice_propagate(gyro); + set_body_state_from_euler(); } - } static void accel_cb(uint8_t sender_id __attribute__((unused)), @@ -118,7 +147,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Vect3 *lp_mag) { if (!ahrs_ice.is_aligned) { - ahrs_ice_align(lp_gyro, lp_accel, lp_mag); + if (ahrs_ice_align(lp_gyro, lp_accel, lp_mag)) { + set_body_state_from_euler(); + } } } @@ -128,9 +159,38 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), ahrs_ice_set_body_to_imu_quat(q_b2i_f); } +static bool_t ahrs_ice_enable_output(bool_t enable) +{ + ahrs_ice_output_enabled = enable; + return ahrs_ice_output_enabled; +} + +/* Rotate angles and rates from imu to body frame and set state */ +static void set_body_state_from_euler(void) +{ + if (ahrs_ice_output_enabled) { + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_ice.body_to_imu); + struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; + /* Compute LTP to IMU rotation matrix */ + int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_ice.ltp_to_imu_euler); + /* Compute LTP to BODY rotation matrix */ + int32_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); + /* Set state */ + stateSetNedToBodyRMat_i(<p_to_body_rmat); + + struct Int32Rates body_rate; + /* compute body rates */ + int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_ice.imu_rate); + /* Set state */ + stateSetBodyRates_i(&body_rate); + } +} + void ahrs_ice_register(void) { - ahrs_register_impl(ahrs_ice_init, NULL); + ahrs_ice_output_enabled = AHRS_ICE_OUTPUT_ENABLED; + ahrs_ice_init(); + ahrs_register_impl(ahrs_ice_enable_output); /* * Subscribe to scaled IMU measurements and attach callbacks @@ -145,5 +205,6 @@ void ahrs_ice_register(void) register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif } diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h index 051ce689f2..119be53543 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h @@ -29,7 +29,9 @@ #include "subsystems/ahrs/ahrs_int_cmpl_euler.h" -#define DefaultAhrsImpl ahrs_ice +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_ice +#endif extern void ahrs_ice_register(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 7aa22a9997..28c58035e7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -32,7 +32,6 @@ #include "subsystems/ahrs/ahrs_int_cmpl_quat.h" #include "subsystems/ahrs/ahrs_int_utils.h" -#include "state.h" #if USE_GPS #include "subsystems/gps.h" @@ -60,7 +59,6 @@ PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES") #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE" #endif - /* * default gains for correcting attitude and bias from accel/mag */ @@ -104,7 +102,6 @@ PRINT_CONFIG_VAR(AHRS_MAG_ZETA) struct AhrsIntCmplQuat ahrs_icq; -static inline void set_body_state_from_quat(void); static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt); static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt); @@ -168,8 +165,6 @@ bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, lp_mag = lp_mag; #endif - set_body_state_from_quat(); - /* Use low passed gyro value as initial bias */ RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro); RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro); @@ -209,8 +204,6 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt) &omega, freq); int32_quat_normalize(&ahrs_icq.ltp_to_imu_quat); - set_body_state_from_quat(); - // increase accel and mag propagation counters ahrs_icq.accel_cnt++; ahrs_icq.mag_cnt++; @@ -270,14 +263,16 @@ void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt) #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = - {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; + {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); + struct Int32Rates body_rate; + int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); struct Int32Vect3 acc_c_body; - VECT3_RATES_CROSS_VECT3(acc_c_body, (*stateGetBodyRates_i()), vel_tangential_body); + VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); 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; - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement @@ -514,11 +509,11 @@ static inline void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt) } -void ahrs_icq_update_gps(void) +void ahrs_icq_update_gps(struct GpsState *gps_s __attribute__((unused))) { #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS - if (gps.fix == GPS_FIX_3D) { - ahrs_icq.ltp_vel_norm = SPEED_BFP_OF_REAL(gps.speed_3d / 100.); + if (gps_s->fix == GPS_FIX_3D) { + ahrs_icq.ltp_vel_norm = SPEED_BFP_OF_REAL(gps_s->speed_3d / 100.); ahrs_icq.ltp_vel_norm_valid = TRUE; } else { ahrs_icq.ltp_vel_norm_valid = FALSE; @@ -528,12 +523,12 @@ void ahrs_icq_update_gps(void) #if AHRS_USE_GPS_HEADING && USE_GPS // got a 3d fix, ground speed > AHRS_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s) // and course accuracy is better than 10deg - if (gps.fix == GPS_FIX_3D && - gps.gspeed >= (AHRS_HEADING_UPDATE_GPS_MIN_SPEED * 100) && - gps.cacc <= RadOfDeg(10 * 1e7)) { + if (gps_s->fix == GPS_FIX_3D && + gps_s->gspeed >= (AHRS_HEADING_UPDATE_GPS_MIN_SPEED * 100) && + gps_s->cacc <= RadOfDeg(10 * 1e7)) { - // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC - int32_t course = gps.course * ((1 << INT32_ANGLE_FRAC) / 1e7); + // gps_s->course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC + int32_t course = gps_s->course * ((1 << INT32_ANGLE_FRAC) / 1e7); /* the assumption here is that there is no side-slip, so heading=course */ @@ -555,10 +550,14 @@ void ahrs_icq_update_heading(int32_t heading) // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y - struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i(); + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); + struct Int32Quat ltp_to_body_quat; + int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat); + struct Int32RMat ltp_to_body_rmat; + int32_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat); struct Int32Vect2 expected_ltp = { - RMAT_ELMT((*ltp_to_body_rmat), 0, 0), - RMAT_ELMT((*ltp_to_body_rmat), 0, 1) + RMAT_ELMT(ltp_to_body_rmat, 0, 0), + RMAT_ELMT(ltp_to_body_rmat, 0, 1) }; int32_t heading_x, heading_y; @@ -608,8 +607,9 @@ void ahrs_icq_update_heading(int32_t heading) void ahrs_icq_realign_heading(int32_t heading) { - - struct Int32Quat ltp_to_body_quat = *stateGetNedToBodyQuat_i(); + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); + struct Int32Quat ltp_to_body_quat; + int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat); /* quaternion representing only the heading rotation from ltp to body */ struct Int32Quat q_h_new; @@ -635,34 +635,11 @@ void ahrs_icq_realign_heading(int32_t heading) QUAT_COPY(ltp_to_body_quat, q); /* compute ltp to imu rotations */ - struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); int32_quat_comp(&ahrs_icq.ltp_to_imu_quat, <p_to_body_quat, body_to_imu_quat); - /* Set state */ - stateSetNedToBodyQuat_i(<p_to_body_quat); - ahrs_icq.heading_aligned = TRUE; } - -/* Rotate angles and rates from imu to body frame and set state */ -static inline void set_body_state_from_quat(void) -{ - /* Compute LTP to BODY quaternion */ - struct Int32Quat ltp_to_body_quat; - struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); - int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat); - /* Set state */ - stateSetNedToBodyQuat_i(<p_to_body_quat); - - /* compute body rates */ - struct Int32Rates body_rate; - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); - int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); - /* Set state */ - stateSetBodyRates_i(&body_rate); -} - void ahrs_icq_set_body_to_imu(struct OrientationReps *body_to_imu) { ahrs_icq_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index 93a0500899..59ea681ebb 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -32,6 +32,7 @@ #define AHRS_INT_CMPL_QUAT_H #include "subsystems/ahrs.h" +#include "subsystems/gps.h" #include "std.h" #include "math/pprz_algebra_int.h" #include "math/pprz_orientation_conversion.h" @@ -113,7 +114,7 @@ extern bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_a extern void ahrs_icq_propagate(struct Int32Rates *gyro, float dt); extern void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt); extern void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt); -extern void ahrs_icq_update_gps(void); +extern void ahrs_icq_update_gps(struct GpsState *gps_s); /** Update yaw based on a heading measurement. * e.g. from GPS course diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c index 5c714181ef..eef1b97313 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c @@ -29,8 +29,22 @@ #include "subsystems/abi.h" #include "state.h" +#ifndef AHRS_ICQ_OUTPUT_ENABLED +#define AHRS_ICQ_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_ICQ_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_icq_output_enabled; +static uint32_t ahrs_icq_last_stamp; + +static void set_body_state_from_quat(void); + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" +#include "mcu_periph/sys_time.h" +#include "state.h" + static void send_quat(struct transport_tx *trans, struct link_device *dev) { struct Int32Quat *quat = stateGetNedToBodyQuat_i(); @@ -75,6 +89,22 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) pprz_msg_send_GEO_MAG(trans, dev, AC_ID, &h_float.x, &h_float.y, &h_float.z); } + +#ifndef AHRS_ICQ_FILTER_ID +#define AHRS_ICQ_FILTER_ID 3 +#endif + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = AHRS_ICQ_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_icq.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_icq_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} #endif @@ -89,12 +119,14 @@ static abi_event accel_ev; static abi_event mag_ev; static abi_event aligner_ev; static abi_event body_to_imu_ev; +static abi_event geo_mag_ev; +static abi_event gps_ev; static void gyro_cb(uint8_t __attribute__((unused)) sender_id, - uint32_t __attribute__((unused)) stamp, - struct Int32Rates *gyro) + uint32_t stamp, struct Int32Rates *gyro) { + ahrs_icq_last_stamp = stamp; #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.") /* timestamp in usec when last callback was received */ @@ -103,6 +135,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_icq.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_icq_propagate(gyro, dt); + set_body_state_from_quat(); } last_stamp = stamp; #else @@ -111,6 +144,7 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_icq.status == AHRS_ICQ_RUNNING) { const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); ahrs_icq_propagate(gyro, dt); + set_body_state_from_quat(); } #endif } @@ -125,6 +159,7 @@ static void accel_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_icq.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_icq_update_accel(accel, dt); + set_body_state_from_quat(); } last_stamp = stamp; #else @@ -133,6 +168,7 @@ static void accel_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_icq.is_aligned) { const float dt = 1. / (AHRS_CORRECT_FREQUENCY); ahrs_icq_update_accel(accel, dt); + set_body_state_from_quat(); } #endif } @@ -147,6 +183,7 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id, if (last_stamp > 0 && ahrs_icq.is_aligned) { float dt = (float)(stamp - last_stamp) * 1e-6; ahrs_icq_update_mag(mag, dt); + set_body_state_from_quat(); } last_stamp = stamp; #else @@ -155,6 +192,7 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id, if (ahrs_icq.is_aligned) { const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); ahrs_icq_update_mag(mag, dt); + set_body_state_from_quat(); } #endif } @@ -165,7 +203,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, struct Int32Vect3 *lp_mag) { if (!ahrs_icq.is_aligned) { - ahrs_icq_align(lp_gyro, lp_accel, lp_mag); + if (ahrs_icq_align(lp_gyro, lp_accel, lp_mag)) { + set_body_state_from_quat(); + } } } @@ -175,9 +215,50 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), ahrs_icq_set_body_to_imu_quat(q_b2i_f); } +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) +{ + VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(h->x), MAG_BFP_OF_REAL(h->y), + MAG_BFP_OF_REAL(h->z)); +} + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ahrs_icq_update_gps(gps_s); +} + +static bool_t ahrs_icq_enable_output(bool_t enable) +{ + ahrs_icq_output_enabled = enable; + return ahrs_icq_output_enabled; +} + +/** Rotate angles and rates from imu to body frame and set state */ +static void set_body_state_from_quat(void) +{ + if (ahrs_icq_output_enabled) { + /* Compute LTP to BODY quaternion */ + struct Int32Quat ltp_to_body_quat; + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); + int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat); + /* Set state */ + stateSetNedToBodyQuat_i(<p_to_body_quat); + + /* compute body rates */ + struct Int32Rates body_rate; + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); + int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); + /* Set state */ + stateSetBodyRates_i(&body_rate); + } +} + void ahrs_icq_register(void) { - ahrs_register_impl(ahrs_icq_init, ahrs_icq_update_gps); + ahrs_icq_output_enabled = AHRS_ICQ_OUTPUT_ENABLED; + ahrs_icq_init(); + ahrs_register_impl(ahrs_icq_enable_output); /* * Subscribe to scaled IMU measurements and attach callbacks @@ -187,11 +268,14 @@ void ahrs_icq_register(void) AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif } diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h index e64c3a9b04..ec688b28df 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h @@ -29,7 +29,9 @@ #include "subsystems/ahrs/ahrs_int_cmpl_quat.h" -#define DefaultAhrsImpl ahrs_icq +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_icq +#endif extern void ahrs_icq_register(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c index 2dd120f445..8d3e59a007 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.c +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c @@ -61,6 +61,8 @@ void update_ahrs_from_sim(void) } -void ahrs_sim_init(void) +void ahrs_sim_register(void) { + // dummy, simple ocaml sim only supports one basic fake AHRS anyway + ahrs_register_impl(NULL); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.h b/sw/airborne/subsystems/ahrs/ahrs_sim.h index 586bd2b4c7..acf3b0952f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_sim.h +++ b/sw/airborne/subsystems/ahrs/ahrs_sim.h @@ -36,6 +36,8 @@ extern float ins_roll_neutral; extern float ins_pitch_neutral; extern void update_ahrs_from_sim(void); -extern void ahrs_sim_init(void); +extern void ahrs_sim_register(void); + +#define PRIMARY_AHRS ahrs_sim #endif /* AHRS_SIM_H */ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index d12f0b3c4d..50e97d4d4d 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -33,13 +33,42 @@ #include "state.h" #endif +#ifndef DefaultInsImpl +#warning "DefaultInsImpl not set!" +#else +PRINT_CONFIG_VAR(DefaultInsImpl) +#endif + +#define __DefaultInsRegister(_x) _x ## _register() +#define _DefaultInsRegister(_x) __DefaultInsRegister(_x) +#define DefaultInsRegister() _DefaultInsRegister(DefaultInsImpl) + +/** Inertial Navigation System state */ +struct Ins { + InsInit init; +}; + struct Ins ins; +void ins_register_impl(InsInit init) +{ + ins.init = init; + + ins.init(); +} + +void ins_init(void) +{ + ins.init = NULL; + +#ifdef DefaultInsImpl + DefaultInsRegister(); +#endif +} + // weak functions, used if not explicitly provided by implementation -void WEAK ins_periodic(void) {} - void WEAK ins_reset_local_origin(void) { #if USE_GPS @@ -83,7 +112,3 @@ void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm) void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm __attribute__((unused))) {} #endif -void WEAK ins_propagate(float dt __attribute__((unused))) {} - -void WEAK ins_update_gps(void) {} - diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index 771cfd4a91..45d2f3791e 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -32,34 +32,20 @@ #include "math/pprz_algebra_float.h" #include "state.h" -enum InsStatus { - INS_UNINIT = 0, - INS_RUNNING = 1 -}; - /* underlying includes (needed for parameters) */ #ifdef INS_TYPE_H #include INS_TYPE_H #endif -/** Inertial Navigation System state */ -struct Ins { - enum InsStatus status; ///< status of the INS -}; +typedef void (*InsInit)(void); -/** global INS state */ -extern struct Ins ins; +extern void ins_register_impl(InsInit init); /** INS initialization. Called at startup. - * Needs to be implemented by each INS algorithm. + * Initializes the global ins struct. */ extern void ins_init(void); -/** INS periodic call. - * Does nothing if not implemented by specific INS algorithm. - */ -extern void ins_periodic(void); - /** INS local origin reset. * Reset horizontal and vertical reference to the current position. * Does nothing if not implemented by specific INS algorithm. @@ -80,17 +66,4 @@ extern void ins_reset_altitude_ref(void); */ extern void ins_reset_utm_zone(struct UtmCoor_f *utm); -/** Propagation. Usually integrates the gyro rates to angles. - * Reads the global #imu data struct. - * Does nothing if not implemented by specific INS algorithm. - * @param dt time difference since last propagation in seconds - */ -extern void ins_propagate(float dt); - -/** Update INS state with GPS measurements. - * Reads the global #gps data struct. - * Does nothing if not implemented by specific INS algorithm. - */ -extern void ins_update_gps(void); - #endif /* INS_H */ diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 848cfcb389..12ba4fc380 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -49,7 +49,7 @@ #endif -struct InsAltFloat ins_impl; +struct InsAltFloat ins_altf; #if USE_BAROMETER #include "subsystems/sensors/baro.h" @@ -70,11 +70,15 @@ abi_event baro_ev; static void baro_cb(uint8_t sender_id, float pressure); #endif /* USE_BAROMETER */ +abi_event gps_ev; + static void alt_kalman_reset(void); static void alt_kalman_init(void); static void alt_kalman(float z_meas, float dt); -void ins_init(void) +void ins_alt_float_update_gps(struct GpsState *gps_s); + +void ins_alt_float_init(void) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 }; @@ -85,21 +89,16 @@ void ins_init(void) alt_kalman_init(); #if USE_BAROMETER - ins_impl.qfe = 0.0f; - ins_impl.baro_initialized = FALSE; - ins_impl.baro_alt = 0.0f; - // Bind to BARO_ABS message - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); + ins_altf.qfe = 0.0f; + ins_altf.baro_initialized = FALSE; + ins_altf.baro_alt = 0.0f; #endif - ins_impl.reset_alt_ref = FALSE; + ins_altf.reset_alt_ref = FALSE; // why do we have this here? alt_kalman(0.0f, 0.1); - - ins.status = INS_RUNNING; } - /** Reset the geographic reference to the current GPS fix */ void ins_reset_local_origin(void) { @@ -122,7 +121,7 @@ void ins_reset_local_origin(void) stateSetLocalUtmOrigin_f(&utm); // reset filter flag - ins_impl.reset_alt_ref = TRUE; + ins_altf.reset_alt_ref = TRUE; } void ins_reset_altitude_ref(void) @@ -133,12 +132,12 @@ void ins_reset_altitude_ref(void) // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); // reset filter flag - ins_impl.reset_alt_ref = TRUE; + ins_altf.reset_alt_ref = TRUE; } #if USE_BAROMETER -static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) +void ins_alt_float_update_baro(float pressure) { // timestamp in usec when last callback was received static uint32_t last_ts = 0; @@ -151,40 +150,44 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) // bound dt (assume baro freq 1Hz-500Hz Bound(dt, 0.002, 1.0) - if (!ins_impl.baro_initialized) { - ins_impl.qfe = pressure; - ins_impl.baro_initialized = TRUE; + if (!ins_altf.baro_initialized) { + ins_altf.qfe = pressure; + ins_altf.baro_initialized = TRUE; } - if (ins_impl.reset_alt_ref) { - ins_impl.reset_alt_ref = FALSE; - ins_impl.alt = ground_alt; - ins_impl.alt_dot = 0.0f; - ins_impl.qfe = pressure; + if (ins_altf.reset_alt_ref) { + ins_altf.reset_alt_ref = FALSE; + ins_altf.alt = ground_alt; + ins_altf.alt_dot = 0.0f; + ins_altf.qfe = pressure; alt_kalman_reset(); } else { /* not realigning, so normal update with baro measurement */ - ins_impl.baro_alt = ground_alt + pprz_isa_height_of_pressure(pressure, ins_impl.qfe); + ins_altf.baro_alt = ground_alt + pprz_isa_height_of_pressure(pressure, ins_altf.qfe); /* run the filter */ - alt_kalman(ins_impl.baro_alt, dt); + alt_kalman(ins_altf.baro_alt, dt); /* set new altitude, just copy old horizontal position */ struct UtmCoor_f utm; UTM_COPY(utm, *stateGetPositionUtm_f()); - utm.alt = ins_impl.alt; + utm.alt = ins_altf.alt; stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel; memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); - ned_vel.z = -ins_impl.alt_dot; + ned_vel.z = -ins_altf.alt_dot; stateSetSpeedNed_f(&ned_vel); } } +#else +void ins_alt_float_update_baro(float pressure __attribute__((unused))) +{ +} #endif -void ins_update_gps(void) +void ins_alt_float_update_gps(struct GpsState *gps_s) { #if USE_GPS struct UtmCoor_f utm; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; + utm.east = gps_s->utm_pos.east / 100.0f; + utm.north = gps_s->utm_pos.north / 100.0f; utm.zone = nav_utm_zone0; #if !USE_BAROMETER @@ -203,25 +206,25 @@ void ins_update_gps(void) Bound(dt, 0.02, 2) #endif - float falt = gps.hmsl / 1000.0f; - if (ins_impl.reset_alt_ref) { - ins_impl.reset_alt_ref = FALSE; - ins_impl.alt = falt; - ins_impl.alt_dot = 0.0f; + float falt = gps_s->hmsl / 1000.0f; + if (ins_altf.reset_alt_ref) { + ins_altf.reset_alt_ref = FALSE; + ins_altf.alt = falt; + ins_altf.alt_dot = 0.0f; alt_kalman_reset(); } else { alt_kalman(falt, dt); - ins_impl.alt_dot = -gps.ned_vel.z / 100.0f; + ins_altf.alt_dot = -gps_s->ned_vel.z / 100.0f; } #endif - utm.alt = ins_impl.alt; + utm.alt = ins_altf.alt; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { - gps.ned_vel.x / 100.0f, - gps.ned_vel.y / 100.0f, - -ins_impl.alt_dot + gps_s->ned_vel.x / 100.0f, + gps_s->ned_vel.y / 100.0f, + -ins_altf.alt_dot }; // set velocity stateSetSpeedNed_f(&ned_vel); @@ -293,7 +296,7 @@ static void alt_kalman(float z_meas, float dt) /* predict */ - ins_impl.alt += ins_impl.alt_dot * dt; + ins_altf.alt += ins_altf.alt_dot * dt; p[0][0] = p[0][0] + p[1][0] * dt + dt * (p[0][1] + p[1][1] * dt) + SIGMA2 * q[0][0]; p[0][1] = p[0][1] + p[1][1] * dt + SIGMA2 * q[0][1]; p[1][0] = p[1][0] + p[1][1] * dt + SIGMA2 * q[1][0]; @@ -305,11 +308,11 @@ static void alt_kalman(float z_meas, float dt) if (fabs(e) > 1e-5) { float k_0 = p[0][0] / e; float k_1 = p[1][0] / e; - e = z_meas - ins_impl.alt; + e = z_meas - ins_altf.alt; /* correction */ - ins_impl.alt += k_0 * e; - ins_impl.alt_dot += k_1 * e; + ins_altf.alt += k_0 * e; + ins_altf.alt_dot += k_1 * e; p[1][0] = -p[0][0] * k_1 + p[1][0]; p[1][1] = -p[0][1] * k_1 + p[1][1]; @@ -322,3 +325,27 @@ static void alt_kalman(float z_meas, float dt) #endif } +#if USE_BAROMETER +static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) +{ + ins_alt_float_update_baro(pressure); +} +#endif + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ins_alt_float_update_gps(gps_s); +} + +void ins_altf_register(void) +{ + ins_register_impl(ins_alt_float_init); + +#if USE_BAROMETER + // Bind to BARO_ABS message + AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); +#endif + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); +} diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 78d78ee9d8..7ced685cb2 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -47,6 +47,14 @@ struct InsAltFloat { #endif }; -extern struct InsAltFloat ins_impl; +extern struct InsAltFloat ins_altf; + +extern void ins_alt_float_init(void); +extern void ins_alt_float_update_baro(float pressure); + +#ifndef DefaultInsImpl +#define DefaultInsImpl ins_altf +#endif +extern void ins_altf_register(void); #endif /* INS_ALT_FLOAT_H */ diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index b081d7f7ca..27209aaa54 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -41,9 +41,9 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif -struct InsArdrone2 ins_impl; +struct InsArdrone2 ins_ardrone2; -void ins_init() +void ins_ardrone2_init(void) { #if USE_INS_NAV_INIT struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ @@ -55,42 +55,35 @@ void ins_init() 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); + ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &ecef_nav0); + ins_ardrone2.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); - ins_impl.ltp_initialized = TRUE; + ins_ardrone2.ltp_initialized = TRUE; #else - ins_impl.ltp_initialized = FALSE; + ins_ardrone2.ltp_initialized = FALSE; #endif - 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; - } + INT32_VECT3_ZERO(ins_ardrone2.ltp_pos); + INT32_VECT3_ZERO(ins_ardrone2.ltp_speed); + INT32_VECT3_ZERO(ins_ardrone2.ltp_accel); } void ins_reset_local_origin(void) { #if USE_GPS if (gps.fix == GPS_FIX_3D) { - 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); + ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &gps.ecef_pos); + ins_ardrone2.ltp_def.lla.alt = gps.lla_pos.alt; + ins_ardrone2.ltp_def.hmsl = gps.hmsl; + ins_ardrone2.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); } else { - ins_impl.ltp_initialized = FALSE; + ins_ardrone2.ltp_initialized = FALSE; } #else - ins_impl.ltp_initialized = FALSE; + ins_ardrone2.ltp_initialized = FALSE; #endif } @@ -102,65 +95,82 @@ void ins_reset_altitude_ref(void) .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; - ltp_def_from_lla_i(&ins_impl.ltp_def, &lla); - ins_impl.ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_impl.ltp_def); + ltp_def_from_lla_i(&ins_ardrone2.ltp_def, &lla); + ins_ardrone2.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_ardrone2.ltp_def); #endif } -void ins_propagate(float __attribute__((unused)) dt) +void ins_ardrone2_periodic(void) { /* untilt accels and speeds */ - float_rmat_transp_vmult((struct FloatVect3 *)&ins_impl.ltp_accel, + float_rmat_transp_vmult((struct FloatVect3 *)&ins_ardrone2.ltp_accel, stateGetNedToBodyRMat_f(), (struct FloatVect3 *)&ahrs_ardrone2.accel); - float_rmat_transp_vmult((struct FloatVect3 *)&ins_impl.ltp_speed, + float_rmat_transp_vmult((struct FloatVect3 *)&ins_ardrone2.ltp_speed, stateGetNedToBodyRMat_f(), (struct FloatVect3 *)&ahrs_ardrone2.speed); //Add g to the accelerations - ins_impl.ltp_accel.z += 9.81; + ins_ardrone2.ltp_accel.z += 9.81; //Save the accelerations and speeds - stateSetAccelNed_f(&ins_impl.ltp_accel); - stateSetSpeedNed_f(&ins_impl.ltp_speed); + stateSetAccelNed_f(&ins_ardrone2.ltp_accel); + stateSetSpeedNed_f(&ins_ardrone2.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_impl.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; - stateSetPositionNed_i(&ins_impl.ltp_pos); + ins_ardrone2.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN; + stateSetPositionNed_i(&ins_ardrone2.ltp_pos); #endif } -void ins_update_gps(void) +void ins_ardrone2_update_gps(void) { #if USE_GPS //Check for GPS fix if (gps.fix == GPS_FIX_3D) { //Set the initial coordinates - 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); + if (!ins_ardrone2.ltp_initialized) { + ltp_def_from_ecef_i(&ins_ardrone2.ltp_def, &gps.ecef_pos); + ins_ardrone2.ltp_def.lla.alt = gps.lla_pos.alt; + ins_ardrone2.ltp_def.hmsl = gps.hmsl; + ins_ardrone2.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_ardrone2.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_impl.ltp_def, &gps.ecef_pos); + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ardrone2.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_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT3_SCALE_2(ins_ardrone2.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); #else - INT32_VECT2_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + INT32_VECT2_SCALE_2(ins_ardrone2.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_impl.ltp_pos); + stateSetPositionNed_i(&ins_ardrone2.ltp_pos); } #endif /* USE_GPS */ } + +#include "subsystems/abi.h" +static abi_event gps_ev; +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ins_ardrone2_update_gps(); +} + +void ins_ardrone2_register(void) +{ + ins_register_impl(ins_ardrone2_init); + + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + // FIXME: ins_ardrone2_periodic is currently called via InsPeriodic hack directly from main +} diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h index 0558ab4e10..9c703d82d0 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.h +++ b/sw/airborne/subsystems/ins/ins_ardrone2.h @@ -44,6 +44,15 @@ struct InsArdrone2 { struct NedCoor_f ltp_accel; }; -extern struct InsArdrone2 ins_impl; +extern struct InsArdrone2 ins_ardrone2; + +#define DefaultInsImpl ins_ardrone2 +#define InsPeriodic ins_ardrone2_periodic + +extern void ins_ardrone2_init(void); +extern void ins_ardrone2_periodic(void); +extern void ins_ardrone2_update_gps(void); + +extern void ins_ardrone2_register(void); #endif /* INS_ARDRONE2_SDK_H */ diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index d5d4e6ed50..5c2c2d0c3c 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -16,18 +16,14 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, see * . - * */ - /** * @file subsystems/ins/ins_float_invariant.c * @author Jean-Philippe Condomines * * INS using invariant filter. * - * Only for fixedwing currenctly - * */ #include "subsystems/ins/ins_float_invariant.h" @@ -37,11 +33,10 @@ #include "subsystems/ins.h" #include "subsystems/gps.h" -#include "subsystems/imu.h" #include "generated/airframe.h" #include "generated/flight_plan.h" -#if INS_UPDATE_FW_ESTIMATOR +#if INS_FINV_USE_UTM #include "firmwares/fixedwing/nav.h" #endif @@ -50,30 +45,13 @@ #include "math/pprz_rk_float.h" #include "math/pprz_isa.h" -#include "subsystems/abi.h" #include "state.h" -#include "led.h" - -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY +// for debugging +#if SEND_INVARIANT_FILTER #include "subsystems/datalink/telemetry.h" -static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) -{ - float foo = 0.; - if (state.ned_initialized_i) { - pprz_msg_send_INS_REF(trans, dev, AC_ID, - &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, &foo); - } -} #endif - #if LOG_INVARIANT_FILTER #include "sdLog.h" #include "subsystems/chibios-libopencm3/chibios_sdlog.h" @@ -158,43 +136,17 @@ bool_t log_started = FALSE; #endif -struct InsFloatInv ins_impl; +struct InsFloatInv ins_float_inv; /* earth gravity model */ static const struct FloatVect3 A = { 0.f, 0.f, 9.81f }; /* earth magnetic model */ //static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) }; -#define B ins_impl.mag_h +#define B ins_float_inv.mag_h /* barometer */ bool_t ins_baro_initialized; -// Baro event on ABI -#ifndef INS_BARO_ID -#if USE_BARO_BOARD -#define INS_BARO_ID BARO_BOARD_SENDER_ID -#else -#define INS_BARO_ID ABI_BROADCAST -#endif -#endif -PRINT_CONFIG_VAR(INS_BARO_ID) -abi_event baro_ev; -static void baro_cb(uint8_t sender_id, float pressure); - -/* magnetometer */ -#ifndef INS_MAG_ID -#define INS_MAG_ID ABI_BROADCAST -#endif -static abi_event mag_ev; -static void mag_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, - struct Int32Vect3 *mag); - -static abi_event aligner_ev; -static void aligner_cb(uint8_t __attribute__((unused)) sender_id, - uint32_t stamp __attribute__((unused)), - struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, - struct Int32Vect3 *lp_mag); - /* gps */ bool_t ins_gps_fix_once; @@ -217,28 +169,28 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, static inline void init_invariant_state(void) { // init state - float_quat_identity(&ins_impl.state.quat); - FLOAT_RATES_ZERO(ins_impl.state.bias); - FLOAT_VECT3_ZERO(ins_impl.state.pos); - FLOAT_VECT3_ZERO(ins_impl.state.speed); - ins_impl.state.as = 1.0f; - ins_impl.state.hb = 0.0f; + float_quat_identity(&ins_float_inv.state.quat); + FLOAT_RATES_ZERO(ins_float_inv.state.bias); + FLOAT_VECT3_ZERO(ins_float_inv.state.pos); + FLOAT_VECT3_ZERO(ins_float_inv.state.speed); + ins_float_inv.state.as = 1.0f; + ins_float_inv.state.hb = 0.0f; // init measures - FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps); - FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps); - ins_impl.meas.baro_alt = 0.0f; + FLOAT_VECT3_ZERO(ins_float_inv.meas.pos_gps); + FLOAT_VECT3_ZERO(ins_float_inv.meas.speed_gps); + ins_float_inv.meas.baro_alt = 0.0f; // init baro ins_baro_initialized = FALSE; ins_gps_fix_once = FALSE; } -void ins_init() +void ins_float_invariant_init(void) { // init position -#if INS_UPDATE_FW_ESTIMATOR +#if INS_FINV_USE_UTM struct UtmCoor_f utm0; utm0.north = (float)nav_utm_north0; utm0.east = (float)nav_utm_east0; @@ -268,38 +220,28 @@ void ins_init() init_invariant_state(); // init gains - ins_impl.gains.lv = INS_INV_LV; - ins_impl.gains.lb = INS_INV_LB; - ins_impl.gains.mv = INS_INV_MV; - ins_impl.gains.mvz = INS_INV_MVZ; - ins_impl.gains.mh = INS_INV_MH; - ins_impl.gains.nx = INS_INV_NX; - ins_impl.gains.nxz = INS_INV_NXZ; - ins_impl.gains.nh = INS_INV_NH; - ins_impl.gains.ov = INS_INV_OV; - ins_impl.gains.ob = INS_INV_OB; - ins_impl.gains.rv = INS_INV_RV; - ins_impl.gains.rh = INS_INV_RH; - ins_impl.gains.sh = INS_INV_SH; + ins_float_inv.gains.lv = INS_INV_LV; + ins_float_inv.gains.lb = INS_INV_LB; + ins_float_inv.gains.mv = INS_INV_MV; + ins_float_inv.gains.mvz = INS_INV_MVZ; + ins_float_inv.gains.mh = INS_INV_MH; + ins_float_inv.gains.nx = INS_INV_NX; + ins_float_inv.gains.nxz = INS_INV_NXZ; + ins_float_inv.gains.nh = INS_INV_NH; + ins_float_inv.gains.ov = INS_INV_OV; + ins_float_inv.gains.ob = INS_INV_OB; + ins_float_inv.gains.rv = INS_INV_RV; + ins_float_inv.gains.rh = INS_INV_RH; + ins_float_inv.gains.sh = INS_INV_SH; - ins.status = INS_UNINIT; - ins_impl.is_aligned = FALSE; - ins_impl.reset = FALSE; - - // Bind to ABI messages - AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); - AbiBindMsgIMU_MAG_INT32(INS_MAG_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); - -#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); -#endif + ins_float_inv.is_aligned = FALSE; + ins_float_inv.reset = FALSE; } void ins_reset_local_origin(void) { -#if INS_UPDATE_FW_ESTIMATOR +#if INS_FINV_USE_UTM struct UtmCoor_f utm; #ifdef GPS_USE_LATLONG /* Recompute UTM coordinates in this zone */ @@ -326,7 +268,7 @@ void ins_reset_local_origin(void) void ins_reset_altitude_ref(void) { -#if INS_UPDATE_FW_ESTIMATOR +#if INS_FINV_USE_UTM struct UtmCoor_f utm = state.utm_origin_f; utm.alt = gps.hmsl / 1000.0f; stateSetLocalUtmOrigin_f(&utm); @@ -348,16 +290,18 @@ void ins_float_invariant_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_mag) { /* Compute an initial orientation from accel and mag directly as quaternion */ - ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, lp_accel, lp_mag); + ahrs_float_get_quat_from_accel_mag(&ins_float_inv.state.quat, lp_accel, lp_mag); /* use average gyro as initial value for bias */ struct FloatRates bias0; RATES_COPY(bias0, *lp_gyro); - RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0); + RATES_FLOAT_OF_BFP(ins_float_inv.state.bias, bias0); + + /* push initial values to state interface */ + stateSetNedToBodyQuat_f(&ins_float_inv.state.quat); // ins and ahrs are now running - ins_impl.is_aligned = TRUE; - ins.status = INS_RUNNING; + ins_float_inv.is_aligned = TRUE; } void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt) @@ -366,48 +310,47 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a // realign all the filter if needed // a complete init cycle is required - if (ins_impl.reset) { - ins_impl.reset = FALSE; - ins.status = INS_UNINIT; - ins_impl.is_aligned = FALSE; + if (ins_float_inv.reset) { + ins_float_inv.reset = FALSE; + ins_float_inv.is_aligned = FALSE; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_float_inv.body_to_imu); int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro); - RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); + RATES_FLOAT_OF_BFP(ins_float_inv.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel); - ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); + ACCELS_FLOAT_OF_BFP(ins_float_inv.cmd.accel, accel_meas_body); // update correction gains - error_output(&ins_impl); + error_output(&ins_float_inv); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, - (float *)&ins_impl.state, INV_STATE_DIM, - (float *)&ins_impl.cmd, INV_COMMAND_DIM, + (float *)&ins_float_inv.state, INV_STATE_DIM, + (float *)&ins_float_inv.cmd, INV_COMMAND_DIM, invariant_model, dt); - ins_impl.state = new_state; + ins_float_inv.state = new_state; // normalize quaternion - FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); + FLOAT_QUAT_NORMALIZE(ins_float_inv.state.quat); // set global state - stateSetNedToBodyQuat_f(&ins_impl.state.quat); - RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); + stateSetNedToBodyQuat_f(&ins_float_inv.state.quat); + RATES_DIFF(body_rates, ins_float_inv.cmd.rates, ins_float_inv.state.bias); stateSetBodyRates_f(&body_rates); - stateSetPositionNed_f(&ins_impl.state.pos); - stateSetSpeedNed_f(&ins_impl.state.speed); + stateSetPositionNed_f(&ins_float_inv.state.pos); + stateSetSpeedNed_f(&ins_float_inv.state.speed); // untilt accel and remove gravity struct FloatQuat q_b2n; - float_quat_invert(&q_b2n, &ins_impl.state.quat); + float_quat_invert(&q_b2n, &ins_float_inv.state.quat); struct FloatVect3 accel_n; - float_quat_vmult(&accel_n, &q_b2n, &ins_impl.cmd.accel); - VECT3_SMUL(accel_n, accel_n, 1. / (ins_impl.state.as)); + float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel); + VECT3_SMUL(accel_n, accel_n, 1. / (ins_float_inv.state.as)); VECT3_ADD(accel_n, A); stateSetAccelNed_f((struct NedCoor_f *)&accel_n); @@ -415,28 +358,28 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a #if SEND_INVARIANT_FILTER struct FloatEulers eulers; - FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); + FLOAT_EULERS_OF_QUAT(eulers, ins_float_inv.state.quat); RunOnceEvery(3, - pprz_msg_send_INV_FILTER( - &(DefaultChannel).trans_tx, &(DefaultDevice).device, AC_ID, - &ins_impl.state.quat.qi, - &eulers.phi, - &eulers.theta, - &eulers.psi, - &ins_impl.state.speed.x, - &ins_impl.state.speed.y, - &ins_impl.state.speed.z, - &ins_impl.state.pos.x, - &ins_impl.state.pos.y, - &ins_impl.state.pos.z, - &ins_impl.state.bias.p, - &ins_impl.state.bias.q, - &ins_impl.state.bias.r, - &ins_impl.state.as, - &ins_impl.state.hb, - &ins_impl.meas.baro_alt, - &ins_impl.meas.pos_gps.z) - ); + pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device, + AC_ID, + &ins_float_inv.state.quat.qi, + &eulers.phi, + &eulers.theta, + &eulers.psi, + &ins_float_inv.state.speed.x, + &ins_float_inv.state.speed.y, + &ins_float_inv.state.speed.z, + &ins_float_inv.state.pos.x, + &ins_float_inv.state.pos.y, + &ins_float_inv.state.pos.z, + &ins_float_inv.state.bias.p, + &ins_float_inv.state.bias.q, + &ins_float_inv.state.bias.r, + &ins_float_inv.state.as, + &ins_float_inv.state.hb, + &ins_float_inv.meas.baro_alt, + &ins_float_inv.meas.pos_gps.z); + ); #endif #if LOG_INVARIANT_FILTER @@ -449,66 +392,66 @@ void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* a } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", - ins_impl.cmd.rates.p, - ins_impl.cmd.rates.q, - ins_impl.cmd.rates.r, - ins_impl.cmd.accel.x, - ins_impl.cmd.accel.y, - ins_impl.cmd.accel.z, - ins_impl.meas.pos_gps.x, - ins_impl.meas.pos_gps.y, - ins_impl.meas.pos_gps.z, - ins_impl.meas.speed_gps.x, - ins_impl.meas.speed_gps.y, - ins_impl.meas.speed_gps.z, - ins_impl.meas.mag.x, - ins_impl.meas.mag.y, - ins_impl.meas.mag.z, - ins_impl.meas.baro_alt, - ins_impl.state.quat.qi, - ins_impl.state.quat.qx, - ins_impl.state.quat.qy, - ins_impl.state.quat.qz, - ins_impl.state.bias.p, - ins_impl.state.bias.q, - ins_impl.state.bias.r, - ins_impl.state.speed.x, - ins_impl.state.speed.y, - ins_impl.state.speed.z, - ins_impl.state.pos.x, - ins_impl.state.pos.y, - ins_impl.state.pos.z, - ins_impl.state.hb, - ins_impl.state.as); + ins_float_inv.cmd.rates.p, + ins_float_inv.cmd.rates.q, + ins_float_inv.cmd.rates.r, + ins_float_inv.cmd.accel.x, + ins_float_inv.cmd.accel.y, + ins_float_inv.cmd.accel.z, + ins_float_inv.meas.pos_gps.x, + ins_float_inv.meas.pos_gps.y, + ins_float_inv.meas.pos_gps.z, + ins_float_inv.meas.speed_gps.x, + ins_float_inv.meas.speed_gps.y, + ins_float_inv.meas.speed_gps.z, + ins_float_inv.meas.mag.x, + ins_float_inv.meas.mag.y, + ins_float_inv.meas.mag.z, + ins_float_inv.meas.baro_alt, + ins_float_inv.state.quat.qi, + ins_float_inv.state.quat.qx, + ins_float_inv.state.quat.qy, + ins_float_inv.state.quat.qz, + ins_float_inv.state.bias.p, + ins_float_inv.state.bias.q, + ins_float_inv.state.bias.r, + ins_float_inv.state.speed.x, + ins_float_inv.state.speed.y, + ins_float_inv.state.speed.z, + ins_float_inv.state.pos.x, + ins_float_inv.state.pos.y, + ins_float_inv.state.pos.z, + ins_float_inv.state.hb, + ins_float_inv.state.as); } } #endif } -void ins_update_gps(void) +void ins_float_invariant_update_gps(struct GpsState *gps_s) { - if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) { + if (gps_s->fix == GPS_FIX_3D && ins_float_inv.is_aligned) { ins_gps_fix_once = TRUE; -#if INS_UPDATE_FW_ESTIMATOR +#if INS_FINV_USE_UTM if (state.utm_initialized_f) { // position (local ned) - ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.0f) - state.utm_origin_f.north; - ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.0f) - state.utm_origin_f.east; - ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f); + ins_float_inv.meas.pos_gps.x = (gps_s->utm_pos.north / 100.0f) - state.utm_origin_f.north; + ins_float_inv.meas.pos_gps.y = (gps_s->utm_pos.east / 100.0f) - state.utm_origin_f.east; + ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - (gps_s->hmsl / 1000.0f); // speed - ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.0f; - ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.0f; - ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.0f; + ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f; + ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f; + ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f; } #else if (state.ned_initialized_f) { struct EcefCoor_f ecef_pos, ecef_vel; - ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos); - ned_of_ecef_point_f(&ins_impl.meas.pos_gps, &state.ned_origin_f, &ecef_pos); - ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel); - ned_of_ecef_vect_f(&ins_impl.meas.speed_gps, &state.ned_origin_f, &ecef_vel); + ECEF_FLOAT_OF_BFP(ecef_pos, gps_s->ecef_pos); + ned_of_ecef_point_f(&ins_float_inv.meas.pos_gps, &state.ned_origin_f, &ecef_pos); + ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel); + ned_of_ecef_vect_f(&ins_float_inv.meas.speed_gps, &state.ned_origin_f, &ecef_vel); } #endif } @@ -516,7 +459,7 @@ void ins_update_gps(void) } -static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) +void ins_float_invariant_update_baro(float pressure) { static float ins_qfe = 101325.0f; static float alpha = 10.0f; @@ -545,7 +488,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) } i++; } else { /* normal update with baro measurement */ - ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down + ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down } } @@ -557,24 +500,24 @@ void ins_float_invariant_update_mag(struct Int32Vect3* mag) static uint32_t mag_frozen_count = MAG_FROZEN_COUNT; static int32_t last_mx = 0; - if (last_mx == imu.mag.x) { + if (last_mx == mag->x) { mag_frozen_count--; if (mag_frozen_count == 0) { // if mag is dead, better set measurements to zero - FLOAT_VECT3_ZERO(ins_impl.meas.mag); + FLOAT_VECT3_ZERO(ins_float_inv.meas.mag); mag_frozen_count = MAG_FROZEN_COUNT; } } else { // values are moving - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_float_inv.body_to_imu); struct Int32Vect3 mag_meas_body; // new values in body frame int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, mag); - MAGS_FLOAT_OF_BFP(ins_impl.meas.mag, mag_meas_body); + MAGS_FLOAT_OF_BFP(ins_float_inv.meas.mag, mag_meas_body); // reset counter mag_frozen_count = MAG_FROZEN_COUNT; } - last_mx = imu.mag.x; + last_mx = mag->x; } @@ -609,7 +552,7 @@ static inline void invariant_model(float *o, const float *x, const int n, const /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */ float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat)); - float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_impl.corr.LE); + float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE); QUAT_ADD(s_dot.quat, tmp_quat); float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat); @@ -622,20 +565,20 @@ static inline void invariant_model(float *o, const float *x, const int n, const float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel)); VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as)); VECT3_ADD(s_dot.speed, A); - VECT3_ADD(s_dot.speed, ins_impl.corr.ME); + VECT3_ADD(s_dot.speed, ins_float_inv.corr.ME); /* dot_X = V + NE */ - VECT3_SUM(s_dot.pos, s->speed, ins_impl.corr.NE); + VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE); /* bias_dot = q-1 * (OE) * q */ - float_quat_vmult(&tmp_vect, &(s->quat), &ins_impl.corr.OE); + float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE); RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z); /* as_dot = as * RE */ - s_dot.as = (s->as) * (ins_impl.corr.RE); + s_dot.as = (s->as) * (ins_float_inv.corr.RE); /* hb_dot = SE */ - s_dot.hb = ins_impl.corr.SE; + s_dot.hb = ins_float_inv.corr.SE; // set output memcpy(o, &s_dot, n * sizeof(float)); @@ -672,8 +615,8 @@ static inline void error_output(struct InsFloatInv *_ins) // pos and speed error only if GPS data are valid // or while waiting first GPS data to prevent diverging - if ((gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING -#if INS_UPDATE_FW_ESTIMATOR + if ((gps.fix == GPS_FIX_3D && ins_float_inv.is_aligned +#if INS_FINV_USE_UTM && state.utm_initialized_f #else && state.ned_initialized_f @@ -750,27 +693,12 @@ void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z); } -/** temporary functions for INS interface compatibility */ -void ins_propagate(float dt) +void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i) { - ins_float_invariant_propagate(&imu.gyro, &imu.accel, dt); -} + orientationSetQuat_f(&ins_float_inv.body_to_imu, q_b2i); -static void mag_cb(uint8_t sender_id __attribute__((unused)), - uint32_t stamp __attribute__((unused)), - struct Int32Vect3 *mag) -{ - if (ins_impl.is_aligned) { - ins_float_invariant_update_mag(mag); - } -} - -static void aligner_cb(uint8_t __attribute__((unused)) sender_id, - uint32_t stamp __attribute__((unused)), - struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, - struct Int32Vect3 *lp_mag) -{ - if (!ins_impl.is_aligned) { - ins_float_invariant_align(lp_gyro, lp_accel, lp_mag); + if (!ins_float_inv.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ins_float_inv.state.quat, q_b2i, sizeof(struct FloatQuat)); } } diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.h b/sw/airborne/subsystems/ins/ins_float_invariant.h index d83dc4f29f..9a80a92f98 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.h +++ b/sw/airborne/subsystems/ins/ins_float_invariant.h @@ -16,11 +16,11 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, see * . - * */ - -/* +/** + * @file subsystems/ins/ins_float_invariant.h + * INS using invariant filter. * For more information, please send an email to "jp.condomines@gmail.com" */ @@ -28,6 +28,7 @@ #define INS_FLOAT_INVARIANT_H #include "subsystems/ins.h" +#include "subsystems/gps.h" #include "math/pprz_algebra_float.h" #include "math/pprz_orientation_conversion.h" @@ -111,27 +112,25 @@ struct InsFloatInv { bool_t reset; ///< flag to request reset/reinit the filter + /** body_to_imu rotation */ + struct OrientationReps body_to_imu; + struct FloatVect3 mag_h; bool_t is_aligned; }; -extern struct InsFloatInv ins_impl; +extern struct InsFloatInv ins_float_inv; -#define DefaultAhrsImpl ins_impl - -/** dummy for now, will be removed when not using ahrs interface anymore */ -static inline void ins_impl_register(void) {} - -/** Currently still called from ins_propagate (declared in INS interface). */ -extern void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt); - -/** called on IMU_LOWPASSED ABI message */ +extern void ins_float_invariant_init(void); +extern void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i); extern void ins_float_invariant_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag); - -/** called on IMU_MAG_INT32 ABI messages */ +extern void ins_float_invariant_propagate(struct Int32Rates* gyro, + struct Int32Vect3* accel, float dt); extern void ins_float_invariant_update_mag(struct Int32Vect3* mag); +extern void ins_float_invariant_update_baro(float pressure); +extern void ins_float_invariant_update_gps(struct GpsState *gps_s); #endif /* INS_FLOAT_INVARIANT_H */ diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c new file mode 100644 index 0000000000..f949ecb615 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c @@ -0,0 +1,202 @@ +/* + * Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger + * 2015 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, see + * . + */ + +/** + * @file subsystems/ins/ins_float_invariant_wrapper.c + * + * Paparazzi specific wrapper to run INVARIANT filter. + */ + +#include "subsystems/ins/ins_float_invariant_wrapper.h" +#include "subsystems/abi.h" +#include "mcu_periph/sys_time.h" +#include "message_pragmas.h" + +#ifndef INS_FINV_FILTER_ID +#define INS_FINV_FILTER_ID 2 +#endif + +/** last accel measurement */ +static struct Int32Vect3 ins_finv_accel; + +/** last gyro msg timestamp */ +static uint32_t ins_finv_last_stamp = 0; + +#if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM +#include "subsystems/datalink/telemetry.h" +#include "state.h" +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) +{ + float foo = 0.; + if (state.ned_initialized_i) { + pprz_msg_send_INS_REF(trans, dev, AC_ID, + &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, &foo); + } +} + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = INS_FINV_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ins_float_inv.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ins_finv_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} +#endif + + +/* + * ABI bindings + */ +/** baro */ +#ifndef INS_FINV_BARO_ID +#if USE_BARO_BOARD +#define INS_FINV_BARO_ID BARO_BOARD_SENDER_ID +#else +#define INS_FINV_BARO_ID ABI_BROADCAST +#endif +#endif +PRINT_CONFIG_VAR(INS_FINV_BARO_ID) + +/** IMU (gyro, accel) */ +#ifndef INS_FINV_IMU_ID +#define INS_FINV_IMU_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_FINV_IMU_ID) + +/** magnetometer */ +#ifndef INS_FINV_MAG_ID +#define INS_FINV_MAG_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(INS_FINV_MAG_ID) + +static abi_event baro_ev; +static abi_event mag_ev; +static abi_event gyro_ev; +static abi_event accel_ev; +static abi_event aligner_ev; +static abi_event body_to_imu_ev; +static abi_event geo_mag_ev; +static abi_event gps_ev; + +static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) +{ + ins_float_invariant_update_baro(pressure); +} + +/** + * Call ins_float_invariant_propagate on new gyro measurements. + * Since acceleration measurement is also needed for propagation, + * use the last stored accel from #ins_finv_accel. + */ +static void gyro_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, struct Int32Rates *gyro) +{ +#if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ins_float_invariant_propagate(gyro, &ins_finv_accel, dt); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS float_invariant propagation.") + PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY) + const float dt = 1. / (INS_PROPAGATE_FREQUENCY); + ins_float_invariant_propagate(gyro, &ins_finv_accel, dt); +#endif + + ins_finv_last_stamp = stamp; +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *accel) +{ + memcpy(&ins_finv_accel, accel, sizeof(struct Int32Vect3)); +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ins_float_inv.is_aligned) { + ins_float_invariant_update_mag(mag); + } +} + +static void aligner_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t stamp __attribute__((unused)), + struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ + if (!ins_float_inv.is_aligned) { + ins_float_invariant_align(lp_gyro, lp_accel, lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ins_float_inv_set_body_to_imu_quat(q_b2i_f); +} + +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) +{ + memcpy(&ins_float_inv.mag_h, h, sizeof(struct FloatVect3)); +} + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ins_float_invariant_update_gps(gps_s); +} + + +void ins_float_invariant_register(void) +{ + ins_register_impl(ins_float_invariant_init); + + // Bind to ABI messages + AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb); + AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb); + AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + +#if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM + register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); +#endif +} diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h new file mode 100644 index 0000000000..9c4cdabf9d --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2015 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, see + * . + */ + +/** + * @file subsystems/ins/ins_float_invariant_wrapper.h + * + * Paparazzi specific wrapper to run INVARIANT filter. + */ + +#ifndef INS_FLOAT_INVARIANT_WRAPPER_H +#define INS_FLOAT_INVARIANT_WRAPPER_H + +#include "subsystems/ins/ins_float_invariant.h" + +#define DefaultInsImpl ins_float_invariant + +extern void ins_float_invariant_register(void); + +#endif /* INS_FLOAT_INVARIANT_WRAPPER_H */ diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 7de8c6dcc2..0412a32432 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -43,6 +43,8 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #include "generated/flight_plan.h" #endif +#include "subsystems/ins/ins_gps_passthrough.h" + struct InsGpsPassthrough { struct LtpDef_i ltp_def; bool_t ltp_initialized; @@ -53,7 +55,7 @@ struct InsGpsPassthrough { struct NedCoor_i ltp_accel; }; -struct InsGpsPassthrough ins_impl; +struct InsGpsPassthrough ins_gp; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -61,32 +63,32 @@ struct InsGpsPassthrough ins_impl; static void send_ins(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_INS(trans, dev, AC_ID, - &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); + &ins_gp.ltp_pos.x, &ins_gp.ltp_pos.y, &ins_gp.ltp_pos.z, + &ins_gp.ltp_speed.x, &ins_gp.ltp_speed.y, &ins_gp.ltp_speed.z, + &ins_gp.ltp_accel.x, &ins_gp.ltp_accel.y, &ins_gp.ltp_accel.z); } static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { static const float fake_baro_z = 0.0; pprz_msg_send_INS_Z(trans, dev, AC_ID, - (float *)&fake_baro_z, &ins_impl.ltp_pos.z, - &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); + (float *)&fake_baro_z, &ins_gp.ltp_pos.z, + &ins_gp.ltp_speed.z, &ins_gp.ltp_accel.z); } static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { static const float fake_qfe = 0.0; - if (ins_impl.ltp_initialized) { + if (ins_gp.ltp_initialized) { pprz_msg_send_INS_REF(trans, dev, AC_ID, - &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, (float *)&fake_qfe); + &ins_gp.ltp_def.ecef.x, &ins_gp.ltp_def.ecef.y, &ins_gp.ltp_def.ecef.z, + &ins_gp.ltp_def.lla.lat, &ins_gp.ltp_def.lla.lon, &ins_gp.ltp_def.lla.alt, + &ins_gp.ltp_def.hmsl, (float *)&fake_qfe); } } #endif -void ins_init(void) +void ins_gps_passthrough_init(void) { #if USE_INS_NAV_INIT @@ -99,18 +101,18 @@ void ins_init(void) 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); + ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_nav0); + ins_gp.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_gp.ltp_def); - ins_impl.ltp_initialized = TRUE; + ins_gp.ltp_initialized = TRUE; #else - ins_impl.ltp_initialized = FALSE; + ins_gp.ltp_initialized = FALSE; #endif - INT32_VECT3_ZERO(ins_impl.ltp_pos); - INT32_VECT3_ZERO(ins_impl.ltp_speed); - INT32_VECT3_ZERO(ins_impl.ltp_accel); + INT32_VECT3_ZERO(ins_gp.ltp_pos); + INT32_VECT3_ZERO(ins_gp.ltp_speed); + INT32_VECT3_ZERO(ins_gp.ltp_accel); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "INS", send_ins); @@ -119,21 +121,13 @@ void ins_init(void) #endif } -void ins_periodic(void) -{ - if (ins_impl.ltp_initialized) { - ins.status = INS_RUNNING; - } -} - - void ins_reset_local_origin(void) { - 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; - stateSetLocalOrigin_i(&ins_impl.ltp_def); - ins_impl.ltp_initialized = TRUE; + ltp_def_from_ecef_i(&ins_gp.ltp_def, &gps.ecef_pos); + ins_gp.ltp_def.lla.alt = gps.lla_pos.alt; + ins_gp.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_gp.ltp_def); + ins_gp.ltp_initialized = TRUE; } void ins_reset_altitude_ref(void) @@ -143,29 +137,40 @@ void ins_reset_altitude_ref(void) .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; - ltp_def_from_lla_i(&ins_impl.ltp_def, &lla); - ins_impl.ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_impl.ltp_def); + ltp_def_from_lla_i(&ins_gp.ltp_def, &lla); + ins_gp.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_gp.ltp_def); } -void ins_update_gps(void) + +#include "subsystems/abi.h" +static abi_event gps_ev; +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) { - if (gps.fix == GPS_FIX_3D) { - if (!ins_impl.ltp_initialized) { + if (gps_s->fix == GPS_FIX_3D) { + if (!ins_gp.ltp_initialized) { ins_reset_local_origin(); } /* simply scale and copy pos/speed from gps */ struct NedCoor_i gps_pos_cm_ned; - ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); - INT32_VECT3_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned, + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos); + INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); - stateSetPositionNed_i(&ins_impl.ltp_pos); + stateSetPositionNed_i(&ins_gp.ltp_pos); 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); - INT32_VECT3_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned, + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel); + INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); - stateSetSpeedNed_i(&ins_impl.ltp_speed); + stateSetSpeedNed_i(&ins_gp.ltp_speed); } } + +void ins_gps_passthrough_register(void); +{ + ins_register_impl(ins_gps_passthrough_init); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); +} diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.h b/sw/airborne/subsystems/ins/ins_gps_passthrough.h new file mode 100644 index 0000000000..96b06e0d60 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2015 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, see + * . + */ + +/** + * @file subsystems/ins/ins_gps_passthrough.h + * + * Simply converts GPS ECEF position and velocity to NED + * and passes it through to the state interface. + */ + +#ifndef INS_GPS_PASSTHROUGH_H +#define INS_GPS_PASSTHROUGH_H + +#define DefaultInsImpl ins_gps_passthrough + +extern void ins_gps_passthrough_init(void); +extern void ins_gps_passthrough_register(void); + +#endif /* INS_GPS_PASSTHROUGH_H */ diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index f655f47f99..7b640c862a 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -35,13 +35,13 @@ #include "subsystems/gps.h" #include "firmwares/fixedwing/nav.h" -void ins_init(void) +#include "subsystems/ins/ins_gps_passthrough_utm.h" + +void ins_gps_utm_init(void) { 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_reset_local_origin(void) @@ -71,22 +71,34 @@ void ins_reset_altitude_ref(void) stateSetLocalUtmOrigin_f(&utm); } -void ins_update_gps(void) + +#include "subsystems/abi.h" +static abi_event gps_ev; + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) { struct UtmCoor_f utm; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; + utm.east = gps_s->utm_pos.east / 100.0f; + utm.north = gps_s->utm_pos.north / 100.0f; utm.zone = nav_utm_zone0; - utm.alt = gps.hmsl / 1000.0f; + utm.alt = gps_s->hmsl / 1000.0f; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { - gps.ned_vel.x / 100.0f, - gps.ned_vel.y / 100.0f, - gps.ned_vel.z / 100.0f + gps_s->ned_vel.x / 100.0f, + gps_s->ned_vel.y / 100.0f, + gps_s->ned_vel.z / 100.0f }; // set velocity stateSetSpeedNed_f(&ned_vel); } + +void ins_gps_utm_register(void) +{ + ins_register_impl(ins_gps_utm_init); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); +} diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.h b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.h new file mode 100644 index 0000000000..98e2c78c71 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2015 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, see + * . + */ + +/** + * @file subsystems/ins/ins_gps_passthrough_utm.h + * + * Simply passes GPS UTM position and velocity through to the state interface. + * For fixedwing firmware since it sets UTM pos only. + */ + +#ifndef INS_GPS_PASSTHROUGH_UTM_H +#define INS_GPS_PASSTHROUGH_UTM_H + +#define DefaultInsImpl ins_gps_utm + +extern void ins_gps_utm_init(void); +extern void ins_gps_utm_register(void); + +#endif /* INS_GPS_PASSTHROUGH_UTM_H */ diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index c8fe048e93..ac4201226f 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -116,7 +116,16 @@ PRINT_CONFIG_VAR(INS_BARO_ID) abi_event baro_ev; static void baro_cb(uint8_t sender_id, float pressure); -struct InsInt ins_impl; +/** ABI binding for IMU data. + * Used accel ABI messages. + */ +#ifndef INS_INT_IMU_ID +#define INS_INT_IMU_ID ABI_BROADCAST +#endif +static abi_event accel_ev; +static abi_event gps_ev; + +struct InsInt ins_int; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -124,24 +133,24 @@ struct InsInt ins_impl; static void send_ins(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_INS(trans, dev, AC_ID, - &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); + &ins_int.ltp_pos.x, &ins_int.ltp_pos.y, &ins_int.ltp_pos.z, + &ins_int.ltp_speed.x, &ins_int.ltp_speed.y, &ins_int.ltp_speed.z, + &ins_int.ltp_accel.x, &ins_int.ltp_accel.y, &ins_int.ltp_accel.z); } static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_INS_Z(trans, dev, AC_ID, - &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); + &ins_int.baro_z, &ins_int.ltp_pos.z, &ins_int.ltp_speed.z, &ins_int.ltp_accel.z); } static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { - if (ins_impl.ltp_initialized) { + if (ins_int.ltp_initialized) { pprz_msg_send_INS_REF(trans, dev, AC_ID, - &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); + &ins_int.ltp_def.ecef.x, &ins_int.ltp_def.ecef.y, &ins_int.ltp_def.ecef.z, + &ins_int.ltp_def.lla.lat, &ins_int.ltp_def.lla.lon, &ins_int.ltp_def.lla.alt, + &ins_int.ltp_def.hmsl, &ins_int.qfe); } } #endif @@ -154,28 +163,28 @@ static void ins_update_from_hff(void); #endif -void ins_init(void) +void ins_int_init(void) { #if USE_INS_NAV_INIT ins_init_origin_from_flightplan(); - ins_impl.ltp_initialized = TRUE; + ins_int.ltp_initialized = TRUE; #else - ins_impl.ltp_initialized = FALSE; + ins_int.ltp_initialized = FALSE; #endif // Bind to BARO_ABS message AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); - ins_impl.baro_initialized = FALSE; + ins_int.baro_initialized = FALSE; #if USE_SONAR - ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL; + ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL; // Bind to AGL message AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb); #endif - ins_impl.vf_reset = FALSE; - ins_impl.hf_realign = FALSE; + ins_int.vf_reset = FALSE; + ins_int.hf_realign = FALSE; /* init vertical and horizontal filters */ vff_init_zero(); @@ -183,9 +192,9 @@ void ins_init(void) b2_hff_init(0., 0., 0., 0.); #endif - INT32_VECT3_ZERO(ins_impl.ltp_pos); - INT32_VECT3_ZERO(ins_impl.ltp_speed); - INT32_VECT3_ZERO(ins_impl.ltp_accel); + INT32_VECT3_ZERO(ins_int.ltp_pos); + INT32_VECT3_ZERO(ins_int.ltp_speed); + INT32_VECT3_ZERO(ins_int.ltp_accel); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "INS", send_ins); @@ -194,34 +203,27 @@ void ins_init(void) #endif } -void ins_periodic(void) -{ - if (ins_impl.ltp_initialized) { - ins.status = INS_RUNNING; - } -} - void ins_reset_local_origin(void) { #if USE_GPS if (gps.fix == GPS_FIX_3D) { - 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); + ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos); + ins_int.ltp_def.lla.alt = gps.lla_pos.alt; + ins_int.ltp_def.hmsl = gps.hmsl; + ins_int.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_int.ltp_def); } else { - ins_impl.ltp_initialized = FALSE; + ins_int.ltp_initialized = FALSE; } #else - ins_impl.ltp_initialized = FALSE; + ins_int.ltp_initialized = FALSE; #endif #if USE_HFF - ins_impl.hf_realign = TRUE; + ins_int.hf_realign = TRUE; #endif - ins_impl.vf_reset = TRUE; + ins_int.vf_reset = TRUE; } void ins_reset_altitude_ref(void) @@ -232,40 +234,40 @@ void ins_reset_altitude_ref(void) .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; - ltp_def_from_lla_i(&ins_impl.ltp_def, &lla); - ins_impl.ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_impl.ltp_def); + ltp_def_from_lla_i(&ins_int.ltp_def, &lla); + ins_int.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_int.ltp_def); #endif - ins_impl.vf_reset = TRUE; + ins_int.vf_reset = TRUE; } -void ins_propagate(float dt) +void ins_int_propagate(struct Int32Vect3 *accel, float dt) { /* untilt accels */ struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); + int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, 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_impl.baro_initialized) { + if (ins_int.baro_initialized) { vff_propagate(z_accel_meas_float, dt); ins_update_from_vff(); } else { // feed accel from the sensors // 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); + ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); } #if USE_HFF /* propagate horizontal filter */ b2_hff_propagate(); - /* convert and copy result to ins_impl */ + /* convert and copy result to ins_int */ ins_update_from_hff(); #else - ins_impl.ltp_accel.x = accel_meas_ltp.x; - ins_impl.ltp_accel.y = accel_meas_ltp.y; + ins_int.ltp_accel.x = accel_meas_ltp.x; + ins_int.ltp_accel.y = accel_meas_ltp.y; #endif /* USE_HFF */ ins_ned_to_state(); @@ -273,24 +275,24 @@ void ins_propagate(float dt) static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { - if (!ins_impl.baro_initialized && pressure > 1e-7) { + if (!ins_int.baro_initialized && pressure > 1e-7) { // wait for a first positive value - ins_impl.qfe = pressure; - ins_impl.baro_initialized = TRUE; + ins_int.qfe = pressure; + ins_int.baro_initialized = TRUE; } - if (ins_impl.baro_initialized) { - if (ins_impl.vf_reset) { - ins_impl.vf_reset = FALSE; - ins_impl.qfe = pressure; + if (ins_int.baro_initialized) { + if (ins_int.vf_reset) { + ins_int.vf_reset = FALSE; + ins_int.qfe = pressure; vff_realign(0.); ins_update_from_vff(); } else { - ins_impl.baro_z = -pprz_isa_height_of_pressure(pressure, ins_impl.qfe); + ins_int.baro_z = -pprz_isa_height_of_pressure(pressure, ins_int.qfe); #if USE_VFF_EXTENDED - vff_update_baro(ins_impl.baro_z); + vff_update_baro(ins_int.baro_z); #else - vff_update(ins_impl.baro_z); + vff_update(ins_int.baro_z); #endif } ins_ned_to_state(); @@ -298,19 +300,19 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) } #if USE_GPS -void ins_update_gps(void) +void ins_int_update_gps(struct GpsState *gps_s) { - if (gps.fix == GPS_FIX_3D) { - 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); + if (gps_s->fix == GPS_FIX_3D) { + if (!ins_int.ltp_initialized) { + ltp_def_from_ecef_i(&ins_int.ltp_def, &gps_s->ecef_pos); + ins_int.ltp_def.lla.alt = gps_s->lla_pos.alt; + ins_int.ltp_def.hmsl = gps_s->hmsl; + ins_int.ltp_initialized = TRUE; + stateSetLocalOrigin_i(&ins_int.ltp_def); } struct NedCoor_i gps_pos_cm_ned; - ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos); /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */ #ifdef INS_BODY_TO_GPS_X @@ -330,9 +332,9 @@ void ins_update_gps(void) VECT3_SUB(gps_pos_cm_ned, b2g_n); #endif - /// @todo maybe use gps.ned_vel directly?? + /// @todo maybe use gps_s->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); + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel); #if INS_USE_GPS_ALT vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS); @@ -348,27 +350,29 @@ void ins_update_gps(void) 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_impl.hf_realign) { - ins_impl.hf_realign = FALSE; + if (ins_int.hf_realign) { + ins_int.hf_realign = FALSE; const struct FloatVect2 zero = {0.0f, 0.0f}; b2_hff_realign(gps_pos_m_ned, zero); } // run horizontal filter b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned); - // convert and copy result to ins_impl + // convert and copy result to ins_int 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_VECT2_SCALE_2(ins_int.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_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); #endif /* USE_HFF */ ins_ned_to_state(); } } +#else +void ins_int_update_gps(void) {} #endif /* USE_GPS */ @@ -383,10 +387,10 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance) && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD #endif #ifdef INS_SONAR_BARO_THRESHOLD - && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ + && ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ #endif - && ins_impl.update_on_agl - && ins_impl.baro_initialized) { + && ins_int.update_on_agl + && ins_int.baro_initialized) { vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance)); last_offset = vff.offset; } else { @@ -410,18 +414,18 @@ static void ins_init_origin_from_flightplan(void) 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); + ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_nav0); + ins_int.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_int.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); + stateSetPositionNed_i(&ins_int.ltp_pos); + stateSetSpeedNed_i(&ins_int.ltp_speed); + stateSetAccelNed_i(&ins_int.ltp_accel); #if defined SITL && USE_NPS if (nps_bypass_ins) { @@ -433,20 +437,53 @@ static void ins_ned_to_state(void) /** update ins state from vertical filter */ static void ins_update_from_vff(void) { - 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); + ins_int.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot); + ins_int.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot); + ins_int.ltp_pos.z = POS_BFP_OF_REAL(vff.z); } #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); + ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); + ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); + ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); + ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); + ins_int.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); + ins_int.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); } #endif + + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, struct Int32Vect3 *accel) +{ + PRINT_CONFIG_MSG("Calculating dt for INS int propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ins_int_propagate(accel, dt); + } + last_stamp = stamp; +} + +static void gps_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + ins_int_update_gps(gps_s); +} + +void ins_int_register(void) +{ + ins_register_impl(ins_int_init); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); +} diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index 0dfa18eb34..34ba6af6d3 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -30,6 +30,7 @@ #define INS_INT_H #include "subsystems/ins.h" +#include "subsystems/gps.h" #include "std.h" #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" @@ -65,6 +66,17 @@ struct InsInt { }; /** global INS state */ -extern struct InsInt ins_impl; +extern struct InsInt ins_int; + +extern void ins_int_init(void); +extern void ins_int_propagate(struct Int32Vect3 *accel, float dt); +extern void ins_int_update_gps(struct GpsState *gps_s); + + +#ifndef DefaultInsImpl +#define DefaultInsImpl ins_int +#endif + +extern void ins_int_register(void); #endif /* INS_INT_H */ diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index 5608f62947..c1f490c932 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -53,10 +53,6 @@ static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_mag_event(void); -#define __DefaultAhrsRegister(_x) _x ## _register() -#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) -#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) - uint16_t datalink_time = 0; int main(void) @@ -82,8 +78,6 @@ static inline void main_init(void) ahrs_init(); downlink_init(); - DefaultAhrsRegister(); - mcu_int_enable(); }