Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[ins] remove hf|vf_realign from global ins struct

Browse Source 
and fix some missing fuctions
This commit is contained in:
Felix Ruess
2014-02-27 13:32:43 +01:00
parent 1b4955e258
commit 4fb484c3d3
13 changed files with 181 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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/modules/ins/ins_xsens.c
+19
View File
@@ -256,6 +256,25 @@ void ins_periodic(void) {
xsens_periodic();
}
void ins_reset_local_origin(void) {
}
void ins_reset_altitude_ref(void) {
}
void ins_reset_utm_zone(struct UtmCoor_f * utm) {
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
}
void ins_update_gps(void) {
struct UtmCoor_f utm;
utm.east = gps.utm_pos.east / 100.;
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.h
+15 -15
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; ///< flag to request to realign horizontal filter to current position (local origin unchanged)
bool_t vf_realign; ///< flag to request to realign vertical filter to ground level (local origin unchanged)
enum InsStatus status; ///< status of the INS
};
/** global INS state */
@@ -53,24 +53,24 @@ extern struct Ins ins;
/** INS initialization. Called at startup.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_init( void );
extern void ins_init(void);
/** INS periodic call.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_periodic( void );
extern void ins_periodic(void);
/** INS local origin reset.
* Reset horizontal and vertical reference to the current position.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_reset_local_origin( void );
extern void ins_reset_local_origin(void);
/** INS altitude reference reset.
* Reset only vertical reference to the current altitude.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_reset_altitude_ref( void );
extern void ins_reset_altitude_ref(void);
/** INS utm zone reset.
* Reset UTM zone according te the actual position.
@@ -82,16 +82,16 @@ extern void ins_reset_utm_zone(struct UtmCoor_f * utm);
/** INS horizontal realign.
* This only reset the filter to a given value, but doesn't change the local reference.
* Needs to be implemented by each INS algorithm.
* @param pos new horizontal position to set
* @param speed new horizontal speed to set
* Needs to be implemented by each INS algorithm.
*/
extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
/** INS vertical realign.
* This only reset the filter to a given value, but doesn't change the local reference.
* @param z new altitude to set
* Needs to be implemented by each INS algorithm.
* @param z new altitude to set
*/
extern void ins_realign_v(float z);
@@ -99,25 +99,25 @@ extern void ins_realign_v(float z);
* Reads the global #imu data struct.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_propagate( void );
extern void ins_propagate(void);
/** Update INS state with barometer measurements.
* Reads the global #baro data struct.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_update_baro( void );
extern void ins_update_baro(void);
/** Update INS state with GPS measurements.
* Reads the global #gps data struct.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_update_gps( void );
extern void ins_update_gps(void);
/** Update INS state with sonar measurements.
* Reads the global #sonar data struct.
* Needs to be implemented by each INS algorithm.
*/
extern void ins_update_sonar( void );
extern void ins_update_sonar(void);
#endif /* INS_H */
sw/airborne/subsystems/ins/ins_alt_float.c
+43 -46
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,25 +78,24 @@ void ins_init() {
alt_kalman_init();
#if USE_BAROMETER
ins_qfe = 0;;
ins_baro_initialized = FALSE;
ins_baro_alt = 0.;
ins_impl.qfe = 0;;
ins_impl.baro_initialized = FALSE;
ins_impl.baro_alt = 0.;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
#endif
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
ins_impl.reset_alt_ref = FALSE;
alt_kalman(0.);
ins.status = INS_RUNNING;
}
void ins_periodic( void ) {
void ins_periodic(void) {
}
/** Reset the geographic reference to the current GPS fix */
void ins_reset_local_origin( void ) {
void ins_reset_local_origin(void) {
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
@@ -119,17 +116,17 @@ void ins_reset_local_origin( void ) {
stateSetLocalUtmOrigin_f(&utm);
// reset filter flag
ins.vf_realign = TRUE;
ins_impl.reset_alt_ref = TRUE;
}
void ins_reset_altitude_ref( void ) {
void ins_reset_altitude_ref(void) {
struct UtmCoor_f utm = state.utm_origin_f;
// ground_alt
utm.alt = gps.hmsl/1000.;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
// reset filter flag
ins.vf_realign = TRUE;
ins_impl.reset_alt_ref = TRUE;
}
void ins_reset_utm_zone(struct UtmCoor_f * utm) {
@@ -148,42 +145,42 @@ void ins_reset_utm_zone(struct UtmCoor_f * utm) {
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
}
void ins_realign_v(float z __attribute__ ((unused))) {
ins_alt = z;
ins_alt_dot = 0.;
void ins_realign_v(float z) {
ins_impl.alt = z;
ins_impl.alt_dot = 0.;
alt_kalman_reset();
}
void ins_propagate() {
void ins_propagate(void) {
}
void ins_update_baro() {}
void ins_update_baro(void) {}
#if USE_BAROMETER
static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
if (!ins_baro_initialized) {
ins_qfe = *pressure;
ins_baro_initialized = TRUE;
if (!ins_impl.baro_initialized) {
ins_impl.qfe = *pressure;
ins_impl.baro_initialized = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_alt = ground_alt;
ins_alt_dot = 0.;
ins_qfe = *pressure;
if (ins_impl.reset_alt_ref) {
ins_impl.reset_alt_ref = FALSE;
ins_impl.alt = ground_alt;
ins_impl.alt_dot = 0.;
ins_impl.qfe = *pressure;
alt_kalman_reset();
}
else { /* not realigning, so normal update with baro measurement */
ins_baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_qfe);
ins_impl.baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_impl.qfe);
/* run the filter */
alt_kalman(ins_baro_alt);
alt_kalman(ins_impl.baro_alt);
/* set new altitude, just copy old horizontal position */
struct UtmCoor_f utm;
UTM_COPY(utm, *stateGetPositionUtm_f());
utm.alt = ins_alt;
utm.alt = ins_impl.alt;
stateSetPositionUtm_f(&utm);
struct NedCoor_f ned_vel;
memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
ned_vel.z = -ins_alt_dot;
ned_vel.z = -ins_impl.alt_dot;
stateSetSpeedNed_f(&ned_vel);
}
}
@@ -200,16 +197,16 @@ void ins_update_gps(void) {
#if !USE_BAROMETER
float falt = gps.hmsl / 1000.;
alt_kalman(falt);
ins_alt_dot = -gps.ned_vel.z / 100.;
ins_impl.alt_dot = -gps.ned_vel.z / 100.;
#endif
utm.alt = ins_alt;
utm.alt = ins_impl.alt;
// set position
stateSetPositionUtm_f(&utm);
struct NedCoor_f ned_vel = {
gps.ned_vel.x / 100.,
gps.ned_vel.y / 100.,
-ins_alt_dot
-ins_impl.alt_dot
};
// set velocity
stateSetSpeedNed_f(&ned_vel);
@@ -217,7 +214,7 @@ void ins_update_gps(void) {
#endif
}
void ins_update_sonar() {
void ins_update_sonar(void) {
}
@@ -230,18 +227,18 @@ void ins_update_sonar() {
static float p[2][2];
void alt_kalman_reset( void ) {
static void alt_kalman_reset(void) {
p[0][0] = 1.;
p[0][1] = 0.;
p[1][0] = 0.;
p[1][1] = 1.;
}
void alt_kalman_init( void ) {
static void alt_kalman_init(void) {
alt_kalman_reset();
}
void alt_kalman(float z_meas) {
static void alt_kalman(float z_meas) {
float DT = GPS_DT;
float R = GPS_R;
float SIGMA2 = GPS_SIGMA2;
@@ -292,7 +289,7 @@ void alt_kalman(float z_meas) {
/* predict */
ins_alt += ins_alt_dot * DT;
ins_impl.alt += ins_impl.alt_dot * DT;
p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
@@ -304,11 +301,11 @@ void alt_kalman(float z_meas) {
if (fabs(e) > 1e-5) {
float k_0 = p[0][0] / e;
float k_1 = p[1][0] / e;
e = z_meas - ins_alt;
e = z_meas - ins_impl.alt;
/* correction */
ins_alt += k_0 * e;
ins_alt_dot += k_1 * e;
ins_impl.alt += k_0 * e;
ins_impl.alt_dot += k_1 * e;
p[1][0] = -p[0][0]*k_1+p[1][0];
p[1][1] = -p[0][1]*k_1+p[1][1];
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
-3
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);
sw/airborne/subsystems/ins/ins_float_invariant.c
+8 -13
View File
@@ -181,7 +181,7 @@ static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
/* barometer */
bool_t ins_baro_initialised;
bool_t ins_baro_initialized;
// Baro event on ABI
#ifndef INS_BARO_ID
#define INS_BARO_ID BARO_BOARD_SENDER_ID
@@ -211,7 +211,7 @@ static inline void init_invariant_state(void) {
ins_impl.meas.baro_alt = 0.;
// init baro
ins_baro_initialised = FALSE;
ins_baro_initialized = FALSE;
}
void ins_init() {
@@ -261,8 +261,7 @@ void ins_init() {
ins_impl.gains.sh = INS_INV_SH;
ins.status = INS_UNINIT;
ins.hf_realign = FALSE;
ins.vf_realign = FALSE;
ins_impl.reset = FALSE;
#if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
@@ -298,8 +297,6 @@ void ins_reset_local_origin( void ) {
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ltp_def);
#endif
ins.hf_realign = FALSE;
ins.vf_realign = FALSE;
}
void ins_reset_altitude_ref( void ) {
@@ -313,7 +310,6 @@ void ins_reset_altitude_ref( void ) {
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ltp_def);
#endif
ins.vf_realign = FALSE;
}
#if INS_UPDATE_FW_ESTIMATOR
@@ -361,12 +357,11 @@ void ahrs_propagate(void) {
// realign all the filter if needed
// a complete init cycle is required
if (ins.hf_realign || ins.vf_realign) {
if (ins_impl.reset) {
ins_impl.reset = FALSE;
ins.status = INS_UNINIT;
ahrs.status = AHRS_UNINIT;
init_invariant_state();
ins.hf_realign = FALSE;
ins.vf_realign = FALSE;
}
// fill command vector
@@ -513,7 +508,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
static float baro_moy = 0.;
static float baro_prev = 0.;
if (!ins_baro_initialised) {
if (!ins_baro_initialized) {
// try to find a stable qfe
// TODO generic function in pprz_isa ?
if (i == 1) {
@@ -526,11 +521,11 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
// test stop condition
if (fabs(alpha) < 0.005) {
ins_qfe = baro_moy;
ins_baro_initialised = TRUE;
ins_baro_initialized = TRUE;
}
if (i == 250) {
ins_qfe = *pressure;
ins_baro_initialised = TRUE;
ins_baro_initialized = TRUE;
}
i++;
}
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
+20 -10
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,10 +42,23 @@ void ins_init() {
ins.status = INS_RUNNING;
}
void ins_periodic( void ) {
void ins_periodic(void) {
}
void ins_reset_local_origin( void ) {
void ins_reset_utm_zone(struct UtmCoor_f * utm) {
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
}
void ins_reset_local_origin(void) {
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
@@ -65,15 +78,12 @@ void ins_reset_local_origin( void ) {
stateSetLocalUtmOrigin_f(&utm);
}
void ins_reset_altitude_ref( void ) {
void ins_reset_altitude_ref(void) {
struct UtmCoor_f utm = state.utm_origin_f;
utm.alt = gps.hmsl/1000.;
stateSetLocalUtmOrigin_f(&utm);
}
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
void ins_realign_v(float z __attribute__ ((unused))) {}
void ins_update_gps(void) {
struct UtmCoor_f utm;
utm.east = gps.utm_pos.east / 100.;
@@ -94,13 +104,13 @@ void ins_update_gps(void) {
}
void ins_propagate() {
void ins_propagate(void) {
}
void ins_update_baro() {
void ins_update_baro(void) {
}
void ins_update_sonar() {
void ins_update_sonar(void) {
}
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
sw/airborne/subsystems/ins/ins_int.c
+31 -16
View File
@@ -142,8 +142,8 @@ void ins_init(void) {
ins_impl.sonar_offset = INS_SONAR_OFFSET;
#endif
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
ins_impl.vf_reset = FALSE;
ins_impl.hf_realign = FALSE;
/* init vertical and horizontal filters */
#if USE_VFF_EXTENDED
@@ -171,21 +171,36 @@ void ins_periodic(void) {
ins.status = INS_RUNNING;
}
void ins_reset_local_origin( void ) {
ins_impl.ltp_initialized = FALSE;
#if USE_HFF
ins.hf_realign = TRUE;
void ins_reset_utm_zone(struct UtmCoor_f * utm) {
#if USE_GPS
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
#endif
ins.vf_realign = TRUE;
}
void ins_reset_altitude_ref( void ) {
void ins_reset_local_origin(void) {
ins_impl.ltp_initialized = FALSE;
#if USE_HFF
ins_impl.hf_realign = TRUE;
#endif
ins_impl.vf_reset = TRUE;
}
void ins_reset_altitude_ref(void) {
#if USE_GPS
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
#endif
ins.vf_realign = TRUE;
ins_impl.vf_reset = TRUE;
}
#if USE_HFF
@@ -204,7 +219,7 @@ void ins_realign_v(float z) {
ins_update_from_vff();
}
void ins_propagate() {
void ins_propagate(void) {
/* untilt accels */
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
@@ -240,8 +255,8 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
ins_impl.qfe = *pressure;
ins_impl.baro_initialized = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
if (ins_impl.vf_reset) {
ins_impl.vf_reset = FALSE;
ins_impl.qfe = *pressure;
vff_realign(0.);
ins_update_from_vff();
@@ -258,7 +273,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres
}
void ins_update_baro() {
void ins_update_baro(void) {
}
@@ -289,8 +304,8 @@ void ins_update_gps(void) {
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
if (ins.hf_realign) {
ins.hf_realign = FALSE;
if (ins_impl.hf_realign) {
ins_impl.hf_realign = FALSE;
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(gps_pos_m_ned, zero);
}
@@ -328,7 +343,7 @@ uint8_t var_idx = 0;
#endif
void ins_update_sonar() {
void ins_update_sonar(void) {
#if USE_SONAR
static float last_offset = 0.;
// new value filtered with median_filter
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;