dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Partially merge branch 'inav_alt_gps' into vector_control2, only estimator part

This commit is contained in:
Anton Babushkin 2013-12-23 11:13:08 +04:00
parent 4c6f6ed12c 98ebcb626d
commit f36ffe0859
6 changed files with 491 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

602
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -71,15 +71,21 @@
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
#define MIN_VALID_W 0.00001f
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;
static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s
static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s
static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@ -95,8 +101,7 @@ 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");
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
}
@ -115,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("position_estimator_inav already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
@ -135,16 +140,23 @@ int position_estimator_inav_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
if (thread_running) {
warnx("stop");
thread_should_exit = true;
} else {
warnx("app not started");
}
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tposition_estimator_inav is running\n");
warnx("app is running");
} else {
printf("\tposition_estimator_inav not started\n");
warnx("app not started");
}
exit(0);
@ -159,27 +171,74 @@ int position_estimator_inav_main(int argc, char *argv[])
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
warnx("started.");
warnx("started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[inav] started");
/* initialize values */
float x_est[3] = { 0.0f, 0.0f, 0.0f };
float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f };
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_alt0 = 0.0f; /* to determine while start up */
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
float surface_offset = 0.0f; // ground level offset from reference altitude
float surface_offset_rate = 0.0f; // surface offset change rate
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
hrt_abstime t_prev = 0;
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D
float corr_gps[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
/* declare and safely initialize all structs */
struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
@ -247,75 +306,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
baro_alt0 += sensor.baro_alt_meter;
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
} else {
wait_baro = false;
baro_alt0 /= (float) baro_init_cnt;
warnx("init baro: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
local_pos.z_global = true;
}
}
}
}
}
bool ref_xy_inited = false;
hrt_abstime ref_xy_init_start = 0;
const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix
hrt_abstime t_prev = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D
float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float baro_corr = 0.0f; // D
float gps_corr[2][2] = {
{ 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[7] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = actuator_sub, .events = POLLIN },
{ .fd = armed_sub, .events = POLLIN },
struct pollfd fds[1] = {
{ .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 }
};
if (!thread_should_exit) {
warnx("main loop started.");
}
while (!thread_should_exit) {
int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
@ -324,40 +337,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
&update);
/* update parameters */
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&pos_inav_param_handles, &params);
}
/* actuator */
if (fds[1].revents & POLLIN) {
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
orb_check(armed_sub, &updated);
/* vehicle attitude */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
flag_armed = armed.armed;
baro_offset -= z_est[0];
corr_baro = 0.0f;
local_pos.ref_alt -= z_est[0];
local_pos.ref_timestamp = t;
z_est[0] = 0.0f;
alt_avg = 0.0f;
}
}
/* sensor combined */
if (fds[4].revents & POLLIN) {
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter != accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[2] -= accel_bias[2];
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
sensor.accelerometer_m_s2[2] -= acc_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
@ -368,12 +401,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
accel_corr[0] = accel_NED[0] - x_est[2];
accel_corr[1] = accel_NED[1] - y_est[2];
accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
corr_acc[0] = accel_NED[0] - x_est[2];
corr_acc[1] = accel_NED[1] - y_est[2];
corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
} else {
memset(accel_corr, 0, sizeof(accel_corr));
memset(corr_acc, 0, sizeof(corr_acc));
}
accel_counter = sensor.accelerometer_counter;
@ -381,180 +414,353 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (sensor.baro_counter != baro_counter) {
baro_corr = - sensor.baro_alt_meter - z_est[0];
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
}
}
/* optical flow */
if (fds[5].revents & POLLIN) {
orb_check(optical_flow_sub, &updated);
if (updated) {
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 (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
corr_sonar_filtered += (corr_sonar - corr_sonar_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;
if (fabsf(corr_sonar) > params.sonar_err) {
/* correction is too large: spike or new ground level? */
if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
/* spike detected, ignore */
corr_sonar = 0.0f;
sonar_valid = false;
} else {
// new ground level
baro_alt0 += sonar_corr;
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
z_est[0] += sonar_corr;
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f;
}
} else {
/* new ground level */
surface_offset -= corr_sonar;
surface_offset_rate = 0.0f;
corr_sonar = 0.0f;
corr_sonar_filtered = 0.0f;
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
}
} else {
/* correction is ok, use it */
sonar_valid_time = t;
sonar_valid = true;
}
}
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/* convert raw flow to angular flow */
float flow_ang[2];
flow_ang[0] = flow.flow_raw_x * params.flow_k;
flow_ang[1] = flow.flow_raw_y * params.flow_k;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
flow_m[1] = -flow_ang[1] * flow_dist;
flow_m[2] = z_est[1];
/* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f };
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += att.R[i][j] * flow_m[j];
}
}
/* velocity correction */
corr_flow[0] = flow_v[0] - x_est[1];
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate)
w_flow *= 0.05f;
flow_valid = true;
} else {
sonar_corr = 0.0f;
w_flow = 0.0f;
flow_valid = false;
}
flow_updates++;
}
/* vehicle GPS position */
if (fds[6].revents & POLLIN) {
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
/* require EPH < 10m */
if (gps.eph_m < 10.0f) {
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
ref_xy_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon);
mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
}
} else {
ref_xy_init_start = 0;
if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
}
if (ref_xy_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
gps_corr[0][0] = gps_proj[0] - x_est[0];
gps_corr[1][0] = gps_proj[1] - y_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
} else {
gps_corr[0][1] = 0.0f;
gps_corr[1][1] = 0.0f;
} else {
if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
gps_valid = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
}
} else {
gps_valid = false;
}
if (gps_valid) {
/* initialize reference position if needed */
if (!ref_inited) {
if (ref_init_start == 0) {
ref_init_start = t;
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
}
}
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
} else {
corr_gps[0][1] = 0.0f;
corr_gps[1][1] = 0.0f;
corr_gps[2][1] = 0.0f;
}
w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
}
} else {
/* no GPS lock */
memset(gps_corr, 0, sizeof(gps_corr));
ref_xy_init_start = 0;
memset(corr_gps, 0, sizeof(corr_gps));
ref_init_start = 0;
}
gps_updates++;
}
}
/* end of poll return value check */
/* check for timeout on FLOW topic */
if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
flow_valid = false;
sonar_valid = false;
warnx("FLOW timeout");
mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
}
/* check for timeout on GPS topic */
if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
}
/* check for sonar measurement timeout */
if (sonar_valid && t > sonar_time + sonar_timeout) {
corr_sonar = 0.0f;
sonar_valid = false;
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
baro_alt0 -= z_est[0];
z_est[0] = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
/* use GPS if it's valid and reference position initialized */
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
/* accel bias correction, now only for Z
* not strictly correct, but stable and works */
accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
bool can_estimate_xy = (t < xy_src_time + xy_src_timeout);
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
if (dist_bottom_valid) {
/* surface distance prediction */
surface_offset += surface_offset_rate * dt;
/* surface distance correction */
if (sonar_valid) {
surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
surface_offset -= corr_sonar * params.w_z_sonar * dt;
}
}
float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_xy_gps_p *= params.w_gps_flow;
w_xy_gps_v *= params.w_gps_flow;
}
/* baro offset correction */
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
baro_counter += offs_corr;
}
/* accelerometer bias correction */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {
accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
}
if (use_gps_z) {
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
}
if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
}
accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
}
acc_bias[i] += c * params.w_acc_bias * dt;
}
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est);
/* inertial filter correction for altitude */
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);
bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
bool flow_valid = false; // TODO implement opt flow
/* try to estimate xy even if no absolute position source available,
* if using optical flow velocity will be correct in this case */
bool can_estimate_xy = gps_valid || flow_valid;
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est);
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
warnx("BAD ESTIMATE AFTER PREDICTION %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]);
thread_should_exit = true;
}
/* 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(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
if (gps_valid) {
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 (use_flow) {
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
}
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
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);
if (use_gps_xy) {
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_v);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
}
}
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
warnx("BAD ESTIMATE AFTER CORRECTION dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]);
warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", corr_acc[0], corr_acc[1], corr_gps[0][0], corr_gps[0][1], corr_gps[1][0], corr_gps[1][1]);
thread_should_exit = true;
}
}
/* detect land */
alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp = z_est[0] - alt_avg;
alt_disp = alt_disp * alt_disp;
alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp2 = - z_est[0] - alt_avg;
alt_disp2 = alt_disp2 * alt_disp2;
float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */
float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp > land_disp2 && thrust > params.land_thr) {
if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
landed = false;
landed_time = 0;
}
} else {
if (alt_disp < land_disp2 && thrust < params.land_thr) {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
if (landed_time == 0) {
landed_time = t; // land detected first time
@ -593,10 +799,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (t > pub_last + pub_interval) {
pub_last = t;
/* publish local position */
local_pos.timestamp = t;
local_pos.xy_valid = can_estimate_xy && gps_valid;
local_pos.xy_valid = can_estimate_xy && use_gps_xy;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z;
local_pos.x = x_est[0];
local_pos.vx = x_est[1];
local_pos.y = y_est[0];
@ -605,6 +811,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.vz = z_est[1];
local_pos.landed = landed;
local_pos.yaw = att.yaw;
local_pos.dist_bottom_valid = dist_bottom_valid;
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = -z_est[0] - surface_offset;
local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
}
local_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
@ -636,17 +850,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (local_pos.v_z_valid) {
global_pos.vz = local_pos.vz;
}
global_pos.yaw = local_pos.yaw;
global_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
flag_armed = armed.armed;
}
warnx("exiting.");
mavlink_log_info(mavlink_fd, "[inav] exiting");
warnx("stopped");
mavlink_log_info(mavlink_fd, "[inav] stopped");
thread_running = false;
return 0;
}

55
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -40,16 +40,19 @@
#include "position_estimator_inav_params.h"
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
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, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
@ -57,15 +60,18 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
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->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_acc = param_find("INAV_W_Z_ACC");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_acc = param_find("INAV_W_XY_ACC");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
h->sonar_filt = param_find("INAV_SONAR_FILT");
h->sonar_err = param_find("INAV_SONAR_ERR");
h->land_t = param_find("INAV_LAND_T");
@ -77,15 +83,18 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
{
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->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_acc, &(p->w_z_acc));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_acc, &(p->w_xy_acc));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
param_get(h->flow_q_min, &(p->flow_q_min));
param_get(h->sonar_filt, &(p->sonar_filt));
param_get(h->sonar_err, &(p->sonar_err));
param_get(h->land_t, &(p->land_t));

34
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -41,15 +41,18 @@
#include <systemlib/param/param.h>
struct position_estimator_inav_params {
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 w_z_baro;
float w_z_gps_p;
float w_z_acc;
float w_z_sonar;
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_acc;
float w_xy_flow;
float w_gps_flow;
float w_acc_bias;
float flow_k;
float flow_q_min;
float sonar_filt;
float sonar_err;
float land_t;
@ -58,15 +61,18 @@ struct position_estimator_inav_params {
};
struct position_estimator_inav_param_handles {
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 w_z_baro;
param_t w_z_gps_p;
param_t w_z_acc;
param_t w_z_sonar;
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_acc;
param_t w_xy_flow;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;
param_t flow_q_min;
param_t sonar_filt;
param_t sonar_err;
param_t land_t;

15
src/modules/sdlog2/sdlog2.c
View File

@ -752,6 +752,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_DIST_s log_DIST;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -909,6 +910,9 @@ int sdlog2_thread_main(int argc, char *argv[])
uint16_t baro_counter = 0;
uint16_t differential_pressure_counter = 0;
/* track changes in distance status */
bool dist_bottom_present = false;
/* enable logging on start if needed */
if (log_on_start)
sdlog2_start_log();
@ -1122,6 +1126,17 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
if (buf.local_pos.dist_bottom_valid) {
dist_bottom_present = true;
}
if (dist_bottom_present) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
}
/* --- LOCAL POSITION SETPOINT --- */

11
src/modules/sdlog2/sdlog2_messages.h
View File

@ -247,6 +247,16 @@ struct log_GVSP_s {
float vz;
};
/* --- DIST - DISTANCE TO SURFACE --- */
#define LOG_DIST_MSG 20
struct log_DIST_s {
float bottom;
float bottom_rate;
uint8_t flags;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@ -290,6 +300,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),

5
src/modules/uORB/topics/vehicle_local_position.h
View File

@ -77,6 +77,11 @@ struct vehicle_local_position_s
int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
bool landed; /**< true if vehicle is landed */
/* Distance to surface */
float dist_bottom; /**< Distance to bottom surface (ground) */
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */
uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */
bool dist_bottom_valid; /**< true if distance to bottom surface is valid */
};
/**