zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

position_estimator_inav: add vehicle_odometry usage; improve inout interface

sbg
TSC21 7 years ago committed by Lorenz Meier
parent
183a63cce9
commit
cce36e69c8
1 changed files with 182 additions and 106 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 288
      src/modules/position_estimator_inav/position_estimator_inav_main.cpp

288
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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;
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_valid) {
x_est[0] = vision.x;
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
/* 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;
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");
}
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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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[]) @@ -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;

Write Preview
Loading…
Cancel
Save