From f4c36f8c5cf69bd3af55a1065ab7a18c999c054c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 20:57:36 +0200 Subject: mc_pos_control: use current velocity to calculate position setpoint on reset to make transition to stabilized modes more smooth --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7c625a0c5..fe0e8fe1a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -493,8 +493,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +504,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } -- cgit v1.2.3 From 7232c0354bee1f8f572bdb2f53f9969ba9778a6e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 23:01:30 +0200 Subject: mc_pos_control: use MPC_XXX_FF to adjust setpoint on reset --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fe0e8fe1a..356f3c15e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -388,9 +388,11 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -493,8 +495,8 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1); + _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); + _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -504,7 +506,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2); + _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } -- cgit v1.2.3 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(-) 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 5fc3bc787a495d15099e177b99c2c4d6a1a4f56c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:38:39 +0200 Subject: code style only: Fix indendation in mavlink.h --- src/modules/mavlink/mavlink_main.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index da989d6c5..adca0e88f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -250,13 +250,13 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; + MavlinkMissionManager *_mission_manager; - orb_advert_t _mission_pub; + orb_advert_t _mission_pub; int _mission_result_sub; - MAVLINK_MODE _mode; + MAVLINK_MODE _mode; - mavlink_channel_t _channel; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; -- 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(-) 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(-) 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(-) 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(-) 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(-) 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(+) 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(-) 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(-) 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(-) 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 e6594e519295ec081c6f962fca6b4fbdd190846f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 Jul 2014 10:20:10 +0900 Subject: tone_alarm: add EKF_WARNING tune Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index b7981e9c4..19f792d19 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -149,6 +149,7 @@ enum { TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, + TONE_EKF_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 810f4aacc..03c7bd399 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -336,6 +336,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Wed, 23 Jul 2014 11:03:04 +0200 Subject: mavlink: parameters handling moved to MavlinkParametersManager class --- src/modules/mavlink/mavlink_parameters.cpp | 197 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_parameters.h | 114 +++++++++++++++++ 2 files changed, 311 insertions(+) create mode 100644 src/modules/mavlink/mavlink_parameters.cpp create mode 100644 src/modules/mavlink/mavlink_parameters.h diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp new file mode 100644 index 000000000..b1dae918c --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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_parameters.cpp + * Mavlink parameters manager implementation. + * + * @author Anton Babushkin + */ + +//#include + +#include "mavlink_parameters.h" + +explicit +MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), + _send_all_index(-1) +{ +} + +unsigned +MavlinkParametersManager::get_size() +{ + if (_send_all_index() >= 0) { + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + } else { + return 0; + } +} + +void +MavlinkParametersManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* request all parameters */ + mavlink_param_request_list_t req_list; + mavlink_msg_param_request_list_decode(msg, &req_list); + + if (req_list.target_system == mavlink_system.sysid && + (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + + _send_all_index = 0; + _mavlink->send_statustext_info("[pm] sending list"); + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + /* set parameter */ + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || 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, 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 param: %s", name); + _mavlink->send_statustext_info(buf); + + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + /* request one parameter */ + mavlink_param_request_read_t req_read; + mavlink_msg_param_request_read_decode(msg, &req_read); + + if (req_read.target_system == mavlink_system.sysid && + (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + + /* when no index is given, loop through string ids and compare them */ + if (req_read.param_index < 0) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_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 */ + send_param(param_find(name)); + + } else { + /* when index is >= 0, send this parameter again */ + send_param(param_for_index(req_read.param_index)); + } + } + break; + } + + default: + break; + } +} + +void +MavlinkParametersManager::send(const hrt_abstime t) +{ + /* send all parameters if requested */ + if (_send_all_index >= 0) { + send_param(param_for_index(_send_all_index)); + _send_all_index++; + if (_send_all_index >= param_count()) { + _send_all_index = -1; + } + } +} + +void +MavlinkParametersManager::send_param(param_t param) +{ + if (param == PARAM_INVALID) { + return; + } + + mavlink_param_value_t msg; + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + if (param_get(param, &msg.param_value) != OK) { + return; + } + + msg.param_count = param_count(); + msg.param_index = param_get_index(param); + + /* copy parameter name */ + strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* query parameter type */ + param_type_t type = param_type(param); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + if (type == PARAM_TYPE_INT32) { + msg.param_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + msg.param_type = MAVLINK_TYPE_FLOAT; + + } else { + msg.param_type = MAVLINK_TYPE_FLOAT; + } + + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 000000000..9b7a04a73 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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_parameters.h + * Mavlink parameters manager definition. + * + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_stream.h" + +class MavlinkParametersManager : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkParametersManager::get_name_static(); + } + + static const char *get_name_static() + { + return "PARAM_VALUE"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_PARAM_VALUE; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkParametersManager(mavlink); + } + + unsigned get_size(); + + void handle_message(const mavlink_message_t *msg); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + void start_send_one(int index); + + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int start_send_for_name(const char *name); + + /** + * 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 start_send_all(); + +private: + int _send_all_index; + + /* do not allow top copying this class */ + MavlinkParametersManager(MavlinkParametersManager &); + MavlinkParametersManager& operator = (const MavlinkParametersManager &); + +protected: + explicit MavlinkParametersManager(Mavlink *mavlink); + + void subscribe() {} + + void send(const hrt_abstime t); + + void send_param(param_t param); +}; -- cgit v1.2.3 From 344a34bb725a65769a227157f41363fd5d180a14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 11:03:37 +0200 Subject: mavlink: MavlinkStream API updated --- src/modules/mavlink/mavlink_stream.cpp | 5 +++-- src/modules/mavlink/mavlink_stream.h | 13 ++++++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 79dc23cc6..da3a55172 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,11 +71,12 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + unsigned int interval = _interval * _mavlink->get_rate_mult(); - if (dt > 0 && dt >= _interval) { + if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); - _last_sent = (t / _interval) * _interval; + _last_sent = t; return 0; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 7d3afbbb5..646931c07 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -46,8 +46,6 @@ class Mavlink; class MavlinkStream; -#include "mavlink_main.h" - class MavlinkStream { @@ -70,7 +68,6 @@ public: * @return the inveral in microseconds (us) between messages */ unsigned get_interval() { return _interval; } - void set_channel(mavlink_channel_t channel); /** * @return 0 if updated / sent, -1 if unchanged @@ -82,6 +79,16 @@ public: virtual const char *get_name() const = 0; virtual uint8_t get_id() = 0; + /** + * @return true if steam rate shouldn't be adjusted + */ + virtual bool const_rate() { return false; } + + /** + * Get maximal total messages size on update + */ + virtual unsigned get_size() = 0; + protected: Mavlink * _mavlink; unsigned int _interval; -- 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(-) 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 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 1fdc666bb0be393f048c85b1827494beedff0426 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:53:19 +0200 Subject: Update NuttX to deal with parity --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 7e1b97bcf..088146b90 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df +Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 -- cgit v1.2.3 From f7a40c5c6dcccf0ad14e97ef7fe1e426230e6e01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:55:42 +0200 Subject: Improve I2C address comment. Make filter much stronger to damp any non-realistic vehicle motion. --- src/drivers/meas_airspeed/meas_airspeed.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 6ab437716..118c35960 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -92,7 +92,7 @@ #include /* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ #define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ @@ -102,9 +102,9 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ -#define MEAS_RATE 100.0f -#define MEAS_DRIVER_FILTER_FREQ 3.0f -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 0.5f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed { -- cgit v1.2.3 From 2313bf7cd5cbf14ffaec61116cce84b9c0c18653 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:59:04 +0200 Subject: mTECS: Note better filter defaults to get airspeed response a bit smoother. Damp also ACC response with filter more than currently --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 4ca31fe20..7cfe63fbc 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -241,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); /** * P gain for the airspeed control @@ -268,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); * * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f); /** * Minimal acceleration (air) @@ -286,13 +293,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); */ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); -/** - * Airspeed derivative calculation lowpass - * - * @group mTECS - */ -PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - /** * Minimal throttle during takeoff * -- 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(-) 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(-) 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 a65b7aee0bc07eeb7545f4a3206e860791b64a36 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Jul 2014 22:54:48 +0200 Subject: mavlink: ATTITUDE, ATTITUDE_QUATERNION, VFR_HUD streams added --- src/modules/mavlink/mavlink_messages.cpp | 544 ++++++++++++++++--------------- 1 file changed, 284 insertions(+), 260 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a77d34c71..575268814 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -563,224 +563,248 @@ protected: } } }; -// -// -//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(Mavlink *mavlink) -// { -// 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(Mavlink *mavlink) -// { -// 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(Mavlink *mavlink) -// { -// 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 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(Mavlink *mavlink) + { + return new MavlinkStreamAttitude(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + + +protected: + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + 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)) { + mavlink_attitude_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.roll = att.roll; + msg.pitch = att.pitch; + msg.yaw = att.yaw; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamAttitudeQuaternion(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *att_sub; + uint64_t att_time; + + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + +protected: + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sub(nullptr), + 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)) { + mavlink_attitude_quaternion_t msg; + + msg.time_boot_ms = att.timestamp / 1000; + msg.q1 = att.q[0]; + msg.q2 = att.q[1]; + msg.q3 = att.q[2]; + msg.q4 = att.q[3]; + msg.rollspeed = att.rollspeed; + msg.pitchspeed = att.pitchspeed; + msg.yawspeed = att.yawspeed; + + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamVFRHUD(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +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(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) + {} + + 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; + 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) { + mavlink_vfr_hud_t msg; + + msg.airspeed = airspeed.true_airspeed_m_s; + msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; + msg.alt = pos.alt; + msg.climb = -pos.vel_d; + + _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); + } + } +}; + + //class MavlinkStreamGPSRawInt : public MavlinkStream //{ //public: @@ -818,9 +842,9 @@ protected: // gps_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); +// gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); // } // // void send(const hrt_abstime t) @@ -886,10 +910,10 @@ protected: // home_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// 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) @@ -953,9 +977,9 @@ protected: // pos_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); // } // // void send(const hrt_abstime t) @@ -1014,9 +1038,9 @@ protected: // pos_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// pos_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); // } // // void send(const hrt_abstime t) @@ -1072,9 +1096,9 @@ protected: // home_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home_sub = _mavlink->add_orb_subscription(ORB_ID(home_position)); // } // // void send(const hrt_abstime t) @@ -1144,7 +1168,7 @@ protected: // act_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { // orb_id_t act_topics[] = { // ORB_ID(actuator_outputs_0), @@ -1153,7 +1177,7 @@ protected: // ORB_ID(actuator_outputs_3) // }; // -// act_sub = mavlink->add_orb_subscription(act_topics[N]); +// act_sub = _mavlink->add_orb_subscription(act_topics[N]); // } // // void send(const hrt_abstime t) @@ -1224,11 +1248,11 @@ protected: // act_time(0) // {} // -// 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)); -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// 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) @@ -1352,9 +1376,9 @@ protected: // pos_sp_triplet_sub(nullptr) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet_sub = _mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); // } // // void send(const hrt_abstime t) @@ -1410,9 +1434,9 @@ protected: // pos_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// pos_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); // } // // void send(const hrt_abstime t) @@ -1468,9 +1492,9 @@ protected: // att_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// att_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); // } // // void send(const hrt_abstime t) @@ -1526,9 +1550,9 @@ protected: // att_rates_sp_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// att_rates_sp_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); // } // // void send(const hrt_abstime t) @@ -1584,9 +1608,9 @@ protected: // rc_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// rc_sub = _mavlink->add_orb_subscription(ORB_ID(input_rc)); // } // // void send(const hrt_abstime t) @@ -1678,9 +1702,9 @@ protected: // manual_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// manual_sub = _mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); // } // // void send(const hrt_abstime t) @@ -1737,9 +1761,9 @@ protected: // flow_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// flow_sub = _mavlink->add_orb_subscription(ORB_ID(optical_flow)); // } // // void send(const hrt_abstime t) @@ -1795,9 +1819,9 @@ protected: // att_ctrl_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// att_ctrl_sub = _mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); // } // // void send(const hrt_abstime t) @@ -1863,9 +1887,9 @@ protected: // debug_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// debug_sub = _mavlink->add_orb_subscription(ORB_ID(debug_key_value)); // } // // void send(const hrt_abstime t) @@ -1919,9 +1943,9 @@ protected: // 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) @@ -1979,9 +2003,9 @@ protected: // range_time(0) // {} // -// void subscribe(Mavlink *mavlink) +// void subscribe() // { -// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// range_sub = _mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); // } // // void send(const hrt_abstime t) @@ -2014,9 +2038,9 @@ StreamListItem *streams_list[] = { 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), -// new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), -// new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::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), -- 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(-) 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(-) 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 eb4015ced1b55a280fe93e10811c0dad5c719438 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Jul 2014 12:10:56 +0200 Subject: mavlink: strange FIXME comment removed --- src/modules/mavlink/mavlink_mission.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index b792b9aaf..5bf80768c 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -46,7 +46,6 @@ #include "mavlink_rate_limiter.h" #include -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, -- 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(-) 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 87334a987a5b571f6cc3d962df1194584de4fd7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 17:42:45 +0200 Subject: Return 0 for a non-reset --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 768e0be35..3d504d395 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2773,7 +2773,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) ResetHeight(); ResetStoredStates(); - ret = 3; + ret = 0; } // Reset the filter if gyro offsets are excessive -- cgit v1.2.3 From f03f6ddd74b1aca710f4675bbee18608eeea1f08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 17:57:15 +0200 Subject: Improve user feedback on mission load fails --- src/modules/navigator/mission.cpp | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10..12f6b9c21 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -314,30 +314,36 @@ Mission::set_mission_items() /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* get home distance state */ + bool home_dist_ok = check_dist_1wp(); + /* the home dist check provides user feedback, so we initialize it to this */ + bool user_feedback_done = !home_dist_ok; + /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { + } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { - /* no mission available, switch to loiter */ + /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: mission finished"); - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: no mission available"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + } else if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; @@ -397,7 +403,7 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; @@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR waypoint could not be read"); + "ERROR waypoint could not be read"); return false; } @@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: DO JUMP repetitions completed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "#audio: ERROR DO JUMP is cycling, giving up"); + "ERROR DO JUMP is cycling, giving up"); return false; } -- 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(-) 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 36d8d73aeb7ef214979a64960df727bf3bab048c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:58:17 +0200 Subject: mavlink: use correct component ID --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6885bebde..92e469619 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1832,11 +1832,11 @@ protected: || 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); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, 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); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- cgit v1.2.3 From 58aa9f28f060f7adf7955b39d72ac972c03f1348 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 24 Jul 2014 17:59:02 +0200 Subject: mavlink: when detecting spoof, be more verbose --- src/modules/mavlink/mavlink_receiver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 54c412ce7..69e3ef31d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -241,7 +241,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP ID"); + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); return; } -- 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(-) 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(-) 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 53f1b3190215f373aa5d8d38264b4ba5771b1676 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 21:41:09 +0200 Subject: Do not abort on submodule init feedback --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 0c03396e7..3904a2775 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -73,8 +73,8 @@ then exit 1 fi else - git submodule init - git submodule update + git submodule init; + git submodule update; fi exit 0 -- cgit v1.2.3 From b72e89697c564c878c4e17c46b435e8fe94736c5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 25 Jul 2014 00:52:42 +0400 Subject: UAVCAN update for #1229 --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index dca2611c3..f51ee9cde 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3 +Subproject commit f51ee9cdecd7841f6208bfb0b16228888daad663 -- cgit v1.2.3 From fd52e5561eaf14f9c6ca1abc00cb82b0e3d38ec8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:46:39 +0200 Subject: update comment about condition_global_position_valid --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b75..cbb148eda 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -186,7 +186,7 @@ struct vehicle_status_s { bool condition_system_sensors_initialized; bool condition_system_returned_to_home; bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; -- cgit v1.2.3 From e92a0cd10dc20ced072c77a0183a6112179f4639 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 25 Jul 2014 02:14:35 +0400 Subject: UAVCAN update for #1229 (windoze) --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index f51ee9cde..d84fc8a84 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit f51ee9cdecd7841f6208bfb0b16228888daad663 +Subproject commit d84fc8a84678d93f97f93b240c81472797ca5889 -- 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(-) 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 80a197f0fffb39037ce271bbc2e2107f2208eb01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 25 Jul 2014 09:45:23 +0200 Subject: Revert "mavlink: use correct component ID" This reverts commit 36d8d73aeb7ef214979a64960df727bf3bab048c. --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 92e469619..6885bebde 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1832,11 +1832,11 @@ protected: || status.arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + 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, mavlink_system.compid, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; -- 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(-) 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(-) 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 c5a623e15892d63eb3571bc450edea368b2894db Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Jul 2014 14:30:45 +0200 Subject: rcS: set rates for parameters and missions on USB connection --- ROMFS/px4fmu_common/init.d/rc.usb | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 1f8d8b862..d31ef11cf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,6 +5,10 @@ mavlink start -r 10000 -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 +mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 usleep 100000 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 -- cgit v1.2.3 From 2de38d0628f3146caea28cd42b30840241269f41 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 25 Jul 2014 23:30:37 -0400 Subject: Improve update performance and clean up compiler warnings in px4io driver - Fix compiler warnings in px4io_serial.cpp - Fix compiler warnings in px4io_uploader.cpp - Rename confusing overloaded send method with nearly identical parameters in px4io_uploader.cpp - Improve update performance by using maximum size programming buffer since we are no longer limited by stack size. --- src/drivers/px4io/px4io_serial.cpp | 8 +++++++- src/drivers/px4io/px4io_uploader.cpp | 32 +++++++++++++++++--------------- src/drivers/px4io/uploader.h | 8 ++++---- 3 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index c39494fb0..d227e15d5 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -157,6 +157,10 @@ private: perf_counter_t _pc_idle; perf_counter_t _pc_badidle; + /* do not allow top copying this class */ + PX4IO_serial(PX4IO_serial &); + PX4IO_serial& operator = (const PX4IO_serial &); + }; IOPacket PX4IO_serial::_dma_buffer; @@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _bus_semaphore(SEM_INITIALIZER(0)), + _completion_semaphore(SEM_INITIALIZER(0)), + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index bf6893a7e..986e39dc8 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -65,7 +65,8 @@ PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), - _fw_fd(-1) + _fw_fd(-1), + bl_rev(0) { } @@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[]) } int -PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout) { struct pollfd fds[1]; @@ -262,22 +263,23 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) return -ETIMEDOUT; } - read(_io_fd, &c, 1); + read(_io_fd, c, 1); #ifdef UDEBUG - log("recv 0x%02x", c); + log("recv_bytes 0x%02x", c); #endif return OK; } int -PX4IO_Uploader::recv(uint8_t *p, unsigned count) +PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { - int ret; - while (count--) { - ret = recv(*p++, 5000); + int ret = OK; + while (count) { + ret = recv_byte_with_timeout(p++, 5000); if (ret != OK) break; + count--; } return ret; } @@ -289,10 +291,10 @@ PX4IO_Uploader::drain() int ret; do { - // the small recv timeout here is to allow for fast + // the small recv_bytes timeout here is to allow for fast // drain when rebooting the io board for a forced // update of the fw without using the safety switch - ret = recv(c, 40); + ret = recv_byte_with_timeout(&c, 40); #ifdef UDEBUG if (ret == OK) { @@ -331,12 +333,12 @@ PX4IO_Uploader::get_sync(unsigned timeout) uint8_t c[2]; int ret; - ret = recv(c[0], timeout); + ret = recv_byte_with_timeout(c, timeout); if (ret != OK) return ret; - ret = recv(c[1], timeout); + ret = recv_byte_with_timeout(c + 1, timeout); if (ret != OK) return ret; @@ -372,7 +374,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) send(param); send(PROTO_EOC); - ret = recv((uint8_t *)&val, sizeof(val)); + ret = recv_bytes((uint8_t *)&val, sizeof(val)); if (ret != OK) return ret; @@ -513,7 +515,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size) for (ssize_t i = 0; i < count; i++) { uint8_t c; - ret = recv(c, 5000); + ret = recv_byte_with_timeout(&c, 5000); if (ret != OK) { log("%d: got %d waiting for bytes", sent + i, ret); @@ -600,7 +602,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) send(PROTO_GET_CRC); send(PROTO_EOC); - ret = recv((uint8_t*)(&crc), sizeof(crc)); + ret = recv_bytes((uint8_t*)(&crc), sizeof(crc)); if (ret != OK) { log("did not receive CRC checksum"); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 3e2142cf2..e17523413 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -74,19 +74,19 @@ private: INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ - PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ + PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */ }; int _io_fd; int _fw_fd; - uint32_t bl_rev; /**< bootloader revision */ + uint32_t bl_rev; /**< bootloader revision */ void log(const char *fmt, ...); - int recv(uint8_t &c, unsigned timeout); - int recv(uint8_t *p, unsigned count); + int recv_byte_with_timeout(uint8_t *c, unsigned timeout); + int recv_bytes(uint8_t *p, unsigned count); void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); -- cgit v1.2.3 From 3a4da7c5fa827970a86777ee6f4dc201246f0d0d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 26 Jul 2014 09:05:44 -0400 Subject: Revert to original loop Original loop was correct, and slightly more efficient. Retain initialization of ret to handle the case where passed in count is 0. --- src/drivers/px4io/px4io_uploader.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 986e39dc8..fb16f891f 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -274,12 +274,11 @@ int PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { int ret = OK; - while (count) { + while (count--) { ret = recv_byte_with_timeout(p++, 5000); if (ret != OK) break; - count--; } return ret; } -- cgit v1.2.3 From 54b9698d6560290e386b8dd2a7b9f0b6f4c5f57a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Jul 2014 17:48:45 +0200 Subject: circuit_breakers: added param to disable airspeed check --- src/modules/commander/commander.cpp | 2 ++ src/modules/commander/state_machine_helper.cpp | 4 +++- src/modules/systemlib/circuit_breaker.c | 12 ++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 5 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a0c896178..daba4e740 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -736,6 +736,7 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; + status.circuit_breaker_engaged_airspd_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -977,6 +978,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_changed = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8c..3c3d2f233 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -669,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) goto system_eval; } - if (!status->is_rotary_wing) { + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { /* accel done, close it */ close(fd); fd = orb_subscribe(ORB_ID(airspeed)); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8f697749e..64317a18a 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 1175dbce8..b60584608 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -52,6 +52,7 @@ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 #include diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index cbb148eda..b683bf98a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -226,6 +226,7 @@ struct vehicle_status_s { uint16_t errors_count4; bool circuit_breaker_engaged_power_check; + bool circuit_breaker_engaged_airspd_check; }; /** -- cgit v1.2.3 From 5d36381dc5f3bbb1eefc70158680f9622ac437b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:38:14 +0200 Subject: EKF: Fix wind publication, fix commented out flags --- .../ekf_att_pos_estimator_main.cpp | 27 ++++------------------ 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ccc497323..91d17e787 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -630,10 +630,10 @@ FixedwingEstimator::check_filter_state() rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); - // rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); - // rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); - // rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); - // rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); + rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4); + rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5); + rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6); + rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); @@ -1465,25 +1465,6 @@ FixedwingEstimator::task_main() _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; - - /* lazily publish the wind estimate only once available */ - if (_wind_pub > 0) { - /* publish the wind estimate */ - orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); - - } else { - /* advertise and publish */ - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); - } - - } - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; -- cgit v1.2.3 From 8e12d79ef4b32da98dfb13af1321a6855ecbdc3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Jul 2014 23:43:39 +0200 Subject: Increase GPS position timeout to real-life timeouts. More work needed. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daba4e740..6c24734e5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -124,7 +124,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 #define DL_TIMEOUT 5 * 1000* 1000 @@ -1111,7 +1111,7 @@ int commander_thread_main(int argc, char *argv[]) bool eph_epv_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { + if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) { eph_epv_good = false; } else { -- cgit v1.2.3 From e1361fdc02a75d72849b52a0102e02e400eecd9e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 00:07:01 +0200 Subject: mavlink: GPS_RAW_INT stream added --- src/modules/mavlink/mavlink_messages.cpp | 134 ++++++++++++++++--------------- 1 file changed, 71 insertions(+), 63 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4333369fe..8f0c15f5a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -839,69 +839,77 @@ protected: }; -//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(Mavlink *mavlink) -// { -// 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() -// { -// 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 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(Mavlink *mavlink) + { + return new MavlinkStreamGPSRawInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; + + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + +protected: + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), + _gps_sub(nullptr), + _gps_time(0) + {} + + void subscribe() + { + _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_gps_raw_int_t msg; + + msg.time_usec = gps.timestamp_position; + msg.fix_type = gps.fix_type; + msg.lat = gps.lat; + msg.lon = gps.lon; + msg.alt = gps.alt; + msg.eph = cm_uint16_from_m_float(gps.eph); + msg.epv = cm_uint16_from_m_float(gps.epv); + msg.vel = cm_uint16_from_m_float(gps.vel_m_s), + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); + } + } +}; + + //class MavlinkStreamGlobalPositionInt : public MavlinkStream //{ //public: -- cgit v1.2.3 From f1b55e578ff17d59cba13193cca4c83e764854b6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 11:02:56 +0200 Subject: mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 713 ++++++++++++++++--------------- 1 file changed, 362 insertions(+), 351 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8f0c15f5a..fe33c1bea 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -877,15 +877,10 @@ private: protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), - _gps_sub(nullptr), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), _gps_time(0) {} - void subscribe() - { - _gps_sub = _mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - } - void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; @@ -910,339 +905,355 @@ protected: }; -//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(Mavlink *mavlink) -// { -// 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() -// { -// 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(Mavlink *mavlink) -// { -// 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() -// { -// 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(Mavlink *mavlink) -// { -// 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() -// { -// 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(Mavlink *mavlink) -// { -// 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() -// { -// 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(Mavlink *mavlink) -// { -// 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() -// { -// 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 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(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +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(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _pos_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0) + {} + + 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_global_position_int_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.lat = pos.lat * 1e7; + msg.lon = pos.lon * 1e7; + msg.alt = pos.alt * 1000.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNED(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + +protected: + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_local_position_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_local_position_ned_t msg; + + msg.time_boot_ms = pos.timestamp / 1000; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamViconPositionEstimate(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sub; + uint64_t _pos_time; + + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + +protected: + explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))), + _pos_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_vicon_position_s pos; + + if (_pos_sub->update(&_pos_time, &pos)) { + mavlink_vicon_position_estimate_t msg; + + msg.usec = pos.timestamp; + msg.x = pos.x; + msg.y = pos.y; + msg.z = pos.z; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamGPSGlobalOrigin(mavlink); + } + + unsigned get_size() + { + return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + MavlinkOrbSubscription *_home_sub; + + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + +protected: + explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(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_gps_global_origin_t msg; + + msg.latitude = home.lat * 1e7; + msg.longitude = home.lon * 1e7; + msg.altitude = home.alt * 1e3f; + + _mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg); + } + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamServoOutputRaw(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_act_sub; + uint64_t _act_time; + + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + +protected: + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _act_sub(nullptr), + _act_time(0) + { + 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_servo_output_raw_t msg; + + msg.time_usec = act.timestamp; + msg.port = N; + msg.servo1_raw = act.output[0]; + msg.servo2_raw = act.output[1]; + msg.servo3_raw = act.output[2]; + msg.servo4_raw = act.output[3]; + msg.servo5_raw = act.output[4]; + msg.servo6_raw = act.output[5]; + msg.servo7_raw = act.output[6]; + msg.servo8_raw = act.output[7]; + + _mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg); + } + } +}; + + //class MavlinkStreamHILControls : public MavlinkStream //{ //public: @@ -1281,7 +1292,7 @@ protected: // MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); // //protected: -// explicit MavlinkStreamHILControls() : MavlinkStream(), +// explicit MavlinkStreamHILControls() : MavlinkStream(mavlink), // status_sub(nullptr), // status_time(0), // pos_sp_triplet_sub(nullptr), @@ -1414,7 +1425,7 @@ protected: // MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); // //protected: -// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), +// explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(mavlink), // pos_sp_triplet_sub(nullptr) // {} // @@ -1471,7 +1482,7 @@ protected: // MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); // //protected: -// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), +// explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(mavlink), // pos_sp_sub(nullptr), // pos_sp_time(0) // {} @@ -1529,7 +1540,7 @@ protected: // MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); // //protected: -// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), +// explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(mavlink), // att_sp_sub(nullptr), // att_sp_time(0) // {} @@ -1587,7 +1598,7 @@ protected: // MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); // //protected: -// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), +// explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(mavlink), // att_rates_sp_sub(nullptr), // att_rates_sp_time(0) // {} @@ -1645,7 +1656,7 @@ protected: // MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); // //protected: -// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), +// explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(mavlink), // rc_sub(nullptr), // rc_time(0) // {} @@ -1739,7 +1750,7 @@ protected: // MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); // //protected: -// explicit MavlinkStreamManualControl() : MavlinkStream(), +// explicit MavlinkStreamManualControl() : MavlinkStream(mavlink), // manual_sub(nullptr), // manual_time(0) // {} @@ -1798,7 +1809,7 @@ protected: // MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); // //protected: -// explicit MavlinkStreamOpticalFlow() : MavlinkStream(), +// explicit MavlinkStreamOpticalFlow() : MavlinkStream(mavlink), // flow_sub(nullptr), // flow_time(0) // {} @@ -1856,7 +1867,7 @@ protected: // MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); // //protected: -// explicit MavlinkStreamAttitudeControls() : MavlinkStream(), +// explicit MavlinkStreamAttitudeControls() : MavlinkStream(mavlink), // att_ctrl_sub(nullptr), // att_ctrl_time(0) // {} @@ -1924,7 +1935,7 @@ protected: // MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); // //protected: -// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), +// explicit MavlinkStreamNamedValueFloat() : MavlinkStream(mavlink), // debug_sub(nullptr), // debug_time(0) // {} @@ -1981,7 +1992,7 @@ protected: // MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); // //protected: -// explicit MavlinkStreamCameraCapture() : MavlinkStream(), +// explicit MavlinkStreamCameraCapture() : MavlinkStream(mavlink), // status_sub(nullptr) // {} // @@ -2040,7 +2051,7 @@ protected: // MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); // //protected: -// explicit MavlinkStreamDistanceSensor() : MavlinkStream(), +// explicit MavlinkStreamDistanceSensor() : MavlinkStream(mavlink), // range_sub(nullptr), // range_time(0) // {} -- cgit v1.2.3 From e087ec81c396d7da8d5f7a006748062ee07b693f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 13:53:45 +0200 Subject: mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 522 ++++++++++++++++--------------- 1 file changed, 268 insertions(+), 254 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fe33c1bea..14195b6a7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1254,260 +1254,274 @@ protected: }; -//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(Mavlink *mavlink) -// { -// 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(mavlink), -// status_sub(nullptr), -// status_time(0), -// pos_sp_triplet_sub(nullptr), -// pos_sp_triplet_time(0), -// act_sub(nullptr), -// act_time(0) -// {} -// -// 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)); -// 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(Mavlink *mavlink) -// { -// 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(mavlink), -// pos_sp_triplet_sub(nullptr) -// {} -// -// void subscribe() -// { -// 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(Mavlink *mavlink) -// { -// 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(mavlink), -// pos_sp_sub(nullptr), -// pos_sp_time(0) -// {} -// -// void subscribe() -// { -// 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 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(Mavlink *mavlink) + { + return new MavlinkStreamHILControls(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +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(Mavlink *mavlink) : MavlinkStream(mavlink), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _pos_sp_triplet_time(0), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_time(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_hil_controls_t msg; + + msg.time_usec = hrt_absolute_time(); + msg.roll_ailerons = out[0]; + msg.pitch_elevator = out[1]; + msg.yaw_rudder = out[2]; + msg.throttle = out[3]; + msg.aux1 = out[4]; + msg.aux2 = out[5]; + msg.aux3 = out[6]; + msg.aux4 = out[7]; + msg.mode = mavlink_base_mode; + msg.nav_mode = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_pos_sp_triplet_sub; + + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + +protected: + explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(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_global_position_setpoint_int_t msg; + + msg.coordinate_frame = MAV_FRAME_GLOBAL; + msg.latitude = pos_sp_triplet.current.lat * 1e7; + msg.longitude = pos_sp_triplet.current.lon * 1e7; + msg.altitude = pos_sp_triplet.current.alt * 1000; + msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + + _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +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(Mavlink *mavlink) : MavlinkStream(mavlink), + _pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))), + _pos_sp_time(0) + {} + + 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_local_position_setpoint_t msg; + + msg.coordinate_frame = MAV_FRAME_LOCAL_NED; + msg.x = pos_sp.x; + msg.y = pos_sp.y; + msg.z = pos_sp.z; + msg.yaw = pos_sp.yaw; + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); + } + } +}; + + //class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream //{ //public: -- cgit v1.2.3 From 3b3e6f5aaafd1247447cad7070e3488e5798ce3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Jul 2014 14:21:50 +0200 Subject: Increase filter pass-band --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 118c35960..03e50e5cb 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 0.5f +#define MEAS_DRIVER_FILTER_FREQ 0.8f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed -- cgit v1.2.3 From 67db8ee4f0908c31f17ed490c48ea21cc7ac3f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Jul 2014 14:27:10 +0200 Subject: Add missing states, build fix for master --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 4 ++++ src/modules/ekf_att_pos_estimator/estimator_utilities.h | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 3d504d395..ffdd29a5b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = n_states; + current_ekf_state.onGround = onGround; + current_ekf_state.staticMode = staticMode; + current_ekf_state.useCompass = useCompass; + current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 6d1f47b68..a6b670c4d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -68,6 +68,10 @@ struct ekf_status_report { bool posTimeout; bool hgtTimeout; bool imuTimeout; + bool onGround; + bool staticMode; + bool useCompass; + bool useAirspeed; uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; -- cgit v1.2.3 From 241802a71f2af56a8dd4510827f8ab5a59f5d6b9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Jul 2014 17:40:53 +0200 Subject: mavlink: more streams ported to new API --- src/modules/mavlink/mavlink_messages.cpp | 584 ++++++++++++++++--------------- 1 file changed, 300 insertions(+), 284 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 14195b6a7..a710cdf6a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1522,275 +1522,291 @@ protected: }; -//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(Mavlink *mavlink) -// { -// 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(mavlink), -// att_sp_sub(nullptr), -// att_sp_time(0) -// {} -// -// void subscribe() -// { -// 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(Mavlink *mavlink) -// { -// 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(mavlink), -// att_rates_sp_sub(nullptr), -// att_rates_sp_time(0) -// {} -// -// void subscribe() -// { -// 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(Mavlink *mavlink) -// { -// 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(mavlink), -// rc_sub(nullptr), -// rc_time(0) -// {} -// -// void subscribe() -// { -// 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(Mavlink *mavlink) -// { -// 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(mavlink), -// manual_sub(nullptr), -// manual_time(0) -// {} -// -// void subscribe() -// { -// 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 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(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +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(Mavlink *mavlink) : MavlinkStream(mavlink), + att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + att_sp_time(0) + {} + + void send(const hrt_abstime t) + { + struct vehicle_attitude_setpoint_s att_sp; + + if (att_sp_sub->update(&att_sp_time, &att_sp)) { + mavlink_roll_pitch_yaw_thrust_setpoint_t msg; + + msg.time_boot_ms = att_sp.timestamp / 1000; + msg.roll = att_sp.roll_body; + msg.pitch = att_sp.pitch_body; + msg.yaw = att_sp.yaw_body; + msg.thrust = att_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +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(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_rates_sp_time(0) + {} + + 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_roll_pitch_yaw_rates_thrust_setpoint_t msg; + + msg.time_boot_ms = att_rates_sp.timestamp / 1000; + msg.roll_rate = att_rates_sp.roll; + msg.pitch_rate = att_rates_sp.pitch; + msg.yaw_rate = att_rates_sp.yaw; + msg.thrust = att_rates_sp.thrust; + + _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamRCChannelsRaw(mavlink); + } + + 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; + } + +private: + MavlinkOrbSubscription *_rc_sub; + uint64_t _rc_time; + + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + +protected: + explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), + _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))), + _rc_time(0) + {} + + 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_rc_channels_raw_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.port = i; + msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); + } + + /* new message */ + mavlink_rc_channels_t msg; + + msg.time_boot_ms = rc.timestamp_publication / 1000; + msg.chancount = rc.channel_count; + msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX; + msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX; + msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX; + msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX; + msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX; + msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX; + msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX; + msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX; + msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX; + msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX; + msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX; + msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; + msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; + msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; + msg.rssi = rc.rssi; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); + } + } +}; + + +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(Mavlink *mavlink) + { + return new MavlinkStreamManualControl(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_manual_sub; + uint64_t _manual_time; + + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + +protected: + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), + _manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))), + _manual_time(0) + {} + + void send(const hrt_abstime t) + { + struct manual_control_setpoint_s manual; + + if (_manual_sub->update(&_manual_time, &manual)) { + mavlink_manual_control_t msg; + + msg.target = mavlink_system.sysid; + msg.x = manual.x * 1000; + msg.y = manual.y * 1000; + msg.z = manual.z * 1000; + msg.r = manual.r * 1000; + msg.buttons = 0; + + _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); + } + } +}; + + //class MavlinkStreamOpticalFlow : public MavlinkStream //{ //public: @@ -2109,21 +2125,21 @@ StreamListItem *streams_list[] = { 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(&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), -- 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(-) 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(-) 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 129c93f22f0717d9d85910de87fecece9fabbc79 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:11:58 +0200 Subject: Revert "Remove all unused TECS parameters" This reverts commit ce78b399691d43bd150c0a5928f08c98076a3892. Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 57 +++++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 176 +++++++++++++++++++++ 2 files changed, 233 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0b111f7bd..a1bc5c24e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -199,6 +199,19 @@ private: float l1_period; float l1_damping; + float time_const; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + float airspeed_min; float airspeed_trim; float airspeed_max; @@ -226,6 +239,19 @@ private: param_t l1_period; param_t l1_damping; + param_t time_const; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; @@ -438,6 +464,21 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); + /* fetch initial parameter values */ parameters_update(); } @@ -488,6 +529,22 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 27039ff51..52128e1b7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,6 +154,182 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + +/** + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + +/** + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +/** + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + +/** + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second square) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + +/** + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + +/** + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + +/** + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + +/** + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + +/** + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + +/** + * Height rate P factor + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/** + * Speed rate P factor + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); + /** * Landing slope angle * -- cgit v1.2.3 From af7385bdc268bf4f19e9d1291a3d753a2c634ef0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:15:23 +0200 Subject: Re-instate TECS default gains --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56..3c336f295 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,4 +11,8 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 fi \ No newline at end of file -- cgit v1.2.3 From ae2de675014c701b45710b646f3a816ad2222b73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:24:11 +0200 Subject: Revert "Remove old TECS implementation - we can really only decently flight-test and support one." This reverts commit 503ded05394767d58359834e73bc63672b701dbe. Conflicts: mavlink/include/mavlink/v1.0 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- makefiles/config_aerocore_default.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 74 +++++++++++++++++----- 4 files changed, 61 insertions(+), 16 deletions(-) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 53a2ad1ab..0d3934bd0 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -80,6 +80,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cc0958c29..a7c10f52f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -108,6 +108,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b40..d0a40445d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -121,6 +121,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a1bc5c24e..78e91bbfd 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -42,8 +42,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Implementation for total energy control class: - * Thomas Gubler + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) * * More details and acknowledgements in the referenced library headers. * @@ -87,6 +87,7 @@ #include #include #include +#include #include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -192,6 +193,7 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; + TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -557,6 +559,23 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); + /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -618,6 +637,9 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + return airspeed_updated; } @@ -790,6 +812,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + /* filter speed and altitude for controller */ + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; + + if (!_mTecs.getEnabled()) { + _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + } + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -815,6 +845,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1123,9 +1156,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getPitchSetpoint(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1305,20 +1338,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { - limitOverride.disablePitchMinOverride(); + /* Using tecs library */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); } int -- cgit v1.2.3 From 2b55eb605ed2216f9f73f5745de7d7a34c851e1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:27:59 +0200 Subject: Make airspeed filter a bit smoother, but do not induce a huge phase delay --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 03e50e5cb..9cfa7f383 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 0.8f +#define MEAS_DRIVER_FILTER_FREQ 1.5f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed -- cgit v1.2.3 From 5171286bbb7eef115bd7fc711f2c8fcebd432e4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:34:46 +0200 Subject: Re-add two params that fell off the truck before --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 78e91bbfd..f0e18d1bb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -205,6 +205,8 @@ private: float min_sink_rate; float max_sink_rate; float max_climb_rate; + float heightrate_p; + float speedrate_p; float throttle_damp; float integrator_gain; float vertical_accel_limit; @@ -245,6 +247,8 @@ private: param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t heightrate_p; + param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; param_t vertical_accel_limit; -- cgit v1.2.3 From c1d3f592b4082333ac454ab62dbdf685d41b3bb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:36:59 +0200 Subject: Make ext libs more flash efficient --- src/lib/external_lgpl/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk index 53f1629e3..29d3514f6 100644 --- a/src/lib/external_lgpl/module.mk +++ b/src/lib/external_lgpl/module.mk @@ -46,3 +46,5 @@ # SRCS = tecs/tecs.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 1bd57f1dbfb9c9896fabf71cec292aa7024a33ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 11:37:12 +0200 Subject: Make ECL more flash efficient --- src/lib/ecl/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index f2aa3db6a..93a5b511f 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ l1/ecl_l1_pos_controller.cpp + +MAXOPTIMIZATION = -Os -- 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(+) 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 e3bc5571556d043ad5c8c4fcabd1f9b371599397 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 11:59:38 +0200 Subject: rc.usb: set RC_CHANNELS_RAW rate to 5Hz --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index d31ef11cf..eb6039b4c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -19,6 +19,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 usleep 100000 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 usleep 100000 +mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 -- cgit v1.2.3 From c9eea8fbfaad7bfb3eee36a49588c9ac3a42ddc6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 12:01:09 +0200 Subject: nshterm: increase stack size to fix crash on 'ls -l' --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index b22b446da..e2fa0ff80 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 1400 -- cgit v1.2.3 From 5a7a6bca777317cc69840c35b82c50486dd061e7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Jul 2014 16:02:58 +0200 Subject: fetch_log.py renamed to fetch_file.py and reworked, works with all files, not only logs, added recursive directory download --- Tools/fetch_file.py | 207 ++++++++++++++++++++++++++++++++++++++++++++++++++++ Tools/fetch_log.py | 133 --------------------------------- 2 files changed, 207 insertions(+), 133 deletions(-) create mode 100644 Tools/fetch_file.py delete mode 100644 Tools/fetch_log.py diff --git a/Tools/fetch_file.py b/Tools/fetch_file.py new file mode 100644 index 000000000..7ad08192d --- /dev/null +++ b/Tools/fetch_file.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2013-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. +# +############################################################################ + +"""Fetch files via nsh console + +Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path +\t-l\tList files +\t-f\tOverwrite existing files +\t-d\tSerial device +\t-s\tSerial baudrate +\t-o\tOutput path +\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively""" + +__author__ = "Anton Babushkin" +__version__ = "1.1" + +import serial, time, sys, os + +def _wait_for_string(ser, s, timeout): + t0 = time.time() + buf = [] + res = [] + while (True): + c = ser.read() + buf.append(c) + if len(buf) > len(s): + res.append(buf.pop(0)) + if "".join(buf) == s: + break + if timeout > 0.0 and time.time() - t0 > timeout: + raise Exception("Timeout while waiting for: " + s) + return "".join(res) + +def _exec_cmd(ser, cmd, timeout): + ser.write(cmd + "\n") + ser.flush() + _wait_for_string(ser, cmd + "\r\n", timeout) + return _wait_for_string(ser, "nsh> \x1b[K", timeout) + +def _ls_dir_raw(ser, dir, timeout): + return _exec_cmd(ser, "ls -l " + dir, timeout) + +def _ls_dir(ser, dir, timeout): + res = [] + for line in _ls_dir_raw(ser, dir, timeout).splitlines(): + if line == dir + ":": + continue + if line.startswith("nsh: ls: no such directory:"): + raise Exception("No such file: " + dir) + res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) + return res + +def _get_file(ser, fn, fn_out, force, timeout): + print "Get %s:" % fn, + if not force: + # Check if file already exists with the same size + try: + os.stat(fn_out) + except: + pass + else: + print "already fetched, skip" + return + + cmd = "dumpfile " + fn + ser.write(cmd + "\n") + ser.flush() + _wait_for_string(ser, cmd + "\r\n", timeout) + res = _wait_for_string(ser, "\n", timeout) + if res.startswith("OK"): + # Got correct responce, open temp file + fn_out_part = fn_out + ".part" + fout = open(fn_out_part, "wb") + + size = int(res.split()[1]) + sys.stdout.write(" [%i bytes] " % size) + n = 0 + while (n < size): + buf = ser.read(min(size - n, 8192)) + n += len(buf) + sys.stdout.write(".") + sys.stdout.flush() + fout.write(buf) + print " done" + fout.close() + os.rename(fn_out_part, fn_out) + else: + raise Exception("Error reading file") + _wait_for_string(ser, "nsh> \x1b[K", timeout) + +def _get_files_in_dir(ser, path, path_out, force, timeout): + try: + os.mkdir(path_out) + except: + pass + for fn in _ls_dir(ser, path, timeout): + path_fn = os.path.join(path, fn[0]) + path_fn_out = os.path.join(path_out, fn[0]) + if fn[2]: + _get_files_in_dir(ser, path_fn[:-1], path_fn_out[:-1], force, timeout) + else: + _get_file(ser, path_fn, path_fn_out, force, timeout) + +def _usage(): + print """Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path +\t-l\tList files +\t-f\tOverwrite existing files +\t-d\tSerial device +\t-s\tSerial baudrate +\t-o\tOutput path +\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively""" + +def _main(): + dev = "/dev/tty.usbmodem1" + speed = "57600" + cmd = "get" + path = None + path_out = None + force = False + + opt = None + for arg in sys.argv[1:]: + if opt != None: + if opt == "d": + dev = arg + elif opt == "s": + speed = arg + elif opt == "o": + path_out = arg + opt = None + else: + if arg == "-l": + cmd = "ls" + elif arg == "-f": + force = True + elif arg == "-d": + opt = "d" + elif arg == "-s": + opt = "s" + elif arg == "-o": + opt = "o" + elif path == None: + path = arg + + if path == None: + _usage() + exit(0) + + # Connect to serial port + ser = serial.Serial(dev, speed, timeout=0.2) + + timeout = 1.0 + + try: + if cmd == "ls": + # List directory + print _ls_dir_raw(ser, path, timeout) + elif cmd == "get": + # Get file(s) + if path.endswith("/"): + # Get all files from directory recursively + if path_out == None: + path_out = os.path.split(path[:-1])[1] + _get_files_in_dir(ser, path[:-1], path_out, force, timeout) + else: + # Get one file + if path_out == None: + path_out = os.path.split(path)[1] + _get_file(ser, path, os.path.split(path)[1], force, timeout) + except Exception as e: + print e + + ser.close() + +if __name__ == "__main__": + _main() diff --git a/Tools/fetch_log.py b/Tools/fetch_log.py deleted file mode 100644 index edcc6557c..000000000 --- a/Tools/fetch_log.py +++ /dev/null @@ -1,133 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# Copyright (C) 2012, 2013 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. -# -############################################################################ - -# -# Log fetcher -# -# Print list of logs: -# python fetch_log.py -# -# Fetch log: -# python fetch_log.py sess001/log001.bin -# - -import serial, time, sys, os - -def wait_for_string(ser, s, timeout=1.0, debug=False): - t0 = time.time() - buf = [] - res = [] - n = 0 - while (True): - c = ser.read() - if debug: - sys.stderr.write(c) - buf.append(c) - if len(buf) > len(s): - res.append(buf.pop(0)) - n += 1 - if n % 10000 == 0: - sys.stderr.write(str(n) + "\n") - if "".join(buf) == s: - break - if timeout > 0.0 and time.time() - t0 > timeout: - raise Exception("Timeout while waiting for: " + s) - return "".join(res) - -def exec_cmd(ser, cmd, timeout): - ser.write(cmd + "\n") - ser.flush() - wait_for_string(ser, cmd + "\r\n", timeout) - return wait_for_string(ser, "nsh> \x1b[K", timeout) - -def ls_dir(ser, dir, timeout=1.0): - res = [] - for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]: - res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) - return res - -def list_logs(ser): - logs_dir = "/fs/microsd/log" - res = [] - for d in ls_dir(ser, logs_dir): - if d[2]: - sess_dir = d[0][:-1] - for f in ls_dir(ser, logs_dir + "/" + sess_dir): - log_file = f[0] - log_size = f[1] - res.append(sess_dir + "/" + log_file + "\t" + str(log_size)) - return "\n".join(res) - -def fetch_log(ser, fn, timeout): - cmd = "dumpfile " + fn - ser.write(cmd + "\n") - ser.flush() - wait_for_string(ser, cmd + "\r\n", timeout, True) - res = wait_for_string(ser, "\n", timeout, True) - data = [] - if res.startswith("OK"): - size = int(res.split()[1]) - n = 0 - print "Reading data:" - while (n < size): - buf = ser.read(min(size - n, 8192)) - data.append(buf) - n += len(buf) - sys.stdout.write(".") - sys.stdout.flush() - print - else: - raise Exception("Error reading log") - wait_for_string(ser, "nsh> \x1b[K", timeout) - return "".join(data) - -def main(): - dev = "/dev/tty.usbmodem1" - ser = serial.Serial(dev, "115200", timeout=0.2) - if len(sys.argv) < 2: - print list_logs(ser) - else: - log_file = sys.argv[1] - data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0) - try: - os.mkdir(log_file.split("/")[0]) - except: - pass - fout = open(log_file, "wb") - fout.write(data) - fout.close() - ser.close() - -if __name__ == "__main__": - main() -- cgit v1.2.3 From 7f2799a78baf022db05484df2168b1b5a89923a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 17:49:51 +0200 Subject: global pos topic docs --- src/modules/uORB/topics/vehicle_global_position.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index e32529cb4..25137c1c6 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -70,8 +70,8 @@ struct vehicle_global_position_s { float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; - float epv; + float eph; /**< Standard deviation of position estimate horizontally */ + float epv; /**< Standard deviation of position vertically */ }; /** -- cgit v1.2.3 From 0cbd943d093f2cb69102e57ef7964b5dbebd9f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 17:50:52 +0200 Subject: Differentiate between EPH and EPV. Do not declare position invalid because of EPV (because we use the baro anyway). No fundamental logic change / cleanup to ensure current approach and arming logic remains intact --- src/modules/commander/commander.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c24734e5..dec5ab46c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -163,7 +163,8 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; -static float eph_epv_threshold = 5.0f; +static float eph_threshold = 5.0f; +static float epv_threshold = 10.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -1108,32 +1109,32 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ - bool eph_epv_good; + bool eph_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) { - eph_epv_good = false; + if (global_position.eph > eph_threshold * 2.5f) { + eph_good = false; } else { - eph_epv_good = true; + eph_good = true; } } else { - if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { - eph_epv_good = true; + if (global_position.eph < eph_threshold) { + eph_good = true; } else { - eph_epv_good = false; + eph_good = false; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1163,8 +1164,8 @@ int commander_thread_main(int argc, char *argv[]) /* hysteresis for EPH */ bool local_eph_good; - if (status.condition_global_position_valid) { - if (local_position.eph > eph_epv_threshold * 2.0f) { + if (status.condition_local_position_valid) { + if (local_position.eph > eph_threshold * 2.5f) { local_eph_good = false; } else { @@ -1172,7 +1173,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (local_position.eph < eph_epv_threshold) { + if (local_position.eph < eph_threshold) { local_eph_good = true; } else { @@ -1504,7 +1505,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; -- cgit v1.2.3 From e3da7f564f1bc7746ba3be946fd02b95642cbf6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Jul 2014 21:36:55 +0200 Subject: NaN check and better init in lowpass --- src/modules/controllib/blocks.cpp | 3 +++ src/modules/controllib/blocks.hpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 0175acda9..f739446fa 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -121,6 +121,9 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { + if (!isfinite(getState())) { + setState(input); + } float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 37d7832b3..bffc355a8 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0), + _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; -- cgit v1.2.3 From 1dc23d0c49d99fa93284a277a6bc4970ac0e7b3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 17:41:45 +0200 Subject: Disable mTECS until runtime error is better understood --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 56 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 + 2 files changed, 29 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f0e18d1bb..2d6c5c121 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -194,7 +194,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - fwPosctrl::mTecs _mTecs; + // fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; struct { @@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), - _mTecs(), + // _mTecs(), _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update() launchDetector.updateParams(); /* Update the mTecs */ - _mTecs.updateParams(); + // _mTecs.updateParams(); return OK; } @@ -820,9 +820,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { + // if (!_mTecs.getEnabled()) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - } + // } /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -838,10 +838,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - if (_mTecs.getEnabled()) { - _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - } + // if (_mTecs.getEnabled()) { + // _mTecs.resetIntegrators(); + // _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + // } } _was_pos_control_mode = true; @@ -1160,9 +1160,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(/*_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + _att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1342,29 +1342,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - } else { - limitOverride.disablePitchMinOverride(); - } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); - } else { + // if (_mTecs.getEnabled()) { + // /* Using mtecs library: prepare arguments for mtecs call */ + // float flightPathAngle = 0.0f; + // float ground_speed_length = ground_speed.length(); + // if (ground_speed_length > FLT_EPSILON) { + // flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + // } + // fwPosctrl::LimitOverride limitOverride; + // if (climbout_mode) { + // limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + // } else { + // limitOverride.disablePitchMinOverride(); + // } + // _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + // limitOverride); + // } else { /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - } + // } } int diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2b..afc411a60 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,6 +76,7 @@ mTecs::mTecs() : _counter(0), _debug(false) { + warnx("starting"); } mTecs::~mTecs() -- cgit v1.2.3 From 39aa9112baf0120538255cf44292ff8e1ff3387c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 17:56:17 +0200 Subject: Fix threading in FW pos controller --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 87 +++++++++++----------- 1 file changed, 43 insertions(+), 44 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2d6c5c121..eadb63f40 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -92,6 +92,8 @@ #include "landingslope.h" #include "mtecs/mTecs.h" +static int _control_task = -1; /**< task handle for sensor task */ + /** * L1 control app start / stop handling function @@ -116,9 +118,9 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ - int start(); + static int start(); /** * Task status @@ -132,7 +134,6 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ int _global_pos_sub; int _pos_sp_triplet_sub; @@ -194,7 +195,7 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; - // fwPosctrl::mTecs _mTecs; + fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; struct { @@ -393,7 +394,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), _task_should_exit(false), _task_running(false), - _control_task(-1), /* subscriptions */ _global_pos_sub(-1), @@ -443,7 +443,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false), _l1_control(), - // _mTecs(), + _mTecs(), _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -603,7 +603,7 @@ FixedwingPositionControl::parameters_update() launchDetector.updateParams(); /* Update the mTecs */ - // _mTecs.updateParams(); + _mTecs.updateParams(); return OK; } @@ -704,7 +704,17 @@ FixedwingPositionControl::vehicle_setpoint_poll() void FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) { + l1_control::g_control = new FixedwingPositionControl(); + + if (l1_control::g_control == nullptr) { + warnx("OUT OF MEM"); + return; + } + + /* only returns on exit */ l1_control::g_control->task_main(); + delete l1_control::g_control; + l1_control::g_control = nullptr; } float @@ -820,9 +830,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - // if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled()) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - // } + } /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -838,10 +848,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_pos_control_mode) { /* reset integrators */ - // if (_mTecs.getEnabled()) { - // _mTecs.resetIntegrators(); - // _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); - // } + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } } _was_pos_control_mode = true; @@ -1160,9 +1170,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(/*_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : */_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = /*_mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : */_tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1178,10 +1188,6 @@ void FixedwingPositionControl::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -1342,29 +1348,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - // if (_mTecs.getEnabled()) { - // /* Using mtecs library: prepare arguments for mtecs call */ - // float flightPathAngle = 0.0f; - // float ground_speed_length = ground_speed.length(); - // if (ground_speed_length > FLT_EPSILON) { - // flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - // } - // fwPosctrl::LimitOverride limitOverride; - // if (climbout_mode) { - // limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - // } else { - // limitOverride.disablePitchMinOverride(); - // } - // _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - // limitOverride); - // } else { + if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); + } else { /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); - // } + } } int @@ -1398,14 +1404,7 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (l1_control::g_control != nullptr) errx(1, "already running"); - l1_control::g_control = new FixedwingPositionControl; - - if (l1_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != l1_control::g_control->start()) { - delete l1_control::g_control; - l1_control::g_control = nullptr; + if (OK != FixedwingPositionControl::start()) { err(1, "start failed"); } -- 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(-) 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(-) 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 391a5c82cbe6d02136e16528b472352e8461b02a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 19:54:40 +0200 Subject: Print avionics voltage --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daba4e740..409cf9d89 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -335,6 +335,7 @@ void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); -- cgit v1.2.3 From edccc826a1834aa21171e2b5f88c97454955bb16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Jul 2014 21:16:29 +0200 Subject: Improved comment of bridge header --- src/modules/mavlink/mavlink_bridge_header.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index d82c2dae7..0cd08769e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -44,7 +44,12 @@ __BEGIN_DECLS -//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +/* + * We are NOT using convenience functions, + * but instead send messages with a custom function. + * So we do NOT do this: + * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS + */ /* use efficient approach, see mavlink_helpers.h */ #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes -- 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(+) 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 9d7e3feffcfb34708cda5a726b7900101f41b205 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 11:18:59 +0200 Subject: airspeed: Filter slightly more, just a touch to not induce phase delay --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 9cfa7f383..159706278 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 1.5f +#define MEAS_DRIVER_FILTER_FREQ 1.2f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed -- 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(-) 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 39699670026f463df79a45c0dbf5cf7ceea8d5a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 31 Jul 2014 12:16:11 +0200 Subject: Default to TECS --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 7cfe63fbc..58a1e9f6b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 1); +PARAM_DEFINE_INT32(MT_ENABLED, 0); /** * Total Energy Rate Control Feedforward -- cgit v1.2.3 From 92a4757971701bc1536db85f62590f9e864d96fe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 31 Jul 2014 12:45:44 +0200 Subject: sdlog2_dump.py: minor fix allowing handling FMT messages in the middle of stream --- Tools/sdlog2/sdlog2_dump.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py index 5b1e55e22..ca2efb021 100644 --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -149,7 +149,8 @@ class SDLog2Parser: break if first_data_msg: # build CSV columns and init data map - self.__initCSV() + if not self.__debug_out: + self.__initCSV() first_data_msg = False self.__parseMsg(msg_descr) bytes_read += self.__ptr -- cgit v1.2.3 From fc2e0fad4731ef543be4c3da73de6b670d40d804 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 31 Jul 2014 15:23:30 +0200 Subject: TECS: Added separate gain / time constant for the throttle loop to allow independent tuning --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6386e37a0..16c7e5ffa 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { // Calculate gain scaler from specific energy error to throttle - float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 5cafb1c79..10c9e9344 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -123,6 +123,10 @@ public: _timeConst = time_const; } + void set_time_const_throt(float time_const_throt) { + _timeConstThrot = time_const_throt; + } + void set_min_sink_rate(float rate) { _minSinkRate = rate; } @@ -204,6 +208,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; -- 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(-) 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 From b8c14417ce5c667920b32e3ba94ca8f496e0d10c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 31 Jul 2014 23:34:34 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index d84fc8a84..6980ee824 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit d84fc8a84678d93f97f93b240c81472797ca5889 +Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 -- cgit v1.2.3 From 87273abc9aa0006baff53741e605c9992ec8c955 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 00:56:44 +0400 Subject: UAVCAN node version info --- src/modules/uavcan/uavcan_main.cpp | 61 +++++++++++++++++++++++++++++--------- src/modules/uavcan/uavcan_main.hpp | 1 + 2 files changed, 48 insertions(+), 14 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 27e77e9c5..ffd599070 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include @@ -173,6 +175,44 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return OK; } +void UavcanNode::fill_node_info() +{ + /* software version */ + uavcan::protocol::SoftwareVersion swver; + + // Extracting the last 8 hex digits of FW_GIT and converting them to int + const unsigned fw_git_len = std::strlen(FW_GIT); + if (fw_git_len >= 8) { + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + } + + warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + + _node.setSoftwareVersion(swver); + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + + _node.setHardwareVersion(hwver); +} + int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; @@ -183,6 +223,13 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + _node.setName("org.pixhawk.pixhawk"); + + _node.setNodeID(node_id); + + fill_node_info(); + + /* initializing the bridges UAVCAN <--> uORB */ ret = _esc_controller.init(); if (ret < 0) return ret; @@ -191,20 +238,6 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; - uavcan::protocol::SoftwareVersion swver; - swver.major = 12; // TODO fill version info - swver.minor = 34; - _node.setSoftwareVersion(swver); - - uavcan::protocol::HardwareVersion hwver; - hwver.major = 42; // TODO fill version info - hwver.minor = 42; - _node.setHardwareVersion(hwver); - - _node.setName("org.pixhawk"); // Huh? - - _node.setNodeID(node_id); - return _node.start(); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 443525379..05b66fd7b 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: static UavcanNode* instance() { return _instance; } private: + void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); int run(); -- cgit v1.2.3 From b606674fc77aa772fba66ad09dd3879a62a7f44c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 Aug 2014 01:04:24 +0400 Subject: Removed misleading comment from the UAVCAN module makefile --- src/modules/uavcan/module.mk | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1ef6f0cfa..3865f2468 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # # Invoke DSDL compiler -# TODO: Add make target for this, or invoke dsdlc manually. -# The second option assumes that the generated headers shall be saved -# under the version control, which may be undesirable. -# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated -- cgit v1.2.3 From 2d4dd0d5c03a7ef3d696f40b6a6988e08e991034 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Jul 2014 20:37:26 +0900 Subject: Tone alarm sound for barometer check fail --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 19f792d19..307f7dbc7 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -150,6 +150,7 @@ enum { TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, TONE_EKF_WARNING_TUNE, + TONE_BARO_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 03c7bd399..8f523b390 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -337,6 +337,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Fri, 1 Aug 2014 13:04:28 +0200 Subject: Move the last throttle demand setting to a better position --- src/lib/external_lgpl/tecs/tecs.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 16c7e5ffa..579fb5d42 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM _throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); - _last_throttle_dem = _throttle_dem; } + // Ensure _last_throttle_dem is always initialized properly + // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! + _last_throttle_dem = _throttle_dem; + // Calculate integrator state upper and lower limits // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand -- cgit v1.2.3 From 3b1c92e42f57277a12d40963c22fbea9820ee0ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:10:07 +0200 Subject: Make throttle time constant tunable separately, group TECS params correctly, make climbout alt configurable --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 ++++- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 49 ++++++++++++++++------ 2 files changed, 46 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index eadb63f40..f05f20cd0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -203,9 +203,11 @@ private: float l1_damping; float time_const; + float time_const_throt; float min_sink_rate; float max_sink_rate; float max_climb_rate; + float climbout_diff; float heightrate_p; float speedrate_p; float throttle_damp; @@ -245,9 +247,11 @@ private: param_t l1_damping; param_t time_const; + param_t time_const_throt; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t climbout_diff; param_t heightrate_p; param_t speedrate_p; param_t throttle_damp; @@ -471,9 +475,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF"); _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); @@ -536,6 +542,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); @@ -547,6 +554,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); @@ -564,6 +572,7 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); _tecs.set_time_const(_parameters.time_const); + _tecs.set_time_const_throt(_parameters.time_const_throt); _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); @@ -1095,7 +1104,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 52128e1b7..a0d9309b9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,6 +154,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to zero to disable climbout mode (not recommended). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); + /** * Maximum climb rate * @@ -181,7 +193,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); @@ -194,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); @@ -205,17 +217,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Smaller values make it faster to respond, larger values make it slower * to respond. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); +/** + * TECS Throttle time constant + * + * This is the time constant of the TECS throttle control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); + /** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); @@ -227,7 +250,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); * and height offsets are trimmed out, but reduces damping and * increases overshoot. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); @@ -240,7 +263,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); @@ -253,7 +276,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); @@ -266,7 +289,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); @@ -282,7 +305,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); @@ -300,7 +323,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); @@ -312,21 +335,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); * will work well provided the pitch to servo controller has been tuned * properly. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** * Height rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); /** * Speed rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -- cgit v1.2.3 From 2e4dff3fea2177538f3549cedd8f84fbbbecd4ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:21:30 +0200 Subject: removed debug statement from mTECS --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index afc411a60..749f57a2b 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,7 +76,6 @@ mTecs::mTecs() : _counter(0), _debug(false) { - warnx("starting"); } mTecs::~mTecs() -- cgit v1.2.3 From 32e32d92bdad3eb4e92e9e53c112fcdcf848c7e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:39:24 +0200 Subject: Add throttle slew rate param --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f05f20cd0..189a19d48 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -229,6 +229,7 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; @@ -273,6 +274,7 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; @@ -464,6 +466,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -538,6 +541,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); @@ -576,6 +580,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_throttle_slewrate(_parameters.throttle_slew_max); _tecs.set_integrator_gain(_parameters.integrator_gain); _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index a0d9309b9..41c374407 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +/** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + /** * Negative pitch limit * -- cgit v1.2.3 From 491c6c29acef343819f32655e6e87334531af8cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:43:56 +0200 Subject: Init values --- src/lib/external_lgpl/tecs/tecs.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 10c9e9344..752b0ddd9 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,26 @@ class __EXPORT TECS { public: TECS() : + _update_50hz_last_usec(0), + _update_speed_last_usec(0), + _update_pitch_throttle_last_usec(0), + // TECS tuning parameters + _hgtCompFiltOmega(0.0f), + _spdCompFiltOmega(0.0f), + _maxClimbRate(2.0f), + _minSinkRate(1.0f), + _maxSinkRate(2.0f), + _timeConst(5.0f), + _timeConstThrot(8.0f), + _ptchDamp(0.0f), + _thrDamp(0.0f), + _integGain(0.0f), + _vertAccLim(0.0f), + _rollComp(0.0f), + _spdWeight(0.5f), + _heightrate_p(0.0f), + _speedrate_p(0.0f), + _throttle_dem(0.0f), _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), -- cgit v1.2.3 From c3666a5504fc89b7e1861e25c9b6cb0c07453784 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 23:18:43 +0200 Subject: Widen default first WP radius --- src/modules/navigator/mission_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 881caa24e..d15e1e771 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * waypoint is more distant than MIS_DIS_1WP from the current position. * * @min 0 - * @max 250 + * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); -- cgit v1.2.3 From 4d03202c4c8c322f9d93da0051dca015ab473cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 23:42:26 +0200 Subject: Warn on far waypoints --- src/modules/navigator/mission.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 12f6b9c21..c0e37a3ed 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -273,6 +273,10 @@ Mission::check_dist_1wp() if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; + if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + } return true; } else { -- cgit v1.2.3 From 9725a1635254ba6c0c1df0413f94398ec28e23f9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 2 Aug 2014 03:34:57 +0400 Subject: UAVCAN: Fixed short git hash computation --- src/modules/uavcan/uavcan_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ffd599070..9d5404baf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -180,16 +180,13 @@ void UavcanNode::fill_node_info() /* software version */ uavcan::protocol::SoftwareVersion swver; - // Extracting the last 8 hex digits of FW_GIT and converting them to int - const unsigned fw_git_len = std::strlen(FW_GIT); - if (fw_git_len >= 8) { - char fw_git_short[9] = {}; - std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8); - assert(fw_git_short[8] == '\0'); - char *end = nullptr; - swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; - } + // Extracting the first 8 hex digits of FW_GIT and converting them to int + char fw_git_short[9] = {}; + std::memmove(fw_git_short, FW_GIT, 8); + assert(fw_git_short[8] == '\0'); + char *end = nullptr; + swver.vcs_commit = std::strtol(fw_git_short, &end, 16); + swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); -- cgit v1.2.3 From 5824607c76ed38b5c08d1c9031814f741bce7c8b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Aug 2014 13:01:42 +0200 Subject: uavcan: fix actuator groups subcriptions and poll() --- src/modules/uavcan/uavcan_main.cpp | 36 +++++++++++++++++++----------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9d5404baf..5b1539e3e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -253,7 +253,6 @@ int UavcanNode::run() // XXX figure out the output count _output_count = 2; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); actuator_outputs_s outputs; @@ -273,21 +272,23 @@ int UavcanNode::run() _node.setStatusOk(); - while (!_task_should_exit) { + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds_num = 0; + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -301,7 +302,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { @@ -317,7 +318,7 @@ int UavcanNode::run() // XXX trigger failsafe } - //can we mix? + // can we mix? if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -417,7 +418,8 @@ UavcanNode::subscribe() // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; + // the first fd used by CAN + _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -523,8 +525,8 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); } /* -- cgit v1.2.3 From 020ac409be62e6c459e7aa5cb3181941c9bbfd23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:17:18 +0200 Subject: Remove airspeed and altitude raw data from TECS log, as we have these quantities already in the system log --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 ++---- src/modules/sdlog2/sdlog2.c | 6 ++---- src/modules/sdlog2/sdlog2_messages.h | 4 +--- src/modules/uORB/topics/tecs_status.h | 12 ++++++------ 4 files changed, 11 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 749f57a2b..bffeefc1f 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti /* Write part of the status message */ _status.altitudeSp = altitudeSp; - _status.altitude = altitude; - _status.altitudeFiltered = altitudeFiltered; + _status.altitude_filtered = altitudeFiltered; /* use flightpath angle setpoint for total energy control */ @@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* Write part of the status message */ _status.airspeedSp = airspeedSp; - _status.airspeed = airspeed; - _status.airspeedFiltered = airspeedFiltered; + _status.airspeed_filtered = airspeedFiltered; /* use longitudinal acceleration setpoint for total energy control */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 31861c3fc..6c4b49452 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; - log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; - log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; - log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; - log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 853a3811f..fb7609435 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -333,13 +333,11 @@ struct log_GS1B_s { #define LOG_TECS_MSG 30 struct log_TECS_s { float altitudeSp; - float altitude; float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; @@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 33055018c..ddca19d61 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -51,11 +51,13 @@ */ typedef enum { - TECS_MODE_NORMAL, + TECS_MODE_NORMAL = 0, TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF, TECS_MODE_LAND, - TECS_MODE_LAND_THROTTLELIM + TECS_MODE_LAND_THROTTLELIM, + TECS_MODE_BAD_DESCENT, + TECS_MODE_CLIMBOUT } tecs_mode; /** @@ -65,14 +67,12 @@ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ float altitudeSp; - float altitude; - float altitudeFiltered; + float altitude_filtered; float flightPathAngleSp; float flightPathAngle; float flightPathAngleFiltered; float airspeedSp; - float airspeed; - float airspeedFiltered; + float airspeed_filtered; float airspeedDerivativeSp; float airspeedDerivative; -- cgit v1.2.3 From 43ef622725b1114beccb722bad0538c4c0cc3175 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:18:22 +0200 Subject: Reinstate TECS logging --- src/lib/external_lgpl/tecs/tecs.cpp | 40 +++++++++++++++++++----------- src/lib/external_lgpl/tecs/tecs.h | 49 +++++++++++++++++++++++-------------- 2 files changed, 57 insertions(+), 32 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 579fb5d42..a57a0481a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate pitch demand _update_pitch(); -// // Write internal variables to the log_tuning structure. This -// // structure will be logged in dataflash at 10Hz - // log_tuning.hgt_dem = _hgt_dem_adj; - // log_tuning.hgt = _integ3_state; - // log_tuning.dhgt_dem = _hgt_rate_dem; - // log_tuning.dhgt = _integ2_state; - // log_tuning.spd_dem = _TAS_dem_adj; - // log_tuning.spd = _integ5_state; - // log_tuning.dspd = _vel_dot; - // log_tuning.ithr = _integ6_state; - // log_tuning.iptch = _integ7_state; - // log_tuning.thr = _throttle_dem; - // log_tuning.ptch = _pitch_dem; - // log_tuning.dspd_dem = _TAS_rate_dem; + _tecs_state.timestamp = now; + + if (_underspeed) { + _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; + } else if (_badDescent) { + _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_climbOutDem) { + _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; + } else { + // If no error flag applies, conclude normal + _tecs_state.mode = ECL_TECS_MODE_NORMAL; + } + + _tecs_state.hgt_dem = _hgt_dem_adj; + _tecs_state.hgt = _integ3_state; + _tecs_state.dhgt_dem = _hgt_rate_dem; + _tecs_state.dhgt = _integ2_state; + _tecs_state.spd_dem = _TAS_dem_adj; + _tecs_state.spd = _integ5_state; + _tecs_state.dspd = _vel_dot; + _tecs_state.ithr = _integ6_state; + _tecs_state.iptch = _integ7_state; + _tecs_state.thr = _throttle_dem; + _tecs_state.ptch = _pitch_dem; + _tecs_state.dspd_dem = _TAS_rate_dem; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 752b0ddd9..bcc2d90e5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,7 @@ class __EXPORT TECS { public: TECS() : + _tecs_state {}, _update_50hz_last_usec(0), _update_speed_last_usec(0), _update_pitch_throttle_last_usec(0), @@ -120,24 +121,33 @@ public: return _spdWeight; } - // log data on internal state of the controller. Called at 10Hz - // void log_data(DataFlash_Class &dataflash, uint8_t msgid); - - // struct PACKED log_TECS_Tuning { - // LOG_PACKET_HEADER; - // float hgt; - // float dhgt; - // float hgt_dem; - // float dhgt_dem; - // float spd_dem; - // float spd; - // float dspd; - // float ithr; - // float iptch; - // float thr; - // float ptch; - // float dspd_dem; - // } log_tuning; + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + struct tecs_state { + uint64_t timestamp; + float hgt; + float dhgt; + float hgt_dem; + float dhgt_dem; + float spd_dem; + float spd; + float dspd; + float ithr; + float iptch; + float thr; + float ptch; + float dspd_dem; + enum ECL_TECS_MODE mode; + }; + + void get_tecs_state(struct tecs_state& state) { + state = _tecs_state; + } void set_time_const(float time_const) { _timeConst = time_const; @@ -212,6 +222,9 @@ public: } private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; -- cgit v1.2.3 From ff760d07b4d115ce0bc58ca8e8f98a915c4bbb67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Aug 2014 13:19:10 +0200 Subject: Add TECS logging --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 47 ++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 189a19d48..350ce6dec 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -146,6 +146,7 @@ private: int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _tecs_status_pub(-1), _nav_capabilities_pub(-1), /* states */ @@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); + + struct TECS::tecs_state s; + _tecs.get_tecs_state(s); + + struct tecs_status_s t; + + t.timestamp = s.timestamp; + + switch (s.mode) { + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = TECS_MODE_NORMAL; + break; + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = TECS_MODE_UNDERSPEED; + break; + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = TECS_MODE_BAD_DESCENT; + break; + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = TECS_MODE_CLIMBOUT; + break; + } + + t.altitudeSp = s.hgt_dem; + t.altitude_filtered = s.hgt; + t.airspeedSp = s.spd_dem; + t.airspeed_filtered = s.spd; + + t.flightPathAngleSp = s.dhgt_dem; + t.flightPathAngle = s.dhgt; + t.flightPathAngleFiltered = s.dhgt; + + t.airspeedDerivativeSp = s.dspd_dem; + t.airspeedDerivative = s.dspd; + + t.totalEnergyRateSp = s.thr; + t.totalEnergyRate = s.ithr; + t.energyDistributionRateSp = s.ptch; + t.energyDistributionRate = s.iptch; + + if (_tecs_status_pub > 0) { + orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { + _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); + } } } -- cgit v1.2.3 From 6fbeaeaacecfa532c1c4437ba51312e1c46d980d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:40:38 -0700 Subject: Update arming_state_transition calls Add new fRunPreArmChecks flag --- src/modules/commander/commander.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2b528eef6..4f976546e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -1085,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1288,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1309,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1368,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -2156,7 +2156,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } -- cgit v1.2.3 From 014fd5f47baad2f49292216eba851100aa543fef Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:41:29 -0700 Subject: Add fRunPreArmChecks flag This is to allow unit tests to be written which do not perform pre-arm checks --- src/modules/commander/state_machine_helper.cpp | 13 +++++++------ src/modules/commander/state_machine_helper.h | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3c3d2f233..f8589d24b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = { }; transition_result_t -arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(ARMING_STATE_INIT == 0); @@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current int prearm_ret = OK; /* only perform the check if we have to */ - if (new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..69ce8bbce 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -- cgit v1.2.3 From cfe5c45e04f65f24110f222c3de02147146081e1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 4 Aug 2014 09:42:16 -0700 Subject: Fix broken arming unit test Also, update arming_state_transition calls to add new fRunPreArmChecks flag. --- .../commander/commander_tests/state_machine_helper_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 2e18c4284..08dda2fab 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) MTT_ALL_NOT_VALID, MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, - { "transition: MANUAL to AUTO_MISSION - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, - { "transition: AUTO_MISSION to MANUAL - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", -- cgit v1.2.3 From 33762ce861b06cfa521d75fc18df3c7237e3a423 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 6 Aug 2014 12:40:07 +0400 Subject: UAVCAN ESC mixer: removed the failsafe placeholder, it's no use here --- src/modules/uavcan/uavcan_main.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b1539e3e..4535b6d6a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -313,11 +313,6 @@ int UavcanNode::run() } } - if (!controls_updated) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } - // can we mix? if (controls_updated && (_mixers != nullptr)) { -- cgit v1.2.3 From 13b9cd0cec2d10d6252d0e8015d000f4fcc1517f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Aug 2014 07:25:20 +0200 Subject: Do not fuse height after innovation consistency check fail --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index ffdd29a5b..c619735c6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1070,10 +1070,18 @@ void AttPosEKF::FuseVelposNED() // apply a 10-sigma threshold current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode) { current_ekf_state.hgtHealth = true; current_ekf_state.hgtFailTime = millis(); + + // if we just reset from a timeout, do not fuse + // the height data, but reset height and stored states + if (current_ekf_state.hgtTimeout) { + ResetHeight(); + ResetStoredStates(); + fuseHgtData = false; + } } else { -- cgit v1.2.3 From 20c3a329c72ca5950f7bb037473a812b4dd74b1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Aug 2014 07:36:59 +0200 Subject: Introduce similar checks fo all other health checks in the filter --- .../ekf_att_pos_estimator/estimator_23states.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c619735c6..c7c7305b2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED() // apply a 5-sigma threshold current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { + if (current_ekf_state.velHealth || staticMode) { current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); + } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { + // XXX check + current_ekf_state.velHealth = true; + ResetVelocity(); + ResetStoredStates(); + // do not fuse bad data + fuseVelData = false; } else { @@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED() { current_ekf_state.posHealth = true; current_ekf_state.posFailTime = millis(); + + if (current_ekf_state.posTimeout) { + ResetPosition(); + + // XXX cross-check the state reset + ResetStoredStates(); + + // do not fuse position data on this time + // step + fusePosData = false; + } } else { -- cgit v1.2.3 From faaeaeb11333598abd17db2189a2bc47216d0a98 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Aug 2014 00:22:57 +0200 Subject: mc_pos_control: manual and offboard control reorganization and cleanup --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 271 +++++++++++---------- .../uORB/topics/position_setpoint_triplet.h | 8 +- 2 files changed, 140 insertions(+), 139 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d22f43b5a..a0837a2dd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -105,6 +105,7 @@ public: int start(); private: + const float alt_ctl_dz = 0.2f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ @@ -184,6 +185,8 @@ private: math::Vector<3> _vel; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -216,6 +219,16 @@ private: */ void reset_alt_sp(); + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + /** * Select between barometric and global (AMSL) altitudes */ @@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -515,6 +530,120 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode.flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual.x; + _sp_move_rate(1) = _manual.y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet.current.x; + _pos_sp(1) = _pos_sp_triplet.current.y; + _pos_sp(2) = _pos_sp_triplet.current.z; + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet.current.vx; + _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; + } + + if (_control_mode.flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet.current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -553,13 +682,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - - math::Vector<3> sp_move_rate; - sp_move_rate.zero(); - - float yaw_sp_move_rate; - math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; @@ -618,138 +740,17 @@ MulticopterPositionControl::task_main() _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; - sp_move_rate.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); - } - - if (_control_mode.flag_control_position_enabled) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.x; - sp_move_rate(1) = _manual.y; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } + control_manual(dt); } else if (_control_mode.flag_control_offboard_enabled) { - /* Offboard control */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - if (_pos_sp_triplet.current.valid) { - - if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { - - _pos_sp(0) = _pos_sp_triplet.current.x; - _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _pos_sp_triplet.current.vx; - sp_move_rate(1) = _pos_sp_triplet.current.vy; - yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed; - _att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt; - } - - if (_control_mode.flag_control_altitude_enabled) { - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);; - } - - /* limit setpoint move rate */ - float sp_move_norm = sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - sp_move_rate /= sp_move_norm; - } - - /* scale to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); - - /* move position setpoint */ - _pos_sp += sp_move_rate * dt; - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); - } - - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); - } - - float pos_sp_offs_norm = pos_sp_offs.length(); - - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); - } - - } else { - reset_pos_sp(); - reset_alt_sp(); - } + /* offboard control */ + control_offboard(dt); } else { /* AUTO */ @@ -823,7 +824,7 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; @@ -903,7 +904,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a1932180..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -60,7 +60,7 @@ enum SETPOINT_TYPE SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */ + SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ }; struct position_setpoint_s @@ -71,9 +71,9 @@ struct position_setpoint_s float y; /**< local position setpoint in m in NED */ float z; /**< local position setpoint in m in NED */ bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m in NED */ - float vy; /**< local velocity setpoint in m in NED */ - float vz; /**< local velocity setpoint in m in NED */ + float vx; /**< local velocity setpoint in m/s in NED */ + float vy; /**< local velocity setpoint in m/s in NED */ + float vz; /**< local velocity setpoint in m/s in NED */ bool velocity_valid; /**< true if local velocity setpoint valid */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ -- cgit v1.2.3 From 03f839a27ac61178be8ef77526faa13a46c4cd6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Aug 2014 00:24:18 +0200 Subject: mc_pos_control: more accurate position setpoint reset, keep attitude setpoint continuous --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a0837a2dd..0106af80a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -514,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp() { if (_reset_pos_sp) { _reset_pos_sp = false; - _pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0)); - _pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1)); + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -525,7 +528,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2)); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } -- cgit v1.2.3 From 5246d6b49211b7ba73a32c7b73967cabe5fd512a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Aug 2014 19:31:34 +0200 Subject: Updated mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 04b1ad5b2..96425caae 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9 +Subproject commit 96425caae3de0e8cda81405c1a0e9d2fb14369a2 -- cgit v1.2.3 From 312f9e0ccce9e3f530f6fc222e371a48c3b1af0d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 9 Aug 2014 19:43:57 +0200 Subject: mixers: adapted FX79 mixer to have more pitch influence --- ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix index 0a1dca98d..112d9b891 100755 --- a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix @@ -27,12 +27,12 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 5000 8000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 S: 0 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 8000 5000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 S: 0 1 -8000 -8000 0 -10000 10000 Output 2 -- cgit v1.2.3 From 3fdf63d2b3cf25904371cf700e08ce4e8d214824 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:00:37 +0200 Subject: Startup script: added viper script Conflicts: mavlink/include/mavlink/v1.0 --- ROMFS/px4fmu_common/init.d/3035_viper | 10 ++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++++ 2 files changed, 15 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/3035_viper diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper new file mode 100644 index 000000000..9c8f8a585 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -0,0 +1,10 @@ +#!nsh +# +# Viper +# +# Simon Wilks +# + +sh /etc/init.d/rc.fw_defaults + +set MIXER FMU_FX79 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 17541e680..9de7d9ecd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -103,6 +103,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3035 35 +then + sh /etc/init.d/3035_viper +fi + if param compare SYS_AUTOSTART 3100 then sh /etc/init.d/3100_tbs_caipirinha -- cgit v1.2.3 From e6210058a3527bc87a74781159631a1f59ad053d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 9 Aug 2014 19:52:30 +0200 Subject: mixers: added Viper mixer --- ROMFS/px4fmu_common/init.d/3035_viper | 2 +- ROMFS/px4fmu_common/mixers/Viper.mix | 72 +++++++++++++++++++++++++++++++++++ 2 files changed, 73 insertions(+), 1 deletion(-) create mode 100755 ROMFS/px4fmu_common/mixers/Viper.mix diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 9c8f8a585..a4c1e832d 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -7,4 +7,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_FX79 \ No newline at end of file +set MIXER Viper diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix new file mode 100755 index 000000000..79c4447be --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -0,0 +1,72 @@ +Viper Delta-wing mixer +================================= + +Designed for Viper. + +TODO (sjwilks): Add mixers for flaps. + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 7500 7500 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 -- cgit v1.2.3 From 4b065a7e1d2a523ba40abbf63452bf89d1004cd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 00:52:03 +0200 Subject: Updated MAVLink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 96425caae..8cc21472c 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 96425caae3de0e8cda81405c1a0e9d2fb14369a2 +Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41 -- cgit v1.2.3 From 5225d87854bdea1b5e4367e3db6b41ece9e46e13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 01:24:04 +0200 Subject: Updated MAVLink to latest revision --- src/modules/mavlink/mavlink_messages.cpp | 151 ++++++++++--------------------- src/modules/mavlink/mavlink_receiver.cpp | 34 ------- 2 files changed, 49 insertions(+), 136 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 683f0f1e8..7f67475f2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1385,43 +1385,43 @@ protected: }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } static const char *get_name_static() { - return "GLOBAL_POSITION_SETPOINT_INT"; + return "POSITION_TARGET_GLOBAL_INT"; } uint8_t get_id() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamGlobalPositionSetpointInt(mavlink); + return new MavlinkStreamPositionTargetGlobalInt(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: MavlinkOrbSubscription *_pos_sp_triplet_sub; /* do not allow top copying this class */ - MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); - MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: - explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} @@ -1430,15 +1430,14 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_global_position_setpoint_int_t msg; + mavlink_position_target_global_int_t msg; msg.coordinate_frame = MAV_FRAME_GLOBAL; - msg.latitude = pos_sp_triplet.current.lat * 1e7; - msg.longitude = pos_sp_triplet.current.lon * 1e7; - msg.altitude = pos_sp_triplet.current.alt * 1000; - msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f; + msg.lat_int = pos_sp_triplet.current.lat * 1e7; + msg.lon_int = pos_sp_triplet.current.lon * 1e7; + msg.alt = pos_sp_triplet.current.alt; - _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg); } } }; @@ -1454,12 +1453,12 @@ public: static const char *get_name_static() { - return "LOCAL_POSITION_SETPOINT"; + return "POSITION_TARGET_LOCAL_NED"; } uint8_t get_id() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } static MavlinkStream *new_instance(Mavlink *mavlink) @@ -1469,7 +1468,7 @@ public: unsigned get_size() { - return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: @@ -1491,137 +1490,86 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_local_position_setpoint_t msg; + mavlink_position_target_local_ned_t msg; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; msg.x = pos_sp.x; msg.y = pos_sp.y; msg.z = pos_sp.z; - msg.yaw = pos_sp.yaw; - _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg); } } }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +class MavlinkStreamAttitudeTarget : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + return MavlinkStreamAttitudeTarget::get_name_static(); } static const char *get_name_static() { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + return "ATTITUDE_TARGET"; } uint8_t get_id() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return MAVLINK_MSG_ID_ATTITUDE_TARGET; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink); + return new MavlinkStreamAttitudeTarget(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *att_sp_sub; - uint64_t att_sp_time; + MavlinkOrbSubscription *_att_sp_sub; + MavlinkOrbSubscription *_att_rates_sp_sub; + uint64_t _att_sp_time; + uint64_t _att_rates_sp_time; /* do not allow top copying this class */ - MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); - MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: - explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), - att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), - att_sp_time(0) + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), + _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), + _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), + _att_sp_time(0), + _att_rates_sp_time(0) {} void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(&att_sp_time, &att_sp)) { - mavlink_roll_pitch_yaw_thrust_setpoint_t msg; - - msg.time_boot_ms = att_sp.timestamp / 1000; - msg.roll = att_sp.roll_body; - msg.pitch = att_sp.pitch_body; - msg.yaw = att_sp.yaw_body; - msg.thrust = att_sp.thrust; - - _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg); - } - } -}; - + if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { -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"; - } + struct vehicle_rates_setpoint_s att_rates_sp; + (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - uint8_t get_id() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; - } + mavlink_attitude_target_t msg; - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink); - } - - unsigned get_size() - { - return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -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(Mavlink *mavlink) : MavlinkStream(mavlink), - _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))), - _att_rates_sp_time(0) - {} - - void send(const hrt_abstime t) - { - struct vehicle_rates_setpoint_s att_rates_sp; + msg.time_boot_ms = att_sp.timestamp / 1000; + mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); - if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) { - mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg; + msg.body_roll_rate = att_rates_sp.roll; + msg.body_pitch_rate = att_rates_sp.pitch; + msg.body_yaw_rate = att_rates_sp.yaw; - msg.time_boot_ms = att_rates_sp.timestamp / 1000; - msg.roll_rate = att_rates_sp.roll; - msg.pitch_rate = att_rates_sp.pitch; - msg.yaw_rate = att_rates_sp.yaw; - msg.thrust = att_rates_sp.thrust; + msg.thrust = att_sp.thrust; - _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg); } } }; @@ -2149,10 +2097,9 @@ StreamListItem *streams_list[] = { 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(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::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(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::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), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 69e3ef31d..63bac45c0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); - break; - case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -362,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } } -void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) -{ - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); - } - } -} - void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { -- cgit v1.2.3 From 9ecec7fada7da3778c6214defcef68ea05352027 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 01:30:25 +0200 Subject: Add default initializers and timestamp in local position --- src/modules/mavlink/mavlink_messages.cpp | 7 ++++--- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + src/modules/uORB/topics/vehicle_local_position_setpoint.h | 3 ++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7f67475f2..c10be77b5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1430,7 +1430,7 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg; + mavlink_position_target_global_int_t msg{}; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; @@ -1490,8 +1490,9 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg; + mavlink_position_target_local_ned_t msg{}; + msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; msg.x = pos_sp.x; msg.y = pos_sp.y; @@ -1558,7 +1559,7 @@ protected: struct vehicle_rates_setpoint_s att_rates_sp; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg; + mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0106af80a..ecc40428d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -788,6 +788,7 @@ MulticopterPositionControl::task_main() } /* fill local position setpoint */ + _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index 8988a0330..6766bb58a 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -50,11 +50,12 @@ */ struct vehicle_local_position_setpoint_s { + uint64_t timestamp; /**< timestamp of the setpoint */ float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame to go to */ +}; /**< Local position in NED frame */ /** * @} -- cgit v1.2.3 From 1d3edfa62976bdf73eeef67309c67b154be07810 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 10 Aug 2014 12:02:10 -0700 Subject: More FTP support Added: - sequence numbers - directory list returns file sizes - open command returns file size in data --- src/modules/mavlink/mavlink_ftp.cpp | 48 +++++++++++++++++++++++++++++++------ src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a2c900af..17d1babff 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -190,6 +191,8 @@ void MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); + + hdr->magic = kProtocolMagic; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; @@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req) break; } - // name too big to fit? - if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { - break; - } + uint32_t fileSize = 0; + char buf[256]; // store the type marker switch (entry.d_type) { case DTYPE_FILE: hdr->data[offset++] = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } break; case DTYPE_DIRECTORY: hdr->data[offset++] = kDirentDir; @@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req) hdr->data[offset++] = kDirentUnknown; break; } + + if (entry.d_type == DTYPE_FILE) { + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + // name too big to fit? + if ((nameLen + offset + 2) > kMaxDataLength) { + break; + } + // copy the name, which we know will fit - strcpy((char *)&hdr->data[offset], entry.d_name); + strcpy((char *)&hdr->data[offset], buf); //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); - offset += strlen(entry.d_name) + 1; + offset += nameLen + 1; } closedir(dp); @@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create) return kErrNoSession; } + + uint32_t fileSize = 0; + if (!create) { + struct stat st; + if (stat(req->dataAsCString(), &st) != 0) { + return kErrNotFile; + } + fileSize = st.st_size; + } + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; int fd = ::open(req->dataAsCString(), oflag); @@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create) _session_fds[session_index] = fd; hdr->session = session_index; - hdr->size = 0; + if (create) { + hdr->size = 0; + } else { + hdr->size = sizeof(uint32_t); + *((uint32_t*)hdr->data) = fileSize; + } return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1bd1158fb..800c98b69 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -146,7 +146,7 @@ private: mavlink_message_t msg; msg.checksum = 0; unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence(), rawData()); + _mavlink->get_channel(), &msg, sequence()+1, rawData()); _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); -- cgit v1.2.3 From 0633d7f601b38a60c2b0ed1ca98d57bd3f80e482 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 22:05:21 +0200 Subject: Updated mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 8cc21472c..4cd384001 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41 +Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742 -- cgit v1.2.3