From 7ff8ead60e379cf8615329f47eae76eb9ea37210 Mon Sep 17 00:00:00 2001 From: Travis Bottalico Date: Thu, 3 Sep 2020 12:25:50 -0700 Subject: [PATCH 01/39] initial commit for UART ESC support --- ROMFS/px4fmu_common/init.d/rc.interface | 18 +- boards/modalai/fc-v1/default.cmake | 1 + boards/modalai/fc-v1/init/rc.board_defaults | 24 + boards/modalai/fc-v1/init/rc.board_sensors | 6 + .../uart_esc/modalai_esc/CMakeLists.txt | 44 + src/drivers/uart_esc/modalai_esc/crc16.c | 95 ++ src/drivers/uart_esc/modalai_esc/crc16.h | 65 ++ .../uart_esc/modalai_esc/modalai_esc.cpp | 919 ++++++++++++++++++ .../uart_esc/modalai_esc/modalai_esc.hpp | 190 ++++ .../modalai_esc/modalai_esc_serial.cpp | 159 +++ .../modalai_esc/modalai_esc_serial.hpp | 59 ++ .../uart_esc/modalai_esc/qc_esc_packet.c | 241 +++++ .../uart_esc/modalai_esc/qc_esc_packet.h | 240 +++++ .../modalai_esc/qc_esc_packet_types.h | 71 ++ .../uart_esc/modalai_esc/uart_esc_params.c | 122 +++ 15 files changed, 2253 insertions(+), 1 deletion(-) create mode 100644 src/drivers/uart_esc/modalai_esc/CMakeLists.txt create mode 100644 src/drivers/uart_esc/modalai_esc/crc16.c create mode 100644 src/drivers/uart_esc/modalai_esc/crc16.h create mode 100644 src/drivers/uart_esc/modalai_esc/modalai_esc.cpp create mode 100644 src/drivers/uart_esc/modalai_esc/modalai_esc.hpp create mode 100644 src/drivers/uart_esc/modalai_esc/modalai_esc_serial.cpp create mode 100644 src/drivers/uart_esc/modalai_esc/modalai_esc_serial.hpp create mode 100644 src/drivers/uart_esc/modalai_esc/qc_esc_packet.c create mode 100644 src/drivers/uart_esc/modalai_esc/qc_esc_packet.h create mode 100644 src/drivers/uart_esc/modalai_esc/qc_esc_packet_types.h create mode 100644 src/drivers/uart_esc/modalai_esc/uart_esc_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 76d9ff99e89d..a7bc12dfc3a6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -57,7 +57,13 @@ then set OUTPUT_MODE dshot set OUTPUT_CMD dshot else - set OUTPUT_MODE pwm_out + if param greater UART_ESC_CONFIG 0 + then + set OUTPUT_MODE uart_esc + set OUTPUT_CMD uart_esc + else + set OUTPUT_MODE pwm_out + fi fi fi fi @@ -144,6 +150,16 @@ then set OUTPUT_DEV /dev/uavcan/esc fi + if [ $OUTPUT_MODE = tap_esc ] + then + set OUTPUT_DEV /dev/tap_esc + fi + + if [ $OUTPUT_MODE = uart_esc ] + then + set OUTPUT_DEV /dev/uart_esc + fi + if mixer load ${OUTPUT_DEV} ${MIXER_FILE} then echo "INFO [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}" diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 7a21df918b5c..ae91dfb0b200 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -49,6 +49,7 @@ px4_add_board( telemetry # all available telemetry drivers #tone_alarm uavcan + uart_esc/modalai_esc MODULES airspeed_selector attitude_estimator_q diff --git a/boards/modalai/fc-v1/init/rc.board_defaults b/boards/modalai/fc-v1/init/rc.board_defaults index e8abaa09b354..ff92a34ddb2a 100644 --- a/boards/modalai/fc-v1/init/rc.board_defaults +++ b/boards/modalai/fc-v1/init/rc.board_defaults @@ -24,4 +24,28 @@ param set-default MAV_1_CONFIG 102 param set-default MAV_1_MODE 2 param set-default SER_TEL2_BAUD 921600 +# VOXL UART ESC +if param compare UART_ESC_CONFIG 1 +then + # UART ESC is configured to use /dev/ttyS1, same as TELEM3, + # so ensure that this MAVLink instance is disabled + if param compare MAV_0_CONFIG 103 + then + echo "Disabling MAV_0_CONFIG on TELEM3 (/dev/ttyS1) in favor of UART ESC" + param set MAV_0_CONFIG 0 + fi + if param compare MAV_1_CONFIG 103 + then + echo "Disabling MAV_1_CONFIG on TELEM3 (/dev/ttyS1) in favor of UART ESC" + param set MAV_1_CONFIG 0 + fi + if param compare MAV_2_CONFIG 103 + then + echo "Disabling MAV_2_CONFIG on TELEM3 (/dev/ttyS1) in favor of UART ESC" + param set MAV_2_CONFIG 0 + fi +fi + +set LOGGER_BUF 64 + safety_button start diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 8261c9b5c255..073d2d263f05 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -3,6 +3,12 @@ # ModalAI FC-v1 specific board sensors init #------------------------------------------------------------------------------ +# UART ESC +if param greater UART_ESC_CONFIG 0 +then + modalai_esc -d /dev/ttyS1 start +fi + board_adc start # Start Digital power monitors diff --git a/src/drivers/uart_esc/modalai_esc/CMakeLists.txt b/src/drivers/uart_esc/modalai_esc/CMakeLists.txt new file mode 100644 index 000000000000..37458d97a4e7 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__uart_esc__modalai_esc + MAIN modalai_esc + SRCS + crc16.c + qc_esc_packet.c + modalai_esc_serial.cpp + modalai_esc.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/uart_esc/modalai_esc/crc16.c b/src/drivers/uart_esc/modalai_esc/crc16.c new file mode 100644 index 000000000000..78a7dddaece8 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/crc16.c @@ -0,0 +1,95 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +#include +#include "crc16.h" + +// Look-up table for fast CRC16 computations +const uint16_t crc16_table[256] = { + 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, + 0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, + 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40, + 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, + 0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, + 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41, + 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641, + 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, + 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, + 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, + 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, + 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, + 0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, + 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40, + 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, + 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, + 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240, + 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441, + 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, + 0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, + 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, + 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, + 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, + 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, + 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241, + 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, + 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, + 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, + 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, + 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, + 0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, + 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040, +}; + +uint16_t crc16_init() +{ + return 0xFFFF; +} + +uint16_t crc16_byte(uint16_t prev_crc, const uint8_t new_byte) +{ + uint8_t lut = (prev_crc ^ new_byte) & 0xFF; + return (prev_crc >> 8) ^ crc16_table[lut]; +} + +uint16_t crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length) +{ + uint16_t crc = prev_crc; + + while (input_length--) { + crc = crc16_byte(crc, *input_buffer++); + } + + return crc; +} diff --git a/src/drivers/uart_esc/modalai_esc/crc16.h b/src/drivers/uart_esc/modalai_esc/crc16.h new file mode 100644 index 000000000000..c1a308100d01 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/crc16.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains function prototypes for crc16 computations using polynomial 0x8005 + */ + +#ifndef CRC16_H_ +#define CRC16_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +// Returns the seed of crc output, which should be used when computing crc16 of a byte sequence +uint16_t crc16_init(void); + +// Process one byte by providing crc16 from previous step and new byte to consume. +// Output is the new crc16 value +uint16_t crc16_byte(uint16_t prev_crc, const uint8_t new_byte); + +// Process an array of bytes by providing crc16 from previous step (or seed), array of bytes and its length +// Output is the new crc16 value +uint16_t crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length); + +#ifdef __cplusplus +} +#endif + +#endif //CRC16_H_ diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp new file mode 100644 index 000000000000..0b6649206f2d --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -0,0 +1,919 @@ +/**************************************************************************** + * + * 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 "modalai_esc.hpp" +#include "modalai_esc_serial.hpp" +#include "qc_esc_packet.h" +#include "qc_esc_packet_types.h" + +#define MODALAI_ESC_DEVICE_PATH "/dev/uart_esc" +#define MODALAI_ESC_DEFAULT_PORT "/dev/ttyS1" + +const char *_device; + +ModalaiEsc::ModalaiEsc() : + CDev(MODALAI_ESC_DEVICE_PATH), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), + _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) +{ + _device = MODALAI_ESC_DEFAULT_PORT; + + _mixing_output.setAllFailsafeValues(0); + _mixing_output.setAllDisarmedValues(0); +} + +ModalaiEsc::~ModalaiEsc() +{ + _outputs_on = false; + + if (_uart_port) { + _uart_port->uart_close(); + _uart_port = nullptr; + } + + /* clean up the alternate device node */ + unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); + + perf_free(_cycle_perf); + perf_free(_output_update_perf); +} + +int ModalaiEsc::init() +{ + /* do regular cdev init */ + int ret = CDev::init(); + + if (ret != OK) { + return ret; + } + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + _class_instance = register_class_devname(MODALAI_ESC_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* lets not be too verbose */ + } else if (_class_instance < 0) { + PX4_ERR("FAILED registering class device"); + } + + _mixing_output.setDriverInstance(_class_instance); + + /* Getting initial parameter values */ + ret = update_params(); + + if (ret != OK) { + return ret; + } + + _uart_port = new ModalaiEscSerial(); + memset(&_esc_chans, 0x00, sizeof(_esc_chans)); + + ScheduleNow(); + + return 0; +} + +int ModalaiEsc::load_params(uart_esc_params_t *params, ch_assign_t *map) +{ + int ret = PX4_OK; + + param_get(param_find("UART_ESC_CONFIG"), ¶ms->config); + param_get(param_find("UART_ESC_BAUD"), ¶ms->baud_rate); + param_get(param_find("UART_ESC_MOTOR1"), ¶ms->motor_map[0]); + param_get(param_find("UART_ESC_MOTOR2"), ¶ms->motor_map[1]); + param_get(param_find("UART_ESC_MOTOR3"), ¶ms->motor_map[2]); + param_get(param_find("UART_ESC_MOTOR4"), ¶ms->motor_map[3]); + param_get(param_find("UART_ESC_RPM_MIN"), ¶ms->rpm_min); + param_get(param_find("UART_ESC_RPM_MAX"), ¶ms->rpm_max); + + if (params->rpm_min >= params->rpm_max) { + PX4_ERR("Invalid parameter UART_ESC_RPM_MIN. Please verify parameters."); + params->rpm_min = 0; + ret = PX4_ERROR; + } + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + if (params->motor_map[i] == MODALAI_ESC_OUTPUT_DISABLED || + params->motor_map[i] < -(MODALAI_ESC_OUTPUT_CHANNELS) || + params->motor_map[i] > MODALAI_ESC_OUTPUT_CHANNELS) { + PX4_ERR("Invalid parameter UART_ESC_MOTORX. Please verify parameters."); + params->motor_map[i] = 0; + ret = PX4_ERROR; + } + + /* Can map -4 to 4, 0 being disabled. Negative represents reverse direction */ + map[i].number = abs(params->motor_map[i]); + map[i].direction = (params->motor_map[i] > 0) ? 1 : -1; + } + + return ret; +} + +int ModalaiEsc::task_spawn(int argc, char *argv[]) +{ + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _device = argv[myoptind]; + break; + + default: + break; + } + } + + ModalaiEsc *instance = new ModalaiEsc(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ModalaiEsc::readResponse(Command *out_cmd) +{ + px4_usleep(_current_cmd.resp_delay_us); + + int res = _uart_port->uart_read(_current_cmd.buf, sizeof(_current_cmd.buf)); + + if (res > 0) { + if (parseResponse(_current_cmd.buf, res) < 0) { + PX4_ERR("Error parsing response"); + return -1; + } + + } else { + PX4_ERR("Read error: %i", res); + return -1; + } + + _current_cmd.response = false; + + return 0; +} + +int ModalaiEsc::parseResponse(uint8_t *buf, uint8_t len) +{ + if (len < 4 || buf[0] != ESC_PACKET_HEADER) { + return -1; + } + + switch (buf[2]) { + case ESC_PACKET_TYPE_VERSION_RESPONSE: + if (len != sizeof(QC_ESC_VERSION_INFO)) { + return -1; + + } else { + QC_ESC_VERSION_INFO ver; + memcpy(&ver, buf, len); + PX4_INFO("ESC ID: %i", ver.id); + PX4_INFO("HW Version: %i", ver.hw_version); + PX4_INFO("SW Version: %i", ver.sw_version); + PX4_INFO("Unique ID: %i", ver.unique_id); + } + + break; + + case ESC_PACKET_TYPE_FB_RESPONSE: + if (len != sizeof(QC_ESC_FB_RESPONSE)) { + return -1; + + } else { + QC_ESC_FB_RESPONSE fb; + memcpy(&fb, buf, len); + uint8_t id = (fb.state & 0xF0) >> 4; + + if (id < MODALAI_ESC_OUTPUT_CHANNELS) { + _esc_chans[id].rate_meas = fb.rpm; + _esc_chans[id].state = fb.state & 0x0F; + _esc_chans[id].cmd_counter = fb.cmd_counter; + _esc_chans[id].voltage = 9.0 + fb.voltage / 34.0; + } + } + + break; + + default: + return -1; + } + + return 0; +} + +int ModalaiEsc::sendCommandThreadSafe(Command *cmd) +{ + cmd->id = _cmd_id++; + _pending_cmd.store(cmd); + + /* wait until main thread processed it */ + while (_pending_cmd.load()) { + px4_usleep(1000); + } + + return 0; +} + + + +int ModalaiEsc::custom_command(int argc, char *argv[]) +{ + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + Command cmd; + uint8_t esc_id = 255; + uint8_t period = 0; + uint8_t duration = 0; + uint8_t power = 0; + uint16_t led_mask = 0; + int16_t rate = 0; + + if (argc < 3) { + return print_usage("unknown command"); + } + + const char *verb = argv[argc - 1]; + + /* start the FMU if not running */ + if (!strcmp(verb, "start")) { + if (!is_running()) { + return ModalaiEsc::task_spawn(argc, argv); + } + } + + if (!is_running()) { + PX4_INFO("Not running"); + return -1; + + } + + while ((ch = px4_getopt(argc, argv, "i:p:d:v:l:r:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'i': + esc_id = atoi(myoptarg); + break; + + case 'p': + period = atoi(myoptarg); + break; + + case 'd': + duration = atoi(myoptarg); + break; + + case 'v': + power = atoi(myoptarg); + break; + + case 'l': + led_mask = atoi(myoptarg); + break; + + case 'r': + rate = atoi(myoptarg); + break; + + default: + print_usage("Unknown command"); + return 0; + } + } + + if (!strcmp(verb, "reset")) { + if (esc_id < 3) { + PX4_INFO("Reset ESC: %i", esc_id); + cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = false; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC ID, use 0-3"); + return 0; + } + + } else if (!strcmp(verb, "version")) { + if (esc_id < 3) { + PX4_INFO("Request version for ESC: %i", esc_id); + cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = true; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC ID, use 0-3"); + return 0; + } + + } else if (!strcmp(verb, "tone")) { + if (0 < esc_id && esc_id < 16) { + PX4_INFO("Request tone for ESC mask: %i", esc_id); + cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); + cmd.response = false; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + + } else if (!strcmp(verb, "led")) { + if (led_mask <= 0x0FFF) { + get_instance()->_led_rsc.test = true; + get_instance()->_led_rsc.breath_en = false; + PX4_INFO("Request LED control for ESCs with mask: %i", led_mask); + + get_instance()->_esc_chans[0].led = (led_mask & 0x0007); + get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3; + get_instance()->_esc_chans[2].led = (led_mask & 0x01C0) >> 6; + get_instance()->_esc_chans[3].led = (led_mask & 0x0E00) >> 9; + return 0; + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + + } else if (!strcmp(verb, "rpm")) { + if (0 < esc_id && esc_id < 16) { + PX4_INFO("Request RPM for ESC bit mask: %i - RPM: %i", esc_id, rate); + int16_t rate_req[MODALAI_ESC_OUTPUT_CHANNELS]; + int16_t outputs[MODALAI_ESC_OUTPUT_CHANNELS]; + outputs[0] = (esc_id & 1) ? rate : 0; + outputs[1] = (esc_id & 2) ? rate : 0; + outputs[2] = (esc_id & 4) ? rate : 0; + outputs[3] = (esc_id & 8) ? rate : 0; + + uart_esc_params_t params; + ch_assign_t map[MODALAI_ESC_OUTPUT_CHANNELS]; + get_instance()->load_params(¶ms, (ch_assign_t *)&map); + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + int motor_idx = map[i].number; + + if (motor_idx > 0 && motor_idx <= MODALAI_ESC_OUTPUT_CHANNELS) { + /* user defined mapping is 1-4, array is 0-3 */ + motor_idx--; + rate_req[i] = outputs[motor_idx] * map[i].direction; + } + } + + cmd.len = qc_esc_create_rpm_packet4(rate_req[0], + rate_req[1], + rate_req[2], + rate_req[3], + 0, + 0, + 0, + 0, + cmd.buf, + sizeof(cmd.buf)); + cmd.response = false; + cmd.repeats = 500; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + + } else if (!strcmp(verb, "pwm")) { + if (0 < esc_id && esc_id < 16) { + PX4_INFO("Request PWM for ESC mask: %i - PWM: %i", esc_id, rate); + int16_t rate_req[MODALAI_ESC_OUTPUT_CHANNELS]; + int16_t outputs[MODALAI_ESC_OUTPUT_CHANNELS]; + outputs[0] = (esc_id & 1) ? rate : 0; + outputs[1] = (esc_id & 2) ? rate : 0; + outputs[2] = (esc_id & 4) ? rate : 0; + outputs[3] = (esc_id & 8) ? rate : 0; + + uart_esc_params_t params; + ch_assign_t map[MODALAI_ESC_OUTPUT_CHANNELS]; + get_instance()->load_params(¶ms, (ch_assign_t *)&map); + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + int motor_idx = map[i].number; + + if (motor_idx > 0 && motor_idx <= MODALAI_ESC_OUTPUT_CHANNELS) { + /* user defined mapping is 1-4, array is 0-3 */ + motor_idx--; + rate_req[i] = outputs[motor_idx] * map[i].direction; + } + } + + cmd.len = qc_esc_create_pwm_packet4(rate_req[0], + rate_req[1], + rate_req[2], + rate_req[3], + 0, + 0, + 0, + 0, + cmd.buf, + sizeof(cmd.buf)); + cmd.response = false; + cmd.repeats = 500; + return get_instance()->sendCommandThreadSafe(&cmd); + + } else { + print_usage("Invalid ESC mask, use 1-15"); + return 0; + } + } + + return print_usage("unknown command"); +} + +int ModalaiEsc::update_params() +{ + int ret = PX4_ERROR; + + updateParams(); + ret = load_params(&_parameters, (ch_assign_t *)&_output_map); + + if (ret == PX4_OK) { + _mixing_output.setAllMinValues(_parameters.rpm_min); + _mixing_output.setAllMaxValues(_parameters.rpm_max); + } + + return ret; +} + + +int ModalaiEsc::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + PX4_DEBUG("modalai_esc ioctl cmd: %d, arg: %ld", cmd, arg); + + switch (cmd) { + case PWM_SERVO_ARM: + PX4_INFO("PWM_SERVO_ARM"); + break; + + case PWM_SERVO_DISARM: + PX4_INFO("PWM_SERVO_DISARM"); + break; + + case MIXERIOCRESET: + _mixing_output.resetMixerThreadSafe(); + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + } + break; + + default: + ret = -ENOTTY; + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + + return ret; +} + +/* OutputModuleInterface */ +void ModalaiEsc::mixerChanged() +{ + /* + * This driver is only supporting 4 channel ESC + */ +} + + +void ModalaiEsc::updateLeds(vehicle_control_mode_s mode, led_control_s control) +{ + int i = 0; + uint8_t led_mask = _led_rsc.led_mask; + + if (_led_rsc.test) { + return; + } + + /* + * TODO - this is just a simple approach to get started. + * + */ + if (mode.timestamp != _led_rsc.mode.timestamp) { + _led_rsc.mode = mode; + } + + if (control.timestamp != _led_rsc.control.timestamp) { + _led_rsc.control = control; + + switch (_led_rsc.control.color) { + case led_control_s::COLOR_RED: + led_mask = QC_ESC_LED_RED_ON; + break; + + case led_control_s::COLOR_GREEN: + led_mask = QC_ESC_LED_GREEN_ON; + break; + + case led_control_s::COLOR_BLUE: + led_mask = QC_ESC_LED_BLUE_ON; + break; + + case led_control_s::COLOR_WHITE: + led_mask = QC_ESC_LED_RED_ON | QC_ESC_LED_GREEN_ON | QC_ESC_LED_BLUE_ON; + break; + + case led_control_s::COLOR_OFF: + led_mask = 0; + break; + } + + _led_rsc.breath_en = false; + + switch (_led_rsc.control.mode) { + case led_control_s::MODE_OFF: + break; + + case led_control_s::MODE_ON: + break; + + case led_control_s::MODE_DISABLED: + break; + + case led_control_s::MODE_BLINK_SLOW: + break; + + case led_control_s::MODE_BLINK_NORMAL: + break; + + case led_control_s::MODE_BLINK_FAST: + break; + + case led_control_s::MODE_BREATHE: + _led_rsc.breath_en = true; + _led_rsc.breath_counter = 0; + break; + + case led_control_s::MODE_FLASH: + break; + + default: + break; + } + + _led_rsc.led_mask = led_mask; + } + + if (_led_rsc.mode.flag_armed) { + led_mask = QC_ESC_LED_BLUE_ON; + + if (_led_rsc.mode.flag_control_position_enabled) { + led_mask = QC_ESC_LED_GREEN_ON; + + } else if (_led_rsc.mode.flag_control_offboard_enabled) { + led_mask = QC_ESC_LED_RED_ON; + } + + _led_rsc.led_mask = led_mask; + } + + if (_led_rsc.breath_en) { + /* 8 bit counter for a decent blink visual effect for + * 'breathing' use case + */ + if ((_led_rsc.breath_counter += 8) < 128) { + led_mask = 0; + } + } + + for (i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + _esc_chans[i].led = led_mask; + } +} + +/* OutputModuleInterface */ +bool ModalaiEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + if (num_outputs != MODALAI_ESC_OUTPUT_CHANNELS) { + return false; + } + + uint8_t motor_idx; + + /* round robin feedfback reqest while sending RPM requests */ + static int fb_idx = 0; + + for (int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { + if (!_outputs_on || stop_motors) { + _esc_chans[i].rate_req = 0; + + } else { + motor_idx = _output_map[i].number; + + if (motor_idx > 0 && motor_idx <= MODALAI_ESC_OUTPUT_CHANNELS) { + /* user defined mapping is 1-4, array is 0-3 */ + motor_idx--; + _esc_chans[i].rate_req = outputs[motor_idx] * _output_map[i].direction; + } + } + } + + Command cmd; + cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, + _esc_chans[1].rate_req, + _esc_chans[2].rate_req, + _esc_chans[3].rate_req, + _esc_chans[0].led, + _esc_chans[1].led, + _esc_chans[2].led, + _esc_chans[3].led, + fb_idx, + cmd.buf, + sizeof(cmd.buf)); + + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet"); + return false; + } + + if (fb_idx++ >= MODALAI_ESC_OUTPUT_CHANNELS) { + fb_idx = 0; + } + + /* + * Comparing this to SNAV, there wasn't a delay between reading + * feedback... without some delay on the PX4 side of things we + * can have some read failures. The update rate of this task + * is ~2000us, we can afford to delay a little here + */ + px4_usleep(MODALAI_ESC_WRITE_WAIT_US); + + memset(&cmd.buf, 0x00, sizeof(cmd.buf)); + + /* + * Here we parse the feedback response. Rarely the packet is mangled + * but this means we simply miss a feedback response and will come back + * around in roughly 8ms for another... so don't freak out and keep on + * trucking I say + */ + int res = _uart_port->uart_read(cmd.buf, sizeof(cmd.buf)); + + if (res > 0) { + parseResponse(cmd.buf, res); + } + + perf_count(_output_update_perf); + + return true; +} + + +void ModalaiEsc::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + /* Open serial port in this thread */ + if (!_uart_port->is_open()) { + if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) { + PX4_INFO("Opened UART ESC device"); + + } else { + PX4_ERR("Failed openening device"); + return; + } + } + + _mixing_output.update(); + + /* update output status if armed */ + _outputs_on = _mixing_output.armed().armed; + + /* check for parameter updates */ + if (!_outputs_on && _parameter_update_sub.updated()) { + /* clear update */ + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + /* update parameters from storage */ + update_params(); + } + + vehicle_control_mode_s vehicle_control_mode{}; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&vehicle_control_mode); + updateLeds(vehicle_control_mode, _led_rsc.control); + } + + led_control_s led_control{}; + + if (_led_update_sub.updated()) { + _led_update_sub.copy(&led_control); + updateLeds(_led_rsc.mode, led_control); + } + + /* breathing requires continuous updates */ + if (_led_rsc.breath_en) { + updateLeds(_led_rsc.mode, _led_rsc.control); + } + + /* Don't process commands if outputs on */ + if (!_outputs_on) { + if (_current_cmd.valid()) { + do { + if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { + if (_current_cmd.repeats == 0) { + _current_cmd.clear(); + } + + if (_current_cmd.response) { + readResponse(&_current_cmd); + } + + } else { + if (_current_cmd.retries == 0) { + _current_cmd.clear(); + PX4_ERR("Failed to send command, errno: %i", errno); + + } else { + _current_cmd.retries--; + PX4_ERR("Failed to send command, errno: %i", errno); + } + } + + px4_usleep(_current_cmd.repeat_delay_us); + } while (_current_cmd.repeats-- > 0); + + } else { + Command *new_cmd = _pending_cmd.load(); + + if (new_cmd) { + _current_cmd = *new_cmd; + _pending_cmd.store(nullptr); + } + } + } + + /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ + _mixing_output.updateSubscriptions(true); + + perf_end(_cycle_perf); +} + + +int ModalaiEsc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for... + +### Implementation +By default the module runs on a work queue with a callback on the uORB actuator_controls topic. + +### Examples +It is typically started with: +$ todo + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("modalai_esc", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Send reset request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("version", "Send version request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "ESC ID, 0-3", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("rpm", "Closed-Loop RPM test control request"); + PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 3, "RPM, -32,7680 to 32,768", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); + PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("tone", "Send tone generation request to ESC"); + PRINT_MODULE_USAGE_PARAM_INT('i', 1, 1, 15, "ESC ID bitmask, 1-15", false); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 255, "Period of sound, inverse frequency, 0-255", false); + PRINT_MODULE_USAGE_PARAM_INT('d', 0, 0, 255, "Duration of the sound, 0-255, 1LSB = 13ms", false); + PRINT_MODULE_USAGE_PARAM_INT('v', 0, 0, 100, "Power (volume) of sound, 0-100", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("led", "Send LED control request"); + PRINT_MODULE_USAGE_PARAM_INT('l', 0, 0, 4095, "Bitmask 0x0FFF (12 bits) - ESC0 (RGB) ESC1 (RGB) ESC2 (RGB) ESC3 (RGB)", false); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int ModalaiEsc::print_status() +{ + PX4_INFO("Max update rate: %i Hz", _current_update_rate); + PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); + PX4_INFO("UART port: %s", _device); + PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); + + PX4_INFO(""); + + PX4_INFO("Params: UART_ESC_CONFIG: %i", _parameters.config); + PX4_INFO("Params: UART_ESC_BAUD: %i", _parameters.baud_rate); + PX4_INFO("Params: UART_ESC_MOTOR1: %i", _parameters.motor_map[0]); + PX4_INFO("Params: UART_ESC_MOTOR2: %i", _parameters.motor_map[1]); + PX4_INFO("Params: UART_ESC_MOTOR3: %i", _parameters.motor_map[2]); + PX4_INFO("Params: UART_ESC_MOTOR4: %i", _parameters.motor_map[3]); + PX4_INFO("Params: UART_ESC_RPM_MIN: %i", _parameters.rpm_min); + PX4_INFO("Params: UART_ESC_RPM_MAX: %i", _parameters.rpm_max); + + PX4_INFO(""); + + for( int i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++){ + PX4_INFO("-- ID: %i", i); + PX4_INFO(" State: %i", _esc_chans[i].state); + PX4_INFO(" Requested: %i RPM", _esc_chans[i].rate_req); + PX4_INFO(" Measured: %i RPM", _esc_chans[i].rate_meas); + PX4_INFO(" Command Counter: %i", _esc_chans[i].cmd_counter); + PX4_INFO(" Voltage: %f VDC", _esc_chans[i].voltage); + PX4_INFO(""); + } + + perf_print_counter(_cycle_perf); + perf_print_counter(_output_update_perf); + + _mixing_output.printStatus(); + + return 0; +} + +extern "C" __EXPORT int modalai_esc_main(int argc, char *argv[]); + +int modalai_esc_main(int argc, char *argv[]) +{ + return ModalaiEsc::main(argc, argv); +} diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp new file mode 100644 index 000000000000..3affe8fa9824 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "modalai_esc_serial.hpp" + +class ModalaiEsc : public cdev::CDev, public ModuleBase, public OutputModuleInterface +{ +public: + ModalaiEsc(); + virtual ~ModalaiEsc(); + + /** @see ModuleBase */ + static int task_spawn(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; + + /** @see OutputModuleInterface */ + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + /** @see OutputModuleInterface */ + void mixerChanged() override; + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + virtual int init(); + + typedef enum { + UART_ESC_RESET, + UART_ESC_VERSION, + UART_ESC_TONE, + UART_ESC_LED + } uart_esc_cmd_t; + + struct Command { + uint16_t id = 0; + uint8_t BUF_SIZE = 255; + uint8_t buf[255] = {0x00}; + uint8_t len; + uint16_t repeats = 0; + uint16_t repeat_delay_us = 2000; + uint8_t retries = 0; + bool response = false; + uint16_t resp_delay_us = 1000; + + bool valid() const { return len > 0; } + void clear() { len = 0; } + }; + + int sendCommandThreadSafe(Command *cmd); + +private: + static constexpr uint32_t MODALAI_ESC_UART_CONFIG = 1; + static constexpr uint32_t MODALAI_ESC_DEFAULT_BAUD = 250000; + static constexpr uint16_t MODALAI_ESC_OUTPUT_CHANNELS = 4; + static constexpr uint16_t MODALAI_ESC_OUTPUT_DISABLED = 0; + + static constexpr uint32_t MODALAI_ESC_WRITE_WAIT_US = 200; + + static constexpr uint16_t DISARMED_VALUE = 0; + + static constexpr uint16_t MODALAI_ESC_PWM_MIN = 0; + static constexpr uint16_t MODALAI_ESC_PWM_MAX = 800; + static constexpr uint16_t MODALAI_ESC_DEFAULT_RPM_MIN = 5000; + static constexpr uint16_t MODALAI_ESC_DEFAULT_RPM_MAX = 17000; + + //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODALAI_ESC_PWM_MAX); } + //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODALAI_ESC_RPM_MAX); } + + ModalaiEscSerial *_uart_port; + + typedef struct { + int32_t config{MODALAI_ESC_UART_CONFIG}; + int32_t baud_rate{MODALAI_ESC_DEFAULT_BAUD}; + int32_t rpm_min{MODALAI_ESC_DEFAULT_RPM_MIN}; + int32_t rpm_max{MODALAI_ESC_DEFAULT_RPM_MAX}; + int32_t motor_map[MODALAI_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4}; + } uart_esc_params_t; + + struct EscChan { + uint16_t rate_req; + uint8_t state; + uint16_t rate_meas; + uint8_t led; + uint8_t cmd_counter; + double voltage; + }; + + typedef struct { + uint8_t number; + int8_t direction; + } ch_assign_t; + + typedef struct { + led_control_s control{}; + vehicle_control_mode_s mode{}; + uint8_t led_mask;// TODO led_mask[MODALAI_ESC_OUTPUT_CHANNELS]; + bool breath_en; + uint8_t breath_counter; + bool test; + } led_rsc_t; + + ch_assign_t _output_map[MODALAI_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; + MixingOutput _mixing_output{MODALAI_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + int _class_instance{-1}; + + perf_counter_t _cycle_perf; + perf_counter_t _output_update_perf; + + bool _outputs_on{false}; + + unsigned _current_update_rate{0}; + + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _led_update_sub{ORB_ID(led_control)}; + + uart_esc_params_t _parameters; + int update_params(); + int load_params(uart_esc_params_t *params, ch_assign_t *map); + + uint16_t _cmd_id{0}; + Command _current_cmd; + px4::atomic _pending_cmd{nullptr}; + + EscChan _esc_chans[MODALAI_ESC_OUTPUT_CHANNELS]; + Command _esc_cmd; + + led_rsc_t _led_rsc; + void updateLeds(vehicle_control_mode_s mode, led_control_s control); + + int populateCommand(uart_esc_cmd_t cmd_type, uint8_t cmd_mask, Command *out_cmd); + int readResponse(Command *out_cmd); + int parseResponse(uint8_t *buf, uint8_t len); +}; diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc_serial.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc_serial.cpp new file mode 100644 index 000000000000..665bbb1785da --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc_serial.cpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 "string.h" +#include "modalai_esc_serial.hpp" + +ModalaiEscSerial::ModalaiEscSerial() +{ +} + +ModalaiEscSerial::~ModalaiEscSerial() +{ + if (_uart_fd >= 0) { + uart_close(); + } +} + +int ModalaiEscSerial::uart_open(const char *dev, speed_t speed) +{ + if (_uart_fd >= 0) { + PX4_ERR("Port in use: %s (%i)", dev, errno); + return -1; + } + + /* Open UART */ + _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + + if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { + PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); + uart_close(); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &_cfg); + + /* Disable output post-processing */ + _cfg.c_oflag &= ~OPOST; + + _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ + _cfg.c_cflag &= ~CSIZE; + _cfg.c_cflag |= CS8; /* 8-bit characters */ + _cfg.c_cflag &= ~PARENB; /* no parity bit */ + _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ + _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ + + /* setup for non-canonical mode */ + _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + + if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { + PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); + uart_close(); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { + PX4_ERR("Error configuring port: %s (tcsetattr)", dev); + uart_close(); + return -1; + } + + return 0; +} + +int ModalaiEscSerial::uart_set_baud(speed_t speed) +{ + if (_uart_fd < 0) { + return -1; + } + + if (cfsetispeed(&_cfg, speed) < 0) { + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { + return -1; + } + + return 0; +} + +int ModalaiEscSerial::uart_close() +{ + if (_uart_fd < 0) { + PX4_ERR("invalid state for closing"); + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { + PX4_ERR("failed restoring uart to original state"); + } + + if (close(_uart_fd)) { + PX4_ERR("error closing uart"); + } + + _uart_fd = -1; + + return 0; +} + +int ModalaiEscSerial::uart_write(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + + return write(_uart_fd, buf, len); +} + +int ModalaiEscSerial::uart_read(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for reading or buffer"); + return -1; + } + + return read(_uart_fd, buf, len); +} diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc_serial.hpp b/src/drivers/uart_esc/modalai_esc/modalai_esc_serial.hpp new file mode 100644 index 000000000000..5d678e2de278 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc_serial.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + + +class ModalaiEscSerial +{ +public: + ModalaiEscSerial(); + virtual ~ModalaiEscSerial(); + + int uart_open(const char *dev, speed_t speed); + int uart_set_baud(speed_t speed); + int uart_close(); + int uart_write(FAR void *buf, size_t len); + int uart_read(FAR void *buf, size_t len); + bool is_open() { return _uart_fd >= 0; }; + +private: + int _uart_fd = -1; + struct termios _orig_cfg; + struct termios _cfg; +}; diff --git a/src/drivers/uart_esc/modalai_esc/qc_esc_packet.c b/src/drivers/uart_esc/modalai_esc/qc_esc_packet.c new file mode 100644 index 000000000000..47cc6f07cb30 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/qc_esc_packet.c @@ -0,0 +1,241 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +#include "crc16.h" +#include "qc_esc_packet.h" +#include "qc_esc_packet_types.h" + +#include + +int32_t qc_esc_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_packet(ESC_PACKET_TYPE_VERSION_REQUEST, &id, 1, out, out_size); +} + +int32_t qc_esc_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + char payload[] = "RESET0"; + payload[5] += id; + + return qc_esc_create_packet(ESC_PACKET_TYPE_RESET_CMD, (uint8_t *)payload, 6 /*sizeof(payload)*/, out, out_size); +} + + +int32_t qc_esc_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out, + uint16_t out_size) +{ + uint8_t data[4] = {frequency, duration, power, mask}; + return qc_esc_create_packet(ESC_PACKET_TYPE_SOUND_CMD, (uint8_t *) & (data[0]), 4, out, out_size); +} + +int32_t qc_esc_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size) +{ + uint8_t data[2] = {led_byte_1, led_byte_2}; + return qc_esc_create_packet(ESC_PACKET_TYPE_LED_CMD, (uint8_t *) & (data[0]), 2, out, out_size); +} + +int32_t qc_esc_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_packet(ESC_PACKET_TYPE_SET_ID_CMD, (uint8_t *)&id, 1, out, out_size); +} + +int32_t qc_esc_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_pwm_packet4_fb(pwm0, pwm1, pwm2, pwm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size) +{ + uint16_t data[5]; + uint16_t leds = 0; + + if (fb_id != -1) { fb_id = fb_id % 4; } + + //limit the pwm commands + + if (pwm0 > 800) { pwm0 = 800; } if (pwm0 < -800) { pwm0 = -800; } + + if (pwm1 > 800) { pwm1 = 800; } if (pwm1 < -800) { pwm1 = -800; } + + if (pwm2 > 800) { pwm2 = 800; } if (pwm2 < -800) { pwm2 = -800; } + + if (pwm3 > 800) { pwm3 = 800; } if (pwm3 < -800) { pwm3 = -800; } + + //least significant bit is used for feedback request + pwm0 &= ~(0x0001); pwm1 &= ~(0x0001); pwm2 &= ~(0x0001); pwm3 &= ~(0x0001); + + if (fb_id == 0) { pwm0 |= 0x0001; } if (fb_id == 1) { pwm1 |= 0x0001; } + + if (fb_id == 2) { pwm2 |= 0x0001; } if (fb_id == 3) { pwm3 |= 0x0001; } + + leds |= led0 & 0b00000111; + leds |= (led1 & 0b00000111) << 3; + leds |= ((uint16_t)(led2 & 0b00000111)) << 6; + leds |= ((uint16_t)(led3 & 0b00000111)) << 9; + + data[0] = pwm0; data[1] = pwm1; data[2] = pwm2; data[3] = pwm3; data[4] = leds; + return qc_esc_create_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + + +int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size) +{ + return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size) +{ + uint16_t data[5]; + uint16_t leds = 0; + + if (fb_id != -1) { fb_id = fb_id % 4; } + + //least significant bit is used for feedback request + rpm0 &= ~(0x0001); rpm1 &= ~(0x0001); rpm2 &= ~(0x0001); rpm3 &= ~(0x0001); + + if (fb_id == 0) { rpm0 |= 0x0001; } if (fb_id == 1) { rpm1 |= 0x0001; } + + if (fb_id == 2) { rpm2 |= 0x0001; } if (fb_id == 3) { rpm3 |= 0x0001; } + + leds |= led0 & 0b00000111; + leds |= (led1 & 0b00000111) << 3; + leds |= ((uint16_t)(led2 & 0b00000111)) << 6; + leds |= ((uint16_t)(led3 & 0b00000111)) << 9; + + data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; + return qc_esc_create_packet(ESC_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + +int32_t qc_esc_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) +{ + uint16_t packet_size = size + 5; + + if (packet_size > 255) { return -1; } + + if (out_size < packet_size) { return -2; } + + out[0] = 0xAF; + out[1] = packet_size; + out[2] = type; + + memcpy(&(out[3]), data, size); + + uint16_t crc = crc16_init(); + crc = crc16(crc, &(out[1]), packet_size - 3); + + memcpy(&(out[packet_size - 2]), &crc, sizeof(uint16_t)); + + return packet_size; +} + + + + +//feed in a character and see if we got a complete packet +int16_t qc_esc_packet_process_char(uint8_t c, EscPacket *packet) +{ + int16_t ret = ESC_NO_PACKET; + + uint16_t chk_comp; + uint16_t chk_rcvd; + + if (packet->len_received >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + } + + switch (packet->len_received) { + case 0: //header + packet->bp = packet->buffer; //reset the pointer for storing data + qc_esc_packet_checksum_reset(packet); //reset the checksum to starting value + + if (c != ESC_PACKET_HEADER) { //check the packet header + packet->len_received = 0; + ret = -1; + break; + } + + packet->len_received++; + *(packet->bp)++ = c; + break; + + case 1: //length + packet->len_received++; + *(packet->bp)++ = c; + packet->len_expected = c; + + if (packet->len_expected >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + ret = -1; + break; + } + + qc_esc_packet_checksum_process_char(packet, c); + break; + + default: //rest of the packet + packet->len_received++; + *(packet->bp)++ = c; + + if (packet->len_received < (packet->len_expected - 1)) { //do not compute checksum of checksum (last 2 bytes) + qc_esc_packet_checksum_process_char(packet, c); + } + + if (packet->len_received < packet->len_expected) { //waiting for more bytes + break; + } + + //grab the computed checksum and compare against the received value + chk_comp = qc_esc_packet_checksum_get(packet); + + memcpy(&chk_rcvd, packet->bp - 2, sizeof(uint16_t)); + + if (chk_comp == chk_rcvd) { ret = packet->len_received; } + + else { ret = ESC_ERROR_BAD_CHECKSUM; } + + packet->len_received = 0; + break; + } + + return ret; +} diff --git a/src/drivers/uart_esc/modalai_esc/qc_esc_packet.h b/src/drivers/uart_esc/modalai_esc/qc_esc_packet.h new file mode 100644 index 000000000000..91258ccf0c52 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/qc_esc_packet.h @@ -0,0 +1,240 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains function prototypes for Electronic Speed Controller (ESC) UART interface + */ + +#ifndef QC_ESC_PACKET +#define QC_ESC_PACKET + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "crc16.h" + +// Define byte values that correspond to setting red, greed, and blue LEDs +#define QC_ESC_LED_RED_ON 1 +#define QC_ESC_LED_GREEN_ON 2 +#define QC_ESC_LED_BLUE_ON 4 + + +// Header of the packet. Each packet must start with this header +#define ESC_PACKET_HEADER 0xAF + +enum { ESC_ERROR_BAD_CHECKSUM = -1, + ESC_NO_PACKET + }; + +// Defines for the constatnt offsets of different parts of the packet +enum { ESC_PACKET_POS_HEADER1 = 0, + ESC_PACKET_POS_LENGTH, + ESC_PACKET_POS_TYPE, + ESC_PACKET_POS_DATA + }; + +// Definition of the structure that holds the state of the incoming data that is being recived (i.e. incomplete packets) +typedef struct { + uint8_t len_received; // Number of chars received so far + uint8_t len_expected; // Expected number of chars based on header + uint8_t *bp; // Pointer to the next write position in the buffer + uint16_t crc; // Accumulated CRC value so far + uint8_t buffer[32]; // Buffer to hold incoming data that is being parsed +} EscPacket; + + +// Definition of the response packet from ESC, containing the ESC version information +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_VERSION_RESPONSE + + uint8_t id; // ID of the ESC that responded + uint16_t sw_version; // Software version of the ESC firmware + uint16_t hw_version; // HW version of the board + + uint32_t unique_id; // Unique ID of the ESC, if available + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_VERSION_INFO; + +// Definition of the feedbak response packet from ESC +typedef struct { + uint8_t header; + uint8_t length; // Total length of the packet + uint8_t type; // This will be equal to ESC_PACKET_TYPE_VERSION_RESPONSE + + uint8_t state; // bits 0:3 = state, bits 4:7 = ID + uint16_t rpm; // Current RPM of the motor + uint8_t cmd_counter; // Number of commands received by the ESC + uint8_t reserved0; + int8_t voltage; // Voltage = (-28)/34.0 + 9.0 = 8.176V. 0xE4 --> 228 (-28) + uint8_t reserved1; + + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_FB_RESPONSE; + + +//------------------------------------------------------------------------- +//Below are functions for generating packets that would be outgoing to ESCs +//------------------------------------------------------------------------- + +// Create a generic packet by providing all required components +// Inputs are packet type, input data array and its size, output array and maximum size of output array +// Resulting packet will be placed in the output data array together with appropriate header and checksum +// Output value represents total length of the created packet (if positive), otherwise error code +int32_t qc_esc_create_packet(uint8_t type, uint8_t *input_data, uint16_t input_size, uint8_t *out_data, + uint16_t out_data_size); + +// Create a packet for requesting version information from ESC with desired id +// If an ESC with this id is connected and receives this command, it will reply with it's version information +int32_t qc_esc_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for requesting an ESC with desired id to reset +// When ESC with the particular id receives this command, and it's not spinning, ESC will reset +// This is useful for entering bootloader without removing power from the system +int32_t qc_esc_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for generating a tone packet (signals are applied to motor to generate sounds) +// Inputs are relative frequency (0-255), relative duration (0-255), power (0-255) and bit mask for which ESCs should play a tone +// Bit mask definition: if bit i is set to 1, then ESC with ID=i will generate the tone +// Note that tones can only be generated when motor is not spinning +int32_t qc_esc_create_sound_packet(uint8_t frequency, uint8_t duration, uint8_t power, uint8_t mask, uint8_t *out, + uint16_t out_size); + +// Create a packet for standalone LED control +// Bit mask definition: +// led_byte_1 - bit0 = ESC0 Red, bit1 = ESC0, Green, bit2 = ESC0 Blue, bit3 = ESC1 Red, bit4 = ESC1 Green, +// bit5 = ESC1 Blue, bit6 = ESC2 Red, bit7 = ESC2 Green +// led_byte_2 - bit0 = ESC2 Blue, bit1 = ESC3 Red, bit2 = ESC3 Green, bit3 = ESC3 Blue, bits 4:7 = unused +// Note that control can only be sent when motor is not spinning +int32_t qc_esc_create_led_control_packet(uint8_t led_byte_1, uint8_t led_byte_2, uint8_t *out, uint16_t out_size); + +// Create a packet for setting the ID of an ESC +// Return value is the length of generated packet (if positive), otherwise error code +// Note that all ESCs that will receive this command will be set to this ID +int32_t qc_esc_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size); + +// Create a packet for sending open-loop command and LED command to 4 ESCs without requesting any feedback +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_pwm_packet4(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size); + +// Create a packet for sending open-loop command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, int16_t pwm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size); + +// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs without requesting any feedback +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + uint8_t *out, uint16_t out_size); + +// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) +// Return value is the length of generated packet (if positive), otherwise error code +int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, + uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, + int32_t fb_id, uint8_t *out, uint16_t out_size); + + +//------------------------------------------------------------------------- +// Below are functions for processing incoming packets +//------------------------------------------------------------------------- + + +// Feed one char and see if we have accumulated a complete packet +int16_t qc_esc_packet_process_char(uint8_t c, EscPacket *packet); + +// Get a pointer to the packet type from a pointer to EscPacket +static inline uint8_t qc_esc_packet_get_type(EscPacket *packet) { return packet->buffer[ESC_PACKET_POS_TYPE]; } + +// Get a pointer to the packet type from a uint8_t pointer that points to the raw packet data as it comes from UART port +static inline uint8_t qc_esc_packet_raw_get_type(uint8_t *packet) { return packet[ESC_PACKET_POS_TYPE]; } + +//get a pointer to the packet payload from a pointer to EscPacket +static inline uint8_t *qc_esc_packet_get_data_ptr(EscPacket *packet) { return &(packet->buffer[ESC_PACKET_POS_DATA]); } + +// Get a pointer to the packet payload from a uint8_t pointer that points to the raw packet data as it comes from UART port +static inline uint8_t *qc_esc_packet_raw_get_data_ptr(uint8_t *packet) { return &(packet[ESC_PACKET_POS_DATA]); } + +// Get the total size (length) in bytes of the packet +static inline uint8_t qc_esc_packet_get_size(EscPacket *packet) { return packet->buffer[ESC_PACKET_POS_LENGTH]; } + +// Get checksum of the packet from a pointer to EscPacket +static inline uint16_t qc_esc_packet_checksum_get(EscPacket *packet) { return packet->crc; } + +// Calculate the checksum of a data array. Used for packet generation / processing +static inline uint16_t qc_esc_packet_checksum(uint8_t *buf, uint16_t size) +{ + uint16_t crc = crc16_init(); + return crc16(crc, buf, size); +} + +// Reset the checksum of the incoming packet. Used internally for packet reception +static inline void qc_esc_packet_checksum_reset(EscPacket *packet) { packet->crc = crc16_init(); } + +// Process one character for checksum calculation while receiving a packet (used internally for packet reception) +static inline void qc_esc_packet_checksum_process_char(EscPacket *packet, uint8_t c) +{ + packet->crc = crc16_byte(packet->crc, c); +} + + +// Initialize an instance of an EscPacket. This should be called once before using an instance of EscPacket +static inline void qc_esc_packet_init(EscPacket *packet) +{ + packet->len_received = 0; + packet->len_expected = 0; + packet->bp = 0; + + qc_esc_packet_checksum_reset(packet); +} + +// Reset status of the packet that is being parsed. Effectively, this achieves the same thing as _packet_init +// so that _packet_init may be redundant +static inline void qc_esc_packet_reset(EscPacket *packet) +{ + packet->len_received = 0; +} + +#endif //QC_ESC_PACKET + +#ifdef __cplusplus +} +#endif diff --git a/src/drivers/uart_esc/modalai_esc/qc_esc_packet_types.h b/src/drivers/uart_esc/modalai_esc/qc_esc_packet_types.h new file mode 100644 index 000000000000..deb3b655ed85 --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/qc_esc_packet_types.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * Copyright (c) 2017 The Linux Foundation. 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 The Linux Foundation nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY + * THIS LICENSE. + * 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. + * In addition Supplemental Terms apply. See the SUPPLEMENTAL file. + * + ****************************************************************************/ + +/* + * This file contains the type values of all supported ESC UART packets + */ + +#ifndef QC_ESC_PACKET_TYPES +#define QC_ESC_PACKET_TYPES + +#define ESC_PACKET_TYPE_VERSION_REQUEST 0 +#define ESC_PACKET_TYPE_PWM_CMD 1 +#define ESC_PACKET_TYPE_RPM_CMD 2 +#define ESC_PACKET_TYPE_SOUND_CMD 3 +#define ESC_PACKET_TYPE_STEP_CMD 4 +#define ESC_PACKET_TYPE_LED_CMD 5 +#define ESC_PACKET_TYPE_RESET_CMD 10 +#define ESC_PACKET_TYPE_SET_ID_CMD 11 +#define ESC_PACKET_TYPE_SET_DIR_CMD 12 +#define ESC_PACKET_TYPE_CONFIG_BOARD_REQUEST 20 +#define ESC_PACKET_TYPE_CONFIG_USER_REQUEST 21 +#define ESC_PACKET_TYPE_CONFIG_UART_REQUEST 22 +#define ESC_PACKET_TYPE_CONFIG_TUNE_REQUEST 23 + +#define ESC_PACKET_TYPE_SET_FEEDBACK_MODE 50 //reserved for future use + +#define ESC_PACKET_TYPE_EEPROM_WRITE_UNLOCK 70 +#define ESC_PACKET_TYPE_EEPROM_READ_UNLOCK 71 +#define ESC_PACKET_TYPE_EEPROM_WRITE 72 + +#define ESC_PACKET_TYPE_VERSION_RESPONSE 109 +#define ESC_PACKET_TYPE_PARAMS 110 +#define ESC_PACKET_TYPE_BOARD_CONFIG 111 +#define ESC_PACKET_TYPE_USER_CONFIG 112 +#define ESC_PACKET_TYPE_UART_CONFIG 113 +#define ESC_PACKET_TYPE_TUNE_CONFIG 114 +#define ESC_PACKET_TYPE_FB_RESPONSE 128 + +#endif diff --git a/src/drivers/uart_esc/modalai_esc/uart_esc_params.c b/src/drivers/uart_esc/modalai_esc/uart_esc_params.c new file mode 100644 index 000000000000..666dcd4dd24e --- /dev/null +++ b/src/drivers/uart_esc/modalai_esc/uart_esc_params.c @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * UART ESC configuration + * + * Selects what type of UART ESC, if any, is being used. + * + * @reboot_required true + * + * @group UART ESC + * @value 0 - Disabled + * @value 1 - VOXL ESC + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0); + +/** + * UART ESC baud rate + * + * Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products. + * + * @group UART ESC + * @unit bits/sec + */ +PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); + +/** + * Motor mappings for ModalAI ESC M004 + * + * HW Channel Idexes (PX4 Indexes) (note: silkscreen shows 0 indexed) + * 4(1) 3(4) + * [front] + * 1(3) 2(2) + */ + +/** + * UART ESC Motor 1 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 3); + +/** + *UART ESC Motor 2 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); + +/** + * UART ESC Motor 3 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 4); + +/** + * UART ESC Motor 4 Mapping. 1-4 (negative for reversal). + * + * @group UART ESC + * @min -4 + * @max 4 + */ +PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 1); + +/** + * UART ESC RPM Min + * + * Minimum RPM for ESC + * + * @group UART ESC + * @unit RPM + */ +PARAM_DEFINE_INT32(UART_ESC_RPM_MIN, 5500); + +/** + * UART ESC RPM Max + * + * Maximum RPM for ESC + * + * @group UART ESC + * @unit RPM + */ +PARAM_DEFINE_INT32(UART_ESC_RPM_MAX, 15000); From 67419d24a9d5b343920efdf246d4c2364c335120 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Mon, 7 Feb 2022 15:54:56 -0500 Subject: [PATCH 02/39] Using standardized unit type. --- src/drivers/uart_esc/modalai_esc/uart_esc_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/uart_esc/modalai_esc/uart_esc_params.c b/src/drivers/uart_esc/modalai_esc/uart_esc_params.c index 666dcd4dd24e..ef4443671e64 100644 --- a/src/drivers/uart_esc/modalai_esc/uart_esc_params.c +++ b/src/drivers/uart_esc/modalai_esc/uart_esc_params.c @@ -52,7 +52,7 @@ PARAM_DEFINE_INT32(UART_ESC_CONFIG, 0); * Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products. * * @group UART ESC - * @unit bits/sec + * @unit bit/s */ PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); From e0c0738c53f59902ea03e33ca093d66124823a02 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Mon, 7 Feb 2022 15:55:10 -0500 Subject: [PATCH 03/39] Supporting RPM as a valid unit. --- src/lib/parameters/px4params/srcparser.py | 1 + validation/module_schema.yaml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index 7fadf9f7c3e2..b1986f0f8e59 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -362,6 +362,7 @@ def Validate(self): 'Ohm', 'V', 'us', 'ms', 's', 'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad', + 'rpm', 'RPM', 'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C', 'N/(m/s)', 'Nm/rad', 'Nm/(rad/s)', 'Nm', 'N', 'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD','']) diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index b9f772d16a9b..53542f748b1a 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -149,7 +149,8 @@ parameters: 'Ohm', 'V', 'us', 'ms', 's', 'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad', - 'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C', + 'rpm', 'RPM', + 'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C', 'N/(m/s)', 'Nm/(rad/s)', 'Nm', 'N', 'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD'] bit: From b69cddf3fc03d267930035cfc752ca347fd305b3 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 8 Feb 2022 09:10:05 -0500 Subject: [PATCH 04/39] Initial commit of (non-working) SITL target for ModalAI FC. --- boards/modalai/sitl/ctrlalloc.cmake | 122 +++++++++++++++++++++ boards/modalai/sitl/default.cmake | 121 ++++++++++++++++++++ boards/modalai/sitl/replay.cmake | 24 ++++ boards/modalai/sitl/src/CMakeLists.txt | 39 +++++++ boards/modalai/sitl/src/board_config.h | 60 ++++++++++ boards/modalai/sitl/src/board_shutdown.cpp | 59 ++++++++++ boards/modalai/sitl/src/i2c.cpp | 38 +++++++ boards/modalai/sitl/src/sitl_led.c | 81 ++++++++++++++ boards/modalai/sitl/src/spi.cpp | 38 +++++++ 9 files changed, 582 insertions(+) create mode 100644 boards/modalai/sitl/ctrlalloc.cmake create mode 100644 boards/modalai/sitl/default.cmake create mode 100644 boards/modalai/sitl/replay.cmake create mode 100644 boards/modalai/sitl/src/CMakeLists.txt create mode 100644 boards/modalai/sitl/src/board_config.h create mode 100644 boards/modalai/sitl/src/board_shutdown.cpp create mode 100644 boards/modalai/sitl/src/i2c.cpp create mode 100644 boards/modalai/sitl/src/sitl_led.c create mode 100644 boards/modalai/sitl/src/spi.cpp diff --git a/boards/modalai/sitl/ctrlalloc.cmake b/boards/modalai/sitl/ctrlalloc.cmake new file mode 100644 index 000000000000..45571cff0c9d --- /dev/null +++ b/boards/modalai/sitl/ctrlalloc.cmake @@ -0,0 +1,122 @@ + +px4_add_board( + PLATFORM posix + VENDOR px4 + MODEL sitl + ROMFSROOT px4fmu_common + LABEL ctrlalloc + TESTING + DRIVERS + #barometer # all available barometer drivers + #batt_smbus + camera_capture + camera_trigger + #differential_pressure # all available differential pressure drivers + #distance_sensor # all available distance sensor drivers + gps + #imu # all available imu drivers + #magnetometer # all available magnetometer drivers + #protocol_splitter + pwm_out_sim + rpm/rpm_simulator + #telemetry # all available telemetry drivers + tone_alarm + #uavcan + MODULES + airship_att_control + airspeed_selector + angular_velocity_controller + attitude_estimator_q + camera_feedback + commander + control_allocator + dataman + ekf2 + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + gyro_calibration + gyro_fft + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + replay + rover_pos_control + sensors + #sih + simulator + temperature_compensation + uuv_att_control + uuv_pos_control + vmount + vtol_att_control + SYSTEMCMDS + #dumpfile + dyn + esc_calib + failure + led_control + #mft + mixer + motor_ramp + motor_test + #mtd + #nshterm + param + perf + pwm + sd_bench + shutdown + system_time + tests # tests and test runner + #top + topic_listener + tune_control + uorb + ver + work_queue + EXAMPLES + dyn_hello # dynamically loading modules example + fake_gps + fake_imu + fake_magnetometer + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + #hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) + +set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") +set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger disable CACHE STRING "debugger for sitl") +set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") + +# If the environment variable 'replay' is defined, we are building with replay +# support. In this case, we enable the orb publisher rules. +set(REPLAY_FILE "$ENV{replay}") +if(REPLAY_FILE) + message(STATUS "Building with uorb publisher rules support") + add_definitions(-DORB_USE_PUBLISHER_RULES) + + message(STATUS "Building without lockstep for replay") + set(ENABLE_LOCKSTEP_SCHEDULER no) +else() + set(ENABLE_LOCKSTEP_SCHEDULER yes) +endif() diff --git a/boards/modalai/sitl/default.cmake b/boards/modalai/sitl/default.cmake new file mode 100644 index 000000000000..6bf5e1de3bfd --- /dev/null +++ b/boards/modalai/sitl/default.cmake @@ -0,0 +1,121 @@ + +px4_add_board( + PLATFORM posix + VENDOR px4 + MODEL sitl + ROMFSROOT px4fmu_common + LABEL default + TESTING + ETHERNET + DRIVERS + #barometer # all available barometer drivers + #batt_smbus + camera_capture + camera_trigger + #differential_pressure # all available differential pressure drivers + #distance_sensor # all available distance sensor drivers + gps + #imu # all available imu drivers + #magnetometer # all available magnetometer drivers + #protocol_splitter + pwm_out_sim + rpm/rpm_simulator + #telemetry # all available telemetry drivers + tone_alarm + #uavcan + MODULES + airship_att_control + airspeed_selector + attitude_estimator_q + camera_feedback + commander + dataman + ekf2 + events + flight_mode_manager + fw_att_control + fw_pos_control_l1 + gyro_calibration + gyro_fft + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + replay + rover_pos_control + sensors + #sih + simulator + temperature_compensation + uuv_att_control + uuv_pos_control + vmount + vtol_att_control + SYSTEMCMDS + #dumpfile + dyn + esc_calib + failure + led_control + #mft + mixer + motor_ramp + motor_test + #mtd + #nshterm + param + perf + pwm + sd_bench + shutdown + system_time + tests # tests and test runner + #top + topic_listener + tune_control + uorb + ver + work_queue + EXAMPLES + dyn_hello # dynamically loading modules example + fake_gps + fake_imu + fake_magnetometer + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + #hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) + +set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") +set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger disable CACHE STRING "debugger for sitl") +set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") + +# If the environment variable 'replay' is defined, we are building with replay +# support. In this case, we enable the orb publisher rules. +set(REPLAY_FILE "$ENV{replay}") +if(REPLAY_FILE) + message(STATUS "Building with uorb publisher rules support") + add_definitions(-DORB_USE_PUBLISHER_RULES) + + message(STATUS "Building without lockstep for replay") + set(ENABLE_LOCKSTEP_SCHEDULER no) +else() + set(ENABLE_LOCKSTEP_SCHEDULER yes) +endif() diff --git a/boards/modalai/sitl/replay.cmake b/boards/modalai/sitl/replay.cmake new file mode 100644 index 000000000000..fca852c46c6e --- /dev/null +++ b/boards/modalai/sitl/replay.cmake @@ -0,0 +1,24 @@ + +px4_add_board( + PLATFORM posix + VENDOR px4 + MODEL sitl + ROMFSROOT px4fmu_common + LABEL replay + MODULES + ekf2 + logger + replay + SYSTEMCMDS + param + perf + shutdown + topic_listener + ver + work_queue + ) + +message(STATUS "Building with uorb publisher rules support") +add_definitions(-DORB_USE_PUBLISHER_RULES) + +set(ENABLE_LOCKSTEP_SCHEDULER yes) diff --git a/boards/modalai/sitl/src/CMakeLists.txt b/boards/modalai/sitl/src/CMakeLists.txt new file mode 100644 index 000000000000..0286396097de --- /dev/null +++ b/boards/modalai/sitl/src/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2017 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. +# +############################################################################ + +add_library(drivers_board + board_shutdown.cpp + i2c.cpp + sitl_led.c + spi.cpp +) diff --git a/boards/modalai/sitl/src/board_config.h b/boards/modalai/sitl/src/board_config.h new file mode 100644 index 000000000000..09294c523288 --- /dev/null +++ b/boards/modalai/sitl/src/board_config.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 board_config.h + * + * SITL internal definitions + */ + +#pragma once + +#define BOARD_OVERRIDE_UUID "SIMULATIONID0000" // must be of length 16 +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_SITL + +#define BOARD_BATTERY1_V_DIV (10.177939394f) +#define BOARD_BATTERY1_A_PER_V (15.391030303f) + +#define BOARD_HAS_POWER_CONTROL 1 +#define CONFIG_BOARDCTL_POWEROFF 1 + +#define PX4_NUMBER_I2C_BUSES 1 + +#define BOARD_NUMBER_BRICKS 0 +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN + +#include +#include diff --git a/boards/modalai/sitl/src/board_shutdown.cpp b/boards/modalai/sitl/src/board_shutdown.cpp new file mode 100644 index 000000000000..6ed9329c844a --- /dev/null +++ b/boards/modalai/sitl/src/board_shutdown.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_shutdown.cpp + * + * sitl board shutdown backend. + */ + +#include +#include +#include + +#if defined(BOARD_HAS_POWER_CONTROL) +int board_register_power_state_notification_cb(power_button_state_notification_t cb) +{ + return 0; +} +#endif // BOARD_HAS_POWER_CONTROL + +#if defined(CONFIG_BOARDCTL_POWEROFF) +int board_power_off(int status) +{ + printf("Exiting NOW.\n"); + fflush(stdout); + system_exit(0); + return 0; +} +#endif // CONFIG_BOARDCTL_POWEROFF diff --git a/boards/modalai/sitl/src/i2c.cpp b/boards/modalai/sitl/src/i2c.cpp new file mode 100644 index 000000000000..bb1b3e5afd5f --- /dev/null +++ b/boards/modalai/sitl/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { +}; + diff --git a/boards/modalai/sitl/src/sitl_led.c b/boards/modalai/sitl/src/sitl_led.c new file mode 100644 index 000000000000..c9d3f4d05388 --- /dev/null +++ b/boards/modalai/sitl/src/sitl_led.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sitl_led.c + * + * sitl LED backend. + */ + +#include +#include +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static bool _led_state[2] = { false, false }; + +__EXPORT void led_init() +{ + PX4_DEBUG("LED_INIT"); +} + +__EXPORT void led_on(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_ON", led); + _led_state[led] = true; + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_OFF", led); + _led_state[led] = false; + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1 || led == 0) { + _led_state[led] = !_led_state[led]; + PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); + + } +} diff --git a/boards/modalai/sitl/src/spi.cpp b/boards/modalai/sitl/src/spi.cpp new file mode 100644 index 000000000000..ce3841e169a2 --- /dev/null +++ b/boards/modalai/sitl/src/spi.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { +}; From bfbebca416c4c8cd3cf8447d3f25df7590a568c8 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 8 Feb 2022 09:12:30 -0500 Subject: [PATCH 05/39] Updating vendor. --- boards/modalai/sitl/ctrlalloc.cmake | 2 +- boards/modalai/sitl/default.cmake | 2 +- boards/modalai/sitl/replay.cmake | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/boards/modalai/sitl/ctrlalloc.cmake b/boards/modalai/sitl/ctrlalloc.cmake index 45571cff0c9d..bd7ee929a4b8 100644 --- a/boards/modalai/sitl/ctrlalloc.cmake +++ b/boards/modalai/sitl/ctrlalloc.cmake @@ -1,7 +1,7 @@ px4_add_board( PLATFORM posix - VENDOR px4 + VENDOR modalai MODEL sitl ROMFSROOT px4fmu_common LABEL ctrlalloc diff --git a/boards/modalai/sitl/default.cmake b/boards/modalai/sitl/default.cmake index 6bf5e1de3bfd..6ad24cf7be02 100644 --- a/boards/modalai/sitl/default.cmake +++ b/boards/modalai/sitl/default.cmake @@ -1,7 +1,7 @@ px4_add_board( PLATFORM posix - VENDOR px4 + VENDOR modali MODEL sitl ROMFSROOT px4fmu_common LABEL default diff --git a/boards/modalai/sitl/replay.cmake b/boards/modalai/sitl/replay.cmake index fca852c46c6e..aa75870db1ee 100644 --- a/boards/modalai/sitl/replay.cmake +++ b/boards/modalai/sitl/replay.cmake @@ -1,7 +1,7 @@ px4_add_board( PLATFORM posix - VENDOR px4 + VENDOR modalai MODEL sitl ROMFSROOT px4fmu_common LABEL replay From a2948aa1e3695fbc058a410ed54c216818573035 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 8 Feb 2022 09:13:27 -0500 Subject: [PATCH 06/39] Removed unneeded target. --- boards/modalai/sitl/ctrlalloc.cmake | 122 ---------------------------- 1 file changed, 122 deletions(-) delete mode 100644 boards/modalai/sitl/ctrlalloc.cmake diff --git a/boards/modalai/sitl/ctrlalloc.cmake b/boards/modalai/sitl/ctrlalloc.cmake deleted file mode 100644 index bd7ee929a4b8..000000000000 --- a/boards/modalai/sitl/ctrlalloc.cmake +++ /dev/null @@ -1,122 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR modalai - MODEL sitl - ROMFSROOT px4fmu_common - LABEL ctrlalloc - TESTING - DRIVERS - #barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - #protocol_splitter - pwm_out_sim - rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - airship_att_control - airspeed_selector - angular_velocity_controller - attitude_estimator_q - camera_feedback - commander - control_allocator - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - replay - rover_pos_control - sensors - #sih - simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #dumpfile - dyn - esc_calib - failure - led_control - #mft - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_imu - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) - -set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") -set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") - -set(config_sitl_debugger disable CACHE STRING "debugger for sitl") -set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") - -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() From 89896fd5b9b93a23826531e73b82743948e88404 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 8 Feb 2022 10:54:42 -0500 Subject: [PATCH 07/39] Added stub model. --- ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb | 12 ++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + platforms/posix/cmake/sitl_target.cmake | 1 + 3 files changed, 14 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb new file mode 100644 index 000000000000..7bdbe1b2833a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor SITL +# +# @type Quadrotor Wide +# +# @maintainer Julian Oes +# + +. ${R}etc/init.d/rc.mc_defaults + +set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 660e68704c6b..64d1fc92366e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_romfs_files( 10016_iris 10017_iris_ctrlalloc 10018_iris_foggy_lidar + 10019_nhb 10020_if750a 10030_px4vision 1010_iris_opt_flow diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index f1ef21464247..9617a05d5142 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -115,6 +115,7 @@ set(models iris_opt_flow_mockup iris_rplidar iris_vision + nhb nxp_cupcar plane plane_cam From e177250c40feaf0e4ffbc4c9f070d75644840524 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 8 Feb 2022 11:54:48 -0500 Subject: [PATCH 08/39] Updating ModalAI SITL target. --- boards/modalai/sitl/default.cmake | 99 +++++++++++++++++++------------ 1 file changed, 60 insertions(+), 39 deletions(-) diff --git a/boards/modalai/sitl/default.cmake b/boards/modalai/sitl/default.cmake index 6ad24cf7be02..37f865cc47d2 100644 --- a/boards/modalai/sitl/default.cmake +++ b/boards/modalai/sitl/default.cmake @@ -3,34 +3,63 @@ px4_add_board( PLATFORM posix VENDOR modali MODEL sitl - ROMFSROOT px4fmu_common LABEL default + # TOOLCHAIN arm-none-eabi + # ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common TESTING ETHERNET + # UAVCAN_INTERFACES 1 + # SERIAL_PORTS + # GPS1:/dev/ttyS0 # UART1 / J10 + # TEL1:/dev/ttyS6 # UART7 / J5 + # TEL2:/dev/ttyS4 # UART5 / J1 + # TEL3:/dev/ttyS1 # USART2 / J4 DRIVERS - #barometer # all available barometer drivers - #batt_smbus + adc/ads1115 + # adc/board_adc + barometer # all available barometer drivers + batt_smbus camera_capture camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + # dshot gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - #protocol_splitter + # imu/bosch/bmi088 + imu/invensense/icm20602 + imu/invensense/icm20948 # required for ak09916 mag + imu/invensense/icm42688p + irlock + lights # all available light drivers + # magnetometer # all available magnetometer drivers + # optical_flow # all available optical flow drivers + osd + # pca9685 + pca9685_pwm_out + power_monitor/ina226 + power_monitor/voxlpm + # protocol_splitter + # pwm_input pwm_out_sim + # pwm_out + rc_input + roboclaw + rpm rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan + smart_battery/batmon + # safety_button + # telemetry # all available telemetry drivers + # uavcan + # uart_esc/modalai_esc MODULES - airship_att_control airspeed_selector attitude_estimator_q camera_feedback commander dataman ekf2 + esc_battery events flight_mode_manager fw_att_control @@ -47,42 +76,45 @@ px4_add_board( mc_hover_thrust_estimator mc_pos_control mc_rate_control - #micrortps_bridge + # micrortps_bridge navigator rc_update - replay rover_pos_control sensors - #sih - simulator + sih temperature_compensation uuv_att_control uuv_pos_control vmount vtol_att_control SYSTEMCMDS - #dumpfile - dyn + # bl_update + # dmesg + dumpfile esc_calib - failure + # gpio + # hardfault_log + # i2cdetect led_control - #mft + # mft mixer motor_ramp motor_test - #mtd - #nshterm + # mtd + # nshterm param perf pwm + # reboot + # reflect sd_bench - shutdown + # serial_test system_time - tests # tests and test runner - #top + top topic_listener tune_control uorb + # usb_connected ver work_queue EXAMPLES @@ -92,8 +124,8 @@ px4_add_board( fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello - #hwtest # Hardware test - #matlab_csv_serial + # hwtest # Hardware test + # matlab_csv_serial px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app @@ -107,15 +139,4 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") set(config_sitl_debugger disable CACHE STRING "debugger for sitl") set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() +set(ENABLE_LOCKSTEP_SCHEDULER yes) From e4d37a9c3adfbe47c1c7545f5b4feff824403500 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 8 Feb 2022 12:02:34 -0500 Subject: [PATCH 09/39] Added missing modules. --- boards/modalai/sitl/default.cmake | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/boards/modalai/sitl/default.cmake b/boards/modalai/sitl/default.cmake index 37f865cc47d2..40da0bd0d94b 100644 --- a/boards/modalai/sitl/default.cmake +++ b/boards/modalai/sitl/default.cmake @@ -78,10 +78,12 @@ px4_add_board( mc_rate_control # micrortps_bridge navigator + replay rc_update rover_pos_control sensors sih + simulator temperature_compensation uuv_att_control uuv_pos_control @@ -139,4 +141,15 @@ set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") set(config_sitl_debugger disable CACHE STRING "debugger for sitl") set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") -set(ENABLE_LOCKSTEP_SCHEDULER yes) +# If the environment variable 'replay' is defined, we are building with replay +# support. In this case, we enable the orb publisher rules. +set(REPLAY_FILE "$ENV{replay}") +if(REPLAY_FILE) + message(STATUS "Building with uorb publisher rules support") + add_definitions(-DORB_USE_PUBLISHER_RULES) + + message(STATUS "Building without lockstep for replay") + set(ENABLE_LOCKSTEP_SCHEDULER no) +else() + set(ENABLE_LOCKSTEP_SCHEDULER yes) +endif() From c7719a94c711c54a7575152d0a2da914a2ed70ec Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 9 Feb 2022 16:44:59 -0500 Subject: [PATCH 10/39] Added some default params. --- ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index 7bdbe1b2833a..a9b97852caca 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -9,4 +9,11 @@ . ${R}etc/init.d/rc.mc_defaults +# Battery Defaults +param set-default BAT_N_CELLS 6 +param set-default BAT1_N_CELLS 6 + +# Log everything +param set-default SDLOG_PROFILE 5 + set MIXER quad_w From ffd46490069daf58f2b9dbd7999354025183045a Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 10 Feb 2022 12:10:05 -0500 Subject: [PATCH 11/39] Initial commit of new simplified altitude flight mode. Building but untested. --- .../flight_mode_manager/CMakeLists.txt | 1 + .../flight_mode_manager/FlightModeManager.cpp | 4 +- .../ManualAltitudeCommandVel/CMakeLists.txt | 39 ++++ .../FlightTaskManualAltitudeCommandVel.cpp | 169 ++++++++++++++++++ .../FlightTaskManualAltitudeCommandVel.hpp | 90 ++++++++++ .../mc_pos_control/mc_pos_control_params.c | 2 + 6 files changed, 304 insertions(+), 1 deletion(-) create mode 100644 src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/CMakeLists.txt create mode 100644 src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp create mode 100644 src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 4b7cac1852e2..abedb2defe93 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -59,6 +59,7 @@ list(APPEND flight_tasks_all ManualAcceleration ManualAltitude ManualAltitudeSmoothVel + ManualAltitudeCommandVel ManualPosition ManualPositionSmoothVel Transition diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 422844b55292..7c533ed485a9 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -307,7 +307,9 @@ void FlightModeManager::start_flight_task() case 0: error = switchTask(FlightTaskIndex::ManualAltitude); break; - + case 1: + error = switchTask(FlightTaskIndex::ManualAltitudeCommandVel); + break; case 3: default: error = switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/CMakeLists.txt new file mode 100644 index 000000000000..edb14cb10803 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2018 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_library(FlightTaskManualAltitudeCommandVel + FlightTaskManualAltitudeCommandVel.cpp +) + +target_link_libraries(FlightTaskManualAltitudeCommandVel PUBLIC FlightTask FlightTaskUtility) +target_include_directories(FlightTaskManualAltitudeCommandVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp new file mode 100644 index 000000000000..79c5962cae47 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 FlightManualAltitude.cpp + */ + +#include "FlightTaskManualAltitudeCommandVel.hpp" +#include +#include +#include + +using namespace matrix; + +FlightTaskManualAltitudeCommandVel::FlightTaskManualAltitudeCommandVel() : + _sticks(this) +{} + +bool FlightTaskManualAltitudeCommandVel::updateInitialize() +{ + bool ret = FlightTask::updateInitialize(); + + _sticks.checkAndSetStickInputs(); + + if (_sticks_data_required) { + ret = ret && _sticks.isAvailable(); + } + + // in addition to manual require valid position and velocity in D-direction and valid yaw + return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); +} + +bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + _yaw_setpoint = NAN; + _yawspeed_setpoint = 0.f; + _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from velocity + _position_setpoint(2) = NAN; + _velocity_setpoint(2) = 0.f; + _setDefaultConstraints(); + + return ret; +} + +void FlightTaskManualAltitudeCommandVel::_scaleSticks() +{ + // Use stick input with deadzone, exponential curve and first order lpf for yawspeed + const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()); + _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target); + + // Use sticks input with deadzone and exponential curve for vertical velocity + const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; + _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); +} + +float FlightTaskManualAltitudeCommandVel::_applyYawspeedFilter(float yawspeed_target) +{ + const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f); + const float alpha = _deltatime / den; + _yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target; + return _yawspeed_filter_state; +} + +void FlightTaskManualAltitudeCommandVel::_rotateIntoHeadingFrame(Vector2f &v) +{ + float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; + Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); + v(0) = v_r(0); + v(1) = v_r(1); +} + +void FlightTaskManualAltitudeCommandVel::_updateHeadingSetpoints() +{ + /* + * A threshold larger than FLT_EPSILON is required because the + * _yawspeed_setpoint comes from an IIR filter and takes too much + * time to reach zero. + */ + if (fabsf(_yawspeed_setpoint) > 0.001f) { + // no fixed heading when rotating around yaw by stick + _yaw_setpoint = NAN; + } else { + // hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint = _yaw; + } + } +} + +void FlightTaskManualAltitudeCommandVel::_ekfResetHandlerHeading(float delta_psi) +{ + // Only reset the yaw setpoint when the heading is locked + if (PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint += delta_psi; + } +} + +void FlightTaskManualAltitudeCommandVel::_updateSetpoints() +{ + _updateHeadingSetpoints(); // get yaw setpoint + + // Thrust in xy are extracted directly from stick inputs. A magnitude of + // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no + // thrust along xy is demanded. The maximum thrust along xy depends on the thrust + // setpoint along z-direction, which is computed in PositionControl.cpp. + + Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0)); + + _man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get()); + _man_input_filter.update(sp); + sp = _man_input_filter.getState(); + _rotateIntoHeadingFrame(sp); + + if (sp.length() > 1.0f) { + sp.normalize(); + } + + // constrain acceleration + _acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; + // position is never set + _position_setpoint(2) = NAN; +} + +bool FlightTaskManualAltitudeCommandVel::_checkTakeoff() +{ + // stick is deflected above 65% throttle (throttle stick is in the range [-1,1]) + return _sticks.getPosition()(2) < -0.3f; +} + +bool FlightTaskManualAltitudeCommandVel::update() +{ + bool ret = FlightTask::update(); + _scaleSticks(); + _updateSetpoints(); + _constraints.want_takeoff = _checkTakeoff(); + + return ret; +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp new file mode 100644 index 000000000000..6977d662ce17 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 FlightManualAltitude.hpp + * + * Flight task for manual controlled altitude via velocity setpoints. + */ + +#pragma once + +#include "FlightTask.hpp" +#include "Sticks.hpp" +#include +#include + +class FlightTaskManualAltitudeCommandVel : public FlightTask +{ +public: + FlightTaskManualAltitudeCommandVel(); + virtual ~FlightTaskManualAltitudeCommandVel() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool updateInitialize() override; + bool update() override; + +protected: + void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ + void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ + virtual void _updateSetpoints(); /**< updates all setpoints */ + virtual void _scaleSticks(); /**< scales sticks to velocity in z */ + bool _checkTakeoff() override; + + /** + * rotates vector into local frame + */ + void _rotateIntoHeadingFrame(matrix::Vector2f &vec); + + Sticks _sticks; + bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _param_mc_man_tilt_tau + ) +private: + + /** + * Filter between stick input and yaw setpoint + * + * @param yawspeed_target yaw setpoint desired by the stick + * @return filtered value from independent filter state + */ + float _applyYawspeedFilter(float yawspeed_target); + + float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ + + AlphaFilter _man_input_filter; +}; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 46c5fb3946d1..935204a1e5e3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -693,11 +693,13 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); * The supported sub-modes are: * 0 Simple position control where sticks map directly to velocity setpoints * without smoothing. Useful for velocity control tuning. + * 1 Simple velocity control where sticks map directly to velocity setpoints. * 3 Smooth position control with maximum acceleration and jerk limits based on * jerk optimized trajectory generator (different algorithm than 1). * 4 Smooth position control where sticks map to acceleration and there's a virtual brake drag * * @value 0 Simple position control + * @value 1 Hold position with Velocity Input * @value 3 Smooth position control (Jerk optimized) * @value 4 Acceleration based input * @group Multicopter Position Control From 8ccd62451080d298ce70aadd380d45e5e7cdc847 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 10 Feb 2022 14:06:53 -0500 Subject: [PATCH 12/39] Updated default params to use new altitude mode. --- ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index a9b97852caca..00f6f627a36f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -15,5 +15,11 @@ param set-default BAT1_N_CELLS 6 # Log everything param set-default SDLOG_PROFILE 5 +param set-default SDLOG_MODE 2 + +# update control defaults +param set-default MPC_Z_VEL_MAX_UP 3 +param set-default MPC_Z_VEL_MAX_DN 3 +param set-default MPC_POS_MODE 1 set MIXER quad_w From 704f01b0fbec0fa4ed920deb7422b4d72d699495 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Fri, 11 Feb 2022 10:22:49 -0500 Subject: [PATCH 13/39] Added in a (steady state) bias for Z velocity. --- .../FlightTaskManualAltitudeCommandVel.cpp | 20 +++++++++++++++++-- .../FlightTaskManualAltitudeCommandVel.hpp | 5 ++++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp index 79c5962cae47..b074d6d01ad0 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp @@ -66,6 +66,7 @@ bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_s _yaw_setpoint = NAN; _yawspeed_setpoint = 0.f; _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from velocity + _last_position = _position; // initialize loop to assume we're stable _position_setpoint(2) = NAN; _velocity_setpoint(2) = 0.f; _setDefaultConstraints(); @@ -73,7 +74,7 @@ bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_s return ret; } -void FlightTaskManualAltitudeCommandVel::_scaleSticks() +void FlightTaskManualAltitudeCommandVel::_scaleSticks(const float dt) { // Use stick input with deadzone, exponential curve and first order lpf for yawspeed const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()); @@ -82,6 +83,12 @@ void FlightTaskManualAltitudeCommandVel::_scaleSticks() // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); + + // apply bias to overcome steady state errors + const float z_bias = math::constrain(_velocity(2) - (_position(2) - _last_position(2)) / dt, -0.5f, 0.5f); + _velocity_setpoint(2) -= z_bias; + + PX4_INFO("Applying Z_BIAS %f", (double)z_bias); } float FlightTaskManualAltitudeCommandVel::_applyYawspeedFilter(float yawspeed_target) @@ -160,10 +167,19 @@ bool FlightTaskManualAltitudeCommandVel::_checkTakeoff() bool FlightTaskManualAltitudeCommandVel::update() { + // get time delta since last loop + const hrt_abstime time_stamp_now = hrt_absolute_time(); + // Guard against too small (< 0.2ms) and too large (> 100ms) dt's. + const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f); + _time_stamp_last_loop = time_stamp_now; + bool ret = FlightTask::update(); - _scaleSticks(); + _scaleSticks(dt); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); + // update position estimate + _last_position = _position; + return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp index 6977d662ce17..17b962af0a45 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp @@ -57,7 +57,7 @@ class FlightTaskManualAltitudeCommandVel : public FlightTask void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ - virtual void _scaleSticks(); /**< scales sticks to velocity in z */ + virtual void _scaleSticks(const float dt); /**< scales sticks to velocity in z */ bool _checkTakeoff() override; /** @@ -86,5 +86,8 @@ class FlightTaskManualAltitudeCommandVel : public FlightTask float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ + hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration + matrix::Vector3f _last_position; /**< last loop's vehicle position */ + AlphaFilter _man_input_filter; }; From 803383a052846116d255cb2779e15941252f18cb Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Fri, 11 Feb 2022 10:23:16 -0500 Subject: [PATCH 14/39] Removed spam. --- .../FlightTaskManualAltitudeCommandVel.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp index b074d6d01ad0..ebadb0dacef9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp @@ -87,8 +87,6 @@ void FlightTaskManualAltitudeCommandVel::_scaleSticks(const float dt) // apply bias to overcome steady state errors const float z_bias = math::constrain(_velocity(2) - (_position(2) - _last_position(2)) / dt, -0.5f, 0.5f); _velocity_setpoint(2) -= z_bias; - - PX4_INFO("Applying Z_BIAS %f", (double)z_bias); } float FlightTaskManualAltitudeCommandVel::_applyYawspeedFilter(float yawspeed_target) From 750629515cbb6ecf2107401bd110a28d3255b4e5 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 17 Feb 2022 12:33:14 -0500 Subject: [PATCH 15/39] Modifications to correct for position tracking error. --- .../FlightTaskManualAltitudeCommandVel.cpp | 17 +++++++---------- .../FlightTaskManualAltitudeCommandVel.hpp | 4 ++-- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp index ebadb0dacef9..1565c4b6fb19 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp @@ -67,6 +67,7 @@ bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_s _yawspeed_setpoint = 0.f; _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from velocity _last_position = _position; // initialize loop to assume we're stable + _z_bias_lpf = 0; _position_setpoint(2) = NAN; _velocity_setpoint(2) = 0.f; _setDefaultConstraints(); @@ -74,7 +75,7 @@ bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_s return ret; } -void FlightTaskManualAltitudeCommandVel::_scaleSticks(const float dt) +void FlightTaskManualAltitudeCommandVel::_scaleSticks() { // Use stick input with deadzone, exponential curve and first order lpf for yawspeed const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()); @@ -85,8 +86,10 @@ void FlightTaskManualAltitudeCommandVel::_scaleSticks(const float dt) _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); // apply bias to overcome steady state errors - const float z_bias = math::constrain(_velocity(2) - (_position(2) - _last_position(2)) / dt, -0.5f, 0.5f); - _velocity_setpoint(2) -= z_bias; + const float z_bias = math::constrain(_velocity(2) - ((_position(2) - _last_position(2)) / _deltatime), -0.5f, 0.5f); + const float alpha = 0.1; + _z_bias_lpf = (_z_bias_lpf * (1 - alpha)) + (z_bias * alpha); + _velocity_setpoint(2) += _z_bias_lpf; } float FlightTaskManualAltitudeCommandVel::_applyYawspeedFilter(float yawspeed_target) @@ -165,14 +168,8 @@ bool FlightTaskManualAltitudeCommandVel::_checkTakeoff() bool FlightTaskManualAltitudeCommandVel::update() { - // get time delta since last loop - const hrt_abstime time_stamp_now = hrt_absolute_time(); - // Guard against too small (< 0.2ms) and too large (> 100ms) dt's. - const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f); - _time_stamp_last_loop = time_stamp_now; - bool ret = FlightTask::update(); - _scaleSticks(dt); + _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp index 17b962af0a45..d4d2dab421aa 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp @@ -57,7 +57,7 @@ class FlightTaskManualAltitudeCommandVel : public FlightTask void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ - virtual void _scaleSticks(const float dt); /**< scales sticks to velocity in z */ + virtual void _scaleSticks(); /**< scales sticks to velocity in z */ bool _checkTakeoff() override; /** @@ -86,8 +86,8 @@ class FlightTaskManualAltitudeCommandVel : public FlightTask float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ - hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration matrix::Vector3f _last_position; /**< last loop's vehicle position */ + float _z_bias_lpf; /**< low passed version of z vel bias */ AlphaFilter _man_input_filter; }; From 3c266a4ec4b9d4954423bd3cc62f93d5ef59f759 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 22 Feb 2022 10:52:22 -0500 Subject: [PATCH 16/39] Removed attitude setpoints for simplified Altitude control. --- .../FlightTaskManualAltitudeCommandVel.cpp | 30 +------------------ .../FlightTaskManualAltitudeCommandVel.hpp | 2 -- 2 files changed, 1 insertion(+), 31 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp index 1565c4b6fb19..b20ad6c1e04a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp @@ -63,7 +63,7 @@ bool FlightTaskManualAltitudeCommandVel::updateInitialize() bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); - _yaw_setpoint = NAN; + _yaw_setpoint = NAN; // always use stick commands (or 0) _yawspeed_setpoint = 0.f; _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from velocity _last_position = _position; // initialize loop to assume we're stable @@ -108,36 +108,8 @@ void FlightTaskManualAltitudeCommandVel::_rotateIntoHeadingFrame(Vector2f &v) v(1) = v_r(1); } -void FlightTaskManualAltitudeCommandVel::_updateHeadingSetpoints() -{ - /* - * A threshold larger than FLT_EPSILON is required because the - * _yawspeed_setpoint comes from an IIR filter and takes too much - * time to reach zero. - */ - if (fabsf(_yawspeed_setpoint) > 0.001f) { - // no fixed heading when rotating around yaw by stick - _yaw_setpoint = NAN; - } else { - // hold the current heading when no more rotation commanded - if (!PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - } - } -} - -void FlightTaskManualAltitudeCommandVel::_ekfResetHandlerHeading(float delta_psi) -{ - // Only reset the yaw setpoint when the heading is locked - if (PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint += delta_psi; - } -} - void FlightTaskManualAltitudeCommandVel::_updateSetpoints() { - _updateHeadingSetpoints(); // get yaw setpoint - // Thrust in xy are extracted directly from stick inputs. A magnitude of // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no // thrust along xy is demanded. The maximum thrust along xy depends on the thrust diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp index d4d2dab421aa..4088bb134097 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp @@ -54,8 +54,6 @@ class FlightTaskManualAltitudeCommandVel : public FlightTask bool update() override; protected: - void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ - void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ bool _checkTakeoff() override; From 6a24800ac895ef56227a8f3a914e278712ca959e Mon Sep 17 00:00:00 2001 From: gvymr Date: Tue, 22 Feb 2022 14:21:02 -0500 Subject: [PATCH 17/39] Commented out several drivers in modalai sitl so it would compile (MacOS clang). --- boards/modalai/sitl/default.cmake | 36 +++++++++++++++---------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/boards/modalai/sitl/default.cmake b/boards/modalai/sitl/default.cmake index 40da0bd0d94b..af99b00800c7 100644 --- a/boards/modalai/sitl/default.cmake +++ b/boards/modalai/sitl/default.cmake @@ -16,38 +16,38 @@ px4_add_board( # TEL2:/dev/ttyS4 # UART5 / J1 # TEL3:/dev/ttyS1 # USART2 / J4 DRIVERS - adc/ads1115 + #adc/ads1115 # adc/board_adc - barometer # all available barometer drivers - batt_smbus + #barometer # all available barometer drivers + #batt_smbus camera_capture camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers + #differential_pressure # all available differential pressure drivers + #distance_sensor # all available distance sensor drivers # dshot gps # imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20948 # required for ak09916 mag - imu/invensense/icm42688p - irlock - lights # all available light drivers + #imu/invensense/icm20602 + #imu/invensense/icm20948 # required for ak09916 mag + #imu/invensense/icm42688p + #irlock + #lights # all available light drivers # magnetometer # all available magnetometer drivers # optical_flow # all available optical flow drivers - osd + #osd # pca9685 - pca9685_pwm_out - power_monitor/ina226 - power_monitor/voxlpm + #pca9685_pwm_out + #power_monitor/ina226 + #power_monitor/voxlpm # protocol_splitter # pwm_input pwm_out_sim # pwm_out - rc_input - roboclaw - rpm + #rc_input + #roboclaw + #rpm rpm/rpm_simulator - smart_battery/batmon + #smart_battery/batmon # safety_button # telemetry # all available telemetry drivers # uavcan From f5dd1dd81b12deb375ea9ff81c51d47b658b870e Mon Sep 17 00:00:00 2001 From: gvymr Date: Tue, 22 Feb 2022 16:18:32 -0500 Subject: [PATCH 18/39] Added param defaults to nhb sitl to match real world more closely --- .../px4fmu_common/init.d-posix/airframes/10019_nhb | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index 00f6f627a36f..7edd3b8e86fb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -16,10 +16,19 @@ param set-default BAT1_N_CELLS 6 # Log everything param set-default SDLOG_PROFILE 5 param set-default SDLOG_MODE 2 +param set-default COM_ARM_SDCARD 1 # update control defaults -param set-default MPC_Z_VEL_MAX_UP 3 -param set-default MPC_Z_VEL_MAX_DN 3 +param set-default MPC_Z_VEL_MAX_UP 1.5 +param set-default MPC_Z_VEL_MAX_DN 1.5 param set-default MPC_POS_MODE 1 +param set-default MPC_MAN_TILT_MAX 20 +param set-default MPC_THR_HOVER 0.07 + +# misc. +param set-default COM_ARM_CHK_ESCS 0 +param set-default MC_AIRMODE 0 # can consider using '1' for roll/pitch. Don't use '2' b/c yaw has issues. +param set-default EKF2_GND_EFF_DZ 4 # this is default but we initially had it set to 0. Should we go back to 0? +#param set-default CAL_MAG0_ROT 6 # YAW 270 for yhid pixhawk/holybro board set MIXER quad_w From 27818cc4f8b0bab27ae9b614c9aa2103bd2cf0c6 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Mon, 28 Mar 2022 09:59:32 -0400 Subject: [PATCH 19/39] Added a new param to land_detector that sets 'maybe_landed' to always True. --- src/modules/land_detector/LandDetector.cpp | 2 +- src/modules/land_detector/LandDetector.h | 3 ++- src/modules/land_detector/land_detector_params.c | 15 +++++++++++++++ 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 34e0f49fdd10..a69d7ec47091 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -120,7 +120,7 @@ void LandDetector::Run() const bool freefallDetected = _freefall_hysteresis.get_state(); const bool ground_contactDetected = _ground_contact_hysteresis.get_state(); - const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state(); + const bool maybe_landedDetected = (_param_flight_uncertain.get()) ? true : _maybe_landed_hysteresis.get_state(); const bool landDetected = _landed_hysteresis.get_state(); const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : (float)INFINITY; const bool in_ground_effect = _ground_effect_hysteresis.get_state(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 09b502dfa979..d389dc93568e 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -188,7 +188,8 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul DEFINE_PARAMETERS_CUSTOM_PARENT( ModuleParams, (ParamInt) _param_total_flight_time_high, - (ParamInt) _param_total_flight_time_low + (ParamInt) _param_total_flight_time_low, + (ParamBool) _param_flight_uncertain ); }; diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 950956a92d91..87331ed55cb4 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -58,3 +58,18 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_HI, 0); * */ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0); + +/** + * Assume land detection is uncertain, e.g. to allow arming / disarming. + * + * This is a roundabout what of allowing arming and disarming checks + * to ignore landing status, and to ensure we don't disarm when landed. + * + * @min 0 + * @max 1 + * @volatile + * @category system + * @group Land Detector + * + */ +PARAM_DEFINE_INT32(LND_FLIGHT_UNCRT, 0); From e2f5b1f59cf71b53d511b08d85591d80fdae0040 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Mon, 28 Mar 2022 10:03:28 -0400 Subject: [PATCH 20/39] Modified some commander arm / disarm checks to use 'maybe_landed' instead of 'landed'. --- src/modules/commander/Commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1cf60e82baf0..3ac3c0312b25 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1886,7 +1886,7 @@ Commander::run() // prevent disarming via safety button if not landed if (hrt_elapsed_time(&_land_detector.timestamp) < 10_s) { - if (!_land_detector.landed) { + if (!_land_detector.maybe_landed) { safety_disarm_allowed = false; } } @@ -2265,11 +2265,11 @@ Commander::run() const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF); if (rc_arming_enabled) { - if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { + if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.maybe_landed)) { disarm(arm_disarm_reason_t::RC_STICK); } - if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) { + if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.maybe_landed)) { if (_vehicle_control_mode.flag_control_manual_enabled) { arm(arm_disarm_reason_t::RC_STICK); From 9e33d8a18a8895d4872501ba6c2c6e7c4114a33e Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Mon, 28 Mar 2022 10:16:11 -0400 Subject: [PATCH 21/39] Added a new param to Commander to bypass pre-flight checks. --- src/modules/commander/Commander.cpp | 3 ++- src/modules/commander/Commander.hpp | 1 + src/modules/commander/commander_params.c | 10 ++++++++++ 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3ac3c0312b25..0afca03a6a5c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -502,11 +502,12 @@ static constexpr const char *main_state_str(uint8_t main_state) transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) { // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming - if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + if (!_param_preflt_checks_required.get() || (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) { run_preflight_checks = false; } if (run_preflight_checks) { + _manual_control.update(); if (_vehicle_control_mode.flag_control_manual_enabled) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center"); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 0c14b2f75527..18c6e149c5b4 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -243,6 +243,7 @@ class Commander : public ModuleBase, public ModuleParams (ParamBool) _param_arm_mission_required, (ParamBool) _param_arm_auth_required, (ParamBool) _param_escs_checks_required, + (ParamBool) _param_preflt_checks_required, (ParamBool) _param_com_rearm_grace, (ParamInt) _param_flight_uuid, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ebc0589bbc80..df4b87f8c046 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -925,6 +925,16 @@ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0); */ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0); +/** + * Enable pre-flight checks. + * + * If this parameter is set, the system will perform pre-flight checks. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_CHK_PREF, 1); + /** * Condition to enter prearmed mode * From ef0fa8644a5b99a508d033f94a8a8dea04235f6b Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Mon, 28 Mar 2022 10:16:38 -0400 Subject: [PATCH 22/39] Updated default params used in Sim to minimize ARM/DISARM logic. --- ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index 7edd3b8e86fb..91f5475de926 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -27,8 +27,12 @@ param set-default MPC_THR_HOVER 0.07 # misc. param set-default COM_ARM_CHK_ESCS 0 -param set-default MC_AIRMODE 0 # can consider using '1' for roll/pitch. Don't use '2' b/c yaw has issues. -param set-default EKF2_GND_EFF_DZ 4 # this is default but we initially had it set to 0. Should we go back to 0? -#param set-default CAL_MAG0_ROT 6 # YAW 270 for yhid pixhawk/holybro board +#param set-default COM_ARM_CHK_PREF 0 +param set-default MC_AIRMODE 0 # can consider using '1' for roll/pitch. Don't use '2' b/c yaw has issues. +param set-default EKF2_GND_EFF_DZ 4 # this is default but we initially had it set to 0. Should we go back to 0? +#param set-default CAL_MAG0_ROT 6 # YAW 270 for yhid pixhawk/holybro board +param set-default LND_FLIGHT_UNCRT 1 # don't trust the landing detector for arm / disarm checks. +param set-default COM_DISARM_PRFLT -1.0 # don't automatically disarm if preflight checks fail +param set-default COM_DISARM_LAND -1.0 # don't automatically disarm when landed set MIXER quad_w From 93d4f02dffb871c28504e4299e5b5b4cfbbb1b6d Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 29 Mar 2022 07:41:57 -0400 Subject: [PATCH 23/39] Added two new params to selectively enable / disable various pre-flight checks. --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 31 ++++++++++++------- src/modules/commander/commander_params.c | 20 ++++++++++++ 2 files changed, 40 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 80abab1304c5..7a2f7d8bc651 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -100,19 +100,25 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- ACCEL ---- */ { - /* check all sensors individually, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_accel_count; i++) { - const bool required = (i < max_mandatory_accel_count); - bool report_fail = report_failures; - int32_t device_id = -1; + int32_t check_accelerometer = 1; + param_get(param_find("COM_ARM_CHK_ACC"), &check_accelerometer); - if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { - if (required) { - failed = true; - } + if (check_accelerometer != 0) { + /* check all sensors individually, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_accel_count; i++) { + const bool required = (i < max_mandatory_accel_count); + bool report_fail = report_failures; - report_fail = false; // only report the first failure + int32_t device_id = -1; + + if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + if (required) { + failed = true; + } + + report_fail = false; // only report the first failure + } } } @@ -236,9 +242,12 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } if (estimator_type == 2) { + // check if we care about sensor biases during arming + int32_t check_sensor_biases = 1; + param_get(param_find("COM_ARM_CHK_BIAS"), &check_sensor_biases); const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) && - ekf2CheckSensorBias(mavlink_log_pub, report_failures); + (check_sensor_biases == 1) ? ekf2CheckSensorBias(mavlink_log_pub, report_failures) : true; // For the first 10 seconds the ekf2 can be unhealthy, and we just mark it // as not present. diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index df4b87f8c046..5fb46beb071b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -935,6 +935,26 @@ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0); */ PARAM_DEFINE_INT32(COM_ARM_CHK_PREF, 1); +/** + * Enable EKF2 sensor bias arming check. + * + * If this parameter is set, the system will disallow arming if sensor biases are large. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_CHK_BIAS, 1); + +/** + * Enable accelerometer status checks for arming. + * + * If this parameter is set, the system will disallow arming if accelerometer ranges are outside bounds. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_CHK_ACC, 1); + /** * Condition to enter prearmed mode * From aaad4ea1e203543ae1927adf374e7776de72465e Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 29 Mar 2022 07:42:11 -0400 Subject: [PATCH 24/39] Disabling select pre-flight checks. --- ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index 91f5475de926..708c29f14e55 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -27,6 +27,8 @@ param set-default MPC_THR_HOVER 0.07 # misc. param set-default COM_ARM_CHK_ESCS 0 +param set-default COM_ARM_CHK_ACC 0 +param set-default COM_ARM_CHK_BIAS 0 #param set-default COM_ARM_CHK_PREF 0 param set-default MC_AIRMODE 0 # can consider using '1' for roll/pitch. Don't use '2' b/c yaw has issues. param set-default EKF2_GND_EFF_DZ 4 # this is default but we initially had it set to 0. Should we go back to 0? From 9f234ad00bd1e5644b572af6c097f7a96fd2942d Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 29 Mar 2022 10:50:05 -0400 Subject: [PATCH 25/39] Disabling Roll/Pitch angle checks for arming (and some cleanup). --- .../init.d-posix/airframes/10019_nhb | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index 708c29f14e55..a56cda093898 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -25,16 +25,20 @@ param set-default MPC_POS_MODE 1 param set-default MPC_MAN_TILT_MAX 20 param set-default MPC_THR_HOVER 0.07 +# selective disabling of various arming / disarming checks +param set-default FD_FAIL_R 0 # allow arming at any Roll angle +param set-default FD_FAIL_P 0 # allow arming at any Pitch angle +param set-default COM_ARM_CHK_ESCS 0 # ignore ESCS arming check +param set-default COM_ARM_CHK_ACC 0 # ignore Accelerometer arming checking +param set-default COM_ARM_CHK_BIAS 0 # ignore EKF2 sensor bias arming check +param set-default COM_DISARM_PRFLT -1.0 # don't automatically disarm if preflight checks fail +param set-default COM_DISARM_LAND -1.0 # don't automatically disarm when landed +#param set-default COM_ARM_CHK_PREF 0 # disable all pre-flight arm checks + # misc. -param set-default COM_ARM_CHK_ESCS 0 -param set-default COM_ARM_CHK_ACC 0 -param set-default COM_ARM_CHK_BIAS 0 -#param set-default COM_ARM_CHK_PREF 0 param set-default MC_AIRMODE 0 # can consider using '1' for roll/pitch. Don't use '2' b/c yaw has issues. param set-default EKF2_GND_EFF_DZ 4 # this is default but we initially had it set to 0. Should we go back to 0? #param set-default CAL_MAG0_ROT 6 # YAW 270 for yhid pixhawk/holybro board param set-default LND_FLIGHT_UNCRT 1 # don't trust the landing detector for arm / disarm checks. -param set-default COM_DISARM_PRFLT -1.0 # don't automatically disarm if preflight checks fail -param set-default COM_DISARM_LAND -1.0 # don't automatically disarm when landed set MIXER quad_w From 93ac1c333182449d0b0ed98925860f0fe0397145 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 5 Apr 2022 10:03:27 -0400 Subject: [PATCH 26/39] Some (heavy-handed) changes to ensure we always try to switch to the RC commanded flight mode. --- src/modules/commander/Commander.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0afca03a6a5c..99ef1be0be66 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2334,13 +2334,13 @@ Commander::run() _landing_gear_pub.publish(landing_gear); } } + } - // evaluate the main state machine according to mode switches - if (set_main_state(_status_changed) == TRANSITION_CHANGED) { - // play tune on mode change only if armed, blink LED always - tune_positive(_armed.armed); - _status_changed = true; - } + // evaluate the main state machine according to mode switches + if (set_main_state(_status_changed) == TRANSITION_CHANGED) { + // play tune on mode change only if armed, blink LED always + tune_positive(_armed.armed); + _status_changed = true; } /* check throttle kill switch */ @@ -2916,9 +2916,11 @@ transition_result_t Commander::set_main_state(bool &changed) { if (_safety.override_available && _safety.override_enabled) { + PX4_INFO("Overriding main state for safety."); return set_main_state_override_on(changed); } else { + PX4_INFO("Setting main state to RC commanded."); return set_main_state_rc(); } } @@ -2986,6 +2988,7 @@ Commander::set_main_state_rc() return TRANSITION_NOT_CHANGED; } + const auto _penultimate_manual_control_switches = _last_manual_control_switches; _last_manual_control_switches = _manual_control_switches; // reset the position and velocity validity calculation to give the best change of being able to select @@ -3134,6 +3137,9 @@ Commander::set_main_state_rc() } + // check if we've transitioned; if not, keep around our previous state to allow the next iteration to try + if (res == TRANSITION_NOT_CHANGED) + _last_manual_control_switches = _penultimate_manual_control_switches; return res; } From c47ae283c1f5573819857d51ff1990f9952a35fb Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 5 Apr 2022 10:24:08 -0400 Subject: [PATCH 27/39] More changes to effect desired flight mode on startup. --- src/modules/commander/Commander.cpp | 13 ++++++------- src/modules/commander/state_machine_helper.h | 3 ++- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 99ef1be0be66..7de6f5d12080 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -639,6 +639,10 @@ Commander::try_mode_change(main_state_t desired_mode) desired_mode = commander_state_s::MAIN_STATE_MANUAL; res = main_state_transition(_status, desired_mode, _status_flags, _internal_state); } + + // indicate that we're changing to something other than our originally desired mode + if (res == TRANSITION_CHANGED) + res = TRANSITION_CHANGED_FALLBACK; } return res; @@ -2988,9 +2992,6 @@ Commander::set_main_state_rc() return TRANSITION_NOT_CHANGED; } - const auto _penultimate_manual_control_switches = _last_manual_control_switches; - _last_manual_control_switches = _manual_control_switches; - // reset the position and velocity validity calculation to give the best change of being able to select // the desired mode reset_posvel_validity(); @@ -3056,8 +3057,6 @@ Commander::set_main_state_rc() res = try_mode_change(new_mode); } - return res; - } else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) { /* offboard and RTL switches off or denied, check main mode switch */ switch (_manual_control_switches.mode_switch) { @@ -3138,8 +3137,8 @@ Commander::set_main_state_rc() } // check if we've transitioned; if not, keep around our previous state to allow the next iteration to try - if (res == TRANSITION_NOT_CHANGED) - _last_manual_control_switches = _penultimate_manual_control_switches; + if (res == TRANSITION_CHANGED) + _last_manual_control_switches = _manual_control_switches; return res; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f5ce52486158..a2a7ee5c5e19 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,8 @@ typedef enum { TRANSITION_DENIED = -1, TRANSITION_NOT_CHANGED = 0, - TRANSITION_CHANGED + TRANSITION_CHANGED, + TRANSITION_CHANGED_FALLBACK } transition_result_t; enum class link_loss_actions_t { From 6951cb9b9e68e13394bbbe38ebea6e6e586e741c Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 12 Apr 2022 08:52:26 -0400 Subject: [PATCH 28/39] Removed some forgotten spam. --- src/modules/commander/Commander.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7de6f5d12080..0713f698d154 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2920,11 +2920,9 @@ transition_result_t Commander::set_main_state(bool &changed) { if (_safety.override_available && _safety.override_enabled) { - PX4_INFO("Overriding main state for safety."); return set_main_state_override_on(changed); } else { - PX4_INFO("Setting main state to RC commanded."); return set_main_state_rc(); } } From 353ec8364d5e790f9295dc48d81f9f8e3b4915d4 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 7 Jan 2022 16:31:38 +0100 Subject: [PATCH 29/39] PosControl: fix hover update equation The integrator now absorbs properly the change in hover thrust --- .../PositionControl/PositionControl.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index caca1c85a4e6..2626ddecfb61 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -67,8 +67,17 @@ void PositionControl::setThrustLimits(const float min, const float max) void PositionControl::updateHoverThrust(const float hover_thrust_new) { - _vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new); - setHoverThrust(hover_thrust_new); + // Given that the equation for thrust is T = a_sp * Th / g - Th + // with a_sp = desired acceleration, Th = hover thrust and g = gravity constant, + // we want to find the acceleration that needs to be added to the integrator in order obtain + // the same thrust after replacing the current hover thrust by the new one. + // T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th + // so a_sp' = (a_sp - g) * Th / Th' + g + // we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th' + if (hover_thrust_new > FLT_EPSILON) { + _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * _hover_thrust / hover_thrust_new + CONSTANTS_ONE_G - _acc_sp(2); + setHoverThrust(hover_thrust_new); + } } void PositionControl::setState(const PositionControlStates &states) From b84bd1b2108734f1704db4679224a3645d0c6e82 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Tue, 31 May 2022 13:11:38 -0400 Subject: [PATCH 30/39] Updated default hover thrust. --- ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb index a56cda093898..51f528b31776 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_nhb @@ -23,7 +23,7 @@ param set-default MPC_Z_VEL_MAX_UP 1.5 param set-default MPC_Z_VEL_MAX_DN 1.5 param set-default MPC_POS_MODE 1 param set-default MPC_MAN_TILT_MAX 20 -param set-default MPC_THR_HOVER 0.07 +param set-default MPC_THR_HOVER 0.1 # selective disabling of various arming / disarming checks param set-default FD_FAIL_R 0 # allow arming at any Roll angle From 33b764abd136cb15356ac4cd4507b0d70f440642 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Jun 2022 08:05:13 -0400 Subject: [PATCH 31/39] First pass at adding feedback and actuator command logging for modalai_esc. --- msg/actuator_outputs.msg | 2 ++ msg/esc_report.msg | 1 + .../uart_esc/modalai_esc/modalai_esc.cpp | 30 +++++++++++++++++++ .../uart_esc/modalai_esc/modalai_esc.hpp | 5 ++++ 4 files changed, 38 insertions(+) diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg index ff546948d804..c49a650f53e5 100644 --- a/msg/actuator_outputs.msg +++ b/msg/actuator_outputs.msg @@ -3,3 +3,5 @@ uint8 NUM_ACTUATOR_OUTPUTS = 16 uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking uint32 noutputs # valid outputs float32[16] output # output data, in natural output units + +# TOPICS actuator_outputs actuator_outputs_debug diff --git a/msg/esc_report.msg b/msg/esc_report.msg index 508a8b9ebd19..21a4934ad128 100644 --- a/msg/esc_report.msg +++ b/msg/esc_report.msg @@ -5,6 +5,7 @@ float32 esc_voltage # Voltage measured from current ESC [V] - if supported float32 esc_current # Current measured from current ESC [A] - if supported uint8 esc_temperature # Temperature measured from current ESC [degC] - if supported uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) +uint8 esc_cmdcount # Counter of number of commands uint8 esc_state # State of ESC - depend on Vendor diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp index 9a20339d23eb..03079637a72e 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -53,6 +53,11 @@ ModalaiEsc::ModalaiEsc() : _mixing_output.setAllFailsafeValues(0); _mixing_output.setAllDisarmedValues(0); + + _esc_status.timestamp = hrt_absolute_time(); + _esc_status.counter = 0; + _esc_status.esc_count = MODALAI_ESC_OUTPUT_CHANNELS; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; } ModalaiEsc::~ModalaiEsc() @@ -270,6 +275,16 @@ int ModalaiEsc::parseResponse(uint8_t *buf, uint8_t len) _esc_chans[id].state = fb.state & 0x0F; _esc_chans[id].cmd_counter = fb.cmd_counter; _esc_chans[id].voltage = 9.0 + fb.voltage / 34.0; + + // also update our internal report for logging + _esc_status.esc[id].timestamp = hrt_absolute_time(); + _esc_status.esc[id].esc_rpm = fb.rpm; + _esc_status.esc[id].esc_state = fb.state & 0x0F; + _esc_status.esc[id].esc_cmdcount = fb.cmd_counter; + _esc_status.esc[id].esc_voltage = 9.0 + fb.voltage / 34.0; + + _esc_status.timestamp = _esc_status.esc[id].timestamp; + _esc_status.counter++; } } @@ -873,6 +888,21 @@ bool ModalaiEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS] parseResponse(cmd.buf, res); } + // publish the actual command that we sent and the feedback received + { + actuator_outputs_s actuator_outputs{}; + actuator_outputs.noutputs = num_outputs; + + for (size_t i = 0; i < num_outputs; ++i) { + actuator_outputs.output[i] = _esc_chans[i].rate_req; + } + actuator_outputs.timestamp = hrt_absolute_time(); + + _outputs_debug_pub.publish(actuator_outputs); + _esc_status_pub.publish(_esc_status); + } + + perf_count(_output_update_perf); return true; diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp index 9a95fca81cbf..a27667db6275 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.hpp @@ -45,6 +45,7 @@ #include #include +#include #include "modalai_esc_serial.hpp" @@ -187,6 +188,9 @@ class ModalaiEsc : public cdev::CDev, public ModuleBase, public Outp uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; + uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; ///< mixer status flags + uart_esc_params_t _parameters; int update_params(); int load_params(uart_esc_params_t *params, ch_assign_t *map); @@ -201,6 +205,7 @@ class ModalaiEsc : public cdev::CDev, public ModuleBase, public Outp EscChan _esc_chans[MODALAI_ESC_OUTPUT_CHANNELS]; Command _esc_cmd; + esc_status_s _esc_status; led_rsc_t _led_rsc; void updateLeds(vehicle_control_mode_s mode, led_control_s control); From 0551a580d8e8eee1f8d9ca8d76b548f6168af0e4 Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Jun 2022 08:11:04 -0400 Subject: [PATCH 32/39] Logging newly added topics. --- src/modules/logger/logged_topics.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index b668559a8e36..74d5970bcf10 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -204,6 +204,8 @@ void LoggedTopics::add_high_rate_topics() add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); add_topic("vehicle_rates_setpoint"); + add_topic("actuator_outputs_debug"); + add_topic("esc_status"); } void LoggedTopics::add_debug_topics() From b5c92adbd818af60d7df079a487a1595e6482d1e Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Jun 2022 09:10:55 -0400 Subject: [PATCH 33/39] Added online and armed flags. --- src/drivers/uart_esc/modalai_esc/modalai_esc.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp index 03079637a72e..4d4536eef012 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -58,6 +58,9 @@ ModalaiEsc::ModalaiEsc() : _esc_status.counter = 0; _esc_status.esc_count = MODALAI_ESC_OUTPUT_CHANNELS; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + // @TODO replace these with actual feedback from ESCs, if available + _esc_status.esc_online_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1; + _esc_status.esc_armed_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1; } ModalaiEsc::~ModalaiEsc() From 0a1ab9adf68d65ce156fa25b67e46f183dc8a7df Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Wed, 1 Jun 2022 09:37:48 -0400 Subject: [PATCH 34/39] Initializing variables to 0. --- src/drivers/uart_esc/modalai_esc/modalai_esc.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp index 4d4536eef012..bcf9160c64d9 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -58,6 +58,14 @@ ModalaiEsc::ModalaiEsc() : _esc_status.counter = 0; _esc_status.esc_count = MODALAI_ESC_OUTPUT_CHANNELS; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + for (unsigned i = 0; i != MODALAI_ESC_OUTPUT_CHANNELS; ++i) { + _esc_status.esc[i].timestamp = _esc_status.timestamp; + _esc_status.esc[i].esc_rpm = 0; + _esc_status.esc[i].esc_state = 0; + _esc_status.esc[i].esc_cmdcount = 0; + _esc_status.esc[i].esc_voltage = 0; + } + // @TODO replace these with actual feedback from ESCs, if available _esc_status.esc_online_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1; _esc_status.esc_armed_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1; From 3dae77618308fa75080531795df816c26fc9a43a Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 16 Jun 2022 08:35:57 -0400 Subject: [PATCH 35/39] Improved command line argument debugging. --- src/drivers/uart_esc/modalai_esc/modalai_esc.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp index f85bd3f49b47..c44c2291f92b 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -450,7 +450,7 @@ int ModalaiEsc::custom_command(int argc, char *argv[]) uint32_t repeat_delay_us = 10000; if (argc < 3) { - return print_usage("unknown command"); + return print_usage("Not enough input arguments."); } const char *verb = argv[argc - 1]; @@ -513,7 +513,7 @@ int ModalaiEsc::custom_command(int argc, char *argv[]) break; default: - print_usage("Unknown command"); + print_usage("Unknown argument flag."); return 0; } } @@ -702,7 +702,7 @@ int ModalaiEsc::custom_command(int argc, char *argv[]) PX4_INFO("ESC map: %d %d %d %d",map[0].number,map[1].number,map[2].number,map[3].number); PX4_INFO("feedback id debug: %i, %i", id_fb_raw, id_fb); - PX4_INFO("Sending UART ESC power command %i", rate); + PX4_INFO("Sending UART ESC power command %i", rate); return get_instance()->sendCommandThreadSafe(&cmd); From 1f224ece96149b8ca9920b5c0a4a04b54d444dcb Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 16 Jun 2022 13:11:58 -0400 Subject: [PATCH 36/39] Removed offset bias. --- .../FlightTaskManualAltitudeCommandVel.cpp | 7 ------- .../FlightTaskManualAltitudeCommandVel.hpp | 1 - 2 files changed, 8 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp index b20ad6c1e04a..fd4e4f252ee7 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.cpp @@ -67,7 +67,6 @@ bool FlightTaskManualAltitudeCommandVel::activate(const vehicle_local_position_s _yawspeed_setpoint = 0.f; _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from velocity _last_position = _position; // initialize loop to assume we're stable - _z_bias_lpf = 0; _position_setpoint(2) = NAN; _velocity_setpoint(2) = 0.f; _setDefaultConstraints(); @@ -84,12 +83,6 @@ void FlightTaskManualAltitudeCommandVel::_scaleSticks() // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); - - // apply bias to overcome steady state errors - const float z_bias = math::constrain(_velocity(2) - ((_position(2) - _last_position(2)) / _deltatime), -0.5f, 0.5f); - const float alpha = 0.1; - _z_bias_lpf = (_z_bias_lpf * (1 - alpha)) + (z_bias * alpha); - _velocity_setpoint(2) += _z_bias_lpf; } float FlightTaskManualAltitudeCommandVel::_applyYawspeedFilter(float yawspeed_target) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp index 4088bb134097..c6356913b881 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeCommandVel/FlightTaskManualAltitudeCommandVel.hpp @@ -85,7 +85,6 @@ class FlightTaskManualAltitudeCommandVel : public FlightTask float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ matrix::Vector3f _last_position; /**< last loop's vehicle position */ - float _z_bias_lpf; /**< low passed version of z vel bias */ AlphaFilter _man_input_filter; }; From effc07add42d0db121cc05ab06b7a80a4434903b Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 16 Jun 2022 16:05:36 -0400 Subject: [PATCH 37/39] Got rid of conservative landing integral check. --- src/modules/mc_rate_control/MulticopterRateControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 4e907fc4d2d2..fd7fa63f5321 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -219,7 +219,7 @@ MulticopterRateControl::Run() } // run rate controller - const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _maybe_landed || _landed); + const Vector3f att_control = _rate_control.update(rates, _rates_sp, angular_accel, dt, _landed); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; From f7e3e07814d675ae872d7bfe4b724df7e6f6c5cc Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 16 Jun 2022 17:07:49 -0400 Subject: [PATCH 38/39] Added initialization of armed / online flags to true. --- src/drivers/uart_esc/modalai_esc/modalai_esc.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp index c44c2291f92b..50a32158bb0a 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -57,6 +57,8 @@ ModalaiEsc::ModalaiEsc() : _esc_status.counter = 0; _esc_status.esc_count = MODALAI_ESC_OUTPUT_CHANNELS; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; + _esc_status.esc_online_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1; + _esc_status.esc_armed_flags = (1 << MODALAI_ESC_OUTPUT_CHANNELS) - 1; for (unsigned i = 0; i < MODALAI_ESC_OUTPUT_CHANNELS; i++) { _esc_status.esc[i].timestamp = 0; @@ -67,7 +69,7 @@ ModalaiEsc::ModalaiEsc() : _esc_status.esc[i].esc_voltage = 0; _esc_status.esc[i].esc_current = 0; _esc_status.esc[i].esc_temperature = 0; - _esc_status.esc[i].esc_errorcount = 0; + _esc_status.esc[i].esc_errorcount = 0; _esc_status.esc[i].failures = 0; } From 391495a88a88d018bde43be507fd3085acb2402a Mon Sep 17 00:00:00 2001 From: "Daniel M. Sahu" Date: Thu, 16 Jun 2022 17:10:15 -0400 Subject: [PATCH 39/39] Enabled fb logging. --- src/drivers/uart_esc/modalai_esc/modalai_esc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp index 50a32158bb0a..22dfa675886f 100644 --- a/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp +++ b/src/drivers/uart_esc/modalai_esc/modalai_esc.cpp @@ -1085,7 +1085,7 @@ bool ModalaiEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS] } // publish the actual command that we sent and the feedback received - if (0) { + { actuator_outputs_s actuator_outputs{}; actuator_outputs.noutputs = num_outputs;