Skip to content

Commit

Permalink
commander add parameter COM_ARM_MIS_REQ
Browse files Browse the repository at this point in the history 
 - arm without mission on by default
  • Loading branch information
@dagar
dagar committed Apr 20, 2017
1 parent 709bd8c commit c1d9972
Show file tree
Hide file tree
Showing 6 changed files with 99 additions and 55 deletions.
74 changes: 47 additions & 27 deletions src/modules/commander/commander.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

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


Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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));
}
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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) {
Expand All @@ -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.
*
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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,
Expand All @@ -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 {
Expand Down Expand Up @@ -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;
}
Expand Down
15 changes: 11 additions & 4 deletions src/modules/commander/commander_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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);
8 changes: 5 additions & 3 deletions src/modules/commander/commander_tests/state_machine_helper_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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(&current_vehicle_status, test->to_state, main_state_prev,
Expand Down
Loading

0 comments on commit c1d9972

Please sign in to comment.