fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 07:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

position_estimator_inav: add vehicle_odometry usage; improve inout interface

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 183a63cce9
commit cce36e69c8
1 changed files with 182 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+182 -106
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -61,7 +61,6 @@
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_odometry.h> #include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/optical_flow.h> #include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h> #include <uORB/topics/distance_sensor.h>
#include <uORB/topics/vehicle_air_data.h> #include <uORB/topics/vehicle_air_data.h>
@@ -343,8 +342,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool lidar_valid = false; // lidar is valid bool lidar_valid = false; // lidar is valid
bool flow_valid = false; // flow 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) bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
bool vision_valid = false; // vision is valid bool vision_xy_valid = false; // vision XY is valid
bool mocap_valid = false; // mocap is valid bool vision_z_valid = false; // vision Z is valid
bool vision_vxy_valid = false; // vision VXY is valid
bool vision_vz_valid = false; // vision VZ is valid
bool mocap_xy_valid = false; // mocap XY is valid
bool mocap_z_valid = false; // mocap Z is valid
/* set pose/velocity as invalid if standard deviation is bigger than max_std_dev */
/* TODO: the user should be allowed to set these values by a parameter */
static constexpr float ep_max_std_dev = 100.0f; // position estimation max std deviation
static constexpr float ev_max_std_dev = 100.0f; // velocity estimation max std deviation
/* declare and safely initialize all structs */ /* declare and safely initialize all structs */
struct actuator_controls_s actuator; struct actuator_controls_s actuator;
@@ -357,13 +365,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&gps, 0, sizeof(gps)); memset(&gps, 0, sizeof(gps));
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att)); memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos; struct vehicle_local_position_s pos;
memset(&local_pos, 0, sizeof(local_pos)); memset(&pos, 0, sizeof(pos));
struct optical_flow_s flow; struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow)); memset(&flow, 0, sizeof(flow));
struct vehicle_local_position_s vision; struct vehicle_odometry_s visual_odom;
memset(&vision, 0, sizeof(vision)); memset(&visual_odom, 0, sizeof(visual_odom));
struct att_pos_mocap_s mocap; struct vehicle_odometry_s mocap;
memset(&mocap, 0, sizeof(mocap)); memset(&mocap, 0, sizeof(mocap));
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos)); memset(&global_pos, 0, sizeof(global_pos));
@@ -382,8 +390,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int visual_odometry_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry)); int visual_odom_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int mocap_position_sub = orb_subscribe(ORB_ID(vehicle_mocap_odometry));
int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
int vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data)); int vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data));
// because we can have several distance sensor instances with different orientations // because we can have several distance sensor instances with different orientations
@@ -394,7 +402,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* advertise */ /* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos);
orb_advert_t vehicle_global_position_pub = nullptr; orb_advert_t vehicle_global_position_pub = nullptr;
struct position_estimator_inav_params params; struct position_estimator_inav_params params;
@@ -457,8 +465,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else { } else {
wait_baro = false; wait_baro = false;
baro_offset /= (float) baro_init_cnt; baro_offset /= (float) baro_init_cnt;
local_pos.z_valid = true; pos.z_valid = true;
local_pos.v_z_valid = true; pos.v_z_valid = true;
} }
} }
} }
@@ -774,67 +782,120 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* check no vision circuit breaker is set */ /* check no vision circuit breaker is set */
if (params.no_vision != CBRK_NO_VISION_KEY) { if (params.no_vision != CBRK_NO_VISION_KEY) {
/* vehicle vision position */ /* vehicle visual odometry */
orb_check(visual_odometry_sub, &updated); orb_check(visual_odom_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_visual_odometry), visual_odometry_sub, &vision); orb_copy(ORB_ID(vehicle_visual_odometry), visual_odom_sub, &visual_odom);
static float last_vision_x = 0.0f; static float last_vision_x = 0.0f;
static float last_vision_y = 0.0f; static float last_vision_y = 0.0f;
static float last_vision_z = 0.0f; static float last_vision_z = 0.0f;
/* reset position estimate on first vision update */ vision_xy_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? sqrtf(fmaxf(visual_odom.pose_covariance[0], visual_odom.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
if (!vision_valid) { vision_z_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev : true) ? false : true;
x_est[0] = vision.x; vision_vxy_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? sqrtf(fmaxf(visual_odom.velocity_covariance[0], visual_odom.velocity_covariance[6])) > ev_max_std_dev : true) ? false : true;
x_est[1] = vision.vx; vision_vz_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] > ep_max_std_dev : true) ? false : true;
y_est[0] = vision.y;
y_est[1] = vision.vy; /* reset position estimate on first vision update */
if (vision_xy_valid) {
x_est[0] = visual_odom.x;
y_est[0] = visual_odom.y;
last_vision_x = visual_odom.x;
last_vision_y = visual_odom.y;
} else {
warnx("VISION XY estimate not valid");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION XY estimate not valid");
}
if (vision_vxy_valid) {
x_est[1] = visual_odom.vx;
y_est[1] = visual_odom.vy;
} else {
warnx("VISION VXY estimate not valid");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION VXY estimate not valid");
}
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W) {
if (vision_z_valid) {
z_est[0] = visual_odom.z;
last_vision_z = visual_odom.z;
} else {
warnx("VISION Z estimate not valid");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION Z estimate not valid");
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W) {
z_est[0] = vision.z;
z_est[1] = vision.vz;
} }
vision_valid = true; if (vision_vz_valid) {
z_est[1] = visual_odom.vz;
last_vision_x = vision.x; } else {
last_vision_y = vision.y; warnx("VISION VZ estimate not valid");
last_vision_z = vision.z; mavlink_log_info(&mavlink_log_pub, "[inav] VISION VZ estimate not valid");
warnx("VISION estimate valid"); }
mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");
} }
/* calculate correction for position */ /* calculate correction for position */
corr_vision[0][0] = vision.x - x_est[0]; if (vision_xy_valid) {
corr_vision[1][0] = vision.y - y_est[0]; corr_vision[0][0] = visual_odom.x - x_est[0];
corr_vision[2][0] = vision.z - z_est[0]; corr_vision[1][0] = visual_odom.y - y_est[0];
}
if (vision_z_valid) {
corr_vision[2][0] = visual_odom.z - z_est[0];
}
static hrt_abstime last_vision_time = 0; static hrt_abstime last_vision_time = 0;
float vision_dt = (vision.timestamp - last_vision_time) / 1e6f; float vision_dt = (visual_odom.timestamp - last_vision_time) / 1e6f;
last_vision_time = vision.timestamp; last_vision_time = visual_odom.timestamp;
if (vision_dt > 0.000001f && vision_dt < 0.2f) { if (vision_vxy_valid) {
vision.vx = (vision.x - last_vision_x) / vision_dt; /* calculate correction for XY velocity from external estimation */
vision.vy = (vision.y - last_vision_y) / vision_dt; corr_vision[0][1] = visual_odom.vx - x_est[1];
vision.vz = (vision.z - last_vision_z) / vision_dt; corr_vision[1][1] = visual_odom.vy - y_est[1];
last_vision_x = vision.x; } else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_xy_valid) {
last_vision_y = vision.y; visual_odom.vx = (visual_odom.x - last_vision_x) / vision_dt;
last_vision_z = vision.z; visual_odom.vy = (visual_odom.y - last_vision_y) / vision_dt;
/* calculate correction for velocity */ last_vision_x = visual_odom.x;
corr_vision[0][1] = vision.vx - x_est[1]; last_vision_y = visual_odom.y;
corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1]; /* calculate correction for XY velocity */
corr_vision[0][1] = visual_odom.vx - x_est[1];
corr_vision[1][1] = visual_odom.vy - y_est[1];
} else { } else {
/* assume zero motion */ /* assume zero motion in XY plane */
corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[0][1] = 0.0f - x_est[1];
corr_vision[1][1] = 0.0f - y_est[1]; corr_vision[1][1] = 0.0f - y_est[1];
}
if (vision_vz_valid) {
/* calculate correction for Z velocity from external estimation */
corr_vision[2][1] = visual_odom.vz - z_est[1];
} else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_z_valid) {
visual_odom.vz = (visual_odom.z - last_vision_z) / vision_dt;
last_vision_z = visual_odom.z;
/* calculate correction for Z velocity */
corr_vision[2][1] = visual_odom.vz - z_est[1];
} else {
/* assume zero motion in Z plane */
corr_vision[2][1] = 0.0f - z_est[1]; corr_vision[2][1] = 0.0f - z_est[1];
} }
@@ -843,28 +904,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* vehicle mocap position */ /* vehicle mocap position */
orb_check(att_pos_mocap_sub, &updated); orb_check(mocap_position_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap);
mocap_xy_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? sqrtf(fmaxf(mocap.pose_covariance[0], mocap.pose_covariance[6])) > ep_max_std_dev : true) ? false : true;
mocap_z_valid = (!PX4_ISFINITE(mocap.pose_covariance[0]) ? mocap.pose_covariance[11] > ep_max_std_dev : true) ? false : true;
if (!params.disable_mocap) { if (!params.disable_mocap) {
/* reset position estimate on first mocap update */ /* reset position estimate on first mocap update */
if (!mocap_valid) { if (mocap_xy_valid) {
x_est[0] = mocap.x; x_est[0] = mocap.x;
y_est[0] = mocap.y; y_est[0] = mocap.y;
}
if (mocap_z_valid) {
z_est[0] = mocap.z; z_est[0] = mocap.z;
}
mocap_valid = true; if (!mocap_xy_valid || !mocap_z_valid) {
warnx("MOCAP data not valid");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data not valid");;
warnx("MOCAP data valid");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");
} }
/* calculate correction for position */ /* calculate correction for position */
corr_mocap[0][0] = mocap.x - x_est[0]; if (mocap_xy_valid) {
corr_mocap[1][0] = mocap.y - y_est[0]; corr_mocap[0][0] = mocap.x - x_est[0];
corr_mocap[2][0] = mocap.z - z_est[0]; corr_mocap[1][0] = mocap.y - y_est[0];
}
if (mocap_z_valid) {
corr_mocap[2][0] = mocap.z - z_est[0];
}
mocap_updates++; mocap_updates++;
} }
@@ -914,10 +987,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
y_est[0] = 0.0f; y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s; y_est[1] = gps.vel_e_m_s;
local_pos.ref_lat = lat; pos.ref_lat = lat;
local_pos.ref_lon = lon; pos.ref_lon = lon;
local_pos.ref_alt = alt + z_est[0]; pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t; pos.ref_timestamp = t;
/* initialize projection */ /* initialize projection */
map_projection_init(&ref, lat, lon); map_projection_init(&ref, lat, lon);
@@ -950,7 +1023,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* calculate correction for position */ /* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; corr_gps[2][0] = pos.ref_alt - alt - est_buf[est_i][2][0];
/* calculate correction for velocity */ /* calculate correction for velocity */
if (gps.vel_ned_valid) { if (gps.vel_ned_valid) {
@@ -998,15 +1071,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
/* check for timeout on vision topic */ /* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) { if ((vision_xy_valid || vision_z_valid) && (t > (visual_odom.timestamp + vision_topic_timeout))) {
vision_valid = false; vision_xy_valid = false;
vision_z_valid = false;
warnx("VISION timeout"); warnx("VISION timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
} }
/* check for timeout on mocap topic */ /* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) { if ((mocap_xy_valid || mocap_z_valid) && (t > (mocap.timestamp + mocap_topic_timeout))) {
mocap_valid = false; mocap_xy_valid = false;
mocap_z_valid = false;
warnx("MOCAP timeout"); warnx("MOCAP timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");
} }
@@ -1043,10 +1118,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; 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; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
/* use VISION if it's valid and has a valid weight parameter */ /* use VISION if it's valid and has a valid weight parameter */
bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_xy = vision_xy_valid && params.w_xy_vision_p > MIN_VALID_W;
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; bool use_vision_z = vision_z_valid && params.w_z_vision_p > MIN_VALID_W;
/* use MOCAP if it's valid and has a valid weight parameter */ /* use MOCAP if it's valid and has a valid weight parameter */
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W bool use_mocap = mocap_xy_valid && mocap_z_valid && params.w_mocap_p > MIN_VALID_W
&& params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap
if (params.disable_mocap) { //disable mocap if fake gps is used if (params.disable_mocap) { //disable mocap if fake gps is used
@@ -1358,57 +1433,58 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* publish local position */ /* publish local position */
local_pos.xy_valid = can_estimate_xy; pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy; pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && use_gps_xy; pos.xy_global = pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z; pos.z_global = pos.z_valid && use_gps_z;
local_pos.x = x_est[0]; pos.x = x_est[0];
local_pos.vx = x_est[1]; pos.vx = x_est[1];
local_pos.y = y_est[0]; pos.y = y_est[0];
local_pos.vy = y_est[1]; pos.vy = y_est[1];
local_pos.z = z_est[0]; pos.z = z_est[0];
local_pos.vz = z_est[1]; pos.vz = z_est[1];
matrix::Eulerf euler(R); pos.ax = NAN;
local_pos.yaw = euler.psi(); pos.ay = NAN;
local_pos.dist_bottom_valid = dist_bottom_valid; pos.az = NAN;
local_pos.eph = eph; pos.yaw = matrix::Eulerf(matrix::Quatf(att.q)).psi();
local_pos.epv = epv; pos.dist_bottom_valid = dist_bottom_valid;
// TODO provide calculated values for these pos.eph = eph;
local_pos.evh = 0.0f; pos.epv = epv;
local_pos.evv = 0.0f; pos.evh = 0.0f;
local_pos.vxy_max = INFINITY; pos.evv = 0.0f;
local_pos.vz_max = INFINITY; pos.vxy_max = INFINITY;
local_pos.hagl_min = INFINITY; pos.vz_max = INFINITY;
local_pos.hagl_max = INFINITY; pos.hagl_min = INFINITY;
pos.hagl_max = INFINITY;
// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
local_pos.z_deriv = z_est[1]; pos.z_deriv = z_est[1];
if (local_pos.dist_bottom_valid) { if (pos.dist_bottom_valid) {
local_pos.dist_bottom = dist_ground; pos.dist_bottom = dist_ground;
local_pos.dist_bottom_rate = - z_est[1]; pos.dist_bottom_rate = - z_est[1];
} }
local_pos.timestamp = t; pos.timestamp = t;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos);
if (local_pos.xy_global && local_pos.z_global) { if (pos.xy_global && pos.z_global) {
/* publish global position */ /* publish global position */
global_pos.timestamp = t; global_pos.timestamp = t;
double est_lat, est_lon; double est_lat, est_lon;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); map_projection_reproject(&ref, pos.x, pos.y, &est_lat, &est_lon);
global_pos.lat = est_lat; global_pos.lat = est_lat;
global_pos.lon = est_lon; global_pos.lon = est_lon;
global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.alt = pos.ref_alt - pos.z;
global_pos.vel_n = local_pos.vx; global_pos.vel_n = pos.vx;
global_pos.vel_e = local_pos.vy; global_pos.vel_e = pos.vy;
global_pos.vel_d = local_pos.vz; global_pos.vel_d = pos.vz;
global_pos.yaw = local_pos.yaw; global_pos.yaw = matrix::Eulerf(matrix::Quatf(R)).psi();
global_pos.eph = eph; global_pos.eph = eph;
global_pos.epv = epv; global_pos.epv = epv;