diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f716bcb0f2..299b312ba1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -190,6 +190,8 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static float avionics_power_rail_voltage; // voltage of the avionics power rail +static bool can_arm_without_gps = false; + /** * The daemon app only briefly exists to start @@ -365,9 +367,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); + checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -625,7 +627,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co true /* fRunPreArmChecks */, mavlink_log_pub_local, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -1507,6 +1510,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_autostart_id, &autostart_id); param_get(_param_rc_in_off, &rc_in_off); param_get(_param_arm_without_gps, &arm_without_gps); + can_arm_without_gps = (arm_without_gps == 1); status.rc_input_mode = rc_in_off; if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled @@ -1516,7 +1520,7 @@ int commander_thread_main(int argc, char *argv[]) // 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), - (arm_without_gps == 0), false); + !can_arm_without_gps, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } @@ -1635,6 +1639,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_offboard_loss_act, &offboard_loss_act); param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act); param_get(_param_arm_without_gps, &arm_without_gps); + can_arm_without_gps = (arm_without_gps == 1); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1747,7 +1752,7 @@ int commander_thread_main(int argc, char *argv[]) } 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), (arm_without_gps == 0), hotplug_timeout); + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !can_arm_without_gps, hotplug_timeout); } } @@ -1852,7 +1857,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage)) { + avionics_power_rail_voltage, + can_arm_without_gps)) { mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch"); arming_state_changed = true; } @@ -2128,7 +2134,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2382,7 +2389,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2429,7 +2437,8 @@ int commander_thread_main(int argc, char *argv[]) true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -3703,7 +3712,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage)) { + avionics_power_rail_voltage, + can_arm_without_gps)) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { @@ -3787,7 +3797,7 @@ void *commander_low_prio_loop(void *arg) } 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), false, hotplug_timeout); + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !can_arm_without_gps, hotplug_timeout); arming_state_transition(&status, &battery, @@ -3797,7 +3807,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - avionics_power_rail_voltage); + avionics_power_rail_voltage, + can_arm_without_gps); } 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 cc4d32bff3..78bc4bc22b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -278,6 +278,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) for (size_t i=0; icurrent_state.arming_state; status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized; @@ -294,7 +296,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */, &status_flags, - 5.0f); + 5.0f, check_gps); // 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 1a973ff28b..64cd2dfa42 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -118,7 +118,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, - float avionics_power_rail_voltage) + float avionics_power_rail_voltage, + bool can_arm_without_gps) { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); @@ -144,7 +145,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); + status_flags, battery, can_arm_without_gps); } /* re-run the pre-flight check as long as sensors are failing */ @@ -155,7 +156,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); + status_flags, battery, can_arm_without_gps); status_flags->condition_system_sensors_initialized = !prearm_ret; last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -972,7 +973,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in return status->nav_state != nav_state_old; } -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) +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) { /* */ @@ -988,7 +989,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), - !status_flags->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + !can_arm_without_gps, true, reportFailures); 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 a204381dff..1506ff8acb 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -104,7 +104,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, - float avionics_power_rail_voltage); + float avionics_power_rail_voltage, + bool can_arm_without_gps); transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, @@ -117,6 +118,6 @@ 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); +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); #endif /* STATE_MACHINE_HELPER_H_ */