fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-07-12 19:27:04 +01:00 committed by Lorenz Meier
parent 183a63cce9
commit cce36e69c8
1 changed files with 182 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

288
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
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
* modification, are permitted provided that the following conditions
@ -61,7 +61,6 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.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 flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
bool vision_valid = false; // vision is valid
bool mocap_valid = false; // mocap is valid
bool vision_xy_valid = false; // vision XY 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 */
struct actuator_controls_s actuator;
@ -357,13 +365,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&gps, 0, sizeof(gps));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_s pos;
memset(&pos, 0, sizeof(pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vehicle_local_position_s vision;
memset(&vision, 0, sizeof(vision));
struct att_pos_mocap_s mocap;
struct vehicle_odometry_s visual_odom;
memset(&visual_odom, 0, sizeof(visual_odom));
struct vehicle_odometry_s mocap;
memset(&mocap, 0, sizeof(mocap));
struct vehicle_global_position_s 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 optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int visual_odometry_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int visual_odom_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry));
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_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data));
// 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 */
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;
struct position_estimator_inav_params params;
@ -457,8 +465,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
local_pos.z_valid = true;
local_pos.v_z_valid = true;
pos.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 */
if (params.no_vision != CBRK_NO_VISION_KEY) {
/* vehicle vision position */
orb_check(visual_odometry_sub, &updated);
/* vehicle visual odometry */
orb_check(visual_odom_sub, &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_y = 0.0f;
static float last_vision_z = 0.0f;
/* reset position estimate on first vision update */
if (!vision_valid) {
x_est[0] = vision.x;
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
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;
vision_z_valid = (!PX4_ISFINITE(visual_odom.pose_covariance[0]) ? visual_odom.pose_covariance[11] > ep_max_std_dev : true) ? false : true;
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;
vision_vz_valid = (!PX4_ISFINITE(visual_odom.velocity_covariance[0]) ? visual_odom.velocity_covariance[11] > ep_max_std_dev : true) ? false : true;
/* 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;
last_vision_y = vision.y;
last_vision_z = vision.z;
} else {
warnx("VISION VZ estimate not valid");
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 */
corr_vision[0][0] = vision.x - x_est[0];
corr_vision[1][0] = vision.y - y_est[0];
corr_vision[2][0] = vision.z - z_est[0];
if (vision_xy_valid) {
corr_vision[0][0] = visual_odom.x - x_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;
float vision_dt = (vision.timestamp - last_vision_time) / 1e6f;
last_vision_time = vision.timestamp;
float vision_dt = (visual_odom.timestamp - last_vision_time) / 1e6f;
last_vision_time = visual_odom.timestamp;
if (vision_dt > 0.000001f && vision_dt < 0.2f) {
vision.vx = (vision.x - last_vision_x) / vision_dt;
vision.vy = (vision.y - last_vision_y) / vision_dt;
vision.vz = (vision.z - last_vision_z) / vision_dt;
if (vision_vxy_valid) {
/* calculate correction for XY velocity from external estimation */
corr_vision[0][1] = visual_odom.vx - x_est[1];
corr_vision[1][1] = visual_odom.vy - y_est[1];
last_vision_x = vision.x;
last_vision_y = vision.y;
last_vision_z = vision.z;
} else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_xy_valid) {
visual_odom.vx = (visual_odom.x - last_vision_x) / vision_dt;
visual_odom.vy = (visual_odom.y - last_vision_y) / vision_dt;
/* calculate correction for velocity */
corr_vision[0][1] = vision.vx - x_est[1];
corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1];
last_vision_x = visual_odom.x;
last_vision_y = visual_odom.y;
/* 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 {
/* assume zero motion */
/* assume zero motion in XY plane */
corr_vision[0][1] = 0.0f - x_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];
}
@ -843,28 +904,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* vehicle mocap position */
orb_check(att_pos_mocap_sub, &updated);
orb_check(mocap_position_sub, &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) {
/* reset position estimate on first mocap update */
if (!mocap_valid) {
if (mocap_xy_valid) {
x_est[0] = mocap.x;
y_est[0] = mocap.y;
}
if (mocap_z_valid) {
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 */
corr_mocap[0][0] = mocap.x - x_est[0];
corr_mocap[1][0] = mocap.y - y_est[0];
corr_mocap[2][0] = mocap.z - z_est[0];
if (mocap_xy_valid) {
corr_mocap[0][0] = mocap.x - x_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++;
}
@ -914,10 +987,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s;
local_pos.ref_lat = lat;
local_pos.ref_lon = lon;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
pos.ref_lat = lat;
pos.ref_lon = lon;
pos.ref_alt = alt + z_est[0];
pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(&ref, lat, lon);
@ -950,7 +1023,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* calculate correction for position */
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[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 */
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 */
if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) {
vision_valid = false;
if ((vision_xy_valid || vision_z_valid) && (t > (visual_odom.timestamp + vision_topic_timeout))) {
vision_xy_valid = false;
vision_z_valid = false;
warnx("VISION timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
}
/* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) {
mocap_valid = false;
if ((mocap_xy_valid || mocap_z_valid) && (t > (mocap.timestamp + mocap_topic_timeout))) {
mocap_xy_valid = false;
mocap_z_valid = false;
warnx("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_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 */
bool use_vision_xy = vision_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_xy = vision_xy_valid && params.w_xy_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 */
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
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 */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
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];
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
matrix::Eulerf euler(R);
local_pos.yaw = euler.psi();
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;
// TODO provide calculated values for these
local_pos.evh = 0.0f;
local_pos.evv = 0.0f;
local_pos.vxy_max = INFINITY;
local_pos.vz_max = INFINITY;
local_pos.hagl_min = INFINITY;
local_pos.hagl_max = INFINITY;
pos.xy_valid = can_estimate_xy;
pos.v_xy_valid = can_estimate_xy;
pos.xy_global = pos.xy_valid && use_gps_xy;
pos.z_global = pos.z_valid && use_gps_z;
pos.x = x_est[0];
pos.vx = x_est[1];
pos.y = y_est[0];
pos.vy = y_est[1];
pos.z = z_est[0];
pos.vz = z_est[1];
pos.ax = NAN;
pos.ay = NAN;
pos.az = NAN;
pos.yaw = matrix::Eulerf(matrix::Quatf(att.q)).psi();
pos.dist_bottom_valid = dist_bottom_valid;
pos.eph = eph;
pos.epv = epv;
pos.evh = 0.0f;
pos.evv = 0.0f;
pos.vxy_max = INFINITY;
pos.vz_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
local_pos.z_deriv = z_est[1];
pos.z_deriv = z_est[1];
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = dist_ground;
local_pos.dist_bottom_rate = - z_est[1];
if (pos.dist_bottom_valid) {
pos.dist_bottom = dist_ground;
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 */
global_pos.timestamp = t;
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.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_e = local_pos.vy;
global_pos.vel_d = local_pos.vz;
global_pos.vel_n = pos.vx;
global_pos.vel_e = pos.vy;
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.epv = epv;