35#include "generated/airframe.h"
38#ifndef DEBUG_VFF_EXTENDED
39#define DEBUG_VFF_EXTENDED 0
46#include "pprzlink/messages.h"
51#ifndef VFF_EXTENDED_INIT_PXX
52#define VFF_EXTENDED_INIT_PXX 1.0f
56#ifndef VFF_EXTENDED_ACCEL_NOISE
57#define VFF_EXTENDED_ACCEL_NOISE 0.5f
61#ifndef VFF_EXTENDED_R_BARO
62#define VFF_EXTENDED_R_BARO 2.f
77#ifdef VFF_EXTENDED_NON_FLAT_GROUND
87#define R_OBS_HEIGHT 8.f
96static void send_vffe(
struct transport_tx *trans,
struct link_device *
dev)
98 pprz_msg_send_VFF_EXTENDED(trans,
dev, AC_ID,
99 &
vff.z_meas_baro, &
vff.z_meas,
102 &
vff.P[0][0], &
vff.P[1][1], &
vff.P[2][2],
103 &
vff.P[3][3], &
vff.P[4][4]);
112#if DEBUG_VFF_EXTENDED > 1
114static void print_vff(
void)
119 printf(
"%f ",
vff.P[i][j]);
127void vff_init(
float init_z,
float init_zdot,
float init_accel_bias,
float init_offset,
float init_obs_height)
130 vff.zdot = init_zdot;
131 vff.bias = init_accel_bias;
132 vff.offset = init_offset;
133 vff.obs_height = init_obs_height;
142#ifndef VFF_EXTENDED_NON_FLAT_GROUND
151#if PERIODIC_TELEMETRY
184 vff.zdotdot = accel + 9.81f -
vff.bias;
185 vff.z += (
vff.zdot +
vff.zdotdot * dt / 2.f) * dt;
186 vff.zdot +=
vff.zdotdot * dt;
189 const float FPF00 =
vff.P[0][0] + dt * (
vff.P[1][0] +
vff.P[0][1] + dt *
vff.P[1][1]);
190 const float FPF01 =
vff.P[0][1] + dt * (
vff.P[1][1] -
vff.P[0][2] - dt *
vff.P[1][2]);
191 const float FPF02 =
vff.P[0][2] + dt * (
vff.P[1][2]);
192 const float FPF10 =
vff.P[1][0] + dt * (-
vff.P[2][0] +
vff.P[1][1] - dt *
vff.P[2][1]);
193 const float FPF11 =
vff.P[1][1] + dt * (-
vff.P[2][1] -
vff.P[1][2] + dt *
vff.P[2][2]);
194 const float FPF12 =
vff.P[1][2] + dt * (-
vff.P[2][2]);
195 const float FPF20 =
vff.P[2][0] + dt * (
vff.P[2][1]);
196 const float FPF21 =
vff.P[2][1] + dt * (-
vff.P[2][2]);
197 const float FPF22 =
vff.P[2][2];
198 const float FPF33 =
vff.P[3][3];
199 const float FPF44 =
vff.P[4][4];
201 vff.P[0][0] = FPF00 +
vff.accel_noise * dt * dt / 2.f;
205 vff.P[1][1] = FPF11 +
vff.accel_noise * dt;
213#if DEBUG_VFF_EXTENDED
216#if DEBUG_VFF_EXTENDED > 1
217 RunOnceEvery(100, print_vff());
244 const float S =
vff.P[0][0] -
vff.P[0][3] -
vff.P[3][0] +
vff.P[3][3] + conf;
248 K[i] = (
vff.P[i][0] -
vff.P[i][3]) / S;
249 P[i] =
vff.P[0][i] -
vff.P[3][i];
253 vff.zdot +=
K[1] * y;
254 vff.bias +=
K[2] * y;
255 vff.offset +=
K[3] * y;
256 vff.obs_height +=
K[4] * y;
260 vff.P[i][j] -=
K[i] *
P[j];
289 const float S =
vff.P[0][0] -
vff.P[0][4] -
vff.P[4][0] +
vff.P[4][4] + conf;
293 K[i] = (
vff.P[i][0] -
vff.P[i][4]) / S;
294 P[i] =
vff.P[0][i] -
vff.P[4][i];
298 vff.zdot +=
K[1] * y;
299 vff.bias +=
K[2] * y;
300 vff.offset +=
K[3] * y;
301 vff.obs_height +=
K[4] * y;
305 vff.P[i][j] -=
K[i] *
P[j];
312 if (conf < 0.f) {
return; }
323 if (conf < 0.f) {
return; }
329 if (conf < 0.f) {
return; }
358 const float S =
vff.P[4][4] + conf;
362 K[i] =
vff.P[i][4] / S;
367 vff.zdot +=
K[1] * y;
368 vff.bias +=
K[2] * y;
369 vff.offset +=
K[3] * y;
370 vff.obs_height +=
K[4] * y;
374 vff.P[i][j] -=
K[i] *
P[j];
389 vff.obs_height = 0.f;
411 const float yd = vz -
vff.zdot;
412 const float S =
vff.P[1][1] + conf;
416 K[i] =
vff.P[i][1] * 1 / S;
421 vff.zdot +=
K[1] * yd;
422 vff.bias +=
K[2] * yd;
423 vff.offset +=
K[3] * yd;
424 vff.obs_height +=
K[4] * yd;
428 vff.P[i][j] -=
K[i] *
P[j];
435 if (conf < 0.f) {
return; }
Common code for AP and FBW telemetry.
static struct uart_periph * dev
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static void update_vz_conf(float vz, float conf)
#define VFF_EXTENDED_R_BARO
Barometer confidence.
#define VFF_EXTENDED_ACCEL_NOISE
process noise covariance Q
static void update_biased_z_conf(float z_meas, float conf)
Update sensor "with" offset (baro, sonar).
void vff_update_z_conf(float z_meas, float conf)
void vff_update_z(float z_meas)
static void update_obs_height(float obs_height, float conf)
Update obstacle height.
#define Qoffoff
VFF_EXTENDED_NON_FLAT_GROUND removes the assumption of a flat ground and tries to estimate the height...
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_offset, float init_obs_height)
static void send_vffe(struct transport_tx *trans, struct link_device *dev)
void vff_update_vz_conf(float vz_meas, float conf)
void vff_update_obs_height(float obs_height)
static void update_alt_conf(float z_meas, float conf)
Update sensor "without" offset (gps, sonar) H = [1 0 0 0 -1]; // state residual y = rangemeter - H * ...
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_baro_conf(float z_meas, float conf)
#define DEBUG_VFF_EXTENDED
void vff_update_agl(float z_meas, float conf)
#define VFF_EXTENDED_INIT_PXX
initial covariance diagonal
Interface for extended vertical filter (in float).
float obs_height
estimate of height of obstacles under the vehicle
float z_meas
last z measurement in m