fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

HEARTBEAT and commander failsafe handling cleanup

Browse Source
This commit is contained in:
Daniel Agar
2019-03-12 11:24:33 +01:00
committed by Beat Küng
parent f794ee0c8a
commit 6dec451bab
10 changed files with 316 additions and 407 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+11 -9
View File
@@ -699,7 +699,7 @@ Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_cont
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB);
set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
}
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@@ -1580,7 +1580,7 @@ Mavlink::update_rate_mult()
// check for RADIO_STATUS timeout and reset
if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) {
PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC);
set_telemetry_status_type(telemetry_status_s::LINK_TYPE_GENERIC);
_radio_status_available = false;
_radio_status_critical = false;
@@ -1601,7 +1601,7 @@ void
Mavlink::update_radio_status(const radio_status_s &radio_status)
{
_rstatus = radio_status;
set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO);
set_telemetry_status_type(telemetry_status_s::LINK_TYPE_3DR_RADIO);
/* check hardware limits */
_radio_status_available = true;
@@ -2022,7 +2022,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "iridium") == 0) {
_mode = MAVLINK_MODE_IRIDIUM;
set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM);
set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM);
} else if (strcmp(myoptarg, "minimal") == 0) {
_mode = MAVLINK_MODE_MINIMAL;
@@ -2233,14 +2233,16 @@ Mavlink::task_main(int argc, char *argv[])
set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
if (_mode == MAVLINK_MODE_IRIDIUM) {
if (_transmitting_enabled &&
!status.high_latency_data_link_active &&
status.high_latency_data_link_lost &&
!_transmitting_enabled_commanded &&
(_first_heartbeat_sent)) {
_transmitting_enabled = false;
mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
} else if (!_transmitting_enabled && status.high_latency_data_link_active) {
} else if (!_transmitting_enabled && !status.high_latency_data_link_lost) {
_transmitting_enabled = true;
mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
}
@@ -2558,7 +2560,7 @@ void Mavlink::check_radio_config()
{
/* radio config check */
if (_uart_fd >= 0 && _param_radio_id.get() != 0
&& _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
&& _tstatus.type == telemetry_status_s::LINK_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
@@ -2702,7 +2704,7 @@ Mavlink::display_status()
printf("\ttype:\t\t");
switch (_tstatus.type) {
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
case telemetry_status_s::LINK_TYPE_3DR_RADIO:
printf("3DR RADIO\n");
printf("\t rssi:\t\t%d\n", _rstatus.rssi);
printf("\t remote rssi:\t%u\n", _rstatus.remote_rssi);
@@ -2713,7 +2715,7 @@ Mavlink::display_status()
printf("\t fixed:\t%u\n", _rstatus.fix);
break;
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
case telemetry_status_s::LINK_TYPE_USB:
printf("USB CDC\n");
break;
src/modules/mavlink/mavlink_receiver.cpp
+7 -5
View File
@@ -1882,17 +1882,19 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
// ignore own heartbeats
if ((msg->sysid != mavlink_system.sysid) && (msg->compid != mavlink_system.compid)) {
telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.heartbeat_time = tstatus.timestamp;
tstatus.system_id = msg->sysid;
tstatus.component_id = msg->compid;
tstatus.heartbeat_time = hrt_absolute_time();
tstatus.remote_system_id = msg->sysid;
tstatus.remote_component_id = msg->compid;
tstatus.remote_type = hb.type;
tstatus.remote_system_status = hb.system_status;
}
}
}