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

suggestion for treating obstacle avoidance heartbeats

Browse Source
This commit is contained in:
baumanta 2018-11-29 13:29:20 +01:00 committed by Beat Küng
parent 6dec451bab
commit a98f5d2ab2
5 changed files with 76 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
msg/telemetry_status.msg
View File

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

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

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

12
src/modules/commander/Commander.hpp
View File

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

11
src/modules/mavlink/mavlink_receiver.cpp
View File

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

1
src/modules/mavlink/mavlink_receiver.h
View File

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