diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-04-18 11:31:16 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-04-18 11:31:16 +0200 |
commit | 0d403820e0ecbcf1d74c32641a654340ab279535 (patch) | |
tree | e128da95e10fc883cdde7a6b94dc8419162a14d0 /src | |
parent | 8fcdff15a480399541b9ecccf8dc8260d437ec7f (diff) | |
parent | cc5fddd34e0db7b3e0402d4fdd8c400f9a1d39bc (diff) | |
download | px4-firmware-0d403820e0ecbcf1d74c32641a654340ab279535.tar.gz px4-firmware-0d403820e0ecbcf1d74c32641a654340ab279535.tar.bz2 px4-firmware-0d403820e0ecbcf1d74c32641a654340ab279535.zip |
Merge branch 'mtecs' into mtecs_takeoff
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/gps/gps.cpp | 4 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 9 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 18 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_commands.cpp | 73 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_commands.h | 65 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 226 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 48 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 79 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 69 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_stream.h | 2 | ||||
-rw-r--r-- | src/modules/mavlink/module.mk | 5 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 30 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 12 |
16 files changed, 559 insertions, 87 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c72f692d7..a902bdf2f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -282,8 +282,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; + _report.eph_m = 3.0f; + _report.epv_m = 7.0f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3720b5a3b..ce6de88ef 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ @@ -587,11 +592,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ answer_command(*cmd, result); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f4..2f84dc963 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -660,18 +660,24 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp new file mode 100644 index 000000000..1c1e097a4 --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 <anton.babushkin@me.com> + */ + +#include "mavlink_commands.h" + +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +{ + _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); +} + +MavlinkCommandsStream::~MavlinkCommandsStream() +{ +} + +void +MavlinkCommandsStream::update(const hrt_abstime t) +{ + if (_cmd_sub->update(t)) { + /* 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 new file mode 100644 index 000000000..6255d65df --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 <anton.babushkin@me.com> + */ + +#ifndef MAVLINK_COMMANDS_H_ +#define MAVLINK_COMMANDS_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_command.h> + +class Mavlink; +class MavlinkCommansStream; + +#include "mavlink_main.h" + +class MavlinkCommandsStream +{ +private: + MavlinkOrbSubscription *_cmd_sub; + struct vehicle_command_s *_cmd; + mavlink_channel_t _channel; + +public: + MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); + ~MavlinkCommandsStream(); + 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 18df577fe..f4a4ce86c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,6 +81,7 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +#include "mavlink_commands.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -166,12 +167,12 @@ 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) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -185,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } } - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (instance->should_transmit()) { + ssize_t ret = write(uart, ch, desired); + if (ret != desired) { + // XXX do something here, but change to using FIONWRITE and OS buf size for detection + } } + + } static void usage(void); @@ -203,14 +209,21 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _message_buffer({}), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -261,7 +274,6 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } - } Mavlink::~Mavlink() @@ -394,6 +406,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } +void +Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +{ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (inst != self) { + inst->pass_message(msg); + } + } +} + int Mavlink::get_uart_fd(unsigned index) { @@ -1617,6 +1641,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } int +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); + return (_message_buffer.data == 0) ? ERROR : OK; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + + + +int Mavlink::task_main(int argc, char *argv[]) { int ch; @@ -1632,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1672,10 +1815,22 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + case 'v': _verbose = true; break; + case 'w': + _wait_to_transmit = true; + break; + default: err_flag = true; break; @@ -1740,6 +1895,17 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on) { + /* initialize message buffer if multiplexing is on */ + if (OK != message_buffer_init(500)) { + errx(1, "can't allocate message buffer, exiting"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1766,6 +1932,8 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + MavlinkCommandsStream commands_stream(this, _channel); + /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; @@ -1783,6 +1951,8 @@ Mavlink::task_main(int argc, char *argv[]) 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); break; case MAVLINK_MODE_CAMERA: @@ -1826,6 +1996,9 @@ 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)) { @@ -1884,6 +2057,37 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* pass messages from other UARTs */ + if (_passing_on) { + + bool is_part; + void *read_ptr; + + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + /* write first part of buffer */ + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + + /* write second part of buffer if there is some */ + if (is_part) { + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + } + } + } + + + perf_end(_loop_perf); } @@ -1928,6 +2132,10 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); + if (_passing_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -2067,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..66d82b471 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,6 +138,8 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd(); @@ -153,10 +155,12 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_forwarding_on() { return _forwarding_on; } + /** * Handle waypoint related messages. */ @@ -196,6 +200,16 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ + int get_mavlink_fd() { return _mavlink_fd; } + + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + protected: Mavlink *next; @@ -210,6 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -234,6 +250,8 @@ private: mavlink_wpm_storage *_wpm; bool _verbose; + bool _forwarding_on; + bool _passing_on; int _uart_fd; int _baudrate; int _datarate; @@ -252,6 +270,18 @@ private: bool _flow_control_enabled; + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + + + /** * Send one parameter. * @@ -315,6 +345,22 @@ private: int configure_stream(const char *stream_name, const float rate); void configure_stream_threadsafe(const char *stream_name, const float rate); + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + bool message_buffer_write(void *ptr, int size); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); + 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 4ca3840d4..648228e3b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,22 +262,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - 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 * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + 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 * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; @@ -641,6 +640,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + 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: @@ -1253,8 +1293,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) @@ -1265,11 +1303,11 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, 42, 30, 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, 42, 30, 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); } } }; @@ -1300,5 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670..b4fe65fd2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; } } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void @@ -222,12 +226,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -253,7 +255,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +289,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +314,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +375,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +403,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -428,7 +430,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.chan[2].scaled = man.r / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f; - if (_rc_pub == 0) { + if (_rc_pub < 0) { _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { @@ -450,7 +452,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; - if (_manual_pub == 0) { + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { @@ -619,11 +621,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +640,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +696,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +754,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -775,11 +777,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } @@ -816,11 +818,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +859,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } @@ -913,6 +915,11 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with parameter component */ _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); + + if (_mavlink->get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(&msg, _mavlink); + } } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7..8ccb2a035 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -139,6 +138,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce0..def40d9ad 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_stream.cpp + * @file mavlink_stream.h * Mavlink messages stream definition. * * @author Anton Babushkin <anton.babushkin@me.com> diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f09efa2e6..c44354ff0 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -42,6 +42,9 @@ SRCS += mavlink_main.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp + mavlink_rate_limiter.cpp \ + mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 368fa6ee2..763b87563 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -763,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp2 > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 || thrust > params.land_thr) { landed = false; landed_time = 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b71f9472f..dcad5c03b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e62b0fafc..1bf6f1de3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -226,11 +226,11 @@ sdlog2_usage(const char *reason) } errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n" - "\t-r\tLog rate in Hz, 0 means unlimited rate\n" - "\t-b\tLog buffer size in KiB, default is 8\n" - "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 8\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; + struct log_VICN_s log_VICN; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - // TODO not implemented yet + log_msg.msg_type = LOG_VICN_MSG; + log_msg.body.log_VICN.x = buf.vicon_pos.x; + log_msg.body.log_VICN.y = buf.vicon_pos.y; + log_msg.body.log_VICN.z = buf.vicon_pos.z; + log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICN.roll = buf.vicon_pos.roll; + log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICN); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a1a5856bc..d0585df39 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -301,6 +301,17 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; +/* --- VICN - VICON POSITION --- */ +#define LOG_VICN_MSG 25 +struct log_VICN_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -351,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), + LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ |