diff options
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 51 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_main.h | 10 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 3 |
3 files changed, 59 insertions, 5 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3..e25851c7d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN 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); + } } int @@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f..d51120462 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include <uORB/uORB.h> #include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> +#include <uORB/topics/telemetry_status.h> #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -97,6 +98,8 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -229,6 +232,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34d..528c8b72a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); tstatus.heartbeat_time = _telemetry_heartbeat_time; |