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;
warnx("waiting for instances to stop");
while (inst != nullptr) {
printf("instance #%u:\n", iterations);
printf("\ninstance #%u:\n", iterations);
inst->display_status();
/* move on */
@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[])
void
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) {
printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time);
printf("\ttype:\t\t");
switch (_rstatus.type) {
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
printf("\t3DR RADIO\n");
printf("3DR RADIO\n");
break;
default:
printf("\tUNKNOWN RADIO\n");
printf("UNKNOWN RADIO\n");
break;
}
printf("\trssi:\t%d\t\n", _rstatus.rssi);
printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi);
printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf);
printf("\tnoise:\t%d\tus\n", _rstatus.noise);
printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise);
printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors);
printf("\tfixed:\t%u\tus\n", _rstatus.fixed);
printf("\trssi:\t\t%d\n", _rstatus.rssi);
printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
printf("\tnoise:\t\t%d\n", _rstatus.noise);
printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
printf("\trx errors:\t%u\n", _rstatus.rxerrors);
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),
_rc_pub(-1),
_manual_pub(-1),
_telemetry_heartbeat_time(0),
_radio_status_available(false),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_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();
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.rssi = rstatus.rssi;
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 */
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 (!_radio_status_available) {
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
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) {

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

@ -148,7 +148,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available;
int _control_mode_sub;
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 {
uint64_t timestamp;
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 */
uint8_t rssi; /**< local signal strength */
uint8_t remote_rssi; /**< remote signal strength */