fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 19:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

position_estimator_inav - first working version

Browse Source
This commit is contained in:
Anton
2013-03-30 21:30:58 +04:00
parent 72b8abca22
commit 114685a40b
10 changed files with 135 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/Makefile
+1 -2
View File
@@ -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
apps/position_estimator_inav/acceleration_transform.c
+2 -2
View File
@@ -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 <rk3dov@gmail.com>
*
* Transform acceleration vector to true orientation and scale
*
apps/position_estimator_inav/acceleration_transform.h
+2 -2
View File
@@ -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 <rk3dov@gmail.com>
*/
#ifndef ACCELERATION_TRANSFORM_H_
apps/position_estimator_inav/kalman_filter_inertial.c
+10 -4
View File
@@ -1,3 +1,10 @@
/*
* 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]) {
@@ -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
apps/position_estimator_inav/kalman_filter_inertial.h
+7
View File
@@ -1,3 +1,10 @@
/*
* 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]);
apps/position_estimator_inav/position_estimator_inav_main.c
+26 -56
View File
@@ -1,9 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -67,15 +65,13 @@
#include <drivers/drv_hrt.h>
#include "position_estimator_inav_params.h"
//#include <uORB/topics/debug_key_value.h>
#include "sounds.h"
#include <drivers/drv_tone_alarm.h>
#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 <additional params>]\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);
kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.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;
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;
}
}
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]);
}
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();
apps/position_estimator_inav/position_estimator_inav_params.c
+62 -7
View File
@@ -1,9 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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;
}
apps/position_estimator_inav/position_estimator_inav_params.h
+25 -6
View File
@@ -1,9 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Damian Aregger <daregger@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -43,11 +41,32 @@
#include <systemlib/param/param.h>
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;
};
/**
apps/position_estimator_inav/sounds.c
-40
View File
@@ -1,40 +0,0 @@
/*
* sounds.c
*
* Created on: Feb 26, 2013
* Author: samuezih
*/
#include <sys/types.h>
#include <fcntl.h>
#include <drivers/drv_tone_alarm.h>
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);
}
apps/position_estimator_inav/sounds.h
-16
View File
@@ -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_ */