From 4256e43de7ea4c9cad98e8bfc9a811310bfb3d77 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Jun 2013 23:16:04 +0400 Subject: [PATCH] Complete position estimator implemented (GPS + Baro + Accel) --- .../position_estimator_inav/inertial_filter.c | 28 ++ .../position_estimator_inav/inertial_filter.h | 13 + .../kalman_filter_inertial.c | 42 --- .../kalman_filter_inertial.h | 14 - src/modules/position_estimator_inav/module.mk | 2 +- .../position_estimator_inav_main.c | 242 ++++++++++-------- .../position_estimator_inav_params.c | 76 ++---- .../position_estimator_inav_params.h | 31 +-- 8 files changed, 207 insertions(+), 241 deletions(-) create mode 100644 src/modules/position_estimator_inav/inertial_filter.c create mode 100644 src/modules/position_estimator_inav/inertial_filter.h delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.c delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.h diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 0000000000..b70d3504e6 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,28 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#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; + } +} diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h new file mode 100644 index 0000000000..18c194abf2 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -0,0 +1,13 @@ +/* + * inertial_filter.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include +#include + +void inertial_filter_predict(float dt, float x[3]); + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w); diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c deleted file mode 100644 index 390e1b7208..0000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * kalman_filter_inertial.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#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]; - } - } -} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h deleted file mode 100644 index d34f582980..0000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ /dev/null @@ -1,14 +0,0 @@ -/* - * kalman_filter_inertial.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include - -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]); diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 8ac701c535..939d768495 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -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 diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6ecdfe01d9..306ebe5ccc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -66,7 +66,7 @@ #include #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, ¶m_update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ + /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - /* 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, ¶ms); } + /* 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; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 20142b70c5..8dd0ceff82 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -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; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index c0e0042b6e..870227fefc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -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; }; /**