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

MAVLink: Improve message handling / tracking 

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.
This commit is contained in:
Lorenz Meier 2017-07-23 18:28:02 +02:00
parent 556eb9e45a
commit 07ced9895c
8 changed files with 342 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/transponder_report.msg
View File

@ -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

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

@ -1355,23 +1355,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);
@ -1385,7 +1393,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;
}
@ -1404,7 +1412,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
}
/* if we reach here, the stream list does not contain the stream */
warnx("stream %s not found", stream_name);
PX4_WARN("stream %s not found", stream_name);
return PX4_ERROR;
}
@ -1413,7 +1421,7 @@ void
Mavlink::adjust_stream_rates(const float multiplier)
{
/* do not allow to push us to zero */
if (multiplier < 0.0005f) {
if (multiplier < MAVLINK_MIN_MULTIPLIER) {
return;
}
@ -1421,16 +1429,23 @@ Mavlink::adjust_stream_rates(const float multiplier)
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
/* set new interval */
unsigned interval = stream->get_interval();
interval /= multiplier;
int interval = stream->get_interval();
/* allow max ~2000 Hz */
if (interval < 1600) {
interval = 500;
if (interval > 0) {
interval /= multiplier;
/* limit min / max interval */
if (interval < MAVLINK_MIN_INTERVAL) {
interval = MAVLINK_MIN_INTERVAL;
}
if (interval > MAVLINK_MAX_INTERVAL) {
interval = MAVLINK_MAX_INTERVAL;
}
/* set new interval */
stream->set_interval(interval);
}
/* set new interval */
stream->set_interval(interval * multiplier);
}
}
@ -1975,8 +1990,8 @@ Mavlink::task_main(int argc, char *argv[])
/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
configure_stream("STATUSTEXT", 20.0f);
/* COMMAND_LONG stream: use high rate to avoid commands skipping */
configure_stream("COMMAND_LONG", 100.0f);
/* COMMAND_LONG stream: use unlimited rate to send all commands */
configure_stream("COMMAND_LONG");
}
@ -1990,7 +2005,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);
@ -2006,20 +2021,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", 500.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);
@ -2038,9 +2053,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;
@ -2077,7 +2091,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);
@ -2092,8 +2106,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;
@ -2109,14 +2123,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 */
@ -2147,53 +2161,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 */
@ -2261,7 +2229,7 @@ Mavlink::task_main(int argc, char *argv[])
/* check for requested subscriptions */
if (_subscribe_to_stream != nullptr) {
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
if (_subscribe_to_stream_rate > 0.0f) {
if (fabsf(_subscribe_to_stream_rate) > 0.00001f) {
if (get_protocol() == SERIAL) {
PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
(double)_subscribe_to_stream_rate);
@ -2422,6 +2390,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 */

33
src/modules/mavlink/mavlink_main.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,14 +33,17 @@
/**
* @file mavlink_main.h
* MAVLink 1.0 protocol interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* MAVLink 2.0 protocol interface definition.
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include <px4_posix.h>
#include <stdbool.h>
#ifdef __PX4_NUTTX
#include <nuttx/fs/fs.h>
@ -309,7 +312,7 @@ public:
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 */
@ -431,7 +434,7 @@ public:
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; }
@ -455,6 +458,9 @@ public:
if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; }
}
void set_uorb_main_fd(int fd, unsigned int interval);
protected:
Mavlink *next;
@ -464,7 +470,10 @@ private:
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;
static constexpr float MAVLINK_MIN_MULTIPLIER = 0.0005f;
mavlink_message_t _mavlink_buffer;
mavlink_status_t _mavlink_status;
@ -590,13 +599,13 @@ private:
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
@ -619,6 +628,14 @@ private:
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.
*/

236
src/modules/mavlink/mavlink_messages.cpp
View File

@ -318,7 +318,7 @@ protected:
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_status_s status = {};
@ -335,6 +335,8 @@ protected:
mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
base_mode, custom_mode, system_status);
return true;
}
};
@ -383,7 +385,7 @@ protected:
~MavlinkStreamStatustext() {}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
if (!_mavlink->get_logbuffer()->empty() && _mavlink->is_connected()) {
@ -397,8 +399,12 @@ protected:
msg.text[sizeof(msg.text) - 1] = '\0';
mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
return true;
}
}
return false;
}
};
@ -449,9 +455,10 @@ protected:
_cmd_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_command_s cmd;
bool sent = false;
if (_cmd_sub->update(&_cmd_time, &cmd)) {
@ -464,6 +471,7 @@ protected:
}
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
sent = true;
} else {
if (_mavlink->verbose()) {
@ -473,6 +481,8 @@ protected:
}
MavlinkCommandSender::instance().check_timeout(_mavlink->get_channel());
return sent;
}
};
@ -525,7 +535,7 @@ protected:
_battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status)))
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_status_s status = {};
struct cpuload_s cpuload = {};
@ -576,7 +586,11 @@ protected:
}
mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg);
return true;
}
return false;
}
};
@ -642,7 +656,7 @@ protected:
_baro_timestamp(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct sensor_combined_s sensor = {};
struct differential_pressure_s differential_pressure = {};
@ -695,7 +709,11 @@ protected:
msg.fields_updated = fields_updated;
mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -748,7 +766,7 @@ protected:
_att_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
@ -764,7 +782,11 @@ protected:
msg.yawspeed = att.yawspeed;
mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -816,7 +838,7 @@ protected:
_att_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
@ -833,7 +855,11 @@ protected:
msg.yawspeed = att.yawspeed;
mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -916,7 +942,7 @@ protected:
_sensor_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_attitude_s att = {};
struct vehicle_global_position_s pos = {};
@ -966,7 +992,11 @@ protected:
msg.climb = -pos.vel_d;
mavlink_msg_vfr_hud_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1018,7 +1048,7 @@ protected:
_gps_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_gps_position_s gps;
@ -1042,7 +1072,11 @@ protected:
msg.satellites_visible = gps.satellites_used;
mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1088,7 +1122,7 @@ protected:
explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
mavlink_system_time_t msg = {};
timespec tv;
@ -1099,6 +1133,8 @@ protected:
msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
mavlink_msg_system_time_send_struct(_mavlink->get_channel(), &msg);
return true;
}
};
@ -1144,7 +1180,7 @@ protected:
explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
mavlink_timesync_t msg = {};
@ -1152,6 +1188,8 @@ protected:
msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &msg);
return true;
}
};
@ -1183,7 +1221,7 @@ public:
return new MavlinkStreamADSBVehicle(mavlink);
}
virtual bool const_rate()
bool const_rate()
{
return true;
}
@ -1207,9 +1245,10 @@ protected:
_pos_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct transponder_report_s pos;
bool sent = false;
while (_pos_sub->update(&_pos_time, &pos)) {
mavlink_adsb_vehicle_t msg = {};
@ -1229,7 +1268,10 @@ protected:
msg.squawk = pos.squawk;
mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
sent = true;
}
return sent;
}
};
@ -1280,11 +1322,12 @@ protected:
_collision_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct collision_report_s report;
bool sent = false;
if (_collision_sub->update(&_collision_time, &report)) {
while (_collision_sub->update(&_collision_time, &report)) {
mavlink_collision_t msg = {};
msg.src = report.src;
@ -1296,7 +1339,10 @@ protected:
msg.horizontal_minimum_delta = report.horizontal_minimum_delta;
mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg);
sent = true;
}
return sent;
}
};
@ -1328,7 +1374,7 @@ public:
return new MavlinkStreamCameraTrigger(mavlink);
}
virtual bool const_rate()
bool const_rate()
{
return true;
}
@ -1352,7 +1398,7 @@ protected:
_trigger_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct camera_trigger_s trigger;
@ -1400,8 +1446,12 @@ protected:
digicam_ctrl_cmd.param7 = NAN;
mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &digicam_ctrl_cmd);
return true;
}
}
return false;
}
};
@ -1457,7 +1507,7 @@ protected:
_capture_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct camera_capture_s capture;
@ -1481,7 +1531,11 @@ protected:
msg.file_url[0] = '\0';
mavlink_msg_camera_image_captured_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1537,7 +1591,7 @@ protected:
_home_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_global_position_s pos = {};
struct home_position_s home = {};
@ -1560,6 +1614,8 @@ protected:
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
}
return updated;
}
};
@ -1615,7 +1671,7 @@ protected:
_att_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_local_position_s vpos = {};
struct vehicle_attitude_s vatt = {};
@ -1637,6 +1693,8 @@ protected:
mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
}
return (pos_updated || att_updated);
}
};
@ -1687,7 +1745,7 @@ protected:
_pos_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_local_position_s pos;
@ -1703,7 +1761,11 @@ protected:
msg.vz = pos.vz;
mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1755,7 +1817,7 @@ protected:
_est_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct estimator_status_s est = {};
@ -1782,7 +1844,11 @@ protected:
msg.covariance[11] = est.timeout_flags;
mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1833,7 +1899,7 @@ protected:
_est_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct estimator_status_s est;
@ -1863,7 +1929,11 @@ protected:
msg.vibration_z = est.vibe[2];
mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1914,7 +1984,7 @@ protected:
_mocap_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct att_pos_mocap_s mocap;
@ -1931,7 +2001,11 @@ protected:
msg.z = mocap.z;
mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -1981,7 +2055,7 @@ protected:
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
/* we're sending the GPS home periodically to ensure the
* the GCS does pick it up at one point */
@ -2012,8 +2086,12 @@ protected:
msg.approach_z = 0.0f;
mavlink_msg_home_position_send_struct(_mavlink->get_channel(), &msg);
return true;
}
}
return false;
}
};
@ -2080,7 +2158,7 @@ protected:
_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct actuator_outputs_s act;
@ -2099,7 +2177,11 @@ protected:
msg.servo8_raw = act.output[7];
mavlink_msg_servo_output_raw_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -2182,7 +2264,7 @@ protected:
}
}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct actuator_controls_s att_ctrl;
@ -2197,7 +2279,11 @@ protected:
}
mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -2255,7 +2341,7 @@ protected:
_act_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct actuator_outputs_s act;
@ -2363,6 +2449,8 @@ protected:
mavlink_msg_hil_controls_send_struct(_mavlink->get_channel(), &msg);
}
return updated;
}
};
@ -2418,7 +2506,7 @@ protected:
_act_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct actuator_outputs_s act;
@ -2521,6 +2609,8 @@ protected:
mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg);
}
return (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED));
}
};
@ -2569,7 +2659,7 @@ protected:
_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct position_setpoint_triplet_s pos_sp_triplet;
@ -2583,7 +2673,11 @@ protected:
msg.alt = pos_sp_triplet.current.alt;
mavlink_msg_position_target_global_int_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -2635,7 +2729,7 @@ protected:
_pos_sp_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_local_position_setpoint_s pos_sp;
@ -2656,7 +2750,11 @@ protected:
msg.afz = pos_sp.acc_z;
mavlink_msg_position_target_local_ned_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -2712,7 +2810,7 @@ protected:
_att_rates_sp_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_attitude_setpoint_s att_sp = {};
@ -2739,7 +2837,11 @@ protected:
msg.thrust = att_sp.thrust;
mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -2791,7 +2893,7 @@ protected:
_rc_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct rc_input_values rc = {};
@ -2840,7 +2942,11 @@ protected:
over.chan8_raw = msg.chan8_raw;
mavlink_msg_rc_channels_override_send_struct(_mavlink->get_channel(), &over);
return true;
}
return false;
}
};
@ -2892,7 +2998,7 @@ protected:
_manual_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct manual_control_setpoint_s manual = {};
@ -2914,7 +3020,11 @@ protected:
msg.buttons |= (manual.offboard_switch << (shift * 5));
mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -2965,7 +3075,7 @@ protected:
_flow_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct optical_flow_s flow = {};
@ -2987,7 +3097,11 @@ protected:
msg.temperature = flow.gyro_temperature;
mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -3038,7 +3152,7 @@ protected:
_debug_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct debug_key_value_s debug = {};
@ -3052,7 +3166,11 @@ protected:
msg.value = debug.value;
mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -3104,7 +3222,7 @@ protected:
_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status)))
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status = {};
struct tecs_status_s _tecs_status = {};
@ -3125,7 +3243,11 @@ protected:
msg.aspd_error = _tecs_status.airspeed_filtered - _tecs_status.airspeedSp;
mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -3174,7 +3296,7 @@ protected:
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_status_s status = {};
(void)_status_sub->update(&status);
@ -3196,6 +3318,8 @@ protected:
msg.param7 = 0;
mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &msg);
return true;
}
};
@ -3246,7 +3370,7 @@ protected:
_dist_sensor_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct distance_sensor_s dist_sensor = {};
@ -3284,7 +3408,11 @@ protected:
msg.covariance = dist_sensor.covariance;
mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -3344,7 +3472,7 @@ protected:
_msg.landed_state = MAV_LANDED_STATE_UNDEFINED;
}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct vehicle_status_s status = {};
struct vehicle_land_detected_s land_detected = {};
@ -3400,6 +3528,8 @@ protected:
if (updated) {
mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg);
}
return updated;
}
};
@ -3465,7 +3595,7 @@ protected:
_sensor_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
mavlink_altitude_t msg = {};
bool updated = false;
@ -3538,6 +3668,8 @@ protected:
mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);
}
return updated;
}
};
@ -3593,7 +3725,7 @@ protected:
_global_pos_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct wind_estimate_s wind_estimate = {};
@ -3621,7 +3753,11 @@ protected:
msg.vert_accuracy = 0.0f;
mavlink_msg_wind_cov_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -3672,7 +3808,7 @@ protected:
_mount_orientation_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct mount_orientation_s mount_orientation = {};
@ -3687,7 +3823,11 @@ protected:
msg.yaw = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[2];
mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@ -3806,7 +3946,7 @@ protected:
_tecs_time(0)
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
struct actuator_controls_s actuator = {};
struct airspeed_s airspeed = {};
@ -3893,6 +4033,8 @@ protected:
mavlink_msg_high_latency_send_struct(_mavlink->get_channel(), &msg);
}
return updated;
}
};
@ -3953,7 +4095,7 @@ protected:
_gpos()
{}
void send(const hrt_abstime t)
bool send(const hrt_abstime t)
{
bool att_updated = _att_sub->update(&_att_time, &_att);
bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos);
@ -3987,7 +4129,11 @@ protected:
}
mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};

2
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -93,6 +93,8 @@ public:
orb_id_t get_topic() const;
int get_instance() const;
int get_fd() { return _fd; }
private:
const orb_id_t _topic; ///< topic metadata
int _fd; ///< subscription handle

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

@ -2084,7 +2084,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
t.flags = adsb.flags;
t.squawk = adsb.squawk;
//warnx("code: %d callsign: %s, vel: %8.4f", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity);
//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);
if (_transponder_report_pub == nullptr) {
_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);

27
src/modules/mavlink/mavlink_stream.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,6 +36,7 @@
* Mavlink messages stream implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <stdlib.h>
@ -59,7 +60,7 @@ MavlinkStream::~MavlinkStream()
* Set messages interval in ms
*/
void
MavlinkStream::set_interval(const unsigned int interval)
MavlinkStream::set_interval(const int interval)
{
_interval = interval;
}
@ -78,7 +79,7 @@ MavlinkStream::update(const hrt_abstime t)
// on the link scheduling
_last_sent = hrt_absolute_time();
#ifndef __PX4_QURT
send(t);
(void)send(t);
#endif
return 0;
}
@ -90,13 +91,13 @@ MavlinkStream::update(const hrt_abstime t)
}
int64_t dt = t - _last_sent;
int interval = _interval;
int interval = (_interval > 0) ? _interval : 0;
if (!const_rate()) {
interval /= _mavlink->get_rate_mult();
}
// send the message if it is due or
// Send the message if it is due or
// if it will overrun the next scheduled send interval
// by 40% of the interval time. This helps to avoid
// sending a scheduled message on average slower than
@ -106,17 +107,23 @@ MavlinkStream::update(const hrt_abstime t)
// This method is not theoretically optimal but a suitable
// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
if (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4)) {
if (interval == 0 || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4))) {
// interval expired, send message
bool sent = true;
#ifndef __PX4_QURT
send(t);
sent = send(t);
#endif
// if the interval is non-zero do not use the actual time but
// If the interval is non-zero do not use the actual time but
// increment at a fixed rate, so that processing delays do not
// distort the average rate
_last_sent = (interval > 0) ? _last_sent + interval : t;
if (sent) {
_last_sent = (interval > 0) ? _last_sent + interval : t;
return 0;
return 0;
} else {
return -1;
}
}
return -1;

10
src/modules/mavlink/mavlink_stream.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -59,14 +59,14 @@ public:
*
* @param interval the inveral in microseconds (us) between messages
*/
void set_interval(const unsigned int interval);
void set_interval(const int interval);
/**
* Get the interval
*
* @return the inveral in microseconds (us) between messages
*/
unsigned get_interval() { return _interval; }
int get_interval() { return _interval; }
/**
* @return 0 if updated / sent, -1 if unchanged
@ -97,10 +97,10 @@ public:
protected:
Mavlink *_mavlink;
unsigned int _interval; ///< if set to zero = unlimited rate
int _interval; ///< if set to negative value = unlimited rate
#ifndef __PX4_QURT
virtual void send(const hrt_abstime t) = 0;
virtual bool send(const hrt_abstime t) = 0;
#endif
private: