dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Complete position estimator implemented (GPS + Baro + Accel)

This commit is contained in:
Anton Babushkin 2013-06-10 23:16:04 +04:00
parent afb34950a3
commit 4256e43de7
8 changed files with 207 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/position_estimator_inav/inertial_filter.c Normal file
View File

@ -0,0 +1,28 @@
/*
* inertial_filter.c
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include "inertial_filter.h"
void inertial_filter_predict(float dt, float x[3])
{
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
}
void inertial_filter_correct(float dt, float x[3], int i, float z, float w)
{
float e = z - x[i];
x[i] += e * w * dt;
if (i == 0) {
x[1] += e * w * w * dt;
//x[2] += e * w * w * w * dt / 3.0; // ~ 0.0
} else if (i == 1) {
x[2] += e * w * w * dt;
}
}

13
src/modules/position_estimator_inav/inertial_filter.h Normal file
View File

@ -0,0 +1,13 @@
/*
* inertial_filter.h
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include <stdbool.h>
#include <drivers/drv_hrt.h>
void inertial_filter_predict(float dt, float x[3]);
void inertial_filter_correct(float dt, float x[3], int i, float z, float w);

42
src/modules/position_estimator_inav/kalman_filter_inertial.c
View File

@ -1,42 +0,0 @@
/*
* kalman_filter_inertial.c
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include "kalman_filter_inertial.h"
void kalman_filter_inertial_predict(float dt, float x[3]) {
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
}
void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) {
float y[2];
// y = z - H x
y[0] = z[0] - x[0];
y[1] = z[1] - x[2];
// x = x + K * y
for (int i = 0; i < 3; i++) { // Row
for (int j = 0; j < 2; j++) { // Column
if (use[j])
x[i] += k[i][j] * y[j];
}
}
}
void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) {
float y[2];
// y = z - H x
y[0] = z[0] - x[0];
y[1] = z[1] - x[1];
y[2] = z[2] - x[2];
// x = x + K * y
for (int i = 0; i < 3; i++) { // Row
for (int j = 0; j < 3; j++) { // Column
if (use[j])
x[i] += k[i][j] * y[j];
}
}
}

14
src/modules/position_estimator_inav/kalman_filter_inertial.h
View File

@ -1,14 +0,0 @@
/*
* kalman_filter_inertial.h
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include <stdbool.h>
void kalman_filter_inertial_predict(float dt, float x[3]);
void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]);
void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]);

2
src/modules/position_estimator_inav/module.mk
View File

@ -38,4 +38,4 @@
MODULE_COMMAND = position_estimator_inav
SRCS = position_estimator_inav_main.c \
position_estimator_inav_params.c \
kalman_filter_inertial.c
inertial_filter.c

242
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -66,7 +66,7 @@
#include <drivers/drv_hrt.h>
#include "position_estimator_inav_params.h"
#include "kalman_filter_inertial.h"
#include "inertial_filter.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@ -82,13 +82,15 @@ static void usage(const char *reason);
/**
* Print the correct usage.
*/
static void usage(const char *reason) {
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr,
"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
}
fprintf(stderr,
"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
}
/**
* The position_estimator_inav_thread only briefly exists to start
@ -98,7 +100,8 @@ static void usage(const char *reason) {
* The actual stack size should be set in the call
* to task_create().
*/
int position_estimator_inav_main(int argc, char *argv[]) {
int position_estimator_inav_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
@ -108,17 +111,19 @@ int position_estimator_inav_main(int argc, char *argv[]) {
/* this is not an error */
exit(0);
}
if (argc > 1)
if (!strcmp(argv[2], "-v"))
verbose_mode = true;
thread_should_exit = false;
position_estimator_inav_task = task_spawn("position_estimator_inav",
SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL );
SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
@ -127,9 +132,11 @@ int position_estimator_inav_main(int argc, char *argv[]) {
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tposition_estimator_inav is running\n");
} else {
printf("\tposition_estimator_inav not started\n");
}
exit(0);
}
@ -140,10 +147,10 @@ int position_estimator_inav_main(int argc, char *argv[]) {
/****************************************************************************
* main
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* welcome user */
printf("[position_estimator_inav] started\n");
static int mavlink_fd;
int position_estimator_inav_thread_main(int argc, char *argv[])
{
warnx("started.");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[position_estimator_inav] started");
@ -156,9 +163,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
int baro_loop_end = 70; /* measurement for 1 second */
float baro_alt0 = 0.0f; /* to determine while start up */
static double lat_current = 0.0; //[°]] --> 47.0
static double lon_current = 0.0; //[°]] -->8.5
static double alt_current = 0.0; //[m] above MSL
double lat_current = 0.0; //[°]] --> 47.0
double lon_current = 0.0; //[°]] -->8.5
double alt_current = 0.0; //[m] above MSL
/* declare and safely initialize all structs */
struct vehicle_status_s vehicle_status;
@ -188,40 +195,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* initialize parameter handles */
parameters_init(&pos_inav_param_handles);
bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */
/* FIRST PARAMETER READ at START UP*/
bool local_flag_baroINITdone = false;
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param to clear updated flag */
/* FIRST PARAMETER UPDATE */
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
/* first parameters update */
parameters_update(&pos_inav_param_handles, &params);
/* END FIRST PARAMETER UPDATE */
/* wait until gps signal turns valid, only then can we initialize the projection */
/* wait for GPS fix, only then can we initialize the projection */
if (params.use_gps) {
struct pollfd fds_init[1] = {
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
};
while (gps.fix_type < 3) {
if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */
if (poll(fds_init, 1, 5000)) {
if (fds_init[0].revents & POLLIN) {
/* Wait for the GPS update to propagate (we have some time) */
usleep(5000);
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
}
}
static int printcounter = 0;
if (printcounter == 100) {
printcounter = 0;
printf("[position_estimator_inav] wait for GPS fix type 3\n");
warnx("waiting for GPS fix type 3...");
}
printcounter++;
}
/* get GPS position for first initialization */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
lat_current = ((double) (gps.lat)) * 1e-7;
lon_current = ((double) (gps.lon)) * 1e-7;
lat_current = ((double)(gps.lat)) * 1e-7;
lon_current = ((double)(gps.lon)) * 1e-7;
alt_current = gps.alt * 1e-3;
pos.home_lat = lat_current * 1e7;
@ -232,15 +241,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
map_projection_init(lat_current, lon_current);
/* publish global position messages only after first GPS message */
}
printf(
"[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n",
lat_current, lon_current);
hrt_abstime last_time = 0;
warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current);
hrt_abstime t_prev = 0;
thread_running = true;
uint32_t accelerometer_counter = 0;
uint32_t accel_counter = 0;
hrt_abstime accel_t = 0;
float accel_dt = 0.0f;
uint32_t baro_counter = 0;
uint16_t accelerometer_updates = 0;
hrt_abstime baro_t = 0;
hrt_abstime gps_t = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
@ -251,173 +263,184 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* main loop */
struct pollfd fds[5] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = vehicle_status_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = vehicle_status_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
};
printf("[position_estimator_inav] main loop started\n");
while (!thread_should_exit) {
bool accelerometer_updated = false;
bool baro_updated = false;
bool gps_updated = false;
float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f };
float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f };
int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
/* poll error */
printf("[position_estimator_inav] subscriptions poll error\n");
warnx("subscriptions poll error.");
thread_should_exit = true;
continue;
} else if (ret > 0) {
/* parameter update */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
&update);
&update);
/* update parameters */
parameters_update(&pos_inav_param_handles, &params);
}
/* vehicle status */
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
&vehicle_status);
&vehicle_status);
}
/* vehicle attitude */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
}
/* sensor combined */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accelerometer_counter) {
if (sensor.accelerometer_counter > accel_counter) {
accelerometer_updated = true;
accelerometer_counter = sensor.accelerometer_counter;
accelerometer_updates++;
accel_counter = sensor.accelerometer_counter;
accel_updates++;
accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f;
accel_t = t;
}
if (sensor.baro_counter > baro_counter) {
baro_updated = true;
baro_counter = sensor.baro_counter;
baro_updates++;
}
// barometric pressure estimation at start up
/* barometric pressure estimation at start up */
if (!local_flag_baroINITdone && baro_updated) {
// mean calculation over several measurements
/* mean calculation over several measurements */
if (baro_loop_cnt < baro_loop_end) {
baro_alt0 += sensor.baro_alt_meter;
baro_loop_cnt++;
} else {
baro_alt0 /= (float) (baro_loop_cnt);
baro_alt0 /= (float)(baro_loop_cnt);
local_flag_baroINITdone = true;
char str[80];
sprintf(str,
"[position_estimator_inav] baro_alt0 = %.2f",
baro_alt0);
printf("%s\n", str);
mavlink_log_info(mavlink_fd, str);
mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0);
}
}
}
if (params.use_gps) {
/* vehicle GPS position */
if (fds[4].revents & POLLIN) {
/* new GPS value */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
/* Project gps lat lon (Geographic coordinate system) to plane */
map_projection_project(((double) (gps.lat)) * 1e-7,
((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
&(local_pos_gps[1]));
local_pos_gps[2] = (float) (gps.alt * 1e-3);
/* project GPS lat lon (Geographic coordinate system) to plane */
map_projection_project(((double)(gps.lat)) * 1e-7,
((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]),
&(proj_pos_gps[1]));
proj_pos_gps[2] = (float)(gps.alt * 1e-3);
gps_updated = true;
pos.valid = gps.fix_type >= 3;
gps_updates++;
}
} else {
pos.valid = true;
}
} /* end of poll return value check */
}
/* end of poll return value check */
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f;
t_prev = t;
hrt_abstime t = hrt_absolute_time();
float dt = (t - last_time) / 1000000.0;
last_time = t;
if (att.R_valid) {
/* transform acceleration vector from UAV frame to NED frame */
float accel_NED[3];
for (int i = 0; i < 3; i++) {
accel_NED[i] = 0.0f;
for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
}
}
accel_NED[2] += CONSTANTS_ONE_G;
/* kalman filter for altitude */
kalman_filter_inertial_predict(dt, z_est);
/* prepare vectors for kalman filter correction */
float z_meas[2]; // position, acceleration
bool use_z[2] = { false, false };
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est);
/* inertial filter correction for altitude */
if (local_flag_baroINITdone && baro_updated) {
z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt
use_z[0] = true;
if (baro_t > 0) {
inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro);
}
baro_t = t;
}
if (accelerometer_updated) {
z_meas[1] = accel_NED[2];
use_z[1] = true;
}
if (use_z[0] || use_z[1]) {
/* correction */
kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z);
inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc);
}
if (params.use_gps) {
/* kalman filter for position */
kalman_filter_inertial_predict(dt, x_est);
kalman_filter_inertial_predict(dt, y_est);
/* prepare vectors for kalman filter correction */
float x_meas[3]; // position, velocity, acceleration
float y_meas[3]; // position, velocity, acceleration
bool use_xy[3] = { false, false, false };
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est);
/* inertial filter correction for position */
if (gps_updated) {
x_meas[0] = local_pos_gps[0];
y_meas[0] = local_pos_gps[1];
x_meas[1] = gps.vel_n_m_s;
y_meas[1] = gps.vel_e_m_s;
use_xy[0] = true;
use_xy[1] = true;
if (gps_t > 0) {
float gps_dt = (t - gps_t) / 1000000.0f;
inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p);
inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v);
inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p);
inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v);
}
gps_t = t;
}
if (accelerometer_updated) {
x_meas[2] = accel_NED[0];
y_meas[2] = accel_NED[1];
use_xy[2] = true;
}
if (use_xy[0] || use_xy[2]) {
/* correction */
kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy);
kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy);
inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc);
inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc);
}
}
}
if (verbose_mode) {
/* print updates rate */
if (t - updates_counter_start > updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f;
printf(
"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
accelerometer_updates / updates_dt,
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt);
"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
accel_updates / updates_dt,
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt);
updates_counter_start = t;
accelerometer_updates = 0;
accel_updates = 0;
baro_updates = 0;
gps_updates = 0;
attitude_updates = 0;
}
}
if (t - pub_last > pub_interval) {
pub_last = t;
pos.x = x_est[0];
@ -427,17 +450,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
pos.z = z_est[0];
pos.vz = z_est[1];
pos.timestamp = hrt_absolute_time();
if ((isfinite(pos.x)) && (isfinite(pos.vx))
&& (isfinite(pos.y))
&& (isfinite(pos.vy))
&& (isfinite(pos.z))
&& (isfinite(pos.vz))) {
&& (isfinite(pos.y))
&& (isfinite(pos.vy))
&& (isfinite(pos.z))
&& (isfinite(pos.vz))) {
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos);
}
}
}
printf("[position_estimator_inav] exiting.\n");
warnx("exiting.");
mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting");
thread_running = false;
return 0;

76
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -34,73 +34,39 @@
/*
* @file position_estimator_inav_params.c
*
*
* Parameters for position_estimator_inav
*/
#include "position_estimator_inav_params.h"
PARAM_DEFINE_INT32(INAV_USE_GPS, 0);
PARAM_DEFINE_INT32(INAV_USE_GPS, 1);
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f);
PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f);
int parameters_init(struct position_estimator_inav_param_handles *h) {
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->use_gps = param_find("INAV_USE_GPS");
h->k_alt_00 = param_find("INAV_K_ALT_00");
h->k_alt_01 = param_find("INAV_K_ALT_01");
h->k_alt_10 = param_find("INAV_K_ALT_10");
h->k_alt_11 = param_find("INAV_K_ALT_11");
h->k_alt_20 = param_find("INAV_K_ALT_20");
h->k_alt_21 = param_find("INAV_K_ALT_21");
h->k_pos_00 = param_find("INAV_K_POS_00");
h->k_pos_01 = param_find("INAV_K_POS_01");
h->k_pos_02 = param_find("INAV_K_POS_02");
h->k_pos_10 = param_find("INAV_K_POS_10");
h->k_pos_11 = param_find("INAV_K_POS_11");
h->k_pos_12 = param_find("INAV_K_POS_12");
h->k_pos_20 = param_find("INAV_K_POS_20");
h->k_pos_21 = param_find("INAV_K_POS_21");
h->k_pos_22 = param_find("INAV_K_POS_22");
h->w_alt_baro = param_find("INAV_W_ALT_BARO");
h->w_alt_acc = param_find("INAV_W_ALT_ACC");
h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
h->w_pos_acc = param_find("INAV_W_POS_ACC");
return OK;
}
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) {
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
{
param_get(h->use_gps, &(p->use_gps));
param_get(h->k_alt_00, &(p->k_alt[0][0]));
param_get(h->k_alt_01, &(p->k_alt[0][1]));
param_get(h->k_alt_10, &(p->k_alt[1][0]));
param_get(h->k_alt_11, &(p->k_alt[1][1]));
param_get(h->k_alt_20, &(p->k_alt[2][0]));
param_get(h->k_alt_21, &(p->k_alt[2][1]));
param_get(h->k_pos_00, &(p->k_pos[0][0]));
param_get(h->k_pos_01, &(p->k_pos[0][1]));
param_get(h->k_pos_02, &(p->k_pos[0][2]));
param_get(h->k_pos_10, &(p->k_pos[1][0]));
param_get(h->k_pos_11, &(p->k_pos[1][1]));
param_get(h->k_pos_12, &(p->k_pos[1][2]));
param_get(h->k_pos_20, &(p->k_pos[2][0]));
param_get(h->k_pos_21, &(p->k_pos[2][1]));
param_get(h->k_pos_22, &(p->k_pos[2][2]));
param_get(h->w_alt_baro, &(p->w_alt_baro));
param_get(h->w_alt_acc, &(p->w_alt_acc));
param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
param_get(h->w_pos_acc, &(p->w_pos_acc));
return OK;
}

31
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -34,7 +34,7 @@
/*
* @file position_estimator_inav_params.h
*
*
* Parameters for Position Estimator
*/
@ -42,29 +42,20 @@
struct position_estimator_inav_params {
int use_gps;
float k_alt[3][2];
float k_pos[3][3];
float w_alt_baro;
float w_alt_acc;
float w_pos_gps_p;
float w_pos_gps_v;
float w_pos_acc;
};
struct position_estimator_inav_param_handles {
param_t use_gps;
param_t k_alt_00;
param_t k_alt_01;
param_t k_alt_10;
param_t k_alt_11;
param_t k_alt_20;
param_t k_alt_21;
param_t k_pos_00;
param_t k_pos_01;
param_t k_pos_02;
param_t k_pos_10;
param_t k_pos_11;
param_t k_pos_12;
param_t k_pos_20;
param_t k_pos_21;
param_t k_pos_22;
param_t w_alt_baro;
param_t w_alt_acc;
param_t w_pos_gps_p;
param_t w_pos_gps_v;
param_t w_pos_acc;
};
/**