From c1d9972244537b10930a51b4f569ef40b040f9f5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 17 Apr 2017 00:18:39 -0400
Subject: [PATCH] commander add parameter COM_ARM_MIS_REQ
- arm without mission on by default
---
src/modules/commander/commander.cpp | 74 ++++++++++++-------
src/modules/commander/commander_params.c | 15 +++-
.../state_machine_helper_test.cpp | 8 +-
.../commander/state_machine_helper.cpp | 49 ++++++++----
src/modules/commander/state_machine_helper.h | 6 +-
src/modules/systemlib/rc_check.c | 2 -
6 files changed, 99 insertions(+), 55 deletions(-)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8fecc34381..b11b62f751 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -206,11 +206,11 @@ static struct home_position_s _home = {};
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
static struct commander_state_s internal_state = {};
-struct mission_result_s _mission_result;
+static struct mission_result_s _mission_result = {};
static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
static unsigned _last_mission_instance = 0;
-struct manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
+static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch
static uint8_t _last_sp_man_arm_switch = 0;
@@ -228,7 +228,9 @@ 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;
+static bool arm_without_gps = false;
+static bool arm_mission_required = false;
+
static bool _last_condition_global_position_valid = false;
@@ -420,10 +422,12 @@ 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, hrt_elapsed_time(&commander_boot_timestamp));
+ checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, true, 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, hrt_elapsed_time(&commander_boot_timestamp));
+
+ checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_without_gps, arm_mission_required, hrt_elapsed_time(&commander_boot_timestamp));
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
+
return 0;
}
@@ -682,7 +686,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,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp));
if (arming_res == TRANSITION_CHANGED) {
@@ -1322,6 +1327,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS");
param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN");
param_t _param_rc_override = param_find("COM_RC_OVERRIDE");
+ param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ");
param_t _param_fmode_1 = param_find("COM_FLTMODE1");
param_t _param_fmode_2 = param_find("COM_FLTMODE2");
@@ -1670,13 +1676,21 @@ int commander_thread_main(int argc, char *argv[])
// Run preflight check
int32_t rc_in_off = 0;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
- int32_t arm_without_gps = 0;
+
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);
+
int32_t arm_switch_is_button = 0;
param_get(_param_arm_switch_is_button, &arm_switch_is_button);
- can_arm_without_gps = (arm_without_gps == 1);
+
+ int32_t arm_without_gps_param = 0;
+ param_get(_param_arm_without_gps, &arm_without_gps_param);
+ arm_without_gps = (arm_without_gps_param == 1);
+
+ int32_t arm_mission_required_param = 0;
+ param_get(_param_arm_mission_required, &arm_mission_required_param);
+ arm_mission_required = (arm_mission_required_param == 1);
+
status.rc_input_mode = rc_in_off;
if (is_hil_setup(autostart_id)) {
// HIL configuration selected: real sensors will be disabled
@@ -1821,9 +1835,13 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_offboard_loss_timeout, &offboard_loss_timeout);
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);
param_get(_param_arm_switch_is_button, &arm_switch_is_button);
- can_arm_without_gps = (arm_without_gps == 1);
+
+ param_get(_param_arm_without_gps, &arm_without_gps_param);
+ arm_without_gps = (arm_without_gps_param == 1);
+
+ param_get(_param_arm_mission_required, &arm_mission_required_param);
+ arm_mission_required = (arm_mission_required_param == 1);
/* Autostart id */
param_get(_param_autostart_id, &autostart_id);
@@ -1937,7 +1955,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
/* check sensors also */
(void)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,
+ (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !arm_without_gps,
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
}
}
@@ -2048,7 +2066,8 @@ int commander_thread_main(int argc, char *argv[])
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
- can_arm_without_gps,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp))) {
mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch");
arming_state_changed = true;
@@ -2196,8 +2215,6 @@ int commander_thread_main(int argc, char *argv[])
was_falling = land_detector.freefall;
}
-
-
/* Update hysteresis time. Use a time of factor 5 longer if we have not taken off yet. */
hrt_abstime timeout_time = disarm_when_landed * 1000000;
@@ -2382,7 +2399,8 @@ int commander_thread_main(int argc, char *argv[])
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
- can_arm_without_gps,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp));
if (arming_ret == TRANSITION_CHANGED) {
@@ -2391,10 +2409,8 @@ int commander_thread_main(int argc, char *argv[])
/* do not complain if not allowed into standby */
arming_ret = TRANSITION_NOT_CHANGED;
}
-
}
-
/*
* Check for valid position information.
*
@@ -2459,6 +2475,8 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result);
+ status_flags.condition_auto_mission_available = _mission_result.valid && !_mission_result.finished;
+
if (status.mission_failure != _mission_result.mission_failure) {
status.mission_failure = _mission_result.mission_failure;
status_changed = true;
@@ -2679,7 +2697,8 @@ int commander_thread_main(int argc, char *argv[])
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
- can_arm_without_gps,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp));
if (arming_ret == TRANSITION_CHANGED) {
@@ -2733,7 +2752,8 @@ int commander_thread_main(int argc, char *argv[])
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
- can_arm_without_gps,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp));
if (arming_ret == TRANSITION_CHANGED) {
@@ -4101,8 +4121,10 @@ void *commander_low_prio_loop(void *arg)
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
- can_arm_without_gps,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp))) {
+
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
break;
} else {
@@ -4195,7 +4217,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), !can_arm_without_gps,
+ !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !arm_without_gps,
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
arming_state_transition(&status,
@@ -4206,8 +4228,9 @@ void *commander_low_prio_loop(void *arg)
false /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
- avionics_power_rail_voltage,
- can_arm_without_gps,
+ avionics_power_rail_voltage,
+ arm_without_gps,
+ arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp));
} else {
@@ -4373,9 +4396,6 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub) {
if (status_flags.offboard_control_signal_lost) {
v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SIGNAL_LOST_MASK;
}
- if (status_flags.offboard_control_signal_weak) {
- v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SIGNAL_WEAK_MASK;
- }
if (status_flags.offboard_control_set_by_command) {
v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SET_BY_COMMAND_MASK;
}
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 398660c0ea..bba7453385 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -275,10 +275,7 @@ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
* The default allows to arm the vehicle without GPS signal.
*
* @group Commander
- * @min 0
- * @max 1
- * @value 0 Don't allow arming without GPS
- * @value 1 Allow arming without GPS
+ * @boolean
*/
PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
@@ -594,3 +591,13 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.15f);
* @group Commander
*/
PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 0);
+
+/**
+ * Require valid mission to arm
+ *
+ * The default allows to arm the vehicle without a valid mission.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
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 21c3842c09..c74dd96b54 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -297,9 +297,10 @@ bool StateMachineHelperTest::armingStateTransitionTest()
nullptr /* no mavlink_log_pub */,
&status_flags,
5.0f, /* avionics rail voltage */
- check_gps,
- 2e6 /* 2 seconds after boot, everything should be checked */
- );
+ check_gps,
+ true, /* can arm without valid mission */
+ 2e6 /* 2 seconds after boot, everything should be checked */
+ );
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);
@@ -456,6 +457,7 @@ bool StateMachineHelperTest::mainStateTransitionTest()
current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+ current_status_flags.condition_auto_mission_available = true;
// Attempt transition
transition_result_t result = main_state_transition(¤t_vehicle_status, test->to_state, main_state_prev,
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9230d75b9a..40ab033899 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -41,12 +41,6 @@
*/
#include <px4_config.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <string.h>
#include <math.h>
#include <px4_posix.h>
@@ -54,18 +48,17 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
#include "PreflightCheck.h"
+
#ifndef __PX4_NUTTX
#include "DevMgr.hpp"
using namespace DriverFramework;
@@ -129,7 +122,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 arm_without_gps,
+ bool arm_mission_required,
hrt_abstime time_since_boot)
{
// Double check that our static arrays are still valid
@@ -157,7 +151,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, time_since_boot);
+ status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot);
}
/* re-run the pre-flight check as long as sensors are failing */
@@ -168,7 +162,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, time_since_boot);
+ status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot);
status_flags->condition_system_sensors_initialized = (prearm_ret == OK);
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
@@ -258,7 +252,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
}
}
}
-
}
} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY
@@ -420,6 +413,17 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case commander_state_s::MAIN_STATE_AUTO_MISSION:
+
+ /* need global position, home position, and a valid mission */
+ if (status_flags->condition_global_position_valid &&
+ status_flags->condition_home_position_valid &&
+ status_flags->condition_auto_mission_available) {
+
+ ret = TRANSITION_CHANGED;
+ }
+
+ break;
+
case commander_state_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */
@@ -441,8 +445,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case commander_state_s::MAIN_STATE_OFFBOARD:
- /* need offboard signal
- */
+ /* need offboard signal */
if (!status_flags->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
@@ -1008,7 +1011,8 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail
}
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)
+ status_flags_s *status_flags, battery_status_s *battery, bool arm_without_gps, bool arm_mission_required,
+ hrt_abstime time_since_boot)
{
bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
status_flags->condition_system_hotplug_timeout);
@@ -1023,7 +1027,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, prearm, time_since_boot);
+ !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;
@@ -1041,6 +1045,19 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
}
}
+ // mission required
+ if (arm_mission_required &&
+ (!status_flags->condition_auto_mission_available ||
+ !status_flags->condition_home_position_valid ||
+ !status_flags->condition_global_position_valid)) {
+
+ preflight_ok = false;
+
+ if (reportFailures) {
+ mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: valid mission required");
+ }
+ }
+
/* report once, then set the flag */
if (reportFailures && !preflight_ok) {
status_flags->condition_system_prearm_error_reported = true;
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 1963a340d2..1d91e6bcdb 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -90,7 +90,6 @@ struct status_flags_s {
bool circuit_breaker_engaged_usb_check;
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
- bool offboard_control_signal_weak;
bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC
bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time
bool rc_signal_found_once;
@@ -116,7 +115,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 arm_without_gps,
+ bool arm_mission_required,
hrt_abstime time_since_boot);
transition_result_t
@@ -153,6 +153,6 @@ void set_data_link_loss_nav_state(struct vehicle_status_s *status,
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);
+ bool arm_without_gps, bool arm_mission_required, hrt_abstime time_since_boot);
#endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index ce86b35632..fb5fc6f51d 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -88,9 +88,7 @@ int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool i
map_fail_count++;
}
-
}
-
}
--
GitLab