diff --git a/boards/modalai/voxl2-slpi/src/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/CMakeLists.txt index 31a10426fb..111ec4fd9a 100644 --- a/boards/modalai/voxl2-slpi/src/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/CMakeLists.txt @@ -44,3 +44,5 @@ add_library(drivers_board # Add custom drivers for SLPI add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in) diff --git a/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/CMakeLists.txt new file mode 100644 index 0000000000..eb2bf73313 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +message(STATUS "Mavlink include directory: ${PX4_SOURCE_DIR}/../build/modalai_voxl2_default/mavlink/standard") + +px4_add_module( + MODULE drivers__modalai__mavlink_rc_in + MAIN mavlink_rc_in + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + -Wno-address-of-packed-member # TODO: fix in c_library_v2 + INCLUDES + ${PX4_SOURCE_DIR}/src/drivers/rc_input + ${PX4_SOURCE_DIR}/build/modalai_voxl2_default/mavlink/common + SRCS + mavlink_rc_in.cpp + DEPENDS + px4_work_queue + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp b/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp new file mode 100644 index 0000000000..d1f1bdde1e --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp @@ -0,0 +1,499 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "uORB/uORBManager.hpp" +#include +#include +#include +#include + + +#define ASYNC_UART_READ_WAIT_US 500 +#define RC_INPUT_RSSI_MAX 100 + +#define TX_BUFFER_LEN 20 + +extern "C" { __EXPORT int mavlink_rc_in_main(int argc, char *argv[]); } + +namespace mavlink_rc_in +{ + +static bool _is_running = false; +volatile bool _task_should_exit = false; + +static px4_task_t _task_handle = -1; +static px4_task_t _mav_task_handle = -1; + +static int _uart_fd = -1; + +static bool debug = false; +static bool mav_en = true; +static bool crsf_en = false; +static bool fake_heartbeat_enable = true; +static bool filter_local_rc_messages = true; +static bool dump_received_messages = false; +static bool dump_transmitted_messages = false; + +std::string port = "7"; +uint32_t baudrate = 115200; + +uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + +perf_counter_t _perf_rx_rate = nullptr; + +static uint8_t _rc_lq; +static uint8_t _rc_rssi_dbm; + +int open_port(const char *dev, speed_t speed); +int close_port(); + +int write_response(void *buf, size_t len); + +int start(int argc, char *argv[]); +int stop(); +int status(); + +void usage(); +void task_main(int argc, char *argv[]); + +void handle_message_dsp(mavlink_message_t *msg); +void handle_message_rc_channels_override_dsp(mavlink_message_t *msg); + +void handle_message_dsp(mavlink_message_t *msg) +{ + if (debug) { + if (filter_local_rc_messages) { + if ((msg->msgid != MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) && + (msg->msgid != MAVLINK_MSG_ID_RADIO_STATUS)) { + PX4_INFO("**^^ MSG ID: %d ^^*****************************************************", msg->msgid); + } + + } else { + PX4_INFO("msg ID: %d", msg->msgid); + } + } + + switch (msg->msgid) { + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + handle_message_rc_channels_override_dsp(msg); + break; + + default: + break; + } +} + +void mavlink_out_task(int argc, char *argv[]) +{ + uint64_t last_heartbeat_timestamp = hrt_absolute_time(); + + while (! _task_should_exit) { + uint64_t timestamp = hrt_absolute_time(); + int nwrite = 0; + + // We need to provide heartbeat messages to the receiver. Otherwise it + // won't send us anything! + if ((timestamp - last_heartbeat_timestamp) > 1000000) { + mavlink_heartbeat_t hb = {}; + mavlink_message_t hb_message = {}; + hb.type = MAV_TYPE_QUADROTOR; + hb.autopilot = MAV_AUTOPILOT_PX4; + mavlink_msg_heartbeat_encode(1, 1, &hb_message, &hb); + + uint8_t hb_newBuf[MAVLINK_MAX_PACKET_LEN]; + uint16_t hb_newBufLen = 0; + hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message); + + nwrite = qurt_uart_write(_uart_fd, (char *) &hb_newBuf[0], hb_newBufLen); + + if (nwrite != hb_newBufLen) { + PX4_ERR("Heartbeat write failed. Expected %d, got %d", hb_newBufLen, nwrite); + + } else if (debug) { + PX4_INFO("Sending local heartbeat to TBS"); + + if (dump_transmitted_messages) { + for (int i = 0; i <= hb_newBufLen; i += 16) { + PX4_INFO(" %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x", + hb_newBuf[i + 0], hb_newBuf[i + 1], hb_newBuf[i + 2], hb_newBuf[i + 3], + hb_newBuf[i + 4], hb_newBuf[i + 5], hb_newBuf[i + 6], hb_newBuf[i + 7], + hb_newBuf[i + 8], hb_newBuf[i + 9], hb_newBuf[i + 10], hb_newBuf[i + 11], + hb_newBuf[i + 12], hb_newBuf[i + 13], hb_newBuf[i + 14], hb_newBuf[i + 15]); + } + } + } + + last_heartbeat_timestamp = timestamp; + } + + px4_usleep(100000); + } +} + +void task_main(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "rtdmclfp:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + debug = true; + PX4_INFO("Setting debug flag on"); + break; + + case 'm': + mav_en = true; + crsf_en = false; + + if (debug) { PX4_INFO("Using MAVLink mode"); } + + break; + + case 'c': + mav_en = false; + crsf_en = true; + + if (debug) { PX4_INFO("Using CRSF mode"); } + + break; + + case 'p': + port = myoptarg; + + if (debug) { PX4_INFO("Setting port to %s", port.c_str()); } + + break; + + case 'b': + baudrate = atoi(myoptarg); + + if (debug) { PX4_INFO("Setting baudrate to %u", baudrate); } + + break; + + case 'f': + fake_heartbeat_enable = false; + + if (debug) { PX4_INFO("Disabling fake heartbeats"); } + + break; + + case 'l': + filter_local_rc_messages = true; + + if (debug) { PX4_INFO("Filtering debug messages from the RC receiver"); } + + break; + + case 'r': + dump_received_messages = true; + + if (debug) { PX4_INFO("Enabling hex dump of received messages"); } + + break; + + case 't': + dump_transmitted_messages = true; + + if (debug) { PX4_INFO("Enabling hex dump of transmitted messages"); } + + break; + + default: + break; + } + } + + if (open_port(port.c_str(), (speed_t) baudrate) == -1) { + PX4_ERR("Failed to open UART"); + return; + } + + _mav_task_handle = px4_task_spawn_cmd("mavlink_rc_in_mav", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t)&mavlink_out_task, + (char *const *)argv); + + if (_mav_task_handle < 0) { + PX4_ERR("_mav_task_handle task start failed"); + return; + } + + _is_running = true; + _perf_rx_rate = perf_alloc(PC_INTERVAL, "mavlink_rc_in: rx interval"); + + uint8_t rx_buf[1024]; + mavlink_message_t msg; + mavlink_status_t status{}; + + while (! _task_should_exit) { + // Check for incoming messages from the TBS Crossfire receiver + int nread = qurt_uart_read(_uart_fd, (char *) rx_buf, sizeof(rx_buf), ASYNC_UART_READ_WAIT_US); + + if (nread) { + if (debug) { + PX4_INFO("TBS Crossfire (MAVLink mode): read %d bytes", nread); + + if (dump_received_messages) { + for (int i = 0; i <= nread; i += 16) { + PX4_INFO(" %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x", + rx_buf[i + 0], rx_buf[i + 1], rx_buf[i + 2], rx_buf[i + 3], + rx_buf[i + 4], rx_buf[i + 5], rx_buf[i + 6], rx_buf[i + 7], + rx_buf[i + 8], rx_buf[i + 9], rx_buf[i + 10], rx_buf[i + 11], + rx_buf[i + 12], rx_buf[i + 13], rx_buf[i + 14], rx_buf[i + 15]); + } + } + } + + //Take buffer and convert it into mavlink msg + for (int i = 0; i < nread; i++) { + if (mavlink_parse_char(MAVLINK_COMM_0, rx_buf[i], &msg, &status)) { + handle_message_dsp(&msg); + } + } + } + + px4_usleep(5000); + } +} + +void handle_message_rc_channels_override_dsp(mavlink_message_t *msg) +{ + mavlink_rc_channels_override_t man; + mavlink_msg_rc_channels_override_decode(msg, &man); + + // if (debug) PX4_INFO("RC channels override msg received"); + + // Check target + if (man.target_system != 0) { + PX4_ERR("Message has incorrect target system %u", man.target_system); + return; + } + + // fill uORB message + input_rc_s rc{}; + + // metadata + rc.timestamp = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp; + rc.rssi = RC_INPUT_RSSI_MAX; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + + // channels + rc.values[0] = man.chan1_raw; + rc.values[1] = man.chan2_raw; + rc.values[2] = man.chan3_raw; + rc.values[3] = man.chan4_raw; + rc.values[4] = man.chan5_raw; + rc.values[5] = man.chan6_raw; + rc.values[6] = man.chan7_raw; + rc.values[7] = man.chan8_raw; + rc.values[8] = man.chan9_raw; + rc.values[9] = man.chan10_raw; + rc.values[10] = man.chan11_raw; + rc.values[11] = man.chan12_raw; + rc.values[12] = man.chan13_raw; + rc.values[13] = man.chan14_raw; + rc.values[14] = man.chan15_raw; + rc.values[15] = man.chan16_raw; + rc.values[16] = man.chan17_raw; + rc.values[17] = man.chan18_raw; + + // check how many channels are valid + for (int i = 17; i >= 0; i--) { + const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX + const bool ignore_zero = (i > 7) && (rc.values[i] == 0); // ignore channel 8-18 if value is 0 + + if (ignore_max || ignore_zero) { + // set all ignored values to zero + rc.values[i] = 0; + + } else { + // first channel to not ignore -> set count considering zero-based index + rc.channel_count = i + 1; + break; + } + } + + // publish uORB message + _rc_pub.publish(rc); +} + +int open_port(const char *dev, speed_t speed) +{ + if (_uart_fd >= 0) { + PX4_ERR("Port already in use: %s", dev); + return -1; + } + + _uart_fd = qurt_uart_open(dev, speed); + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + + } else if (debug) { PX4_INFO("qurt uart opened successfully"); } + + return 0; +} + +int close_port() +{ + _uart_fd = -1; + + return 0; +} + +int write_response(void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + + return qurt_uart_write(_uart_fd, (const char *) buf, len); +} + +int start(int argc, char *argv[]) +{ + if (_is_running) { + PX4_WARN("already running"); + return -1; + } + + _task_should_exit = false; + + _task_handle = px4_task_spawn_cmd("mavlink_rc_in_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t)&task_main, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +int stop() +{ + if (!_is_running) { + PX4_WARN("not running"); + return -1; + } + + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; + return 0; +} + +int status() +{ + PX4_INFO("running: %s", _is_running ? "yes" : "no"); + PX4_INFO(""); + PX4_INFO(" RSSI: %i", _rc_rssi_dbm); + PX4_INFO(" LQ: %i", _rc_lq); + PX4_INFO(""); + perf_print_counter(_perf_rx_rate); + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: mavlink_rc_in {start|info|stop} [options]"); + PX4_INFO("Options: -d enable debug messages"); + PX4_INFO(" -p uart port number"); + PX4_INFO(" -b uart baudrate"); + PX4_INFO(" -c use CRSF protocol over the wire"); + PX4_INFO(" -m use MAVLink protocol over the wire"); + PX4_INFO(" -f disable fake hearbeats"); + PX4_INFO(" -l filter out local rc received debug messages"); + PX4_INFO(" -r enable received messages hex dump"); + PX4_INFO(" -t enable transmitted messages hex dump"); +} + +} + +int mavlink_rc_in_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + mavlink_rc_in::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return mavlink_rc_in::start(argc - 1, argv + 1); + + } else if (!strcmp(verb, "stop")) { + return mavlink_rc_in::stop(); + + } else if (!strcmp(verb, "status")) { + return mavlink_rc_in::status(); + + } else { + mavlink_rc_in::usage(); + return -1; + } +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/CMakeLists.txt new file mode 100644 index 0000000000..1c51cb5555 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2021 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__modalai__rc_controller + MAIN rc_controller + SRCS + rc_controller.cpp + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp new file mode 100644 index 0000000000..47b8b90212 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 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 px4_simple_app.c + * Minimal application example for PX4 autopilot + * + * @author Example User + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "rc_controller.hpp" + +int RC_ControllerModule::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int RC_ControllerModule::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("not running"); + return 1; + } + + if (!strcmp(argv[0], "throttle")) { + uint16_t val = atoi(argv[1]); + get_instance()->set_throttle(val); + PX4_INFO("Setting throttle to %u", val); + return 0; + } + + if (!strcmp(argv[0], "yaw")) { + uint16_t val = atoi(argv[1]); + get_instance()->set_yaw(val); + PX4_INFO("Setting yaw to %u", val); + return 0; + } + + if (!strcmp(argv[0], "pitch")) { + uint16_t val = atoi(argv[1]); + get_instance()->set_pitch(val); + PX4_INFO("Setting pitch to %u", val); + return 0; + } + + if (!strcmp(argv[0], "roll")) { + uint16_t val = atoi(argv[1]); + get_instance()->set_roll(val); + PX4_INFO("Setting roll to %u", val); + return 0; + } + + return print_usage("unknown command"); +} + + +int RC_ControllerModule::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("RC_ControllerModule", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1024, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +RC_ControllerModule *RC_ControllerModule::instantiate(int argc, char *argv[]) +{ + // int example_param = 0; + // bool example_flag = false; + // bool error_flag = false; + // + // int myoptind = 1; + // int ch; + // const char *myoptarg = nullptr; + // + // // parse CLI arguments + // while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) { + // switch (ch) { + // case 'p': + // example_param = (int)strtol(myoptarg, nullptr, 10); + // break; + // + // case 'f': + // example_flag = true; + // break; + // + // case '?': + // error_flag = true; + // break; + // + // default: + // PX4_WARN("unrecognized flag"); + // error_flag = true; + // break; + // } + // } + // + // if (error_flag) { + // return nullptr; + // } + + // RC_ControllerModule *instance = new RC_ControllerModule(example_param, example_flag); + RC_ControllerModule *instance = new RC_ControllerModule(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +// RC_ControllerModule::RC_ControllerModule(int example_param, bool example_flag) +RC_ControllerModule::RC_ControllerModule() + : ModuleParams(nullptr) +{ + _throttle = 800; + // _yaw = 1500; + // _pitch = 1500; + // _roll = 1500; + _yaw = 800; + _pitch = 800; + _roll = 800; +} + +void RC_ControllerModule::run() +{ + PX4_INFO("ModalAI RC_ControllerModule starting"); + + input_rc_s rc_data; + memset(&rc_data, 0, sizeof(input_rc_s)); + orb_advert_t input_rc_pub_fd = orb_advertise(ORB_ID(input_rc), &rc_data); + + while (!should_exit()) { + + usleep(10000); + + rc_data.input_source = input_rc_s::RC_INPUT_SOURCE_QURT; + rc_data.channel_count = 4; + // for (unsigned i = 0; i < rc_data.channel_count; ++i) { + // rc_data.values[i] = 1024; + // } + rc_data.values[0] = _throttle; + rc_data.values[1] = _yaw; + rc_data.values[2] = _pitch; + rc_data.values[3] = _roll; + + rc_data.timestamp = hrt_absolute_time(); + rc_data.timestamp_last_signal = rc_data.timestamp; + rc_data.rc_ppm_frame_length = 0; + rc_data.rssi = 100; + rc_data.rc_failsafe = false; + rc_data.rc_lost = 0; + rc_data.rc_lost_frame_count = 0; + rc_data.rc_total_frame_count = 0; + + orb_publish(ORB_ID(input_rc), input_rc_pub_fd, &rc_data); + + parameters_update(); + } + + PX4_INFO("RC_ControllerModule Exiting"); +} + +void RC_ControllerModule::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } +} + +int RC_ControllerModule::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + // TODO: PRINT_MODULE_DESCRIPTION + + PRINT_MODULE_USAGE_NAME("rc_controller", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("rc"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int rc_controller_main(int argc, char *argv[]) +{ + return RC_ControllerModule::main(argc, argv); +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp new file mode 100644 index 0000000000..bfcaf40028 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +extern "C" __EXPORT int rc_controller_main(int argc, char *argv[]); + +class RC_ControllerModule : public ModuleBase, public ModuleParams +{ +public: + RC_ControllerModule(); + + virtual ~RC_ControllerModule() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static RC_ControllerModule *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void set_throttle(uint16_t val) { _throttle = val; } + void set_yaw(uint16_t val) { _yaw = val; } + void set_pitch(uint16_t val) { _pitch = val; } + void set_roll(uint16_t val) { _roll = val; } + +private: + + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); + + + DEFINE_PARAMETERS( + (ParamInt) _param_sys_autostart, /**< example parameter */ + (ParamInt) _param_sys_autoconfig /**< another parameter */ + ) + + uint16_t _throttle; + uint16_t _yaw; + uint16_t _pitch; + uint16_t _roll; + + // Subscriptions + // uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1000000}; + +}; diff --git a/platforms/common/work_queue/work_thread.c b/platforms/common/work_queue/work_thread.c index 4e4713a19c..b1e0e72355 100644 --- a/platforms/common/work_queue/work_thread.c +++ b/platforms/common/work_queue/work_thread.c @@ -107,6 +107,14 @@ static void work_process(struct wqueue_s *wqueue, int lock_id) next = CONFIG_SCHED_WORKPERIOD; +#ifdef __PX4_QURT + // In Posix certain signals wake up a sleeping thread but it isn't the case + // with the Qurt POSIX implementation. So rather than assume we can come out + // of the sleep early by a signal we just wake up more often. + next = 1000; +#endif + + work_lock(lock_id); work = (struct work_s *)wqueue->q.head; diff --git a/platforms/qurt/src/px4/main.cpp b/platforms/qurt/src/px4/main.cpp index 1372fa9179..1155576552 100644 --- a/platforms/qurt/src/px4/main.cpp +++ b/platforms/qurt/src/px4/main.cpp @@ -164,6 +164,8 @@ const char *get_commands() int slpi_entry(int argc, char *argv[]) { PX4_INFO("Inside slpi_entry"); + sleep(1); // give time for apps side to finish initialization + apps_map_type apps; init_app_map(apps); process_commands(apps, get_commands()); diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp index 91e19eb102..ef545f8b5c 100644 --- a/platforms/qurt/src/px4/tasks.cpp +++ b/platforms/qurt/src/px4/tasks.cpp @@ -70,6 +70,35 @@ static pthread_mutex_t task_mutex; extern "C" { int pthread_setname_np(pthread_t __target_thread, const char *__name) { return 0; } + + // This function is in pthread.h but is not, apparently, defined in the + // Qurt system image. So, it is being defined here. Unfortunately it doesn't + // seem to work. It looks like the Qurt pthread implementation likes to allocate + // the stack itself and not give us that ability. + // int pthread_attr_setstackaddr(pthread_attr_t *attr, void * stackaddr) { + // if (attr == NULL) return -1; + // if (stackaddr == NULL) return -1; + // attr->stackaddr = stackaddr; + // attr->internal_stack = 0; + // return 0; + // } + + // This function is in pthread.h but is not, apparently, defined in the + // Qurt system image. So, it is being defined here. + int pthread_attr_setthreadname(pthread_attr_t *attr, const char *name) + { + if (attr == NULL) { return -1; } + + if (&attr->name[0] == NULL) { return -1; } + + if (name == NULL) { return -1; } + + memcpy(attr->name, name, PTHREAD_NAME_LEN); + attr->name[PTHREAD_NAME_LEN - 1] = 0; + return 0; + } + + // Qurt only has one scheduling policy so this just returns a success int pthread_attr_setschedpolicy(pthread_attr_t *attr, int policy) { return 0; } } @@ -172,11 +201,30 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma strcpy(&taskmap[task_index].name[4], name); struct sched_param param; + + // Qurt threads have different priority numbers. 1 is the highest + // priority and 255 is the lowest. But we are using the pthread + // implementation on Qurt which returns 255 when you call sched_get_priority_max. + // However, the Qurt pthread implementation deals with this properly when + // creating the underlying Qurt task by mapping the high number into a low number. + + // For high priorities bump everything down a little so that critical Qurt + // threads are not impacted. + if (priority > 128) { + priority -= 16; + } + + // Likewise, for low priorities, bump everything up a little. + else if (priority < 128) { + priority += 10; + } + param.sched_priority = priority; pthread_attr_init(&taskmap[task_index].attr); - //pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name); - //pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack); + pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name); + // See note above about the pthread_attr_setstackaddr function + // pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack); pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE); pthread_attr_setschedparam(&taskmap[task_index].attr, ¶m); diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index f32470c0c2..425167e599 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -268,6 +268,8 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) return -1; } + hrt_init(); + uORB::Manager::initialize(); uORB::Manager::get_instance()->set_uorb_communicator( uORB::ProtobufChannel::GetInstance()); @@ -288,6 +290,7 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) // Initialize the interrupt callback registration register_interrupt_callback_initalizer(muorb_func_ptrs.register_interrupt_callback); + work_queues_init(); hrt_work_queue_init(); const char *argv[3] = { "slpi", "start" }; @@ -304,6 +307,9 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) qurt_thread_attr_init(&aggregator_attr); qurt_thread_attr_set_stack_addr(&aggregator_attr, aggregator_stack); qurt_thread_attr_set_stack_size(&aggregator_attr, aggregator_stack_size); + char thread_name[QURT_THREAD_ATTR_NAME_MAXLEN]; + strncpy(thread_name, "PX4_muorb_agg", QURT_THREAD_ATTR_NAME_MAXLEN); + qurt_thread_attr_set_name(&aggregator_attr, thread_name); qurt_thread_attr_set_priority(&aggregator_attr, aggregator_thread_priority); (void) qurt_thread_create(&aggregator_tid, &aggregator_attr, aggregator_thread_func, NULL);