yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00: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

74
src/modules/commander/commander.cpp
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;
}

15
src/modules/commander/commander_params.c
View File

@@ -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);

8
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@@ -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(&current_vehicle_status, test->to_state, main_state_prev,

49
src/modules/commander/state_machine_helper.cpp
View File

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

6
src/modules/commander/state_machine_helper.h
View File

@@ -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_ */

2
src/modules/systemlib/rc_check.c
View File

@@ -88,9 +88,7 @@ int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool i
map_fail_count++;
}
}
}