diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 378cbba170..adffe93c11 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -807,6 +807,15 @@ else mavlink start -r 800000 -d /dev/ttyACM0 -m config -x fi +# XXX Do not run any mavlink instances since we need the serial port for communication with snapdragon +mavlink stop-all + +# XXX Stop multicopter attitude controller, the controls come from snapdragon +mc_att_control stop + +# XXX Start snapdragon interface on serial port. On pixfalcon this is the standard telemetry port. +snapdragon_rc_pwm start -d /dev/ttyS1 + if [ $EXIT_ON_END == yes ] then echo "Exit from nsh" diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 6e96c71e25..80c0038c49 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -46,6 +46,7 @@ set(config_module_list drivers/pwm_input drivers/camera_trigger drivers/bst + drivers/snapdragon_rc_pwm # # System commands diff --git a/posix-configs/eagle/200qx/mainapp-flight.config b/posix-configs/eagle/200qx/mainapp-flight.config index b511bb1121..fcad72bfe5 100644 --- a/posix-configs/eagle/200qx/mainapp-flight.config +++ b/posix-configs/eagle/200qx/mainapp-flight.config @@ -4,4 +4,5 @@ mavlink start -u 14556 sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink stream -u 14556 -s RC_CHANNELS -r 50 mavlink boot_complete diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index fbfaaec54a..790f7f0c74 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -3,6 +3,7 @@ qshell start param set SYS_AUTOSTART 4001 gps start -d /dev/tty-4 sleep 1 +param set MAV_TYPE 2 df_mpu9250_wrapper start df_bmp280_wrapper start df_hmc5883_wrapper start diff --git a/src/drivers/snapdragon_rc_pwm/CMakeLists.txt b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt new file mode 100644 index 0000000000..6fae0f3157 --- /dev/null +++ b/src/drivers/snapdragon_rc_pwm/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE drivers__snapdragon_rc_pwm + MAIN snapdragon_rc_pwm + COMPILE_FLAGS + -Os + SRCS + snapdragon_rc_pwm.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : \ No newline at end of file diff --git a/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp new file mode 100644 index 0000000000..5f7be00ee6 --- /dev/null +++ b/src/drivers/snapdragon_rc_pwm/snapdragon_rc_pwm.cpp @@ -0,0 +1,428 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 snapdragon_rc_pwm.cpp + * @author Roman Bapst + * + * This driver sends rc commands to the Snapdragon via UART. On the same UART it receives pwm + * motor commands from the Snapdragon. + */ + + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + + +#include + + +#define MAX_LEN_DEV_PATH 32 + +namespace snapdragon_rc_pwm +{ + +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + +volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit +static char _device[MAX_LEN_DEV_PATH]; +static bool _is_running = false; // flag indicating if uart_esc app is running +static px4_task_t _task_handle = -1; // handle to the task main thread +static int _uart_fd = -1; +static bool _flow_control_enabled = false; +int _rc_sub = -1; + +orb_advert_t _actuator_controls_pub = nullptr; + +struct input_rc_s _rc = {}; +struct actuator_controls_s _actuators; + +// Print out the usage information +void usage(); + +void start(); + +/** uart_esc stop */ +void stop(); + +int initialise_uart(); + +int enable_flow_control(bool enabled); + +void send_rc_mavlink(); + +void handle_message(mavlink_message_t *msg); + +/** task main trampoline function */ +void task_main_trampoline(int argc, char *argv[]); + +/** uart_esc thread primary entry point */ +void task_main(int argc, char *argv[]); + +void task_main(int argc, char *argv[]) +{ + char serial_buf[128]; + mavlink_status_t serial_status = {}; + + _rc_sub = orb_subscribe(ORB_ID(input_rc)); + + + initialise_uart(); + + // we wait for uart actuator controls messages from snapdragon + px4_pollfd_struct_t fds[1]; + fds[0].fd = _uart_fd; + fds[0].events = POLLIN; + + while (true) { + + // wait for up to 100ms for data + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + // timed out + if (pret == 0) { + continue; + } + + if (pret < 0) { + PX4_WARN("snapdragon_rc_pwm poll error"); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + int len = ::read(_uart_fd, serial_buf, sizeof(serial_buf)); + + if (len > 0) { + mavlink_message_t msg; + + for (int i = 0; i < len; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { + // have a message, handle it + handle_message(&msg); + } + } + } + + // check if we have new rc data, if yes send it to snapdragon + bool rc_updated = false; + orb_check(_rc_sub, &rc_updated); + + if (rc_updated) { + orb_copy(ORB_ID(input_rc), _rc_sub, &_rc); + // send mavlink message + send_rc_mavlink(); + } + } + } + + +} + +void handle_message(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET) { + mavlink_actuator_control_target_t actuator_controls; + mavlink_msg_actuator_control_target_decode(msg, &actuator_controls); + _actuators.control[0] = actuator_controls.controls[0]; + _actuators.control[1] = actuator_controls.controls[1]; + _actuators.control[2] = actuator_controls.controls[2]; + _actuators.control[3] = actuator_controls.controls[3]; + _actuators.timestamp = hrt_absolute_time(); + + // publish actuator controls received from snapdragon + if (_actuator_controls_pub == nullptr) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &_actuators); + } + + } +} + +void send_rc_mavlink() +{ + mavlink_rc_channels_t rc_message; + rc_message.time_boot_ms = _rc.timestamp_publication / 1000; + rc_message.chancount = _rc.channel_count; + rc_message.chan1_raw = (_rc.channel_count > 0) ? _rc.values[0] : UINT16_MAX; + rc_message.chan2_raw = (_rc.channel_count > 1) ? _rc.values[1] : UINT16_MAX; + rc_message.chan3_raw = (_rc.channel_count > 2) ? _rc.values[2] : UINT16_MAX; + rc_message.chan4_raw = (_rc.channel_count > 3) ? _rc.values[3] : UINT16_MAX; + rc_message.chan5_raw = (_rc.channel_count > 4) ? _rc.values[4] : UINT16_MAX; + rc_message.chan6_raw = (_rc.channel_count > 5) ? _rc.values[5] : UINT16_MAX; + rc_message.chan7_raw = (_rc.channel_count > 6) ? _rc.values[6] : UINT16_MAX; + rc_message.chan8_raw = (_rc.channel_count > 7) ? _rc.values[7] : UINT16_MAX; + rc_message.chan9_raw = (_rc.channel_count > 8) ? _rc.values[8] : UINT16_MAX; + rc_message.chan10_raw = (_rc.channel_count > 9) ? _rc.values[9] : UINT16_MAX; + rc_message.chan11_raw = (_rc.channel_count > 10) ? _rc.values[10] : UINT16_MAX; + rc_message.chan12_raw = (_rc.channel_count > 11) ? _rc.values[11] : UINT16_MAX; + rc_message.chan13_raw = (_rc.channel_count > 12) ? _rc.values[12] : UINT16_MAX; + rc_message.chan14_raw = (_rc.channel_count > 13) ? _rc.values[13] : UINT16_MAX; + rc_message.chan15_raw = (_rc.channel_count > 14) ? _rc.values[14] : UINT16_MAX; + rc_message.chan16_raw = (_rc.channel_count > 15) ? _rc.values[15] : UINT16_MAX; + rc_message.chan17_raw = (_rc.channel_count > 16) ? _rc.values[16] : UINT16_MAX; + rc_message.chan18_raw = (_rc.channel_count > 17) ? _rc.values[17] : UINT16_MAX; + rc_message.rssi = _rc.rssi; + + const uint8_t msgid = MAVLINK_MSG_ID_RC_CHANNELS; + uint8_t component_ID = 0; + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* no idea which numbers should be here*/ + buf[2] = 100; + buf[3] = 0; + buf[4] = component_ID; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&rc_message, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + 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); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + int len = ::write(_uart_fd, &buf[0], packet_len); + + if (len < 1) { + PX4_WARN("failed sending rc mavlink message"); + } +} + +int initialise_uart() +{ + // open uart + _uart_fd = px4_open(_device, O_RDWR | O_NOCTTY); + int termios_state = -1; + + if (_uart_fd < 0) { + PX4_WARN("failed to open uart device!"); + return -1; + } + + // set baud rate + int speed = B115200; + struct termios uart_config; + tcgetattr(_uart_fd, &uart_config); + // clear ONLCR flag (which appends a CR for every LF) + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR SET BAUD %s: %d\n", _device, termios_state); + ::close(_uart_fd); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_WARN("ERR SET CONF %s\n", _device); + px4_close(_uart_fd); + return -1; + } + + (void)tcgetattr(_uart_fd, &uart_config); +#ifdef CRTS_IFLOW + uart_config.c_cflag |= CRTS_IFLOW; +#else + uart_config.c_cflag |= CRTSCTS; +#endif + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + + /* setup output flow control */ + if (enable_flow_control(true)) { + PX4_WARN("hardware flow control not supported"); + } + + return _uart_fd; +} + +int enable_flow_control(bool enabled) +{ + struct termios uart_config; + + int ret = tcgetattr(_uart_fd, &uart_config); + + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + if (!ret) { + _flow_control_enabled = enabled; + } + + return ret; +} + +// uart_esc main entrance +void task_main_trampoline(int argc, char *argv[]) +{ + PX4_WARN("task_main_trampoline"); + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + /* start the task */ + _task_handle = px4_task_spawn_cmd("snapdragon_rc_pwm_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("task start failed"); + return; + } + + _is_running = true; +} + +void stop() +{ + // TODO - set thread exit signal to terminate the task main thread + + _is_running = false; + _task_handle = -1; +} + +void usage() +{ + PX4_WARN("missing command: try 'start', 'stop', 'status'"); + PX4_WARN("options:"); + PX4_WARN(" -d device"); +} + + +} + +extern "C" __EXPORT int snapdragon_rc_pwm_main(int argc, char *argv[]); + +int snapdragon_rc_pwm_main(int argc, char *argv[]) +{ + const char *device = NULL; + int ch; + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + break; + + default: + snapdragon_rc_pwm::usage(); + return 1; + } + } + + // Check on required arguments + if (device == NULL || strlen(device) == 0) { + snapdragon_rc_pwm::usage(); + return 1; + } + + memset(snapdragon_rc_pwm::_device, 0, MAX_LEN_DEV_PATH); + strncpy(snapdragon_rc_pwm::_device, device, strlen(device)); + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (snapdragon_rc_pwm::_is_running) { + PX4_WARN("uart_esc already running"); + return 1; + } + + snapdragon_rc_pwm::start(); + } + + else if (!strcmp(verb, "stop")) { + if (snapdragon_rc_pwm::_is_running) { + PX4_WARN("snapdragon_rc_pwm is not running"); + return 1; + } + + snapdragon_rc_pwm::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("snapdragon_rc_pwm is %s", snapdragon_rc_pwm::_is_running ? "running" : "stopped"); + return 0; + + } else { + snapdragon_rc_pwm::usage(); + return 1; + } + + return 0; +} \ No newline at end of file diff --git a/src/drivers/uart_esc/uart_esc.cpp b/src/drivers/uart_esc/uart_esc.cpp index c28755c096..565dc26c99 100644 --- a/src/drivers/uart_esc/uart_esc.cpp +++ b/src/drivers/uart_esc/uart_esc.cpp @@ -52,6 +52,8 @@ #include #include #include +#include +#include #define MAX_LEN_DEV_PATH 32 #define RC_MAGIC 0xf6 @@ -60,6 +62,9 @@ namespace uart_esc { #define UART_ESC_MAX_MOTORS 4 +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit static char _device[MAX_LEN_DEV_PATH]; static bool _is_running = false; // flag indicating if uart_esc app is running @@ -78,7 +83,7 @@ static const char *MIXER_FILENAME = "/dev/fs/quad_x.main.mix"; // publications orb_advert_t _outputs_pub; -orb_advert_t _rc_pub; +orb_advert_t _rc_pub = nullptr; // topic structures actuator_controls_s _controls; @@ -96,6 +101,12 @@ void start(const char *device); /** uart_esc stop */ void stop(); +void send_controls_mavlink(); + +void serial_callback(void *context, char *buffer, size_t num_bytes); + +void handle_message(mavlink_message_t *rc_message); + /** task main trampoline function */ void task_main_trampoline(int argc, char *argv[]); @@ -276,21 +287,17 @@ int uart_initialize(const char *device, int baud) return -1; } - struct dspal_serial_open_options options; + struct dspal_serial_ioctl_data_rate rate; - options.bit_rate = DSPAL_SIO_BITRATE_57600; + rate.bit_rate = DSPAL_SIO_BITRATE_115200; - options.tx_flow = DSPAL_SIO_FCTL_OFF; + int ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate); - options.rx_flow = DSPAL_SIO_FCTL_OFF; + struct dspal_serial_ioctl_receive_data_callback callback; - options.rx_data_callback = nullptr; + callback.rx_data_callback_func_ptr = serial_callback; - options.tx_data_callback = nullptr; - - options.is_tx_data_synchronous = false; - - int ret = ::ioctl(fd, SERIAL_IOCTL_OPEN_OPTIONS, (void *)&options); + ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback); if (ret != 0) { PX4_ERR("Failed to setup UART flow control options"); @@ -300,18 +307,107 @@ int uart_initialize(const char *device, int baud) return 0; } -void handleRC(int uart_fd, struct input_rc_s *rc) +// send actuator controls message to Pixhawk +void send_controls_mavlink() { - // read uart until we get the magic number - char buf[10]; + mavlink_actuator_control_target_t controls_message; + controls_message.controls[0] = _controls.control[0]; + controls_message.controls[1] = _controls.control[1]; + controls_message.controls[2] = _controls.control[2]; + controls_message.controls[3] = _controls.control[3]; + controls_message.time_usec = _controls.timestamp; - while (true) { - int ret = ::read(uart_fd, &buf, sizeof(buf)); - PX4_ERR("got %d", ret); + const uint8_t msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; + uint8_t component_ID = 0; + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - if (ret < 1) { - break; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* no idea which numbers should be here*/ + buf[2] = 100; + buf[3] = 0; + buf[4] = component_ID; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], (const void *)&controls_message, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + 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); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + int len = ::write(_fd, &buf[0], packet_len); + + if (len < 1) { + PX4_WARN("failed sending rc mavlink message %.5f", (double)_fd); + } +} + +// callback function for the uart +void serial_callback(void *context, char *buffer, size_t num_bytes) +{ + mavlink_status_t serial_status = {}; + + if (num_bytes > 0) { + mavlink_message_t msg; + + for (int i = 0; i < num_bytes; ++i) { + if (mavlink_parse_char(MAVLINK_COMM_1, buffer[i], &msg, &serial_status)) { + // have a message, handle it + if (msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS) { + // we should publish but would be great if this works + handle_message(&msg); + } + } } + + } else { + + PX4_ERR("error: read callback with no data in the buffer"); + } +} + +void handle_message(mavlink_message_t *rc_message) +{ + mavlink_rc_channels_t rc; + mavlink_msg_rc_channels_decode(rc_message, &rc); + _rc.timestamp_publication = hrt_absolute_time(); + _rc.timestamp_last_signal = hrt_absolute_time(); + _rc.channel_count = 8; + _rc.rc_lost = false; + _rc.values[0] = rc.chan1_raw; + _rc.values[1] = rc.chan2_raw; + _rc.values[2] = rc.chan3_raw; + _rc.values[3] = rc.chan4_raw; + _rc.values[4] = rc.chan5_raw; + _rc.values[5] = rc.chan6_raw; + _rc.values[6] = rc.chan7_raw; + _rc.values[7] = rc.chan8_raw; + _rc.values[8] = rc.chan9_raw; + _rc.values[9] = rc.chan10_raw; + _rc.values[10] = rc.chan11_raw; + _rc.values[11] = rc.chan12_raw; + _rc.values[12] = rc.chan13_raw; + _rc.values[13] = rc.chan14_raw; + _rc.values[14] = rc.chan15_raw; + _rc.values[15] = rc.chan16_raw; + _rc.values[16] = rc.chan17_raw; + _rc.values[17] = rc.chan18_raw; + + if (_rc_pub != nullptr) { + orb_publish(ORB_ID(input_rc), _rc_pub, &_rc); + + } else { + _rc_pub = orb_advertise(ORB_ID(input_rc), &_rc); } } @@ -347,6 +443,8 @@ void task_main(int argc, char *argv[]) _task_should_exit = true; } + _rc_pub = orb_advertise(ORB_ID(input_rc), &_rc); + // Main loop while (!_task_should_exit) { int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); @@ -368,89 +466,19 @@ void task_main(int argc, char *argv[]) if (fds[0].revents & POLLIN) { // Grab new controls data orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); - // Mix to the outputs - _outputs.timestamp = hrt_absolute_time(); - int16_t motor_rpms[UART_ESC_MAX_MOTORS]; - if (_armed.armed) { - _outputs.noutputs = mixer->mix(&_outputs.output[0], - actuator_controls_0_s::NUM_ACTUATOR_CONTROLS, - NULL); + send_controls_mavlink(); - // Make sure we support only up to UART_ESC_MAX_MOTORS motors - if (_outputs.noutputs > UART_ESC_MAX_MOTORS) { - PX4_ERR("Unsupported motors %d, up to %d motors supported", - _outputs.noutputs, UART_ESC_MAX_MOTORS); - continue; - } + // this is needed otherwise the uart internal states will flood. Probably + // we need to make update rate faster + usleep(5000); - // iterate actuators - for (unsigned i = 0; i < _outputs.noutputs; i++) { - // last resort: catch NaN, INF and out-of-band errors - if (i < _outputs.noutputs && - PX4_ISFINITE(_outputs.output[i]) && - _outputs.output[i] >= -1.0f && - _outputs.output[i] <= 1.0f) { - // scale for PWM output 1000 - 2000us - _outputs.output[i] = 1500 + (500 * _outputs.output[i]); - - } else { - // - // Value is NaN, INF or out of band - set to the minimum value. - // This will be clearly visible on the servo status and will limit the risk of accidentally - // spinning motors. It would be deadly in flight. - // - _outputs.output[i] = 900; - } - } - - uart_esc_rotate_motors(motor_rpms, _outputs.noutputs); - - } else { - _outputs.noutputs = UART_ESC_MAX_MOTORS; - - for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { - _outputs.output[outIdx] = 900; - } - } - - uint8_t data[11]; - struct PACKED { - uint8_t magic = 0xF7; - uint16_t period[4]; - uint16_t crc; - } frame; - - for (uint8_t i = 0; i < 4; i++) { - frame.period[i] = _outputs.output[i]; - } - - frame.crc = crc_calculate((uint8_t *)frame.period, 4 * 2); - - data[0] = frame.magic; - memcpy(&data[1], (uint8_t *)frame.period, sizeof(frame.period)); - memcpy(&data[9], (uint8_t *)&frame.crc, sizeof(frame.crc)); - - ::write(_fd, data, sizeof(data)); - - /* - static int count=0; - count++; - if (!(count % 1)) { - PX4_DEBUG(" "); - PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed); - PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]); - PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]); - } - */ - - /* Publish mixed control outputs */ - if (_outputs_pub != nullptr) { + /*if (_outputs_pub != nullptr) { orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); } else { _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); - } + }*/ } // Check for updates in other subscripions