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

Publish telemetry status on telemetry update and on heartbeat update events to avoid inducing heartbeat update latencies resulting in spurious telemetry link dropped detections. Makes overall state handling simpler

This commit is contained in:
Lorenz Meier 2014-08-17 10:58:28 +02:00
parent 5a5e1a976e
commit c3522f8592
3 changed files with 11 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -217,6 +217,8 @@ Mavlink::Mavlink() :
errx(1, "instance ID is out of range"); errx(1, "instance ID is out of range");
break; break;
} }
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
} }
Mavlink::~Mavlink() Mavlink::~Mavlink()

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

@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1), _telemetry_status_pub(-1),
_rc_pub(-1), _rc_pub(-1),
_manual_pub(-1), _manual_pub(-1),
_radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0), _hil_frames(0),
_old_timestamp(0), _old_timestamp(0),
@ -430,9 +429,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else { } else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
} }
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
} }
} }
@ -474,25 +470,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time(); /* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = tstatus.timestamp;
/* always set heartbeat, publish only if telemetry link not up */ if (_telemetry_status_pub < 0) {
tstatus.heartbeat_time = tnow; _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
/* if no radio status messages arrive, lets at least publish that heartbeats were received */ } else {
if (!_radio_status_available) { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
tstatus.timestamp = tnow;
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
tstatus.telem_time = 0;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
} }
} }
} }

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

@ -151,7 +151,6 @@ private:
orb_advert_t _telemetry_status_pub; orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub; orb_advert_t _rc_pub;
orb_advert_t _manual_pub; orb_advert_t _manual_pub;
bool _radio_status_available;
int _control_mode_sub; int _control_mode_sub;
int _hil_frames; int _hil_frames;
uint64_t _old_timestamp; uint64_t _old_timestamp;