forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
commit
45fe0164a3
2
NuttX
2
NuttX
|
@ -1 +1 @@
|
|||
Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df
|
||||
Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172
|
|
@ -74,7 +74,7 @@ MODULES += modules/commander
|
|||
MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
MODULES += modules/gpio_led
|
||||
#MODULES += modules/uavcan
|
||||
MODULES += modules/uavcan
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
|
|
|
@ -36,3 +36,5 @@
|
|||
#
|
||||
|
||||
SRCS = airspeed.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -38,3 +38,5 @@
|
|||
MODULE_COMMAND = bma180
|
||||
|
||||
SRCS = bma180.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -40,3 +40,5 @@ MODULE_COMMAND = ets_airspeed
|
|||
SRCS = ets_airspeed.cpp
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -38,3 +38,5 @@
|
|||
MODULE_COMMAND = hil
|
||||
|
||||
SRCS = hil.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -36,3 +36,5 @@
|
|||
#
|
||||
|
||||
SRCS = led.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -40,3 +40,5 @@ MODULE_COMMAND = md25
|
|||
|
||||
SRCS = md25.cpp \
|
||||
md25_main.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -42,3 +42,5 @@ SRCS = meas_airspeed.cpp
|
|||
MODULE_STACKSIZE = 1200
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -38,3 +38,5 @@
|
|||
MODULE_COMMAND = ms5611
|
||||
|
||||
SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -38,3 +38,5 @@
|
|||
MODULE_COMMAND = px4flow
|
||||
|
||||
SRCS = px4flow.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -39,3 +39,5 @@ MODULE_COMMAND = roboclaw
|
|||
|
||||
SRCS = roboclaw_main.cpp \
|
||||
RoboClaw.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -81,7 +81,7 @@
|
|||
#include "mavlink_commands.h"
|
||||
|
||||
#ifndef MAVLINK_CRC_EXTRA
|
||||
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
|
||||
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
|
||||
#endif
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
|
@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
instance = Mavlink::get_instance(6);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
int buf_free = 0;
|
||||
|
||||
if (instance->get_flow_control_enabled()
|
||||
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
||||
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
||||
|
||||
/* Disable hardware flow control:
|
||||
* if no successful write since a defined time
|
||||
* and if the last try was not the last successful write
|
||||
*/
|
||||
if (last_write_try_times[(unsigned)channel] != 0 &&
|
||||
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
|
||||
last_write_success_times[(unsigned)channel] !=
|
||||
last_write_try_times[(unsigned)channel])
|
||||
{
|
||||
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
|
||||
last_write_success_times[(unsigned)channel] !=
|
||||
last_write_try_times[(unsigned)channel]) {
|
||||
warnx("DISABLING HARDWARE FLOW CONTROL");
|
||||
instance->enable_flow_control(false);
|
||||
}
|
||||
|
@ -197,15 +197,20 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
if (buf_free < desired) {
|
||||
/* we don't want to send anything just in half, so return */
|
||||
instance->count_txerr();
|
||||
instance->count_txerrbytes(desired);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t ret = write(uart, ch, desired);
|
||||
|
||||
if (ret != desired) {
|
||||
instance->count_txerr();
|
||||
instance->count_txerrbytes(desired);
|
||||
|
||||
} else {
|
||||
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
|
||||
instance->count_txbytes(desired);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -232,32 +237,40 @@ Mavlink::Mavlink() :
|
|||
_mission_result_sub(-1),
|
||||
_mode(MAVLINK_MODE_NORMAL),
|
||||
_channel(MAVLINK_COMM_0),
|
||||
_logbuffer{},
|
||||
_total_counter(0),
|
||||
_receive_thread{},
|
||||
_verbose(false),
|
||||
_forwarding_on(false),
|
||||
_passing_on(false),
|
||||
_ftp_on(false),
|
||||
_uart_fd(-1),
|
||||
_baudrate(57600),
|
||||
_datarate(10000),
|
||||
_mavlink_param_queue_index(0),
|
||||
mavlink_link_termination_allowed(false),
|
||||
_subscribe_to_stream(nullptr),
|
||||
_subscribe_to_stream_rate(0.0f),
|
||||
_flow_control_enabled(true),
|
||||
_message_buffer{},
|
||||
_message_buffer_mutex{},
|
||||
_param_initialized(false),
|
||||
_param_system_id(0),
|
||||
_param_component_id(0),
|
||||
_param_system_type(0),
|
||||
_param_use_hil_gps(0),
|
||||
_logbuffer {},
|
||||
_total_counter(0),
|
||||
_receive_thread {},
|
||||
_verbose(false),
|
||||
_forwarding_on(false),
|
||||
_passing_on(false),
|
||||
_ftp_on(false),
|
||||
_uart_fd(-1),
|
||||
_baudrate(57600),
|
||||
_datarate(10000),
|
||||
_mavlink_param_queue_index(0),
|
||||
mavlink_link_termination_allowed(false),
|
||||
_subscribe_to_stream(nullptr),
|
||||
_subscribe_to_stream_rate(0.0f),
|
||||
_flow_control_enabled(true),
|
||||
_bytes_tx(0),
|
||||
_bytes_txerr(0),
|
||||
_bytes_rx(0),
|
||||
_bytes_timestamp(0),
|
||||
_rate_tx(0.0f),
|
||||
_rate_txerr(0.0f),
|
||||
_rate_rx(0.0f),
|
||||
_rstatus {},
|
||||
_message_buffer {},
|
||||
_message_buffer_mutex {},
|
||||
_param_initialized(false),
|
||||
_param_system_id(0),
|
||||
_param_component_id(0),
|
||||
_param_system_type(0),
|
||||
_param_use_hil_gps(0),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
|
||||
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
|
||||
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
|
||||
{
|
||||
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
|
||||
|
||||
|
@ -424,6 +437,27 @@ Mavlink::destroy_all_instances()
|
|||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::get_status_all_instances()
|
||||
{
|
||||
Mavlink *inst = ::_mavlink_instances;
|
||||
|
||||
unsigned iterations = 0;
|
||||
|
||||
while (inst != nullptr) {
|
||||
|
||||
printf("\ninstance #%u:\n", iterations);
|
||||
inst->display_status();
|
||||
|
||||
/* move on */
|
||||
inst = inst->next;
|
||||
iterations++;
|
||||
}
|
||||
|
||||
/* return an error if there are no instances */
|
||||
return (iterations == 0);
|
||||
}
|
||||
|
||||
bool
|
||||
Mavlink::instance_exists(const char *device_name, Mavlink *self)
|
||||
{
|
||||
|
@ -615,7 +649,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
|
|||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
|
||||
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
|
||||
baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
|
@ -845,8 +880,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
|
|||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
|
||||
mavlink_param_request_list_t req;
|
||||
mavlink_msg_param_request_list_decode(msg, &req);
|
||||
|
||||
if (req.target_system == mavlink_system.sysid &&
|
||||
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
|
||||
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
|
||||
/* Start sending parameters */
|
||||
mavlink_pm_start_queued_send();
|
||||
send_statustext_info("[pm] sending list");
|
||||
|
@ -861,7 +897,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
|
|||
mavlink_param_set_t mavlink_param_set;
|
||||
mavlink_msg_param_set_decode(msg, &mavlink_param_set);
|
||||
|
||||
if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
|
||||
if (mavlink_param_set.target_system == mavlink_system.sysid
|
||||
&& ((mavlink_param_set.target_component == mavlink_system.compid)
|
||||
|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
|
||||
/* local name buffer to enforce null-terminated string */
|
||||
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
|
||||
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
|
@ -888,7 +926,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
|
|||
mavlink_param_request_read_t mavlink_param_request_read;
|
||||
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
|
||||
|
||||
if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
|
||||
if (mavlink_param_request_read.target_system == mavlink_system.sysid
|
||||
&& ((mavlink_param_request_read.target_component == mavlink_system.compid)
|
||||
|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
|
||||
/* when no index is given, loop through string ids and compare them */
|
||||
if (mavlink_param_request_read.param_index == -1) {
|
||||
/* local name buffer to enforce null-terminated string */
|
||||
|
@ -951,15 +991,17 @@ Mavlink::send_statustext(unsigned severity, const char *string)
|
|||
|
||||
/* Map severity */
|
||||
switch (severity) {
|
||||
case MAVLINK_IOC_SEND_TEXT_INFO:
|
||||
statustext.severity = MAV_SEVERITY_INFO;
|
||||
break;
|
||||
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
|
||||
statustext.severity = MAV_SEVERITY_CRITICAL;
|
||||
break;
|
||||
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
|
||||
statustext.severity = MAV_SEVERITY_EMERGENCY;
|
||||
break;
|
||||
case MAVLINK_IOC_SEND_TEXT_INFO:
|
||||
statustext.severity = MAV_SEVERITY_INFO;
|
||||
break;
|
||||
|
||||
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
|
||||
statustext.severity = MAV_SEVERITY_CRITICAL;
|
||||
break;
|
||||
|
||||
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
|
||||
statustext.severity = MAV_SEVERITY_EMERGENCY;
|
||||
break;
|
||||
}
|
||||
|
||||
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
|
||||
|
@ -1076,12 +1118,14 @@ Mavlink::message_buffer_init(int size)
|
|||
_message_buffer.size = size;
|
||||
_message_buffer.write_ptr = 0;
|
||||
_message_buffer.read_ptr = 0;
|
||||
_message_buffer.data = (char*)malloc(_message_buffer.size);
|
||||
_message_buffer.data = (char *)malloc(_message_buffer.size);
|
||||
|
||||
int ret;
|
||||
|
||||
if (_message_buffer.data == 0) {
|
||||
ret = ERROR;
|
||||
_message_buffer.size = 0;
|
||||
|
||||
} else {
|
||||
ret = OK;
|
||||
}
|
||||
|
@ -1395,7 +1439,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
|
||||
configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
|
||||
configure_stream("ATTITUDE", 10.0f * rate_mult);
|
||||
configure_stream("VFR_HUD", 10.0f * rate_mult);
|
||||
configure_stream("VFR_HUD", 8.0f * rate_mult);
|
||||
configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
|
||||
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
|
||||
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
|
||||
|
@ -1453,7 +1497,8 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
if (_subscribe_to_stream != nullptr) {
|
||||
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
|
||||
if (_subscribe_to_stream_rate > 0.0f) {
|
||||
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
|
||||
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
|
||||
(double)_subscribe_to_stream_rate);
|
||||
|
||||
} else {
|
||||
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
|
||||
|
@ -1492,48 +1537,63 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
|
||||
bool is_part;
|
||||
uint8_t *read_ptr;
|
||||
uint8_t *write_ptr;
|
||||
uint8_t *write_ptr;
|
||||
|
||||
pthread_mutex_lock(&_message_buffer_mutex);
|
||||
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
|
||||
int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
|
||||
pthread_mutex_unlock(&_message_buffer_mutex);
|
||||
|
||||
if (available > 0) {
|
||||
// Reconstruct message from buffer
|
||||
// Reconstruct message from buffer
|
||||
|
||||
mavlink_message_t msg;
|
||||
write_ptr = (uint8_t*)&msg;
|
||||
write_ptr = (uint8_t *)&msg;
|
||||
|
||||
// Pull a single message from the buffer
|
||||
size_t read_count = available;
|
||||
|
||||
if (read_count > sizeof(mavlink_message_t)) {
|
||||
read_count = sizeof(mavlink_message_t);
|
||||
}
|
||||
|
||||
memcpy(write_ptr, read_ptr, read_count);
|
||||
|
||||
// We hold the mutex until after we complete the second part of the buffer. If we don't
|
||||
// we may end up breaking the empty slot overflow detection semantics when we mark the
|
||||
// possibly partial read below.
|
||||
pthread_mutex_lock(&_message_buffer_mutex);
|
||||
|
||||
// Pull a single message from the buffer
|
||||
size_t read_count = available;
|
||||
if (read_count > sizeof(mavlink_message_t)) {
|
||||
read_count = sizeof(mavlink_message_t);
|
||||
}
|
||||
|
||||
memcpy(write_ptr, read_ptr, read_count);
|
||||
|
||||
// We hold the mutex until after we complete the second part of the buffer. If we don't
|
||||
// we may end up breaking the empty slot overflow detection semantics when we mark the
|
||||
// possibly partial read below.
|
||||
pthread_mutex_lock(&_message_buffer_mutex);
|
||||
|
||||
message_buffer_mark_read(read_count);
|
||||
|
||||
/* write second part of buffer if there is some */
|
||||
if (is_part && read_count < sizeof(mavlink_message_t)) {
|
||||
write_ptr += read_count;
|
||||
available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
|
||||
read_count = sizeof(mavlink_message_t) - read_count;
|
||||
memcpy(write_ptr, read_ptr, read_count);
|
||||
write_ptr += read_count;
|
||||
available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
|
||||
read_count = sizeof(mavlink_message_t) - read_count;
|
||||
memcpy(write_ptr, read_ptr, read_count);
|
||||
message_buffer_mark_read(available);
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&_message_buffer_mutex);
|
||||
|
||||
_mavlink_resend_uart(_channel, &msg);
|
||||
pthread_mutex_unlock(&_message_buffer_mutex);
|
||||
|
||||
_mavlink_resend_uart(_channel, &msg);
|
||||
}
|
||||
}
|
||||
|
||||
/* update TX/RX rates*/
|
||||
if (t > _bytes_timestamp + 1000000) {
|
||||
if (_bytes_timestamp != 0) {
|
||||
float dt = (t - _bytes_timestamp) / 1000.0f;
|
||||
_rate_tx = _bytes_tx / dt;
|
||||
_rate_txerr = _bytes_txerr / dt;
|
||||
_rate_rx = _bytes_rx / dt;
|
||||
_bytes_tx = 0;
|
||||
_bytes_txerr = 0;
|
||||
_bytes_rx = 0;
|
||||
}
|
||||
_bytes_timestamp = t;
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
|
@ -1584,6 +1644,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
message_buffer_destroy();
|
||||
pthread_mutex_destroy(&_message_buffer_mutex);
|
||||
}
|
||||
|
||||
/* destroy log buffer */
|
||||
mavlink_logbuffer_destroy(&_logbuffer);
|
||||
|
||||
|
@ -1605,6 +1666,7 @@ int Mavlink::start_helper(int argc, char *argv[])
|
|||
/* out of memory */
|
||||
res = -ENOMEM;
|
||||
warnx("OUT OF MEM");
|
||||
|
||||
} else {
|
||||
/* this will actually only return once MAVLink exits */
|
||||
res = instance->task_main(argc, argv);
|
||||
|
@ -1664,7 +1726,40 @@ Mavlink::start(int argc, char *argv[])
|
|||
void
|
||||
Mavlink::display_status()
|
||||
{
|
||||
warnx("running");
|
||||
|
||||
if (_rstatus.heartbeat_time > 0) {
|
||||
printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
|
||||
}
|
||||
|
||||
if (_rstatus.timestamp > 0) {
|
||||
|
||||
printf("\ttype:\t\t");
|
||||
|
||||
switch (_rstatus.type) {
|
||||
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
|
||||
printf("3DR RADIO\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("UNKNOWN RADIO\n");
|
||||
break;
|
||||
}
|
||||
|
||||
printf("\trssi:\t\t%d\n", _rstatus.rssi);
|
||||
printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
|
||||
printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
|
||||
printf("\tnoise:\t\t%d\n", _rstatus.noise);
|
||||
printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
|
||||
printf("\trx errors:\t%u\n", _rstatus.rxerrors);
|
||||
printf("\tfixed:\t\t%u\n", _rstatus.fixed);
|
||||
|
||||
} else {
|
||||
printf("\tno telem status.\n");
|
||||
}
|
||||
printf("\trates:\n");
|
||||
printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
|
||||
printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
|
||||
printf("\trx: %.3f kB/s\n", (double)_rate_rx);
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -1752,8 +1847,8 @@ int mavlink_main(int argc, char *argv[])
|
|||
} else if (!strcmp(argv[1], "stop-all")) {
|
||||
return Mavlink::destroy_all_instances();
|
||||
|
||||
// } else if (!strcmp(argv[1], "status")) {
|
||||
// mavlink::g_mavlink->status();
|
||||
} else if (!strcmp(argv[1], "status")) {
|
||||
return Mavlink::get_status_all_instances();
|
||||
|
||||
} else if (!strcmp(argv[1], "stream")) {
|
||||
return Mavlink::stream_command(argc, argv);
|
||||
|
|
|
@ -51,6 +51,7 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include "mavlink_orb_subscription.h"
|
||||
|
@ -97,6 +98,8 @@ public:
|
|||
|
||||
static int destroy_all_instances();
|
||||
|
||||
static int get_status_all_instances();
|
||||
|
||||
static bool instance_exists(const char *device_name, Mavlink *self);
|
||||
|
||||
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
|
||||
|
@ -229,6 +232,26 @@ public:
|
|||
*/
|
||||
void count_txerr();
|
||||
|
||||
/**
|
||||
* Count transmitted bytes
|
||||
*/
|
||||
void count_txbytes(unsigned n) { _bytes_tx += n; };
|
||||
|
||||
/**
|
||||
* Count bytes not transmitted because of errors
|
||||
*/
|
||||
void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
|
||||
|
||||
/**
|
||||
* Count received bytes
|
||||
*/
|
||||
void count_rxbytes(unsigned n) { _bytes_rx += n; };
|
||||
|
||||
/**
|
||||
* Get the receive status of this MAVLink link
|
||||
*/
|
||||
struct telemetry_status_s& get_rx_status() { return _rstatus; }
|
||||
|
||||
protected:
|
||||
Mavlink *next;
|
||||
|
||||
|
@ -250,13 +273,13 @@ private:
|
|||
MavlinkOrbSubscription *_subscriptions;
|
||||
MavlinkStream *_streams;
|
||||
|
||||
MavlinkMissionManager *_mission_manager;
|
||||
MavlinkMissionManager *_mission_manager;
|
||||
|
||||
orb_advert_t _mission_pub;
|
||||
orb_advert_t _mission_pub;
|
||||
int _mission_result_sub;
|
||||
MAVLINK_MODE _mode;
|
||||
MAVLINK_MODE _mode;
|
||||
|
||||
mavlink_channel_t _channel;
|
||||
mavlink_channel_t _channel;
|
||||
|
||||
struct mavlink_logbuffer _logbuffer;
|
||||
unsigned int _total_counter;
|
||||
|
@ -285,6 +308,16 @@ private:
|
|||
|
||||
bool _flow_control_enabled;
|
||||
|
||||
unsigned _bytes_tx;
|
||||
unsigned _bytes_txerr;
|
||||
unsigned _bytes_rx;
|
||||
uint64_t _bytes_timestamp;
|
||||
float _rate_tx;
|
||||
float _rate_txerr;
|
||||
float _rate_rx;
|
||||
|
||||
struct telemetry_status_s _rstatus; ///< receive status
|
||||
|
||||
struct mavlink_message_buffer {
|
||||
int write_ptr;
|
||||
int read_ptr;
|
||||
|
|
|
@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
_telemetry_status_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_manual_pub(-1),
|
||||
_telemetry_heartbeat_time(0),
|
||||
_radio_status_available(false),
|
||||
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
||||
_hil_frames(0),
|
||||
|
@ -400,11 +399,11 @@ 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;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
|
||||
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.telem_time = tstatus.timestamp;
|
||||
/* tstatus.heartbeat_time is set by system heartbeats */
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
|
||||
tstatus.rssi = rstatus.rssi;
|
||||
tstatus.remote_rssi = rstatus.remrssi;
|
||||
|
@ -461,16 +460,20 @@ 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) {
|
||||
_telemetry_heartbeat_time = hrt_absolute_time();
|
||||
|
||||
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
|
||||
|
||||
hrt_abstime tnow = hrt_absolute_time();
|
||||
|
||||
/* always set heartbeat, publish only if telemetry link not up */
|
||||
tstatus.heartbeat_time = tnow;
|
||||
|
||||
/* 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));
|
||||
|
||||
tstatus.timestamp = _telemetry_heartbeat_time;
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.timestamp = tnow;
|
||||
/* telem_time indicates the timestamp of a telemetry status packet and we got none */
|
||||
tstatus.telem_time = 0;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
|
||||
if (_telemetry_status_pub < 0) {
|
||||
|
@ -955,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||
_mavlink->handle_message(&msg);
|
||||
}
|
||||
}
|
||||
|
||||
/* count received bytes */
|
||||
_mavlink->count_rxbytes(nread);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -148,7 +148,6 @@ private:
|
|||
orb_advert_t _telemetry_status_pub;
|
||||
orb_advert_t _rc_pub;
|
||||
orb_advert_t _manual_pub;
|
||||
hrt_abstime _telemetry_heartbeat_time;
|
||||
bool _radio_status_available;
|
||||
int _control_mode_sub;
|
||||
int _hil_frames;
|
||||
|
|
|
@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
|
|||
|
||||
struct telemetry_status_s {
|
||||
uint64_t timestamp;
|
||||
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
|
||||
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
|
||||
uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */
|
||||
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
|
||||
uint8_t rssi; /**< local signal strength */
|
||||
uint8_t remote_rssi; /**< remote signal strength */
|
||||
|
|
Loading…
Reference in New Issue