From a2da34be16531404b4c4507ab71b0518a51d60c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Jul 2014 20:53:01 +0200 Subject: Fixes #1207 --- src/modules/mavlink/mavlink_main.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3..e235ff0f9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -525,22 +525,39 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); - _param_initialized = true; } /* update system and component id */ int32_t system_id; param_get(_param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - int32_t component_id; param_get(_param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); } int32_t system_type; -- cgit v1.2.3 From 730a520362caf9c9d3e506a31441d9921e008144 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:59:13 +0200 Subject: Print mavlink radio module rates --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 10 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 3 files changed, 59 insertions(+), 5 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3..e25851c7d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%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) { @@ -1395,7 +1419,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); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t%d\t\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); + printf("\tnoise:\t%d\tus\n", _rstatus.noise); + printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); + printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + } } int @@ -1752,8 +1797,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); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f..d51120462 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include #include #include +#include #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,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34d..528c8b72a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ 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; -- cgit v1.2.3 From ed31d6a59a21753e326ba03933ce0eb962b8412a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:56:37 +0200 Subject: Cleanup of heartbeat handling and status printing. Ready to go mainline --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++-------- src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/uORB/topics/telemetry_status.h | 3 ++- 4 files changed, 32 insertions(+), 24 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e25851c7d..0e8f17c77 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -432,11 +432,9 @@ Mavlink::get_status_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); - while (inst != nullptr) { - printf("instance #%u:\n", iterations); + printf("\ninstance #%u:\n", iterations); inst->display_status(); /* move on */ @@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } if (_rstatus.timestamp > 0) { - printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + printf("\ttype:\t\t"); switch (_rstatus.type) { case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("\t3DR RADIO\n"); + printf("3DR RADIO\n"); break; default: - printf("\tUNKNOWN RADIO\n"); + printf("UNKNOWN RADIO\n"); break; } - printf("\trssi:\t%d\t\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); - printf("\tnoise:\t%d\tus\n", _rstatus.noise); - printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); - printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + 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"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a..454d730d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) 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; @@ -460,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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b64a060e3..f4d0c52d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 3347724a5..1297c1a9d 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -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 */ -- cgit v1.2.3 From 5ace06f3b820d43bc313347f3d23b121a36fbef1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:57:42 +0200 Subject: Add hint that heartbeats are only considered if coming from a GCS --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0e8f17c77..7bd2b9754 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1688,7 +1688,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } if (_rstatus.timestamp > 0) { -- cgit v1.2.3 From 6e3ebd3a36ce8bf3a9132b76986074ad8d9fcf35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:00:37 +0200 Subject: Fix code style, no actual code edits --- src/modules/mavlink/mavlink_main.cpp | 170 +++++++++++++++++++---------------- 1 file changed, 94 insertions(+), 76 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7bd2b9754..cb8c298c0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); } @@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t ret = write(uart, ch, desired); + if (ret != desired) { instance->count_txerr(); + } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -232,33 +234,33 @@ 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), - _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")) + _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), + _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")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -637,7 +639,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; } @@ -867,8 +870,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"); @@ -883,7 +887,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); @@ -910,7 +916,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 */ @@ -973,15 +981,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); @@ -1098,12 +1108,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; } @@ -1475,7 +1487,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); @@ -1514,45 +1527,46 @@ 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; - - // 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); - + 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); + 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); } } @@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1627,6 +1642,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); @@ -1696,10 +1712,11 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; - default: + + default: printf("UNKNOWN RADIO\n"); break; } @@ -1711,6 +1728,7 @@ Mavlink::display_status() 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"); } -- cgit v1.2.3 From 52a9513e64dacf1004fd0e11df3daf5eb607b0f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:22:02 +0200 Subject: Update rates --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb8c298c0..2e3a9ca33 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1454,7 +1454,7 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; -- cgit v1.2.3 From 41003a243793e465a71e9b142e7c9aac132d3923 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 17:37:13 +0200 Subject: mavlink: TX/RX rate counters added --- src/modules/mavlink/mavlink_main.cpp | 28 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 23 +++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 3 files changed, 54 insertions(+) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2e3a9ca33..ea60bb226 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -197,6 +197,7 @@ 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; } } @@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length 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); } } } @@ -249,6 +252,13 @@ Mavlink::Mavlink() : _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 {}, @@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* 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); } @@ -1732,6 +1756,10 @@ Mavlink::display_status() } 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 diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d51120462..70d13acb0 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -232,6 +232,21 @@ 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 */ @@ -293,6 +308,14 @@ 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 { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454d730d8..54c412ce7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } -- cgit v1.2.3 From cd8a0cd21771fb3a6fb8cddfc3da322eb04f10db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 23:39:02 +0200 Subject: mavlink: Only send event-based messages if there is space in the buffer --- src/modules/mavlink/mavlink_main.cpp | 60 +++++++++++++++++++++++++++++++++--- src/modules/mavlink/mavlink_main.h | 17 +++++++++- src/modules/mavlink/mavlink_stream.h | 13 ++++++++ 3 files changed, 84 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ea60bb226..e6b558ca6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -246,7 +246,8 @@ Mavlink::Mavlink() : _ftp_on(false), _uart_fd(-1), _baudrate(57600), - _datarate(10000), + _datarate(1000), + _datarate_events(500), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -352,6 +353,18 @@ Mavlink::count_txerr() perf_count(_txerr_perf); } +unsigned +Mavlink::get_free_tx_buf() +{ + unsigned buf_free; + + if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { + return buf_free; + } else { + return 0; + } +} + void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -1083,6 +1096,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate) return ERROR; } +void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + void Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) { @@ -1464,7 +1502,7 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(35000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; @@ -1519,10 +1557,22 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { - mavlink_pm_queued_send(); - _mission_manager->eventloop(); - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { + unsigned buf_free = get_free_tx_buf(); + + /* only send messages if they fit the buffer */ + if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + mavlink_pm_queued_send(); + } + + buf_free = get_free_tx_buf(); + if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + _mission_manager->eventloop(); + } + + buf_free = get_free_tx_buf(); + if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) && + (!mavlink_logbuffer_is_empty(&_logbuffer))) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 70d13acb0..e9d6b9c86 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -247,6 +247,13 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + /** * Get the receive status of this MAVLink link */ @@ -292,7 +299,8 @@ private: bool _ftp_on; int _uart_fd; int _baudrate; - int _datarate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages /** * If the queue index is not at 0, the queue sending @@ -385,6 +393,13 @@ private: int configure_stream(const char *stream_name, const float rate); + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + int message_buffer_init(int size); void message_buffer_destroy(); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 20e1c7c44..f539d225b 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -56,7 +56,20 @@ public: MavlinkStream(); virtual ~MavlinkStream(); + + /** + * Get the interval + * + * @param interval the inveral in microseconds (us) between messages + */ void set_interval(const unsigned int interval); + + /** + * Get the interval + * + * @return the inveral in microseconds (us) between messages + */ + unsigned get_interval() { return _interval; } void set_channel(mavlink_channel_t channel); /** -- cgit v1.2.3 From 1f3625371d787cdc452b45ad9d9a01423ae51f96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 10:18:00 +0200 Subject: Obey radio status in opportunistic transmissions as well. Last missing bit are adaptive rates --- src/modules/mavlink/mavlink_main.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e6b558ca6..e7e96dc3a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -359,7 +359,34 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - return buf_free; + if (_rstatus.timestamp > 0 && + (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { + + unsigned low_buf; + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + low_buf = 50; + break; + default: + low_buf = 50; + break; + } + + if (_rstatus.txbuf < low_buf) { + /* + * If the TX buf measure is initialized and up to date and low + * return 0 to slow event based transmission + */ + return 0; + } else { + /* there is no SW flow control option, just use what our buffer tells us */ + return buf_free; + } + + } else { + return buf_free; + } } else { return 0; } -- cgit v1.2.3 From ee7cd04e4c480a1931223889081074c469be0dbb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Jul 2014 20:34:32 +0200 Subject: mavlink: use send_message() method of Mavlink class instead of using helpers --- src/modules/mavlink/mavlink_main.cpp | 237 +++++++++++++------------------ src/modules/mavlink/mavlink_main.h | 6 + src/modules/mavlink/mavlink_messages.cpp | 32 ++--- src/modules/mavlink/mavlink_stream.cpp | 13 +- src/modules/mavlink/mavlink_stream.h | 8 +- 5 files changed, 123 insertions(+), 173 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e7e96dc3a..e3c452fb2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -94,11 +94,15 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate +#define TX_BUFFER_GAP 256 + static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; + /** * mavlink app start / stop handling function * @@ -108,113 +112,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); extern mavlink_system_t mavlink_system; -static uint64_t last_write_success_times[6] = {0}; -static uint64_t last_write_try_times[6] = {0}; - -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - - Mavlink *instance; - - switch (channel) { - case MAVLINK_COMM_0: - instance = Mavlink::get_instance(0); - break; - - case MAVLINK_COMM_1: - instance = Mavlink::get_instance(1); - break; - - case MAVLINK_COMM_2: - instance = Mavlink::get_instance(2); - break; - - case MAVLINK_COMM_3: - instance = Mavlink::get_instance(3); - break; -#ifdef MAVLINK_COMM_4 - - case MAVLINK_COMM_4: - instance = Mavlink::get_instance(4); - break; -#endif -#ifdef MAVLINK_COMM_5 - - case MAVLINK_COMM_5: - instance = Mavlink::get_instance(5); - break; -#endif -#ifdef MAVLINK_COMM_6 - - case MAVLINK_COMM_6: - instance = Mavlink::get_instance(6); - break; -#endif - - default: - return; - } - - int uart = instance->get_uart_fd(); - - ssize_t desired = (sizeof(uint8_t) * length); - - /* - * Check if the OS buffer is full and disable HW - * flow control if it continues to be full - */ - int buf_free = 0; - - if (instance->get_flow_control_enabled() - && 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]) { - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } - - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (instance->should_transmit()) { - last_write_try_times[(unsigned)channel] = hrt_absolute_time(); - - /* check if there is space in the buffer, let it overflow else */ - if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - - 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); - } - } -} - static void usage(void); Mavlink::Mavlink() : @@ -253,6 +150,8 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -359,30 +258,10 @@ Mavlink::get_free_tx_buf() unsigned buf_free; if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - if (_rstatus.timestamp > 0 && - (hrt_elapsed_time(&_rstatus.timestamp) < (2 * 1000 * 1000))) { - - unsigned low_buf; - - switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - low_buf = 50; - break; - default: - low_buf = 50; - break; - } + if (_rstatus.telem_time > 0 && + (hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) { - if (_rstatus.txbuf < low_buf) { - /* - * If the TX buf measure is initialized and up to date and low - * return 0 to slow event based transmission - */ - return 0; - } else { - /* there is no SW flow control option, just use what our buffer tells us */ - return buf_free; - } + return (unsigned)(buf_free * 0.01f * _rstatus.txbuf); } else { return buf_free; @@ -808,13 +687,93 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } +bool +Mavlink::check_can_send(const int prio, const unsigned packet_len) +{ + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + + if (get_flow_control_enabled() + && ioctl(_uart_fd, 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_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } + } + + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (should_transmit()) { + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + + if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) { + warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio); + /* we don't want to send anything just in half, so return */ + count_txerr(); + count_txerrbytes(packet_len); + return true; + } + } + return false; +} + void -Mavlink::send_message(const mavlink_message_t *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (!check_can_send(prio, packet_len)) { + return; + } + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; // TODO use internal seq counter? + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } } void @@ -1585,21 +1544,17 @@ Mavlink::task_main(int argc, char *argv[]) if (fast_rate_limiter.check(t)) { - unsigned buf_free = get_free_tx_buf(); - /* only send messages if they fit the buffer */ - if (buf_free >= (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { mavlink_pm_queued_send(); } - buf_free = get_free_tx_buf(); - if (buf_free >= (MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { _mission_manager->eventloop(); } - buf_free = get_free_tx_buf(); - if (buf_free >= (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) && - (!mavlink_logbuffer_is_empty(&_logbuffer))) { + if (!mavlink_logbuffer_is_empty(&_logbuffer) && + check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e9d6b9c86..1c62b03d2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -160,6 +160,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + bool check_can_send(const int prio, const unsigned packet_len); + + void send_message(const uint8_t msgid, const void *msg, const int prio); + void send_message(const mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -315,6 +319,8 @@ private: float _subscribe_to_stream_rate; bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; unsigned _bytes_tx; unsigned _bytes_txerr; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde..fe33985d2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -249,9 +249,9 @@ public: return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHeartbeat(); + return new MavlinkStreamHeartbeat(mavlink); } private: @@ -263,15 +263,15 @@ private: MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: - explicit MavlinkStreamHeartbeat() : MavlinkStream(), + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr), pos_sp_triplet_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); } void send(const hrt_abstime t) @@ -290,17 +290,15 @@ protected: memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + mavlink_heartbeat_t msg; + msg.base_mode = 0; + msg.custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); + msg.type = mavlink_system.type; + msg.autopilot = mavlink_system.type; + msg.mavlink_version = 3; + + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); } }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 7279206db..79dc23cc6 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,9 +43,9 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : +MavlinkStream::MavlinkStream(Mavlink *mavlink) : next(nullptr), - _channel(MAVLINK_COMM_0), + _mavlink(mavlink), _interval(1000000), _last_sent(0) { @@ -64,15 +64,6 @@ MavlinkStream::set_interval(const unsigned int interval) _interval = interval; } -/** - * Set mavlink channel - */ -void -MavlinkStream::set_channel(mavlink_channel_t channel) -{ - _channel = channel; -} - /** * Update subscriptions and send message if necessary */ diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index f539d225b..7d3afbbb5 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -54,7 +54,7 @@ class MavlinkStream public: MavlinkStream *next; - MavlinkStream(); + MavlinkStream(Mavlink *mavlink); virtual ~MavlinkStream(); /** @@ -76,14 +76,14 @@ public: * @return 0 if updated / sent, -1 if unchanged */ int update(const hrt_abstime t); - static MavlinkStream *new_instance(); + static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe(Mavlink *mavlink) = 0; + virtual void subscribe() = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; protected: - mavlink_channel_t _channel; + Mavlink * _mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; -- cgit v1.2.3 From a5f2d1b06645d4db33751aa8392fcbaf7f0856cc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:11:49 +0200 Subject: mavlink: new message sending API, includes fixed --- src/modules/mavlink/mavlink_ftp.h | 1 + src/modules/mavlink/mavlink_main.cpp | 306 +-- src/modules/mavlink/mavlink_main.h | 58 +- src/modules/mavlink/mavlink_messages.cpp | 3186 ++++++++++++++-------------- src/modules/mavlink/mavlink_messages.h | 4 +- src/modules/mavlink/mavlink_mission.cpp | 26 +- src/modules/mavlink/mavlink_parameters.cpp | 6 +- src/modules/mavlink/mavlink_parameters.h | 3 + src/modules/mavlink/module.mk | 1 + 9 files changed, 1715 insertions(+), 1876 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 59237a4c4..1bd1158fb 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -55,6 +55,7 @@ #include #include "mavlink_messages.h" +#include "mavlink_main.h" class MavlinkFTP { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e3c452fb2..dca76c950 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,7 @@ static Mavlink *_mavlink_instances = nullptr; static struct file_operations fops; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; /** * mavlink app start / stop handling function @@ -130,6 +131,7 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_manager(nullptr), + _parameters_manager(nullptr), _mission_pub(-1), _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), @@ -145,6 +147,7 @@ Mavlink::Mavlink() : _baudrate(57600), _datarate(1000), _datarate_events(500), + _rate_mult(0.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -687,18 +690,23 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -bool -Mavlink::check_can_send(const int prio, const unsigned packet_len) +void +Mavlink::send_message(const uint8_t msgid, const void *msg) { + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + /* * Check if the OS buffer is full and disable HW * flow control if it continues to be full */ int buf_free = 0; + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - if (get_flow_control_enabled() - && ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free) == 0) { - + if (get_flow_control_enabled() && 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 @@ -711,36 +719,22 @@ Mavlink::check_can_send(const int prio, const unsigned packet_len) } } - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (should_transmit()) { - _last_write_try_time = hrt_absolute_time(); + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - /* check if there is space in the buffer, let it overflow else */ - ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + _last_write_try_time = hrt_absolute_time(); - if (buf_free >= (prio <= 0 ? packet_len : TX_BUFFER_GAP)) { - warnx("\t\t\tSKIP %i bytes prio %i", packet_len, (int)prio); - /* we don't want to send anything just in half, so return */ - count_txerr(); - count_txerrbytes(packet_len); - return true; - } + /* check if there is space in the buffer, let it overflow else */ + if (buf_free >= TX_BUFFER_GAP) { + warnx("SKIP msgid %i, %i bytes", msgid, packet_len); + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + return; } - return false; -} -void -Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) -{ uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint8_t payload_len = mavlink_message_lengths[msgid]; - unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - if (!check_can_send(prio, packet_len)) { - return; - } - /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; @@ -755,9 +749,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) /* checksum */ uint16_t checksum; crc_init(&checksum); - crc_accumulate_buffer(&checksum, &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); #if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); #endif buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); @@ -766,7 +760,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, const int prio) /* send message to UART */ ssize_t ret = write(_uart_fd, buf, packet_len); - if (ret != packet_len) { + if (ret != (int) packet_len) { count_txerr(); count_txerrbytes(packet_len); @@ -783,7 +777,7 @@ Mavlink::handle_message(const mavlink_message_t *msg) _mission_manager->handle_message(msg); /* handle packet with parameter component */ - mavlink_pm_message_handler(_channel, msg); + _parameters_manager->handle_message(msg); if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ @@ -791,163 +785,6 @@ Mavlink::handle_message(const mavlink_message_t *msg) } } -int -Mavlink::mavlink_pm_queued_send() -{ - if (_mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); - _mavlink_param_queue_index++; - return 0; - - } else { - return 1; - } -} - -void Mavlink::mavlink_pm_start_queued_send() -{ - _mavlink_param_queue_index = 0; -} - -int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) -{ - return mavlink_pm_send_param(param_for_index(index)); -} - -int Mavlink::mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} - -int Mavlink::mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) { return 1; } - - /* buffers for param transmission */ - char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - mavlink_message_t tx_msg; - - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; - - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; - - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; - - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } - - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ - - int ret; - - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; - } - - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - _channel, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - send_message(&tx_msg); - return OK; -} - -void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) -{ - switch (msg->msgid) { - 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)) { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - send_statustext_info("[pm] sending list"); - } - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - 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))) { - /* 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); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown: %s", name); - send_statustext_info(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - 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))) { - /* 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 */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } - - } break; - } -} - int Mavlink::send_statustext_info(const char *string) { @@ -1031,11 +868,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) return sub_new; } +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; @@ -1066,10 +909,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); + stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(this); + stream->subscribe(); LL_APPEND(_streams, stream); return OK; @@ -1267,7 +1109,26 @@ Mavlink::pass_message(const mavlink_message_t *msg) float Mavlink::get_rate_mult() { - return _datarate / 1000.0f; + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + _rate_mult = ((float)_datarate - const_rate) / (rate - const_rate); } int @@ -1390,6 +1251,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } + update_rate_mult(); + warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1452,32 +1315,37 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); - /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = get_rate_mult(); + /* add default streams depending on mode */ + /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); 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", 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); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("NAMED_VALUE_FLOAT", 1.0f); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f); + configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); break; @@ -1488,10 +1356,12 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); + float base_rate_mult = _datarate / 1000.0f; + + MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1543,18 +1413,9 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { + _mission_manager->eventloop(); - /* only send messages if they fit the buffer */ - if (check_can_send(0, MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { - mavlink_pm_queued_send(); - } - - if (check_can_send(0, MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { - _mission_manager->eventloop(); - } - - if (!mavlink_logbuffer_is_empty(&_logbuffer) && - check_can_send(0, MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)) { + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); @@ -1630,6 +1491,7 @@ Mavlink::task_main(int argc, char *argv[]) } delete _mission_manager; + delete _parameters_manager; delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1c62b03d2..7fcbae72e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -58,7 +58,7 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" #include "mavlink_mission.h" - +#include "mavlink_parameters.h" class Mavlink { @@ -160,11 +160,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - bool check_can_send(const int prio, const unsigned packet_len); - - void send_message(const uint8_t msgid, const void *msg, const int prio); - - void send_message(const mavlink_message_t *msg); + void send_message(const uint8_t msgid, const void *msg); void handle_message(const mavlink_message_t *msg); @@ -285,6 +281,7 @@ private: MavlinkStream *_streams; MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; orb_advert_t _mission_pub; int _mission_result_sub; @@ -305,6 +302,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -352,51 +350,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ - /** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ - int mavlink_pm_send_param(param_t param); - - /** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_index(uint16_t index); - - /** - * Send one parameter identified by name. - * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. - */ - int mavlink_pm_send_param_for_name(const char *name); - - /** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ - int mavlink_pm_queued_send(void); - - /** - * Start sending the parameter queue. - * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() - */ - void mavlink_pm_start_queued_send(); - void mavlink_update_system(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + static unsigned int interval_from_rate(float rate); + int configure_stream(const char *stream_name, const float rate); /** @@ -420,6 +379,11 @@ private: void pass_message(const mavlink_message_t *msg); + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33985d2..7dc3ebac1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -76,7 +76,7 @@ #include #include "mavlink_messages.h" - +#include "mavlink_main.h" static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, @@ -254,6 +254,15 @@ public: return new MavlinkStreamHeartbeat(mavlink); } + unsigned get_size() + { + return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + bool const_rate() { + return true; + } + private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; @@ -291,6 +300,7 @@ protected: } mavlink_heartbeat_t msg; + msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); @@ -298,7 +308,7 @@ protected: msg.autopilot = mavlink_system.type; msg.mavlink_version = 3; - _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 1); + _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); } }; @@ -311,7 +321,7 @@ public: return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static () + static const char *get_name_static() { return "SYS_STATUS"; } @@ -321,9 +331,14 @@ public: return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance() + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamSysStatus(mavlink); + } + + unsigned get_size() { - return new MavlinkStreamSysStatus(); + return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: @@ -334,13 +349,13 @@ private: MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: - explicit MavlinkStreamSysStatus() : MavlinkStream(), + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), status_sub(nullptr) {} - void subscribe(Mavlink *mavlink) + void subscribe() { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); } void send(const hrt_abstime t) @@ -348,1591 +363,1594 @@ protected: struct vehicle_status_s status; if (status_sub->update(&status)) { - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); - } - } -}; - - -class MavlinkStreamHighresIMU : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHighresIMU::get_name_static(); - } - - static const char *get_name_static() - { - return "HIGHRES_IMU"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_HIGHRES_IMU; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamHighresIMU(); - } - -private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; - - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; - - /* do not allow top copying this class */ - MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); - -protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) - {} - - void subscribe(Mavlink *mavlink) - { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - - void send(const hrt_abstime t) - { - struct sensor_combined_s sensor; - - if (sensor_sub->update(&sensor_time, &sensor)) { - uint16_t fields_updated = 0; - - if (accel_timestamp != sensor.accelerometer_timestamp) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; - } - - if (gyro_timestamp != sensor.timestamp) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; - } - - if (mag_timestamp != sensor.magnetometer_timestamp) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; - } - - if (baro_timestamp != sensor.baro_timestamp) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; - } - - mavlink_msg_highres_imu_send(_channel, - sensor.timestamp, - sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], - sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], - sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], - sensor.baro_pres_mbar, sensor.differential_pressure_pa, - sensor.baro_alt_meter, sensor.baro_temp_celcius, - fields_updated); - } - } -}; - - -class MavlinkStreamAttitude : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitude::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ATTITUDE; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitude(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); - - -protected: - explicit MavlinkStreamAttitude() : MavlinkStream(), - att_sub(nullptr), - att_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_send(_channel, - att.timestamp / 1000, - att.roll, att.pitch, att.yaw, - att.rollspeed, att.pitchspeed, att.yawspeed); - } - } -}; - - -class MavlinkStreamAttitudeQuaternion : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeQuaternion::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE_QUATERNION"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeQuaternion(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); - -protected: - explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), - att_sub(nullptr), - att_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - - if (att_sub->update(&att_time, &att)) { - mavlink_msg_attitude_quaternion_send(_channel, - att.timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); - } - } -}; - - -class MavlinkStreamVFRHUD : public MavlinkStream -{ -public: - - const char *get_name() const - { - return MavlinkStreamVFRHUD::get_name_static(); - } - - static const char *get_name_static() - { - return "VFR_HUD"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_VFR_HUD; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamVFRHUD(); - } - -private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; - - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; - - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; - - /* do not allow top copying this class */ - MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); - -protected: - explicit MavlinkStreamVFRHUD() : MavlinkStream(), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_s att; - struct vehicle_global_position_s pos; - struct actuator_armed_s armed; - struct actuator_controls_s act; - struct airspeed_s airspeed; - - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); - - if (updated) { - float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - - mavlink_msg_vfr_hud_send(_channel, - airspeed.true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos.alt, - -pos.vel_d); - } - } -}; - - -class MavlinkStreamGPSRawInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGPSRawInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GPS_RAW_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GPS_RAW_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGPSRawInt(); - } - -private: - MavlinkOrbSubscription *gps_sub; - uint64_t gps_time; - - /* do not allow top copying this class */ - MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); - -protected: - explicit MavlinkStreamGPSRawInt() : MavlinkStream(), - gps_sub(nullptr), - gps_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_gps_position_s gps; - - if (gps_sub->update(&gps_time, &gps)) { - mavlink_msg_gps_raw_int_send(_channel, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph), - cm_uint16_from_m_float(gps.epv), - gps.vel_m_s * 100.0f, - _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps.satellites_used); + mavlink_sys_status_t msg; + + msg.onboard_control_sensors_present = status.onboard_control_sensors_present; + msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; + msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = status.load * 1000.0f; + msg.voltage_battery = status.battery_voltage * 1000.0f; + msg.current_battery = status.battery_current * 100.0f; + msg.drop_rate_comm = status.drop_rate_comm; + msg.errors_comm = status.errors_comm; + msg.errors_count1 = status.errors_count1; + msg.errors_count2 = status.errors_count2; + msg.errors_count3 = status.errors_count3; + msg.errors_count4 = status.errors_count4; + msg.battery_remaining = status.battery_remaining * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); } } }; -class MavlinkStreamGlobalPositionInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGlobalPositionInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GLOBAL_POSITION_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionInt(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - MavlinkOrbSubscription *home_sub; - uint64_t home_time; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); - -protected: - explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0), - home_sub(nullptr), - home_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_global_position_s pos; - struct home_position_s home; - - bool updated = pos_sub->update(&pos_time, &pos); - updated |= home_sub->update(&home_time, &home); - - if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos.timestamp / 1000, - pos.lat * 1e7, - pos.lon * 1e7, - pos.alt * 1000.0f, - (pos.alt - home.alt) * 1000.0f, - pos.vel_n * 100.0f, - pos.vel_e * 100.0f, - pos.vel_d * 100.0f, - _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); - } - } -}; - - -class MavlinkStreamLocalPositionNED : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamLocalPositionNED::get_name_static(); - } - - static const char *get_name_static() - { - return "LOCAL_POSITION_NED"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_LOCAL_POSITION_NED; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionNED(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); - -protected: - explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_local_position_s pos; - - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_local_position_ned_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.vx, - pos.vy, - pos.vz); - } - } -}; - - - -class MavlinkStreamViconPositionEstimate : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamViconPositionEstimate::get_name_static(); - } - - static const char *get_name_static() - { - return "VICON_POSITION_ESTIMATE"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } - -private: - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; - - /* do not allow top copying this class */ - MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); - -protected: - explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), - pos_sub(nullptr), - pos_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - } - - void send(const hrt_abstime t) - { - struct vehicle_vicon_position_s pos; - - if (pos_sub->update(&pos_time, &pos)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos.timestamp / 1000, - pos.x, - pos.y, - pos.z, - pos.roll, - pos.pitch, - pos.yaw); - } - } -}; - - -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGPSGlobalOrigin::get_name_static(); - } - - static const char *get_name_static() - { - return "GPS_GLOBAL_ORIGIN"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGPSGlobalOrigin(); - } - -private: - MavlinkOrbSubscription *home_sub; - - /* do not allow top copying this class */ - MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); - -protected: - explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), - home_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - } - - void send(const hrt_abstime t) - { - /* we're sending the GPS home periodically to ensure the - * the GCS does pick it up at one point */ - if (home_sub->is_published()) { - struct home_position_s home; - - if (home_sub->update(&home)) { - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); - } - } - } -}; - -template -class MavlinkStreamServoOutputRaw : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamServoOutputRaw::get_name_static(); - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - } - - static const char *get_name_static() - { - switch (N) { - case 0: - return "SERVO_OUTPUT_RAW_0"; - - case 1: - return "SERVO_OUTPUT_RAW_1"; - - case 2: - return "SERVO_OUTPUT_RAW_2"; - - case 3: - return "SERVO_OUTPUT_RAW_3"; - } - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamServoOutputRaw(); - } - -private: - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - /* do not allow top copying this class */ - MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); - -protected: - explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - act_sub = mavlink->add_orb_subscription(act_topics[N]); - } - - void send(const hrt_abstime t) - { - struct actuator_outputs_s act; - - if (act_sub->update(&act_time, &act)) { - mavlink_msg_servo_output_raw_send(_channel, - act.timestamp / 1000, - N, - act.output[0], - act.output[1], - act.output[2], - act.output[3], - act.output[4], - act.output[5], - act.output[6], - act.output[7]); - } - } -}; - - -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamHILControls::get_name_static(); - } - - static const char *get_name_static() - { - return "HIL_CONTROLS"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_HIL_CONTROLS; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamHILControls(); - } - -private: - MavlinkOrbSubscription *status_sub; - uint64_t status_time; - - MavlinkOrbSubscription *pos_sp_triplet_sub; - uint64_t pos_sp_triplet_time; - - MavlinkOrbSubscription *act_sub; - uint64_t act_time; - - /* do not allow top copying this class */ - MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); - -protected: - explicit MavlinkStreamHILControls() : MavlinkStream(), - status_sub(nullptr), - status_time(0), - pos_sp_triplet_sub(nullptr), - pos_sp_triplet_time(0), - act_sub(nullptr), - act_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - } - - void send(const hrt_abstime t) - { - struct vehicle_status_s status; - struct position_setpoint_triplet_s pos_sp_triplet; - struct actuator_outputs_s act; - - bool updated = act_sub->update(&act_time, &act); - updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); - updated |= status_sub->update(&status_time, &status); - - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - - /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* multirotors: set number of rotor outputs depending on type */ - - unsigned n; - - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - default: - n = 8; - break; - } - - for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (i < n) { - /* scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - - } else { - /* scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - } - - } else { - /* send 0 when disarmed */ - out[i] = 0.0f; - } - } - - } else { - /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ - - for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - - } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - } - - } - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } - } -}; - - -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); - } - - static const char *get_name_static() - { - return "GLOBAL_POSITION_SETPOINT_INT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionSetpointInt(); - } - -private: - MavlinkOrbSubscription *pos_sp_triplet_sub; - - /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); - -protected: - explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), - pos_sp_triplet_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - - void send(const hrt_abstime t) - { - struct position_setpoint_triplet_s pos_sp_triplet; - - if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet.current.lat * 1e7), - (int32_t)(pos_sp_triplet.current.lon * 1e7), - (int32_t)(pos_sp_triplet.current.alt * 1000), - (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } - } -}; - - -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamLocalPositionSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "LOCAL_POSITION_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionSetpoint(); - } - -private: - MavlinkOrbSubscription *pos_sp_sub; - uint64_t pos_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); - -protected: - explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), - pos_sp_sub(nullptr), - pos_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_local_position_setpoint_s pos_sp; - - if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp.x, - pos_sp.y, - pos_sp.z, - pos_sp.yaw); - } - } -}; - - -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), - att_sp_sub(nullptr), - att_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_attitude_setpoint_s att_sp; - - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); - } - - static const char *get_name_static() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_rates_sp_sub; - uint64_t att_rates_sp_time; - - /* do not allow top copying this class */ - MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); - MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); - -protected: - explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), - att_rates_sp_sub(nullptr), - att_rates_sp_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - } - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; - - if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp.timestamp / 1000, - att_rates_sp.roll, - att_rates_sp.pitch, - att_rates_sp.yaw, - att_rates_sp.thrust); - } - } -}; - - -class MavlinkStreamRCChannelsRaw : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamRCChannelsRaw::get_name_static(); - } - - static const char *get_name_static() - { - return "RC_CHANNELS_RAW"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_RC_CHANNELS_RAW; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamRCChannelsRaw(); - } - -private: - MavlinkOrbSubscription *rc_sub; - uint64_t rc_time; - - /* do not allow top copying this class */ - MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); - -protected: - explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), - rc_sub(nullptr), - rc_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - } - - void send(const hrt_abstime t) - { - struct rc_input_values rc; - - if (rc_sub->update(&rc_time, &rc)) { - const unsigned port_width = 8; - - // Deprecated message (but still needed for compatibility!) - for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc.timestamp_publication / 1000, - i, - (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, - (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, - rc.rssi); - } - - // New message - mavlink_msg_rc_channels_send(_channel, - rc.timestamp_publication / 1000, - rc.channel_count, - ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), - ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), - ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), - ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), - ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), - ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), - ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), - ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), - ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), - ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), - ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), - ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), - ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), - ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), - ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), - ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), - ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), - ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.rssi); - } - } -}; - - -class MavlinkStreamManualControl : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamManualControl::get_name_static(); - } - - static const char *get_name_static() - { - return "MANUAL_CONTROL"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_MANUAL_CONTROL; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamManualControl(); - } - -private: - MavlinkOrbSubscription *manual_sub; - uint64_t manual_time; - - /* do not allow top copying this class */ - MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); - -protected: - explicit MavlinkStreamManualControl() : MavlinkStream(), - manual_sub(nullptr), - manual_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - } - - void send(const hrt_abstime t) - { - struct manual_control_setpoint_s manual; - - if (manual_sub->update(&manual_time, &manual)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual.x * 1000, - manual.y * 1000, - manual.z * 1000, - manual.r * 1000, - 0); - } - } -}; - - -class MavlinkStreamOpticalFlow : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamOpticalFlow::get_name_static(); - } - - static const char *get_name_static() - { - return "OPTICAL_FLOW"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_OPTICAL_FLOW; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamOpticalFlow(); - } - -private: - MavlinkOrbSubscription *flow_sub; - uint64_t flow_time; - - /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); - -protected: - explicit MavlinkStreamOpticalFlow() : MavlinkStream(), - flow_sub(nullptr), - flow_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - } - - void send(const hrt_abstime t) - { - struct optical_flow_s flow; - - if (flow_sub->update(&flow_time, &flow)) { - mavlink_msg_optical_flow_send(_channel, - flow.timestamp, - flow.sensor_id, - flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, - flow.quality, - flow.ground_distance_m); - } - } -}; - -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamAttitudeControls::get_name_static(); - } - - static const char *get_name_static() - { - return "ATTITUDE_CONTROLS"; - } - - uint8_t get_id() - { - return 0; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeControls(); - } - -private: - MavlinkOrbSubscription *att_ctrl_sub; - uint64_t att_ctrl_time; - - /* do not allow top copying this class */ - MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); - -protected: - explicit MavlinkStreamAttitudeControls() : MavlinkStream(), - att_ctrl_sub(nullptr), - att_ctrl_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - } - - void send(const hrt_abstime t) - { - struct actuator_controls_s att_ctrl; - - if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "rll ctrl ", - att_ctrl.control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "ptch ctrl ", - att_ctrl.control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "yaw ctrl ", - att_ctrl.control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl.timestamp / 1000, - "thr ctrl ", - att_ctrl.control[3]); - } - } -}; - -class MavlinkStreamNamedValueFloat : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamNamedValueFloat::get_name_static(); - } - - static const char *get_name_static() - { - return "NAMED_VALUE_FLOAT"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamNamedValueFloat(); - } - -private: - MavlinkOrbSubscription *debug_sub; - uint64_t debug_time; - - /* do not allow top copying this class */ - MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); - -protected: - explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), - debug_sub(nullptr), - debug_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - } - - void send(const hrt_abstime t) - { - struct debug_key_value_s debug; - - if (debug_sub->update(&debug_time, &debug)) { - /* enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(_channel, - debug.timestamp_ms, - debug.key, - debug.value); - } - } -}; - -class MavlinkStreamCameraCapture : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamCameraCapture::get_name_static(); - } - - static const char *get_name_static() - { - return "CAMERA_CAPTURE"; - } - - uint8_t get_id() - { - return 0; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamCameraCapture(); - } - -private: - MavlinkOrbSubscription *status_sub; - - /* do not allow top copying this class */ - MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); - -protected: - explicit MavlinkStreamCameraCapture() : MavlinkStream(), - status_sub(nullptr) - {} - - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - - void send(const hrt_abstime t) - { - struct vehicle_status_s status; - (void)status_sub->update(&status); - - if (status.arming_state == ARMING_STATE_ARMED - || status.arming_state == ARMING_STATE_ARMED_ERROR) { - - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } - } -}; - -class MavlinkStreamDistanceSensor : public MavlinkStream -{ -public: - const char *get_name() const - { - return MavlinkStreamDistanceSensor::get_name_static(); - } - - static const char *get_name_static() - { - return "DISTANCE_SENSOR"; - } - - uint8_t get_id() - { - return MAVLINK_MSG_ID_DISTANCE_SENSOR; - } - - static MavlinkStream *new_instance() - { - return new MavlinkStreamDistanceSensor(); - } - -private: - MavlinkOrbSubscription *range_sub; - uint64_t range_time; - - /* do not allow top copying this class */ - MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); - -protected: - explicit MavlinkStreamDistanceSensor() : MavlinkStream(), - range_sub(nullptr), - range_time(0) - {} - - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - } - - void send(const hrt_abstime t) - { - struct range_finder_report range; - - if (range_sub->update(&range_time, &range)) { - - uint8_t type; - - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, - range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); - } - } -}; +//class MavlinkStreamHighresIMU : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamHighresIMU::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "HIGHRES_IMU"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_HIGHRES_IMU; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamHighresIMU(); +// } +// +//private: +// MavlinkOrbSubscription *sensor_sub; +// uint64_t sensor_time; +// +// uint64_t accel_timestamp; +// uint64_t gyro_timestamp; +// uint64_t mag_timestamp; +// uint64_t baro_timestamp; +// +// /* do not allow top copying this class */ +// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); +// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); +// +//protected: +// explicit MavlinkStreamHighresIMU() : MavlinkStream(), +// sensor_sub(nullptr), +// sensor_time(0), +// accel_timestamp(0), +// gyro_timestamp(0), +// mag_timestamp(0), +// baro_timestamp(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); +// } +// +// void send(const hrt_abstime t) +// { +// struct sensor_combined_s sensor; +// +// if (sensor_sub->update(&sensor_time, &sensor)) { +// uint16_t fields_updated = 0; +// +// if (accel_timestamp != sensor.accelerometer_timestamp) { +// /* mark first three dimensions as changed */ +// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); +// accel_timestamp = sensor.accelerometer_timestamp; +// } +// +// if (gyro_timestamp != sensor.timestamp) { +// /* mark second group dimensions as changed */ +// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); +// gyro_timestamp = sensor.timestamp; +// } +// +// if (mag_timestamp != sensor.magnetometer_timestamp) { +// /* mark third group dimensions as changed */ +// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); +// mag_timestamp = sensor.magnetometer_timestamp; +// } +// +// if (baro_timestamp != sensor.baro_timestamp) { +// /* mark last group dimensions as changed */ +// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); +// baro_timestamp = sensor.baro_timestamp; +// } +// +// mavlink_msg_highres_imu_send(_channel, +// sensor.timestamp, +// sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], +// sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], +// sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], +// sensor.baro_pres_mbar, sensor.differential_pressure_pa, +// sensor.baro_alt_meter, sensor.baro_temp_celcius, +// fields_updated); +// } +// } +//}; +// +// +//class MavlinkStreamAttitude : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitude::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ATTITUDE; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitude(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitude(MavlinkStreamAttitude &); +// MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); +// +// +//protected: +// explicit MavlinkStreamAttitude() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// +// if (att_sub->update(&att_time, &att)) { +// mavlink_msg_attitude_send(_channel, +// att.timestamp / 1000, +// att.roll, att.pitch, att.yaw, +// att.rollspeed, att.pitchspeed, att.yawspeed); +// } +// } +//}; +// +// +//class MavlinkStreamAttitudeQuaternion : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitudeQuaternion::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE_QUATERNION"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeQuaternion(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); +// MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); +// +//protected: +// explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// +// if (att_sub->update(&att_time, &att)) { +// mavlink_msg_attitude_quaternion_send(_channel, +// att.timestamp / 1000, +// att.q[0], +// att.q[1], +// att.q[2], +// att.q[3], +// att.rollspeed, +// att.pitchspeed, +// att.yawspeed); +// } +// } +//}; +// +// +//class MavlinkStreamVFRHUD : public MavlinkStream +//{ +//public: +// +// const char *get_name() const +// { +// return MavlinkStreamVFRHUD::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "VFR_HUD"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_VFR_HUD; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamVFRHUD(); +// } +// +//private: +// MavlinkOrbSubscription *att_sub; +// uint64_t att_time; +// +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// MavlinkOrbSubscription *armed_sub; +// uint64_t armed_time; +// +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// MavlinkOrbSubscription *airspeed_sub; +// uint64_t airspeed_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); +// MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); +// +//protected: +// explicit MavlinkStreamVFRHUD() : MavlinkStream(), +// att_sub(nullptr), +// att_time(0), +// pos_sub(nullptr), +// pos_time(0), +// armed_sub(nullptr), +// armed_time(0), +// act_sub(nullptr), +// act_time(0), +// airspeed_sub(nullptr), +// airspeed_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); +// airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_s att; +// struct vehicle_global_position_s pos; +// struct actuator_armed_s armed; +// struct actuator_controls_s act; +// struct airspeed_s airspeed; +// +// bool updated = att_sub->update(&att_time, &att); +// updated |= pos_sub->update(&pos_time, &pos); +// updated |= armed_sub->update(&armed_time, &armed); +// updated |= act_sub->update(&act_time, &act); +// updated |= airspeed_sub->update(&airspeed_time, &airspeed); +// +// if (updated) { +// float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); +// uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; +// float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; +// +// mavlink_msg_vfr_hud_send(_channel, +// airspeed.true_airspeed_m_s, +// groundspeed, +// heading, +// throttle, +// pos.alt, +// -pos.vel_d); +// } +// } +//}; +// +// +//class MavlinkStreamGPSRawInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGPSRawInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GPS_RAW_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GPS_RAW_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSRawInt(); +// } +// +//private: +// MavlinkOrbSubscription *gps_sub; +// uint64_t gps_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); +// MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); +// +//protected: +// explicit MavlinkStreamGPSRawInt() : MavlinkStream(), +// gps_sub(nullptr), +// gps_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_gps_position_s gps; +// +// if (gps_sub->update(&gps_time, &gps)) { +// mavlink_msg_gps_raw_int_send(_channel, +// gps.timestamp_position, +// gps.fix_type, +// gps.lat, +// gps.lon, +// gps.alt, +// cm_uint16_from_m_float(gps.eph), +// cm_uint16_from_m_float(gps.epv), +// gps.vel_m_s * 100.0f, +// _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, +// gps.satellites_used); +// } +// } +//}; +// +// +//class MavlinkStreamGlobalPositionInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGlobalPositionInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GLOBAL_POSITION_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionInt(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// MavlinkOrbSubscription *home_sub; +// uint64_t home_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); +// MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0), +// home_sub(nullptr), +// home_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_global_position_s pos; +// struct home_position_s home; +// +// bool updated = pos_sub->update(&pos_time, &pos); +// updated |= home_sub->update(&home_time, &home); +// +// if (updated) { +// mavlink_msg_global_position_int_send(_channel, +// pos.timestamp / 1000, +// pos.lat * 1e7, +// pos.lon * 1e7, +// pos.alt * 1000.0f, +// (pos.alt - home.alt) * 1000.0f, +// pos.vel_n * 100.0f, +// pos.vel_e * 100.0f, +// pos.vel_d * 100.0f, +// _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +// } +// } +//}; +// +// +//class MavlinkStreamLocalPositionNED : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamLocalPositionNED::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "LOCAL_POSITION_NED"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_LOCAL_POSITION_NED; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionNED(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); +// MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); +// +//protected: +// explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_local_position_s pos; +// +// if (pos_sub->update(&pos_time, &pos)) { +// mavlink_msg_local_position_ned_send(_channel, +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.vx, +// pos.vy, +// pos.vz); +// } +// } +//}; +// +// +// +//class MavlinkStreamViconPositionEstimate : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamViconPositionEstimate::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "VICON_POSITION_ESTIMATE"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamViconPositionEstimate(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sub; +// uint64_t pos_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); +// MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); +// +//protected: +// explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), +// pos_sub(nullptr), +// pos_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_vicon_position_s pos; +// +// if (pos_sub->update(&pos_time, &pos)) { +// mavlink_msg_vicon_position_estimate_send(_channel, +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.roll, +// pos.pitch, +// pos.yaw); +// } +// } +//}; +// +// +//class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGPSGlobalOrigin::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GPS_GLOBAL_ORIGIN"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSGlobalOrigin(); +// } +// +//private: +// MavlinkOrbSubscription *home_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); +// MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); +// +//protected: +// explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), +// home_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// } +// +// void send(const hrt_abstime t) +// { +// /* we're sending the GPS home periodically to ensure the +// * the GCS does pick it up at one point */ +// if (home_sub->is_published()) { +// struct home_position_s home; +// +// if (home_sub->update(&home)) { +// mavlink_msg_gps_global_origin_send(_channel, +// (int32_t)(home.lat * 1e7), +// (int32_t)(home.lon * 1e7), +// (int32_t)(home.alt) * 1000.0f); +// } +// } +// } +//}; +// +//template +//class MavlinkStreamServoOutputRaw : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamServoOutputRaw::get_name_static(); +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; +// } +// +// static const char *get_name_static() +// { +// switch (N) { +// case 0: +// return "SERVO_OUTPUT_RAW_0"; +// +// case 1: +// return "SERVO_OUTPUT_RAW_1"; +// +// case 2: +// return "SERVO_OUTPUT_RAW_2"; +// +// case 3: +// return "SERVO_OUTPUT_RAW_3"; +// } +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamServoOutputRaw(); +// } +// +//private: +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); +// MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); +// +//protected: +// explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// orb_id_t act_topics[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; +// +// act_sub = mavlink->add_orb_subscription(act_topics[N]); +// } +// +// void send(const hrt_abstime t) +// { +// struct actuator_outputs_s act; +// +// if (act_sub->update(&act_time, &act)) { +// mavlink_msg_servo_output_raw_send(_channel, +// act.timestamp / 1000, +// N, +// act.output[0], +// act.output[1], +// act.output[2], +// act.output[3], +// act.output[4], +// act.output[5], +// act.output[6], +// act.output[7]); +// } +// } +//}; +// +// +//class MavlinkStreamHILControls : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamHILControls::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "HIL_CONTROLS"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_HIL_CONTROLS; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamHILControls(); +// } +// +//private: +// MavlinkOrbSubscription *status_sub; +// uint64_t status_time; +// +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// uint64_t pos_sp_triplet_time; +// +// MavlinkOrbSubscription *act_sub; +// uint64_t act_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamHILControls(MavlinkStreamHILControls &); +// MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); +// +//protected: +// explicit MavlinkStreamHILControls() : MavlinkStream(), +// status_sub(nullptr), +// status_time(0), +// pos_sp_triplet_sub(nullptr), +// pos_sp_triplet_time(0), +// act_sub(nullptr), +// act_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_status_s status; +// struct position_setpoint_triplet_s pos_sp_triplet; +// struct actuator_outputs_s act; +// +// bool updated = act_sub->update(&act_time, &act); +// updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); +// updated |= status_sub->update(&status_time, &status); +// +// if (updated && (status.arming_state == ARMING_STATE_ARMED)) { +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state; +// uint8_t mavlink_base_mode; +// uint32_t mavlink_custom_mode; +// get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); +// +// float out[8]; +// +// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; +// +// /* scale outputs depending on system type */ +// if (mavlink_system.type == MAV_TYPE_QUADROTOR || +// mavlink_system.type == MAV_TYPE_HEXAROTOR || +// mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// /* multirotors: set number of rotor outputs depending on type */ +// +// unsigned n; +// +// switch (mavlink_system.type) { +// case MAV_TYPE_QUADROTOR: +// n = 4; +// break; +// +// case MAV_TYPE_HEXAROTOR: +// n = 6; +// break; +// +// default: +// n = 8; +// break; +// } +// +// for (unsigned i = 0; i < 8; i++) { +// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { +// if (i < n) { +// /* scale PWM out 900..2100 us to 0..1 for rotors */ +// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// +// } else { +// /* scale PWM out 900..2100 us to -1..1 for other channels */ +// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); +// } +// +// } else { +// /* send 0 when disarmed */ +// out[i] = 0.0f; +// } +// } +// +// } else { +// /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ +// +// for (unsigned i = 0; i < 8; i++) { +// if (i != 3) { +// /* scale PWM out 900..2100 us to -1..1 for normal channels */ +// out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); +// +// } else { +// /* scale PWM out 900..2100 us to 0..1 for throttle */ +// out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// } +// +// } +// } +// +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } +// } +//}; +// +// +//class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "GLOBAL_POSITION_SETPOINT_INT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionSetpointInt(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); +// MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); +// +//protected: +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// pos_sp_triplet_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// } +// +// void send(const hrt_abstime t) +// { +// struct position_setpoint_triplet_s pos_sp_triplet; +// +// if (pos_sp_triplet_sub->update(&pos_sp_triplet)) { +// mavlink_msg_global_position_setpoint_int_send(_channel, +// MAV_FRAME_GLOBAL, +// (int32_t)(pos_sp_triplet.current.lat * 1e7), +// (int32_t)(pos_sp_triplet.current.lon * 1e7), +// (int32_t)(pos_sp_triplet.current.alt * 1000), +// (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); +// } +// } +//}; +// +// +//class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamLocalPositionSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "LOCAL_POSITION_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *pos_sp_sub; +// uint64_t pos_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); +// MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); +// +//protected: +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// pos_sp_sub(nullptr), +// pos_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_local_position_setpoint_s pos_sp; +// +// if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { +// mavlink_msg_local_position_setpoint_send(_channel, +// MAV_FRAME_LOCAL_NED, +// pos_sp.x, +// pos_sp.y, +// pos_sp.z, +// pos_sp.yaw); +// } +// } +//}; +// +// +//class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawThrustSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *att_sp_sub; +// uint64_t att_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); +// MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// att_sp_sub(nullptr), +// att_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_attitude_setpoint_s att_sp; +// +// if (att_sp_sub->update(&att_sp_time, &att_sp)) { +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +// } +// } +//}; +// +// +//class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); +// } +// +//private: +// MavlinkOrbSubscription *att_rates_sp_sub; +// uint64_t att_rates_sp_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); +// +//protected: +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// att_rates_sp_sub(nullptr), +// att_rates_sp_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_rates_setpoint_s att_rates_sp; +// +// if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, +// att_rates_sp.timestamp / 1000, +// att_rates_sp.roll, +// att_rates_sp.pitch, +// att_rates_sp.yaw, +// att_rates_sp.thrust); +// } +// } +//}; +// +// +//class MavlinkStreamRCChannelsRaw : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamRCChannelsRaw::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "RC_CHANNELS_RAW"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_RC_CHANNELS_RAW; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamRCChannelsRaw(); +// } +// +//private: +// MavlinkOrbSubscription *rc_sub; +// uint64_t rc_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); +// MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); +// +//protected: +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// rc_sub(nullptr), +// rc_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// } +// +// void send(const hrt_abstime t) +// { +// struct rc_input_values rc; +// +// if (rc_sub->update(&rc_time, &rc)) { +// const unsigned port_width = 8; +// +// // Deprecated message (but still needed for compatibility!) +// for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(_channel, +// rc.timestamp_publication / 1000, +// i, +// (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, +// (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, +// rc.rssi); +// } +// +// // New message +// mavlink_msg_rc_channels_send(_channel, +// rc.timestamp_publication / 1000, +// rc.channel_count, +// ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), +// ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), +// ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), +// ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), +// ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), +// ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), +// ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), +// ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), +// ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), +// ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), +// ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), +// ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), +// ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), +// ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), +// ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), +// ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), +// ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), +// ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), +// rc.rssi); +// } +// } +//}; +// +// +//class MavlinkStreamManualControl : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamManualControl::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "MANUAL_CONTROL"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_MANUAL_CONTROL; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamManualControl(); +// } +// +//private: +// MavlinkOrbSubscription *manual_sub; +// uint64_t manual_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamManualControl(MavlinkStreamManualControl &); +// MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); +// +//protected: +// explicit MavlinkStreamManualControl() : MavlinkStream(), +// manual_sub(nullptr), +// manual_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// } +// +// void send(const hrt_abstime t) +// { +// struct manual_control_setpoint_s manual; +// +// if (manual_sub->update(&manual_time, &manual)) { +// mavlink_msg_manual_control_send(_channel, +// mavlink_system.sysid, +// manual.x * 1000, +// manual.y * 1000, +// manual.z * 1000, +// manual.r * 1000, +// 0); +// } +// } +//}; +// +// +//class MavlinkStreamOpticalFlow : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamOpticalFlow::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "OPTICAL_FLOW"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_OPTICAL_FLOW; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamOpticalFlow(); +// } +// +//private: +// MavlinkOrbSubscription *flow_sub; +// uint64_t flow_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); +// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); +// +//protected: +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// flow_sub(nullptr), +// flow_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// } +// +// void send(const hrt_abstime t) +// { +// struct optical_flow_s flow; +// +// if (flow_sub->update(&flow_time, &flow)) { +// mavlink_msg_optical_flow_send(_channel, +// flow.timestamp, +// flow.sensor_id, +// flow.flow_raw_x, flow.flow_raw_y, +// flow.flow_comp_x_m, flow.flow_comp_y_m, +// flow.quality, +// flow.ground_distance_m); +// } +// } +//}; +// +//class MavlinkStreamAttitudeControls : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamAttitudeControls::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "ATTITUDE_CONTROLS"; +// } +// +// uint8_t get_id() +// { +// return 0; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeControls(); +// } +// +//private: +// MavlinkOrbSubscription *att_ctrl_sub; +// uint64_t att_ctrl_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); +// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); +// +//protected: +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// att_ctrl_sub(nullptr), +// att_ctrl_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// } +// +// void send(const hrt_abstime t) +// { +// struct actuator_controls_s att_ctrl; +// +// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "rll ctrl ", +// att_ctrl.control[0]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "ptch ctrl ", +// att_ctrl.control[1]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "yaw ctrl ", +// att_ctrl.control[2]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl.timestamp / 1000, +// "thr ctrl ", +// att_ctrl.control[3]); +// } +// } +//}; +// +//class MavlinkStreamNamedValueFloat : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamNamedValueFloat::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "NAMED_VALUE_FLOAT"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamNamedValueFloat(); +// } +// +//private: +// MavlinkOrbSubscription *debug_sub; +// uint64_t debug_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); +// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); +// +//protected: +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// debug_sub(nullptr), +// debug_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// } +// +// void send(const hrt_abstime t) +// { +// struct debug_key_value_s debug; +// +// if (debug_sub->update(&debug_time, &debug)) { +// /* enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(_channel, +// debug.timestamp_ms, +// debug.key, +// debug.value); +// } +// } +//}; +// +//class MavlinkStreamCameraCapture : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamCameraCapture::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "CAMERA_CAPTURE"; +// } +// +// uint8_t get_id() +// { +// return 0; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamCameraCapture(); +// } +// +//private: +// MavlinkOrbSubscription *status_sub; +// +// /* do not allow top copying this class */ +// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); +// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); +// +//protected: +// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// status_sub(nullptr) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// } +// +// void send(const hrt_abstime t) +// { +// struct vehicle_status_s status; +// (void)status_sub->update(&status); +// +// if (status.arming_state == ARMING_STATE_ARMED +// || status.arming_state == ARMING_STATE_ARMED_ERROR) { +// +// /* send camera capture on */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); +// +// } else { +// /* send camera capture off */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); +// } +// } +//}; +// +//class MavlinkStreamDistanceSensor : public MavlinkStream +//{ +//public: +// const char *get_name() const +// { +// return MavlinkStreamDistanceSensor::get_name_static(); +// } +// +// static const char *get_name_static() +// { +// return "DISTANCE_SENSOR"; +// } +// +// uint8_t get_id() +// { +// return MAVLINK_MSG_ID_DISTANCE_SENSOR; +// } +// +// static MavlinkStream *new_instance() +// { +// return new MavlinkStreamDistanceSensor(); +// } +// +//private: +// MavlinkOrbSubscription *range_sub; +// uint64_t range_time; +// +// /* do not allow top copying this class */ +// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); +// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); +// +//protected: +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// range_sub(nullptr), +// range_time(0) +// {} +// +// void subscribe(Mavlink *mavlink) +// { +// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// } +// +// void send(const hrt_abstime t) +// { +// struct range_finder_report range; +// +// if (range_sub->update(&range_time, &range)) { +// +// uint8_t type; +// +// switch (range.type) { +// case RANGE_FINDER_TYPE_LASER: +// type = MAV_DISTANCE_SENSOR_LASER; +// break; +// } +// +// uint8_t id = 0; +// uint8_t orientation = 0; +// uint8_t covariance = 20; +// +// mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, +// range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); +// } +// } +//}; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), - new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), - new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), - new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), - new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), - new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), - new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), - new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), - new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), - new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), - new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), - new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), - new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), - new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), - new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), +// new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), +// new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), +// new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), +// new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), +// new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), +// new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), +// new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), +// new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), +// new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), +// new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), +// new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), +// new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), +// new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), +// new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), +// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), +// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), +// new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), +// new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), +// new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), +// new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index ee64d0e42..7e4416609 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -46,10 +46,10 @@ class StreamListItem { public: - MavlinkStream* (*new_instance)(); + MavlinkStream* (*new_instance)(Mavlink *mavlink); const char* (*get_name)(); - StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) : new_instance(inst), get_name(name) {}; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 068774c47..d17ccc6f9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -157,15 +157,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int void MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) { - mavlink_message_t msg; mavlink_mission_ack_t wpa; wpa.target_system = sysid; wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa); if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } } @@ -175,13 +173,11 @@ void MavlinkMissionManager::send_mission_current(uint16_t seq) { if (seq < _count) { - mavlink_message_t msg; mavlink_mission_current_t wpc; wpc.seq = seq; - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc); } else if (seq == 0 && _count == 0) { /* don't broadcast if no WPs */ @@ -199,15 +195,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_count_t wpc; wpc.target_system = sysid; wpc.target_component = compid; wpc.count = _count; - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc); if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } } @@ -226,13 +220,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mavlink_mission_item_t wp; format_mavlink_mission_item(&mission_item, &wp); - mavlink_message_t msg; wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp); if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -251,13 +244,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (seq < _max_count) { _time_last_sent = hrt_absolute_time(); - mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); - _mavlink->send_message(&msg); + + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr); if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } @@ -272,13 +264,11 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 void MavlinkMissionManager::send_mission_item_reached(uint16_t seq) { - mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); - _mavlink->send_message(&msg); + _mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached); if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } } diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b1dae918c..4e8cd4ba4 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -38,11 +38,11 @@ * @author Anton Babushkin */ -//#include +#include #include "mavlink_parameters.h" +#include "mavlink_main.h" -explicit MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1) { @@ -51,7 +51,7 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkSt unsigned MavlinkParametersManager::get_size() { - if (_send_all_index() >= 0) { + if (_send_all_index >= 0) { return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } else { diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 9b7a04a73..2a5a205e9 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -40,6 +40,9 @@ #pragma once +#include + +#include "mavlink_bridge_header.h" #include "mavlink_stream.h" class MavlinkParametersManager : public MavlinkStream diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 1986ae3c8..08faf102a 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -40,6 +40,7 @@ SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ mavlink_mission.cpp \ + mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ -- cgit v1.2.3 From d70b21c51aacae1a3dae755dca4ba9c3fa7a0d88 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 15:37:56 +0200 Subject: mavlink: move commands stream to mavlink_messages.cpp, bugs fixed --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- src/modules/mavlink/mavlink_commands.cpp | 73 ----------------------------- src/modules/mavlink/mavlink_commands.h | 65 ------------------------- src/modules/mavlink/mavlink_main.cpp | 14 +++--- src/modules/mavlink/mavlink_messages.cpp | 70 +++++++++++++++++++++++++++ src/modules/mavlink/mavlink_parameters.cpp | 2 +- src/modules/mavlink/module.mk | 1 - 7 files changed, 78 insertions(+), 149 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_commands.cpp delete mode 100644 src/modules/mavlink/mavlink_commands.h (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 374d1511c..d82c2dae7 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,7 @@ __BEGIN_DECLS -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp deleted file mode 100644 index b502c3c86..000000000 --- a/src/modules/mavlink/mavlink_commands.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_commands.cpp - * Mavlink commands stream implementation. - * - * @author Anton Babushkin - */ - -#include "mavlink_commands.h" - -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : - _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), - _cmd{}, - _channel(channel), - _cmd_time(0) -{ -} - -void -MavlinkCommandsStream::update(const hrt_abstime t) -{ - struct vehicle_command_s cmd; - - if (_cmd_sub->update(&_cmd_time, &cmd)) { - /* only send commands for other systems/components */ - if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_msg_command_long_send(_channel, - cmd.target_system, - cmd.target_component, - cmd.command, - cmd.confirmation, - cmd.param1, - cmd.param2, - cmd.param3, - cmd.param4, - cmd.param5, - cmd.param6, - cmd.param7); - } - } -} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h deleted file mode 100644 index abdba34b9..000000000 --- a/src/modules/mavlink/mavlink_commands.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_commands.h - * Mavlink commands stream definition. - * - * @author Anton Babushkin - */ - -#ifndef MAVLINK_COMMANDS_H_ -#define MAVLINK_COMMANDS_H_ - -#include -#include - -class Mavlink; -class MavlinkCommansStream; - -#include "mavlink_main.h" - -class MavlinkCommandsStream -{ -private: - MavlinkOrbSubscription *_cmd_sub; - struct vehicle_command_s *_cmd; - mavlink_channel_t _channel; - uint64_t _cmd_time; - -public: - MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - void update(const hrt_abstime t); -}; - -#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dca76c950..4f4bf8691 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -78,7 +78,6 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" -#include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems @@ -840,7 +839,7 @@ Mavlink::send_statustext(unsigned severity, const char *string) break; } - mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); + send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext); return OK; } else { @@ -1313,13 +1312,14 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s status; status_sub->update(&status_time, &status); - MavlinkCommandsStream commands_stream(this, _channel); - /* add default streams depending on mode */ /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); _parameters_manager->set_interval(interval_from_rate(30.0f)); @@ -1384,9 +1384,6 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == HIL_STATE_ON); } - /* update commands stream */ - commands_stream.update(t); - /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1469,7 +1466,8 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + // TODO implement message resending + //_mavlink_resend_uart(_channel, &msg); } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7dc3ebac1..a8b4d11cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -312,6 +312,75 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCommandLong::get_name_static(); + } + + static const char *get_name_static() + { + return "COMMAND_LONG"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_COMMAND_LONG; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCommandLong(mavlink); + } + + unsigned get_size() { return 0; } // commands stream is not regular and not predictable + +private: + MavlinkOrbSubscription *_cmd_sub; + uint64_t _cmd_time; + + /* do not allow top copying this class */ + MavlinkStreamCommandLong(MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + +protected: + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), + _cmd_sub(nullptr), + _cmd_time(0) + {} + + void subscribe() { + _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + } + + void send(const hrt_abstime t) + { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(&_cmd_time, &cmd)) { + /* only send commands for other systems/components */ + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { + mavlink_command_long_t mavcmd; + + mavcmd.target_system = cmd.target_system; + mavcmd.target_component = cmd.target_component; + mavcmd.command = cmd.command; + mavcmd.confirmation = cmd.confirmation; + mavcmd.param1 = cmd.param1; + mavcmd.param2 = cmd.param2; + mavcmd.param3 = cmd.param3; + mavcmd.param4 = cmd.param4; + mavcmd.param5 = cmd.param5; + mavcmd.param6 = cmd.param6; + mavcmd.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd); + } + } + } +}; class MavlinkStreamSysStatus : public MavlinkStream { @@ -1926,6 +1995,7 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), // new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), // new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 4e8cd4ba4..cd5f53d88 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -147,7 +147,7 @@ MavlinkParametersManager::send(const hrt_abstime t) if (_send_all_index >= 0) { send_param(param_for_index(_send_all_index)); _send_all_index++; - if (_send_all_index >= param_count()) { + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 08faf102a..91fdd6154 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -45,7 +45,6 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp \ mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -- cgit v1.2.3 From 7ecf66c06d15fb9a8c04f96b5bd05fe1c93138fe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 17:36:10 +0200 Subject: mavlink: bugs fixed --- src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++------------ src/modules/mavlink/mavlink_messages.cpp | 4 ++-- src/modules/mavlink/mavlink_stream.cpp | 13 +++++++++++-- 3 files changed, 23 insertions(+), 16 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4f4bf8691..f3246c380 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -146,7 +146,7 @@ Mavlink::Mavlink() : _baudrate(57600), _datarate(1000), _datarate_events(500), - _rate_mult(0.0f), + _rate_mult(1.0f), _mavlink_param_queue_index(0), mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), @@ -724,7 +724,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free >= TX_BUFFER_GAP) { + if (buf_free < TX_BUFFER_GAP) { warnx("SKIP msgid %i, %i bytes", msgid, packet_len); /* no enough space in buffer to send */ count_txerr(); @@ -749,9 +749,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) uint16_t checksum; crc_init(&checksum); crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); -#if MAVLINK_CRC_EXTRA crc_accumulate(mavlink_message_crcs[msgid], &checksum); -#endif buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); @@ -1127,7 +1125,7 @@ Mavlink::update_rate_mult() } } - _rate_mult = ((float)_datarate - const_rate) / (rate - const_rate); + _rate_mult = ((float)_datarate - const_rate) / rate; } int @@ -1250,8 +1248,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - update_rate_mult(); - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1328,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + /*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); configure_stream("ATTITUDE", 10.0f); configure_stream("VFR_HUD", 8.0f); @@ -1340,6 +1336,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + */ break; case MAVLINK_MODE_CAMERA: @@ -1353,9 +1350,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - /* don't send parameters on startup without request */ - _mavlink_param_queue_index = param_count(); - float base_rate_mult = _datarate / 1000.0f; MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); @@ -1374,6 +1368,9 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); + update_rate_mult(); + warnx("rate mult %f", (double)_rate_mult); + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); @@ -1410,7 +1407,8 @@ Mavlink::task_main(int argc, char *argv[]) } if (fast_rate_limiter.check(t)) { - _mission_manager->eventloop(); + // TODO missions + //_mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a8b4d11cc..20b1a966f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -305,7 +305,7 @@ protected: msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); msg.type = mavlink_system.type; - msg.autopilot = mavlink_system.type; + msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; _mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg); @@ -352,7 +352,7 @@ protected: {} void subscribe() { - _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index da3a55172..5b9e45d3e 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,12 +71,21 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; - unsigned int interval = _interval * _mavlink->get_rate_mult(); + unsigned int interval = _interval; + + if (!const_rate()) { + interval /= _mavlink->get_rate_mult(); + } if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = t; + if (const_rate()) { + _last_sent = (t / _interval) * _interval; + + } else { + _last_sent = t; + } return 0; } -- cgit v1.2.3 From 20698c751c62ca6f11aa910b3c3f180fe30211ff Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 22:40:55 +0200 Subject: mavlink: HIGHRES_IMU stream added --- src/modules/mavlink/mavlink_main.cpp | 5 +- src/modules/mavlink/mavlink_messages.cpp | 246 ++++++++++++++++--------------- 2 files changed, 133 insertions(+), 118 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f3246c380..e49e99a9b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1324,7 +1324,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); - /*configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); configure_stream("ATTITUDE", 10.0f); configure_stream("VFR_HUD", 8.0f); @@ -1336,7 +1336,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - */ break; case MAVLINK_MODE_CAMERA: @@ -1369,7 +1368,7 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %f", (double)_rate_mult); + warnx("rate mult %.2f", (double)_rate_mult); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 20b1a966f..a77d34c71 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -454,99 +454,115 @@ protected: }; -//class MavlinkStreamHighresIMU : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamHighresIMU::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "HIGHRES_IMU"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_HIGHRES_IMU; -// } -// -// static MavlinkStream *new_instance() -// { -// return new MavlinkStreamHighresIMU(); -// } -// -//private: -// MavlinkOrbSubscription *sensor_sub; -// uint64_t sensor_time; -// -// uint64_t accel_timestamp; -// uint64_t gyro_timestamp; -// uint64_t mag_timestamp; -// uint64_t baro_timestamp; -// -// /* do not allow top copying this class */ -// MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); -// MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); -// -//protected: -// explicit MavlinkStreamHighresIMU() : MavlinkStream(), -// sensor_sub(nullptr), -// sensor_time(0), -// accel_timestamp(0), -// gyro_timestamp(0), -// mag_timestamp(0), -// baro_timestamp(0) -// {} -// -// void subscribe(Mavlink *mavlink) -// { -// sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); -// } -// -// void send(const hrt_abstime t) -// { -// struct sensor_combined_s sensor; -// -// if (sensor_sub->update(&sensor_time, &sensor)) { -// uint16_t fields_updated = 0; -// -// if (accel_timestamp != sensor.accelerometer_timestamp) { -// /* mark first three dimensions as changed */ -// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); -// accel_timestamp = sensor.accelerometer_timestamp; -// } -// -// if (gyro_timestamp != sensor.timestamp) { -// /* mark second group dimensions as changed */ -// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); -// gyro_timestamp = sensor.timestamp; -// } -// -// if (mag_timestamp != sensor.magnetometer_timestamp) { -// /* mark third group dimensions as changed */ -// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); -// mag_timestamp = sensor.magnetometer_timestamp; -// } -// -// if (baro_timestamp != sensor.baro_timestamp) { -// /* mark last group dimensions as changed */ -// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); -// baro_timestamp = sensor.baro_timestamp; -// } -// -// mavlink_msg_highres_imu_send(_channel, -// sensor.timestamp, -// sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], -// sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], -// sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], -// sensor.baro_pres_mbar, sensor.differential_pressure_pa, -// sensor.baro_alt_meter, sensor.baro_temp_celcius, -// fields_updated); -// } -// } -//}; +class MavlinkStreamHighresIMU : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHighresIMU::get_name_static(); + } + + static const char *get_name_static() + { + return "HIGHRES_IMU"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighresIMU(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *sensor_sub; + uint64_t sensor_time; + + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; + + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + +protected: + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), + sensor_sub(nullptr), + sensor_time(0), + accel_timestamp(0), + gyro_timestamp(0), + mag_timestamp(0), + baro_timestamp(0) + {} + + void subscribe() + { + sensor_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_combined)); + } + + void send(const hrt_abstime t) + { + struct sensor_combined_s sensor; + + if (sensor_sub->update(&sensor_time, &sensor)) { + uint16_t fields_updated = 0; + + if (accel_timestamp != sensor.accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor.accelerometer_timestamp; + } + + if (gyro_timestamp != sensor.timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor.timestamp; + } + + if (mag_timestamp != sensor.magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor.magnetometer_timestamp; + } + + if (baro_timestamp != sensor.baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor.baro_timestamp; + } + + mavlink_highres_imu_t msg; + + msg.time_usec = sensor.timestamp; + msg.xacc = sensor.accelerometer_m_s2[0]; + msg.yacc = sensor.accelerometer_m_s2[1]; + msg.zacc = sensor.accelerometer_m_s2[2]; + msg.xgyro = sensor.gyro_rad_s[0]; + msg.ygyro = sensor.gyro_rad_s[1]; + msg.zgyro = sensor.gyro_rad_s[2]; + msg.xmag = sensor.magnetometer_ga[0]; + msg.ymag = sensor.magnetometer_ga[1]; + msg.zmag = sensor.magnetometer_ga[2]; + msg.abs_pressure = sensor.baro_pres_mbar; + msg.diff_pressure = sensor.differential_pressure_pa; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; + msg.fields_updated = fields_updated; + + _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); + } + } +}; // // //class MavlinkStreamAttitude : public MavlinkStream @@ -567,7 +583,7 @@ protected: // return MAVLINK_MSG_ID_ATTITUDE; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitude(); // } @@ -624,7 +640,7 @@ protected: // return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitudeQuaternion(); // } @@ -686,7 +702,7 @@ protected: // return MAVLINK_MSG_ID_VFR_HUD; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamVFRHUD(); // } @@ -783,7 +799,7 @@ protected: // return MAVLINK_MSG_ID_GPS_RAW_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGPSRawInt(); // } @@ -846,7 +862,7 @@ protected: // return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGlobalPositionInt(); // } @@ -918,7 +934,7 @@ protected: // return MAVLINK_MSG_ID_LOCAL_POSITION_NED; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamLocalPositionNED(); // } @@ -979,7 +995,7 @@ protected: // return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamViconPositionEstimate(); // } @@ -1039,7 +1055,7 @@ protected: // return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGPSGlobalOrigin(); // } @@ -1109,7 +1125,7 @@ protected: // } // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamServoOutputRaw(); // } @@ -1179,7 +1195,7 @@ protected: // return MAVLINK_MSG_ID_HIL_CONTROLS; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamHILControls(); // } @@ -1319,7 +1335,7 @@ protected: // return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamGlobalPositionSetpointInt(); // } @@ -1375,7 +1391,7 @@ protected: // return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamLocalPositionSetpoint(); // } @@ -1433,7 +1449,7 @@ protected: // return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRollPitchYawThrustSetpoint(); // } @@ -1491,7 +1507,7 @@ protected: // return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); // } @@ -1549,7 +1565,7 @@ protected: // return MAVLINK_MSG_ID_RC_CHANNELS_RAW; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamRCChannelsRaw(); // } @@ -1643,7 +1659,7 @@ protected: // return MAVLINK_MSG_ID_MANUAL_CONTROL; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamManualControl(); // } @@ -1702,7 +1718,7 @@ protected: // return MAVLINK_MSG_ID_OPTICAL_FLOW; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamOpticalFlow(); // } @@ -1760,7 +1776,7 @@ protected: // return 0; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamAttitudeControls(); // } @@ -1828,7 +1844,7 @@ protected: // return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamNamedValueFloat(); // } @@ -1886,7 +1902,7 @@ protected: // return 0; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamCameraCapture(); // } @@ -1944,7 +1960,7 @@ protected: // return MAVLINK_MSG_ID_DISTANCE_SENSOR; // } // -// static MavlinkStream *new_instance() +// static MavlinkStream *new_instance(Mavlink *mavlink) // { // return new MavlinkStreamDistanceSensor(); // } @@ -1997,7 +2013,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), -// new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), + new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), // new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), // new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), // new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), -- cgit v1.2.3 From ea2dce39927b7afcdfae79c059cc57342c70904e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 23:10:10 +0200 Subject: mavlink: MavlinkStream API simplifyed --- src/modules/mavlink/mavlink_main.cpp | 1 - src/modules/mavlink/mavlink_messages.cpp | 171 ++++++++++++------------------- src/modules/mavlink/mavlink_parameters.h | 2 - src/modules/mavlink/mavlink_stream.h | 1 - 4 files changed, 66 insertions(+), 109 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e49e99a9b..0cf708017 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -908,7 +908,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* create new instance */ stream = streams_list[i]->new_instance(this); stream->set_interval(interval); - stream->subscribe(); LL_APPEND(_streams, stream); return OK; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 575268814..863b7a1d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -264,8 +264,8 @@ public: } private: - MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); @@ -273,28 +273,22 @@ private: protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), - status_sub(nullptr), - pos_sp_triplet_sub(nullptr) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void subscribe() - { - status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - if (!status_sub->update(&status)) { + if (!_status_sub->update(&status)) { /* if topic update failed fill it with defaults */ memset(&status, 0, sizeof(status)); } - if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) { /* if topic update failed fill it with defaults */ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); } @@ -347,14 +341,10 @@ private: protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), - _cmd_sub(nullptr), + _cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))), _cmd_time(0) {} - void subscribe() { - _cmd_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - } - void send(const hrt_abstime t) { struct vehicle_command_s cmd; @@ -411,7 +401,7 @@ public: } private: - MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *_status_sub; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); @@ -419,19 +409,14 @@ private: protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), - status_sub(nullptr) + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void subscribe() - { - status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - } - void send(const hrt_abstime t) { struct vehicle_status_s status; - if (status_sub->update(&status)) { + if (_status_sub->update(&status)) { mavlink_sys_status_t msg; msg.onboard_control_sensors_present = status.onboard_control_sensors_present; @@ -483,13 +468,13 @@ public: } private: - MavlinkOrbSubscription *sensor_sub; - uint64_t sensor_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; - uint64_t accel_timestamp; - uint64_t gyro_timestamp; - uint64_t mag_timestamp; - uint64_t baro_timestamp; + uint64_t _accel_timestamp; + uint64_t _gyro_timestamp; + uint64_t _mag_timestamp; + uint64_t _baro_timestamp; /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); @@ -497,48 +482,43 @@ private: protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), - sensor_sub(nullptr), - sensor_time(0), - accel_timestamp(0), - gyro_timestamp(0), - mag_timestamp(0), - baro_timestamp(0) + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _accel_timestamp(0), + _gyro_timestamp(0), + _mag_timestamp(0), + _baro_timestamp(0) {} - void subscribe() - { - sensor_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - } - void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(&sensor_time, &sensor)) { + if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp; } mavlink_highres_imu_t msg; @@ -594,8 +574,8 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); @@ -604,20 +584,15 @@ private: protected: explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { + if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_t msg; msg.time_boot_ms = att.timestamp / 1000; @@ -663,8 +638,8 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); @@ -672,20 +647,15 @@ private: protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; - if (att_sub->update(&att_time, &att)) { + if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_quaternion_t msg; msg.time_boot_ms = att.timestamp / 1000; @@ -733,20 +703,20 @@ public: } private: - MavlinkOrbSubscription *att_sub; - uint64_t att_time; + MavlinkOrbSubscription *_att_sub; + uint64_t _att_time; - MavlinkOrbSubscription *pos_sub; - uint64_t pos_time; + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; - MavlinkOrbSubscription *armed_sub; - uint64_t armed_time; + MavlinkOrbSubscription *_armed_sub; + uint64_t _armed_time; - MavlinkOrbSubscription *act_sub; - uint64_t act_time; + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; - MavlinkOrbSubscription *airspeed_sub; - uint64_t airspeed_time; + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); @@ -754,27 +724,18 @@ private: protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sub(nullptr), - att_time(0), - pos_sub(nullptr), - pos_time(0), - armed_sub(nullptr), - armed_time(0), - act_sub(nullptr), - act_time(0), - airspeed_sub(nullptr), - airspeed_time(0) + _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _att_time(0), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))), + _armed_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _act_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0) {} - void subscribe() - { - att_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - armed_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - airspeed_sub = _mavlink->add_orb_subscription(ORB_ID(airspeed)); - } - void send(const hrt_abstime t) { struct vehicle_attitude_s att; @@ -783,11 +744,11 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(&att_time, &att); - updated |= pos_sub->update(&pos_time, &pos); - updated |= armed_sub->update(&armed_time, &armed); - updated |= act_sub->update(&act_time, &act); - updated |= airspeed_sub->update(&airspeed_time, &airspeed); + bool updated = _att_sub->update(&_att_time, &att); + updated |= _pos_sub->update(&_pos_time, &pos); + updated |= _armed_sub->update(&_armed_time, &armed); + updated |= _act_sub->update(&_act_time, &act); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); if (updated) { mavlink_vfr_hud_t msg; diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 2a5a205e9..5576e6b84 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -109,8 +109,6 @@ private: protected: explicit MavlinkParametersManager(Mavlink *mavlink); - void subscribe() {} - void send(const hrt_abstime t); void send_param(param_t param); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 646931c07..ef22dc443 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -75,7 +75,6 @@ public: int update(const hrt_abstime t); static MavlinkStream *new_instance(const Mavlink *mavlink); static const char *get_name_static(); - virtual void subscribe() = 0; virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; -- cgit v1.2.3 From 9df1da1b7cd140e9452c73c6307befeadd6ce4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:10:21 +0200 Subject: mavlink: STATUSTEXT implemented via streams API --- src/modules/mavlink/mavlink_main.cpp | 72 +++++--------------- src/modules/mavlink/mavlink_main.h | 18 +++-- src/modules/mavlink/mavlink_messages.cpp | 110 ++++++++++++++++++++++++++----- 3 files changed, 119 insertions(+), 81 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0cf708017..7d3fb97e1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -452,7 +452,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; -// printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); msg.severity = (unsigned char)cmd; @@ -782,67 +781,32 @@ Mavlink::handle_message(const mavlink_message_t *msg) } } -int +void Mavlink::send_statustext_info(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); } -int +void Mavlink::send_statustext_critical(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); } -int +void Mavlink::send_statustext_emergency(const char *string) { - return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); } -int +void Mavlink::send_statustext(unsigned severity, const char *string) { - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') { - break; - } - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; - /* 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; - } - - send_message(MAVLINK_MSG_ID_STATUSTEXT, &statustext); - return OK; - - } else { - return ERROR; - } + mavlink_logbuffer_write(&_logbuffer, &logmsg); } MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) @@ -1312,6 +1276,9 @@ Mavlink::task_main(int argc, char *argv[]) /* HEARTBEAT is constant rate stream, rate never adjusted */ configure_stream("HEARTBEAT", 1.0f); + /* 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); @@ -1367,7 +1334,7 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %.2f", (double)_rate_mult); + warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ @@ -1407,15 +1374,6 @@ Mavlink::task_main(int argc, char *argv[]) if (fast_rate_limiter.check(t)) { // TODO missions //_mission_manager->eventloop(); - - if (!mavlink_logbuffer_is_empty(&_logbuffer)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); - - if (lb_ret == OK) { - send_statustext(msg.severity, msg.text); - } - } } /* pass messages from other UARTs or FTP worker */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7fcbae72e..87bb9d9ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -188,29 +188,33 @@ public: * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_info(const char *string); + void send_statustext_info(const char *string); /** * Send a status text with loglevel CRITICAL * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_critical(const char *string); + void send_statustext_critical(const char *string); /** * Send a status text with loglevel EMERGENCY * * @param string the message to send (will be capped by mavlink max string length) */ - int send_statustext_emergency(const char *string); + void send_statustext_emergency(const char *string); /** - * Send a status text with loglevel + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level */ - int send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned severity, const char *string); + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -259,6 +263,8 @@ public: */ struct telemetry_status_s& get_rx_status() { return _rstatus; } + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + protected: Mavlink *next; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 863b7a1d4..4333369fe 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -40,9 +40,9 @@ */ #include + #include #include - #include #include #include @@ -72,8 +72,8 @@ #include #include #include - #include +#include #include "mavlink_messages.h" #include "mavlink_main.h" @@ -306,6 +306,77 @@ protected: } }; +class MavlinkStreamStatustext : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamStatustext::get_name_static(); + } + + static const char *get_name_static() + { + return "STATUSTEXT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_STATUSTEXT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamStatustext(mavlink); + } + + unsigned get_size() { + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + /* do not allow top copying this class */ + MavlinkStreamStatustext(MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + +protected: + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) + { + if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { + struct mavlink_logmessage logmsg; + int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); + + if (lb_ret == OK) { + mavlink_statustext_t msg; + + /* map severity */ + switch (logmsg.severity) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } + strncpy(msg.text, logmsg.text, sizeof(msg.text)); + + _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + } + } + } +}; + class MavlinkStreamCommandLong : public MavlinkStream { public: @@ -329,7 +400,9 @@ public: return new MavlinkStreamCommandLong(mavlink); } - unsigned get_size() { return 0; } // commands stream is not regular and not predictable + unsigned get_size() { + return 0; // commands stream is not regular and not predictable + } private: MavlinkOrbSubscription *_cmd_sub; @@ -352,21 +425,21 @@ protected: if (_cmd_sub->update(&_cmd_time, &cmd)) { /* only send commands for other systems/components */ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_command_long_t mavcmd; - - mavcmd.target_system = cmd.target_system; - mavcmd.target_component = cmd.target_component; - mavcmd.command = cmd.command; - mavcmd.confirmation = cmd.confirmation; - mavcmd.param1 = cmd.param1; - mavcmd.param2 = cmd.param2; - mavcmd.param3 = cmd.param3; - mavcmd.param4 = cmd.param4; - mavcmd.param5 = cmd.param5; - mavcmd.param6 = cmd.param6; - mavcmd.param7 = cmd.param7; - - _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &mavcmd); + mavlink_command_long_t msg; + + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.command = cmd.command; + msg.confirmation = cmd.confirmation; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); } } } @@ -1996,6 +2069,7 @@ protected: StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static), new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static), new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), -- cgit v1.2.3 From 897984c4f4a43136320c39f75fef106d3447ce47 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:28:02 +0200 Subject: mavlink: bug fixed, don't delete parameters stream --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7d3fb97e1..44329e0d2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1443,7 +1443,6 @@ Mavlink::task_main(int argc, char *argv[]) } delete _mission_manager; - delete _parameters_manager; delete _subscribe_to_stream; _subscribe_to_stream = nullptr; -- cgit v1.2.3 From b4e6f535ea15055f39b122eb87004c97796eb584 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:57:30 +0200 Subject: mavlink: onboard links should only pass on messages from the same system ID --- src/modules/mavlink/mavlink_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c6f8c42f..8cb0152fd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -483,7 +483,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { - inst->pass_message(msg); + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } } } } -- cgit v1.2.3 From b31ddc193c0e3dff3a8e4b4c343848f0f64e61ed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 19:31:25 +0200 Subject: mavlink: tix mission manager to use MavlinkStream API --- src/modules/mavlink/mavlink_main.cpp | 27 ++---- src/modules/mavlink/mavlink_main.h | 2 - src/modules/mavlink/mavlink_mission.cpp | 25 ++++-- src/modules/mavlink/mavlink_mission.h | 118 +++++++++++++++------------ src/modules/mavlink/mavlink_rate_limiter.cpp | 2 +- 5 files changed, 97 insertions(+), 77 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 44329e0d2..df078325a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -131,8 +131,6 @@ Mavlink::Mavlink() : _streams(nullptr), _mission_manager(nullptr), _parameters_manager(nullptr), - _mission_pub(-1), - _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, @@ -1255,12 +1253,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - - /* create mission manager */ - _mission_manager = new MavlinkMissionManager(this); - _mission_manager->set_verbose(_verbose); - _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); @@ -1287,6 +1279,14 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); @@ -1315,12 +1315,8 @@ Mavlink::task_main(int argc, char *argv[]) break; } - float base_rate_mult = _datarate / 1000.0f; - - MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult); - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult; + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -1371,11 +1367,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - if (fast_rate_limiter.check(t)) { - // TODO missions - //_mission_manager->eventloop(); - } - /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 87bb9d9ad..e3bad828f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -289,8 +289,6 @@ private: MavlinkMissionManager *_mission_manager; MavlinkParametersManager *_parameters_manager; - orb_advert_t _mission_pub; - int _mission_result_sub; MAVLINK_MODE _mode; mavlink_channel_t _channel; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d17ccc6f9..7a761588a 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -59,8 +59,7 @@ static const int ERROR = -1; -MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : - _mavlink(mavlink), +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), @@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(-1), - _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _slow_rate_limiter(_interval / 10.0f), _verbose(false), - _channel(mavlink->get_channel()), _comp_id(MAV_COMP_ID_MISSIONPLANNER) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager() close(_mission_result_sub); } +unsigned +MavlinkMissionManager::get_size() +{ + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + void MavlinkMissionManager::init_offboard_mission() { @@ -275,9 +287,10 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) void -MavlinkMissionManager::eventloop() +MavlinkMissionManager::send(const hrt_abstime now) { - hrt_abstime now = hrt_absolute_time(); + /* update interval for slow rate limiter */ + _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); bool updated = false; orb_check(_mission_result_sub, &updated); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 5bf80768c..8ff27f87d 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -42,9 +42,11 @@ #pragma once +#include + #include "mavlink_bridge_header.h" #include "mavlink_rate_limiter.h" -#include +#include "mavlink_stream.h" enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, @@ -65,20 +67,75 @@ enum MAVLINK_WPM_CODES { #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -class Mavlink; - -class MavlinkMissionManager { +class MavlinkMissionManager : public MavlinkStream { public: - MavlinkMissionManager(Mavlink *parent); - ~MavlinkMissionManager(); + const char *get_name() const + { + return MavlinkMissionManager::get_name_static(); + } + + static const char *get_name_static() + { + return "MISSION_ITEM"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_MISSION_ITEM; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkMissionManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + void set_verbose(bool v) { _verbose = v; } + +private: + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximum number of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + unsigned _transfer_seq; ///< Item sequence in current transmission + unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission + unsigned _transfer_partner_compid; ///< Partner component ID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; + + uint8_t _comp_id; + + /* do not allow top copying this class */ + MavlinkMissionManager(MavlinkMissionManager &); + MavlinkMissionManager& operator = (const MavlinkMissionManager &); + void init_offboard_mission(); int update_active_mission(int dataman_id, unsigned count, int seq); - void send_message(mavlink_message_t *msg); - /** * @brief Sends an waypoint ack message */ @@ -110,10 +167,6 @@ public: */ void send_mission_item_reached(uint16_t seq); - void eventloop(); - - void handle_message(const mavlink_message_t *msg); - void handle_mission_ack(const mavlink_message_t *msg); void handle_mission_set_current(const mavlink_message_t *msg); @@ -138,43 +191,8 @@ public: */ int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - void set_verbose(bool v) { _verbose = v; } - -private: - Mavlink* _mavlink; - - enum MAVLINK_WPM_STATES _state; ///< Current state - - uint64_t _time_last_recv; - uint64_t _time_last_sent; - - uint32_t _action_timeout; - uint32_t _retry_timeout; - unsigned _max_count; ///< Maximum number of mission items - - int _dataman_id; ///< Dataman storage ID for active mission - unsigned _count; ///< Count of items in active mission - int _current_seq; ///< Current item sequence in active mission - - int _transfer_dataman_id; ///< Dataman storage ID for current transmission - unsigned _transfer_count; ///< Items count in current transmission - unsigned _transfer_seq; ///< Item sequence in current transmission - unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) - unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission - unsigned _transfer_partner_compid; ///< Partner component ID for current transmission - - int _offboard_mission_sub; - int _mission_result_sub; - orb_advert_t _offboard_mission_pub; - - MavlinkRateLimiter _slow_rate_limiter; - - bool _verbose; - - mavlink_channel_t _channel; - uint8_t _comp_id; +protected: + explicit MavlinkMissionManager(Mavlink *mavlink); - /* do not allow copying this class */ - MavlinkMissionManager(const MavlinkMissionManager&); - MavlinkMissionManager operator=(const MavlinkMissionManager&); + void send(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index 9cdda8b17..d3b3e1cde 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t) uint64_t dt = t - _last_sent; if (dt > 0 && dt >= _interval) { - _last_sent = (t / _interval) * _interval; + _last_sent = t; return true; } -- cgit v1.2.3 From c486aa536f3c0169e1abb0f4c2b60baa1e2b053b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 19:34:19 +0200 Subject: mavlink: bug fixed, don't delete mission manages twice --- src/modules/mavlink/mavlink_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index df078325a..e27a940cf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1433,8 +1433,6 @@ Mavlink::task_main(int argc, char *argv[]) perf_end(_loop_perf); } - delete _mission_manager; - delete _subscribe_to_stream; _subscribe_to_stream = nullptr; -- cgit v1.2.3 From 8f0af1c5fe5a843c56fff2dc70acb2fc0e7e1b90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 00:16:02 +0200 Subject: mavlink: forwarding and FTP fixed, flow control detector fixed --- src/modules/mavlink/mavlink_main.cpp | 97 ++++++++++++++++++++++++------------ src/modules/mavlink/mavlink_main.h | 19 ++++--- 2 files changed, 77 insertions(+), 39 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e27a940cf..aaee0455a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate -#define TX_BUFFER_GAP 256 +#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN static Mavlink *_mavlink_instances = nullptr; @@ -252,25 +252,6 @@ Mavlink::count_txerr() perf_count(_txerr_perf); } -unsigned -Mavlink::get_free_tx_buf() -{ - unsigned buf_free; - - if (!ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free)) { - if (_rstatus.telem_time > 0 && - (hrt_elapsed_time(&_rstatus.telem_time) < (2 * 1000 * 1000))) { - - return (unsigned)(buf_free * 0.01f * _rstatus.txbuf); - - } else { - return buf_free; - } - } else { - return 0; - } -} - void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -686,15 +667,9 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::send_message(const uint8_t msgid, const void *msg) +unsigned +Mavlink::get_free_tx_buf() { - /* If the wait until transmit flag is on, only transmit after we've received messages. - Otherwise, transmit all the time. */ - if (!should_transmit()) { - return; - } - /* * Check if the OS buffer is full and disable HW * flow control if it continues to be full @@ -702,7 +677,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) int buf_free = 0; (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); - if (get_flow_control_enabled() && buf_free == 0) { + if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { /* Disable hardware flow control: * if no successful write since a defined time * and if the last try was not the last successful write @@ -714,6 +689,19 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) enable_flow_control(false); } } + return buf_free; +} + +void +Mavlink::send_message(const uint8_t msgid, const void *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + int buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -722,7 +710,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP msgid %i, %i bytes", msgid, packet_len); + warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -764,6 +752,52 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) } } +void +Mavlink::resend_message(mavlink_message_t *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + int buf_free = get_free_tx_buf(); + + _last_write_try_time = hrt_absolute_time(); + + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < TX_BUFFER_GAP) { + warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free); + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); + + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); + + /* send message to UART */ + ssize_t ret = write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } +} + void Mavlink::handle_message(const mavlink_message_t *msg) { @@ -1411,8 +1445,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); - // TODO implement message resending - //_mavlink_resend_uart(_channel, &msg); + resend_message(&msg); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e3bad828f..4121e286e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -162,6 +162,11 @@ public: void send_message(const uint8_t msgid, const void *msg); + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + void handle_message(const mavlink_message_t *msg); MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); @@ -251,13 +256,6 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - /** * Get the receive status of this MAVLink link */ @@ -358,6 +356,13 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); -- cgit v1.2.3 From 1938ef16e39ff937a3076489f3eff0a2eb4bbb65 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:27:07 +0200 Subject: mavlink: don't scale up rates, debug output removed --- src/modules/mavlink/mavlink_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index aaee0455a..572d5c19b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -610,6 +610,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if (enable_flow_control(true)) { warnx("hardware flow control not supported"); } + + } else { + _flow_control_enabled = false; } return _uart_fd; @@ -651,8 +654,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - float rate_mult = _datarate / 1000.0f; - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + configure_stream("HIL_CONTROLS", 150.0f); } /* disable HIL */ @@ -710,7 +712,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP msgid %i, %i bytes, free %i", msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -769,7 +770,6 @@ Mavlink::resend_message(mavlink_message_t *msg) /* check if there is space in the buffer, let it overflow else */ if (buf_free < TX_BUFFER_GAP) { - warnx("SKIP resent msgid %i, %i bytes, free %i", msg->msgid, packet_len, buf_free); /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1120,7 +1120,8 @@ Mavlink::update_rate_mult() } } - _rate_mult = ((float)_datarate - const_rate) / rate; + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); } int @@ -1364,7 +1365,6 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); update_rate_mult(); - warnx("rate mult %.2f rate %.3f err %.3f", (double)_rate_mult, (double)_rate_tx, (double)_rate_txerr); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ -- cgit v1.2.3 From 07c4144cde1026c4d966d043d12e36ba686c0781 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:30:06 +0200 Subject: mavlink: message buffer size fixed --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 572d5c19b..408799be4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1268,7 +1268,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } -- cgit v1.2.3 From 019dc1b5264072c785433c53ca11995ed291f924 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 17:46:35 +0200 Subject: mavlink: log message severity fixed --- src/modules/mavlink/mavlink_main.cpp | 27 ++++++++++++++++++++++----- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 19 +------------------ 3 files changed, 24 insertions(+), 24 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 408799be4..f9c7fcedf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -433,7 +433,24 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) const char *txt = (const char *)arg; struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - msg.severity = (unsigned char)cmd; + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { @@ -816,23 +833,23 @@ Mavlink::handle_message(const mavlink_message_t *msg) void Mavlink::send_statustext_info(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); + send_statustext(MAV_SEVERITY_INFO, string); } void Mavlink::send_statustext_critical(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); + send_statustext(MAV_SEVERITY_CRITICAL, string); } void Mavlink::send_statustext_emergency(const char *string) { - send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); + send_statustext(MAV_SEVERITY_EMERGENCY, string); } void -Mavlink::send_statustext(unsigned severity, const char *string) +Mavlink::send_statustext(unsigned char severity, const char *string) { struct mavlink_logmessage logmsg; strncpy(logmsg.text, string, sizeof(logmsg.text)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4121e286e..38149cf10 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -218,7 +218,7 @@ public: * @param string the message to send (will be capped by mavlink max string length) * @param severity the log level */ - void send_statustext(unsigned severity, const char *string); + void send_statustext(unsigned char severity, const char *string); MavlinkStream * get_streams() const { return _streams; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a710cdf6a..083a6301a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -351,24 +351,7 @@ protected: if (lb_ret == OK) { mavlink_statustext_t msg; - /* map severity */ - switch (logmsg.severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - msg.severity = MAV_SEVERITY_INFO; - break; - - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - msg.severity = MAV_SEVERITY_CRITICAL; - break; - - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - msg.severity = MAV_SEVERITY_EMERGENCY; - break; - - default: - msg.severity = MAV_SEVERITY_INFO; - break; - } + msg.severity = logmsg.severity; strncpy(msg.text, logmsg.text, sizeof(msg.text)); _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); -- cgit v1.2.3 From f3ec46369b68d57489a5fa57a320ae99a1bda220 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 20:30:58 +0200 Subject: mavlink: all streams ported to new API --- src/modules/mavlink/mavlink_main.cpp | 1 - src/modules/mavlink/mavlink_messages.cpp | 663 ++++++++++++++++--------------- 2 files changed, 347 insertions(+), 317 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f9c7fcedf..d72fb8508 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1350,7 +1350,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("NAMED_VALUE_FLOAT", 1.0f); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 083a6301a..683f0f1e8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1652,8 +1652,8 @@ public: unsigned get_size() { - return (RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1753,7 +1753,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1790,313 +1790,344 @@ protected: }; -//class MavlinkStreamOpticalFlow : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamOpticalFlow::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "OPTICAL_FLOW"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_OPTICAL_FLOW; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamOpticalFlow(); -// } -// -//private: -// MavlinkOrbSubscription *flow_sub; -// uint64_t flow_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); -// MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); -// -//protected: -// explicit MavlinkStreamOpticalFlow() : MavlinkStream(mavlink), -// flow_sub(nullptr), -// flow_time(0) -// {} -// -// void subscribe() -// { -// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow)); -// } -// -// void send(const hrt_abstime t) -// { -// struct optical_flow_s flow; -// -// if (flow_sub->update(&flow_time, &flow)) { -// mavlink_msg_optical_flow_send(_channel, -// flow.timestamp, -// flow.sensor_id, -// flow.flow_raw_x, flow.flow_raw_y, -// flow.flow_comp_x_m, flow.flow_comp_y_m, -// flow.quality, -// flow.ground_distance_m); -// } -// } -//}; -// -//class MavlinkStreamAttitudeControls : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamAttitudeControls::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "ATTITUDE_CONTROLS"; -// } -// -// uint8_t get_id() -// { -// return 0; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamAttitudeControls(); -// } -// -//private: -// MavlinkOrbSubscription *att_ctrl_sub; -// uint64_t att_ctrl_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); -// MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); -// -//protected: -// explicit MavlinkStreamAttitudeControls() : MavlinkStream(mavlink), -// att_ctrl_sub(nullptr), -// att_ctrl_time(0) -// {} -// -// void subscribe() -// { -// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); -// } -// -// void send(const hrt_abstime t) -// { -// struct actuator_controls_s att_ctrl; -// -// if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "rll ctrl ", -// att_ctrl.control[0]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "ptch ctrl ", -// att_ctrl.control[1]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "yaw ctrl ", -// att_ctrl.control[2]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl.timestamp / 1000, -// "thr ctrl ", -// att_ctrl.control[3]); -// } -// } -//}; -// -//class MavlinkStreamNamedValueFloat : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamNamedValueFloat::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "NAMED_VALUE_FLOAT"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamNamedValueFloat(); -// } -// -//private: -// MavlinkOrbSubscription *debug_sub; -// uint64_t debug_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); -// MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); -// -//protected: -// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(mavlink), -// debug_sub(nullptr), -// debug_time(0) -// {} -// -// void subscribe() -// { -// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value)); -// } -// -// void send(const hrt_abstime t) -// { -// struct debug_key_value_s debug; -// -// if (debug_sub->update(&debug_time, &debug)) { -// /* enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(_channel, -// debug.timestamp_ms, -// debug.key, -// debug.value); -// } -// } -//}; -// -//class MavlinkStreamCameraCapture : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamCameraCapture::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "CAMERA_CAPTURE"; -// } -// -// uint8_t get_id() -// { -// return 0; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamCameraCapture(); -// } -// -//private: -// MavlinkOrbSubscription *status_sub; -// -// /* do not allow top copying this class */ -// MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); -// MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); -// -//protected: -// explicit MavlinkStreamCameraCapture() : MavlinkStream(mavlink), -// status_sub(nullptr) -// {} -// -// void subscribe() -// { -// status_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// } -// -// void send(const hrt_abstime t) -// { -// struct vehicle_status_s status; -// (void)status_sub->update(&status); -// -// if (status.arming_state == ARMING_STATE_ARMED -// || status.arming_state == ARMING_STATE_ARMED_ERROR) { -// -// /* send camera capture on */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -// -// } else { -// /* send camera capture off */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -// } -// } -//}; -// -//class MavlinkStreamDistanceSensor : public MavlinkStream -//{ -//public: -// const char *get_name() const -// { -// return MavlinkStreamDistanceSensor::get_name_static(); -// } -// -// static const char *get_name_static() -// { -// return "DISTANCE_SENSOR"; -// } -// -// uint8_t get_id() -// { -// return MAVLINK_MSG_ID_DISTANCE_SENSOR; -// } -// -// static MavlinkStream *new_instance(Mavlink *mavlink) -// { -// return new MavlinkStreamDistanceSensor(); -// } -// -//private: -// MavlinkOrbSubscription *range_sub; -// uint64_t range_time; -// -// /* do not allow top copying this class */ -// MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); -// MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); -// -//protected: -// explicit MavlinkStreamDistanceSensor() : MavlinkStream(mavlink), -// range_sub(nullptr), -// range_time(0) -// {} -// -// void subscribe() -// { -// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); -// } -// -// void send(const hrt_abstime t) -// { -// struct range_finder_report range; -// -// if (range_sub->update(&range_time, &range)) { -// -// uint8_t type; -// -// switch (range.type) { -// case RANGE_FINDER_TYPE_LASER: -// type = MAV_DISTANCE_SENSOR_LASER; -// break; -// } -// -// uint8_t id = 0; -// uint8_t orientation = 0; -// uint8_t covariance = 20; -// -// mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, -// range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); -// } -// } -//}; +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamOpticalFlow::get_name_static(); + } + + static const char *get_name_static() + { + return "OPTICAL_FLOW"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamOpticalFlow(mavlink); + } + + unsigned get_size() + { + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_flow_sub; + uint64_t _flow_time; + + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + +protected: + explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), + _flow_time(0) + {} + + void send(const hrt_abstime t) + { + struct optical_flow_s flow; + + if (_flow_sub->update(&_flow_time, &flow)) { + mavlink_optical_flow_t msg; + + msg.time_usec = flow.timestamp; + msg.sensor_id = flow.sensor_id; + msg.flow_x = flow.flow_raw_x; + msg.flow_y = flow.flow_raw_y; + msg.flow_comp_m_x = flow.flow_comp_x_m; + msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.quality = flow.quality; + msg.ground_distance = flow.ground_distance_m; + + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + } + } +}; + +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeControls::get_name_static(); + } + + static const char *get_name_static() + { + return "ATTITUDE_CONTROLS"; + } + + uint8_t get_id() + { + return 0; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeControls(mavlink); + } + + unsigned get_size() + { + return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + } + +private: + MavlinkOrbSubscription *_att_ctrl_sub; + uint64_t _att_ctrl_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + +protected: + explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)), + _att_ctrl_time(0) + {} + + void send(const hrt_abstime t) + { + struct actuator_controls_s att_ctrl; + + if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_named_value_float_t msg; + + msg.time_boot_ms = att_ctrl.timestamp / 1000; + + snprintf(msg.name, sizeof(msg.name), "rll ctrl"); + msg.value = att_ctrl.control[0]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "ptch ctrl"); + msg.value = att_ctrl.control[1]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "yaw ctrl"); + msg.value = att_ctrl.control[2]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + + snprintf(msg.name, sizeof(msg.name), "thr ctrl"); + msg.value = att_ctrl.control[3]; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + } + } +}; + + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNamedValueFloat::get_name_static(); + } + + static const char *get_name_static() + { + return "NAMED_VALUE_FLOAT"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamNamedValueFloat(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_debug_sub; + uint64_t _debug_time; + + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + +protected: + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), + _debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))), + _debug_time(0) + {} + + void send(const hrt_abstime t) + { + struct debug_key_value_s debug; + + if (_debug_sub->update(&_debug_time, &debug)) { + mavlink_named_value_float_t msg; + + msg.time_boot_ms = debug.timestamp_ms; + memcpy(msg.name, debug.key, sizeof(msg.name)); + /* enforce null termination */ + msg.name[sizeof(msg.name) - 1] = '\0'; + msg.value = debug.value; + + _mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg); + } + } +}; + + +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCameraCapture::get_name_static(); + } + + static const char *get_name_static() + { + return "CAMERA_CAPTURE"; + } + + uint8_t get_id() + { + return 0; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamCameraCapture(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_status_sub; + + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + +protected: + explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) + {} + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + (void)_status_sub->update(&status); + + mavlink_command_long_t msg; + + msg.target_system = mavlink_system.sysid; + msg.target_component = MAV_COMP_ID_ALL; + msg.command = MAV_CMD_DO_CONTROL_VIDEO; + msg.confirmation = 0; + msg.param1 = 0; + msg.param2 = 0; + msg.param3 = 0; + /* set camera capture ON/OFF depending on arming state */ + msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param5 = 0; + msg.param6 = 0; + msg.param7 = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg); + } +}; + + +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamDistanceSensor::get_name_static(); + } + + static const char *get_name_static() + { + return "DISTANCE_SENSOR"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamDistanceSensor(mavlink); + } + + unsigned get_size() + { + return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_range_sub; + uint64_t _range_time; + + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + +protected: + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), + _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), + _range_time(0) + {} + + void send(const hrt_abstime t) + { + struct range_finder_report range; + + if (_range_sub->update(&_range_time, &range)) { + + mavlink_distance_sensor_t msg; + + msg.time_boot_ms = range.timestamp / 1000; + + switch (range.type) { + case RANGE_FINDER_TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + default: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + msg.id = 0; + msg.orientation = 0; + msg.min_distance = range.minimum_distance * 100; + msg.max_distance = range.minimum_distance * 100; + msg.current_distance = range.distance * 100; + msg.covariance = 20; + + _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); + } + } +}; StreamListItem *streams_list[] = { @@ -2111,6 +2142,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), @@ -2123,11 +2155,10 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), -// new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), -// new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), -// new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), -// new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), -// new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), -// new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), + new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), + new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), + new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), nullptr }; -- cgit v1.2.3 From 2cf688312a83d3c8ad5b813de0643901891886c0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 11:58:59 +0200 Subject: mavlink: show 'rate mult' in 'mavlink status' output --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4ae90c344..69beb9cc9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1660,6 +1660,7 @@ Mavlink::display_status() 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); + printf("\trate mult: %.3f\n", (double)_rate_mult); } int -- cgit v1.2.3 From 05e253b357caca3cdeb274027c7627a8a59d190d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:49:01 +0200 Subject: Proper locking for message send calls --- src/modules/mavlink/mavlink_main.cpp | 22 ++++++++++++++++------ src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 17 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 49781e8f8..8c79a1fee 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,9 +159,10 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, + _rstatus{}, + _message_buffer{}, + _message_buffer_mutex{}, + _send_mutex{}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -716,7 +717,7 @@ Mavlink::get_free_tx_buf() * flow control if it continues to be full */ int buf_free = 0; - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) { /* Disable hardware flow control: @@ -742,6 +743,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) return; } + pthread_mutex_lock(_send_mutex); + int buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; @@ -754,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - return; + goto out; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -762,7 +765,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* header */ buf[0] = MAVLINK_STX; buf[1] = payload_len; - buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; // TODO use internal seq counter? + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; buf[4] = mavlink_system.compid; buf[5] = msgid; @@ -790,6 +794,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + +out: + pthread_mutex_unlock(_send_mutex); } void @@ -1298,6 +1305,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 38149cf10..1e2e94cef 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -342,6 +342,7 @@ private: mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; bool _param_initialized; param_t _param_system_id; -- cgit v1.2.3 From 5bad95c92dc7b974a793074ca173fe129d258fb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:52:02 +0200 Subject: make GCC happy --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8c79a1fee..618b9fa29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -743,7 +743,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) return; } - pthread_mutex_lock(_send_mutex); + pthread_mutex_lock(&_send_mutex); int buf_free = get_free_tx_buf(); @@ -757,7 +757,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); - goto out; + pthread_mutex_unlock(&_send_mutex); + return; } uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -795,8 +796,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) count_txbytes(packet_len); } -out: - pthread_mutex_unlock(_send_mutex); + pthread_mutex_unlock(&_send_mutex); } void -- cgit v1.2.3 From fbbfe61d745202f7163d874ab61d4568f3a18a21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 10:49:28 +0200 Subject: Add missing lock --- src/modules/mavlink/mavlink_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 618b9fa29..84422c4aa 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -808,6 +808,8 @@ Mavlink::resend_message(mavlink_message_t *msg) return; } + pthread_mutex_lock(&_send_mutex); + int buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); @@ -819,6 +821,7 @@ Mavlink::resend_message(mavlink_message_t *msg) /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); return; } @@ -842,6 +845,8 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } + + pthread_mutex_unlock(&_send_mutex); } void -- cgit v1.2.3 From 2b71067618b2b4333c44072e27b960435407166d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 11:28:17 +0200 Subject: Code style fixes --- src/modules/mavlink/mavlink_main.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 84422c4aa..5589738c1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,10 +159,10 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _send_mutex{}, + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, _param_initialized(false), _param_system_id(0), _param_component_id(0), @@ -387,7 +387,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) /* if not in normal mode, we are an onboard link * onboard links should only pass on messages from the same system ID */ - if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { inst->pass_message(msg); } } @@ -731,6 +731,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } + return buf_free; } @@ -778,7 +779,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* checksum */ uint16_t checksum; crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char*) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); crc_accumulate(mavlink_message_crcs[msgid], &checksum); buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); @@ -985,7 +986,7 @@ Mavlink::adjust_stream_rates(const float multiplier) /* allow max ~600 Hz */ if (interval < 1600) { interval = 1600; - } + } /* set new interval */ stream->set_interval(interval * multiplier); @@ -1513,6 +1514,7 @@ Mavlink::task_main(int argc, char *argv[]) _bytes_txerr = 0; _bytes_rx = 0; } + _bytes_timestamp = t; } @@ -1676,6 +1678,7 @@ Mavlink::display_status() } 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); -- cgit v1.2.3 From 7f293be7d77603768899aedb438821dd19b8b4d7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 31 Jul 2014 17:30:00 +0200 Subject: mavlink, rc.usb: increase HIL_CONTROLS rate and datarate on USB to allow HIL simulation @ 200Hz --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index eb6039b4c..f1c21f295 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 10000 -d /dev/ttyACM0 -x +mavlink start -r 20000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 usleep 100000 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5589738c1..9a39d3bed 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAX_DATA_RATE 20000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate #define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN @@ -694,7 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) /* enable HIL */ if (hil_enabled && !_hil_enabled) { _hil_enabled = true; - configure_stream("HIL_CONTROLS", 150.0f); + configure_stream("HIL_CONTROLS", 200.0f); } /* disable HIL */ -- cgit v1.2.3