36#include "generated/airframe.h"
48#if defined SITL && USE_NPS
64#error USE_SONAR needs USE_VFF_EXTENDED
67#ifdef INS_SONAR_THROTTLE_THRESHOLD
71#ifndef INS_SONAR_MIN_RANGE
72#define INS_SONAR_MIN_RANGE 0.001
74#ifndef INS_SONAR_MAX_RANGE
75#define INS_SONAR_MAX_RANGE 4.0
77#define VFF_R_SONAR_0 0.2
78#ifndef VFF_R_SONAR_OF_M
79#define VFF_R_SONAR_OF_M 0.2
86#define INS_VFF_R_GPS 2.0
89#ifndef INS_VFF_VZ_R_GPS
90#define INS_VFF_VZ_R_GPS 2.0
95#ifndef INS_MAX_PROPAGATION_STEPS
96#define INS_MAX_PROPAGATION_STEPS 200
99#ifndef USE_INS_NAV_INIT
100#define USE_INS_NAV_INIT TRUE
105#define INS_BARO_MAX_INIT_VAR 1.f
106#ifndef INS_INT_BARO_ID
107#define INS_INT_BARO_ID AIR_DATA_SENDER_ID
116#ifndef INS_INT_IMU_ID
117#define INS_INT_IMU_ID ABI_BROADCAST
123#ifndef INS_INT_GPS_ID
124#define INS_INT_GPS_ID GPS_MULTI_ID
133#ifndef INS_INT_VEL_ID
134#define INS_INT_VEL_ID ABI_BROADCAST
140 float x,
float y,
float z,
141 float noise_x,
float noise_y,
float noise_z);
142#ifndef INS_INT_POS_ID
143#define INS_INT_POS_ID ABI_BROADCAST
149 float x,
float y,
float z,
150 float noise_x,
float noise_y,
float noise_z);
155#ifndef INS_INT_AGL_ID
156#define INS_INT_AGL_ID ABI_BROADCAST
168#if PERIODIC_TELEMETRY
171static void send_ins(
struct transport_tx *trans,
struct link_device *
dev)
173 pprz_msg_send_INS(trans,
dev, AC_ID,
181 pprz_msg_send_INS_Z(trans,
dev, AC_ID,
188 pprz_msg_send_INS_REF(trans,
dev, AC_ID,
199static void ins_update_from_hff(
void);
208 ins_int.ltp_initialized =
true;
210 ins_int.ltp_initialized =
false;
218 ins_int.baro_initialized =
false;
233#if PERIODIC_TELEMETRY
259 ins_int.ltp_initialized =
true;
262 ins_int.ltp_initialized =
false;
265 ins_int.ltp_initialized =
false;
345 ins_update_from_hff();
347 ins_int.ltp_accel.x = accel_meas_ltp.
x;
348 ins_int.ltp_accel.y = accel_meas_ltp.
y;
367 if (!
ins_int.baro_initialized) {
368#define press_hist_len 10
372 press_hist[
idx] = pressure;
378 ins_int.baro_initialized =
true;
382 if (
ins_int.baro_initialized) {
383 float height_correction = 0.f;
387 double dist2_to_origin = enu->
x * enu->
x + enu->
y * enu->
y;
390 const double earth_radius = 6378137.0;
391 height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
404 ins_int.baro_z = -(baro_up - height_correction);
424 if (!
ins_int.ltp_initialized) {
433#ifdef INS_BODY_TO_GPS_X
436 .x = INS_BODY_TO_GPS_X,
437 .y = INS_BODY_TO_GPS_Y,
438 .z = INS_BODY_TO_GPS_Z
457#if INS_USE_GPS_ALT_SPEED
466 VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
469 VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.
x, gps_speed_cm_s_ned.
y);
470 VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.f);
479 ins_update_from_hff();
503static void agl_cb(
uint8_t __attribute__((unused)) sender_id, __attribute__((unused))
uint32_t stamp,
float distance) {
504 if (distance <= 0 || !(
ins_int.baro_initialized)) {
509 if (distance > INS_SONAR_MAX_RANGE || distance < INS_SONAR_MIN_RANGE){
513#ifdef INS_AGL_THROTTLE_THRESHOLD
514 if(
stabilization.cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
518#ifdef INS_AGL_BARO_THRESHOLD
519 if(
ins_int.baro_z < -INS_SONAR_BARO_THRESHOLD){
525 vff_update_agl(-distance, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
534static void agl_cb(
uint8_t __attribute__((unused)) sender_id, __attribute__((unused))
uint32_t stamp, __attribute__((unused))
float distance) {}
544#if defined SITL && USE_NPS
561static void ins_update_from_hff(
void)
580 if (last_stamp > 0) {
581 float dt = (float)(stamp - last_stamp) * 1e-6;
588 uint32_t stamp __attribute__((unused)),
598 uint32_t stamp __attribute__((unused)),
599 float x,
float y,
float z,
600 float noise_x,
float noise_y,
float noise_z)
616 ins_update_from_hff();
627 static uint32_t last_stamp_x = 0, last_stamp_y = 0;
628 if (noise_x >= 0.f) {
629 if (last_stamp_x > 0)
631 float dt = (float)(stamp - last_stamp_x) * 1e-6;
634 last_stamp_x = stamp;
639 if (last_stamp_y > 0)
641 float dt = (float)(stamp - last_stamp_y) * 1e-6;
644 last_stamp_y = stamp;
660 uint32_t stamp __attribute__((unused)),
661 float x,
float y,
float z,
662 float noise_x,
float noise_y,
float noise_z)
670 ins_update_from_hff();
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)
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct GpsState gps
global GPS state
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define QUAT_INVERT(_qo, _qi)
#define VECT3_SUB(_a, _b)
#define VECT2_SDIV(_vo, _vi, _s)
#define VECT2_ASSIGN(_a, _x, _y)
#define INT32_POS_OF_CM_DEN
#define INT32_SPEED_OF_CM_S_NUM
#define ACCEL_BFP_OF_REAL(_af)
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_ZERO(_v)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define POS_BFP_OF_REAL(_af)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
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 Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Update position.
Horizontal filter (x,y) to estimate position and velocity.
Inertial Measurement Unit interface.
void ins_init_origin_i_from_flightplan(uint16_t id, struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
#define INS_RESET_VERTICAL_REF
#define INS_RESET_VERTICAL_POS
#define INS_RESET_REF
flags for INS reset
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static void send_ins(struct transport_tx *trans, struct link_device *dev)
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
struct InsInt ins_int
global INS state
#define INS_INT_IMU_ID
ABI binding for IMU data.
static void reset_vertical_ref(void)
#define INS_INT_AGL_ID
ABI binding for AGL.
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void ins_update_from_vff(void)
update ins state from vertical filter
static void reset_cb(uint8_t sender_id, uint8_t flag)
static void send_ins(struct transport_tx *trans, struct link_device *dev)
static abi_event pos_est_ev
static void reset_ref(void)
#define INS_BARO_MAX_INIT_VAR
default barometer to use in INS
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
void ins_int_update_gps(struct GpsState *gps_s)
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
agl_cb This callback handles all estimates of the height of the vehicle above the ground under it Thi...
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
#define INS_INT_VEL_ID
ABI binding for VELOCITY_ESTIMATE.
#define INS_MAX_PROPAGATION_STEPS
maximum number of propagation steps without any updates in between
static abi_event vel_est_ev
static void ins_ned_to_state(void)
copy position and speed to state interface
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static void reset_vertical_pos(void)
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
static void pos_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
INS for rotorcrafts combining vertical and horizontal filters.
Ins implementation state (fixed point).
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
void sim_overwrite_ins(void)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event agl_ev
The altitude ABI event.
vector in East North Up coordinates Units: meters
Paparazzi fixed point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
float variance_f(float *array, uint32_t n_elements)
Compute the variance of an array of values (float).
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
void vff_update_z_conf(float z_meas, float conf)
void vff_update_vz_conf(float vz_meas, float conf)
void vff_update_baro(float z_meas)
void vff_realign(float z_meas)
void vff_propagate(float accel, float dt)
Propagate the filter in time.
void vff_update_agl(float z_meas, float conf)
Interface for extended vertical filter (in float).
void vff_update(float z_meas)
Vertical filter (in float) estimating altitude, velocity and accel bias.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.