Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge pull request #644 from paparazzi/ins_reset

Browse Source 
- reset reference position from INS not from NAV
- remove hf_realign, vf_realign from ins struct
- remove ins_realign_v and ins_realign_h API functions
- weak functions for things that don't need to be implemented in each algoritm
This commit is contained in:
Felix Ruess
2014-02-27 18:53:42 +01:00
parent c48e0975be 02b32c97ea
commit 7cb1f8a890
24 changed files with 593 additions and 836 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/versatile.xml
+2 -2
View File
@@ -12,8 +12,8 @@
<waypoint name="MOB" x="-11.5" y="-21.2"/>
<waypoint name="S1" x="-151.6" y="80.4"/>
<waypoint name="S2" x="180.1" y="214.9"/>
<waypoint alt="30" name="AF" x="200" y="-10"/>
<waypoint alt="0" name="TD" x="80.0" y="20.0"/>
<waypoint alt="215.0" name="AF" x="200" y="-10"/>
<waypoint alt="185.0" name="TD" x="80.0" y="20.0"/>
<waypoint name="BASELEG" x="26.9" y="-23.0"/>
<waypoint name="_1" x="-100" y="0"/>
<waypoint name="_2" x="-100" y="200"/>
conf/settings/control/rotorcraft_guidance.xml
-2
View File
@@ -10,7 +10,6 @@
<dl_setting var="guidance_v_nominal_throttle" min="0.2" step="0.01" max="0.8" module="guidance/guidance_v" shortname="nominal_throttle" param="GUIDANCE_V_NOMINAL_HOVER_THROTTLE"/>
<dl_setting var="guidance_v_adapt_throttle_enabled" min="0" step="1" max="1" module="guidance/guidance_v" shortname="adapt_throttle" param="GUIDANCE_V_ADAPT_THROTTLE_ENABLED" values="FALSE|TRUE"/>
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins.vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
@@ -22,7 +21,6 @@
<dl_setting var="guidance_h_again" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ka" param="GUIDANCE_H_AGAIN"/>
<dl_setting var="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins.hf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="NAV">
sw/airborne/firmwares/rotorcraft/navigation.c
+4 -13
View File
@@ -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) {
sw/airborne/modules/cam_control/cam_track.c
-248
View File
@@ -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 <stdio.h>
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;
}
sw/airborne/modules/cam_control/cam_track.h
-70
View File
@@ -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 <inttypes.h>
#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
sw/airborne/subsystems/imu/imu_um6.c
-4
View File
@@ -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';
sw/airborne/subsystems/imu/imu_um6.h
+21 -22
View File
@@ -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) { \
sw/airborne/subsystems/ins.c
+40
View File
@@ -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) {}
sw/airborne/subsystems/ins.h
+32 -27
View File
@@ -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 */
sw/airborne/subsystems/ins/hf_float.c
+1 -1
View File
@@ -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;
sw/airborne/subsystems/ins/hf_float.h
+1 -8
View File
@@ -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);
sw/airborne/subsystems/ins/ins_alt_float.c
+64 -47
View File
@@ -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];
sw/airborne/subsystems/ins/ins_alt_float.h
+11 -8
View File
@@ -33,17 +33,20 @@
#include <inttypes.h>
#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 */
sw/airborne/subsystems/ins/ins_ardrone2.c
+8 -15
View File
@@ -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() {
}
sw/airborne/subsystems/ins/ins_float_invariant.c
+74 -21
View File
@@ -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(&ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ltp_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(&ltp_def, &gps.ecef_pos);
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ltp_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(&ltp_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;
sw/airborne/subsystems/ins/ins_float_invariant.h
+3 -1
View File
@@ -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;
sw/airborne/subsystems/ins/ins_gps_passthrough.c
+25 -19
View File
@@ -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))) {
}
sw/airborne/subsystems/ins/ins_int.c
+35 -38
View File
@@ -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 */
sw/airborne/subsystems/ins/ins_int.h
+10
View File
@@ -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;
sw/airborne/subsystems/ins/vf_extended_float.c
+135 -140
View File
@@ -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<VFF_STATE_SIZE; i++) {
for (j=0; j<VFF_STATE_SIZE; j++)
vff_P[i][j] = 0.;
vff_P[i][i] = INIT_PXX;
vff.P[i][j] = 0.;
vff.P[i][i] = INIT_PXX;
}
#if PERIODIC_TELEMETRY
@@ -131,31 +126,31 @@ void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_b
*/
void vff_propagate(float accel) {
/* update state */
vff_zdotdot = accel + 9.81 - vff_bias;
vff_z = vff_z + DT_VFILTER * vff_zdot;
vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
vff.zdotdot = accel + 9.81 - vff.bias;
vff.z = vff.z + DT_VFILTER * vff.zdot;
vff.zdot = vff.zdot + DT_VFILTER * vff.zdotdot;
/* update covariance */
const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
const float FPF22 = vff_P[2][2];
const float FPF33 = vff_P[3][3];
const float FPF00 = vff.P[0][0] + DT_VFILTER * ( vff.P[1][0] + vff.P[0][1] + DT_VFILTER * vff.P[1][1] );
const float FPF01 = vff.P[0][1] + DT_VFILTER * ( vff.P[1][1] - vff.P[0][2] - DT_VFILTER * vff.P[1][2] );
const float FPF02 = vff.P[0][2] + DT_VFILTER * ( vff.P[1][2] );
const float FPF10 = vff.P[1][0] + DT_VFILTER * (-vff.P[2][0] + vff.P[1][1] - DT_VFILTER * vff.P[2][1] );
const float FPF11 = vff.P[1][1] + DT_VFILTER * (-vff.P[2][1] - vff.P[1][2] + DT_VFILTER * vff.P[2][2] );
const float FPF12 = vff.P[1][2] + DT_VFILTER * (-vff.P[2][2] );
const float FPF20 = vff.P[2][0] + DT_VFILTER * ( vff.P[2][1] );
const float FPF21 = vff.P[2][1] + DT_VFILTER * (-vff.P[2][2] );
const float FPF22 = vff.P[2][2];
const float FPF33 = vff.P[3][3];
vff_P[0][0] = FPF00 + Qzz;
vff_P[0][1] = FPF01;
vff_P[0][2] = FPF02;
vff_P[1][0] = FPF10;
vff_P[1][1] = FPF11 + Qzdotzdot;
vff_P[1][2] = FPF12;
vff_P[2][0] = FPF20;
vff_P[2][1] = FPF21;
vff_P[2][2] = FPF22 + Qbiasbias;
vff_P[3][3] = FPF33 + Qoffoff;
vff.P[0][0] = FPF00 + Qzz;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
vff.P[1][1] = FPF11 + Qzdotzdot;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
vff.P[2][2] = FPF22 + Qbiasbias;
vff.P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe());
@@ -176,42 +171,42 @@ void vff_propagate(float accel) {
// update covariance
Pp = Pm - K*H*Pm;
*/
__attribute__ ((always_inline)) static inline void update_baro_conf(float z_meas, float conf) {
vff_z_meas_baro = z_meas;
static void update_baro_conf(float z_meas, float conf) {
vff.z_meas_baro = z_meas;
const float y = z_meas - vff_z + vff_offset;
const float S = vff_P[0][0] - vff_P[0][3] - vff_P[3][0] + vff_P[3][3] + conf;
const float K0 = (vff_P[0][0] - vff_P[0][3]) * 1/S;
const float K1 = (vff_P[1][0] - vff_P[1][3]) * 1/S;
const float K2 = (vff_P[2][0] - vff_P[2][3]) * 1/S;
const float K3 = (vff_P[3][0] - vff_P[3][3]) * 1/S;
const float y = z_meas - vff.z + vff.offset;
const float S = vff.P[0][0] - vff.P[0][3] - vff.P[3][0] + vff.P[3][3] + conf;
const float K0 = (vff.P[0][0] - vff.P[0][3]) * 1/S;
const float K1 = (vff.P[1][0] - vff.P[1][3]) * 1/S;
const float K2 = (vff.P[2][0] - vff.P[2][3]) * 1/S;
const float K3 = (vff.P[3][0] - vff.P[3][3]) * 1/S;
vff_z = vff_z + K0 * y;
vff_zdot = vff_zdot + K1 * y;
vff_bias = vff_bias + K2 * y;
vff_offset = vff_offset + K3 * y;
vff.z = vff.z + K0 * y;
vff.zdot = vff.zdot + K1 * y;
vff.bias = vff.bias + K2 * y;
vff.offset = vff.offset + K3 * y;
const float P0 = vff_P[0][0] - vff_P[3][0];
const float P1 = vff_P[0][1] - vff_P[3][1];
const float P2 = vff_P[0][2] - vff_P[3][2];
const float P3 = vff_P[0][3] - vff_P[3][3];
const float P0 = vff.P[0][0] - vff.P[3][0];
const float P1 = vff.P[0][1] - vff.P[3][1];
const float P2 = vff.P[0][2] - vff.P[3][2];
const float P3 = vff.P[0][3] - vff.P[3][3];
vff_P[0][0] -= K0 * P0;
vff_P[0][1] -= K0 * P1;
vff_P[0][2] -= K0 * P2;
vff_P[0][3] -= K0 * P3;
vff_P[1][0] -= K1 * P0;
vff_P[1][1] -= K1 * P1;
vff_P[1][2] -= K1 * P2;
vff_P[1][3] -= K1 * P3;
vff_P[2][0] -= K2 * P0;
vff_P[2][1] -= K2 * P1;
vff_P[2][2] -= K2 * P2;
vff_P[2][3] -= K2 * P3;
vff_P[3][0] -= K3 * P0;
vff_P[3][1] -= K3 * P1;
vff_P[3][2] -= K3 * P2;
vff_P[3][3] -= K3 * P3;
vff.P[0][0] -= K0 * P0;
vff.P[0][1] -= K0 * P1;
vff.P[0][2] -= K0 * P2;
vff.P[0][3] -= K0 * P3;
vff.P[1][0] -= K1 * P0;
vff.P[1][1] -= K1 * P1;
vff.P[1][2] -= K1 * P2;
vff.P[1][3] -= K1 * P3;
vff.P[2][0] -= K2 * P0;
vff.P[2][1] -= K2 * P1;
vff.P[2][2] -= K2 * P2;
vff.P[2][3] -= K2 * P3;
vff.P[3][0] -= K3 * P0;
vff.P[3][1] -= K3 * P1;
vff.P[3][2] -= K3 * P2;
vff.P[3][3] -= K3 * P3;
}
@@ -237,42 +232,42 @@ void vff_update_baro_conf(float z_meas, float conf) {
// update covariance
Pp = Pm - K*H*Pm;
*/
__attribute__ ((always_inline)) static inline void update_alt_conf(float z_meas, float conf) {
vff_z_meas = z_meas;
static void update_alt_conf(float z_meas, float conf) {
vff.z_meas = z_meas;
const float y = z_meas - vff_z;
const float S = vff_P[0][0] + conf;
const float K0 = vff_P[0][0] * 1/S;
const float K1 = vff_P[1][0] * 1/S;
const float K2 = vff_P[2][0] * 1/S;
const float K3 = vff_P[3][0] * 1/S;
const float y = z_meas - vff.z;
const float S = vff.P[0][0] + conf;
const float K0 = vff.P[0][0] * 1/S;
const float K1 = vff.P[1][0] * 1/S;
const float K2 = vff.P[2][0] * 1/S;
const float K3 = vff.P[3][0] * 1/S;
vff_z = vff_z + K0 * y;
vff_zdot = vff_zdot + K1 * y;
vff_bias = vff_bias + K2 * y;
vff_offset = vff_offset + K3 * y;
vff.z = vff.z + K0 * y;
vff.zdot = vff.zdot + K1 * y;
vff.bias = vff.bias + K2 * y;
vff.offset = vff.offset + K3 * y;
const float P0 = vff_P[0][0];
const float P1 = vff_P[0][1];
const float P2 = vff_P[0][2];
const float P3 = vff_P[0][3];
const float P0 = vff.P[0][0];
const float P1 = vff.P[0][1];
const float P2 = vff.P[0][2];
const float P3 = vff.P[0][3];
vff_P[0][0] -= K0 * P0;
vff_P[0][1] -= K0 * P1;
vff_P[0][2] -= K0 * P2;
vff_P[0][3] -= K0 * P3;
vff_P[1][0] -= K1 * P0;
vff_P[1][1] -= K1 * P1;
vff_P[1][2] -= K1 * P2;
vff_P[1][3] -= K1 * P3;
vff_P[2][0] -= K2 * P0;
vff_P[2][1] -= K2 * P1;
vff_P[2][2] -= K2 * P2;
vff_P[2][3] -= K2 * P3;
vff_P[3][0] -= K3 * P0;
vff_P[3][1] -= K3 * P1;
vff_P[3][2] -= K3 * P2;
vff_P[3][3] -= K3 * P3;
vff.P[0][0] -= K0 * P0;
vff.P[0][1] -= K0 * P1;
vff.P[0][2] -= K0 * P2;
vff.P[0][3] -= K0 * P3;
vff.P[1][0] -= K1 * P0;
vff.P[1][1] -= K1 * P1;
vff.P[1][2] -= K1 * P2;
vff.P[1][3] -= K1 * P3;
vff.P[2][0] -= K2 * P0;
vff.P[2][1] -= K2 * P1;
vff.P[2][2] -= K2 * P2;
vff.P[2][3] -= K2 * P3;
vff.P[3][0] -= K3 * P0;
vff.P[3][1] -= K3 * P1;
vff.P[3][2] -= K3 * P2;
vff.P[3][3] -= K3 * P3;
}
void vff_update_alt(float z_meas) {
@@ -297,41 +292,41 @@ void vff_update_alt_conf(float z_meas, float conf) {
// update covariance
Pp = Pm - K*H*Pm;
*/
__attribute__ ((always_inline)) static inline void update_offset_conf(float offset, float conf) {
static void update_offset_conf(float offset, float conf) {
const float y = offset - vff_offset;
const float S = vff_P[3][3] + conf;
const float K0 = vff_P[0][3] * 1/S;
const float K1 = vff_P[1][3] * 1/S;
const float K2 = vff_P[2][3] * 1/S;
const float K3 = vff_P[3][3] * 1/S;
const float y = offset - vff.offset;
const float S = vff.P[3][3] + conf;
const float K0 = vff.P[0][3] * 1/S;
const float K1 = vff.P[1][3] * 1/S;
const float K2 = vff.P[2][3] * 1/S;
const float K3 = vff.P[3][3] * 1/S;
vff_z = vff_z + K0 * y;
vff_zdot = vff_zdot + K1 * y;
vff_bias = vff_bias + K2 * y;
vff_offset = vff_offset + K3 * y;
vff.z = vff.z + K0 * y;
vff.zdot = vff.zdot + K1 * y;
vff.bias = vff.bias + K2 * y;
vff.offset = vff.offset + K3 * y;
const float P0 = vff_P[3][0];
const float P1 = vff_P[3][1];
const float P2 = vff_P[3][2];
const float P3 = vff_P[3][3];
const float P0 = vff.P[3][0];
const float P1 = vff.P[3][1];
const float P2 = vff.P[3][2];
const float P3 = vff.P[3][3];
vff_P[0][0] -= K0 * P0;
vff_P[0][1] -= K0 * P1;
vff_P[0][2] -= K0 * P2;
vff_P[0][3] -= K0 * P3;
vff_P[1][0] -= K1 * P0;
vff_P[1][1] -= K1 * P1;
vff_P[1][2] -= K1 * P2;
vff_P[1][3] -= K1 * P3;
vff_P[2][0] -= K2 * P0;
vff_P[2][1] -= K2 * P1;
vff_P[2][2] -= K2 * P2;
vff_P[2][3] -= K2 * P3;
vff_P[3][0] -= K3 * P0;
vff_P[3][1] -= K3 * P1;
vff_P[3][2] -= K3 * P2;
vff_P[3][3] -= K3 * P3;
vff.P[0][0] -= K0 * P0;
vff.P[0][1] -= K0 * P1;
vff.P[0][2] -= K0 * P2;
vff.P[0][3] -= K0 * P3;
vff.P[1][0] -= K1 * P0;
vff.P[1][1] -= K1 * P1;
vff.P[1][2] -= K1 * P2;
vff.P[1][3] -= K1 * P3;
vff.P[2][0] -= K2 * P0;
vff.P[2][1] -= K2 * P1;
vff.P[2][2] -= K2 * P2;
vff.P[2][3] -= K2 * P3;
vff.P[3][0] -= K3 * P0;
vff.P[3][1] -= K3 * P1;
vff.P[3][2] -= K3 * P2;
vff.P[3][3] -= K3 * P3;
}
void vff_update_offset(float offset) {
@@ -340,8 +335,8 @@ void vff_update_offset(float offset) {
void vff_realign(float z_meas) {
//vff_z = z_meas;
//vff_zdot = 0.;
//vff_offset = 0.;
//vff.z = z_meas;
//vff.zdot = 0.;
//vff.offset = 0.;
vff_init(z_meas, 0., 0., 0.);
}
sw/airborne/subsystems/ins/vf_extended_float.h
+15 -8
View File
@@ -32,16 +32,23 @@
#define VFF_STATE_SIZE 4
extern float vff_z;
extern float vff_zdot;
extern float vff_bias;
extern float vff_offset;
extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
extern float vff_zdotdot;
struct VffExtended {
/* state vector */
float z; ///< z-position estimate in m (NED, z-down)
float zdot; ///< z-velocity estimate in m/s (NED, z-down)
float bias; ///< accel bias estimate in m/s^2
float offset; ///< baro offset estimate
extern float vff_z_meas;
extern float vff_z_meas_baro;
float zdotdot; ///< z-acceleration in m/s^2 (NED, z-down)
float z_meas; ///< last z measurement in m
float z_meas_baro; ///< last z measurement from baro in m
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]; ///< covariance matrix
};
extern struct VffExtended vff;
extern void vff_init_zero(void);
extern void vff_init(float z, float zdot, float accel_bias, float baro_offset);
extern void vff_propagate(float accel);
extern void vff_update_baro(float z_meas);
sw/airborne/subsystems/ins/vf_float.c
+88 -91
View File
@@ -66,34 +66,31 @@ temps :
#define Qzdotzdot VF_FLOAT_ACCEL_NOISE * DT_VFILTER
#define Qbiasbias 1e-7
float vff_z;
float vff_bias;
float vff_zdot;
float vff_zdotdot;
float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
float vff_z_meas;
struct Vff vff;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_vff(void) {
DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice,
&vff_z_meas, &vff_z, &vff_zdot, &vff_bias,
&vff_P[0][0], &vff_P[1][1], &vff_P[2][2]);
&vff.z_meas, &vff.z, &vff.zdot, &vff.bias,
&vff.P[0][0], &vff.P[1][1], &vff.P[2][2]);
}
#endif
void vff_init_zero(void) {
vff_init(0., 0., 0.);
}
void vff_init(float init_z, float init_zdot, float init_bias) {
vff_z = init_z;
vff_zdot = init_zdot;
vff_bias = init_bias;
vff.z = init_z;
vff.zdot = init_zdot;
vff.bias = init_bias;
int i, j;
for (i=0; i<VFF_STATE_SIZE; i++) {
for (j=0; j<VFF_STATE_SIZE; j++)
vff_P[i][j] = 0.;
vff_P[i][i] = VF_FLOAT_INIT_PXX;
vff.P[i][j] = 0.;
vff.P[i][i] = VF_FLOAT_INIT_PXX;
}
#if PERIODIC_TELEMETRY
@@ -121,29 +118,29 @@ void vff_init(float init_z, float init_zdot, float init_bias) {
*/
void vff_propagate(float accel) {
/* update state (Xk1) */
vff_zdotdot = accel + 9.81 - vff_bias;
vff_z = vff_z + DT_VFILTER * vff_zdot;
vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
vff.zdotdot = accel + 9.81 - vff.bias;
vff.z = vff.z + DT_VFILTER * vff.zdot;
vff.zdot = vff.zdot + DT_VFILTER * vff.zdotdot;
/* update covariance (Pk1) */
const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
const float FPF22 = vff_P[2][2];
const float FPF00 = vff.P[0][0] + DT_VFILTER * ( vff.P[1][0] + vff.P[0][1] + DT_VFILTER * vff.P[1][1] );
const float FPF01 = vff.P[0][1] + DT_VFILTER * ( vff.P[1][1] - vff.P[0][2] - DT_VFILTER * vff.P[1][2] );
const float FPF02 = vff.P[0][2] + DT_VFILTER * ( vff.P[1][2] );
const float FPF10 = vff.P[1][0] + DT_VFILTER * (-vff.P[2][0] + vff.P[1][1] - DT_VFILTER * vff.P[2][1] );
const float FPF11 = vff.P[1][1] + DT_VFILTER * (-vff.P[2][1] - vff.P[1][2] + DT_VFILTER * vff.P[2][2] );
const float FPF12 = vff.P[1][2] + DT_VFILTER * (-vff.P[2][2] );
const float FPF20 = vff.P[2][0] + DT_VFILTER * ( vff.P[2][1] );
const float FPF21 = vff.P[2][1] + DT_VFILTER * (-vff.P[2][2] );
const float FPF22 = vff.P[2][2];
vff_P[0][0] = FPF00 + Qzz;
vff_P[0][1] = FPF01;
vff_P[0][2] = FPF02;
vff_P[1][0] = FPF10;
vff_P[1][1] = FPF11 + Qzdotzdot;
vff_P[1][2] = FPF12;
vff_P[2][0] = FPF20;
vff_P[2][1] = FPF21;
vff_P[2][2] = FPF22 + Qbiasbias;
vff.P[0][0] = FPF00 + Qzz;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
vff.P[1][1] = FPF11 + Qzdotzdot;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
vff.P[2][2] = FPF22 + Qbiasbias;
}
/*
@@ -161,37 +158,37 @@ void vff_propagate(float accel) {
Pp = Pm - K*H*Pm;
*/
static inline void update_z_conf(float z_meas, float conf) {
vff_z_meas = z_meas;
vff.z_meas = z_meas;
const float y = z_meas - vff_z;
const float S = vff_P[0][0] + conf;
const float K1 = vff_P[0][0] * 1/S;
const float K2 = vff_P[1][0] * 1/S;
const float K3 = vff_P[2][0] * 1/S;
const float y = z_meas - vff.z;
const float S = vff.P[0][0] + conf;
const float K1 = vff.P[0][0] * 1/S;
const float K2 = vff.P[1][0] * 1/S;
const float K3 = vff.P[2][0] * 1/S;
vff_z = vff_z + K1 * y;
vff_zdot = vff_zdot + K2 * y;
vff_bias = vff_bias + K3 * y;
vff.z = vff.z + K1 * y;
vff.zdot = vff.zdot + K2 * y;
vff.bias = vff.bias + K3 * y;
const float P11 = (1. - K1) * vff_P[0][0];
const float P12 = (1. - K1) * vff_P[0][1];
const float P13 = (1. - K1) * vff_P[0][2];
const float P21 = -K2 * vff_P[0][0] + vff_P[1][0];
const float P22 = -K2 * vff_P[0][1] + vff_P[1][1];
const float P23 = -K2 * vff_P[0][2] + vff_P[1][2];
const float P31 = -K3 * vff_P[0][0] + vff_P[2][0];
const float P32 = -K3 * vff_P[0][1] + vff_P[2][1];
const float P33 = -K3 * vff_P[0][2] + vff_P[2][2];
const float P11 = (1. - K1) * vff.P[0][0];
const float P12 = (1. - K1) * vff.P[0][1];
const float P13 = (1. - K1) * vff.P[0][2];
const float P21 = -K2 * vff.P[0][0] + vff.P[1][0];
const float P22 = -K2 * vff.P[0][1] + vff.P[1][1];
const float P23 = -K2 * vff.P[0][2] + vff.P[1][2];
const float P31 = -K3 * vff.P[0][0] + vff.P[2][0];
const float P32 = -K3 * vff.P[0][1] + vff.P[2][1];
const float P33 = -K3 * vff.P[0][2] + vff.P[2][2];
vff_P[0][0] = P11;
vff_P[0][1] = P12;
vff_P[0][2] = P13;
vff_P[1][0] = P21;
vff_P[1][1] = P22;
vff_P[1][2] = P23;
vff_P[2][0] = P31;
vff_P[2][1] = P32;
vff_P[2][2] = P33;
vff.P[0][0] = P11;
vff.P[0][1] = P12;
vff.P[0][2] = P13;
vff.P[1][0] = P21;
vff.P[1][1] = P22;
vff.P[1][2] = P23;
vff.P[2][0] = P31;
vff.P[2][1] = P32;
vff.P[2][2] = P33;
}
@@ -218,35 +215,35 @@ void vff_update_z_conf(float z_meas, float conf) {
Pp = Pm - K*H*Pm;
*/
static inline void update_vz_conf(float vz, float conf) {
const float yd = vz - vff_zdot;
const float S = vff_P[1][1] + conf;
const float K1 = vff_P[0][1] * 1/S;
const float K2 = vff_P[1][1] * 1/S;
const float K3 = vff_P[2][1] * 1/S;
const float yd = vz - vff.zdot;
const float S = vff.P[1][1] + conf;
const float K1 = vff.P[0][1] * 1/S;
const float K2 = vff.P[1][1] * 1/S;
const float K3 = vff.P[2][1] * 1/S;
vff_z = vff_z + K1 * yd;
vff_zdot = vff_zdot + K2 * yd;
vff_bias = vff_bias + K3 * yd;
vff.z = vff.z + K1 * yd;
vff.zdot = vff.zdot + K2 * yd;
vff.bias = vff.bias + K3 * yd;
const float P11 = -K1 * vff_P[1][0] + vff_P[0][0];
const float P12 = -K1 * vff_P[1][1] + vff_P[0][1];
const float P13 = -K1 * vff_P[1][2] + vff_P[0][2];
const float P21 = (1. - K2) * vff_P[1][0];
const float P22 = (1. - K2) * vff_P[1][1];
const float P23 = (1. - K2) * vff_P[1][2];
const float P31 = -K3 * vff_P[1][0] + vff_P[2][0];
const float P32 = -K3 * vff_P[1][1] + vff_P[2][1];
const float P33 = -K3 * vff_P[1][2] + vff_P[2][2];
const float P11 = -K1 * vff.P[1][0] + vff.P[0][0];
const float P12 = -K1 * vff.P[1][1] + vff.P[0][1];
const float P13 = -K1 * vff.P[1][2] + vff.P[0][2];
const float P21 = (1. - K2) * vff.P[1][0];
const float P22 = (1. - K2) * vff.P[1][1];
const float P23 = (1. - K2) * vff.P[1][2];
const float P31 = -K3 * vff.P[1][0] + vff.P[2][0];
const float P32 = -K3 * vff.P[1][1] + vff.P[2][1];
const float P33 = -K3 * vff.P[1][2] + vff.P[2][2];
vff_P[0][0] = P11;
vff_P[0][1] = P12;
vff_P[0][2] = P13;
vff_P[1][0] = P21;
vff_P[1][1] = P22;
vff_P[1][2] = P23;
vff_P[2][0] = P31;
vff_P[2][1] = P32;
vff_P[2][2] = P33;
vff.P[0][0] = P11;
vff.P[0][1] = P12;
vff.P[0][2] = P13;
vff.P[1][0] = P21;
vff.P[1][1] = P22;
vff.P[1][2] = P23;
vff.P[2][0] = P31;
vff.P[2][1] = P32;
vff.P[2][2] = P33;
}
@@ -255,6 +252,6 @@ void vff_update_vz_conf(float vz_meas, float conf) {
}
void vff_realign(float z_meas) {
vff_z = z_meas;
vff_zdot = 0;
vff.z = z_meas;
vff.zdot = 0;
}
sw/airborne/subsystems/ins/vf_float.h
+10 -6
View File
@@ -31,14 +31,18 @@
#define VFF_STATE_SIZE 3
extern float vff_z;
extern float vff_zdot;
extern float vff_bias;
extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
extern float vff_zdotdot;
struct Vff {
float z; ///< z-position estimate in m (NED, z-down)
float zdot; ///< z-velocity estimate in m/s (NED, z-down)
float bias; ///< accel bias estimate in m/s^2
float zdotdot; ///< z-acceleration in m/s^2 (NED, z-down)
float z_meas; ///< last measurement
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]; ///< covariance matrix
};
extern float vff_z_meas;
extern struct Vff vff;
extern void vff_init_zero(void);
extern void vff_init(float z, float zdot, float bias);
extern void vff_propagate(float accel);
extern void vff_update(float z_meas);
sw/airborne/subsystems/navigation/common_nav.c
+14 -45
View File
@@ -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_local_origin();
/* 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;
}