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

Cleanup of heartbeat handling and status printing. Ready to go mainline

This commit is contained in:
Lorenz Meier 2014-07-19 15:56:37 +02:00
parent 730a520362
commit ed31d6a59a
4 changed files with 32 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -432,11 +432,9 @@ Mavlink::get_status_all_instances()
unsigned iterations = 0; unsigned iterations = 0;
warnx("waiting for instances to stop");
while (inst != nullptr) { while (inst != nullptr) {
printf("instance #%u:\n", iterations); printf("\ninstance #%u:\n", iterations);
inst->display_status(); inst->display_status();
/* move on */ /* move on */
@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[])
void void
Mavlink::display_status() Mavlink::display_status()
{ {
warnx("running");
if (_rstatus.heartbeat_time > 0) {
printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
}
if (_rstatus.timestamp > 0) { if (_rstatus.timestamp > 0) {
printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time);
printf("\ttype:\t\t");
switch (_rstatus.type) { switch (_rstatus.type) {
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
printf("\t3DR RADIO\n"); printf("3DR RADIO\n");
break; break;
default: default:
printf("\tUNKNOWN RADIO\n"); printf("UNKNOWN RADIO\n");
break; break;
} }
printf("\trssi:\t%d\t\n", _rstatus.rssi); printf("\trssi:\t\t%d\n", _rstatus.rssi);
printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
printf("\tnoise:\t%d\tus\n", _rstatus.noise); printf("\tnoise:\t\t%d\n", _rstatus.noise);
printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); printf("\trx errors:\t%u\n", _rstatus.rxerrors);
printf("\tfixed:\t%u\tus\n", _rstatus.fixed); printf("\tfixed:\t\t%u\n", _rstatus.fixed);
} else {
printf("\tno telem status.\n");
} }
} }

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

@ -111,7 +111,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),
_telemetry_heartbeat_time(0),
_radio_status_available(false), _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),
@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
tstatus.timestamp = hrt_absolute_time(); tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time; tstatus.telem_time = tstatus.timestamp;
/* tstatus.heartbeat_time is set by system heartbeats */
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi; tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi; tstatus.remote_rssi = rstatus.remrssi;
@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
/* ignore own heartbeats, accept only heartbeats from GCS */ /* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
hrt_abstime tnow = hrt_absolute_time();
/* always set heartbeat, publish only if telemetry link not up */
tstatus.heartbeat_time = tnow;
/* if no radio status messages arrive, lets at least publish that heartbeats were received */ /* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) { if (!_radio_status_available) {
struct telemetry_status_s tstatus; tstatus.timestamp = tnow;
memset(&tstatus, 0, sizeof(tstatus)); /* telem_time indicates the timestamp of a telemetry status packet and we got none */
tstatus.telem_time = 0;
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) { if (_telemetry_status_pub < 0) {

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

@ -148,7 +148,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;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available; bool _radio_status_available;
int _control_mode_sub; int _control_mode_sub;
int _hil_frames; int _hil_frames;

1
src/modules/uORB/topics/telemetry_status.h
View File

@ -58,6 +58,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
struct telemetry_status_s { struct telemetry_status_s {
uint64_t timestamp; uint64_t timestamp;
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
uint8_t rssi; /**< local signal strength */ uint8_t rssi; /**< local signal strength */
uint8_t remote_rssi; /**< remote signal strength */ uint8_t remote_rssi; /**< remote signal strength */