fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 01:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Acceleration vector transform implemented. Accelerometer calibration procedure defined but not implemented yet.

Browse Source
This commit is contained in:
Anton
2013-03-30 01:54:04 +04:00
parent fb663bbb0c
commit 382af2b52d
4 changed files with 242 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/Makefile
+1
View File
@@ -42,6 +42,7 @@ STACKSIZE = 4096
CSRCS = position_estimator_inav_main.c \
position_estimator_inav_params.c \
kalman_filter_inertial.c \
acceleration_transform.c \
sounds.c
include $(APPDIR)/mk/app.mk
apps/position_estimator_inav/acceleration_transform.c
+93
View File
@@ -0,0 +1,93 @@
/*
* acceleration_transform.c
*
* Created on: 30.03.2013
* Author: Anton Babushkin
*
* Transform acceleration vector to true orientation and scale
*
* * * * Model * * *
* accel_corr = accel_T * (accel_raw - accel_offs)
*
* accel_corr[3] - fully corrected acceleration vector in UAV frame
* accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
* accel_raw[3] - raw acceleration vector
* accel_offs[3] - true acceleration offset vector, don't use sensors offset because
* it's caused not only by real offset but also by sensor rotation
*
* * * * Calibration * * *
*
* Tests
*
* accel_corr_x_p = [ g 0 0 ]^T // positive x
* accel_corr_x_n = [ -g 0 0 ]^T // negative x
* accel_corr_y_p = [ 0 g 0 ]^T // positive y
* accel_corr_y_n = [ 0 -g 0 ]^T // negative y
* accel_corr_z_p = [ 0 0 g ]^T // positive z
* accel_corr_z_n = [ 0 0 -g ]^T // negative z
*
* 6 tests * 3 axes = 18 equations
* 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
*
* Find accel_offs
*
* accel_offs = (accel_raw_x_p + accel_raw_x_n +
* accel_raw_y_p + accel_raw_y_n +
* accel_raw_z_p + accel_raw_z_n) / 6
*
*
* Find accel_T
*
* 9 unknown constants
* need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
*
* accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) +
* accel_T[i][1] * (accel_raw[1] - accel_offs[1]) +
* accel_T[i][2] * (accel_raw[2] - accel_offs[2])
* A x = b
*
* x = [ accel_T[0][0] ]
* | accel_T[0][1] |
* | accel_T[0][2] |
* | accel_T[1][0] |
* | accel_T[1][1] |
* | accel_T[1][2] |
* | accel_T[2][0] |
* | accel_T[2][1] |
* [ accel_T[2][2] ]
*
* b = [ accel_corr_x_p[0] ] // One measurement per axis is enough
* | accel_corr_x_p[1] |
* | accel_corr_x_p[2] |
* | accel_corr_y_p[0] |
* | accel_corr_y_p[1] |
* | accel_corr_y_p[2] |
* | accel_corr_z_p[0] |
* | accel_corr_z_p[1] |
* [ accel_corr_z_p[2] ]
*
* a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis
* ...
* A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ]
* | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 |
* | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] |
* | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 |
* | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 |
* | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] |
* | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 |
* | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 |
* [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ]
*
* x = A^-1 b
*/
#include "acceleration_transform.h"
void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) {
for (int i = 0; i < 3; i++) {
accel_corr[i] = 0.0f;
for (int j = 0; j < 3; j++) {
accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]);
}
}
}
apps/position_estimator_inav/acceleration_transform.h
+15
View File
@@ -0,0 +1,15 @@
/*
* acceleration_transform.h
*
* Created on: 30.03.2013
* Author: ton
*/
#ifndef ACCELERATION_TRANSFORM_H_
#define ACCELERATION_TRANSFORM_H_
#include <unistd.h>
void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]);
#endif /* ACCELERATION_TRANSFORM_H_ */
apps/position_estimator_inav/position_estimator_inav_main.c
+133 -83
View File
@@ -71,6 +71,7 @@
#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 */
@@ -147,9 +148,18 @@ 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 k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f,
0.99f } };
static float x_est[3] = { 0.0f, 0.0f, 0.0f };
static float y_est[3] = { 0.0f, 0.0f, 0.0f };
static float z_est[3] = { 0.0f, 0.0f, 0.0f };
@@ -160,7 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
int baro_loop_cnt = 0;
int baro_loop_end = 70; /* measurement for 1 second */
float baro_alt0 = 0.0f; /* to determin while start up */
float rho0 = 1.293f; /* standard pressure */
const static float const_earth_gravity = 9.81f;
static double lat_current = 0.0d; //[°]] --> 47.0
@@ -203,6 +212,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* FIRST PARAMETER UPDATE */
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,
@@ -226,12 +244,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
static int printcounter = 0;
if (printcounter == 100) {
printcounter = 0;
printf("[pos_est1D] wait for GPS fix type 3\n");
printf("[position_estimator_inav] wait for GPS fix type 3\n");
}
printcounter++;
}
/* get gps value for first initialization */
/* 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;
@@ -240,20 +258,38 @@ 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("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n",
printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n",
lat_current, lon_current);
hrt_abstime last_time = 0;
thread_running = true;
/** main loop */
struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } };
printf("[position_estimator_inav] main loop started\n");
static int printatt_counter = 0;
uint32_t accelerometer_counter = 0;
uint32_t baro_counter = 0;
uint16_t accelerometer_updates = 0;
uint16_t baro_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
uint32_t updates_counter_len = 1000000;
hrt_abstime pub_last = 0;
uint32_t pub_interval = 5000; // limit publish rate to 200 Hz
/* main loop */
struct pollfd fds[3] = {
{ .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) {
int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
bool accelerometer_updated = false;
bool baro_updated = false;
int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
if (ret < 0) {
/* poll error */
} else {
printf("[position_estimator_inav] subscriptions poll error\n", ret);
} else if (ret > 0) {
/* parameter update */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -262,7 +298,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
&update);
/* update parameters */
parameters_update(&pos_inav_param_handles, &pos_inav_params);
//r = pos_inav_params.r;
}
/* vehicle status */
if (fds[1].revents & POLLIN) {
@@ -270,16 +305,41 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
&vehicle_status);
}
/* vehicle attitude */
//if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
//}
/* sensor combined */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
}
/* sensor combined */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accelerometer_counter) {
accelerometer_updated = true;
accelerometer_counter = sensor.accelerometer_counter;
accelerometer_updates++;
}
if (sensor.baro_counter > baro_counter) {
baro_updated = true;
baro_counter = sensor.baro_counter;
baro_updates++;
}
// barometric pressure estimation at start up
if (!local_flag_baroINITdone && baro_updated) {
// 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);
local_flag_baroINITdone = true;
char str[80];
sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0);
printf("%s\n", str);
mavlink_log_info(mavlink_fd, str);
}
}
}
if (use_gps) {
/* vehicle GPS position */
//if (fds[4].revents & POLLIN) {
if (fds[4].revents & POLLIN) {
/* new GPS value */
orb_copy(ORB_ID(vehicle_gps_position),
vehicle_gps_position_sub, &gps);
@@ -289,75 +349,65 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
&(local_pos_gps[1]));
local_pos_gps[2] = (float) (gps.alt * 1e-3);
//}
}
// barometric pressure estimation at start up
if (!local_flag_baroINITdone) {
// 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);
local_flag_baroINITdone = true;
char *baro_m_start = "barometer initialized with alt0 = ";
char p0_char[15];
sprintf(p0_char, "%8.2f", baro_alt0 / 100);
char *baro_m_end = " m";
char str[80];
strcpy(str, baro_m_start);
strcat(str, p0_char);
strcat(str, baro_m_end);
mavlink_log_info(mavlink_fd, str);
}
}
hrt_abstime t = hrt_absolute_time();
float dt = (t - last_time) / 1000000.0;
last_time = t;
/* convert accelerations from UAV frame to NED frame */
float accel_NED[3] = { 0.0f, 0.0f, 0.0f };
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
}
}
accel_NED[2] += const_earth_gravity;
/* prediction */
kalman_filter_inertial_predict(dt, z_est);
/* prepare vectors for kalman filter correction */
float z_meas[2]; // pos, accel
bool use_z[2] = { false, true };
if (local_flag_baroINITdone) {
z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt
use_z[0] = true;
} /* end of poll return value check */
hrt_abstime t = hrt_absolute_time();
float dt = (t - last_time) / 1000000.0;
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);
/* 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] * accel_corr[j];
}
}
accel_NED[2] += const_earth_gravity;
/* kalman filter prediction */
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 };
if (local_flag_baroINITdone && baro_updated) {
z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt
use_z[0] = true;
}
if (accelerometer_updated) {
z_meas[1] = accel_NED[2];
use_z[1] = true;
}
if (use_z[0] || use_z[1]) {
/* correction */
kalman_filter_inertial_update(z_est, z_meas, k, use_z);
if (printatt_counter == 100) {
printatt_counter = 0;
printf("[pos_est_inav] dt = %.6f\n", dt);
printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n",
att.pitch, att.roll, att.yaw);
printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n",
sensor.accelerometer_m_s2[0],
sensor.accelerometer_m_s2[1],
sensor.accelerometer_m_s2[2]);
printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n",
accel_NED[0], accel_NED[1], accel_NED[2]);
/*
printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n",
att.R[0][0], att.R[0][1], att.R[0][2]);
printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n",
att.R[1][0], att.R[1][1], att.R[1][2]);
printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n",
att.R[2][0], att.R[2][1], att.R[2][2]);
*/
printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n",
z_est[0], z_est[1], z_est[2]);
}
printatt_counter++;
}
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 (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;
@@ -374,11 +424,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
vehicle_local_position), local_pos_est_pub,
&local_pos_est);
}
} /* end of poll return value check */
}
}
printf("[pos_est_inav] exiting.\n");
mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting");
printf("[position_estimator_inav] exiting.\n");
mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting");
thread_running = false;
return 0;
}