From cce36e69c85d7482fed7cf079b340a50aa8f003e Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 12 Jul 2018 19:27:04 +0100 Subject: [PATCH] position_estimator_inav: add vehicle_odometry usage; improve inout interface --- .../position_estimator_inav_main.cpp | 288 +++++++++++------- 1 file changed, 182 insertions(+), 106 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 63c1899931..8090fcb92d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -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 #include #include -#include #include #include #include @@ -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;