From def43503022507c78179cc4452394e616a52af1d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 26 Feb 2014 16:40:01 +0100 Subject: [PATCH] [ins] reset reference position from INS not from NAV Nav is (almost) independent of the GPS which is a sensor and should only be seen by the INS/AHRS subsystems. It also allow to make shared INS filters more easily (example with invariant filter). --- conf/flight_plans/versatile.xml | 4 +- sw/airborne/firmwares/rotorcraft/navigation.c | 15 +-- sw/airborne/subsystems/ins.h | 20 ++++ sw/airborne/subsystems/ins/ins_alt_float.c | 49 ++++++++++ sw/airborne/subsystems/ins/ins_alt_float.h | 8 ++ sw/airborne/subsystems/ins/ins_ardrone2.c | 12 +++ .../subsystems/ins/ins_float_invariant.c | 96 +++++++++++++++++-- .../subsystems/ins/ins_gps_passthrough.c | 29 ++++++ sw/airborne/subsystems/ins/ins_int.c | 17 ++++ .../subsystems/navigation/common_nav.c | 59 +++--------- 10 files changed, 244 insertions(+), 65 deletions(-) diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index 0c98954cbc..da679523a1 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -12,8 +12,8 @@ - - + + diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 4e507ff9b7..9db6c263e5 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -307,21 +307,12 @@ static inline void nav_set_altitude( void ) { /** Reset the geographic reference to the current GPS fix */ unit_t nav_reset_reference( void ) { - ins_impl.ltp_initialized = FALSE; - ins.hf_realign = TRUE; - ins.vf_realign = TRUE; + ins_reset_ground_ref(); return 0; } unit_t nav_reset_alt( void ) { - ins.vf_realign = TRUE; - -#if USE_GPS - ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; - ins_impl.ltp_def.hmsl = gps.hmsl; - stateSetLocalOrigin_i(&ins_impl.ltp_def); -#endif - + ins_reset_altitude_ref(); return 0; } @@ -342,7 +333,7 @@ void nav_periodic_task() { /* run carrot loop */ nav_run(); - ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.); + ground_alt = POS_BFP_OF_REAL(state.ned_origin_f.hmsl); } void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) { diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index 918dc07050..2c52499352 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -60,6 +60,26 @@ extern void ins_init( void ); */ extern void ins_periodic( void ); +/** INS ground reference reset. + * Reset horizontal and vertical reference to the current position. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_reset_ground_ref( void ); + +/** INS altitude reference reset. + * Reset only vertical reference to the current altitude. + * Needs to be implemented by each INS algorithm. + */ +extern void ins_reset_altitude_ref( void ); + +/** INS utm zone reset. + * Reset UTM zone according te the actual position. + * Needs to be implemented by each INS algorithm but is only + * used with fixedwing firmware. + * @param utm initial utm zone, returns the corrected utm position + */ +extern void ins_reset_utm_zone(struct UtmCoor_f * utm); + /** INS horizontal realign. * @param pos new horizontal position to set * @param speed new horizontal speed to set diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index abfb7c5b5c..9e0142af92 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -87,6 +87,7 @@ void ins_init() { AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); #endif ins.vf_realign = FALSE; + ins.hf_realign = FALSE; alt_kalman(0.); @@ -96,6 +97,54 @@ void ins_init() { void ins_periodic( void ) { } +/** Reset the geographic reference to the current GPS fix */ +void ins_reset_ground_ref( void ) { + struct UtmCoor_f utm; +#ifdef GPS_USE_LATLONG + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + lla.lat = gps.lla_pos.lat/1e7; + lla.lon = gps.lla_pos.lon/1e7; + utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; + utm_of_lla_f(&utm, &lla); +#else + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east/100; + utm.north = gps.utm_pos.north/100; +#endif + // ground_alt + utm.alt = gps.hmsl/1000.; + + // reset state UTM ref + stateSetLocalUtmOrigin_f(&utm); + + // reset filter flag + ins.vf_realign = TRUE; +} + +void ins_reset_altitude_ref( void ) { + struct UtmCoor_f utm = state.utm_origin_f; + // ground_alt + utm.alt = gps.hmsl/1000.; + // reset state UTM ref + stateSetLocalUtmOrigin_f(&utm); + // reset filter flag + ins.vf_realign = TRUE; +} + +void ins_reset_utm_zone(struct UtmCoor_f * utm) { + struct LlaCoor_f lla0; + lla_of_utm_f(&lla0, utm); +#ifdef GPS_USE_LATLONG + utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; +#else + utm->zone = gps.utm_pos.zone; +#endif + utm_of_lla_f(utm, &lla0); + + stateSetLocalUtmOrigin_f(utm); +} + void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { } diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 5aa353fc13..018065ce37 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -42,6 +42,14 @@ extern float ins_baro_alt; extern bool_t ins_baro_initialized; #endif +/** Reset the UTM zone to the current gps fix + * + * This function must be called with a valid GPS position + * + * @param utm initial utm position, returned with a corrected utm zone + */ +extern void ins_reset_utm_zone(struct UtmCoor_f * utm); + extern void alt_kalman_reset( void ); extern void alt_kalman_init( void ); extern void alt_kalman( float ); diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index cdce78e65d..48278941d0 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -76,6 +76,18 @@ void ins_periodic( void ) { ins.status = INS_RUNNING; } +void ins_reset_ground_ref( void ) { + ins_impl.ltp_initialized = FALSE; +} + +void ins_reset_altitude_ref( void ) { +#if USE_GPS + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_impl.ltp_def); +#endif +} + void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { } diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 2b84d5c174..26ca259825 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -58,6 +58,20 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" +#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +static void send_ins_ref(void) { + float foo = 0.; + if (state.ned_initialized_i) { + DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, + &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" @@ -221,7 +235,7 @@ void ins_init() { ecef_of_lla_i(&ecef_nav0, &llh_nav0); struct LtpDef_i ltp_def; ltp_def_from_ecef_i(<p_def, &ecef_nav0); - ins_impl.ltp_def.hmsl = NAV_ALT0; + ltp_def.hmsl = NAV_ALT0; stateSetLocalOrigin_i(<p_def); #endif @@ -250,9 +264,76 @@ void ins_init() { ins.hf_realign = FALSE; ins.vf_realign = FALSE; +#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); +#endif } void ins_periodic(void) {} +void ins_propagate(void) {} + + +void ins_reset_ground_ref( void ) { +#if INS_UPDATE_FW_ESTIMATOR + struct UtmCoor_f utm; +#ifdef GPS_USE_LATLONG + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + lla.lat = gps.lla_pos.lat/1e7; + lla.lon = gps.lla_pos.lon/1e7; + utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; + utm_of_lla_f(&utm, &lla); +#else + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east/100; + utm.north = gps.utm_pos.north/100; +#endif + // ground_alt + utm.alt = gps.hmsl/1000.; + // reset state UTM ref + stateSetLocalUtmOrigin_f(&utm); +#else + struct LtpDef_i ltp_def; + ltp_def_from_ecef_i(<p_def, &gps.ecef_pos); + ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(<p_def); +#endif + ins.hf_realign = FALSE; + ins.vf_realign = FALSE; +} + +void ins_reset_altitude_ref( void ) { +#if INS_UPDATE_FW_ESTIMATOR + struct UtmCoor_f utm = state.utm_origin_f; + utm.alt = gps.hmsl/1000.; + stateSetLocalUtmOrigin_f(&utm); +#else + struct LtpDef_i ltp_def = state.ned_origin_i; + ltp_def.lla.alt = gps.lla_pos.alt; + ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(<p_def); +#endif + ins.vf_realign = FALSE; +} + +#if INS_UPDATE_FW_ESTIMATOR +void ins_reset_utm_zone(struct UtmCoor_f * utm) { + struct LlaCoor_f lla0; + lla_of_utm_f(&lla0, utm); +#ifdef GPS_USE_LATLONG + utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; +#else + utm->zone = gps.utm_pos.zone; +#endif + utm_of_lla_f(utm, &lla0); + + stateSetLocalUtmOrigin_f(utm); +} +#endif + +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {} +void ins_realign_v(float z __attribute__ ((unused))) {} + void ahrs_init(void) { ahrs.status = AHRS_UNINIT; @@ -407,10 +488,13 @@ void ahrs_update_gps(void) { #else if (state.ned_initialized_f) { struct NedCoor_f gps_pos_cm_ned; - ned_of_ecef_point_f(&gps_pos_cm_ned, &state.ned_origin_f, &gps.ecef_pos); - VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_m_ned, 100.); + struct EcefCoor_f ecef_pos, ecef_vel; + ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos); + ned_of_ecef_point_f(&gps_pos_cm_ned, &state.ned_origin_f, &ecef_pos); + VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_cm_ned, 100.); struct NedCoor_f gps_speed_cm_s_ned; - ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &state.ned_origin_f, &gps.ecef_vel); + ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel); + ned_of_ecef_vect_f(&gps_speed_cm_s_ned, &state.ned_origin_f, &ecef_vel); VECT3_SDIV(ins_impl.meas.speed_gps, gps_speed_cm_s_ned, 100.); } #endif @@ -469,8 +553,8 @@ void ahrs_update_mag(void) { */ static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m __attribute__((unused))) { - const struct inv_state * s = (struct inv_state *)x; - const struct inv_command * c = (struct inv_command *)u; + const struct inv_state * s = (const struct inv_state *)x; + const struct inv_command * c = (const struct inv_command *)u; struct inv_state s_dot; struct FloatRates rates; struct FloatVect3 tmp_vect; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index ab1d5356f1..0ca722667d 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -45,6 +45,35 @@ void ins_init() { void ins_periodic( void ) { } +void ins_reset_ground_ref( void ) { + struct UtmCoor_f utm; +#ifdef GPS_USE_LATLONG + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + lla.lat = gps.lla_pos.lat/1e7; + lla.lon = gps.lla_pos.lon/1e7; + utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; + utm_of_lla_f(&utm, &lla); +#else + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east/100; + utm.north = gps.utm_pos.north/100; +#endif + // ground_alt + utm.alt = gps.hmsl/1000.; + // reset state UTM ref + stateSetLocalUtmOrigin_f(&utm); +} + +void ins_reset_altitude_ref( void ) { + struct UtmCoor_f utm = state.utm_origin_f; + utm.alt = gps.hmsl/1000.; + stateSetLocalUtmOrigin_f(&utm); +} + +void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {} +void ins_realign_v(float z __attribute__ ((unused))) {} + void ins_update_gps(void) { struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.; diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 46133726a1..729a0996ec 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -170,6 +170,23 @@ void ins_periodic(void) { ins.status = INS_RUNNING; } +void ins_reset_ground_ref( void ) { + ins_impl.ltp_initialized = FALSE; +#if USE_HFF + ins.hf_realign = TRUE; +#endif + ins.vf_realign = TRUE; +} + +void ins_reset_altitude_ref( void ) { +#if USE_GPS + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_impl.ltp_def); +#endif + ins.vf_realign = TRUE; +} + #if USE_HFF void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { b2_hff_realign(pos, speed); diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 7989779d36..28345e37f7 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -27,7 +27,6 @@ #include "subsystems/navigation/common_nav.h" #include "generated/flight_plan.h" #include "subsystems/ins.h" -#include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" float dist2_to_home; @@ -65,64 +64,34 @@ static float previous_ground_alt; /** Reset the UTM zone to current GPS fix */ unit_t nav_reset_utm_zone(void) { - struct UtmCoor_f utm0_old; - utm0_old.zone = nav_utm_zone0; - utm0_old.north = nav_utm_north0; - utm0_old.east = nav_utm_east0; - utm0_old.alt = ground_alt; - struct LlaCoor_f lla0; - lla_of_utm_f(&lla0, &utm0_old); - -#ifdef GPS_USE_LATLONG - /* Set the real UTM zone */ - nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; -#else - nav_utm_zone0 = gps.utm_pos.zone; -#endif - struct UtmCoor_f utm0; utm0.zone = nav_utm_zone0; - utm_of_lla_f(&utm0, &lla0); + utm0.north = nav_utm_north0; + utm0.east = nav_utm_east0; + utm0.alt = ground_alt; + ins_reset_utm_zone(&utm0); + /* Set the real UTM ref */ + nav_utm_zone0 = utm0.zone; nav_utm_east0 = utm0.east; nav_utm_north0 = utm0.north; - stateSetLocalUtmOrigin_f(&utm0); - return 0; } /** Reset the geographic reference to the current GPS fix */ unit_t nav_reset_reference( void ) { -#ifdef GPS_USE_LATLONG - /* Set the real UTM zone */ - nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; + /* realign INS */ + ins_reset_ground_ref(); - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - lla.lat = gps.lla_pos.lat/1e7; - lla.lon = gps.lla_pos.lon/1e7; - struct UtmCoor_f utm; - utm.zone = nav_utm_zone0; - utm_of_lla_f(&utm, &lla); - nav_utm_east0 = utm.east; - nav_utm_north0 = utm.north; -#else - nav_utm_zone0 = gps.utm_pos.zone; - nav_utm_east0 = gps.utm_pos.east/100; - nav_utm_north0 = gps.utm_pos.north/100; -#endif + /* Set nav UTM ref */ + nav_utm_east0 = state.utm_origin_f.east; + nav_utm_north0 = state.utm_origin_f.north; + nav_utm_zone0 = state.utm_origin_f.zone; + /* Ground alt */ previous_ground_alt = ground_alt; - ground_alt = gps.hmsl/1000.; - - // reset state UTM ref - struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 }; - stateSetLocalUtmOrigin_f(&utm0); - - // realign INS if needed - ins.hf_realign = TRUE; - ins.vf_realign = TRUE; + ground_alt = state.utm_origin_f.alt; return 0; }