From 59d167d206e14617bf0bb7757f217e70c1c8de1c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 27 Feb 2014 15:41:01 +0100 Subject: [PATCH] [ins] use weak functions for things that don't need to be implemented by each algorithm --- sw/airborne/modules/ins/ins_xsens.c | 19 --------- sw/airborne/subsystems/ins.c | 40 +++++++++++++++++++ sw/airborne/subsystems/ins.h | 20 +++++----- sw/airborne/subsystems/ins/ins_alt_float.c | 23 ----------- sw/airborne/subsystems/ins/ins_ardrone2.c | 8 ---- .../subsystems/ins/ins_float_invariant.c | 21 ---------- .../subsystems/ins/ins_gps_passthrough.c | 26 ------------ sw/airborne/subsystems/ins/ins_int.c | 19 --------- 8 files changed, 49 insertions(+), 127 deletions(-) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index b4b2d45ea1..5f3186d9a1 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -256,25 +256,6 @@ 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/ins.c b/sw/airborne/subsystems/ins.c index f42a85ac2d..e99e8d0b84 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -27,5 +27,45 @@ #include "subsystems/ins.h" +#if USE_GPS +// for ins_reset_utm_zone +#include "subsystems/gps.h" +#include "state.h" +#endif + struct Ins ins; + +#define WEAK __attribute__((weak)) +// weak functions, used if not explicitly provided by implementation + +void WEAK ins_periodic(void) {} + +void WEAK ins_reset_local_origin(void) {} + +void WEAK ins_reset_altitude_ref(void) {} + +#if USE_GPS +void WEAK 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); +} +#else +void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm __attribute__((unused))) {} +#endif + +void WEAK ins_propagate(void) {} + +void WEAK ins_update_baro(void) {} + +void WEAK ins_update_gps(void) {} + +void WEAK ins_update_sonar(void) {} diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index cdb15483c8..899eeb70dc 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -56,51 +56,49 @@ extern struct Ins ins; extern void ins_init(void); /** INS periodic call. - * Needs to be implemented by each INS algorithm. + * 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. - * Needs to be implemented by each INS algorithm. + * Does nothing if not implemented by specific INS algorithm. */ 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. + * Does nothing if not implemented by specific 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. + * Only used with fixedwing firmware. + * Can be overwritte by specifc INS implementation. * @param utm initial utm zone, returns the corrected utm position */ extern void ins_reset_utm_zone(struct UtmCoor_f * utm); /** Propagation. Usually integrates the gyro rates to angles. * Reads the global #imu data struct. - * Needs to be implemented by each INS algorithm. + * Does nothing if not implemented by specific INS algorithm. */ 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. + * Does nothing if not implemented by specific INS algorithm. */ 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. + * Does nothing if not implemented by specific INS algorithm. */ 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. + * Does nothing if not implemented by specific INS algorithm. */ extern void ins_update_sonar(void); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 493bbec908..19f63dea9f 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -91,8 +91,6 @@ void ins_init(void) { ins.status = INS_RUNNING; } -void ins_periodic(void) { -} /** Reset the geographic reference to the current GPS fix */ void ins_reset_local_origin(void) { @@ -129,23 +127,6 @@ void ins_reset_altitude_ref(void) { ins_impl.reset_alt_ref = 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_propagate(void) { -} - -void ins_update_baro(void) {} #if USE_BAROMETER static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { @@ -205,9 +186,6 @@ void ins_update_gps(void) { #endif } -void ins_update_sonar(void) { -} - #ifndef GPS_DT #define GPS_DT 0.25 @@ -215,7 +193,6 @@ void ins_update_sonar(void) { #define GPS_SIGMA2 1. #define GPS_R 2. - static float p[2][2]; static void alt_kalman_reset(void) { diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index 19ede2bab3..6ecf0eb8b3 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -105,10 +105,6 @@ void ins_propagate() { #endif } -void ins_update_baro() { - -} - void ins_update_gps(void) { #if USE_GPS @@ -139,7 +135,3 @@ void ins_update_gps(void) { } #endif /* USE_GPS */ } - -void ins_update_sonar() { - -} diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 09a526c00a..15cfebbc3a 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -268,9 +268,6 @@ void ins_init() { #endif } -void ins_periodic(void) {} -void ins_propagate(void) {} - void ins_reset_local_origin( void ) { #if INS_UPDATE_FW_ESTIMATOR @@ -312,21 +309,6 @@ void ins_reset_altitude_ref( void ) { #endif } -#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 ahrs_init(void) { ahrs.status = AHRS_UNINIT; } @@ -492,9 +474,6 @@ void ahrs_update_gps(void) { } -void ins_update_gps(void) {} - -void ins_update_baro(void) {} static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { static float ins_qfe = 101325.0; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index af37654828..75729b341a 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -42,22 +42,6 @@ void ins_init(void) { ins.status = INS_RUNNING; } -void ins_periodic(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 @@ -102,13 +86,3 @@ void ins_update_gps(void) { // set velocity stateSetSpeedNed_f(&ned_vel); } - - -void ins_propagate(void) { -} - -void ins_update_baro(void) { -} - -void ins_update_sonar(void) { -} diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 48568e7e94..049781fe49 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -171,21 +171,6 @@ void ins_periodic(void) { ins.status = INS_RUNNING; } -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 -} - void ins_reset_local_origin(void) { ins_impl.ltp_initialized = FALSE; #if USE_HFF @@ -257,10 +242,6 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres } -void ins_update_baro(void) { -} - - void ins_update_gps(void) { #if USE_GPS if (gps.fix == GPS_FIX_3D) {