diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index c44ecb0069..114b18aa83 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -10,7 +10,6 @@ - @@ -22,7 +21,6 @@ - diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 5f3186d9a1..b4b2d45ea1 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -256,6 +256,25 @@ void ins_periodic(void) { xsens_periodic(); } +void ins_reset_local_origin(void) { +} + +void ins_reset_altitude_ref(void) { +} + +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_update_gps(void) { struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.; diff --git a/sw/airborne/subsystems/imu/imu_um6.c b/sw/airborne/subsystems/imu/imu_um6.c index 079c3f34dc..db90335469 100644 --- a/sw/airborne/subsystems/imu/imu_um6.c +++ b/sw/airborne/subsystems/imu/imu_um6.c @@ -187,10 +187,6 @@ void imu_impl_init(void) { void imu_periodic(void) { - if (ins.vf_realign == TRUE) { - UM6_imu_align(); - } - /* We would request for data here - optional //GET_DATA command 0xAE buf_out[0] = 's'; diff --git a/sw/airborne/subsystems/imu/imu_um6.h b/sw/airborne/subsystems/imu/imu_um6.h index 849d3fd7c5..04def06cdb 100644 --- a/sw/airborne/subsystems/imu/imu_um6.h +++ b/sw/airborne/subsystems/imu/imu_um6.h @@ -37,7 +37,6 @@ #include "subsystems/imu.h" #include "mcu_periph/uart.h" #include "subsystems/ahrs.h" -#include "subsystems/ins.h" #define __UM6Link(dev, _x) dev##_x #define _UM6Link(dev, _x) __UM6Link(dev, _x) @@ -107,12 +106,12 @@ struct UM6Packet { }; enum UM6PacketStatus { - UM6PacketWaiting, - UM6PacketReadingS, - UM6PacketReadingN, - UM6PacketReadingPT, - UM6PacketReadingAddr, - UM6PacketReadingData + UM6PacketWaiting, + UM6PacketReadingS, + UM6PacketReadingN, + UM6PacketReadingPT, + UM6PacketReadingAddr, + UM6PacketReadingData }; enum UM6Status { @@ -120,22 +119,22 @@ enum UM6Status { UM6Running }; -#define imu_um6_event(_callback1, _callback2, _callback3) { \ - if (UM6Buffer()) { \ - ReadUM6Buffer(); \ - } \ - if (UM6_packet.msg_available) { \ - UM6_packet.msg_available = FALSE; \ - UM6_packet_read_message(); \ - _callback1(); \ - _callback2(); \ - _callback3(); \ - } \ -} +#define imu_um6_event(_callback1, _callback2, _callback3) { \ + if (UM6Buffer()) { \ + ReadUM6Buffer(); \ + } \ + if (UM6_packet.msg_available) { \ + UM6_packet.msg_available = FALSE; \ + UM6_packet_read_message(); \ + _callback1(); \ + _callback2(); \ + _callback3(); \ + } \ + } -#define ReadUM6Buffer() { \ - while (UM6Link(ChAvailable())&&!UM6_packet.msg_available) \ - UM6_packet_parse(UM6Link(Getch())); \ +#define ReadUM6Buffer() { \ + while (UM6Link(ChAvailable())&&!UM6_packet.msg_available) \ + UM6_packet_parse(UM6Link(Getch())); \ } #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index cb3a69044b..3509b1412c 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -32,8 +32,10 @@ #include "math/pprz_algebra_float.h" #include "state.h" -#define INS_UNINIT 0 -#define INS_RUNNING 1 +enum InsStatus { + INS_UNINIT=0, + INS_RUNNING=1 +}; /* underlying includes (needed for parameters) */ #ifdef INS_TYPE_H @@ -42,9 +44,7 @@ /** Inertial Navigation System state */ struct Ins { - uint8_t status; ///< status of the INS - bool_t hf_realign; ///< flag to request to realign horizontal filter to current position (local origin unchanged) - bool_t vf_realign; ///< flag to request to realign vertical filter to ground level (local origin unchanged) + enum InsStatus status; ///< status of the INS }; /** global INS state */ @@ -53,24 +53,24 @@ extern struct Ins ins; /** INS initialization. Called at startup. * Needs to be implemented by each INS algorithm. */ -extern void ins_init( void ); +extern void ins_init(void); /** INS periodic call. * Needs to be implemented by each INS algorithm. */ -extern void ins_periodic( void ); +extern void ins_periodic(void); /** INS local origin reset. * Reset horizontal and vertical reference to the current position. * Needs to be implemented by each INS algorithm. */ -extern void ins_reset_local_origin( void ); +extern void ins_reset_local_origin(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 ); +extern void ins_reset_altitude_ref(void); /** INS utm zone reset. * Reset UTM zone according te the actual position. @@ -82,16 +82,16 @@ extern void ins_reset_utm_zone(struct UtmCoor_f * utm); /** INS horizontal realign. * This only reset the filter to a given value, but doesn't change the local reference. + * Needs to be implemented by each INS algorithm. * @param pos new horizontal position to set * @param speed new horizontal speed to set - * Needs to be implemented by each INS algorithm. */ extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed); /** INS vertical realign. * This only reset the filter to a given value, but doesn't change the local reference. - * @param z new altitude to set * Needs to be implemented by each INS algorithm. + * @param z new altitude to set */ extern void ins_realign_v(float z); @@ -99,25 +99,25 @@ extern void ins_realign_v(float z); * Reads the global #imu data struct. * Needs to be implemented by each INS algorithm. */ -extern void ins_propagate( void ); +extern void ins_propagate(void); /** Update INS state with barometer measurements. * Reads the global #baro data struct. * Needs to be implemented by each INS algorithm. */ -extern void ins_update_baro( void ); +extern void ins_update_baro(void); /** Update INS state with GPS measurements. * Reads the global #gps data struct. * Needs to be implemented by each INS algorithm. */ -extern void ins_update_gps( void ); +extern void ins_update_gps(void); /** Update INS state with sonar measurements. * Reads the global #sonar data struct. * Needs to be implemented by each INS algorithm. */ -extern void ins_update_sonar( void ); +extern void ins_update_sonar(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 4ddeb0ef3d..0499e48b30 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -47,9 +47,8 @@ #warning Please remove the obsolete ALT_KALMAN and ALT_KALMAN_ENABLED defines from your airframe file. #endif -/* vertical position and speed in meters (z-up)*/ -float ins_alt; -float ins_alt_dot; + +struct InsAltFloat ins_impl; #if USE_BAROMETER #include "subsystems/sensors/baro.h" @@ -57,10 +56,6 @@ float ins_alt_dot; PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.") -float ins_qfe; -bool_t ins_baro_initialized; -float ins_baro_alt; - // Baro event on ABI #ifndef INS_BARO_ID #define INS_BARO_ID BARO_BOARD_SENDER_ID @@ -69,8 +64,11 @@ abi_event baro_ev; static void baro_cb(uint8_t sender_id, const float *pressure); #endif /* USE_BAROMETER */ +static void alt_kalman_reset(void); +static void alt_kalman_init(void); +static void alt_kalman(float); -void ins_init() { +void ins_init(void) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); @@ -80,25 +78,24 @@ void ins_init() { alt_kalman_init(); #if USE_BAROMETER - ins_qfe = 0;; - ins_baro_initialized = FALSE; - ins_baro_alt = 0.; + ins_impl.qfe = 0;; + ins_impl.baro_initialized = FALSE; + ins_impl.baro_alt = 0.; // Bind to BARO_ABS message AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); #endif - ins.vf_realign = FALSE; - ins.hf_realign = FALSE; + ins_impl.reset_alt_ref = FALSE; alt_kalman(0.); ins.status = INS_RUNNING; } -void ins_periodic( void ) { +void ins_periodic(void) { } /** Reset the geographic reference to the current GPS fix */ -void ins_reset_local_origin( void ) { +void ins_reset_local_origin(void) { struct UtmCoor_f utm; #ifdef GPS_USE_LATLONG /* Recompute UTM coordinates in this zone */ @@ -119,17 +116,17 @@ void ins_reset_local_origin( void ) { stateSetLocalUtmOrigin_f(&utm); // reset filter flag - ins.vf_realign = TRUE; + ins_impl.reset_alt_ref = TRUE; } -void ins_reset_altitude_ref( void ) { +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; + ins_impl.reset_alt_ref = TRUE; } void ins_reset_utm_zone(struct UtmCoor_f * utm) { @@ -148,42 +145,42 @@ void ins_reset_utm_zone(struct UtmCoor_f * utm) { void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { } -void ins_realign_v(float z __attribute__ ((unused))) { - ins_alt = z; - ins_alt_dot = 0.; +void ins_realign_v(float z) { + ins_impl.alt = z; + ins_impl.alt_dot = 0.; alt_kalman_reset(); } -void ins_propagate() { +void ins_propagate(void) { } -void ins_update_baro() {} +void ins_update_baro(void) {} #if USE_BAROMETER static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { - if (!ins_baro_initialized) { - ins_qfe = *pressure; - ins_baro_initialized = TRUE; + if (!ins_impl.baro_initialized) { + ins_impl.qfe = *pressure; + ins_impl.baro_initialized = TRUE; } - if (ins.vf_realign) { - ins.vf_realign = FALSE; - ins_alt = ground_alt; - ins_alt_dot = 0.; - ins_qfe = *pressure; + if (ins_impl.reset_alt_ref) { + ins_impl.reset_alt_ref = FALSE; + ins_impl.alt = ground_alt; + ins_impl.alt_dot = 0.; + ins_impl.qfe = *pressure; alt_kalman_reset(); } else { /* not realigning, so normal update with baro measurement */ - ins_baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_qfe); + ins_impl.baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); /* run the filter */ - alt_kalman(ins_baro_alt); + alt_kalman(ins_impl.baro_alt); /* set new altitude, just copy old horizontal position */ struct UtmCoor_f utm; UTM_COPY(utm, *stateGetPositionUtm_f()); - utm.alt = ins_alt; + utm.alt = ins_impl.alt; stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel; memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f)); - ned_vel.z = -ins_alt_dot; + ned_vel.z = -ins_impl.alt_dot; stateSetSpeedNed_f(&ned_vel); } } @@ -200,16 +197,16 @@ void ins_update_gps(void) { #if !USE_BAROMETER float falt = gps.hmsl / 1000.; alt_kalman(falt); - ins_alt_dot = -gps.ned_vel.z / 100.; + ins_impl.alt_dot = -gps.ned_vel.z / 100.; #endif - utm.alt = ins_alt; + utm.alt = ins_impl.alt; // set position stateSetPositionUtm_f(&utm); struct NedCoor_f ned_vel = { gps.ned_vel.x / 100., gps.ned_vel.y / 100., - -ins_alt_dot + -ins_impl.alt_dot }; // set velocity stateSetSpeedNed_f(&ned_vel); @@ -217,7 +214,7 @@ void ins_update_gps(void) { #endif } -void ins_update_sonar() { +void ins_update_sonar(void) { } @@ -230,18 +227,18 @@ void ins_update_sonar() { static float p[2][2]; -void alt_kalman_reset( void ) { +static void alt_kalman_reset(void) { p[0][0] = 1.; p[0][1] = 0.; p[1][0] = 0.; p[1][1] = 1.; } -void alt_kalman_init( void ) { +static void alt_kalman_init(void) { alt_kalman_reset(); } -void alt_kalman(float z_meas) { +static void alt_kalman(float z_meas) { float DT = GPS_DT; float R = GPS_R; float SIGMA2 = GPS_SIGMA2; @@ -292,7 +289,7 @@ void alt_kalman(float z_meas) { /* predict */ - ins_alt += ins_alt_dot * DT; + ins_impl.alt += ins_impl.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]; @@ -304,11 +301,11 @@ void alt_kalman(float z_meas) { if (fabs(e) > 1e-5) { float k_0 = p[0][0] / e; float k_1 = p[1][0] / e; - e = z_meas - ins_alt; + e = z_meas - ins_impl.alt; /* correction */ - ins_alt += k_0 * e; - ins_alt_dot += k_1 * e; + ins_impl.alt += k_0 * e; + ins_impl.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]; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 5aa353fc13..78d78ee9d8 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -33,17 +33,20 @@ #include #include "std.h" -extern float ins_alt; ///< estimated altitude above MSL in meters -extern float ins_alt_dot; ///< estimated vertical speed in m/s (positive-up) +/** Ins implementation state (altitude, float) */ +struct InsAltFloat { + float alt; ///< estimated altitude above MSL in meters + float alt_dot; ///< estimated vertical speed in m/s (positive-up) + + bool_t reset_alt_ref; ///< flag to request reset of altitude reference to current alt #if USE_BAROMETER -extern float ins_qfe; -extern float ins_baro_alt; -extern bool_t ins_baro_initialized; + float qfe; + float baro_alt; + bool_t baro_initialized; #endif +}; -extern void alt_kalman_reset( void ); -extern void alt_kalman_init( void ); -extern void alt_kalman( float ); +extern struct InsAltFloat ins_impl; #endif /* INS_ALT_FLOAT_H */ diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index c9ae7ff5ed..f166152b91 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -63,9 +63,6 @@ void ins_init() { ins_impl.ltp_initialized = FALSE; #endif - ins.vf_realign = FALSE; - ins.hf_realign = FALSE; - INT32_VECT3_ZERO(ins_impl.ltp_pos); INT32_VECT3_ZERO(ins_impl.ltp_speed); INT32_VECT3_ZERO(ins_impl.ltp_accel); diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 48ac3732ba..9b64c3699c 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -181,7 +181,7 @@ static const struct FloatVect3 A = { 0.f, 0.f, 9.81f }; static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) }; /* barometer */ -bool_t ins_baro_initialised; +bool_t ins_baro_initialized; // Baro event on ABI #ifndef INS_BARO_ID #define INS_BARO_ID BARO_BOARD_SENDER_ID @@ -211,7 +211,7 @@ static inline void init_invariant_state(void) { ins_impl.meas.baro_alt = 0.; // init baro - ins_baro_initialised = FALSE; + ins_baro_initialized = FALSE; } void ins_init() { @@ -261,8 +261,7 @@ void ins_init() { ins_impl.gains.sh = INS_INV_SH; ins.status = INS_UNINIT; - ins.hf_realign = FALSE; - ins.vf_realign = FALSE; + ins_impl.reset = FALSE; #if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); @@ -298,8 +297,6 @@ void ins_reset_local_origin( void ) { ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(<p_def); #endif - ins.hf_realign = FALSE; - ins.vf_realign = FALSE; } void ins_reset_altitude_ref( void ) { @@ -313,7 +310,6 @@ void ins_reset_altitude_ref( void ) { ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(<p_def); #endif - ins.vf_realign = FALSE; } #if INS_UPDATE_FW_ESTIMATOR @@ -361,12 +357,11 @@ void ahrs_propagate(void) { // realign all the filter if needed // a complete init cycle is required - if (ins.hf_realign || ins.vf_realign) { + if (ins_impl.reset) { + ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); - ins.hf_realign = FALSE; - ins.vf_realign = FALSE; } // fill command vector @@ -513,7 +508,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres static float baro_moy = 0.; static float baro_prev = 0.; - if (!ins_baro_initialised) { + if (!ins_baro_initialized) { // try to find a stable qfe // TODO generic function in pprz_isa ? if (i == 1) { @@ -526,11 +521,11 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres // test stop condition if (fabs(alpha) < 0.005) { ins_qfe = baro_moy; - ins_baro_initialised = TRUE; + ins_baro_initialized = TRUE; } if (i == 250) { ins_qfe = *pressure; - ins_baro_initialised = TRUE; + ins_baro_initialized = TRUE; } i++; } diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.h b/sw/airborne/subsystems/ins/ins_float_invariant.h index 9a7e5aafd5..eaebf1529e 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.h +++ b/sw/airborne/subsystems/ins/ins_float_invariant.h @@ -22,7 +22,7 @@ /* * For more information, please send an email to "jp.condomines@gmail.com" -*/ + */ #ifndef INS_FLOAT_INVARIANT_H #define INS_FLOAT_INVARIANT_H @@ -107,6 +107,8 @@ struct InsFloatInv { struct inv_command cmd; ///< command vector struct inv_correction_gains corr; ///< correction gains struct inv_gains gains; ///< tuning gains + + bool_t reset; ///< flag to request reset/reinit the filter }; extern struct InsFloatInv ins_impl; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 3cb75ca88d..8dbe0d9ac5 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -34,7 +34,7 @@ #include "subsystems/gps.h" #include "subsystems/nav.h" -void ins_init() { +void ins_init(void) { struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); @@ -42,10 +42,23 @@ void ins_init() { ins.status = INS_RUNNING; } -void ins_periodic( void ) { +void ins_periodic(void) { } -void ins_reset_local_origin( void ) { +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_reset_local_origin(void) { struct UtmCoor_f utm; #ifdef GPS_USE_LATLONG /* Recompute UTM coordinates in this zone */ @@ -65,15 +78,12 @@ void ins_reset_local_origin( void ) { stateSetLocalUtmOrigin_f(&utm); } -void ins_reset_altitude_ref( void ) { +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.; @@ -94,13 +104,13 @@ void ins_update_gps(void) { } -void ins_propagate() { +void ins_propagate(void) { } -void ins_update_baro() { +void ins_update_baro(void) { } -void ins_update_sonar() { +void ins_update_sonar(void) { } void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 1068f4b0e0..81b892f3f8 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -142,8 +142,8 @@ void ins_init(void) { ins_impl.sonar_offset = INS_SONAR_OFFSET; #endif - ins.vf_realign = FALSE; - ins.hf_realign = FALSE; + ins_impl.vf_reset = FALSE; + ins_impl.hf_realign = FALSE; /* init vertical and horizontal filters */ #if USE_VFF_EXTENDED @@ -171,21 +171,36 @@ void ins_periodic(void) { ins.status = INS_RUNNING; } -void ins_reset_local_origin( void ) { - ins_impl.ltp_initialized = FALSE; -#if USE_HFF - ins.hf_realign = TRUE; +void ins_reset_utm_zone(struct UtmCoor_f * utm) { +#if USE_GPS + 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 - ins.vf_realign = TRUE; } -void ins_reset_altitude_ref( void ) { +void ins_reset_local_origin(void) { + ins_impl.ltp_initialized = FALSE; +#if USE_HFF + ins_impl.hf_realign = TRUE; +#endif + ins_impl.vf_reset = 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; + ins_impl.vf_reset = TRUE; } #if USE_HFF @@ -204,7 +219,7 @@ void ins_realign_v(float z) { ins_update_from_vff(); } -void ins_propagate() { +void ins_propagate(void) { /* untilt accels */ struct Int32Vect3 accel_meas_body; INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); @@ -240,8 +255,8 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_impl.qfe = *pressure; ins_impl.baro_initialized = TRUE; } - if (ins.vf_realign) { - ins.vf_realign = FALSE; + if (ins_impl.vf_reset) { + ins_impl.vf_reset = FALSE; ins_impl.qfe = *pressure; vff_realign(0.); ins_update_from_vff(); @@ -258,7 +273,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres } -void ins_update_baro() { +void ins_update_baro(void) { } @@ -289,8 +304,8 @@ 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.hf_realign) { - ins.hf_realign = FALSE; + if (ins_impl.hf_realign) { + ins_impl.hf_realign = FALSE; const struct FloatVect2 zero = {0.0, 0.0}; b2_hff_realign(gps_pos_m_ned, zero); } @@ -328,7 +343,7 @@ uint8_t var_idx = 0; #endif -void ins_update_sonar() { +void ins_update_sonar(void) { #if USE_SONAR static float last_offset = 0.; // new value filtered with median_filter diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index 71171c5fbf..d04ef30c6e 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -43,6 +43,16 @@ struct InsInt { struct LtpDef_i ltp_def; bool_t ltp_initialized; + /** request to realign horizontal filter. + * Sets to current position (local origin unchanged). + */ + bool_t hf_realign; + + /** request to reset vertical filter. + * Sets the z-position to zero and resets the the z-reference to current altitude. + */ + bool_t vf_reset; + /* output LTP NED */ struct NedCoor_i ltp_pos; struct NedCoor_i ltp_speed;