forked from Archive/PX4-Autopilot
Cleanup of heartbeat handling and status printing. Ready to go mainline
This commit is contained in:
parent
0c0b1a4c66
commit
91becef344
|
@ -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");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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 */
|
||||
|
|
Loading…
Reference in New Issue