fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 17:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

telemetry_statur: use 4 separate topics

Browse Source
This commit is contained in:
Anton Babushkin
2014-07-06 16:08:37 +02:00
parent d4eae37e47
commit bd5d3ebf70
7 changed files with 163 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+43 -37
View File
@@ -421,32 +421,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
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;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = 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.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = 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;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
void
@@ -475,28 +478,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* 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();
/* 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();
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
/* 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));
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
}
}
}
}