From 114685a40bf38dc4281224ea18f898b6159df037 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 21:30:58 +0400 Subject: position_estimator_inav - first working version --- apps/position_estimator_inav/Makefile | 3 +- .../acceleration_transform.c | 4 +- .../acceleration_transform.h | 4 +- .../kalman_filter_inertial.c | 14 ++-- .../kalman_filter_inertial.h | 7 ++ .../position_estimator_inav_main.c | 82 +++++++--------------- .../position_estimator_inav_params.c | 69 ++++++++++++++++-- .../position_estimator_inav_params.h | 31 ++++++-- apps/position_estimator_inav/sounds.c | 40 ----------- apps/position_estimator_inav/sounds.h | 16 ----- 10 files changed, 135 insertions(+), 135 deletions(-) delete mode 100644 apps/position_estimator_inav/sounds.c delete mode 100644 apps/position_estimator_inav/sounds.h (limited to 'apps') diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index e5f3b4b42..192ec1825 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,7 +42,6 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ - acceleration_transform.c \ - sounds.c + acceleration_transform.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 9080c4e20..bd933cab4 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -1,8 +1,8 @@ /* * acceleration_transform.c * - * Created on: 30.03.2013 - * Author: Anton Babushkin + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Transform acceleration vector to true orientation and scale * diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 3692da77e..26bf0d68f 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -1,8 +1,8 @@ /* * acceleration_transform.h * - * Created on: 30.03.2013 - * Author: ton + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin */ #ifndef ACCELERATION_TRANSFORM_H_ diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c index 7de06cb44..64031ee7b 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.c +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -1,3 +1,10 @@ +/* + * 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]) { @@ -7,10 +14,9 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; - // y = z - x - for (int i = 0; i < 2; i++) { - y[i] = z[i] - x[i]; - } + // 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 diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h index e6bbf1315..3e5134c33 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.h +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -1,3 +1,10 @@ +/* + * 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]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 1c6d21ded..b8ef1f76a 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,15 +65,13 @@ #include #include "position_estimator_inav_params.h" -//#include -#include "sounds.h" -#include #include "kalman_filter_inertial.h" #include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -89,7 +85,7 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -106,12 +102,14 @@ int position_estimator_inav_main(int argc, char *argv[]) { usage("missing command"); if (!strcmp(argv[1], "start")) { - if (thread_running) { printf("position_estimator_inav already running\n"); /* 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", @@ -148,17 +146,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); - /* kalman filter K for simulation parameters: - * rate_accel = 200 Hz - * rate_baro = 100 Hz - * err_accel = 1.0 m/s^2 - * err_baro = 0.2 m - */ - static float k[3][2] = { - { 0.0262f, 0.00001f }, - { 0.0349f, 0.00191f }, - { 0.000259f, 0.618f } - }; /* initialize values */ static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -213,14 +200,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ - // TODO implement calibration procedure, now put dummy values - int16_t accel_offs[3] = { 0, 0, 0 }; - float accel_T[3][3] = { - { 1.0f, 0.0f, 0.0f }, - { 0.0f, 1.0f, 0.0f }, - { 0.0f, 0.0f, 1.0f } - }; - /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -270,11 +249,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint16_t baro_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = 0; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[3] = { + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -288,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n", ret); + if (verbose_mode) + printf("[position_estimator_inav] subscriptions poll error\n"); } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -359,7 +339,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -384,34 +364,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, k, use_z); - } - if (t - updates_counter_start > updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); - updates_counter_start = t; + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); } - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[position_estimator_inav] dt = %.6f\n", dt); - printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); + 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] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + accelerometer_updates = 0; + baro_updates = 0; + } } - printatt_counter++; if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0; - local_pos_est.vx = 0.0; - local_pos_est.y = 0.0; - local_pos_est.vy = 0.0; + local_pos_est.x = 0.0f; + local_pos_est.vx = 0.0f; + local_pos_est.y = 0.0f; + local_pos_est.vy = 0.0f; local_pos_est.z = z_est[0]; local_pos_est.vz = z_est[1]; local_pos_est.timestamp = hrt_absolute_time(); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 5567fd2cf..fb082fbcb 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli -* Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,16 +42,73 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_R, 1.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_INT32(INAV_ACC_OFFS_0, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); + +PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->r = param_find("POS_EST_R"); + 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->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); + h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); + h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); + + h->acc_t_00 = param_find("INAV_ACC_T_00"); + h->acc_t_01 = param_find("INAV_ACC_T_01"); + h->acc_t_02 = param_find("INAV_ACC_T_02"); + h->acc_t_10 = param_find("INAV_ACC_T_10"); + h->acc_t_11 = param_find("INAV_ACC_T_11"); + h->acc_t_12 = param_find("INAV_ACC_T_12"); + h->acc_t_20 = param_find("INAV_ACC_T_20"); + h->acc_t_21 = param_find("INAV_ACC_T_21"); + h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->r, &(p->r)); + param_get(h->k_alt_00, &(p->k[0][0])); + param_get(h->k_alt_01, &(p->k[0][1])); + param_get(h->k_alt_10, &(p->k[1][0])); + param_get(h->k_alt_11, &(p->k[1][1])); + param_get(h->k_alt_20, &(p->k[2][0])); + param_get(h->k_alt_21, &(p->k[2][1])); + + param_get(h->acc_offs_0, &(p->acc_offs[0])); + param_get(h->acc_offs_1, &(p->acc_offs[1])); + param_get(h->acc_offs_2, &(p->acc_offs[2])); + + param_get(h->acc_t_00, &(p->acc_T[0][0])); + param_get(h->acc_t_01, &(p->acc_T[0][1])); + param_get(h->acc_t_02, &(p->acc_T[0][2])); + param_get(h->acc_t_10, &(p->acc_T[1][0])); + param_get(h->acc_t_11, &(p->acc_T[1][1])); + param_get(h->acc_t_12, &(p->acc_T[1][2])); + param_get(h->acc_t_20, &(p->acc_T[2][0])); + param_get(h->acc_t_21, &(p->acc_T[2][1])); + param_get(h->acc_t_22, &(p->acc_T[2][2])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index c45ca8135..694bf015b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,11 +41,32 @@ #include struct position_estimator_inav_params { - float r; + float k[3][2]; + int16_t acc_offs[3]; + float acc_T[3][3]; }; struct position_estimator_inav_param_handles { - param_t r; + 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 acc_offs_0; + param_t acc_offs_1; + param_t acc_offs_2; + + param_t acc_t_00; + param_t acc_t_01; + param_t acc_t_02; + param_t acc_t_10; + param_t acc_t_11; + param_t acc_t_12; + param_t acc_t_20; + param_t acc_t_21; + param_t acc_t_22; }; /** diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c deleted file mode 100644 index 4f7b05fea..000000000 --- a/apps/position_estimator_inav/sounds.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * sounds.c - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#include -#include -#include - - -static int buzzer; - -int sounds_init(void) -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void sounds_deinit(void) -{ - close(buzzer); -} - -void tune_tetris(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 6); -} - -void tune_sonar(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 7); -} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h deleted file mode 100644 index 356e42169..000000000 --- a/apps/position_estimator_inav/sounds.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * sounds.h - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#ifndef SOUNDS_H_ -#define SOUNDS_H_ - -int sounds_init(void); -void sounds_deinit(void); -void tune_tetris(void); -void tune_sonar(void); - -#endif /* SOUNDS_H_ */ -- cgit v1.2.3