vehicle_global_position remove redundant evh and evv

- vehicle_status_flags condition_global_velocity_valid is also unnecessary
This commit is contained in:
Daniel Agar 2018-03-05 02:09:37 -05:00
parent 03b8cd78b3
commit 458db2e508
7 changed files with 1 additions and 23 deletions

View File

@ -21,8 +21,6 @@ float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (r
float32 eph # Standard deviation of horizontal position error, (metres) float32 eph # Standard deviation of horizontal position error, (metres)
float32 epv # Standard deviation of vertical position error, (metres) float32 epv # Standard deviation of vertical position error, (metres)
float32 evh # Standard deviation of horizontal velocity error, (metres/sec)
float32 evv # Standard deviation of horizontal velocity error, (metres/sec)
float32 terrain_alt # Terrain altitude WGS84, (metres) float32 terrain_alt # Terrain altitude WGS84, (metres)
bool terrain_alt_valid # Terrain altitude estimate is valid bool terrain_alt_valid # Terrain altitude estimate is valid

View File

@ -7,7 +7,6 @@ bool condition_system_hotplug_timeout # true if the hotplug sensor search is ov
bool condition_system_returned_to_home bool condition_system_returned_to_home
bool condition_auto_mission_available bool condition_auto_mission_available
bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
bool condition_global_velocity_valid # set to true by the commander app if the quality of the global horizontal velocity data is good enough to use for navigation
bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation

View File

@ -165,7 +165,6 @@ static int32_t posctl_nav_loss_gain = 10; /**< the rate at which the probation d
// Probation times for position and velocity validity checks to pass if failed // Probation times for position and velocity validity checks to pass if failed
static uint64_t gpos_probation_time_us = POSVEL_PROBATION_MIN; static uint64_t gpos_probation_time_us = POSVEL_PROBATION_MIN;
static uint64_t gvel_probation_time_us = POSVEL_PROBATION_MIN;
static uint64_t lpos_probation_time_us = POSVEL_PROBATION_MIN; static uint64_t lpos_probation_time_us = POSVEL_PROBATION_MIN;
static uint64_t lvel_probation_time_us = POSVEL_PROBATION_MIN; static uint64_t lvel_probation_time_us = POSVEL_PROBATION_MIN;
@ -193,7 +192,6 @@ static float evh_threshold = 1.0f; // Horizontal velocity error threshold (m)
static hrt_abstime last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec) static hrt_abstime last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec)
static hrt_abstime last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec) static hrt_abstime last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec)
static hrt_abstime last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec) static hrt_abstime last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec)
static hrt_abstime last_gvel_fail_time_us = 0; // Last time that the global velocity validity recovery check failed (usec)
static hrt_abstime gpos_last_update_time_us = 0; // last time a global position update was received (usec) static hrt_abstime gpos_last_update_time_us = 0; // last time a global position update was received (usec)
@ -1332,7 +1330,6 @@ Commander::run()
/* Set position and velocity validty to false */ /* Set position and velocity validty to false */
status_flags.condition_global_position_valid = false; status_flags.condition_global_position_valid = false;
status_flags.condition_global_velocity_valid = false;
status_flags.condition_local_position_valid = false; status_flags.condition_local_position_valid = false;
status_flags.condition_local_velocity_valid = false; status_flags.condition_local_velocity_valid = false;
status_flags.condition_local_altitude_valid = false; status_flags.condition_local_altitude_valid = false;
@ -1510,7 +1507,6 @@ Commander::run()
last_lpos_fail_time_us = commander_boot_timestamp; last_lpos_fail_time_us = commander_boot_timestamp;
last_gpos_fail_time_us = commander_boot_timestamp; last_gpos_fail_time_us = commander_boot_timestamp;
last_lvel_fail_time_us = commander_boot_timestamp; last_lvel_fail_time_us = commander_boot_timestamp;
last_gvel_fail_time_us = commander_boot_timestamp;
// Run preflight check // Run preflight check
int32_t rc_in_off = 0; int32_t rc_in_off = 0;
@ -1967,7 +1963,6 @@ Commander::run()
// This is necessary because the global position message is by definition valid if published. // This is necessary because the global position message is by definition valid if published.
if ((hrt_absolute_time() - gpos_last_update_time_us) > 1000000) { if ((hrt_absolute_time() - gpos_last_update_time_us) > 1000000) {
status_flags.condition_global_position_valid = false; status_flags.condition_global_position_valid = false;
status_flags.condition_global_velocity_valid = false;
} }
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure /* Check estimator status for signs of bad yaw induced post takeoff navigation failure
@ -2021,11 +2016,9 @@ Commander::run()
// If nav is failed, then declare local position and velocity as invalid // If nav is failed, then declare local position and velocity as invalid
if (nav_test_failed) { if (nav_test_failed) {
status_flags.condition_global_position_valid = false; status_flags.condition_global_position_valid = false;
status_flags.condition_global_velocity_valid = false;
} else { } else {
// use global position message to determine validity // use global position message to determine validity
check_posvel_validity(true, global_position.eph, eph_threshold, global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed); check_posvel_validity(true, global_position.eph, eph_threshold, global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed);
check_posvel_validity(true, global_position.evh, evh_threshold, global_position.timestamp, &last_gvel_fail_time_us, &gvel_probation_time_us, &status_flags.condition_global_velocity_valid, &status_changed);
} }
} }
} }
@ -2072,7 +2065,6 @@ Commander::run()
// This is a larger value to give the vehicle time to complete a failsafe landing // This is a larger value to give the vehicle time to complete a failsafe landing
// if faulty sensors cause loss of navigation shortly after takeoff. // if faulty sensors cause loss of navigation shortly after takeoff.
gpos_probation_time_us = posctl_nav_loss_prob; gpos_probation_time_us = posctl_nav_loss_prob;
gvel_probation_time_us = posctl_nav_loss_prob;
lpos_probation_time_us = posctl_nav_loss_prob; lpos_probation_time_us = posctl_nav_loss_prob;
lvel_probation_time_us = posctl_nav_loss_prob; lvel_probation_time_us = posctl_nav_loss_prob;
} }
@ -3614,13 +3606,11 @@ reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_
{ {
// reset all the check probation times back to the minimum value // reset all the check probation times back to the minimum value
gpos_probation_time_us = POSVEL_PROBATION_MIN; gpos_probation_time_us = POSVEL_PROBATION_MIN;
gvel_probation_time_us = POSVEL_PROBATION_MIN;
lpos_probation_time_us = POSVEL_PROBATION_MIN; lpos_probation_time_us = POSVEL_PROBATION_MIN;
lvel_probation_time_us = POSVEL_PROBATION_MIN; lvel_probation_time_us = POSVEL_PROBATION_MIN;
// recheck validity // recheck validity
check_posvel_validity(true, global_position->eph, eph_threshold, global_position->timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed); check_posvel_validity(true, global_position->eph, eph_threshold, global_position->timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed);
check_posvel_validity(true, global_position->evh, evh_threshold, global_position->timestamp, &last_gvel_fail_time_us, &gvel_probation_time_us, &status_flags.condition_global_velocity_valid, changed);
check_posvel_validity(local_position->xy_valid, local_position->eph, eph_threshold, local_position->timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed); check_posvel_validity(local_position->xy_valid, local_position->eph, eph_threshold, local_position->timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
check_posvel_validity(local_position->v_xy_valid, local_position->evh, evh_threshold, local_position->timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed); check_posvel_validity(local_position->v_xy_valid, local_position->evh, evh_threshold, local_position->timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed);
} }

View File

@ -966,7 +966,7 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
{ {
bool fallback_required = false; bool fallback_required = false;
if (using_global_pos && (!status_flags->condition_global_position_valid || !status_flags->condition_global_velocity_valid)) { if (using_global_pos && !status_flags->condition_global_position_valid) {
fallback_required = true; fallback_required = true;
} else if (!using_global_pos && (!status_flags->condition_local_position_valid || !status_flags->condition_local_velocity_valid)) { } else if (!using_global_pos && (!status_flags->condition_local_position_valid || !status_flags->condition_local_velocity_valid)) {
fallback_required = true; fallback_required = true;

View File

@ -1051,8 +1051,6 @@ void Ekf2::run()
global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI. global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI.
_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv, &global_pos.dead_reckoning); _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv, &global_pos.dead_reckoning);
global_pos.evh = lpos.evh;
global_pos.evv = lpos.evv;
global_pos.terrain_alt_valid = lpos.dist_bottom_valid; global_pos.terrain_alt_valid = lpos.dist_bottom_valid;

View File

@ -729,9 +729,6 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ; _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY); _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
_pub_gpos.update(); _pub_gpos.update();
// TODO provide calculated values for these
_pub_gpos.get().evh = 0.0f;
_pub_gpos.get().evv = 0.0f;
} }
} }

View File

@ -1397,10 +1397,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.eph = eph; global_pos.eph = eph;
global_pos.epv = epv; global_pos.epv = epv;
// TODO provide calculated values for these
global_pos.evh = 0.0f;
global_pos.evv = 0.0f;
if (terrain_estimator.is_valid()) { if (terrain_estimator.is_valid()) {
global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground(); global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground();
global_pos.terrain_alt_valid = true; global_pos.terrain_alt_valid = true;