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

Commander: Add preflight check for missing GPS module

This commit is contained in:
Johan Jansen 2015-05-14 19:19:30 +02:00
parent c0a9e08a30
commit 45f1fd6634
3 changed files with 34 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/commander/PreflightCheck.cpp
View File

@ -61,6 +61,7 @@
#include <drivers/drv_airspeed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
@ -269,8 +270,24 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd)
{
bool success = true;
int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps;
if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) ||
(hrt_elapsed_time(&gps.timestamp_position) > 500000)) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
success = false;
}
close(gpsSub);
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
{
bool failed = false;
@ -336,6 +353,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if(checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
failed = true;
}
}
/* Report status */
return !failed;
}

6
src/modules/commander/PreflightCheck.h
View File

@ -59,11 +59,15 @@ namespace Commander
* true if the gyroscopes should be checked
* @param checkBaro
* true if the barometer should be checked
* @param checkAirspeed
* true if the airspeed sensor should be checked
* @param checkRC
* true if the Remote Controller should be checked
* @param checkGNSS
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;

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

@ -916,6 +916,7 @@ int commander_thread_main(int argc, char *argv[])
status.circuit_breaker_engaged_airspd_check = false;
status.circuit_breaker_engaged_enginefailure_check = false;
status.circuit_breaker_engaged_gpsfailure_check = false;
get_circuit_breaker_params();
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@ -1117,8 +1118,6 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_sys_type, &(status.system_type)); // get system type
status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
get_circuit_breaker_params();
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@ -1127,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[])
}
// Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
@ -1300,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* provide RC and sensor status feedback to the user */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true);
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@ -2749,7 +2748,7 @@ void *commander_low_prio_loop(void *arg)
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);