From a98f5d2ab2e7272fd14f25d40553f2ea560376be Mon Sep 17 00:00:00 2001
From: baumanta <baumanta@student.ethz.ch>
Date: Thu, 29 Nov 2018 13:29:20 +0100
Subject: [PATCH] suggestion for treating obstacle avoidance heartbeats
---
msg/telemetry_status.msg | 7 ++--
src/modules/commander/Commander.cpp | 53 +++++++++++++++++++++++-
src/modules/commander/Commander.hpp | 12 +++++-
src/modules/mavlink/mavlink_receiver.cpp | 11 ++++-
src/modules/mavlink/mavlink_receiver.h | 1 +
5 files changed, 76 insertions(+), 8 deletions(-)
diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg
index bce7ffc520..d9643c1bfa 100644
--- a/msg/telemetry_status.msg
+++ b/msg/telemetry_status.msg
@@ -6,9 +6,10 @@ uint8 LINK_TYPE_USB = 4
uint8 LINK_TYPE_IRIDIUM = 5
# MAV_COMPONENT (fill in as needed)
-uint8 COMPONENT_ID_ALL = 0
-uint8 COMPONENT_ID_AUTOPILOT1 = 1
-uint8 COMPONENT_ID_CAMERA = 100
+uint8 COMPONENT_ID_ALL = 0
+uint8 COMPONENT_ID_AUTOPILOT1 = 1
+uint8 COMPONENT_ID_CAMERA = 100
+uint8 COMPONENT_ID_OBSTACLE_AVOIDANCE = 196
# MAV_TYPE (fill in as needed)
uint8 MAV_TYPE_GENERIC = 0
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 6959b8e98f..544815c665 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -3903,7 +3903,18 @@ void Commander::data_link_check(bool &status_changed)
case (telemetry_status_s::MAV_TYPE_ONBOARD_CONTROLLER):
_datalink_last_heartbeat_onboard_controller = telemetry.heartbeat_time;
- // TODO: FYI @baumanta
+ _onboard_controller_lost = false;
+
+ 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;
+ }
+
+ _datalink_last_heartbeat_avoidance_system = telemetry.heartbeat_time;
+ _datalink_last_status_avoidance_system = telemetry.remote_system_status;
+ _avoidance_system_lost = false;
+ }
+
break;
}
}
@@ -3936,6 +3947,46 @@ void Commander::data_link_check(bool &status_changed)
mavlink_log_critical(&mavlink_log_pub, "ONBOARD CONTROLLER LOST");
}
+ // AVOIDANCE SYSTEM state check (only if it is enabled)
+ if (_obs_avoid.get()) {
+
+ //if avoidance never started
+ if (_datalink_last_heartbeat_avoidance_system == 0 && hrt_elapsed_time(&_avoidance_system_not_started) > 5_s) {
+ _avoidance_system_not_started = hrt_absolute_time();
+ mavlink_log_critical(&mavlink_log_pub, "AVOIDANCE SYSTEM DID NOT START");
+ }
+
+ //if heartbeats stop
+ if ((_datalink_last_heartbeat_avoidance_system > 0)
+ && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s) &&
+ (hrt_elapsed_time(&_avoidance_system_lost) > 5_s)) {
+ _avoidance_system_lost = hrt_absolute_time();
+ mavlink_log_critical(&mavlink_log_pub, "AVOIDANCE SYSTEM LOST");
+ }
+
+ //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 HEALTHY");
+ }
+
+ if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
+ mavlink_log_critical(&mavlink_log_pub, "AVOIDANCE SYSTEM TIMEOUT");
+ }
+
+ if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
+ mavlink_log_critical(&mavlink_log_pub, "AVOIDANCE SYSTEM ABORT");
+ }
+
+ _avoidance_system_status_change = false;
+ }
+ }
+
+
// high latency data link loss failsafe
if (hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_high_latency_datalink_loss_threshold.get() * 1_s)) {
_high_latency_datalink_lost = hrt_absolute_time();
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index f6a8f4013d..345f739d06 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -116,7 +116,9 @@ private:
(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain,
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _low_bat_action,
- (ParamFloat<px4::params::COM_DISARM_LAND>) _disarm_when_landed_timeout
+ (ParamFloat<px4::params::COM_DISARM_LAND>) _disarm_when_landed_timeout,
+
+ (ParamInt<px4::params::MPC_OBS_AVOID>) _obs_avoid
)
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
@@ -178,7 +180,13 @@ private:
uint64_t _datalink_last_heartbeat_onboard_controller{0};
uint64_t _onboard_controller_lost{0};
- int _iridiumsbd_status_sub{-1};
+ uint64_t _datalink_last_heartbeat_avoidance_system{0};
+ uint64_t _avoidance_system_lost{0};
+ uint64_t _avoidance_system_not_started{0};
+ bool _avoidance_system_status_change{0};
+ uint64_t _datalink_last_status_avoidance_system{9};
+
+ int _iridiumsbd_status_sub{-1};
uint64_t _high_latency_datalink_heartbeat{0};
uint64_t _high_latency_datalink_lost{0};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 716c1791ff..9a4c81e6f9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1882,8 +1882,8 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
- // ignore own heartbeats
- if ((msg->sysid != mavlink_system.sysid) && (msg->compid != mavlink_system.compid)) {
+ /* ignore own heartbeats, accept only heartbeats from GCS */
+ if (msg->sysid != mavlink_system.sysid || hb.type == MAV_TYPE_ONBOARD_CONTROLLER) {
telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
@@ -1895,6 +1895,13 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
tstatus.remote_component_id = msg->compid;
tstatus.remote_type = hb.type;
tstatus.remote_system_status = hb.system_status;
+
+ if (_telem_status_pub == nullptr) {
+ _telem_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+ } else {
+ orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &tstatus);
+ }
}
}
}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index caa294ea6e..676b441d77 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -253,6 +253,7 @@ private:
orb_advert_t _rc_pub{nullptr};
orb_advert_t _trajectory_waypoint_pub{nullptr};
orb_advert_t _transponder_report_pub{nullptr};
+ orb_advert_t _telem_status_pub{nullptr};
orb_advert_t _visual_odometry_pub{nullptr};
static constexpr int _gps_inject_data_queue_size{6};
--
GitLab