Skip to content

Commit

Permalink
adding feature to prevent arming before onboard logging is ready
Browse files Browse the repository at this point in the history
  • Loading branch information
@catch-twenty-two
catch-twenty-two committed Oct 21, 2019
1 parent c58cfce commit 0633db8
Show file tree
Hide file tree
Showing 6 changed files with 155 additions and 66 deletions.
1 change: 1 addition & 0 deletions msg/telemetry_status.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ uint8 COMPONENT_ID_ALL = 0
uint8 COMPONENT_ID_AUTOPILOT1 = 1
uint8 COMPONENT_ID_CAMERA = 100
uint8 COMPONENT_ID_OBSTACLE_AVOIDANCE = 196
uint8 COMPONENT_ID_LOG = 155

# MAV_TYPE (fill in as needed)
uint8 MAV_TYPE_GENERIC = 0
Expand Down
3 changes: 3 additions & 0 deletions msg/vehicle_status_flags.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,3 +36,6 @@ bool usb_connected # status of the USB power supp

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

bool onboard_logging_system_required # Set to true if logging system is required for arming via the COM_ARM_OBLOG parameter
bool onboard_logging_system_valid # Status of the onboard logging system
165 changes: 107 additions & 58 deletions src/modules/commander/Commander.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -607,6 +607,8 @@ Commander::Commander() :
status_flags.rc_calibration_valid = true;

status_flags.avoidance_system_valid = false;
status_flags.onboard_logging_system_valid = false;

}

Commander::~Commander()
Expand Down Expand Up @@ -1242,6 +1244,7 @@ Commander::run()
param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW");

status_flags.avoidance_system_required = _param_com_obs_avoid.get();
status_flags.onboard_logging_system_required = _param_com_arm_ob_logger.get();

/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
Expand Down Expand Up @@ -1518,6 +1521,7 @@ Commander::run()

/* Update OA parameter */
status_flags.avoidance_system_required = _param_com_obs_avoid.get();
status_flags.onboard_logging_system_required = _param_com_arm_ob_logger.get();

/* handle power button state */
if (power_button_state_sub.updated()) {
Expand Down Expand Up @@ -3778,31 +3782,49 @@ void Commander::data_link_check(bool &status_changed)
status_changed = true;
}

}
} else {

_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
// AVOIDANCE SYSTEM update heartbeat values if needed

if (telemetry.remote_component_id == telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE) {
if (telemetry.heartbeat_time != _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = _datalink_last_status_avoidance_system != telemetry.remote_system_status;
if (status_flags.avoidance_system_required) {
process_onboard_system_heartbeat(_avoidance, status_flags.avoidance_system_valid, status_changed, telemetry);
}

_datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time;
_datalink_last_status_avoidance_system = telemetry.remote_system_status;
// ONBOARD LOGGING SYSTEM update heartbeat values if needed

if (_avoidance_system_lost) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
if (status_flags.onboard_logging_system_required) {
process_onboard_system_heartbeat(_logger, status_flags.onboard_logging_system_valid, status_changed, telemetry);
}
}

_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;

break;
}
}
}

// AVOIDANCE SYSTEM state check (only if it is enabled)

if (status_flags.avoidance_system_required) {

//if avoidance never started
if (_datalink_last_heartbeat_avoidance_system == 0 &&
_avoidance.print_msg_once == false &&
hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) {
_avoidance.print_msg_once = true;
mavlink_log_critical(&mavlink_log_pub, "avoidance system not available");

} else {
update_onboard_system_state(_avoidance, status_flags.avoidance_system_valid, status_changed);
}
}

// ON BOARD LOGGING SYSTEM state check (only if it is enabled)

if (status_flags.onboard_logging_system_required) {
update_onboard_system_state(_logger, status_flags.onboard_logging_system_valid, status_changed);
}

// GCS data link loss failsafe
if (!status.data_link_lost) {
Expand All @@ -3828,67 +3850,94 @@ void Commander::data_link_check(bool &status_changed)
status_changed = true;
}

// AVOIDANCE SYSTEM state check (only if it is enabled)
if (status_flags.avoidance_system_required && !_onboard_controller_lost) {

//if avoidance never started
if (_datalink_last_heartbeat_avoidance_system == 0
&& hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) {
if (!_print_avoidance_msg_once) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available");
_print_avoidance_msg_once = true;

}
}
// high latency data link loss failsafe
if (_high_latency_datalink_heartbeat > 0
&& hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
_high_latency_datalink_lost = hrt_absolute_time();

//if heartbeats stop
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
&& (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;
_print_avoidance_msg_once = false;
if (!status.high_latency_data_link_lost) {
status.high_latency_data_link_lost = true;
mavlink_log_critical(&mavlink_log_pub, "High latency data link lost");
status_changed = true;
}
}
}

//if status changed
if (_avoidance_system_status_change) {
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system starting");
}

if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system connected");
status_flags.avoidance_system_valid = true;
}
void Commander::process_onboard_system_heartbeat(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid,
bool &status_changed, telemetry_status_s &telemetry)
{
if (telemetry.remote_component_id == monitor.hb_component_id) {

if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system timed out");
}
if (telemetry.heartbeat_time != monitor.datalink_last_heartbeat) {
monitor.system_status_change = monitor.datalink_last_status != telemetry.remote_system_status;
}

if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected");
status_flags.avoidance_system_valid = false;
status_changed = true;
}
monitor.datalink_last_heartbeat = telemetry.heartbeat_time;
monitor.datalink_last_status = telemetry.remote_system_status;

_avoidance_system_status_change = false;
if (monitor.system_lost) {
mavlink_log_warning(&mavlink_log_pub, "%s system regained", monitor.hb_name);
status_changed = true;
monitor.system_lost = false;
status_flag_system_valid = true;
}
}
}

void Commander::update_onboard_system_state(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid,
bool &status_changed)
{

// high latency data link loss failsafe
if (_high_latency_datalink_heartbeat > 0
&& hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
_high_latency_datalink_lost = hrt_absolute_time();
//if heartbeat stops
if (!monitor.system_lost && (monitor.datalink_last_heartbeat > 0)
&& (hrt_elapsed_time(&monitor.datalink_last_heartbeat) > 5_s)) {
monitor.system_lost = true;
mavlink_log_critical(&mavlink_log_pub, "%s system lost", monitor.hb_name);
status_flag_system_valid = false;
monitor.print_msg_once = false;
}

if (!status.high_latency_data_link_lost) {
status.high_latency_data_link_lost = true;
mavlink_log_critical(&mavlink_log_pub, "High latency data link lost");
status_changed = true;
//if status changed
if (monitor.system_status_change) {
switch (monitor.datalink_last_status) {
case telemetry_status_s::MAV_STATE_UNINIT:
break;

case telemetry_status_s::MAV_STATE_BOOT:
case telemetry_status_s::MAV_STATE_CALIBRATING:
mavlink_log_info(&mavlink_log_pub, "%s system starting", monitor.hb_name);
status_flag_system_valid = false;
break;

case telemetry_status_s::MAV_STATE_STANDBY:
case telemetry_status_s::MAV_STATE_ACTIVE:
mavlink_log_info(&mavlink_log_pub, "%s system connected", monitor.hb_name);
status_flag_system_valid = true;
break;

case telemetry_status_s::MAV_STATE_CRITICAL:
mavlink_log_critical(&mavlink_log_pub, "%s system failure", monitor.hb_name);
status_flag_system_valid = false;
break;

case telemetry_status_s::MAV_STATE_EMERGENCY:
mavlink_log_critical(&mavlink_log_pub, "%s system emergency", monitor.hb_name);
status_flag_system_valid = false;
break;

case telemetry_status_s::MAV_STATE_POWEROFF:
case telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION:
mavlink_log_info(&mavlink_log_pub, "%s system shutdown", monitor.hb_name);
status_flag_system_valid = false;
break;
}

status_changed = true;
monitor.system_status_change = false;
}
}


void Commander::battery_status_check()
{
/* update battery status */
Expand Down
34 changes: 27 additions & 7 deletions src/modules/commander/Commander.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,10 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
(ParamInt<px4::params::CBRK_GPSFAIL>) _param_cbrk_gpsfail,
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr
(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,

(ParamInt<px4::params::COM_ARM_OBLOG>) _param_com_arm_ob_logger

)

enum class PrearmedMode {
Expand Down Expand Up @@ -249,14 +252,33 @@ class Commander : public ModuleBase<Commander>, public ModuleParams

hrt_abstime _datalink_last_heartbeat_gcs{0};

struct OnboardHeartBeatMonitor {

OnboardHeartBeatMonitor(uint8_t component_id, const char *name) :
hb_name(name),
hb_component_id(component_id)
{}

const char *hb_name;
hrt_abstime datalink_last_heartbeat{0};
bool system_lost{false};
bool system_status_change{false};
uint8_t datalink_last_status{telemetry_status_s::MAV_STATE_UNINIT};
bool print_msg_once{true};
uint8_t hb_component_id;

} _avoidance{telemetry_status_s::COMPONENT_ID_OBSTACLE_AVOIDANCE, "avoidance"},
_logger{telemetry_status_s::COMPONENT_ID_LOG, "logger"};

void update_onboard_system_state(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid,
bool &status_changed);
void process_onboard_system_heartbeat(OnboardHeartBeatMonitor &monitor, bool &status_flag_system_valid,
bool &status_changed, telemetry_status_s &telemetry);

hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
bool _onboard_controller_lost{false};

hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
bool _avoidance_system_lost{false};

bool _avoidance_system_status_change{false};
uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};

uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};

Expand All @@ -272,8 +294,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
systemlib::Hysteresis _auto_disarm_landed{false};
systemlib::Hysteresis _auto_disarm_killed{false};

bool _print_avoidance_msg_once{false};

// Subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
Expand Down
7 changes: 7 additions & 0 deletions src/modules/commander/commander_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -967,3 +967,10 @@ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1);
* @group Commander
*/
PARAM_DEFINE_INT32(COM_PREARM_MODE, 1);

/*
* Allow arming without onboard logger being enabled and in ready state
*
* The default allows the vehicle to arm without the onboard logger reporting as ready via it's heartbeat
*/
PARAM_DEFINE_INT32(COM_ARM_OBLOG, 0);
11 changes: 10 additions & 1 deletion src/modules/commander/state_machine_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1105,6 +1105,16 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
}
}


if (status_flags.onboard_logging_system_required && !status_flags.onboard_logging_system_valid) {
if (prearm_ok && reportFailures) {
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: Onboard logging system not ready");
}

prearm_ok = false;

}

// Arm Requirements: authorization
// check last, and only if everything else has passed
if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) {
Expand All @@ -1114,7 +1124,6 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
}
}


return prearm_ok;
}

Expand Down

0 comments on commit 0633db8

Please sign in to comment.