dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Converted telemetry status topic to generated topic

This commit is contained in:
Lorenz Meier 2015-07-12 12:34:35 +02:00
parent c3d10c4cb3
commit 16cb9edf19
3 changed files with 46 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
msg/telemetry_status.msg Normal file
View File

@ -0,0 +1,18 @@
uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0
uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
uint64 timestamp
uint64 heartbeat_time # Time of last received heartbeat from remote system
uint64 telem_time # Time of last received telemetry status packet, 0 for none
uint8 type # type of the radio hardware
uint8 rssi # local signal strength
uint8 remote_rssi # remote signal strength
uint16 rxerrors # receive errors
uint16 fixed # count of error corrected packets
uint8 noise # background noise level
uint8 remote_noise # remote background noise level
uint8 txbuf # how full the tx buffer is as a percentage
uint8 system_id # system id of the remote system
uint8 component_id # component id of the remote system

6
src/modules/mavlink/mavlink_main.cpp
View File

@ -246,7 +246,7 @@ Mavlink::Mavlink() :
break;
}
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
@ -1721,7 +1721,7 @@ Mavlink::task_main(int argc, char *argv[])
}
/* radio config check */
if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
if (_radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
@ -2013,7 +2013,7 @@ Mavlink::display_status()
printf("\ttype:\t\t");
switch (_rstatus.type) {
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
printf("3DR RADIO\n");
break;

34
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -281,7 +281,21 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
/* evaluate if this system should accept this command */
bool target_ok;
switch (cmd_mavlink.command) {
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
/* broadcast */
target_ok = (cmd_mavlink.target_system == 0);
break;
default:
target_ok = (cmd_mavlink.target_system == mavlink_system.sysid);
break;
}
if (target_ok && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
@ -942,8 +956,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
/* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */
if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) {
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
@ -952,7 +966,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.timestamp = hrt_absolute_time();
tstatus.telem_time = tstatus.timestamp;
/* tstatus.heartbeat_time is set by system heartbeats */
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
@ -964,10 +978,11 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.component_id = msg->compid;
if (_telemetry_status_pub == nullptr) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
int multi_instance;
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
}
@ -1156,7 +1171,7 @@ void
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
if (_mavlink->get_channel() < ORB_MULTI_MAX_INSTANCES) {
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
@ -1172,10 +1187,11 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
tstatus.heartbeat_time = tstatus.timestamp;
if (_telemetry_status_pub == nullptr) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
int multi_instance;
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else {
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
}