Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

vehicle_global_position remove redundant evh and evv #9018

Merged
merged 1 commit into from
Mar 5, 2018
Merged

vehicle_global_position remove redundant evh and evv #9018

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions msg/vehicle_global_position.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 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)
bool terrain_alt_valid # Terrain altitude estimate is valid
Expand Down
1 change: 0 additions & 1 deletion msg/vehicle_status_flags.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_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_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_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
Expand Down
10 changes: 0 additions & 10 deletions src/modules/commander/commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
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 lvel_probation_time_us = POSVEL_PROBATION_MIN;

Expand Down Expand Up @@ -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_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_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)

Expand Down Expand Up @@ -1332,7 +1330,6 @@ Commander::run()

/* Set position and velocity validty to 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_velocity_valid = false;
status_flags.condition_local_altitude_valid = false;
Expand Down Expand Up @@ -1510,7 +1507,6 @@ Commander::run()
last_lpos_fail_time_us = commander_boot_timestamp;
last_gpos_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
int32_t rc_in_off = 0;
Expand Down Expand Up @@ -1967,7 +1963,6 @@ Commander::run()
// This is necessary because the global position message is by definition valid if published.
if ((hrt_absolute_time() - gpos_last_update_time_us) > 1000000) {
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
Expand Down Expand Up @@ -2021,11 +2016,9 @@ Commander::run()
// If nav is failed, then declare local position and velocity as invalid
if (nav_test_failed) {
status_flags.condition_global_position_valid = false;
status_flags.condition_global_velocity_valid = false;
} else {
// 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.evh, evh_threshold, global_position.timestamp, &last_gvel_fail_time_us, &gvel_probation_time_us, &status_flags.condition_global_velocity_valid, &status_changed);
}
}
}
Expand Down Expand Up @@ -2072,7 +2065,6 @@ Commander::run()
// 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.
gpos_probation_time_us = posctl_nav_loss_prob;
gvel_probation_time_us = posctl_nav_loss_prob;
lpos_probation_time_us = posctl_nav_loss_prob;
lvel_probation_time_us = posctl_nav_loss_prob;
}
Expand Down Expand Up @@ -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
gpos_probation_time_us = POSVEL_PROBATION_MIN;
gvel_probation_time_us = POSVEL_PROBATION_MIN;
lpos_probation_time_us = POSVEL_PROBATION_MIN;
lvel_probation_time_us = POSVEL_PROBATION_MIN;

// 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->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->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);
}
Expand Down
2 changes: 1 addition & 1 deletion src/modules/commander/state_machine_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -966,7 +966,7 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
{
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;
} else if (!using_global_pos && (!status_flags->condition_local_position_valid || !status_flags->condition_local_velocity_valid)) {
fallback_required = true;
Expand Down
2 changes: 0 additions & 2 deletions src/modules/ekf2/ekf2_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1051,8 +1051,6 @@ void Ekf2::run()
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);
global_pos.evh = lpos.evh;
global_pos.evv = lpos.evv;

global_pos.terrain_alt_valid = lpos.dist_bottom_valid;

Expand Down
3 changes: 0 additions & 3 deletions src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -729,9 +729,6 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
_pub_gpos.update();
// TODO provide calculated values for these
_pub_gpos.get().evh = 0.0f;
_pub_gpos.get().evv = 0.0f;
}
}

Expand Down
4 changes: 0 additions & 4 deletions src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1397,10 +1397,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.eph = eph;
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()) {
global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground();
global_pos.terrain_alt_valid = true;
Expand Down