40#include "generated/airframe.h"
41#include "generated/modules.h"
42#include "generated/flight_plan.h"
44#ifdef DEBUG_ALT_KALMAN
49#ifndef USE_INS_NAV_INIT
50#define USE_INS_NAV_INIT TRUE
59PRINT_CONFIG_MSG(
"USE_BAROMETER is TRUE: Using baro for altitude estimation.")
62#ifndef INS_ALT_BARO_ID
63#define INS_ALT_BARO_ID AIR_DATA_SENDER_ID
75#define INS_ALT_GPS_ID GPS_MULTI_ID
82#define INS_ALT_IMU_ID ABI_BROADCAST
105 ins_altf.origin_initialized =
false;
144 utm.
alt =
gps.hmsl / 1000.0f;
174 float dt = (float)(now_ts -
last_ts) / 1e6;
178 Bound(dt, 0.002, 1.0)
228 const float dt = GPS_DT;
235 float dt = (float)(now_ts -
last_ts) / 1e6;
292#elif USE_BARO_MS5534A
321 q[0][0] = dt * dt * dt * dt / 4.;
322 q[0][1] = dt * dt * dt / 2.;
323 q[1][0] = dt * dt * dt / 2.;
329 p[0][0] =
p[0][0] +
p[1][0] * dt + dt * (
p[0][1] +
p[1][1] * dt) + SIGMA2 * q[0][0];
330 p[0][1] =
p[0][1] +
p[1][1] * dt + SIGMA2 * q[0][1];
331 p[1][0] =
p[1][0] +
p[1][1] * dt + SIGMA2 * q[1][0];
332 p[1][1] =
p[1][1] + SIGMA2 * q[1][1];
335 float e =
p[0][0] + R;
337 if (fabs(e) > 1e-5) {
338 float k_0 =
p[0][0] / e;
339 float k_1 =
p[1][0] / e;
346 p[1][0] = -
p[0][0] * k_1 +
p[1][0];
347 p[1][1] = -
p[0][1] * k_1 +
p[1][1];
348 p[0][0] =
p[0][0] * (1 - k_0);
349 p[0][1] =
p[0][1] * (1 - k_0);
352#ifdef DEBUG_ALT_KALMAN
365 uint32_t stamp __attribute__((unused)),
372 uint32_t stamp __attribute__((unused)),
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ABI_BROADCAST
Broadcast address.
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
float baro_MS5534A_sigma2
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Common code for AP and FBW telemetry.
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
struct GpsState gps
global GPS state
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
#define VECT3_COPY(_a, _b)
#define ACCEL_BFP_OF_REAL(_af)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define UTM_COPY(_u1, _u2)
vector in North East Down coordinates
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
static void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int).
static void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
#define INS_RESET_VERTICAL_REF
#define INS_RESET_REF
flags for INS reset
static void reset_vertical_ref(void)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void reset_cb(uint8_t sender_id, uint8_t flag)
static void reset_ref(void)
#define INS_ALT_GPS_ID
ABI binding for gps data.
static void alt_kalman_init(void)
static abi_event reset_ev
static void alt_kalman(float z_meas, float dt)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
struct InsAltFloat ins_altf
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
void ins_alt_float_update_gps(struct GpsState *gps_s)
void ins_alt_float_init(void)
static void alt_kalman_reset(void)
void ins_alt_float_update_baro(float pressure)
Filters altitude and climb rate for fixedwings.
Ins implementation state (altitude, float).
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Fixedwing Navigation library.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
API to get/set the generic vehicle states.
Architecture independent timing functions.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.