fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 17:49:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Airspeed: preflight check for bad offset, fixed calls to preflight checks (vtol & airspeed)

Browse Source
This commit is contained in:
Andreas Antener 2016-11-14 00:15:20 +01:00 committed by Lorenz Meier
parent f092b31f54
commit f772fc2d02
8 changed files with 72 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/airspeed.msg
View File

@ -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

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

@ -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;
}
}

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

@ -39,6 +39,8 @@
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
#include <drivers/drv_hrt.h>
#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;

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

@ -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);

8
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -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);

11
src/modules/commander/state_machine_helper.cpp
View File

@ -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;

9
src/modules/commander/state_machine_helper.h
View File

@ -42,6 +42,8 @@
#ifndef STATE_MACHINE_HELPER_H_
#define STATE_MACHINE_HELPER_H_
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
@ -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_ */

1
src/modules/sensors/sensors.cpp
View File

@ -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) {