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/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/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 4e507ff9b7..c4628a3803 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -31,7 +31,7 @@ #include "firmwares/rotorcraft/navigation.h" #include "pprz_debug.h" -#include "subsystems/gps.h" +#include "subsystems/gps.h" // needed by auto_nav from the flight plan #include "subsystems/ins.h" #include "state.h" @@ -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_local_origin(); 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/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c deleted file mode 100644 index d570b58d33..0000000000 --- a/sw/airborne/modules/cam_control/cam_track.c +++ /dev/null @@ -1,248 +0,0 @@ -/* - * Copyright (C) 2010 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "cam_track.h" - -#include "subsystems/ins.h" -#include "state.h" - -#if USE_HFF -#include "subsystems/ins/hf_float.h" -#endif - -#include "subsystems/datalink/telemetry.h" - -struct FloatVect3 target_pos_ned; -struct FloatVect3 target_speed_ned; -struct FloatVect3 target_accel_ned; - -struct FloatVect3 last_pos_ned; - -#define CAM_DATA_LEN (3*4) -#define CAM_START_1 0xFF -#define CAM_START_2 0xFE -#define CAM_END 0xF0 - -#define UNINIT 0 -#define GOT_START_1 1 -#define GOT_START_2 2 -#define GOT_LEN 3 -#define GOT_DATA 4 -#define GOT_END 5 - -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -volatile uint8_t cam_msg_received; -uint8_t cam_status; -uint8_t cam_data_len; - -static void send_cam_track(void) { - DOWNLINK_SEND_NPS_SPEED_POS(DefaultChannel, DefaultDevice, - &target_accel_ned.x, &target_accel_ned.y, &target_accel_ned.z, - &target_speed_ned.x, &target_speed_ned.y, &target_speed_ned.z, - &target_pos_ned.x, &target_pos_ned.y, &target_pos_ned.z); -} - -void track_init(void) { - ins_impl.ltp_initialized = TRUE; // ltp is initialized and centered on the target - ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground) - - cam_status = UNINIT; - cam_data_len = CAM_DATA_LEN; - - register_periodic_telemetry(DefaultPeriodic, "CAM_TRACK", send_cam_track); -} - -#include -void track_periodic_task(void) { - char cmd_msg[256]; - uint8_t c = 0; - - cmd_msg[c++] = 'A'; - cmd_msg[c++] = ' '; - struct FloatEulers* att = stateGetNedToBodyEulers_f(); - float phi = att->phi; - if (phi > 0) cmd_msg[c++] = ' '; - else { cmd_msg[c++] = '-'; phi = -phi; } - cmd_msg[c++] = '0' + ((unsigned int) phi % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10*phi) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (100*phi) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10); - cmd_msg[c++] = ' '; - float theta = att->theta; - if (theta > 0) cmd_msg[c++] = ' '; - else { cmd_msg[c++] = '-'; theta = -theta; } - cmd_msg[c++] = '0' + ((unsigned int) theta % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10*theta) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (100*theta) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10); - cmd_msg[c++] = ' '; - float psi = att->psi; - if (psi > 0) cmd_msg[c++] = ' '; - else { cmd_msg[c++] = '-'; psi = -psi; } - cmd_msg[c++] = '0' + ((unsigned int) psi % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10*psi) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (100*psi) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10); - cmd_msg[c++] = ' '; - float alt = stateGetPositionEnu_f()->z; - //alt = 0.40; - if (alt > 0) cmd_msg[c++] = ' '; - else { cmd_msg[c++] = '-'; alt = -alt; } - cmd_msg[c++] = '0' + ((unsigned int) (alt/10) % 10); - cmd_msg[c++] = '0' + ((unsigned int) alt % 10); - cmd_msg[c++] = '0' + ((unsigned int) (10*alt) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (100*alt) % 10); - cmd_msg[c++] = '0' + ((unsigned int) (1000*alt) % 10); - cmd_msg[c++] = ' '; - cmd_msg[c++] = '\n';; - - int i; - for (i = 0; i < c; i++) { - CamUartSend1(cmd_msg[i]); - } - //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,c,cmd_msg); - -} - -void track_event(void) { - if (!ins_impl.ltp_initialized) { - ins_impl.ltp_initialized = TRUE; - ins.hf_realign = TRUE; - } - -#if USE_HFF - if (ins.hf_realign) { - ins.hf_realign = FALSE; - struct FloatVect2 pos, zero; - pos.x = -target_pos_ned.x; - pos.y = -target_pos_ned.y; - ins_realign_h(pos, zero); - } - const stuct FlotVect2 measuremet_noise = { 10.0, 10.0 }; - b2_hff_update_pos(-target_pos_ned, measurement_noise); - ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); - ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); - ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); - ins_impl.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot); - ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x); - ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y); - - INS_NED_TO_STATE(); -#else - // store pos in ins - ins_impl.ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x)); - ins_impl.ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y)); - // compute speed from last pos - // TODO get delta T - // store last pos - VECT3_COPY(last_pos_ned, target_pos_ned); - - stateSetPositionNed_i(&ins_impl.ltp_pos); -#endif - - b2_hff_lost_counter = 0; -} - -#define CAM_MAX_PAYLOAD 254 -uint8_t cam_data_buf[CAM_MAX_PAYLOAD]; -uint8_t cam_data_idx; - -void parse_cam_msg( void ) { - uint8_t* ptr; - // pos x - ptr = (uint8_t*)(&(target_pos_ned.x)); - *ptr = cam_data_buf[0]; - ptr++; - *ptr = cam_data_buf[1]; - ptr++; - *ptr = cam_data_buf[2]; - ptr++; - *ptr = cam_data_buf[3]; - // pos y - ptr = (uint8_t*)(&(target_pos_ned.y)); - *ptr = cam_data_buf[4]; - ptr++; - *ptr = cam_data_buf[5]; - ptr++; - *ptr = cam_data_buf[6]; - ptr++; - *ptr = cam_data_buf[7]; - // pos z - ptr = (uint8_t*)(&(target_pos_ned.z)); - *ptr = cam_data_buf[8]; - ptr++; - *ptr = cam_data_buf[9]; - ptr++; - *ptr = cam_data_buf[10]; - ptr++; - *ptr = cam_data_buf[11]; - - //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,12,cam_data_buf); -} - -void parse_cam_buffer( uint8_t c ) { - char bla[1]; - bla[1] = c; - //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,1,bla); - switch (cam_status) { - case UNINIT: - if (c != CAM_START_1) - goto error; - cam_status++; - break; - case GOT_START_1: - if (c != CAM_START_2) - goto error; - cam_status++; - break; - case GOT_START_2: - cam_data_len = c; - if (cam_data_len > CAM_MAX_PAYLOAD) - goto error; - cam_data_idx = 0; - cam_status++; - break; - case GOT_LEN: - cam_data_buf[cam_data_idx] = c; - cam_data_idx++; - if (cam_data_idx >= cam_data_len) - cam_status++; - break; - case GOT_DATA: - if (c != CAM_END) - goto error; - cam_msg_received = TRUE; - goto restart; - break; - } - return; - error: - restart: - cam_status = UNINIT; - return; -} - diff --git a/sw/airborne/modules/cam_control/cam_track.h b/sw/airborne/modules/cam_control/cam_track.h deleted file mode 100644 index 379f51b25f..0000000000 --- a/sw/airborne/modules/cam_control/cam_track.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright (C) 2010 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** \file cam_track.h - * - * blob tracking with cmucam - */ - -#ifndef CAM_TRACK_H -#define CAM_TRACK_H - -#include -#include "math/pprz_algebra_float.h" - -extern struct FloatVect3 target_pos_ned; -extern struct FloatVect3 target_speed_ned; -extern struct FloatVect3 target_accel_ned; - -extern void track_init(void); -extern void track_periodic_task(void); -extern void track_event(void); - -extern volatile uint8_t cam_msg_received; -extern void parse_cam_msg( void ); -extern void parse_cam_buffer( uint8_t ); - -#include "mcu_periph/uart.h" - -#define __CamLink(dev, _x) dev##_x -#define _CamLink(dev, _x) __CamLink(dev, _x) -#define CamLink(_x) _CamLink(CAM_LINK, _x) - -#define CamBuffer() CamLink(ChAvailable()) -#define ReadCamBuffer() { while (CamLink(ChAvailable())&&!cam_msg_received) parse_cam_buffer(CamLink(Getch())); } -#define CamUartSend1(c) CamLink(Transmit(c)) -#define CamUartSetBaudrate(_b) CamLink(SetBaudrate(_b)) -#define CamUartRunning CamLink(TxRunning) - -#define CamEventCheckAndHandle() { \ - if (CamBuffer()) { \ - ReadCamBuffer(); \ - } \ - if (cam_msg_received) { \ - parse_cam_msg(); \ - track_event(); \ - cam_msg_received = FALSE; \ - } \ -} - - -#endif 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.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 918dc07050..899eeb70dc 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; ///< realign horizontally if true - bool_t vf_realign; ///< realign vertically if true + enum InsStatus status; ///< status of the INS }; /** global INS state */ @@ -53,49 +53,54 @@ 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. + * Does nothing if not implemented by specific INS algorithm. */ -extern void ins_periodic( void ); +extern void ins_periodic(void); -/** INS horizontal realign. - * @param pos new horizontal position to set - * @param speed new horizontal speed to set - * Needs to be implemented by each INS algorithm. +/** INS local origin reset. + * Reset horizontal and vertical reference to the current position. + * Does nothing if not implemented by specific INS algorithm. */ -extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed); +extern void ins_reset_local_origin(void); -/** INS vertical realign. - * @param z new altitude to set - * Needs to be implemented by each INS algorithm. +/** INS altitude reference reset. + * Reset only vertical reference to the current altitude. + * Does nothing if not implemented by specific INS algorithm. */ -extern void ins_realign_v(float z); +extern void ins_reset_altitude_ref(void); + +/** INS utm zone reset. + * Reset UTM zone according te the actual position. + * 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 ); +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 ); +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 ); +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 ); +extern void ins_update_sonar(void); #endif /* INS_H */ diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 29987ca784..ac9511bb71 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -127,7 +127,7 @@ void b2_hff_store_accel_body(void) { } } -/* compute the mean of the last n accel measurements */ +/** compute the mean of the last n accel measurements */ static inline void b2_hff_compute_accel_body_mean(uint8_t n) { struct Int32Vect3 sum; int i, j; diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h index c53ddb6b8b..d485513290 100644 --- a/sw/airborne/subsystems/ins/hf_float.h +++ b/sw/airborne/subsystems/ins/hf_float.h @@ -45,7 +45,7 @@ #elif AHRS_PROPAGATE_FREQUENCY == 500 #define HFF_PRESCALER 10 #else -#error "HFF_PRESCALER needs to be a divisor of AHRS_PROPAGATE_FREQUENCY" +#error "HFF_PRESCALER not set, needs to be a divisor of AHRS_PROPAGATE_FREQUENCY" #endif #endif @@ -72,13 +72,6 @@ struct HfilterFloat { extern struct HfilterFloat b2_hff_state; -extern float b2_hff_x_meas; -extern float b2_hff_y_meas; -extern float b2_hff_xd_meas; -extern float b2_hff_yd_meas; -extern float b2_hff_xdd_meas; -extern float b2_hff_ydd_meas; - extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot); extern void b2_hff_propagate(void); extern void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index abfb7c5b5c..19f63dea9f 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,58 +78,81 @@ 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_impl.reset_alt_ref = FALSE; alt_kalman(0.); ins.status = INS_RUNNING; } -void ins_periodic( void ) { + +/** Reset the geographic reference to the current GPS fix */ +void ins_reset_local_origin(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_impl.reset_alt_ref = TRUE; } -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { +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_impl.reset_alt_ref = TRUE; } -void ins_realign_v(float z __attribute__ ((unused))) { -} - -void ins_propagate() { -} - -void ins_update_baro() {} #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); } } @@ -148,16 +169,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); @@ -165,9 +186,6 @@ void ins_update_gps(void) { #endif } -void ins_update_sonar() { -} - #ifndef GPS_DT #define GPS_DT 0.25 @@ -175,21 +193,20 @@ void ins_update_sonar() { #define GPS_SIGMA2 1. #define GPS_R 2. - 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; @@ -240,7 +257,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]; @@ -252,11 +269,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 cdce78e65d..6ecf0eb8b3 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); @@ -76,12 +73,16 @@ void ins_periodic( void ) { ins.status = INS_RUNNING; } -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { - +void ins_reset_local_origin( void ) { + ins_impl.ltp_initialized = FALSE; } -void ins_realign_v(float z __attribute__ ((unused))) { - +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_propagate() { @@ -104,10 +105,6 @@ void ins_propagate() { #endif } -void ins_update_baro() { - -} - void ins_update_gps(void) { #if USE_GPS @@ -138,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 2b84d5c174..15cfebbc3a 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" @@ -167,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 @@ -197,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() { @@ -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 @@ -247,18 +261,58 @@ 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); +#endif } -void ins_periodic(void) {} + +void ins_reset_local_origin( 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 +} + +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 +} void ahrs_init(void) { ahrs.status = AHRS_UNINIT; } - void ahrs_align(void) { /* Compute an initial orientation from accel and mag directly as quaternion */ @@ -280,12 +334,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 @@ -407,10 +460,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 @@ -418,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; @@ -429,7 +482,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) { @@ -442,11 +495,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++; } @@ -469,8 +522,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_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 ab1d5356f1..75729b341a 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,7 +42,30 @@ void ins_init() { ins.status = INS_RUNNING; } -void ins_periodic( void ) { +void ins_reset_local_origin(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_update_gps(void) { @@ -63,20 +86,3 @@ void ins_update_gps(void) { // set velocity stateSetSpeedNed_f(&ned_vel); } - - -void ins_propagate() { -} - -void ins_update_baro() { -} - -void ins_update_sonar() { -} - -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) { -} - -void ins_realign_v(float z __attribute__ ((unused))) { -} - diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 46133726a1..c70f43b4aa 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -117,6 +117,7 @@ static void send_ins_ref(void) { static void ins_init_origin_from_flightplan(void); static void ins_ned_to_state(void); +static void ins_update_from_vff(void); #if USE_HFF static void ins_update_from_hff(void); #endif @@ -141,15 +142,11 @@ 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 - vff_init(0., 0., 0., 0.); -#else - vff_init(0., 0., 0.); -#endif + vff_init_zero(); #if USE_HFF b2_hff_init(0., 0., 0., 0.); #endif @@ -170,21 +167,24 @@ void ins_periodic(void) { ins.status = INS_RUNNING; } +void ins_reset_local_origin(void) { + ins_impl.ltp_initialized = FALSE; #if USE_HFF -void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) { - b2_hff_realign(pos, speed); -} -#else -void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), - struct FloatVect2 speed __attribute__ ((unused))) {} -#endif /* USE_HFF */ - - -void ins_realign_v(float z) { - vff_realign(z); + ins_impl.hf_realign = TRUE; +#endif + ins_impl.vf_reset = TRUE; } -void ins_propagate() { +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_impl.vf_reset = TRUE; +} + +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); @@ -194,9 +194,7 @@ void ins_propagate() { float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); if (ins_impl.baro_initialized) { vff_propagate(z_accel_meas_float); - 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_update_from_vff(); } else { // feed accel from the sensors // subtract -9.81m/s2 (acceleration measured due to gravity, @@ -222,13 +220,11 @@ 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_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_update_from_vff(); } else { ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); @@ -241,13 +237,8 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_ned_to_state(); } - -void ins_update_baro() { -} - - -void ins_update_gps(void) { #if USE_GPS +void ins_update_gps(void) { if (gps.fix == GPS_FIX_3D) { if (!ins_impl.ltp_initialized) { ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); @@ -273,8 +264,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); } @@ -293,8 +284,8 @@ void ins_update_gps(void) { ins_ned_to_state(); } -#endif /* USE_GPS */ } +#endif /* USE_GPS */ //#define INS_SONAR_VARIANCE_THRESHOLD 0.01 @@ -312,8 +303,8 @@ uint8_t var_idx = 0; #endif -void ins_update_sonar() { #if USE_SONAR +void ins_update_sonar(void) { static float last_offset = 0.; // new value filtered with median_filter ins_impl.sonar_alt = update_median_filter(&ins_impl.sonar_median, sonar_meas); @@ -360,8 +351,8 @@ void ins_update_sonar() { /* update offset with last value to avoid divergence */ vff_update_offset(last_offset); } -#endif // USE_SONAR } +#endif // USE_SONAR /** initialize the local origin (ltp_def) from flight plan position */ @@ -394,6 +385,12 @@ static void ins_ned_to_state(void) { #endif } +/** 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); +} #if USE_HFF /** update ins state from horizontal 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; diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index 968640398e..f6c8bfa88c 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -70,38 +70,33 @@ temps : #define R_ALT 0.1 #define R_OFFSET 1. -float vff_z; -float vff_zdot; -float vff_bias; -float vff_offset; -float vff_zdotdot; - -float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; - -float vff_z_meas; -float vff_z_meas_baro; +struct VffExtended vff; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" static void send_vffe(void) { DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, - &vff_z_meas, &vff_z_meas_baro, - &vff_z, &vff_zdot, &vff_zdotdot, - &vff_bias, &vff_offset); + &vff.z_meas, &vff.z_meas_baro, + &vff.z, &vff.zdot, &vff.zdotdot, + &vff.bias, &vff.offset); } #endif +void vff_init_zero(void) { + vff_init(0., 0., 0., 0.); +} + void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) { - vff_z = init_z; - vff_zdot = init_zdot; - vff_bias = init_accel_bias; - vff_offset = init_baro_offset; + vff.z = init_z; + vff.zdot = init_zdot; + vff.bias = init_accel_bias; + vff.offset = init_baro_offset; int i, j; for (i=0; i