diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp new file mode 100644 index 0000000000..815c9361db --- /dev/null +++ b/src/modules/commander/Commander.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef COMMANDER_HPP_ +#define COMMANDER_HPP_ + +#include +#include + +// publications +#include +#include +#include +#include +#include +#include +#include + +// subscriptions +#include +#include +#include +#include +#include +#include +#include + +using control::BlockParamFloat; +using control::BlockParamInt; +using uORB::Publication; +using uORB::Subscription; + +class Commander : public control::SuperBlock, public ModuleBase +{ +public: + Commander() : + SuperBlock(nullptr, "COM") + { + updateParams(); + } + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static Commander *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + void enable_hil(); + +private: + + bool handle_command(vehicle_status_s *status, const safety_s *safety, vehicle_command_s *cmd, + actuator_armed_s *armed, home_position_s *home, vehicle_global_position_s *global_pos, + vehicle_local_position_s *local_pos, vehicle_attitude_s *attitude, orb_advert_t *home_pub, + orb_advert_t *command_ack_pub, bool *changed); + + bool set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude, bool set_alt_only_to_lpos_ref); + +}; + +#endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 5fb03515d1..3e6dfd0711 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -75,7 +75,7 @@ using namespace DriverFramework; -namespace Commander +namespace Preflight { static int check_calibration(DevHandle &h, const char *param_template, int &devid) diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index d9b1929f01..12105240b2 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -43,7 +43,7 @@ #pragma once -namespace Commander +namespace Preflight { /** * Runs a preflight check on all sensors to see if they are properly calibrated and healthy diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04a8ed9fc7..c181975aed 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -36,14 +36,10 @@ * * Main state machine / business logic * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * @author Anton Babushkin - * @author Sander Smeets */ +#include "Commander.hpp" + #include // NAN /* commander module headers */ @@ -104,7 +100,6 @@ #include #include #include -#include #include #include #include @@ -184,10 +179,9 @@ static orb_advert_t mavlink_log_pub = nullptr; static orb_advert_t power_button_state_pub = nullptr; /* flags */ -static bool commander_initialized = false; static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ + static bool _usb_telemetry_active = false; static hrt_abstime commander_boot_timestamp = 0; @@ -276,19 +270,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]); */ void usage(const char *reason); -/** - * React to commands that are sent e.g. from the mavlink module. - */ -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, - struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, - orb_advert_t *command_ack_pub, bool *changed); - -/** - * Mainloop of commander. - */ -int commander_thread_main(int argc, char *argv[]); - void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery_local, const cpuload_s *cpuload_local); @@ -327,15 +308,6 @@ void print_status(); transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); -/** -* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each -* time the vehicle is armed with a good GPS fix. -**/ -static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, - const vehicle_attitude_s &attitude, - bool set_alt_only_to_lpos_ref); - /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -399,12 +371,8 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = px4_task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 40, - 3700, - commander_thread_main, - (char * const *)&argv[0]); + + Commander::main(argc, argv); unsigned constexpr max_wait_us = 1000000; unsigned constexpr max_wait_steps = 2000; @@ -429,10 +397,7 @@ int commander_main(int argc, char *argv[]) thread_should_exit = true; - while (thread_running) { - usleep(200000); - warnx("."); - } + Commander::main(argc, argv); warnx("terminated."); @@ -761,10 +726,11 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co return arming_res; } -bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, - struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, +bool +Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety_local, + vehicle_command_s *cmd, actuator_armed_s *armed_local, + home_position_s *home, vehicle_global_position_s *global_pos, + vehicle_local_position_s *local_pos, vehicle_attitude_s *attitude, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed) { /* only handle commands that are meant to be handled by this system and component */ @@ -974,7 +940,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !home->manual_home) { - commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false); + set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false); } } } @@ -1043,7 +1009,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (use_current) { /* use current position */ - if (commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false)) { + if (set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1250,10 +1216,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. **/ -static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &home, +bool +Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, - const vehicle_attitude_s &attitude, - bool set_alt_only_to_lpos_ref) + const vehicle_attitude_s &attitude, bool set_alt_only_to_lpos_ref) { if (!set_alt_only_to_lpos_ref) { //Need global and local position fix to be able to set home @@ -1312,11 +1278,9 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s & return true; } -int commander_thread_main(int argc, char *argv[]) +void +Commander::run() { - /* not yet initialized */ - commander_initialized = false; - bool sensor_fail_tune_played = false; bool arm_tune_played = false; bool was_landed = true; @@ -1326,26 +1290,11 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status_flags.condition_system_sensors_initialized = true; -#ifdef __PX4_NUTTX - /* NuttX indicates 3 arguments when only 2 are present */ - argc -= 1; - argv += 1; -#endif - /* vehicle status topic */ status = {}; status.hil_state = vehicle_status_s::HIL_STATE_OFF; - if (argc > 2) { - if (!strcmp(argv[2],"--hil")) { - status.hil_state = vehicle_status_s::HIL_STATE_ON; - } else { - PX4_ERR("Argument %s not supported, abort.", argv[2]); - thread_should_exit = true; - } - } - /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); @@ -1397,40 +1346,6 @@ int commander_thread_main(int argc, char *argv[]) /* failsafe response to loss of navigation accuracy */ param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL"); - // These are too verbose, but we will retain them a little longer - // until we are sure we really don't need them. - - // const char *main_states_str[commander_state_s::MAIN_STATE_MAX]; - // main_states_str[commander_state_s::MAIN_STATE_MANUAL] = "MANUAL"; - // main_states_str[commander_state_s::MAIN_STATE_ALTCTL] = "ALTCTL"; - // main_states_str[commander_state_s::MAIN_STATE_POSCTL] = "POSCTL"; - // main_states_str[commander_state_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - // main_states_str[commander_state_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - // main_states_str[commander_state_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - // main_states_str[commander_state_s::MAIN_STATE_ACRO] = "ACRO"; - // main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB"; - // main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; - - // const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LAND] = "LAND"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - // nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; - /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1567,8 +1482,6 @@ int commander_thread_main(int argc, char *argv[]) orb_unadvertise(mission_pub); } - int ret; - /* Start monitoring loop */ unsigned counter = 0; unsigned stick_off_counter = 0; @@ -1678,11 +1591,6 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - /* Subscribe to position setpoint triplet */ - int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - struct position_setpoint_triplet_s pos_sp_triplet; - memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); - /* Subscribe to system power */ int system_power_sub = orb_subscribe(ORB_ID(system_power)); struct system_power_s system_power; @@ -1724,8 +1632,6 @@ int commander_thread_main(int argc, char *argv[]) posctl_nav_loss_prob = init_param_val * sec_to_usec; // convert to uSec param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain); - /* now initialized */ - commander_initialized = true; thread_running = true; /* update vehicle status to find out vehicle type (required for preflight checks) */ @@ -1769,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[]) set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } 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, + status_flags.condition_system_sensors_initialized = Preflight::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 @@ -1835,7 +1741,7 @@ int commander_thread_main(int argc, char *argv[]) arm_auth_init(&mavlink_log_pub, &status.system_id); - while (!thread_should_exit) { + while (!should_exit()) { arming_ret = TRANSITION_NOT_CHANGED; @@ -1907,8 +1813,7 @@ int commander_thread_main(int argc, char *argv[]) // After that it will be set in the main state // machine based on the arming state. if (param_init_forced) { - auto_disarm_hysteresis.set_hysteresis_time_from(false, - (hrt_abstime)disarm_when_landed * 1000000); + auto_disarm_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)disarm_when_landed * 1000000); } param_get(_param_low_bat_act, &low_bat_action); @@ -2037,12 +1942,12 @@ int commander_thread_main(int argc, char *argv[]) /* provide RC and sensor status feedback to the user */ if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { /* HITL configuration: check only RC input */ - (void)Commander::preflightCheck(&mavlink_log_pub, false, false, + Preflight::preflightCheck(&mavlink_log_pub, false, false, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp)); } else { /* check sensors also */ - (void)Commander::preflightCheck(&mavlink_log_pub, true, checkAirspeed, + Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); } @@ -2512,13 +2417,6 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - /* update position setpoint triplet */ - orb_check(pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); - } - /* If in INIT state, try to proceed to STANDBY state */ if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, @@ -3145,7 +3043,7 @@ int commander_thread_main(int argc, char *argv[]) (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); + set_home_position(home_pub, _home, local_position, global_position, attitude, false); } } else { if (status_flags.condition_home_position_valid) { @@ -3159,12 +3057,12 @@ int commander_thread_main(int argc, char *argv[]) if (home_dist_xy > local_position.epv * 2 || home_dist_z > local_position.eph * 2) { /* update when disarmed, landed and moved away from current home position */ - commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); + set_home_position(home_pub, _home, local_position, global_position, attitude, false); } } } else { /* First time home position update - but only if disarmed */ - commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); + set_home_position(home_pub, _home, local_position, global_position, attitude, false); } } @@ -3172,7 +3070,7 @@ int commander_thread_main(int argc, char *argv[]) * This allows home atitude to be used in the calculation of height above takeoff location when GPS * use has commenced after takeoff. */ if (!_home.valid_alt && local_position.z_global) { - commander_set_home_position(home_pub, _home, local_position, global_position, attitude, true); + set_home_position(home_pub, _home, local_position, global_position, attitude, true); } } @@ -3307,7 +3205,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update timeout flag */ - if(!(hotplug_timeout == status_flags.condition_system_hotplug_timeout)) { + if (!(hotplug_timeout == status_flags.condition_system_hotplug_timeout)) { status_flags.condition_system_hotplug_timeout = hotplug_timeout; status_changed = true; } @@ -3340,8 +3238,10 @@ int commander_thread_main(int argc, char *argv[]) usleep(COMMANDER_MONITORING_INTERVAL); } + thread_should_exit = true; + /* wait for threads to complete */ - ret = pthread_join(commander_low_prio_thread, nullptr); + int ret = pthread_join(commander_low_prio_thread, nullptr); if (ret) { warn("join failed: %d", ret); @@ -3368,8 +3268,6 @@ int commander_thread_main(int argc, char *argv[]) px4_close(estimator_status_sub); thread_running = false; - - return 0; } void @@ -3983,7 +3881,7 @@ check_posvel_validity(bool data_valid, float data_accuracy, float required_accur } else { if (*probation_time_us < POSVEL_PROBATION_MIN) { *probation_time_us = POSVEL_PROBATION_MIN; - } else if (*probation_time_us > POSVEL_PROBATION_MAX) { + } else if (*probation_time_us > POSVEL_PROBATION_MAX) { *probation_time_us = POSVEL_PROBATION_MAX; } } @@ -4467,7 +4365,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, checkAirspeed, + status_flags.condition_system_sensors_initialized = Preflight::preflightCheck(&mavlink_log_pub, true, checkAirspeed, !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); @@ -4699,3 +4597,52 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status } } } + +int Commander::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int Commander::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + return 0; +} + +int Commander::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 40, + 3000, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +Commander *Commander::instantiate(int argc, char *argv[]) +{ + Commander *instance = new Commander(); + + if (instance) { + if (argc >= 2 && !strcmp(argv[1], "--hil")) { + instance->enable_hil(); + } + } + + return instance; +} + +void Commander::enable_hil() +{ + status.hil_state = vehicle_status_s::HIL_STATE_OFF; +}; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7d902ba34..e42f195c9c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1111,7 +1111,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF); - bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks, + bool preflight_ok = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot);