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

mavlink: telemetry status only log simple HEARTBEAT validity 

* delete telemetry_heartbeat msg
 * delete unused _telemetry_status_mutex
This commit is contained in:
Daniel Agar 2020-10-13 13:37:10 -04:00 committed by GitHub
parent d71ca37087
commit 8d1b99be31
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 187 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -130,7 +130,6 @@ set(msg_files
system_power.msg
task_stack_info.msg
tecs_status.msg
telemetry_heartbeat.msg
telemetry_status.msg
test_motor.msg
timesync.msg

51
msg/telemetry_heartbeat.msg
View File

@ -1,51 +0,0 @@
uint64 timestamp # time since system start (microseconds)
# COMPONENT (fill in as needed)
uint8 COMP_ID_ALL = 0
uint8 COMP_ID_AUTOPILOT1 = 1
uint8 COMP_ID_TELEMETRY_RADIO = 68
uint8 COMP_ID_CAMERA = 100
uint8 COMP_ID_GIMBAL = 154
uint8 COMP_ID_LOG = 155
uint8 COMP_ID_ADSB = 156
uint8 COMP_ID_OSD = 157
uint8 COMP_ID_PERIPHERAL = 158
uint8 COMP_ID_FLARM = 160
uint8 COMP_ID_MISSIONPLANNER = 190
uint8 COMP_ID_OBSTACLE_AVOIDANCE = 196
uint8 COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197
uint8 COMP_ID_PAIRING_MANAGER = 198
uint8 COMP_ID_UDP_BRIDGE = 240
uint8 COMP_ID_UART_BRIDGE = 241
uint8 COMP_ID_TUNNEL_NODE = 242
uint8 system_id # system id of the remote system (Mavlink header sys_id)
uint8 component_id # component id of the remote system (Mavlink header comp_id)
# TYPE (fill in as needed)
uint8 TYPE_GENERIC = 0
uint8 TYPE_ANTENNA_TRACKER = 5
uint8 TYPE_GCS = 6
uint8 TYPE_ONBOARD_CONTROLLER = 18
uint8 TYPE_GIMBAL = 26
uint8 TYPE_ADSB = 27
uint8 TYPE_CAMERA = 30
uint8 TYPE_CHARGING_STATION = 31
uint8 type
# STATE
uint8 STATE_UNINIT = 0
uint8 STATE_BOOT = 1
uint8 STATE_CALIBRATING = 2
uint8 STATE_STANDBY = 3
uint8 STATE_ACTIVE = 4
uint8 STATE_CRITICAL = 5
uint8 STATE_EMERGENCY = 6
uint8 STATE_POWEROFF = 7
uint8 STATE_FLIGHT_TERMINATION = 8
uint8 state

26
msg/telemetry_status.msg
View File

@ -6,8 +6,6 @@ uint8 LINK_TYPE_IRIDIUM = 4
uint64 timestamp # time since system start (microseconds)
telemetry_heartbeat[4] heartbeats
uint8 type # type of the radio hardware (LINK_TYPE_*)
uint8 mode
@ -27,3 +25,27 @@ float32 rate_rx
float32 rate_tx
float32 rate_txerr
uint64 HEARTBEAT_TIMEOUT_US = 1500000 # Heartbeat timeout 1.5 seconds
# Heartbeats per type
bool heartbeat_type_antenna_tracker # MAV_TYPE_ANTENNA_TRACKER
bool heartbeat_type_gcs # MAV_TYPE_GCS
bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER
bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL
bool heartbeat_type_adsb # MAV_TYPE_ADSB
bool heartbeat_type_camera # MAV_TYPE_CAMERA
# Heartbeats per component
bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO
bool heartbeat_component_log # MAV_COMP_ID_LOG
bool heartbeat_component_osd # MAV_COMP_ID_OSD
bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE
bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY
bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER
bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE
bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE
# Misc component health
bool avoidance_system_healthy

12
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -277,18 +277,16 @@ rtps:
id: 131
- msg: yaw_estimator_status
id: 132
- msg: telemetry_heartbeat
id: 133
- msg: sensor_preflight_mag
id: 134
id: 133
- msg: estimator_states
id: 135
id: 134
- msg: generator_status
id: 136
id: 135
- msg: sensor_gyro_fft
id: 137
id: 136
- msg: navigator_mission_item
id: 138
id: 137
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

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

@ -3809,65 +3809,47 @@ void Commander::data_link_check()
}
}
for (const auto &hb : telemetry.heartbeats) {
// handle different remote types
switch (hb.type) {
case telemetry_heartbeat_s::TYPE_GCS:
if (telemetry.heartbeat_type_gcs) {
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
status.data_link_lost = false;
_status_changed = true;
// Initial connection or recovery from data link lost
if (status.data_link_lost) {
if (hb.timestamp > _datalink_last_heartbeat_gcs) {
status.data_link_lost = false;
_status_changed = true;
if (!armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
}
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&mavlink_log_pub, "Data link regained");
}
// Only keep the very last heartbeat timestamp, so we don't get confused
// by multiple mavlink instances publishing different timestamps.
if (hb.timestamp > _datalink_last_heartbeat_gcs) {
_datalink_last_heartbeat_gcs = hb.timestamp;
if (!armed.armed && !status_flags.condition_calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags,
_arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp));
}
break;
case telemetry_heartbeat_s::TYPE_ONBOARD_CONTROLLER:
if (_onboard_controller_lost) {
if (hb.timestamp > _datalink_last_heartbeat_onboard_controller) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
_onboard_controller_lost = false;
_status_changed = true;
}
}
_datalink_last_heartbeat_onboard_controller = hb.timestamp;
if (hb.component_id == telemetry_heartbeat_s::COMP_ID_OBSTACLE_AVOIDANCE) {
if (hb.timestamp > _datalink_last_heartbeat_avoidance_system) {
_avoidance_system_status_change = (_datalink_last_status_avoidance_system != hb.state);
}
_datalink_last_heartbeat_avoidance_system = hb.timestamp;
_datalink_last_status_avoidance_system = hb.state;
if (_avoidance_system_lost) {
_status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
}
}
break;
}
_datalink_last_heartbeat_gcs = telemetry.timestamp;
}
if (telemetry.heartbeat_type_onboard_controller) {
if (_onboard_controller_lost) {
_onboard_controller_lost = false;
_status_changed = true;
if (_datalink_last_heartbeat_onboard_controller != 0) {
mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
}
}
_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
}
if (telemetry.heartbeat_component_obstacle_avoidance) {
if (_avoidance_system_lost) {
_avoidance_system_lost = false;
_status_changed = true;
}
_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
}
}
}
@ -3899,40 +3881,15 @@ void Commander::data_link_check()
// AVOIDANCE SYSTEM state check (only if it is enabled)
if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
//if heartbeats stop
// 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;
status_flags.avoidance_system_valid = false;
}
//if status changed
if (_avoidance_system_status_change) {
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_BOOT) {
status_flags.avoidance_system_valid = false;
}
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_ACTIVE) {
status_flags.avoidance_system_valid = true;
}
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_CRITICAL) {
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
if (_datalink_last_status_avoidance_system == telemetry_heartbeat_s::STATE_FLIGHT_TERMINATION) {
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
_avoidance_system_status_change = false;
}
}
// 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)) {

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

@ -322,8 +322,6 @@ private:
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
bool _onboard_controller_lost{false};
bool _avoidance_system_lost{false};
bool _avoidance_system_status_change{false};
uint8_t _datalink_last_status_avoidance_system{telemetry_heartbeat_s::STATE_UNINIT};
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};

22
src/modules/mavlink/mavlink_main.cpp
View File

@ -355,18 +355,6 @@ Mavlink::get_instance_for_network_port(unsigned long port)
}
#endif // MAVLINK_UDP
bool
Mavlink::is_connected()
{
for (auto &hb : _tstatus.heartbeats) {
if ((hb.type == telemetry_heartbeat_s::TYPE_GCS) && (hrt_elapsed_time(&hb.timestamp) < 3_s)) {
return true;
}
}
return false;
}
int
Mavlink::destroy_all_instances()
{
@ -2120,7 +2108,6 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize send mutex */
pthread_mutex_init(&_send_mutex, nullptr);
pthread_mutex_init(&_telemetry_status_mutex, nullptr);
/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
if (_forwarding_on) {
@ -2524,7 +2511,6 @@ Mavlink::task_main(int argc, char *argv[])
}
pthread_mutex_destroy(&_send_mutex);
pthread_mutex_destroy(&_telemetry_status_mutex);
PX4_INFO("exiting channel %i", (int)_channel);
@ -2618,11 +2604,9 @@ void Mavlink::publish_telemetry_status()
_tstatus.streams = _streams.size();
// telemetry_status is also updated from the receiver thread, but never the same fields
lock_telemetry_status();
_tstatus.timestamp = hrt_absolute_time();
_telem_status_pub.publish(_tstatus);
_tstatus_updated = false;
unlock_telemetry_status();
}
void Mavlink::configure_sik_radio()
@ -2761,10 +2745,8 @@ Mavlink::start(int argc, char *argv[])
void
Mavlink::display_status()
{
for (const auto &hb : _tstatus.heartbeats) {
if ((hb.timestamp > 0) && (hb.type == telemetry_heartbeat_s::TYPE_GCS)) {
printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&hb.timestamp));
}
if (_tstatus.heartbeat_type_gcs) {
printf("\tGCS heartbeat valid\n");
}
printf("\tmavlink chan: #%u\n", _channel);

7
src/modules/mavlink/mavlink_main.h
View File

@ -258,7 +258,7 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
bool is_connected();
bool is_connected() { return _tstatus.heartbeat_type_gcs; }
#if defined(MAVLINK_UDP)
static Mavlink *get_instance_for_network_port(unsigned long port);
@ -430,9 +430,7 @@ public:
/**
* Get the receive status of this MAVLink link
*/
telemetry_status_s &get_telemetry_status() { return _tstatus; }
void lock_telemetry_status() { pthread_mutex_lock(&_telemetry_status_mutex); }
void unlock_telemetry_status() { pthread_mutex_unlock(&_telemetry_status_mutex); }
telemetry_status_s &telemetry_status() { return _tstatus; }
void telemetry_status_updated() { _tstatus_updated = true; }
void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; }
@ -652,7 +650,6 @@ private:
pthread_mutex_t _message_buffer_mutex {};
pthread_mutex_t _send_mutex {};
pthread_mutex_t _telemetry_status_mutex {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,

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

@ -2075,50 +2075,75 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
if (same_system || hb.type == MAV_TYPE_GCS) {
telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
switch (hb.type) {
case MAV_TYPE_ANTENNA_TRACKER:
_heartbeat_type_antenna_tracker = now;
break;
bool heartbeat_slot_found = false;
int heartbeat_slot = 0;
case MAV_TYPE_GCS:
_heartbeat_type_gcs = now;
break;
// find existing HEARTBEAT slot
for (size_t i = 0; i < (sizeof(tstatus.heartbeats) / sizeof(tstatus.heartbeats[0])); i++) {
if ((tstatus.heartbeats[i].system_id == msg->sysid)
&& (tstatus.heartbeats[i].component_id == msg->compid)
&& (tstatus.heartbeats[i].type == hb.type)) {
case MAV_TYPE_ONBOARD_CONTROLLER:
_heartbeat_type_onboard_controller = now;
break;
// found matching heartbeat slot
heartbeat_slot = i;
heartbeat_slot_found = true;
break;
}
case MAV_TYPE_GIMBAL:
_heartbeat_type_gimbal = now;
break;
case MAV_TYPE_ADSB:
_heartbeat_type_adsb = now;
break;
case MAV_TYPE_CAMERA:
_heartbeat_type_camera = now;
break;
default:
PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %d from SYSID: %d, COMPID: %d", hb.type, msg->sysid, msg->compid);
}
// otherwise use first available slot
if (!heartbeat_slot_found) {
for (size_t i = 0; i < (sizeof(tstatus.heartbeats) / sizeof(tstatus.heartbeats[0])); i++) {
if ((tstatus.heartbeats[i].system_id == 0) && (tstatus.heartbeats[i].timestamp == 0)) {
heartbeat_slot = i;
heartbeat_slot_found = true;
break;
}
}
switch (msg->compid) {
case MAV_COMP_ID_TELEMETRY_RADIO:
_heartbeat_component_telemetry_radio = now;
break;
case MAV_COMP_ID_LOG:
_heartbeat_component_log = now;
break;
case MAV_COMP_ID_OSD:
_heartbeat_component_osd = now;
break;
case MAV_COMP_ID_OBSTACLE_AVOIDANCE:
_heartbeat_component_obstacle_avoidance = now;
_mavlink->telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE);
break;
case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY:
_heartbeat_component_visual_inertial_odometry = now;
break;
case MAV_COMP_ID_PAIRING_MANAGER:
_heartbeat_component_pairing_manager = now;
break;
case MAV_COMP_ID_UDP_BRIDGE:
_heartbeat_component_udp_bridge = now;
break;
case MAV_COMP_ID_UART_BRIDGE:
_heartbeat_component_uart_bridge = now;
break;
default:
PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %d from SYSID: %d, COMPID: %d", hb.type, msg->sysid, msg->compid);
}
if (heartbeat_slot_found) {
_mavlink->lock_telemetry_status();
tstatus.heartbeats[heartbeat_slot].timestamp = now;
tstatus.heartbeats[heartbeat_slot].system_id = msg->sysid;
tstatus.heartbeats[heartbeat_slot].component_id = msg->compid;
tstatus.heartbeats[heartbeat_slot].type = hb.type;
tstatus.heartbeats[heartbeat_slot].state = hb.system_status;
_mavlink->telemetry_status_updated();
_mavlink->unlock_telemetry_status();
} else {
PX4_ERR("no telemetry heartbeat slots available");
}
CheckHeartbeats(now, true);
}
}
}
@ -2825,6 +2850,39 @@ void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg)
}
}
void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
{
// check HEARTBEATs for timeout
static constexpr uint64_t TIMEOUT = telemetry_status_s::HEARTBEAT_TIMEOUT_US;
if (t <= TIMEOUT) {
return;
}
if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) {
telemetry_status_s &tstatus = _mavlink->telemetry_status();
tstatus.heartbeat_type_antenna_tracker = (t <= TIMEOUT + _heartbeat_type_antenna_tracker);
tstatus.heartbeat_type_gcs = (t <= TIMEOUT + _heartbeat_type_gcs);
tstatus.heartbeat_type_onboard_controller = (t <= TIMEOUT + _heartbeat_type_onboard_controller);
tstatus.heartbeat_type_gimbal = (t <= TIMEOUT + _heartbeat_type_gimbal);
tstatus.heartbeat_type_adsb = (t <= TIMEOUT + _heartbeat_type_adsb);
tstatus.heartbeat_type_camera = (t <= TIMEOUT + _heartbeat_type_camera);
tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio);
tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log);
tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd);
tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance);
tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry);
tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager);
tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge);
tstatus.heartbeat_component_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge);
_mavlink->telemetry_status_updated();
_last_heartbeat_check = t;
}
}
/**
* Receive data from UART/UDP
*/
@ -2993,7 +3051,9 @@ MavlinkReceiver::Run()
usleep(10000);
}
hrt_abstime t = hrt_absolute_time();
const hrt_abstime t = hrt_absolute_time();
CheckHeartbeats(t);
if (t - last_send_update > timeout * 1000) {
_mission_manager.check_active_mission();

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

@ -182,6 +182,7 @@ private:
void handle_message_utm_global_position(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void CheckHeartbeats(const hrt_abstime &t, bool force = false);
void Run();
@ -314,6 +315,24 @@ private:
// Allocated if needed.
TunePublisher *_tune_publisher{nullptr};
hrt_abstime _last_heartbeat_check{0};
hrt_abstime _heartbeat_type_antenna_tracker{0};
hrt_abstime _heartbeat_type_gcs{0};
hrt_abstime _heartbeat_type_onboard_controller{0};
hrt_abstime _heartbeat_type_gimbal{0};
hrt_abstime _heartbeat_type_adsb{0};
hrt_abstime _heartbeat_type_camera{0};
hrt_abstime _heartbeat_component_telemetry_radio{0};
hrt_abstime _heartbeat_component_log{0};
hrt_abstime _heartbeat_component_osd{0};
hrt_abstime _heartbeat_component_obstacle_avoidance{0};
hrt_abstime _heartbeat_component_visual_inertial_odometry{0};
hrt_abstime _heartbeat_component_pairing_manager{0};
hrt_abstime _heartbeat_component_udp_bridge{0};
hrt_abstime _heartbeat_component_uart_bridge{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,