Skip to content

Commit

Permalink
MAVLink: Improve message handling / tracking
Browse files Browse the repository at this point in the history 
The message handling was not obeying action focused messages and high-rate messages properly before. With this change update rates track the desired rates closely. Critical high-rate messages such as ADS-B are queued additionally to guarantee that all received packets are being correctly forwarded.
  • Loading branch information
@LorenzMeier
LorenzMeier committed Jul 23, 2017
1 parent 286c3d4 commit 34da0d8
Show file tree
Hide file tree
Showing 8 changed files with 310 additions and 128 deletions.
2 changes: 1 addition & 1 deletion msg/transponder_report.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,4 @@ uint8 tslc # Time since last communication in seconds
uint16 flags # Flags to indicate various statuses including valid data fields
uint16 squawk # Squawk code

uint32 ORB_QUEUE_LENGTH = 3
uint32 ORB_QUEUE_LENGTH = 10
147 changes: 80 additions & 67 deletions src/modules/mavlink/mavlink_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1348,23 +1348,31 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int
return sub_new;
}

unsigned int
int
Mavlink::interval_from_rate(float rate)
{
return (rate > 0.0f) ? (1000000.0f / rate) : 0;
if (rate > 0.000001f) {
return (1000000.0f / rate);

} else if (rate < 0.0f) {
return -1;

} else {
return 0;
}
}

int
Mavlink::configure_stream(const char *stream_name, const float rate)
{
/* calculate interval in us, 0 means disabled stream */
unsigned int interval = interval_from_rate(rate);
/* calculate interval in us, -1 means unlimited stream, 0 means disabled */
int interval = interval_from_rate(rate);

/* search if stream exists */
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
if (strcmp(stream_name, stream->get_name()) == 0) {
if (interval > 0) {
if (interval != 0) {
/* set new interval */
stream->set_interval(interval);

Expand All @@ -1378,7 +1386,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
}
}

if (interval <= 0) {
if (interval == 0) {
/* stream was not active and is requested to be disabled, do nothing */
return OK;
}
Expand Down Expand Up @@ -1983,7 +1991,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream("ALTITUDE", 1.0f);
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("ADSB_VEHICLE", 2.0f);
configure_stream("ADSB_VEHICLE");
configure_stream("COLLISION", 2.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW_RAD", 1.0f);
Expand All @@ -1999,19 +2007,20 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
configure_stream("VFR_HUD", 4.0f);
configure_stream("WIND_COV", 1.0f);
configure_stream("CAMERA_IMAGE_CAPTURED");
break;

case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 5.0f);
configure_stream("EXTENDED_SYS_STATE", 5.0f);
configure_stream("HIGHRES_IMU", 50.0f);
configure_stream("ATTITUDE", 250.0f);
configure_stream("ATTITUDE", 100.0f);
configure_stream("ATTITUDE_QUATERNION", 50.0f);
configure_stream("RC_CHANNELS", 20.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 10.0f);
configure_stream("ALTITUDE", 10.0f);
configure_stream("GPS_RAW_INT", 5.0f);
configure_stream("ADSB_VEHICLE", 10.0f);
configure_stream("ADSB_VEHICLE");
configure_stream("COLLISION", 10.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
Expand All @@ -2030,9 +2039,8 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SYSTEM_TIME", 1.0f);
configure_stream("TIMESYNC", 10.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
//camera trigger is rate limited at the source, do not limit here
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
configure_stream("CAMERA_TRIGGER");
configure_stream("CAMERA_IMAGE_CAPTURED");
configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
break;

Expand Down Expand Up @@ -2069,7 +2077,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream("ALTITUDE", 10.0f);
configure_stream("GPS_RAW_INT", 10.0f);
configure_stream("ADSB_VEHICLE", 20.0f);
configure_stream("ADSB_VEHICLE");
configure_stream("COLLISION", 20.0f);
configure_stream("DISTANCE_SENSOR", 10.0f);
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
Expand All @@ -2084,8 +2092,8 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
configure_stream("VFR_HUD", 20.0f);
configure_stream("WIND_COV", 10.0f);
configure_stream("CAMERA_TRIGGER", 500.0f);
configure_stream("CAMERA_IMAGE_CAPTURED", 5.0f);
configure_stream("CAMERA_TRIGGER");
configure_stream("CAMERA_IMAGE_CAPTURED");
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
configure_stream("MANUAL_CONTROL", 5.0f);
break;
Expand All @@ -2101,14 +2109,14 @@ Mavlink::task_main(int argc, char *argv[])
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;

/* hard limit to 500 Hz at max */
if (_main_loop_delay < 2000) {
_main_loop_delay = 2000;
/* hard limit to 1000 Hz at max */
if (_main_loop_delay < MAVLINK_MIN_INTERVAL) {
_main_loop_delay = MAVLINK_MIN_INTERVAL;
}

/* hard limit to 100 Hz at least */
if (_main_loop_delay > 10000) {
_main_loop_delay = 10000;
if (_main_loop_delay > MAVLINK_MAX_INTERVAL) {
_main_loop_delay = MAVLINK_MAX_INTERVAL;
}

/* now the instance is fully initialized and we can bump the instance count */
Expand Down Expand Up @@ -2139,53 +2147,7 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
}

/* radio config check */
if (_uart_fd >= 0 && _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");

if (fs) {
/* switch to AT command mode */
usleep(1200000);
fprintf(fs, "+++\n");
usleep(1200000);

if (_radio_id > 0) {
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);

} else {
/* reset to factory defaults */
fprintf(fs, "AT&F\n");
usleep(200000);
}

/* write config */
fprintf(fs, "AT&W");
usleep(200000);

/* reboot */
fprintf(fs, "ATZ");
usleep(200000);

// XXX NuttX suffers from a bug where
// fclose() also closes the fd, not just
// the file stream. Since this is a one-time
// config thing, we leave the file struct
// allocated.
#ifndef __PX4_NUTTX
fclose(fs);
#endif

} else {
PX4_WARN("open fd %d failed", _uart_fd);
}

/* reset param and save */
_radio_id = 0;
param_set(_param_radio_id, &_radio_id);
}
check_radio_config();

if (status_sub->update(&status_time, &status)) {
/* switch HIL mode if required */
Expand Down Expand Up @@ -2414,6 +2376,57 @@ Mavlink::task_main(int argc, char *argv[])
return OK;
}

void Mavlink::check_radio_config()
{
/* radio config check */
if (_uart_fd >= 0 && _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");

if (fs) {
/* switch to AT command mode */
usleep(1200000);
fprintf(fs, "+++\n");
usleep(1200000);

if (_radio_id > 0) {
/* set channel */
fprintf(fs, "ATS3=%u\n", _radio_id);
usleep(200000);

} else {
/* reset to factory defaults */
fprintf(fs, "AT&F\n");
usleep(200000);
}

/* write config */
fprintf(fs, "AT&W");
usleep(200000);

/* reboot */
fprintf(fs, "ATZ");
usleep(200000);

// XXX NuttX suffers from a bug where
// fclose() also closes the fd, not just
// the file stream. Since this is a one-time
// config thing, we leave the file struct
// allocated.
#ifndef __PX4_NUTTX
fclose(fs);
#endif

} else {
PX4_WARN("open fd %d failed", _uart_fd);
}

/* reset param and save */
_radio_id = 0;
param_set(_param_radio_id, &_radio_id);
}
}

int Mavlink::start_helper(int argc, char *argv[])
{
/* create the instance in task context */
Expand Down
25 changes: 20 additions & 5 deletions src/modules/mavlink/mavlink_main.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@

#pragma once

#include <px4_posix.h>

#include <stdbool.h>
#ifdef __PX4_NUTTX
#include <nuttx/fs/fs.h>
Expand Down Expand Up @@ -309,7 +311,7 @@ class Mavlink

mavlink_channel_t get_channel();

void configure_stream_threadsafe(const char *stream_name, float rate);
void configure_stream_threadsafe(const char *stream_name, float rate = -1.0f);

bool _task_should_exit; /**< if true, mavlink task should exit */

Expand Down Expand Up @@ -431,7 +433,7 @@ class Mavlink

bool verbose() { return _verbose; }

int get_data_rate() { return _datarate; }
int get_data_rate() { return _datarate; }
void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }

uint64_t get_main_loop_delay() { return _main_loop_delay; }
Expand All @@ -455,6 +457,9 @@ class Mavlink
if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; }
}


void set_uorb_main_fd(int fd, unsigned int interval);

protected:
Mavlink *next;

Expand All @@ -464,7 +469,9 @@ class Mavlink
orb_advert_t _mavlink_log_pub;
bool _task_running;
static bool _boot_complete;
static const unsigned MAVLINK_MAX_INSTANCES = 4;
static constexpr unsigned MAVLINK_MAX_INSTANCES = 4;
static constexpr unsigned MAVLINK_MIN_INTERVAL = 1500;
static constexpr unsigned MAVLINK_MAX_INTERVAL = 10000;
mavlink_message_t _mavlink_buffer;
mavlink_status_t _mavlink_status;

Expand Down Expand Up @@ -590,13 +597,13 @@ class Mavlink
int mavlink_open_uart(int baudrate, const char *uart_name);
#endif

static unsigned int interval_from_rate(float rate);
static int interval_from_rate(float rate);

static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;

int configure_stream(const char *stream_name, const float rate);
int configure_stream(const char *stream_name, const float rate = -1.0f);

/**
* Adjust the stream rates based on the current rate
Expand All @@ -619,6 +626,14 @@ class Mavlink

void pass_message(const mavlink_message_t *msg);

/**
* Check the configuration of a connected radio
*
* This convenience function allows to re-configure a connected
* radio without removing it from the main system harness.
*/
void check_radio_config();

/**
* Update rate mult so total bitrate will be equal to _datarate.
*/
Expand Down
Loading

0 comments on commit 34da0d8

Please sign in to comment.