fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-13 14:51:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink expand telemetry_status and split radio_status

Browse Source
This commit is contained in:
Daniel Agar
2018-08-16 17:32:15 +02:00
committed by Lorenz Meier
parent f4f112424f
commit 36403e9025
8 changed files with 190 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+21 -66
View File
@@ -89,6 +89,7 @@
#include <commander/px4_custom_mode.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "mavlink_bridge_header.h"
@@ -132,7 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_pos_mocap_pub(nullptr),
_vision_position_pub(nullptr),
_vision_attitude_pub(nullptr),
_telemetry_status_pub(nullptr),
_radio_status_pub(nullptr),
_ping_pub(nullptr),
_rc_pub(nullptr),
_manual_pub(nullptr),
@@ -193,25 +194,13 @@ void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t comman
void
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
if (!_mavlink->get_config_link_on()) {
if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_CONFIG) {
_mavlink->set_config_link_on(true);
}
}
switch (msg->msgid) {
case MAVLINK_MSG_ID_COMMAND_LONG:
if (_mavlink->accepting_commands()) {
handle_message_command_long(msg);
}
handle_message_command_long(msg);
break;
case MAVLINK_MSG_ID_COMMAND_INT:
if (_mavlink->accepting_commands()) {
handle_message_command_int(msg);
}
handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_COMMAND_ACK:
@@ -227,10 +216,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
case MAVLINK_MSG_ID_SET_MODE:
if (_mavlink->accepting_commands()) {
handle_message_set_mode(msg);
}
handle_message_set_mode(msg);
break;
case MAVLINK_MSG_ID_ATT_POS_MOCAP:
@@ -1426,29 +1412,21 @@ 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 = _mavlink->get_rx_status();
radio_status_s status = {};
status.timestamp = hrt_absolute_time();
status.type = radio_status_s::RADIO_TYPE_3DR_RADIO;
status.rssi = rstatus.rssi;
status.remote_rssi = rstatus.remrssi;
status.txbuf = rstatus.txbuf;
status.noise = rstatus.noise;
status.remote_noise = rstatus.remnoise;
status.rxerrors = rstatus.rxerrors;
status.fixed = rstatus.fixed;
tstatus.timestamp = hrt_absolute_time();
tstatus.telem_time = tstatus.timestamp;
/* tstatus.heartbeat_time is set by system heartbeats */
tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
tstatus.noise = rstatus.noise;
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
tstatus.system_id = msg->sysid;
tstatus.component_id = msg->compid;
_mavlink->update_radio_status(status);
if (_telemetry_status_pub == nullptr) {
int multi_instance;
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
int multi_instance;
orb_publish_auto(ORB_ID(radio_status), &_radio_status_pub, &status, &multi_instance, ORB_PRIO_HIGH);
}
}
@@ -1895,21 +1873,14 @@ 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) {
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
/* 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;
if (_telemetry_status_pub == nullptr) {
int multi_instance;
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
tstatus.system_id = msg->sysid;
tstatus.component_id = msg->compid;
}
}
}
@@ -2538,22 +2509,6 @@ MavlinkReceiver::receive_thread(void *arg)
// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
const int timeout = 10;
// publish the telemetry status once for the iridium telemetry
if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
tstatus.timestamp = hrt_absolute_time();
tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
if (_telemetry_status_pub == nullptr) {
int multi_instance;
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
#ifdef __PX4_POSIX
/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
uint8_t buf[1600 * 5];