fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 16:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Fix GPS loss not handled properly

Browse Source
This commit is contained in:
Johan Jansen
2015-02-11 16:42:01 +01:00
parent 7f7c36b5b1
commit dc522f217f
3 changed files with 21 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+15 -20
View File
@@ -1388,30 +1388,25 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
} }
/* update condition_global_position_valid */ //update condition_global_position_valid
/* hysteresis for EPH/EPV */ //Global positions are only published by the estimators if they are valid
bool eph_good; if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
//We have had no good fix for POSITION_TIMEOUT amount of time
if (status.condition_global_position_valid) { if(status.condition_global_position_valid) {
if (global_position.eph > eph_threshold * 2.5f) { set_tune_override(TONE_GPS_WARNING_TUNE);
eph_good = false; status_changed = true;
} else {
eph_good = true;
} }
} else { status.condition_global_position_valid = false;
if (global_position.eph < eph_threshold) { }
eph_good = true; else if(hrt_absolute_time() > POSITION_TIMEOUT) {
//Got good global position estimate
} else { if(!status.condition_global_position_valid) {
eph_good = false; status_changed = true;
status.condition_global_position_valid = true;
} }
} }
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
&status_changed);
/* update condition_local_position_valid and condition_local_altitude_valid */ /* update condition_local_position_valid and condition_local_altitude_valid */
/* hysteresis for EPH */ /* hysteresis for EPH */
bool local_eph_good; bool local_eph_good;
@@ -2119,7 +2114,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
} else { } else {
if (status_local->condition_local_position_valid) { if (status_local->condition_global_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_color(RGBLED_COLOR_GREEN);
} else { } else {
src/modules/commander/commander_helper.cpp
+5
View File
@@ -115,6 +115,11 @@ void buzzer_deinit()
close(buzzer); close(buzzer);
} }
void set_tune_override(int tune)
{
ioctl(buzzer, TONE_SET_ALARM, tune);
}
void set_tune(int tune) void set_tune(int tune)
{ {
unsigned int new_tune_duration = tune_durations[tune]; unsigned int new_tune_duration = tune_durations[tune];
src/modules/commander/commander_helper.h
+1
View File
@@ -55,6 +55,7 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
int buzzer_init(void); int buzzer_init(void);
void buzzer_deinit(void); void buzzer_deinit(void);
void set_tune_override(int tune);
void set_tune(int tune); void set_tune(int tune);
void tune_positive(bool use_buzzer); void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer); void tune_neutral(bool use_buzzer);