dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: Only copy global position is valid. This is because the app assumed that it only gets published once valid.

This commit is contained in:
Lorenz Meier 2015-07-04 10:45:01 +02:00
parent 1da72df72d
commit b27b864cf0
1 changed files with 21 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/commander/commander.cpp
View File

@ -1479,7 +1479,22 @@ int commander_thread_main(int argc, char *argv[])
if (updated) { if (updated) {
/* position changed */ /* position changed */
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); vehicle_global_position_s gpos;
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
/* copy to global struct if valid, with hysteresis */
// XXX consolidate this with local position handling and timeouts after release
// but we want a low-risk change now.
if (status.condition_global_position_valid) {
if (gpos.eph < eph_threshold * 2.5f) {
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
} else {
if (gpos.eph < eph_threshold) {
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
}
} }
/* update local position estimate */ /* update local position estimate */
@ -1492,17 +1507,16 @@ int commander_thread_main(int argc, char *argv[])
//update condition_global_position_valid //update condition_global_position_valid
//Global positions are only published by the estimators if they are valid //Global positions are only published by the estimators if they are valid
if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
//We have had no good fix for POSITION_TIMEOUT amount of time //We have had no good fix for POSITION_TIMEOUT amount of time
if(status.condition_global_position_valid) { if (status.condition_global_position_valid) {
set_tune_override(TONE_GPS_WARNING_TUNE); set_tune_override(TONE_GPS_WARNING_TUNE);
status_changed = true; status_changed = true;
status.condition_global_position_valid = false; status.condition_global_position_valid = false;
} }
} } else if (global_position.timestamp != 0) {
else if(global_position.timestamp != 0) { // Got good global position estimate
//Got good global position estimate if (!status.condition_global_position_valid) {
if(!status.condition_global_position_valid) {
status_changed = true; status_changed = true;
status.condition_global_position_valid = true; status.condition_global_position_valid = true;
} }