diff --git a/msg/airspeed.msg b/msg/airspeed.msg index 744f9d8830..b0ca817043 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -3,3 +3,4 @@ float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown float32 confidence # confidence value from 0 to 1 for this sensor +float32 differential_pressure_filtered_pa # filtered differential pressure, can be negative diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index ba36de31a3..930da0180a 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -363,7 +363,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt return success; } -static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail) +static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm, hrt_abstime time_since_boot) { bool success = true; int ret; @@ -388,11 +388,24 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep goto out; } - if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { + /** + * Check if differential pressure is off by more than 15Pa which equals to 5m/s when measuring no airspeed. + * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover + * might have been removed. + */ + + if (time_since_boot < 1e6) { + // the airspeed driver filter doesn't deliver the actual value yet + success = false; + goto out; + } + + if (fabsf(airspeed.differential_pressure_filtered_pa) > 15.0f && !prearm) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED CALIBRATION ISSUE"); } - // XXX do not make this fatal yet + success = false; + goto out; } out: @@ -511,7 +524,8 @@ out: } bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL, bool reportFailures) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, + bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot) { #ifdef __PX4_QURT @@ -651,7 +665,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, /* ---- AIRSPEED ---- */ if (checkAirspeed) { - if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) { + if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm, time_since_boot)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 2c59923dbf..4561932328 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -39,6 +39,8 @@ * @author Johan Jansen */ +#include + #pragma once namespace Commander @@ -67,7 +69,8 @@ namespace Commander * true if the GNSS receiver should be checked **/ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL, bool reportFailures = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, + bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71fc29ac89..e92a7e9bad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -401,9 +401,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false); + checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -663,7 +663,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co mavlink_log_pub_local, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps); + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp)); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -1634,7 +1635,8 @@ int commander_thread_main(int argc, char *argv[]) bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + if (!status_flags.circuit_breaker_engaged_airspd_check && + (!status.is_rotary_wing || status.is_vtol)) { checkAirspeed = true; } @@ -1654,8 +1656,8 @@ int commander_thread_main(int argc, char *argv[]) } else { // sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !can_arm_without_gps, /*checkDynamic */ false, is_vtol(&status), /* reportFailures */ false); + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, + /* checkDynamic */ false, is_vtol(&status), /* reportFailures */ false, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1894,25 +1896,19 @@ int commander_thread_main(int argc, char *argv[]) /* and the system is not already armed (and potentially flying) */ !armed.armed) { - bool chAirspeed = false; hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing - */ - if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { - chAirspeed = true; - } - /* provide RC and sensor status feedback to the user */ if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), /* checkGNSS */ false, /* checkDynamic */ true, /* reportFailures */ false); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, + /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ false, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); } else { /* check sensors also */ - (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout); + (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, + /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); } } @@ -2022,7 +2018,8 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps)) { + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp))) { mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch"); arming_state_changed = true; } @@ -2322,7 +2319,8 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps); + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2583,7 +2581,8 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps); + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2631,7 +2630,8 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps); + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -3920,7 +3920,8 @@ void *commander_low_prio_loop(void *arg) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps)) { + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp))) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { @@ -3999,12 +4000,14 @@ void *commander_low_prio_loop(void *arg) bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status_flags.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + if (!status_flags.circuit_breaker_engaged_airspd_check && + (!status.is_rotary_wing || status.is_vtol)) { checkAirspeed = true; } status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, /* checkDynamic */ true, hotplug_timeout); + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status_flags.circuit_breaker_engaged_gpsfailure_check, + /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); arming_state_transition(&status, &battery, @@ -4015,7 +4018,8 @@ void *commander_low_prio_loop(void *arg) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - can_arm_without_gps); + can_arm_without_gps, + hrt_elapsed_time(&commander_boot_timestamp)); } else { tune_negative(true); diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 27756ffea1..19a5a96e21 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -293,10 +293,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Attempt transition transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed, - false /* no pre-arm checks */, - nullptr /* no mavlink_log_pub */, - &status_flags, - 5.0f, check_gps); + false /* no pre-arm checks */, + nullptr /* no mavlink_log_pub */, + &status_flags, + 5.0f, check_gps, 2e6); // Validate result of transition ut_compare(test->assertMsg, test->expected_transition_result, result); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1f9302e655..dfdfd04a1c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -126,7 +126,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage, - bool can_arm_without_gps) + bool can_arm_without_gps, + hrt_abstime time_since_boot) { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); @@ -152,7 +153,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, - status_flags, battery, can_arm_without_gps); + status_flags, battery, can_arm_without_gps, time_since_boot); } /* re-run the pre-flight check as long as sensors are failing */ @@ -163,7 +164,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */, - status_flags, battery, can_arm_without_gps); + status_flags, battery, can_arm_without_gps, time_since_boot); status_flags->condition_system_sensors_initialized = !prearm_ret; last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -1102,7 +1103,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in } int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, - status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps) + status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps, hrt_abstime time_since_boot) { /* */ @@ -1119,7 +1120,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true, checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !can_arm_without_gps, true, status->is_vtol, reportFailures); + !can_arm_without_gps, true, status->is_vtol, reportFailures, prearm, time_since_boot); if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) { preflight_ok = false; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index c0119ece3c..0dce9ef9b4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -42,6 +42,8 @@ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ +#include + #include #include #include @@ -106,7 +108,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage, - bool can_arm_without_gps); + bool can_arm_without_gps, + hrt_abstime time_since_boot); transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, @@ -124,6 +127,8 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in const bool stay_in_failsafe, status_flags_s *status_flags, bool landed, const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act); -int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps); +int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, + bool force_report, status_flags_s *status_flags, battery_status_s *battery, + bool can_arm_without_gps, hrt_abstime time_since_boot); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8d7bd91ebd..b0f14684c1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1409,6 +1409,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _last_best_baro_pressure * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; + _airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub != nullptr) {