dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: Improved logic for OA prearm checks. 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2019-03-12 14:02:20 +01:00 committed by Julian Oes
parent 977a4c8e9b
commit f4a4dab65a
4 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_status_flags.msg
View File

@ -34,4 +34,5 @@ bool rc_calibration_valid # set if RC calibration is
bool vtol_transition_failure # Set to true if vtol transition failed
bool usb_connected # status of the USB power supply
bool avoidance_system_valid # status of the obstacle avoidance system
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
bool avoidance_system_valid # Status of the obstacle avoidance system

18
src/modules/commander/Commander.cpp
View File

@ -123,6 +123,7 @@ static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s; /**< wait for hotplug sens
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms;
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms;
static constexpr uint8_t AVOIDANCE_MAX_TRIALS = 4; /* Maximum number of trials for avoidance system to start */
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = nullptr;
static orb_advert_t power_button_state_pub = nullptr;
@ -173,6 +174,8 @@ static float _eph_threshold_adj =
INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition
static bool _skip_pos_accuracy_check = false;
static int avoidance_waiting_count = 0;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@ -580,7 +583,11 @@ Commander::Commander() :
status_flags.condition_power_input_valid = true;
status_flags.rc_calibration_valid = true;
status_flags.avoidance_system_required = _obs_avoid.get();
status_flags.avoidance_system_valid = false;
}
Commander::~Commander()
@ -3954,12 +3961,18 @@ void Commander::data_link_check(bool &status_changed)
}
// AVOIDANCE SYSTEM state check (only if it is enabled)
if (_obs_avoid.get() && !_onboard_controller_lost) {
if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
//if avoidance never started
if (_datalink_last_heartbeat_avoidance_system == 0 && hrt_elapsed_time(&_avoidance_system_not_started) > 5_s) {
if (_datalink_last_heartbeat_avoidance_system == 0 && hrt_elapsed_time(&_avoidance_system_not_started) > 5_s
&& avoidance_waiting_count < AVOIDANCE_MAX_TRIALS) {
_avoidance_system_not_started = hrt_absolute_time();
mavlink_log_info(&mavlink_log_pub, "Waiting for avoidance system to start");
avoidance_waiting_count++;
} else if (avoidance_waiting_count == AVOIDANCE_MAX_TRIALS) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system stuck. Try reboot vehicle.");
avoidance_waiting_count++;
}
//if heartbeats stop
@ -3979,6 +3992,7 @@ void Commander::data_link_check(bool &status_changed)
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system healthy");
status_flags.avoidance_system_valid = true;
avoidance_waiting_count = 0;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {

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

@ -802,6 +802,7 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
* Temporary Parameter to enable interface testing
*
* @boolean
* @reboot_required true
* @group Mission
*/
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);

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

@ -999,9 +999,9 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
}
}
if (!status_flags.avoidance_system_valid) {
if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
if (prearm_ok && reportFailures) {
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: AVOIDANCE SYSTEM NOT READY");
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: Avoidance system not ready");
}
prearm_ok = false;