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

preflightcheck EKF2 GPS requirement after 20s

This commit is contained in:
Daniel Agar 2017-08-11 01:27:08 -04:00
parent 9a9923c517
commit 2495f8942b
2 changed files with 23 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -172,8 +172,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
sensor_preflight_s sensors = {};
if ((sensors_sub == -1) ||
(orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK)) {
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) {
goto out;
}
@ -404,7 +403,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
differential_pressure_s differential_pressure = {};
if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) ||
(hrt_elapsed_time(&differential_pressure.timestamp) > (500 * 1000))) {
(hrt_elapsed_time(&differential_pressure.timestamp) > 1000000)) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
@ -413,7 +412,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
}
if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
(hrt_elapsed_time(&airspeed.timestamp) > 1000000)) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
@ -457,7 +456,6 @@ out:
static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
{
bool success = true;
int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
//Wait up to 2000ms to allow the driver to detect a GNSS receiver module
@ -497,8 +495,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
int sub = orb_subscribe(ORB_ID(estimator_status));
estimator_status_s status = {};
if ((sub == -1) ||
(orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK)) {
if (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK) {
goto out;
}
@ -534,7 +531,8 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
// If GPS aiding is required, declare fault condition if the EKF is not using GPS
if (enforce_gps_required) {
if (!(status.control_mode_flags & (1 << 2))) {
bool ekf_gps_fusion = status.control_mode_flags & (1 << 2);
if (!ekf_gps_fusion) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
}
@ -595,12 +593,17 @@ out:
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot)
{
if (time_since_boot < 2000000) {
// the airspeed driver filter doesn't deliver the actual value yet
return true;
}
#ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when
// all the sensors are supported
PX4_WARN("Preflight checks always pass on Snapdragon.");
checkSensors = false;
return true;
#elif defined(__PX4_POSIX_RPI)
PX4_WARN("Preflight checks for mag, acc, gyro always pass on RPI");
checkSensors = false;
@ -738,7 +741,11 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
}
/* ---- IMU CONSISTENCY ---- */
imuConsistencyCheck(mavlink_log_pub, reportFailures);
if (checkSensors) {
if (!imuConsistencyCheck(mavlink_log_pub, reportFailures)) {
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
@ -769,7 +776,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
int32_t estimator_type;
param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
if (estimator_type == 2 && checkGNSS) {
if (!ekf2Check(mavlink_log_pub, true, reportFailures, checkGNSS)) {
// don't require GPS for the first 20s
bool enforce_gps_required = (time_since_boot > 20 * 1000000);
if (!ekf2Check(mavlink_log_pub, true, reportFailures, enforce_gps_required)) {
failed = true;
}
}

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

@ -1706,7 +1706,7 @@ int commander_thread_main(int argc, char *argv[])
int32_t arm_without_gps_param = 0;
param_get(_param_arm_without_gps, &arm_without_gps_param);
arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
int32_t arm_mission_required_param = 0;
param_get(_param_arm_mission_required, &arm_mission_required_param);
@ -1722,7 +1722,6 @@ int commander_thread_main(int argc, char *argv[])
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check,
false, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
@ -1865,7 +1864,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_arm_switch_is_button, &arm_switch_is_button);
param_get(_param_arm_without_gps, &arm_without_gps_param);
arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
param_get(_param_arm_mission_required, &arm_mission_required_param);
arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));