fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 15:24:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added obstacle avoidance healthiness topic in vehicle status msg.

Browse Source 
This allows to perform pre-arm checks and prevent arming if obstacle avoidance is enabled but not yet running.
Added a print once flag to prevent excessive message spamming in QGC.

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2019-02-13 16:04:22 +01:00 committed by Julian Oes
parent 7be05396ba
commit 107746ded5
3 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_status_flags.msg
View File

@ -33,3 +33,5 @@ bool rc_input_blocked # set if RC input should be
bool rc_calibration_valid # set if RC calibration is valid
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

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

@ -580,6 +580,7 @@ Commander::Commander() :
status_flags.condition_power_input_valid = true;
status_flags.rc_calibration_valid = true;
status_flags.avoidance_system_valid = false;
}
Commander::~Commander()
@ -3918,6 +3919,7 @@ void Commander::data_link_check(bool &status_changed)
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
}
}
@ -3965,6 +3967,7 @@ void Commander::data_link_check(bool &status_changed)
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
_avoidance_system_lost = true;
mavlink_log_critical(&mavlink_log_pub, "Avoidance system lost");
status_flags.avoidance_system_valid = false;
}
//if status changed
@ -3975,6 +3978,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;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
@ -3983,6 +3987,7 @@ void Commander::data_link_check(bool &status_changed)
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system abort");
status_flags.avoidance_system_valid = false;
}
_avoidance_system_status_change = false;

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

@ -46,6 +46,7 @@
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
#include "PreflightCheck.h"
@ -915,6 +916,7 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
bool reportFailures = true;
bool prearm_ok = true;
// USB not connected
if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
if (reportFailures) {
@ -997,6 +999,15 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
}
}
if (!status_flags.avoidance_system_valid) {
if (prearm_ok && reportFailures) {
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: AVOIDANCE SYSTEM NOT READY");
}
prearm_ok = false;
}
return prearm_ok;
}