fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 17:30:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+29 -25
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);