fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-19 04:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander add parameter COM_ARM_MIS_REQ

Browse Source 
- arm without mission on by default
This commit is contained in:
Daniel Agar
2017-04-17 00:18:39 -04:00
parent 709bd8cb28
commit c1d9972244
6 changed files with 99 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+47 -27
View File
@@ -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;
}