fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 06:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sonar added to position_estimator_inav

Browse Source
This commit is contained in:
Anton Babushkin
2013-06-28 11:10:46 +04:00
parent 665a9b8865
commit 1759f30e3a
5 changed files with 93 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/inertial_filter.c
+5 -2
View File
@@ -13,9 +13,12 @@ void inertial_filter_predict(float dt, float x[3])
x[1] += x[2] * dt;
}
void inertial_filter_correct(float edt, float x[3], int i, float w)
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{
float ewdt = w * edt;
float ewdt = w * dt;
if (ewdt > 1.0f)
ewdt = 1.0f; // prevent over-correcting
ewdt *= e;
x[i] += ewdt;
if (i == 0) {
src/modules/position_estimator_inav/inertial_filter.h
+1 -1
View File
@@ -10,4 +10,4 @@
void inertial_filter_predict(float dt, float x[3]);
void inertial_filter_correct(float edt, float x[3], int i, float w);
void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+62 -17
View File
@@ -59,6 +59,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/optical_flow.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/geo/geo.h>
@@ -183,6 +184,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
@@ -191,6 +194,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
/* advertise */
@@ -263,12 +267,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime t_prev = 0;
uint16_t accel_updates = 0;
hrt_abstime accel_t = 0;
uint16_t baro_updates = 0;
hrt_abstime baro_t = 0;
hrt_abstime gps_t = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
uint32_t updates_counter_len = 1000000;
@@ -283,6 +285,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
};
float sonar_corr = 0.0f;
float sonar_corr_filtered = 0.0f;
float flow_corr[] = { 0.0f, 0.0f }; // X, Y
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0;
/* main loop */
struct pollfd fds[5] = {
@@ -290,6 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
{ .fd = vehicle_status_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
};
@@ -297,7 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
warnx("main loop started.");
while (!thread_should_exit) {
int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate
int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
@@ -355,13 +364,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (sensor.baro_counter > baro_counter) {
baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0];
baro_corr = - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
}
}
if (params.use_gps && fds[4].revents & POLLIN) {
/* optical flow */
if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
if (flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
sonar_corr = -flow.ground_distance_m - z_est[0];
sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
if (fabsf(sonar_corr) > params.sonar_err) {
// correction is too large: spike or new ground level?
if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
// spike detected, ignore
sonar_corr = 0.0f;
} else {
// new ground level
baro_alt0 += sonar_corr;
warnx("new home: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.home_alt = baro_alt0;
local_pos.home_timestamp = hrt_absolute_time();
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0;
}
}
}
} else {
sonar_corr = 0.0f;
}
flow_updates++;
}
if (params.use_gps && fds[5].revents & POLLIN) {
/* vehicle GPS position */
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) {
@@ -393,8 +434,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_predict(dt, z_est);
/* inertial filter correction for altitude */
inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro);
inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc);
baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
/* dont't try to estimate position when no any position source available */
bool can_estimate_pos = params.use_gps && gps.fix_type >= 3;
@@ -404,15 +447,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_predict(dt, y_est);
/* inertial filter correction for position */
inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc);
inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc);
inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
if (params.use_gps && gps.fix_type >= 3) {
inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
if (gps.vel_ned_valid) {
inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
}
}
}
@@ -421,17 +464,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* print updates rate */
if (t - updates_counter_start > updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f;
printf(
"[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
warnx(
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
accel_updates / updates_dt,
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt);
attitude_updates / updates_dt,
flow_updates / updates_dt);
updates_counter_start = t;
accel_updates = 0;
baro_updates = 0;
gps_updates = 0;
attitude_updates = 0;
flow_updates = 0;
}
}
src/modules/position_estimator_inav/position_estimator_inav_params.c
+15
View File
@@ -43,18 +43,28 @@
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_ALT_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->use_gps = param_find("INAV_USE_GPS");
h->w_alt_baro = param_find("INAV_W_ALT_BARO");
h->w_alt_acc = param_find("INAV_W_ALT_ACC");
h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
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");
h->w_pos_flow = param_find("INAV_W_POS_FLOW");
h->flow_k = param_find("INAV_FLOW_K");
h->sonar_filt = param_find("INAV_SONAR_FILT");
h->sonar_err = param_find("INAV_SONAR_ERR");
return OK;
}
@@ -64,9 +74,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->use_gps, &(p->use_gps));
param_get(h->w_alt_baro, &(p->w_alt_baro));
param_get(h->w_alt_acc, &(p->w_alt_acc));
param_get(h->w_alt_sonar, &(p->w_alt_sonar));
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));
param_get(h->w_pos_flow, &(p->w_pos_flow));
param_get(h->flow_k, &(p->flow_k));
param_get(h->sonar_filt, &(p->sonar_filt));
param_get(h->sonar_err, &(p->sonar_err));
return OK;
}
src/modules/position_estimator_inav/position_estimator_inav_params.h
+10
View File
@@ -44,18 +44,28 @@ struct position_estimator_inav_params {
int use_gps;
float w_alt_baro;
float w_alt_acc;
float w_alt_sonar;
float w_pos_gps_p;
float w_pos_gps_v;
float w_pos_acc;
float w_pos_flow;
float flow_k;
float sonar_filt;
float sonar_err;
};
struct position_estimator_inav_param_handles {
param_t use_gps;
param_t w_alt_baro;
param_t w_alt_acc;
param_t w_alt_sonar;
param_t w_pos_gps_p;
param_t w_pos_gps_v;
param_t w_pos_acc;
param_t w_pos_flow;
param_t flow_k;
param_t sonar_filt;
param_t sonar_err;
};
/**