From 2b69a3d290aa92af76e5a5ffe49fe61b4b204184 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Thu, 18 Jan 2024 09:14:17 -0800 Subject: [PATCH] VOXL2 specific drivers, modules, and miscellaneous support files (#22588) --- .gitmodules | 3 + boards/modalai/fc-v1/default.px4board | 2 +- boards/modalai/fc-v1/init/rc.board_sensors | 4 +- boards/modalai/fc-v1/src/board_config.h | 2 +- boards/modalai/fc-v2/default.px4board | 16 +- boards/modalai/fc-v2/init/rc.board_sensors | 4 +- boards/modalai/fc-v2/src/board_config.h | 2 +- boards/modalai/voxl2-io/default.px4board | 5 + boards/modalai/voxl2-io/firmware.prototype | 13 + .../voxl2-io/nuttx-config/include/board.h | 141 +++ .../voxl2-io/nuttx-config/nsh/defconfig | 61 + .../voxl2-io/nuttx-config/scripts/script.ld | 131 ++ boards/modalai/voxl2-io/src/CMakeLists.txt | 42 + boards/modalai/voxl2-io/src/board_config.h | 157 +++ boards/modalai/voxl2-io/src/init.c | 133 ++ boards/modalai/voxl2-io/src/timer_config.cpp | 54 + boards/modalai/voxl2-slpi/default.px4board | 53 +- boards/modalai/voxl2-slpi/src/CMakeLists.txt | 8 +- boards/modalai/voxl2-slpi/src/board_config.h | 19 +- .../src/drivers/dsp_hitl/CMakeLists.txt | 49 + .../src/drivers/dsp_hitl/dsp_hitl.cpp | 1064 ++++++++++++++++ .../src/drivers/dsp_sbus/CMakeLists.txt | 41 + .../src/drivers/dsp_sbus/dsp_sbus.cpp | 383 ++++++ .../src/drivers/dsp_sbus/protocol.h | 405 ++++++ .../src/drivers/elrs_led/CMakeLists.txt | 41 + .../src/drivers/elrs_led/elrs_led.cpp | 317 +++++ .../src/drivers/elrs_led/elrs_led.h | 113 ++ .../src/drivers/ghst_rc/CMakeLists.txt | 47 + .../voxl2-slpi/src/drivers/ghst_rc/Kconfig | 5 + .../src/drivers/ghst_rc/ghst_rc.cpp | 312 +++++ .../src/drivers/ghst_rc/ghst_rc.hpp | 126 ++ .../src/drivers/ghst_rc/module.yaml | 11 + .../src/drivers/icm42688p/ICM42688P.cpp | 152 +-- .../src/drivers/icm42688p/ICM42688P.hpp | 10 +- .../InvenSense_ICM42688P_registers.hpp | 9 - .../src/drivers/spektrum_rc/spektrum_rc.cpp | 8 +- .../modalai/voxl2/cmake/link_libraries.cmake | 4 +- boards/modalai/voxl2/default.px4board | 44 +- boards/modalai/voxl2/scripts/build-deps.sh | 10 + boards/modalai/voxl2/scripts/build-slpi.sh | 2 - boards/modalai/voxl2/scripts/clean.sh | 7 +- .../voxl2/scripts/install-voxl-bins.sh | 9 + boards/modalai/voxl2/scripts/install-voxl.sh | 9 + boards/modalai/voxl2/scripts/patch-gazebo.sh | 13 + boards/modalai/voxl2/src/CMakeLists.txt | 3 + boards/modalai/voxl2/src/board_config.h | 3 +- .../src/drivers/apps_sbus/CMakeLists.txt | 41 + .../voxl2/src/drivers/apps_sbus/apps_sbus.cpp | 453 +++++++ .../voxl2/src/drivers/apps_sbus/protocol.h | 405 ++++++ boards/modalai/voxl2/target/voxl-px4 | 24 +- boards/modalai/voxl2/target/voxl-px4-hitl | 39 + ...oxl-px4-hitl-set-default-parameters.config | 111 ++ .../modalai/voxl2/target/voxl-px4-hitl-start | 101 ++ .../voxl-px4-set-default-parameters.config | 41 +- boards/modalai/voxl2/target/voxl-px4-start | 186 ++- msg/Buffer128.msg | 9 + msg/CMakeLists.txt | 1 + .../cmake/Toolchain-aarch64-linux-gnu.cmake | 12 +- platforms/qurt/cmake/qurt_reqs.cmake | 3 +- .../px4/common/include/px4_platform/cpuload.h | 37 + platforms/qurt/src/px4/drv_hrt.cpp | 8 +- platforms/qurt/src/px4/tasks.cpp | 8 +- src/drivers/actuators/modal_io/Kconfig | 5 - src/drivers/actuators/modal_io/module.yaml | 26 - .../{modal_io => voxl_esc}/CMakeLists.txt | 13 +- src/drivers/actuators/voxl_esc/Kconfig | 5 + .../actuators/{modal_io => voxl_esc}/crc16.c | 0 .../actuators/{modal_io => voxl_esc}/crc16.h | 0 src/drivers/actuators/voxl_esc/module.yaml | 41 + .../{modal_io => voxl_esc}/qc_esc_packet.c | 56 +- .../{modal_io => voxl_esc}/qc_esc_packet.h | 27 +- .../qc_esc_packet_types.h | 2 + .../modal_io.cpp => voxl_esc/voxl_esc.cpp} | 465 ++++--- .../modal_io.hpp => voxl_esc/voxl_esc.hpp} | 101 +- .../voxl_esc_params.c} | 84 +- .../voxl_esc_serial.cpp} | 16 +- .../voxl_esc_serial.hpp} | 6 +- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 8 +- src/drivers/voxl2_io/CMakeLists.txt | 52 + src/drivers/voxl2_io/Kconfig | 5 + src/drivers/voxl2_io/module.yaml | 13 + src/drivers/voxl2_io/voxl2_io.cpp | 1122 +++++++++++++++++ src/drivers/voxl2_io/voxl2_io.hpp | 228 ++++ src/drivers/voxl2_io/voxl2_io_crc16.c | 95 ++ src/drivers/voxl2_io/voxl2_io_crc16.h | 65 + src/drivers/voxl2_io/voxl2_io_packet.c | 253 ++++ src/drivers/voxl2_io/voxl2_io_packet.h | 287 +++++ src/drivers/voxl2_io/voxl2_io_packet_types.h | 75 ++ src/drivers/voxl2_io/voxl2_io_params.c | 66 + src/drivers/voxl2_io/voxl2_io_serial.cpp | 191 +++ .../voxl2_io/voxl2_io_serial.hpp} | 58 +- src/modules/muorb/apps/CMakeLists.txt | 1 + src/modules/muorb/apps/libfc-sensor-api | 1 + .../muorb/apps/uORBAppsProtobufChannel.cpp | 5 + .../muorb/slpi/uORBProtobufChannel.cpp | 38 +- .../muorb/slpi/uORBProtobufChannel.hpp | 8 +- 96 files changed, 8266 insertions(+), 678 deletions(-) create mode 100644 boards/modalai/voxl2-io/default.px4board create mode 100644 boards/modalai/voxl2-io/firmware.prototype create mode 100644 boards/modalai/voxl2-io/nuttx-config/include/board.h create mode 100644 boards/modalai/voxl2-io/nuttx-config/nsh/defconfig create mode 100644 boards/modalai/voxl2-io/nuttx-config/scripts/script.ld create mode 100644 boards/modalai/voxl2-io/src/CMakeLists.txt create mode 100644 boards/modalai/voxl2-io/src/board_config.h create mode 100644 boards/modalai/voxl2-io/src/init.c create mode 100644 boards/modalai/voxl2-io/src/timer_config.cpp create mode 100644 boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt create mode 100644 boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp create mode 100644 boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt create mode 100644 boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp create mode 100644 boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h create mode 100644 boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt create mode 100644 boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp create mode 100644 boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h create mode 100644 boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt create mode 100644 boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig create mode 100644 boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp create mode 100644 boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp create mode 100644 boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml create mode 100755 boards/modalai/voxl2/scripts/build-deps.sh create mode 100755 boards/modalai/voxl2/scripts/install-voxl-bins.sh create mode 100755 boards/modalai/voxl2/scripts/patch-gazebo.sh create mode 100644 boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt create mode 100644 boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp create mode 100644 boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h create mode 100755 boards/modalai/voxl2/target/voxl-px4-hitl create mode 100755 boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config create mode 100755 boards/modalai/voxl2/target/voxl-px4-hitl-start create mode 100644 msg/Buffer128.msg create mode 100644 platforms/qurt/src/px4/common/include/px4_platform/cpuload.h delete mode 100644 src/drivers/actuators/modal_io/Kconfig delete mode 100644 src/drivers/actuators/modal_io/module.yaml rename src/drivers/actuators/{modal_io => voxl_esc}/CMakeLists.txt (93%) create mode 100644 src/drivers/actuators/voxl_esc/Kconfig rename src/drivers/actuators/{modal_io => voxl_esc}/crc16.c (100%) rename src/drivers/actuators/{modal_io => voxl_esc}/crc16.h (100%) create mode 100644 src/drivers/actuators/voxl_esc/module.yaml rename src/drivers/actuators/{modal_io => voxl_esc}/qc_esc_packet.c (83%) rename src/drivers/actuators/{modal_io => voxl_esc}/qc_esc_packet.h (91%) rename src/drivers/actuators/{modal_io => voxl_esc}/qc_esc_packet_types.h (97%) rename src/drivers/actuators/{modal_io/modal_io.cpp => voxl_esc/voxl_esc.cpp} (76%) rename src/drivers/actuators/{modal_io/modal_io.hpp => voxl_esc/voxl_esc.hpp} (68%) rename src/drivers/actuators/{modal_io/modal_io_params.c => voxl_esc/voxl_esc_params.c} (73%) rename src/drivers/actuators/{modal_io/modal_io_serial.cpp => voxl_esc/voxl_esc_serial.cpp} (93%) rename src/drivers/actuators/{modal_io/modal_io_serial.hpp => voxl_esc/voxl_esc_serial.hpp} (97%) create mode 100644 src/drivers/voxl2_io/CMakeLists.txt create mode 100644 src/drivers/voxl2_io/Kconfig create mode 100644 src/drivers/voxl2_io/module.yaml create mode 100644 src/drivers/voxl2_io/voxl2_io.cpp create mode 100644 src/drivers/voxl2_io/voxl2_io.hpp create mode 100644 src/drivers/voxl2_io/voxl2_io_crc16.c create mode 100644 src/drivers/voxl2_io/voxl2_io_crc16.h create mode 100644 src/drivers/voxl2_io/voxl2_io_packet.c create mode 100644 src/drivers/voxl2_io/voxl2_io_packet.h create mode 100644 src/drivers/voxl2_io/voxl2_io_packet_types.h create mode 100644 src/drivers/voxl2_io/voxl2_io_params.c create mode 100644 src/drivers/voxl2_io/voxl2_io_serial.cpp rename src/{modules/muorb/apps/fc_sensor.h => drivers/voxl2_io/voxl2_io_serial.hpp} (62%) create mode 160000 src/modules/muorb/apps/libfc-sensor-api diff --git a/.gitmodules b/.gitmodules index 983daae405..e91cf2b63a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -80,3 +80,6 @@ path = Tools/simulation/gz url = https://github.com/PX4/PX4-gazebo-models.git branch = main +[submodule "boards/modalai/voxl2/libfc-sensor-api"] + path = src/modules/muorb/apps/libfc-sensor-api + url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 5999659075..2a33d1ddb8 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -5,7 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_COMMON_BAROMETERS=y diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 7268fce687..03824da992 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -3,9 +3,9 @@ # ModalAI FC-v1 specific board sensors init #------------------------------------------------------------------------------ -if param greater MODAL_IO_CONFIG 0 +if param greater VOXL_ESC_CONFIG 0 then - modal_io start + voxl_esc start fi board_adc start diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index dcc182420a..93032bb31f 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -264,7 +264,7 @@ #define BOARD_NUM_IO_TIMERS 5 // J4 / TELEM3 / UART2 -#define MODAL_IO_DEFAULT_PORT "/dev/ttyS1" +#define VOXL_ESC_DEFAULT_PORT "/dev/ttyS1" __BEGIN_DECLS diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index a34d1ad7a4..fd5cb46283 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -3,9 +3,10 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y +# CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y @@ -14,6 +15,10 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y +#CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +#CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +#CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +#CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y @@ -25,11 +30,13 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPM=y CONFIG_COMMON_TELEMETRY=y -CONFIG_DRIVERS_UAVCAN=y -CONFIG_BOARD_UAVCAN_INTERFACES=1 -CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +#CONFIG_DRIVERS_UAVCAN=y +#CONFIG_BOARD_UAVCAN_INTERFACES=1 +#CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y @@ -48,6 +55,7 @@ CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y diff --git a/boards/modalai/fc-v2/init/rc.board_sensors b/boards/modalai/fc-v2/init/rc.board_sensors index 318a8bc146..4fd143d503 100644 --- a/boards/modalai/fc-v2/init/rc.board_sensors +++ b/boards/modalai/fc-v2/init/rc.board_sensors @@ -3,9 +3,9 @@ # ModalAI FC-v2 specific board sensors init #------------------------------------------------------------------------------ -if param greater MODAL_IO_CONFIG 0 +if param greater VOXL_ESC_CONFIG 0 then - modal_io start + voxl_esc start fi board_adc start diff --git a/boards/modalai/fc-v2/src/board_config.h b/boards/modalai/fc-v2/src/board_config.h index 7e0314cdbc..987f692b22 100644 --- a/boards/modalai/fc-v2/src/board_config.h +++ b/boards/modalai/fc-v2/src/board_config.h @@ -351,7 +351,7 @@ #define BOARD_NUM_IO_TIMERS 5 // J5 USART5 TELEM2 Port next to PWM connector -#define MODAL_IO_DEFAULT_PORT "/dev/ttyS4" +#define VOXL_ESC_DEFAULT_PORT "/dev/ttyS4" __BEGIN_DECLS diff --git a/boards/modalai/voxl2-io/default.px4board b/boards/modalai/voxl2-io/default.px4board new file mode 100644 index 0000000000..9e69a1e746 --- /dev/null +++ b/boards/modalai/voxl2-io/default.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m3" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_MODULES_PX4IOFIRMWARE=y diff --git a/boards/modalai/voxl2-io/firmware.prototype b/boards/modalai/voxl2-io/firmware.prototype new file mode 100644 index 0000000000..81665310ce --- /dev/null +++ b/boards/modalai/voxl2-io/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 41777, + "magic": "PX4FWv2", + "description": "Firmware for the voxl2-io board", + "image": "", + "build_time": 0, + "summary": "voxl2io", + "version": "2.0", + "image_size": 0, + "image_maxsize": 61440, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/modalai/voxl2-io/nuttx-config/include/board.h b/boards/modalai/voxl2-io/nuttx-config/include/board.h new file mode 100644 index 0000000000..936360ab13 --- /dev/null +++ b/boards/modalai/voxl2-io/nuttx-config/include/board.h @@ -0,0 +1,141 @@ +/************************************************************************************ + * nuttx-configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Copyright (C) 2012 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 16000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + + +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (STM32_PCLK1_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1, 15-17 are on APB2, others on APB1 + */ + +#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN +#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN +#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN +#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN +#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN +#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN +#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN +#define BOARD_TIM12_FREQUENCY STM32_APB1_TIM12_CLKIN +#define BOARD_TIM13_FREQUENCY STM32_APB1_TIM13_CLKIN +#define BOARD_TIM14_FREQUENCY STM32_APB1_TIM14_CLKIN +#define BOARD_TIM15_FREQUENCY STM32_APB2_TIM15_CLKIN +#define BOARD_TIM16_FREQUENCY STM32_APB2_TIM16_CLKIN +#define BOARD_TIM17_FREQUENCY STM32_APB2_TIM17_CLKIN + + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#undef GPIO_USART2_CTS +#define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS +#define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK +#define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_CK +#define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS +#define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS +#define GPIO_USART3_RTS 0xffffffff + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/modalai/voxl2-io/nuttx-config/nsh/defconfig b/boards/modalai/voxl2-io/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..2fec96fb26 --- /dev/null +++ b/boards/modalai/voxl2-io/nuttx-config/nsh/defconfig @@ -0,0 +1,61 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_NULL is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/voxl2-io/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=316 +CONFIG_INIT_ENTRYPOINT="user_start" +CONFIG_INIT_STACKSIZE=1100 +CONFIG_MM_FILL_ALLOCATIONS=y +CONFIG_MM_SMALL=y +CONFIG_NAME_MAX=12 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_RAM_SIZE=8192 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/modalai/voxl2-io/nuttx-config/scripts/script.ld b/boards/modalai/voxl2-io/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..c84215389d --- /dev/null +++ b/boards/modalai/voxl2-io/nuttx-config/scripts/script.ld @@ -0,0 +1,131 @@ +/**************************************************************************** + * configs/px4io-v2/scripts/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + . = ALIGN(4); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/modalai/voxl2-io/src/CMakeLists.txt b/boards/modalai/voxl2-io/src/CMakeLists.txt new file mode 100644 index 0000000000..5cef2f668c --- /dev/null +++ b/boards/modalai/voxl2-io/src/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 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 + init.c + timer_config.cpp +) + +target_link_libraries(drivers_board + PRIVATE + nuttx_arch +) diff --git a/boards/modalai/voxl2-io/src/board_config.h b/boards/modalai/voxl2-io/src/board_config.h new file mode 100644 index 0000000000..d69aee59b2 --- /dev/null +++ b/boards/modalai/voxl2-io/src/board_config.h @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include +#include +#include + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * Serial + ******************************************************************************/ +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 921600 + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) + +#define LED_BLUE(on_true) stm32_gpiowrite(GPIO_LED_BLUE, !(on_true)) +#define LED_AMBER(on_true) stm32_gpiowrite(GPIO_LED_AMBER, !(on_true)) +#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true)) + +//#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) + +//#define GPIO_HEATER_OFF 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) + +#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) +#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) +# define SENSE_PH1 0b10 /* Floating pulled as set */ +# define SENSE_PH2 0b01 /* Driven as tied */ + +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) + +/* Safety switch button *******************************************************/ + +/* CONNECTED TO TP8 - pulled down via SW */ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define SPEKTRUM_POWER(_on_true) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_on_true)) + +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_one_true)) +#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM) + +#define GPIO_SERVO_FAULT_DETECT 0 // (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) + +/* Analog inputs **************************************************************/ + +/* PWM pins **************************************************************/ + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +#define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define BOARD_HAS_NO_CAPTURE + +/* SBUS pins *************************************************************/ + +/* XXX these should be UART pins */ +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 PA8 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8) + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +#define BOARD_NUM_IO_TIMERS 3 + +#include diff --git a/boards/modalai/voxl2-io/src/init.c b/boards/modalai/voxl2-io/src/init.c new file mode 100644 index 0000000000..1e432eb71f --- /dev/null +++ b/boards/modalai/voxl2-io/src/init.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 init.c + * + * PX4FMU-specific early startup code. This file implements the + * stm32_boardinitialize() function that is called during cpu startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure GPIOs */ + + /* Set up for sensing HW */ + stm32_configgpio(GPIO_SENSE_PC14_DN); + stm32_configgpio(GPIO_SENSE_PC15_UP); + + /* LEDS - default to off */ + stm32_configgpio(GPIO_LED_BLUE); + stm32_configgpio(GPIO_LED_AMBER); + stm32_configgpio(GPIO_LED_SAFETY); + + stm32_configgpio(GPIO_PC14); + stm32_configgpio(GPIO_PC15); + + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - enable it by default */ + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + stm32_configgpio(GPIO_SBUS_OUTPUT); + + stm32_gpiowrite(GPIO_PWM1, true); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, true); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, true); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, true); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, true); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, true); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, true); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, true); + stm32_configgpio(GPIO_PWM8); +} diff --git a/boards/modalai/voxl2-io/src/timer_config.cpp b/boards/modalai/voxl2-io/src/timer_config.cpp new file mode 100644 index 0000000000..f84e69921c --- /dev/null +++ b/boards/modalai/voxl2-io/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer2), + initIOTimer(Timer::Timer3), + initIOTimer(Timer::Timer4), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortB, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortB, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortA, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortA, GPIO::Pin7}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 8dcfe4c0d3..a3e4cb9c28 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -1,26 +1,37 @@ CONFIG_PLATFORM_QURT=y CONFIG_BOARD_TOOLCHAIN="qurt" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y -CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y -CONFIG_DRIVERS_GPS=y -CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y -CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y -CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y -CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y -CONFIG_DRIVERS_QSHELL_QURT=y -CONFIG_DRIVERS_RC_CRSF_RC=y -CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y -CONFIG_MODULES_EKF2=y -CONFIG_MODULES_LAND_DETECTOR=y -CONFIG_MODULES_MANUAL_CONTROL=y -CONFIG_MODULES_MC_ATT_CONTROL=y -CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y -CONFIG_MODULES_MC_POS_CONTROL=y -CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_BOARD_ROOTFSDIR="/" CONFIG_MODULES_MUORB_SLPI=y -CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y +CONFIG_DRIVERS_QSHELL_QURT=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y +CONFIG_DRIVERS_VOXL2_IO=y +CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y +# CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y +CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y +# CONFIG_DRIVERS_GPS=y +# CONFIG_DRIVERS_RC_CRSF_RC=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_PARAM_SET_SELECTOR=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_PARAM_CLIENT=y diff --git a/boards/modalai/voxl2-slpi/src/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/CMakeLists.txt index 7d1867d1ec..2803359247 100644 --- a/boards/modalai/voxl2-slpi/src/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/CMakeLists.txt @@ -44,7 +44,11 @@ add_library(drivers_board ) # Add custom drivers for SLPI -add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in) -add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl) +# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led) diff --git a/boards/modalai/voxl2-slpi/src/board_config.h b/boards/modalai/voxl2-slpi/src/board_config.h index 2aa91b4b36..2729397967 100644 --- a/boards/modalai/voxl2-slpi/src/board_config.h +++ b/boards/modalai/voxl2-slpi/src/board_config.h @@ -62,4 +62,21 @@ /* * Default port for the ESC */ -#define MODAL_IO_DEFAULT_PORT "2" +#define VOXL_ESC_DEFAULT_PORT "2" + +/* + * Default port for the GHST RC + */ +#define GHST_RC_DEFAULT_PORT "7" + +/* + * Default port for M0065 +*/ +#define VOXL2_IO_DEFAULT_PORT "2" + + +/* + * M0065 PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 4 +#define MAX_IO_TIMERS 3 diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt new file mode 100644 index 0000000000..65026b83cf --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +message(STATUS "Mavlink include directory: ${PX4_SOURCE_DIR}/../build/modalai_voxl2_default/mavlink/common") + +px4_add_module( + MODULE drivers__modalai__dsp_hitl + MAIN dsp_hitl + INCLUDES + ${PX4_SOURCE_DIR}/src/drivers/dsp_hitl + ${PX4_SOURCE_DIR}/build/modalai_voxl2_default/mavlink/common + SRCS + dsp_hitl.cpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope + drivers_magnetometer + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp new file mode 100644 index 0000000000..0fbdfff7ea --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -0,0 +1,1064 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include // For DeviceId union + +#include +#include + +#include + +#include + +#define ASYNC_UART_READ_WAIT_US 2000 + +extern "C" { __EXPORT int dsp_hitl_main(int argc, char *argv[]); } + +namespace dsp_hitl +{ + +using matrix::wrap_2pi; + +static bool _is_running = false; +volatile bool _task_should_exit = false; +static px4_task_t _task_handle = -1; +int _uart_fd = -1; +bool debug = false; +std::string port = "2"; +int baudrate = 921600; +const unsigned mode_flag_custom = 1; +const unsigned mode_flag_armed = 128; +bool _send_gps = false; +bool _send_mag = false; + +uORB::Publication _battery_pub{ORB_ID(battery_status)}; +uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; +uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; +uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; +uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; +uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; +uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; +uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + +int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; + +// hil_sensor and hil_state_quaternion +enum SensorSource { + ACCEL = 0b111, + GYRO = 0b111000, + MAG = 0b111000000, + BARO = 0b1101000000000, + DIFF_PRESS = 0b10000000000 +}; + +PX4Accelerometer *_px4_accel{nullptr}; +PX4Gyroscope *_px4_gyro{nullptr}; +PX4Magnetometer *_px4_mag{nullptr}; + +bool got_first_sensor_msg = false; +float x_accel = 0; +float y_accel = 0; +float z_accel = 0; +float x_gyro = 0; +float y_gyro = 0; +float z_gyro = 0; +uint64_t gyro_accel_time = 0; +bool _use_software_mav_throttling{false}; + +int heartbeat_counter = 0; +int imu_counter = 0; +int hil_sensor_counter = 0; +int vision_msg_counter = 0; +int gps_counter = 0; + +vehicle_status_s _vehicle_status{}; +vehicle_control_mode_s _control_mode{}; +actuator_outputs_s _actuator_outputs{}; +battery_status_s _battery_status{}; + +sensor_accel_fifo_s accel_fifo{}; +sensor_gyro_fifo_s gyro_fifo{}; + + +int openPort(const char *dev, speed_t speed); +int closePort(); + +int readResponse(void *buf, size_t len); +int writeResponse(void *buf, size_t len); + +int start(int argc, char *argv[]); +int stop(); +int get_status(); +bool isOpen() { return _uart_fd >= 0; }; + +void usage(); +void task_main(int argc, char *argv[]); + +void *send_actuator(void *); +void send_actuator_data(); + +void handle_message_hil_sensor_dsp(mavlink_message_t *msg); +void handle_message_hil_gps_dsp(mavlink_message_t *msg); +void handle_message_odometry_dsp(mavlink_message_t *msg); +void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg); +void handle_message_command_long_dsp(mavlink_message_t *msg); + +void handle_message_dsp(mavlink_message_t *msg); +void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg); +void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control); + +void +handle_message_dsp(mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor_dsp(msg); + break; + + case MAVLINK_MSG_ID_HIL_GPS: + if (_send_gps) { handle_message_hil_gps_dsp(msg); } + + break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate_dsp(msg); + break; + + case MAVLINK_MSG_ID_ODOMETRY: + handle_message_odometry_dsp(msg); + break; + + case MAVLINK_MSG_ID_COMMAND_LONG: + handle_message_command_long_dsp(msg); + break; + + case MAVLINK_MSG_ID_HEARTBEAT: + PX4_DEBUG("Heartbeat msg received"); + break; + + case MAVLINK_MSG_ID_SYSTEM_TIME: + PX4_DEBUG("MAVLINK SYSTEM TIME"); + break; + + default: + PX4_DEBUG("Unknown msg ID: %d", msg->msgid); + break; + } +} + +void *send_actuator(void *) +{ + send_actuator_data(); + return nullptr; +} + +void send_actuator_data() +{ + + int _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0); + int _vehicle_control_mode_sub_ = orb_subscribe(ORB_ID(vehicle_control_mode)); + int previous_timestamp = 0; + int previous_uorb_timestamp = 0; + int differential = 0; + bool first_sent = false; + + while (true) { + + bool controls_updated = false; + (void) orb_check(_vehicle_control_mode_sub_, &controls_updated); + + if (controls_updated) { + orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub_, &_control_mode); + } + + bool actuator_updated = false; + (void) orb_check(_actuator_outputs_sub, &actuator_updated); + + if (actuator_updated) { + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs); + px4_lockstep_wait_for_components(); + + if (_actuator_outputs.timestamp > 0) { + mavlink_hil_actuator_controls_t hil_act_control; + actuator_controls_from_outputs_dsp(&hil_act_control); + + mavlink_message_t message{}; + mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control); + previous_timestamp = _actuator_outputs.timestamp; + previous_uorb_timestamp = _actuator_outputs.timestamp; + uint8_t newBuf[512]; + uint16_t newBufLen = 0; + newBufLen = mavlink_msg_to_send_buffer(newBuf, &message); + int writeRetval = writeResponse(&newBuf, newBufLen); + PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); + first_sent = true; + send_esc_telemetry_dsp(hil_act_control); + } + + } else if (!actuator_updated && first_sent && differential > 4000) { + mavlink_hil_actuator_controls_t hil_act_control; + actuator_controls_from_outputs_dsp(&hil_act_control); + previous_timestamp = hrt_absolute_time(); + + mavlink_message_t message{}; + mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control); + uint8_t newBuf[512]; + uint16_t newBufLen = 0; + newBufLen = mavlink_msg_to_send_buffer(newBuf, &message); + int writeRetval = writeResponse(&newBuf, newBufLen); + //PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential); + + PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); + send_esc_telemetry_dsp(hil_act_control); + } + + differential = hrt_absolute_time() - previous_timestamp; + } +} + +void task_main(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 's': + _use_software_mav_throttling = true; + break; + + case 'd': + debug = true; + break; + + case 'p': + port = myoptarg; + break; + + case 'b': + baudrate = atoi(myoptarg); + break; + + case 'm': + _send_mag = true; + break; + + case 'g': + _send_gps = true; + break; + + default: + break; + } + } + + const char *charport = port.c_str(); + int openRetval = openPort(charport, (speed_t) baudrate); + int open = isOpen(); + + if (open) { + PX4_ERR("Port is open: %d", openRetval); + } + + uint64_t last_heartbeat_timestamp = hrt_absolute_time(); + uint64_t last_imu_update_timestamp = last_heartbeat_timestamp; + + _px4_accel = new PX4Accelerometer(1310988); + _px4_gyro = new PX4Gyroscope(1310988); + _px4_mag = new PX4Magnetometer(197388); + + // Create a thread for sending data to the simulator. + pthread_t sender_thread; + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(8000)); + pthread_create(&sender_thread, &sender_thread_attr, send_actuator, nullptr); + pthread_attr_destroy(&sender_thread_attr); + + int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub); + + _is_running = true; + + while (!_task_should_exit) { + + uint8_t rx_buf[1024]; + //rx_buf[511] = '\0'; + + uint64_t timestamp = hrt_absolute_time(); + + // Send out sensor messages every 10ms + if (got_first_sensor_msg) { + uint64_t delta_time = timestamp - last_imu_update_timestamp; + + if (delta_time > 15000) { + PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time); + } + + uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time(); + _px4_gyro->update(_px4_gyro_accel_timestamp, x_gyro, y_gyro, z_gyro); + _px4_accel->update(_px4_gyro_accel_timestamp, x_accel, y_accel, z_accel); + last_imu_update_timestamp = timestamp; + imu_counter++; + } + + // Check for incoming messages from the simulator + int readRetval = readResponse(&rx_buf[0], sizeof(rx_buf)); + + if (readRetval) { + //Take readRetval and convert it into mavlink msg + mavlink_message_t msg; + mavlink_status_t _status{}; + + for (int i = 0; i <= readRetval; i++) { + if (mavlink_parse_char(MAVLINK_COMM_0, rx_buf[i], &msg, &_status)) { + //PX4_INFO("Value of msg id: %i", msg.msgid); + handle_message_dsp(&msg); + } + } + } + + if ((timestamp - last_heartbeat_timestamp) > 1000000) { + mavlink_heartbeat_t hb = {}; + mavlink_message_t hb_message = {}; + hb.autopilot = 12; + hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; + mavlink_msg_heartbeat_encode(1, 1, &hb_message, &hb); + + uint8_t hb_newBuf[MAVLINK_MAX_PACKET_LEN]; + uint16_t hb_newBufLen = 0; + hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message); + (void) writeResponse(&hb_newBuf, hb_newBufLen); + last_heartbeat_timestamp = timestamp; + heartbeat_counter++; + } + + bool vehicle_updated = false; + (void) orb_check(_vehicle_status_sub, &vehicle_updated); + + if (vehicle_updated) { + // PX4_INFO("Value of updated vehicle status: %d", vehicle_updated); + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } + + uint64_t elapsed_time = hrt_absolute_time() - timestamp; + // if (elapsed_time < 10000) usleep(10000 - elapsed_time); + + if (elapsed_time < 5000) { usleep(5000 - elapsed_time); } + } + + _is_running = false; +} + +void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control) +{ + esc_status_s esc_status{}; + esc_status.timestamp = hrt_absolute_time(); + const int max_esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX); + + const bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + int max_esc_index = 0; + _battery_status_sub.update(&_battery_status); + + for (int i = 0; i < max_esc_count; i++) { + if (_output_functions[i] != 0) { + max_esc_index = i; + } + + esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... + esc_status.esc[i].timestamp = esc_status.timestamp; + esc_status.esc[i].esc_errorcount = 0; // TODO + esc_status.esc[i].esc_voltage = _battery_status.voltage_v; + esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f : + 0.0f; // TODO: magic number + esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number + esc_status.esc[i].esc_temperature = 20.0 + math::abs_t((double)hil_act_control.controls[i]) * 40.0; + } + + esc_status.esc_count = max_esc_index + 1; + esc_status.esc_armed_flags = (1u << esc_status.esc_count) - 1; + esc_status.esc_online_flags = (1u << esc_status.esc_count) - 1; + + _esc_status_pub.publish(esc_status); +} + + +void +handle_message_command_long_dsp(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (debug) { + PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); + } + + mavlink_command_ack_t ack = {}; + ack.result = MAV_RESULT_UNSUPPORTED; + + mavlink_message_t ack_message = {}; + mavlink_msg_command_ack_encode(1, 1, &ack_message, &ack); + + uint8_t acknewBuf[512]; + uint16_t acknewBufLen = 0; + acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message); + int writeRetval = writeResponse(&acknewBuf, acknewBufLen); + PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); +} + +void +handle_message_vision_position_estimate_dsp(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t vpe; + mavlink_msg_vision_position_estimate_decode(msg, &vpe); + + // fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE + vehicle_odometry_s odom{}; + uint64_t timestamp = hrt_absolute_time(); + odom.timestamp_sample = timestamp; + + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + odom.position[0] = vpe.x; + odom.position[1] = vpe.y; + odom.position[2] = vpe.z; + + const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw)); + q.copyTo(odom.q); + + // VISION_POSITION_ESTIMATE covariance + // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle + // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). + // If unknown, assign NaN value to first element in the array. + odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0 + odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1 + odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2 + + odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3 + odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4 + odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5 + + odom.reset_counter = vpe.reset_counter; + + odom.timestamp = hrt_absolute_time(); + + _visual_odometry_pub.publish(odom); + vision_msg_counter++; +} + +void +handle_message_odometry_dsp(mavlink_message_t *msg) +{ + mavlink_odometry_t odom_in; + mavlink_msg_odometry_decode(msg, &odom_in); + + // fill vehicle_odometry from Mavlink ODOMETRY + vehicle_odometry_s odom{}; + uint64_t timestamp = hrt_absolute_time(); + odom.timestamp_sample = timestamp; + + const matrix::Vector3f odom_in_p(odom_in.x, odom_in.y, odom_in.z); + + // position x/y/z (m) + if (odom_in_p.isAllFinite()) { + // frame_id: Coordinate frame of reference for the pose data. + switch (odom_in.frame_id) { + case MAV_FRAME_LOCAL_NED: + // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + odom_in_p.copyTo(odom.position); + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + odom.position[0] = odom_in.y; // y: North + odom.position[1] = odom_in.x; // x: East + odom.position[2] = -odom_in.z; // z: Up + break; + + case MAV_FRAME_LOCAL_FRD: + // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; + odom_in_p.copyTo(odom.position); + break; + + case MAV_FRAME_LOCAL_FLU: + // FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. + odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD; + odom.position[0] = odom_in.x; // x: Forward + odom.position[1] = -odom_in.y; // y: Left + odom.position[2] = -odom_in.z; // z: Up + break; + + default: + break; + } + + // pose_covariance + // Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw) + // first six entries are the first ROW, next five entries are the second ROW, etc. + if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) { + switch (odom_in.frame_id) { + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_FRD: + case MAV_FRAME_LOCAL_FLU: + // position variances copied directly + odom.position_variance[0] = odom_in.pose_covariance[0]; // X row 0, col 0 + odom.position_variance[1] = odom_in.pose_covariance[6]; // Y row 1, col 1 + odom.position_variance[2] = odom_in.pose_covariance[11]; // Z row 2, col 2 + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.position_variance[0] = odom_in.pose_covariance[6]; // Y row 1, col 1 + odom.position_variance[1] = odom_in.pose_covariance[0]; // X row 0, col 0 + odom.position_variance[2] = odom_in.pose_covariance[11]; // Z row 2, col 2 + break; + + default: + break; + } + } + } + + // q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame + if (matrix::Quatf(odom_in.q).isAllFinite()) { + + odom.q[0] = odom_in.q[0]; + odom.q[1] = odom_in.q[1]; + odom.q[2] = odom_in.q[2]; + odom.q[3] = odom_in.q[3]; + + // pose_covariance (roll, pitch, yaw) + // states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc. + // TODO: fix pose_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU + if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) { + odom.orientation_variance[0] = odom_in.pose_covariance[15]; // R row 3, col 3 + odom.orientation_variance[1] = odom_in.pose_covariance[18]; // P row 4, col 4 + odom.orientation_variance[2] = odom_in.pose_covariance[20]; // Y row 5, col 5 + } + } + + const matrix::Vector3f odom_in_v(odom_in.vx, odom_in.vy, odom_in.vz); + + // velocity vx/vy/vz (m/s) + if (odom_in_v.isAllFinite()) { + // child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data. + switch (odom_in.child_frame_id) { + case MAV_FRAME_LOCAL_NED: + // NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; + odom_in_v.copyTo(odom.velocity); + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED; + odom.velocity[0] = odom_in.vy; // y: East + odom.velocity[1] = odom_in.vx; // x: North + odom.velocity[2] = -odom_in.vz; // z: Up + break; + + case MAV_FRAME_LOCAL_FRD: + // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD; + odom_in_v.copyTo(odom.velocity); + break; + + case MAV_FRAME_LOCAL_FLU: + // FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD; + odom.velocity[0] = odom_in.vx; // x: Forward + odom.velocity[1] = -odom_in.vy; // y: Left + odom.velocity[2] = -odom_in.vz; // z: Up + break; + + case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_FRD: + // FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. + odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; + odom.velocity[0] = odom_in.vx; + odom.velocity[1] = odom_in.vy; + odom.velocity[2] = odom_in.vz; + break; + + default: + // unsupported child_frame_id + break; + } + + // velocity_covariance (vx, vy, vz) + // states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc. + // TODO: fix velocity_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU, MAV_FRAME_LOCAL_FLU + if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) { + switch (odom_in.child_frame_id) { + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_FRD: + case MAV_FRAME_LOCAL_FLU: + case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08). + case MAV_FRAME_BODY_FRD: + // velocity covariances copied directly + odom.velocity_variance[0] = odom_in.velocity_covariance[0]; // X row 0, col 0 + odom.velocity_variance[1] = odom_in.velocity_covariance[6]; // Y row 1, col 1 + odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z row 2, col 2 + break; + + case MAV_FRAME_LOCAL_ENU: + // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. + odom.velocity_variance[0] = odom_in.velocity_covariance[6]; // Y row 1, col 1 + odom.velocity_variance[1] = odom_in.velocity_covariance[0]; // X row 0, col 0 + odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z row 2, col 2 + break; + + default: + // unsupported child_frame_id + break; + } + } + } + + // Roll/Pitch/Yaw angular speed (rad/s) + if (PX4_ISFINITE(odom_in.rollspeed) + && PX4_ISFINITE(odom_in.pitchspeed) + && PX4_ISFINITE(odom_in.yawspeed)) { + + odom.angular_velocity[0] = odom_in.rollspeed; + odom.angular_velocity[1] = odom_in.pitchspeed; + odom.angular_velocity[2] = odom_in.yawspeed; + } + + odom.reset_counter = odom_in.reset_counter; + odom.quality = odom_in.quality; + + switch (odom_in.estimator_type) { + case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support + case MAV_ESTIMATOR_TYPE_NAIVE: + case MAV_ESTIMATOR_TYPE_VISION: + case MAV_ESTIMATOR_TYPE_VIO: + odom.timestamp = hrt_absolute_time(); + _visual_odometry_pub.publish(odom); + break; + + case MAV_ESTIMATOR_TYPE_MOCAP: + odom.timestamp = hrt_absolute_time(); + _mocap_odometry_pub.publish(odom); + break; + + case MAV_ESTIMATOR_TYPE_GPS: + case MAV_ESTIMATOR_TYPE_GPS_INS: + case MAV_ESTIMATOR_TYPE_LIDAR: + case MAV_ESTIMATOR_TYPE_AUTOPILOT: + default: + //mavlink_log_critical(&_mavlink_log_pub, "ODOMETRY: estimator_type %" PRIu8 " unsupported\t", + // odom_in.estimator_type); + //events::send(events::ID("mavlink_rcv_odom_unsup_estimator_type"), events::Log::Error, + // "ODOMETRY: unsupported estimator_type {1}", odom_in.estimator_type); + return; + } +} + +void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg) +{ + memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t)); + + msg->time_usec = hrt_absolute_time(); + + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (armed) { + for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { + msg->controls[i] = _actuator_outputs.output[i]; + } + + //PX4_INFO("Value of actuator data: %f, %f, %f, %f", (double)msg->controls[0], (double)msg->controls[1], (double)msg->controls[2], (double)msg->controls[3]); + } + + msg->mode = mode_flag_custom; + msg->mode |= (armed) ? mode_flag_armed : 0; + msg->flags = 0; + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + msg->flags |= 1; +#endif +} + +int openPort(const char *dev, speed_t speed) +{ + if (_uart_fd >= 0) { + PX4_ERR("Port in use: %s (%i)", dev, errno); + return -1; + } + + _uart_fd = qurt_uart_open(dev, speed); + PX4_DEBUG("qurt_uart_opened"); + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + } + + return 0; +} + +int closePort() +{ + _uart_fd = -1; + + return 0; +} + +int readResponse(void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for reading or buffer"); + return -1; + } + + return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); +} + +int writeResponse(void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + + return qurt_uart_write(_uart_fd, (const char *) buf, len); +} + +int start(int argc, char *argv[]) +{ + if (_is_running) { + PX4_WARN("already running"); + return -1; + } + + _task_should_exit = false; + + _task_handle = px4_task_spawn_cmd("dsp_hitl__main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t)&task_main, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +int stop() +{ + if (!_is_running) { + PX4_WARN("not running"); + return -1; + } + + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; + return 0; +} + +void usage() +{ + PX4_INFO("Usage: dsp_hitl {start|info|status|stop}"); +} + +int get_status() +{ + PX4_INFO("Running: %s", _is_running ? "yes" : "no"); + PX4_INFO("Status of IMU_Data counter: %i", imu_counter); + PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel)); + PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro)); + PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter); + PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter); + PX4_INFO("Value of Vision data counter: %i", vision_msg_counter); + PX4_INFO("Value of GPS Data counter: %i", gps_counter); + return 0; +} + +uint64_t first_sensor_msg_timestamp = 0; +uint64_t first_sensor_report_timestamp = 0; +uint64_t last_sensor_report_timestamp = 0; + +void +handle_message_hil_sensor_dsp(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t hil_sensor; + mavlink_msg_hil_sensor_decode(msg, &hil_sensor); + + // temperature only updated with baro + gyro_accel_time = hrt_absolute_time(); + + // temperature only updated with baro + float temperature = NAN; + + got_first_sensor_msg = true; + + if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + temperature = hil_sensor.temperature; + } + + // gyro + if ((hil_sensor.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) { + if (_px4_gyro == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(1310988); + } + + if (_px4_gyro != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_gyro->set_temperature(temperature); + } + + x_gyro = hil_sensor.xgyro; + y_gyro = hil_sensor.ygyro; + z_gyro = hil_sensor.zgyro; + } + } + + // accelerometer + if ((hil_sensor.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) { + if (_px4_accel == nullptr) { + // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel = new PX4Accelerometer(1310988); + } + + if (_px4_accel != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_accel->set_temperature(temperature); + } + + x_accel = hil_sensor.xacc; + y_accel = hil_sensor.yacc; + z_accel = hil_sensor.zacc; + } + } + + + // magnetometer + if ((_send_mag) && ((hil_sensor.fields_updated & SensorSource::MAG) == SensorSource::MAG)) { + if (_px4_mag == nullptr) { + // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + _px4_mag = new PX4Magnetometer(197388); + } + + if (_px4_mag != nullptr) { + if (PX4_ISFINITE(temperature)) { + _px4_mag->set_temperature(temperature); + } + + _px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag); + } + } + + // baro + if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) { + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = gyro_accel_time; + sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + sensor_baro.pressure = hil_sensor.abs_pressure * 100.0f; // hPa to Pa + sensor_baro.temperature = hil_sensor.temperature; + sensor_baro.error_count = 0; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + } + + // differential pressure + if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) { + differential_pressure_s report{}; + report.timestamp_sample = gyro_accel_time; + report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION + report.temperature = hil_sensor.temperature; + report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa + report.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(report); + } + + // battery status + { + battery_status_s hil_battery_status{}; + + hil_battery_status.timestamp = gyro_accel_time; + hil_battery_status.voltage_v = 16.0f; + hil_battery_status.voltage_filtered_v = 16.0f; + hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; + hil_battery_status.connected = true; + hil_battery_status.remaining = 0.70; + hil_battery_status.time_remaining_s = NAN; + + _battery_pub.publish(hil_battery_status); + } + hil_sensor_counter++; +} + +void +handle_message_hil_gps_dsp(mavlink_message_t *msg) +{ + mavlink_hil_gps_t hil_gps; + mavlink_msg_hil_gps_decode(msg, &hil_gps); + + sensor_gps_s gps{}; + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = 1; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + + gps.device_id = device_id.devid; + + gps.lat = hil_gps.lat; + gps.lon = hil_gps.lon; + gps.alt = hil_gps.alt; + gps.alt_ellipsoid = hil_gps.alt; + + gps.s_variance_m_s = 0.25f; + gps.c_variance_rad = 0.5f; + gps.fix_type = hil_gps.fix_type; + + gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m + gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m + + gps.hdop = 0; // TODO + gps.vdop = 0; // TODO + + gps.noise_per_ms = 0; + gps.automatic_gain_control = 0; + gps.jamming_indicator = 0; + gps.jamming_state = 0; + gps.spoofing_state = 0; + + gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s + gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s + gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s + gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s + gps.cog_rad = ((hil_gps.cog == 65535) ? (float)NAN : matrix::wrap_2pi(math::radians( + hil_gps.cog * 1e-2f))); // cdeg -> rad + gps.vel_ned_valid = true; + + gps.timestamp_time_relative = 0; + gps.time_utc_usec = hil_gps.time_usec; + + gps.satellites_used = hil_gps.satellites_visible; + + gps.heading = NAN; + gps.heading_offset = NAN; + + gps.timestamp = hrt_absolute_time(); + + _sensor_gps_pub.publish(gps); + gps_counter++; +} + +} +int dsp_hitl_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + dsp_hitl::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + + if (!strcmp(verb, "start")) { + return dsp_hitl::start(argc - 1, argv + 1); + } + + else if (!strcmp(verb, "stop")) { + return dsp_hitl::stop(); + } + + else if (!strcmp(verb, "status")) { + return dsp_hitl::get_status(); + } + + else { + dsp_hitl::usage(); + return 1; + } +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt new file mode 100644 index 0000000000..ccb76edadc --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 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__dsp_sbus + MAIN dsp_sbus + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + dsp_sbus.cpp + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp new file mode 100644 index 0000000000..e6b622b403 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp @@ -0,0 +1,383 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "protocol.h" + +#define ASYNC_UART_READ_WAIT_US 2000 + + +extern "C" { __EXPORT int dsp_sbus_main(int argc, char *argv[]); } + +namespace dsp_sbus +{ + +std::string _port = "7"; +int _uart_fd = -1; +IOPacket _packet; +bool _initialized = false; +bool _is_running = false; +uint64_t _rc_last_valid; ///< last valid timestamp +uint16_t _rc_valid_update_count = 0; + +static px4_task_t _task_handle = -1; + +uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + +int bus_exchange(IOPacket *packet) +{ + int ret = 0; + int read_retries = 3; + int read_succeeded = 0; + int packet_size = sizeof(IOPacket); + + (void) qurt_uart_write(_uart_fd, (const char *) packet, packet_size); + + usleep(100); + + // The UART read on SLPI is via an asynchronous service so specify a timeout + // for the return. The driver will poll periodically until the read comes in + // so this may block for a while. However, it will timeout if no read comes in. + while (read_retries) { + ret = qurt_uart_read(_uart_fd, (char *) packet, packet_size, ASYNC_UART_READ_WAIT_US); + + if (ret) { + // PX4_INFO("Read %d bytes", ret); + + /* Check CRC */ + uint8_t crc = packet->crc; + packet->crc = 0; + + if (crc != crc_packet(packet)) { + PX4_ERR("PX4IO packet CRC error"); + return -EIO; + + } else if (PKT_CODE(*packet) == PKT_CODE_CORRUPT) { + PX4_ERR("PX4IO packet corruption"); + return -EIO; + + } else { + read_succeeded = 1; + break; + } + } + + PX4_ERR("Read attempt %d failed", read_retries); + read_retries--; + } + + + if (! read_succeeded) { + return -EIO; + } + + return 0; +} + +int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + /* range check the transfer */ + // if (num_values > ((_max_transfer) / sizeof(*values))) { + // PX4_ERR("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + // return -1; + // } + + // int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + int ret = 0; + + _packet.count_code = num_values | PKT_CODE_READ; + _packet.page = page; + _packet.offset = offset; + + _packet.crc = 0; + _packet.crc = crc_packet(&_packet); + + ret = bus_exchange(&_packet); + + if (ret != 0) { + // PX4_ERR("px4io io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); + return -1; + } + + memcpy(values, &_packet.regs[0], num_values * 2); + + return OK; +} + +uint32_t io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1) != OK) { + // Registers are only 16 bit so any value over 0xFFFF can signal a fault + return 0xFFFFFFFF; + } + + return value; +} + +int initialize() +{ + if (_initialized) { + // Already successfully initialized + return 0; + } + + if (_uart_fd < 0) { + _uart_fd = qurt_uart_open(_port.c_str(), 921600); + } + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + } + + // Verify connectivity and version number + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + PX4_ERR("dsp_sbus version error: %u", protocol); + _uart_fd = -1; + return -1; + } + + _initialized = true; + + return 0; +} + +void dsp_sbus_task() +{ + + uint16_t status_regs[2] {}; + input_rc_s rc_val; + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t rc_regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; + uint32_t channel_count = 0; + + _is_running = true; + + while (true) { + + usleep(20000); // Update every 20ms + + memset(&rc_val, 0, sizeof(input_rc_s)); + + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status_regs[0], + sizeof(status_regs) / sizeof(status_regs[0])) == OK) { + // PX4_INFO("dsp_sbus status 0x%.4x", status_regs[0]); + // PX4_INFO("dsp_sbus alarms 0x%.4x", status_regs[1]); + } else { + // PX4_ERR("Failed to read status / alarm registers"); + continue; + } + + /* fetch values from IO */ + + // When starting the RC flag will not be okay if the receiver isn't + // getting a signal from the transmitter. Once it does, then this flag + // will say okay even if later the signal is lost. + if (!(status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_OK)) { + // PX4_INFO("RC lost status flag set"); + rc_val.rc_lost = true; + + } else { + // PX4_INFO("RC lost status flag is not set"); + rc_val.rc_lost = false; + } + + if (status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + // PX4_INFO("Got valid SBUS"); + + } else { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; + // PX4_INFO("SBUS not valid"); + } + + rc_val.timestamp = hrt_absolute_time(); + + // No point in reading the registers if we haven't acquired a transmitter signal yet + if (! rc_val.rc_lost) { + if (io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &rc_regs[0], + sizeof(rc_regs) / sizeof(rc_regs[0])) != OK) { + // PX4_ERR("Failed to read RC registers"); + continue; + // } else { + // PX4_INFO("Successfully read RC registers"); + // PX4_INFO("Prolog: %u 0x%.4x 0x%.4x 0x%.4x 0x%.4x 0x%.4x", + // rc_regs[0], rc_regs[1], rc_regs[2], rc_regs[3], rc_regs[4], rc_regs[5]); + } + + channel_count = rc_regs[PX4IO_P_RAW_RC_COUNT]; + + // const uint16_t rc_valid_update_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + // const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count); + // + // if (!rc_updated) { + // PX4_INFO("Didn't get an RC update indication. %u %u", rc_valid_update_count, _rc_valid_update_count); + // continue; + // } + // + // _rc_valid_update_count = rc_valid_update_count; + // + // PX4_INFO("Got an RC update indication"); + + /* limit the channel count */ + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + // PX4_INFO("Got %u for channel count. Limiting to 18", channel_count); + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + rc_val.channel_count = channel_count; + // PX4_INFO("RC channel count: %u", rc_val.channel_count); + + // rc_val.rc_ppm_frame_length = rc_regs[PX4IO_P_RAW_RC_DATA]; + rc_val.rc_ppm_frame_length = 0; + + rc_val.rc_failsafe = (rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + // rc_val.rc_lost = !(rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); + rc_val.rc_lost = rc_val.rc_failsafe; + rc_val.rc_lost_frame_count = rc_regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + rc_val.rc_total_frame_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + + if (!rc_val.rc_lost && !rc_val.rc_failsafe) { + _rc_last_valid = rc_val.timestamp; + rc_val.rssi = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + rc_val.link_quality = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + rc_val.values[i] = rc_regs[prolog + i]; + // PX4_INFO("RC channel %u: %.4u", i, rc_val.values[i]); + } + + /* zero the remaining fields */ + for (unsigned i = channel_count; i < (sizeof(rc_val.values) / sizeof(rc_val.values[0])); i++) { + rc_val.values[i] = 0; + } + } + + rc_val.timestamp_last_signal = _rc_last_valid; + } + + _rc_pub.publish(rc_val); + + } +} + +int start(int argc, char *argv[]) +{ + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "p:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + _port = myoptarg; + PX4_INFO("Setting port to %s", _port.c_str()); + break; + + default: + break; + } + } + + if (! _initialized) { + if (initialize()) { + return -1; + } + } + + if (_is_running) { + PX4_WARN("Already started"); + return 0; + } + + _task_handle = px4_task_spawn_cmd("dsp_sbus_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t) &dsp_sbus_task, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: dsp_sbus start [options]"); + PX4_INFO("Options: -p uart port number"); +} + +} // End namespance dsp_sbus + +int dsp_sbus_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + dsp_sbus::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return dsp_sbus::start(argc - 1, argv + 1); + + } else { + dsp_sbus::usage(); + return -1; + } + + return 0; +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h new file mode 100644 index 0000000000..3cfcee2d12 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h @@ -0,0 +1,405 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * @file protocol.h + * + * PX4IO interface protocol. + * + * @author Lorenz Meier + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Some pages are read- or write-only. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. + */ + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) + +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_MAX_TRANSFER_LEN 64 + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ + +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ + +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 50 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS_PAD 5 + +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; +/* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +/* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ + +#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */ + +#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ + +#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ + +#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ + +#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */ + +#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ + +#define PX4IO_THERMAL_IGNORE UINT16_MAX +#define PX4IO_THERMAL_OFF 0 +#define PX4IO_THERMAL_FULL 10000 + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 52 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM mtrim values for central position */ +#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2) +#error The max transfer length of the IO protocol must not be larger than the IO packet size +#endif + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } + + return c; +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt new file mode 100644 index 0000000000..a16e655a67 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2023 ModalAI, Inc. 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__elrs_led + MAIN elrs_led + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + elrs_led.cpp + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp new file mode 100644 index 0000000000..a40bd8eda2 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (c) 2023 ModalAI, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include "elrs_led.h" + + +extern "C" { __EXPORT int elrs_led_main(int argc, char *argv[]); } + +namespace elrs_led +{ + +std::string _port = "7"; +int _uart_fd = -1; +bool _initialized = false; +bool _is_running = false; +static bool _debug = false; + +static GENERIC_CRC8 crsf_crc{}; +static LEDState _state = LEDState::DEFAULT; +static ControllerInput _off = ControllerInput::DEFAULT; +static ControllerInput _on = ControllerInput::DEFAULT; +static ControllerInput _ir = ControllerInput::DEFAULT; +static ControllerInput _cmd = ControllerInput::DEFAULT; +static ControllerInput _prev_cmd = ControllerInput::DEFAULT; +static std::map ControllerInputMap{ + {ControllerInput::DLEFT, "DLEFT"}, + {ControllerInput::DRIGHT, "DRIGHT"}, + {ControllerInput::DDOWN, "DDOWN"}, + {ControllerInput::DUP, "DUP"}, + {ControllerInput::BACK, "BACK"}, + {ControllerInput::START, "START"}, + {ControllerInput::Y, "Y"}, + {ControllerInput::B, "B"}, + {ControllerInput::A, "A"}, + {ControllerInput::X, "X"}, + {ControllerInput::STICK_RIGHT, "STICK_RIGHT"}, + {ControllerInput::STICK_LEFT, "STICK_LEFT"}, + {ControllerInput::BUMPER_RIGHT, "BUMPER_RIGHT"}, + {ControllerInput::BUMPER_LEFT, "BUMPER_LEFT"}, + {ControllerInput::DEFAULT, "Unkown"} +}; +static px4_task_t _task_handle = -1; +void debug_info(LEDState, uint8_t *); +void make_packet(LEDState, uint8_t *); + +int initialize() +{ + if (_initialized) { + // Already successfully initialized + return 0; + } + + if (_uart_fd < 0) { + _uart_fd = qurt_uart_open(_port.c_str(), 420000); + } + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + } + + _initialized = true; + + return 0; +} + +void elrs_led_task() +{ + + PX4_INFO("Starting task for elrs_led"); + + int ret = 0; + int manual_control_input_fd = orb_subscribe(ORB_ID(manual_control_input)); + uint8_t pwmPacket[11] = {0xEC, 0x09, 0x32, 0x70, 0x77, 0x6D, 0x07, 0x75, 0x00, 0x00, 0x00}; + + px4_pollfd_struct_t fds[1] = { { .fd = manual_control_input_fd, .events = POLLIN } }; + + struct manual_control_setpoint_s setpoint_req; + + _is_running = true; + + while (true) { + px4_poll(fds, 1, 10000); + + if (fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(manual_control_input), manual_control_input_fd, &setpoint_req); + + _cmd = (ControllerInput)setpoint_req.aux1; + + // skip duplicate cmds + if (_cmd == _prev_cmd) { + continue; + } + + if (_cmd == _off) { + _prev_cmd = _cmd; + _state = LEDState::OFF; + make_packet(_state, pwmPacket); + ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket)); + + if (_debug) { + debug_info(_state, pwmPacket); + } + + } else if (_cmd == _on) { + _prev_cmd = _cmd; + _state = LEDState::ON; + make_packet(_state, pwmPacket); + ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket)); + + if (_debug) { + debug_info(_state, pwmPacket); + } + + } else if (_cmd == _ir) { + _prev_cmd = _cmd; + _state = LEDState::IR; + make_packet(_state, pwmPacket); + ret = qurt_uart_write(_uart_fd, (char *) &pwmPacket[0], sizeof(pwmPacket)); + + if (_debug) { + debug_info(_state, pwmPacket); + } + + } + + } + } +} + +int start(int argc, char *argv[]) +{ + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "p:o:l:i:d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + _port = myoptarg; + break; + + case 'o': + _off = getKey(ControllerInputMap, myoptarg); + break; + + case 'l': + _on = getKey(ControllerInputMap, myoptarg); + break; + + case 'i': + _ir = getKey(ControllerInputMap, myoptarg); + break; + + case 'd': + _debug = true; + break; + + default: + break; + } + } + + if (_debug) { + PX4_INFO("ELRS LED Debug Mode Enabled"); + PX4_INFO("Port: %s", _port.c_str()); + PX4_INFO("Button Configuration:"); + PX4_INFO("\tOn: %s", ControllerInputMap.at(_on).c_str()); + PX4_INFO("\tIR: %s", ControllerInputMap.at(_ir).c_str()); + PX4_INFO("\tOff: %s", ControllerInputMap.at(_off).c_str()); + } + + if (! _initialized) { + if (initialize()) { + return -1; + } + } + + if (_is_running) { + PX4_WARN("Already started"); + return 0; + } + + _task_handle = px4_task_spawn_cmd("elrs_led_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t) &elrs_led_task, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: elrs_led start [options]"); + PX4_INFO("Options: -p uart port number"); + PX4_INFO("Options: -o LEDs off button"); + PX4_INFO("Options: -l Overt LEDs on button"); + PX4_INFO("Options: -i IR LEDs on button"); + PX4_INFO("Options: -d enable debug messages"); +} + +void debug_info(LEDState led_state, uint8_t *pwmPacket) +{ + PX4_INFO(""); + + if (led_state == LEDState::ON) { + PX4_INFO("Turning LEDs on"); + + } else if (led_state == LEDState::OFF) { + PX4_INFO("Turning LEDs off"); + + } else if (led_state == LEDState::IR) { + PX4_INFO("Turning IR LEDs on"); + + } else { + PX4_WARN("ELRS LED: LED state unknown: 0x%x", led_state); + } + + PX4_INFO("Wrote packet: [0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x]", + pwmPacket[0], pwmPacket[1], pwmPacket[2], pwmPacket[3], pwmPacket[4], pwmPacket[5], + pwmPacket[6], pwmPacket[7], pwmPacket[8], pwmPacket[9], pwmPacket[10]); +} + +void make_packet(LEDState led_state, uint8_t *pwmPacket) +{ + if (led_state == LEDState::OFF) { + pwmPacket[8] = 0x03; + pwmPacket[9] = 0x84; + pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0); + + } else if (led_state == LEDState::ON) { + pwmPacket[8] = 0x05; + pwmPacket[9] = 0xAA; + pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0); + + } else if (led_state == LEDState::IR) { + pwmPacket[8] = 0x07; + pwmPacket[9] = 0xFF; + pwmPacket[10] = crsf_crc.calc(&pwmPacket[CRSF_FRAME_NOT_COUNTED_BYTES], PWM_FRAME_SIZE - 1, 0); + + } else { + PX4_WARN("ELRS LED: Unknown LED state."); + } +} + +} // End namespance elrs_led + +int elrs_led_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + elrs_led::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return elrs_led::start(argc - 1, argv + 1); + + } else { + elrs_led::usage(); + return -1; + } + + return 0; +} \ No newline at end of file diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h new file mode 100644 index 0000000000..25de041d4b --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2023 ModalAI, Inc. 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 + +#define crclen 256 +#define CRSF_CRC_POLY 0xd5 +#define CRSF_FRAME_NOT_COUNTED_BYTES 2 +#define PWM_FRAME_SIZE 9 + + +enum class ControllerInput : uint32_t { + DLEFT = 0x2000, + DRIGHT = 0x4000, + DDOWN = 0x1000, + DUP = 0x800, + BACK = 0x10, + START = 0x40, + Y = 0x08, + B = 0x02, + A = 0x01, + X = 0x04, + STICK_RIGHT = 0x100, + STICK_LEFT = 0x80, + BUMPER_RIGHT = 0x400, + BUMPER_LEFT = 0x200, + DEFAULT = 0xFFFFFFFF +}; + +enum class LEDState : uint8_t { + OFF = 0x00, + ON = 0x01, + IR = 0x02, + DEFAULT = 0xFF +}; + + +class GENERIC_CRC8 +{ +public: + GENERIC_CRC8() {}; + uint8_t calc(const uint8_t *data, uint16_t len, uint8_t crc) + { + while (len--) { + crc = crc8tab[crc ^ *data++]; + } + + return crc; + } + +private: + uint8_t crc8tab[crclen] = {0, 213, 127, 170, 254, 43, 129, 84, 41, 252, 86, 131, 215, 2, 168, 125, 82, 135, + 45, 248, 172, 121, 211, 6, 123, 174, 4, 209, 133, 80, 250, 47, 164, 113, 219, 14, + 90, 143, 37, 240, 141, 88, 242, 39, 115, 166, 12, 217, 246, 35, 137, 92, 8, 221, + 119, 162, 223, 10, 160, 117, 33, 244, 94, 139, 157, 72, 226, 55, 99, 182, 28, 201, + 180, 97, 203, 30, 74, 159, 53, 224, 207, 26, 176, 101, 49, 228, 78, 155, 230, 51, + 153, 76, 24, 205, 103, 178, 57, 236, 70, 147, 199, 18, 184, 109, 16, 197, 111, 186, + 238, 59, 145, 68, 107, 190, 20, 193, 149, 64, 234, 63, 66, 151, 61, 232, 188, 105, + 195, 22, 239, 58, 144, 69, 17, 196, 110, 187, 198, 19, 185, 108, 56, 237, 71, 146, + 189, 104, 194, 23, 67, 150, 60, 233, 148, 65, 235, 62, 106, 191, 21, 192, 75, 158, + 52, 225, 181, 96, 202, 31, 98, 183, 29, 200, 156, 73, 227, 54, 25, 204, 102, 179, 231, + 50, 152, 77, 48, 229, 79, 154, 206, 27, 177, 100, 114, 167, 13, 216, 140, 89, 243, + 38, 91, 142, 36, 241, 165, 112, 218, 15, 32, 245, 95, 138, 222, 11, 161, 116, 9, 220, + 118, 163, 247, 34, 136, 93, 214, 3, 169, 124, 40, 253, 87, 130, 255, 42, 128, 85, 1, + 212, 126, 171, 132, 81, 251, 46, 122, 175, 5, 208, 173, 120, 210, 7, 83, 134, 44, 249 + }; +}; + + +ControllerInput getKey(const std::map &map, const std::string &value) +{ + for (const auto &pair : map) { + if (pair.second == value) { + return pair.first; + } + } + + return ControllerInput::DEFAULT; +} + + + diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt new file mode 100644 index 0000000000..ad2576f3e6 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__ghst_rc + MAIN ghst_rc + COMPILE_FLAGS + INCLUDES + ${PX4_SOURCE_DIR}/src/drivers/rc_input + ${PX4_SOURCE_DIR}/src/lib/rc/spektrum_rssi.h + SRCS + ghst_rc.cpp + ghst_rc.hpp + MODULE_CONFIG + module.yaml + DEPENDS + rc + ) diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig new file mode 100644 index 0000000000..0d2b31437f --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_GHST_RC + bool "ghst_rc" + default n + ---help--- + Enable support for ghst rc diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp new file mode 100644 index 0000000000..e213179922 --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "ghst_rc.hpp" +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +using namespace time_literals; + +uint32_t GhstRc::baudrate = GHST_BAUDRATE; + +GhstRc::GhstRc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(GHST_RC_DEFAULT_PORT)) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +GhstRc::~GhstRc() +{ + perf_free(_cycle_interval_perf); + perf_free(_publish_interval_perf); +} + +int GhstRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + break; + + case 'b': + baudrate = atoi(myoptarg); + PX4_INFO("Setting GHST baudrate to %u", baudrate); + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return PX4_ERROR; + } + + if (!device_name) { + PX4_ERR("Valid device required"); + return PX4_ERROR; + } + + GhstRc *instance = new GhstRc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleNow(); + + return PX4_OK; +} + +void GhstRc::fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[GHST_MAX_NUM_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi = -1) +{ + // fill rc_in struct for publishing + _rc_in.channel_count = raw_rc_count_local; + + if (_rc_in.channel_count > GHST_MAX_NUM_CHANNELS) { + _rc_in.channel_count = GHST_MAX_NUM_CHANNELS; + } + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values_local[i]; + + if (raw_rc_values_local[i] != UINT16_MAX) { + valid_chans++; + } + + // once filled, reset values back to default + _raw_rc_values[i] = UINT16_MAX; + } + + _rc_in.timestamp = now; + _rc_in.timestamp_last_signal = _rc_in.timestamp; + _rc_in.rc_ppm_frame_length = 0; + + /* fake rssi if no value was provided */ + if (rssi == -1) { + _rc_in.rssi = 255; + + } else { + _rc_in.rssi = rssi; + } + + if (valid_chans == 0) { + _rc_in.rssi = 0; + } + + _rc_in.rc_failsafe = failsafe; + _rc_in.rc_lost = (valid_chans == 0); + _rc_in.rc_lost_frame_count = frame_drops; + _rc_in.rc_total_frame_count = 0; +} + +void GhstRc::Run() +{ + if (should_exit()) { + ScheduleClear(); + _rc_fd = -1; + exit_and_cleanup(); + return; + } + + if (_rc_fd < 0) { + _rc_fd = qurt_uart_open(_device, baudrate); + + if (_rc_fd < 0) { + PX4_ERR("Error opening port: %s", _device); + return; + } + + if (_rc_fd >= 0) { + _is_singlewire = true; + + // Configure serial port for GHST + ghst_config(_rc_fd); + } + + _rc_in.rssi_dbm = NAN; + _rc_in.link_quality = -1; + + } + + const hrt_abstime time_now_us = hrt_absolute_time(); + const hrt_abstime cycle_timestamp = time_now_us; + perf_count_interval(_cycle_interval_perf, time_now_us); + + // Read all available data from the serial RC input UART + int new_bytes = qurt_uart_read(_rc_fd, (char *) &_rcs_buf[0], RC_MAX_BUFFER_SIZE, 500); + + if (new_bytes > 0) { + _bytes_rx += new_bytes; + int8_t ghst_rssi = -1; + bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &_raw_rc_values[0], &ghst_rssi, + &_raw_rc_count, GHST_MAX_NUM_CHANNELS); + + if (rc_updated) { + _last_packet_seen = time_now_us; + // we have a new GHST frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + + // ghst telemetry works on fmu-v5 + // on other Pixhawk (-related) boards we cannot write to the RC UART + // another option is to use a different UART port +#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + + if (!_ghst_telemetry) { + _ghst_telemetry = new GHSTTelemetry(_rcs_fd); + } + + if (_ghst_telemetry) { + _ghst_telemetry->update(cycle_timestamp); + } + +#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ + } + } + + // If no communication + if (time_now_us - _last_packet_seen > 100_ms) { + // Invalidate link statistics + _rc_in.rssi_dbm = NAN; + _rc_in.link_quality = -1; + } + + // If we have not gotten RC updates specifically + if (time_now_us - _rc_in.timestamp_last_signal > 50_ms) { + _rc_in.rc_lost = 1; + _rc_in.rc_failsafe = 1; + _rc_in.rssi_dbm = NAN; + _rc_in.link_quality = -1; + + } else { + _rc_in.rc_lost = 0; + _rc_in.rc_failsafe = 0; + } + + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + _rc_in.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(_rc_in); + + perf_count(_publish_interval_perf); + + ScheduleDelayed(4_ms); +} + +int GhstRc::print_status() +{ + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + if (_is_singlewire) { + PX4_INFO("Telemetry disabled: Singlewire RC port"); + } + + perf_print_counter(_cycle_interval_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int GhstRc::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int GhstRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module parses the GHST RC uplink protocol and can generate GHST downlink telemetry data + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ghst_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[]) +{ + return GhstRc::main(argc, argv); +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp new file mode 100644 index 0000000000..004fa020fb --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// telemetry +#include +#include +#include +#include +#include + +#define GHST_MAX_NUM_CHANNELS (16) + +class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + GhstRc(const char *device); + ~GhstRc() override; + + /** @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::print_status() */ + int print_status() override; + + void fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[GHST_MAX_NUM_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi); + +private: + void Run() override; + + bool SendTelemetryBattery(const uint16_t voltage, const uint16_t current, const int fuel, const uint8_t remaining); + + bool SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed, + const uint16_t gps_heading, const uint16_t altitude, const uint8_t num_satellites); + + bool SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw); + + bool SendTelemetryFlightMode(const char *flight_mode); + + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + input_rc_s _rc_in{}; + int _rc_fd{-1}; + char _device[20] {}; // device / serial port path + bool _is_singlewire{true}; + + static constexpr size_t RC_MAX_BUFFER_SIZE{64}; + uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; + uint32_t _bytes_rx{0}; + + uint16_t _raw_rc_values[GHST_MAX_NUM_CHANNELS] {}; + uint16_t _raw_rc_count{}; + + hrt_abstime _last_packet_seen{0}; + + // telemetry + hrt_abstime _telemetry_update_last{0}; + static constexpr int num_data_types{4}; ///< number of different telemetry data types + int _next_type{0}; + static uint32_t baudrate; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + + perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")}; + + // DEFINE_PARAMETERS( + // (ParamBool) _param_rc_ghst_tel_en + // ) +}; + diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml new file mode 100644 index 0000000000..21be68b97e --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml @@ -0,0 +1,11 @@ +module_name: GHST RC Input Driver +serial_config: + - command: "ghst_rc start -d ${SERIAL_DEV}" + port_config_param: + name: RC_GHST_PRT_CFG + group: Serial + #default: RC + #depends_on_port: RC + description_extended: | + Ghost RC (GHST) driver. + diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp index daf593d80c..50c26171e3 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp @@ -650,8 +650,7 @@ bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) if (ProcessTemperature(buffer.f, valid_samples)) { ProcessGyro(timestamp_sample, buffer.f, valid_samples); ProcessAccel(timestamp_sample, buffer.f, valid_samples); - // Pass only most recent valid sample to IMU server - // ProcessIMU(timestamp_sample, buffer.f[valid_samples - 1]); + ProcessIMU(timestamp_sample, buffer.f, valid_samples); return true; } } @@ -687,79 +686,86 @@ static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, co return static_cast(x); } -void ICM42688P::ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA &fifo) +void ICM42688P::ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - // float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0; - // float gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0; - // - // // 20 bit hires mode - // - // // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) - // // Accel data is 18 bit - // int32_t temp_accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0, - // fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4); - // int32_t temp_accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0, - // fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4); - // int32_t temp_accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0, - // fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4); - // - // // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) - // int32_t temp_gyro_x = reassemble_20bit(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0, - // fifo.Ext_Accel_X_Gyro_X & 0x0F); - // int32_t temp_gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0, - // fifo.Ext_Accel_Y_Gyro_Y & 0x0F); - // int32_t temp_gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0, - // fifo.Ext_Accel_Z_Gyro_Z & 0x0F); + float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0; + float gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0; - // // accel samples invalid if -524288 - // if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) { - // // shift accel by 2 (2 least significant bits are always 0) - // accel_x = (float) temp_accel_x / 4.f; - // accel_y = (float) temp_accel_y / 4.f; - // accel_z = (float) temp_accel_z / 4.f; - // - // // shift gyro by 1 (least significant bit is always 0) - // gyro_x = (float) temp_gyro_x / 2.f; - // gyro_y = (float) temp_gyro_y / 2.f; - // gyro_z = (float) temp_gyro_z / 2.f; - // - // // correct frame for publication - // // sensor's frame is +x forward, +y left, +z up - // // flip y & z to publish right handed with z down (x forward, y right, z down) - // accel_y = -accel_y; - // accel_z = -accel_z; - // gyro_y = -gyro_y; - // gyro_z = -gyro_z; - // - // // Scale everything appropriately - // float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f); - // accel_x *= accel_scale_factor; - // accel_y *= accel_scale_factor; - // accel_z *= accel_scale_factor; - // - // float gyro_scale_factor = math::radians(1.f / 131.f); - // gyro_x *= gyro_scale_factor; - // gyro_y *= gyro_scale_factor; - // gyro_z *= gyro_scale_factor; - // - // // Store the data in our array - // _imu_server_data.accel_x[_imu_server_index] = accel_x; - // _imu_server_data.accel_y[_imu_server_index] = accel_y; - // _imu_server_data.accel_z[_imu_server_index] = accel_z; - // _imu_server_data.gyro_x[_imu_server_index] = gyro_x; - // _imu_server_data.gyro_y[_imu_server_index] = gyro_y; - // _imu_server_data.gyro_z[_imu_server_index] = gyro_z; - // _imu_server_data.ts[_imu_server_index] = timestamp_sample; - // _imu_server_index++; - // - // // If array is full, publish the data - // if (_imu_server_index == 10) { - // _imu_server_index = 0; - // _imu_server_data.timestamp = hrt_absolute_time(); - // _imu_server_data.temperature = 0; // Not used right now - // _imu_server_pub.publish(_imu_server_data); - // } - // } + for (int i = 0; i < samples; i++) { + _imu_server_decimator++; + + if (_imu_server_decimator == 8) { + _imu_server_decimator = 0; + // 20 bit hires mode + + // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) + // Accel data is 18 bit + int32_t temp_accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0, + fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4); + int32_t temp_accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0, + fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4); + int32_t temp_accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0, + fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4); + + // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) + int32_t temp_gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, + fifo[i].Ext_Accel_X_Gyro_X & 0x0F); + int32_t temp_gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, + fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F); + int32_t temp_gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, + fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F); + + // accel samples invalid if -524288 + if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) { + // shift accel by 2 (2 least significant bits are always 0) + accel_x = (float) temp_accel_x / 4.f; + accel_y = (float) temp_accel_y / 4.f; + accel_z = (float) temp_accel_z / 4.f; + + // shift gyro by 1 (least significant bit is always 0) + gyro_x = (float) temp_gyro_x / 2.f; + gyro_y = (float) temp_gyro_y / 2.f; + gyro_z = (float) temp_gyro_z / 2.f; + + // correct frame for publication + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel_y = -accel_y; + accel_z = -accel_z; + gyro_y = -gyro_y; + gyro_z = -gyro_z; + + // Scale everything appropriately + float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f); + accel_x *= accel_scale_factor; + accel_y *= accel_scale_factor; + accel_z *= accel_scale_factor; + + float gyro_scale_factor = math::radians(1.f / 131.f); + gyro_x *= gyro_scale_factor; + gyro_y *= gyro_scale_factor; + gyro_z *= gyro_scale_factor; + + // Store the data in our array + _imu_server_data.accel_x[_imu_server_index] = accel_x; + _imu_server_data.accel_y[_imu_server_index] = accel_y; + _imu_server_data.accel_z[_imu_server_index] = accel_z; + _imu_server_data.gyro_x[_imu_server_index] = gyro_x; + _imu_server_data.gyro_y[_imu_server_index] = gyro_y; + _imu_server_data.gyro_z[_imu_server_index] = gyro_z; + _imu_server_data.ts[_imu_server_index] = timestamp_sample - (125 * (samples - 1 - i)); + _imu_server_index++; + + // If array is full, publish the data + if (_imu_server_index == 10) { + _imu_server_index = 0; + _imu_server_data.timestamp = hrt_absolute_time(); + _imu_server_data.temperature = 0; // Not used right now + _imu_server_pub.publish(_imu_server_data); + } + } + } + } } void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp index c1aa595d70..e5c63dd9f9 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -147,7 +148,7 @@ private: bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); void FIFOReset(); - void ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA &fifo); + void ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples); @@ -226,8 +227,9 @@ private: uint32_t _temperature_samples{0}; // Support for the IMU server - // uint32_t _imu_server_index{0}; - // imu_server_s _imu_server_data; - // uORB::Publication _imu_server_pub{ORB_ID(imu_server)}; + uint32_t _imu_server_index{0}; + uint32_t _imu_server_decimator{0}; + imu_server_s _imu_server_data; + uORB::Publication _imu_server_pub{ORB_ID(imu_server)}; }; diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp index 797df2e632..b3c020bd76 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -133,15 +133,6 @@ enum INT_CONFIG_BIT : uint8_t { INT1_POLARITY = Bit0, }; -// INTF_CONFIG0 -enum INTF_CONFIG0_BIT : uint8_t { - FIFO_HOLD_LAST_DATA_EN = Bit7, - FIFO_COUNT_REC = Bit6, - FIFO_COUNT_ENDIAN = Bit5, - SENSOR_DATA_ENDIAN = Bit4, - UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0, -}; - // FIFO_CONFIG enum FIFO_CONFIG_BIT : uint8_t { // 7:6 FIFO_MODE diff --git a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp index c2405de1fe..0cf350edeb 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp @@ -150,11 +150,15 @@ void task_main(int argc, char *argv[]) newbytes = read(uart_fd, &rx_buf[0], sizeof(rx_buf)); #endif + uint8_t protocol_version = rx_buf[1] & 0x0F; + if (newbytes <= 0) { if (print_msg) { PX4_INFO("Spektrum RC: Read no bytes from UART"); } - } else if (((newbytes != DSM_FRAME_SIZE) || ((rx_buf[1] & 0x0F) != 0x02)) && (! first_correct_frame_received)) { - PX4_ERR("Spektrum RC: Read something other than correct DSM frame on read. Got %d bytes. Protocol byte is 0x%.2x", + } else if (((newbytes != DSM_FRAME_SIZE) || + ((protocol_version != 0x02) && (protocol_version != 0x01))) && + (! first_correct_frame_received)) { + PX4_ERR("Spektrum RC: Invalid DSM frame. %d bytes. Protocol byte 0x%.2x", newbytes, rx_buf[1]); } else { diff --git a/boards/modalai/voxl2/cmake/link_libraries.cmake b/boards/modalai/voxl2/cmake/link_libraries.cmake index 42c9feae35..f250f52e21 100644 --- a/boards/modalai/voxl2/cmake/link_libraries.cmake +++ b/boards/modalai/voxl2/cmake/link_libraries.cmake @@ -1,7 +1,7 @@ -# libfc_sensor.so is provided in the Docker build environment +# Link against the public stub version of the proprietary fc sensor library target_link_libraries(px4 PRIVATE - /home/libfc_sensor.so + ${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so px4_layer ${module_libraries} ) diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index ed1e0ed682..32e334a985 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -2,23 +2,29 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" CONFIG_BOARD_ROOTFSDIR="/data/px4" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y -CONFIG_DRIVERS_OSD_MSP_OSD=y -CONFIG_DRIVERS_QSHELL_POSIX=y -CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y -CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -CONFIG_MODULES_LOGGER=y -CONFIG_MODULES_MAVLINK=y -CONFIG_MODULES_MUORB_APPS=y -CONFIG_MODULES_NAVIGATOR=y -CONFIG_MODULES_RC_UPDATE=y -CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y -CONFIG_SYSTEMCMDS_BSONDUMP=y -CONFIG_SYSTEMCMDS_PARAM=y -CONFIG_SYSTEMCMDS_PERF=y -CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y -CONFIG_SYSTEMCMDS_UORB=y -CONFIG_SYSTEMCMDS_VER=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_PARAM_SERVER=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_QSHELL_POSIX=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_VOXL2_IO=y +CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y +CONFIG_DRIVERS_IMU_SERVER=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_MODULES_MUORB_APPS=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y diff --git a/boards/modalai/voxl2/scripts/build-deps.sh b/boards/modalai/voxl2/scripts/build-deps.sh new file mode 100755 index 0000000000..b76cd0d7fa --- /dev/null +++ b/boards/modalai/voxl2/scripts/build-deps.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +cd src/modules/muorb/apps/libfc-sensor-api +rm -fR build +mkdir build +cd build +CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake .. +make +cd ../../../../../.. + diff --git a/boards/modalai/voxl2/scripts/build-slpi.sh b/boards/modalai/voxl2/scripts/build-slpi.sh index 4c05f8f4f1..cbe9ef6ba9 100755 --- a/boards/modalai/voxl2/scripts/build-slpi.sh +++ b/boards/modalai/voxl2/scripts/build-slpi.sh @@ -6,6 +6,4 @@ source /home/build-env.sh make modalai_voxl2-slpi -cat build/modalai_voxl2-slpi_default/src/lib/version/build_git_version.h - echo "*** End of qurt slpi build ***" diff --git a/boards/modalai/voxl2/scripts/clean.sh b/boards/modalai/voxl2/scripts/clean.sh index 51cef9c3f2..80832b0b70 100755 --- a/boards/modalai/voxl2/scripts/clean.sh +++ b/boards/modalai/voxl2/scripts/clean.sh @@ -1,5 +1,8 @@ #!/bin/bash # Clean out the build artifacts -source /home/build-env.sh -make clean +# source /home/build-env.sh +# make clean + +# This gets rid of everything and starts fresh +sudo rm -fR build diff --git a/boards/modalai/voxl2/scripts/install-voxl-bins.sh b/boards/modalai/voxl2/scripts/install-voxl-bins.sh new file mode 100755 index 0000000000..d58627bcf4 --- /dev/null +++ b/boards/modalai/voxl2/scripts/install-voxl-bins.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# Push slpi image to voxl2 +adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp + +# Push apps processor image to voxl2 +adb push build/modalai_voxl2_default/bin/px4 /usr/bin + +adb shell sync diff --git a/boards/modalai/voxl2/scripts/install-voxl.sh b/boards/modalai/voxl2/scripts/install-voxl.sh index 37f2ede020..6bf21f1e56 100755 --- a/boards/modalai/voxl2/scripts/install-voxl.sh +++ b/boards/modalai/voxl2/scripts/install-voxl.sh @@ -10,14 +10,19 @@ adb push build/modalai_voxl2_default/bin/px4 /usr/bin adb push build/modalai_voxl2_default/bin/px4-alias.sh /usr/bin adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin adb push boards/modalai/voxl2/target/voxl-px4-start /usr/bin +adb push boards/modalai/voxl2/target/voxl-px4-hitl /usr/bin +adb push boards/modalai/voxl2/target/voxl-px4-hitl-start /usr/bin adb shell chmod a+x /usr/bin/px4-alias.sh adb shell chmod a+x /usr/bin/voxl-px4 adb shell chmod a+x /usr/bin/voxl-px4-start +adb shell chmod a+x /usr/bin/voxl-px4-hitl +adb shell chmod a+x /usr/bin/voxl-px4-hitl-start # Push configuration file adb shell mkdir -p /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai +adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai # Make sure to setup all of the needed px4 aliases. adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-accelsim" @@ -34,6 +39,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-commander_tests" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-control_allocator" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-controllib_test" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dataman" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dsp_hitl" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ekf2" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-esc_calib" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ets_airspeed" @@ -65,7 +71,9 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mb12xx" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_att_control" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_pos_control" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-measairspeedsim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-microdds_client" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mixer" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-voxl2_io_bridge" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-motor_ramp" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-modalai_gps_timer" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms4525_airspeed" @@ -119,6 +127,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rc_controller" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uart_esc_driver" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-flight_mode_manager" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-imu_server" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-apps_sbus" # Make sure any required directories exist adb shell "/bin/mkdir -p /data/px4/param" diff --git a/boards/modalai/voxl2/scripts/patch-gazebo.sh b/boards/modalai/voxl2/scripts/patch-gazebo.sh new file mode 100755 index 0000000000..9e8b22cf60 --- /dev/null +++ b/boards/modalai/voxl2/scripts/patch-gazebo.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/src +patch < ../../../../../boards/modalai/voxl2/gazebo-docker/patch/mavlink_interface.patch +cd - + +cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_hitl +patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_hitl.patch +cd - + +cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_vision +patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_vision.patch +cd - \ No newline at end of file diff --git a/boards/modalai/voxl2/src/CMakeLists.txt b/boards/modalai/voxl2/src/CMakeLists.txt index f23ed05ec1..97cc043a14 100644 --- a/boards/modalai/voxl2/src/CMakeLists.txt +++ b/boards/modalai/voxl2/src/CMakeLists.txt @@ -41,3 +41,6 @@ add_library(drivers_board board_config.h init.c ) + +# Add custom drivers +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus) diff --git a/boards/modalai/voxl2/src/board_config.h b/boards/modalai/voxl2/src/board_config.h index 9fe0f35807..a83f28966b 100644 --- a/boards/modalai/voxl2/src/board_config.h +++ b/boards/modalai/voxl2/src/board_config.h @@ -51,4 +51,5 @@ #define BOARD_OVERRIDE_UUID "MODALAIVOXL20000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_VOXL2 -#define MODAL_IO_DEFAULT_PORT "2" +#define VOXL_ESC_DEFAULT_PORT "2" +#define VOXL2_IO_DEFAULT_PORT "2" \ No newline at end of file diff --git a/boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt b/boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt new file mode 100644 index 0000000000..1fd4c37c21 --- /dev/null +++ b/boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2015 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__apps_sbus + MAIN apps_sbus + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + apps_sbus.cpp + ) diff --git a/boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp b/boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp new file mode 100644 index 0000000000..d708fea029 --- /dev/null +++ b/boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp @@ -0,0 +1,453 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "protocol.h" + +extern "C" { __EXPORT int apps_sbus_main(int argc, char *argv[]); } + +namespace apps_sbus +{ + +std::string _port = ""; +int _uart_fd = -1; +IOPacket _packet; +bool _initialized = false; +bool _is_running = false; +uint64_t _rc_last_valid; ///< last valid timestamp +uint16_t _rc_valid_update_count = 0; + +static px4_task_t _task_handle = -1; + +uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + +int bus_exchange(IOPacket *packet) +{ + int ret = 0; + int read_retries = 3; + int read_succeeded = 0; + int packet_size = sizeof(IOPacket); + + tcflush(_uart_fd, TCIOFLUSH); + + ret = ::write(_uart_fd, packet, packet_size); + + usleep(100); + + struct pollfd fd_monitor; + fd_monitor.fd = _uart_fd; + fd_monitor.events = POLLIN; + + while (read_retries) { + (void) ::poll(&fd_monitor, 1, 1000); + ret = ::read(_uart_fd, packet, sizeof(IOPacket)); + + if (ret) { + // PX4_INFO("Read %d bytes", ret); + + /* Check CRC */ + uint8_t crc = packet->crc; + packet->crc = 0; + + if (crc != crc_packet(packet)) { + PX4_ERR("PX4IO packet CRC error"); + return -EIO; + + } else if (PKT_CODE(*packet) == PKT_CODE_CORRUPT) { + PX4_ERR("PX4IO packet corruption"); + return -EIO; + + } else { + read_succeeded = 1; + break; + } + } + + PX4_ERR("Read attempt %d failed", read_retries); + read_retries--; + } + + + if (! read_succeeded) { + return -EIO; + } + + return 0; +} + +int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + /* range check the transfer */ + // if (num_values > ((_max_transfer) / sizeof(*values))) { + // PX4_ERR("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + // return -1; + // } + + // int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + int ret = 0; + + _packet.count_code = num_values | PKT_CODE_READ; + _packet.page = page; + _packet.offset = offset; + + _packet.crc = 0; + _packet.crc = crc_packet(&_packet); + + ret = bus_exchange(&_packet); + + if (ret != 0) { + // PX4_ERR("px4io io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); + return -1; + } + + memcpy(values, &_packet.regs[0], num_values * 2); + + return OK; +} + +uint32_t io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1) != OK) { + // Registers are only 16 bit so any value over 0xFFFF can signal a fault + return 0xFFFFFFFF; + } + + return value; +} + +int initialize() +{ + if (_initialized) { + // Already successfully initialized + return 0; + } + + if (_uart_fd < 0) { + _uart_fd = open("/dev/ttyHS1", O_RDWR | O_NONBLOCK); + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + + } else { + PX4_INFO("serial port fd %d", _uart_fd); + } + + // Configuration copied from dsm_config + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, B921600)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return -1; + } + + if ((termios_state = cfsetospeed(&uart_config, B921600)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return -1; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return -1; + } + + } + + if (_uart_fd < 0) { + PX4_ERR("Open failed in %s", __FUNCTION__); + return -1; + } + + // Verify connectivity and version number + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + PX4_ERR("apps_sbus version error: %u", protocol); + _uart_fd = -1; + return -1; + } + + _initialized = true; + + return 0; +} + +void apps_sbus_task() +{ + + uint16_t status_regs[2] {}; + input_rc_s rc_val; + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t rc_regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; + uint32_t channel_count = 0; + + _is_running = true; + + while (true) { + + usleep(20000); // Update every 20ms + + memset(&rc_val, 0, sizeof(input_rc_s)); + + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status_regs[0], + sizeof(status_regs) / sizeof(status_regs[0])) == OK) { + // PX4_INFO("apps_sbus status 0x%.4x", status_regs[0]); + // PX4_INFO("apps_sbus alarms 0x%.4x", status_regs[1]); + } else { + // PX4_ERR("Failed to read status / alarm registers"); + continue; + } + + /* fetch values from IO */ + + // When starting the RC flag will not be okay if the receiver isn't + // getting a signal from the transmitter. Once it does, then this flag + // will say okay even if later the signal is lost. + if (!(status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_OK)) { + // PX4_INFO("RC lost status flag set"); + rc_val.rc_lost = true; + + } else { + // PX4_INFO("RC lost status flag is not set"); + rc_val.rc_lost = false; + } + + if (status_regs[0] & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + // PX4_INFO("Got valid SBUS"); + + } else { + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; + // PX4_INFO("SBUS not valid"); + } + + rc_val.timestamp = hrt_absolute_time(); + + // No point in reading the registers if we haven't acquired a transmitter signal yet + if (! rc_val.rc_lost) { + if (io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &rc_regs[0], + sizeof(rc_regs) / sizeof(rc_regs[0])) != OK) { + // PX4_ERR("Failed to read RC registers"); + continue; + // } else { + // PX4_INFO("Successfully read RC registers"); + // PX4_INFO("Prolog: %u 0x%.4x 0x%.4x 0x%.4x 0x%.4x 0x%.4x", + // rc_regs[0], rc_regs[1], rc_regs[2], rc_regs[3], rc_regs[4], rc_regs[5]); + } + + channel_count = rc_regs[PX4IO_P_RAW_RC_COUNT]; + + // const uint16_t rc_valid_update_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + // const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count); + // + // if (!rc_updated) { + // PX4_INFO("Didn't get an RC update indication. %u %u", rc_valid_update_count, _rc_valid_update_count); + // continue; + // } + // + // _rc_valid_update_count = rc_valid_update_count; + // + // PX4_INFO("Got an RC update indication"); + + /* limit the channel count */ + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + // PX4_INFO("Got %u for channel count. Limiting to 18", channel_count); + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + rc_val.channel_count = channel_count; + // PX4_INFO("RC channel count: %u", rc_val.channel_count); + + // rc_val.rc_ppm_frame_length = rc_regs[PX4IO_P_RAW_RC_DATA]; + rc_val.rc_ppm_frame_length = 0; + + rc_val.rc_failsafe = (rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + // rc_val.rc_lost = !(rc_regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); + rc_val.rc_lost = rc_val.rc_failsafe; + rc_val.rc_lost_frame_count = rc_regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + rc_val.rc_total_frame_count = rc_regs[PX4IO_P_RAW_FRAME_COUNT]; + + if (!rc_val.rc_lost && !rc_val.rc_failsafe) { + _rc_last_valid = rc_val.timestamp; + rc_val.rssi = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + rc_val.link_quality = rc_regs[PX4IO_P_RAW_RC_NRSSI]; + + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + rc_val.values[i] = rc_regs[prolog + i]; + // PX4_INFO("RC channel %u: %.4u", i, rc_val.values[i]); + } + + /* zero the remaining fields */ + for (unsigned i = channel_count; i < (sizeof(rc_val.values) / sizeof(rc_val.values[0])); i++) { + rc_val.values[i] = 0; + } + } + + rc_val.timestamp_last_signal = _rc_last_valid; + } + + _rc_pub.publish(rc_val); + + } +} + +int start(int argc, char *argv[]) +{ + + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "p:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'p': + _port = myoptarg; + PX4_INFO("Setting port to %s", _port.c_str()); + break; + + default: + break; + } + } + + if (! _initialized) { + if (initialize()) { + return -1; + } + } + + if (_is_running) { + PX4_WARN("Already started"); + return 0; + } + + _task_handle = px4_task_spawn_cmd("apps_sbus_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t) &apps_sbus_task, + (char *const *)argv); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: apps_sbus start [options]"); + PX4_INFO("Options: -p uart port number"); +} + +} // End namespance apps_sbus + +int apps_sbus_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + apps_sbus::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return apps_sbus::start(argc - 1, argv + 1); + + } else { + apps_sbus::usage(); + return -1; + } + + return 0; +} diff --git a/boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h b/boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h new file mode 100644 index 0000000000..3cfcee2d12 --- /dev/null +++ b/boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h @@ -0,0 +1,405 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * @file protocol.h + * + * PX4IO interface protocol. + * + * @author Lorenz Meier + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Some pages are read- or write-only. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. + */ + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) + +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_MAX_TRANSFER_LEN 64 + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ + +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ + +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 50 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS_PAD 5 + +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; +/* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +/* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + +#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ +#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ + +#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */ + +#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ + +#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ + +#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ + +#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */ + +#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ + +#define PX4IO_THERMAL_IGNORE UINT16_MAX +#define PX4IO_THERMAL_OFF 0 +#define PX4IO_THERMAL_FULL 10000 + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 52 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ +#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM mtrim values for central position */ +#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#if (PX4IO_MAX_TRANSFER_LEN > PKT_MAX_REGS * 2) +#error The max transfer length of the IO protocol must not be larger than the IO packet size +#endif + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = { + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } + + return c; +} diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index 38560b47a0..3ce4309ee2 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -4,7 +4,9 @@ CONFIG_FILE="/etc/modalai/voxl-px4.conf" GPS=NONE RC=SPEKTRUM -IMU_ROTATION=NONE +ESC=VOXL_ESC +POWER_MANAGER=VOXLPM +DISTANCE_SENSOR=NONE OSD=DISABLE DAEMON_MODE=DISABLE SENSOR_CAL=ACTUAL @@ -39,9 +41,8 @@ fi print_usage() { echo -e "\nUsage: voxl-px4 [-b (Specify Holybro GPS unit)]" echo " [-c delete configuration file and exit]" - echo " [-d start px4 in daemon mode]" + echo " [-d start px4 without daemon mode]" echo " [-f (Use fake rc input instead of from a real transmitter)]" - echo " [-i (Set IMU_ROTATION to ROTATION_YAW_180)]" echo " [-m (Specify Matek GPS unit)]" echo " [-o (Start OSD module on the apps processor)]" echo " [-r (Specify TBS Crossfire RC receiver, MAVLINK)]" @@ -56,7 +57,9 @@ print_config_settings(){ echo -e "\n*************************" echo "GPS=$GPS" echo "RC=$RC" - echo "IMU_ROTATION=$IMU_ROTATION" + echo "ESC=$ESC" + echo "POWER MANAGER=$POWER_MANAGER" + echo "DISTANCE SENSOR=$DISTANCE_SENSOR" echo "OSD=$OSD" echo "DAEMON_MODE=$DAEMON_MODE" echo "SENSOR_CAL=$SENSOR_CAL" @@ -68,7 +71,7 @@ print_config_settings(){ echo -e "*************************\n" } -while getopts "bcdhifmorwz" flag +while getopts "bcdhfmorwz" flag do case "${flag}" in b) @@ -83,16 +86,12 @@ do exit 0 ;; d) - echo "[INFO] Enabling daemon mode" - DAEMON_MODE=ENABLE + echo "[INFO] Disabling daemon mode" + DAEMON_MODE=DISABLE ;; h) print_usage ;; - i) - echo "[INFO] Setting IMU_ROTATION to 4: ROTATION_YAW_180" - IMU_ROTATION=4 - ;; f) echo "[INFO] Setting RC to FAKE_RC_INPUT" RC=FAKE_RC_INPUT @@ -144,4 +143,5 @@ fi print_config_settings -GPS=$GPS RC=$RC OSD=$OSD IMU_ROTATION=$IMU_ROTATION EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start +GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \ +OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl b/boards/modalai/voxl2/target/voxl-px4-hitl new file mode 100755 index 0000000000..c2b2f41583 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-hitl @@ -0,0 +1,39 @@ +#!/bin/bash + +# Make sure that the SLPI DSP test signature is there otherwise px4 cannot run +# on the DSP +if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then + /bin/echo "Found DSP signature file" +else + /bin/echo "[WARNING] Could not find DSP signature file" + # Look for the DSP signature generation script + if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then + /bin/echo "[INFO] Attempting to generate the DSP signature file" + # Automatically generate the test signature so that px4 can run on SLPI DSP + /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh + else + /bin/echo "[ERROR] Could not find the DSP signature file generation script" + exit 0 + fi +fi + +print_usage() { + echo -e "\nUsage: voxl-px4-hitl" + echo " [-h (show help)]" + + exit 1 +} + +while getopts "h" flag +do + case "${flag}" in + h) + print_usage + ;; + *) + print_usage + ;; + esac +done + +px4 -s /usr/bin/voxl-px4-hitl-start diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config b/boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config new file mode 100755 index 0000000000..f4455955f7 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config @@ -0,0 +1,111 @@ +# Param setting and declaring file + +param select /data/px4/param/hitl_parameters + +# Sys config parameters +param set SYS_AUTOSTART 1001 +param set SYS_HITL 1 + +# Make sure we are running at 800Hz on IMU +param set IMU_GYRO_RATEMAX 800 + +# Some parameters for control allocation +param set CA_ROTOR_COUNT 4 +param set CA_AIRFRAME 0 +param set CA_ROTOR_COUNT 4 +param set CA_ROTOR0_PX 0.15 +param set CA_ROTOR0_PY 0.25 +param set CA_ROTOR1_PX -0.15 +param set CA_ROTOR1_PY -0.19 +param set CA_ROTOR2_PX 0.15 +param set CA_ROTOR2_PY -0.25 +param set CA_ROTOR2_KM -0.05 +param set CA_ROTOR3_PX -0.15 +param set CA_ROTOR3_PY 0.19 +param set CA_ROTOR3_KM -0.05 + +# Sensor calibration parameters +param set CAL_ACC0_ID 2490378 +param set CAL_ACC0_PRIO 50 +param set CAL_ACC0_XOFF -0.018255233764648438 +param set CAL_ACC0_XSCALE 1.000119328498840332 +param set CAL_ACC0_YOFF 0.097194194793701172 +param set CAL_ACC0_YSCALE 1.003928661346435547 +param set CAL_ACC0_ZOFF 0.081269264221191406 +param set CAL_ACC0_ZSCALE 0.992971897125244141 +param set CAL_ACC1_PRIO 50 +param set CAL_ACC2_PRIO 50 +param set CAL_ACC3_PRIO 50 +param set CAL_AIR_CMODEL 0 +param set CAL_AIR_TUBED_MM 1.500000000000000000 +param set CAL_AIR_TUBELEN 0.200000002980232239 +param set CAL_GYRO0_ID 2490378 +param set CAL_GYRO0_PRIO 50 +param set CAL_GYRO0_XOFF 0.013671654276549816 +param set CAL_GYRO0_YOFF -0.000422076700488105 +param set CAL_GYRO0_ZOFF -0.003227389883249998 +param set CAL_GYRO1_PRIO 50 +param set CAL_GYRO2_PRIO 50 +param set CAL_GYRO3_PRIO 50 +param set CAL_MAG0_ID 396809 +param set CAL_MAG0_PRIO 75 +param set CAL_MAG0_ROT 0 +param set CAL_MAG0_XODIAG -0.011825157329440117 +param set CAL_MAG0_XOFF -0.011212680488824844 +param set CAL_MAG0_XSCALE 1.001187443733215332 +param set CAL_MAG0_YODIAG 0.022539729252457619 +param set CAL_MAG0_YOFF -0.030884368345141411 +param set CAL_MAG0_YSCALE 0.940797865390777588 +param set CAL_MAG0_ZODIAG -0.006671304814517498 +param set CAL_MAG0_ZOFF -0.097350947558879852 +param set CAL_MAG0_ZSCALE 1.050295352935791016 +param set CAL_MAG1_PRIO 50 +param set CAL_MAG2_PRIO 50 +param set CAL_MAG3_PRIO 50 + +# Commander parameters +param set COM_CPU_MAX 0 +param set COM_DISARM_PRFLT -1 +param set COM_RC_IN_MODE 1 +param set NAV_RCL_ACT 1 +param set COM_FLIGHT_UUID 15 + +# EKF2 parameters +param set EKF2_ABL_LIM 0.8 +param set EKF2_EV_DELAY 5.0 +param set EKF2_IMU_POS_X 0.027 +param set EKF2_IMU_POS_Y 0.009 +param set EKF2_IMU_POS_Z -0.019 +param set EKF2_MAG_DECL 3.2 + +# Control allocator parameters for HIL +param set HIL_ACT_FUNC1 101 +param set HIL_ACT_FUNC2 102 +param set HIL_ACT_FUNC3 103 +param set HIL_ACT_FUNC4 104 +param set PWM_MAIN_FUNC1 101 +param set PWM_MAIN_FUNC2 102 +param set PWM_MAIN_FUNC3 103 +param set PWM_MAIN_FUNC4 104 + +# Mavlink parameters +param set MAV_TYPE 2 + +# Autotune parameters +param set MC_AT_EN 1 + +# RC Mapping parameters +param set RC_MAP_PITCH 2 +param set RC_MAP_ROLL 1 +param set RC_MAP_THROTTLE 3 +param set RC_MAP_YAW 4 +param set RC_MAP_FLTMODE 6 +param set RC_MAP_KILL_SW 7 + +# CBRK parameters +param set CBRK_SUPPLY_CHK 894281 + +# Landing parameters +param set LND_FLIGHT_T_LO 483791313 + +param save diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start new file mode 100755 index 0000000000..d56f489e29 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start @@ -0,0 +1,101 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +# Figure out what platform we are running on. +PLATFORM=`/usr/bin/voxl-platform 2> /dev/null` +RETURNCODE=$? +if [ $RETURNCODE -ne 0 ]; then + # If we couldn't get the platform from the voxl-platform utility then check + # /etc/version to see if there is an M0052 substring in the version string. If so, + # then we assume that we are on M0052. + VERSIONSTRING=$( /dev/null` +RETURNCODE=$? +if [ $RETURNCODE -ne 0 ]; then + # If we couldn't get the platform from the voxl-platform utility then check + # /etc/version to see if there is an M0052 substring in the version string. If so, + # then we assume that we are on M0052. + VERSIONSTRING=$((name); + taskmap[task_index].argv[0] = charPointer; + for (i = 0; i < PX4_TASK_MAX_ARGC; i++) { if (i < taskmap[task_index].argc) { int argument_length = strlen(argv[i]); @@ -172,12 +175,13 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma } else { strcpy(taskmap[task_index].argv_storage[i], argv[i]); - taskmap[task_index].argv[i] = taskmap[task_index].argv_storage[i]; + taskmap[task_index].argv[i + 1] = taskmap[task_index].argv_storage[i]; } } else { // Must add NULL at end of argv - taskmap[task_index].argv[i] = nullptr; + taskmap[task_index].argv[i + 1] = nullptr; + taskmap[task_index].argc = i + 1; break; } } diff --git a/src/drivers/actuators/modal_io/Kconfig b/src/drivers/actuators/modal_io/Kconfig deleted file mode 100644 index 742014f417..0000000000 --- a/src/drivers/actuators/modal_io/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_ACTUATORS_MODAL_IO - bool "modal_io" - default n - ---help--- - Enable support for modal_io diff --git a/src/drivers/actuators/modal_io/module.yaml b/src/drivers/actuators/modal_io/module.yaml deleted file mode 100644 index a7c79fae62..0000000000 --- a/src/drivers/actuators/modal_io/module.yaml +++ /dev/null @@ -1,26 +0,0 @@ -module_name: MODAL IO Output -actuator_output: - show_subgroups_if: 'MODAL_IO_CONFIG>0' - config_parameters: - - param: 'MODAL_IO_CONFIG' - label: 'Configure' - function: 'enable' - - param: 'MODAL_IO_BAUD' - label: 'Bitrate' - - param: 'MODAL_IO_RPM_MIN' - label: 'RPM Min' - - param: 'MODAL_IO_RPM_MAX' - label: 'RPM Max' - - param: 'MODAL_IO_SDIR1' - label: 'ESC 1 Spin Direction' - - param: 'MODAL_IO_SDIR2' - label: 'ESC 2 Spin Direction' - - param: 'MODAL_IO_SDIR3' - label: 'ESC 3 Spin Direction' - - param: 'MODAL_IO_SDIR4' - label: 'ESC 4 Spin Direction' - output_groups: - - param_prefix: MODAL_IO - group_label: 'ESCs' - channel_label: 'ESC' - num_channels: 4 diff --git a/src/drivers/actuators/modal_io/CMakeLists.txt b/src/drivers/actuators/voxl_esc/CMakeLists.txt similarity index 93% rename from src/drivers/actuators/modal_io/CMakeLists.txt rename to src/drivers/actuators/voxl_esc/CMakeLists.txt index 06368a53e4..d588dc29a6 100644 --- a/src/drivers/actuators/modal_io/CMakeLists.txt +++ b/src/drivers/actuators/voxl_esc/CMakeLists.txt @@ -32,20 +32,21 @@ ############################################################################ px4_add_module( - MODULE drivers__actuators__modal_io - MAIN modal_io + MODULE drivers__actuators__voxl_esc + MAIN voxl_esc SRCS crc16.c crc16.h - modal_io_serial.cpp - modal_io_serial.hpp - modal_io.cpp - modal_io.hpp + voxl_esc_serial.cpp + voxl_esc_serial.hpp + voxl_esc.cpp + voxl_esc.hpp qc_esc_packet_types.h qc_esc_packet.c qc_esc_packet.h DEPENDS + battery px4_work_queue mixer_module MODULE_CONFIG diff --git a/src/drivers/actuators/voxl_esc/Kconfig b/src/drivers/actuators/voxl_esc/Kconfig new file mode 100644 index 0000000000..5df8bb0bb4 --- /dev/null +++ b/src/drivers/actuators/voxl_esc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ACTUATORS_VOXL_ESC + bool "voxl_esc" + default n + ---help--- + Enable support for voxl_esc diff --git a/src/drivers/actuators/modal_io/crc16.c b/src/drivers/actuators/voxl_esc/crc16.c similarity index 100% rename from src/drivers/actuators/modal_io/crc16.c rename to src/drivers/actuators/voxl_esc/crc16.c diff --git a/src/drivers/actuators/modal_io/crc16.h b/src/drivers/actuators/voxl_esc/crc16.h similarity index 100% rename from src/drivers/actuators/modal_io/crc16.h rename to src/drivers/actuators/voxl_esc/crc16.h diff --git a/src/drivers/actuators/voxl_esc/module.yaml b/src/drivers/actuators/voxl_esc/module.yaml new file mode 100644 index 0000000000..1e227ae0b7 --- /dev/null +++ b/src/drivers/actuators/voxl_esc/module.yaml @@ -0,0 +1,41 @@ +module_name: VOXL ESC Output +actuator_output: + show_subgroups_if: 'VOXL_ESC_CONFIG>0' + config_parameters: + - param: 'VOXL_ESC_CONFIG' + label: 'Configure' + function: 'enable' + - param: 'VOXL_ESC_BAUD' + label: 'Bitrate' + - param: 'VOXL_ESC_RPM_MIN' + label: 'RPM Min' + - param: 'VOXL_ESC_RPM_MAX' + label: 'RPM Max' + - param: 'VOXL_ESC_SDIR1' + label: 'ESC 1 Spin Direction' + - param: 'VOXL_ESC_SDIR2' + label: 'ESC 2 Spin Direction' + - param: 'VOXL_ESC_SDIR3' + label: 'ESC 3 Spin Direction' + - param: 'VOXL_ESC_SDIR4' + label: 'ESC 4 Spin Direction' + output_groups: + - param_prefix: VOXL_ESC + group_label: 'ESCs' + channel_label: 'ESC' + num_channels: 4 + +parameters: + - group: ModalAI Custom Configuration + definitions: + MODALAI_CONFIG: + description: + short: Custom configuration for ModalAI drones + long: | + This can be set to indicate that drone behavior + needs to be changed to match a custom setting + type: int32 + reboot_required: true + num_instances: 1 + instance_start: 1 + default: 0 \ No newline at end of file diff --git a/src/drivers/actuators/modal_io/qc_esc_packet.c b/src/drivers/actuators/voxl_esc/qc_esc_packet.c similarity index 83% rename from src/drivers/actuators/modal_io/qc_esc_packet.c rename to src/drivers/actuators/voxl_esc/qc_esc_packet.c index 0dc8fab664..086622379c 100644 --- a/src/drivers/actuators/modal_io/qc_esc_packet.c +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.c @@ -119,36 +119,60 @@ int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, i } -int32_t qc_esc_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, +int32_t qc_esc_create_rpm_packet4(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3, uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, - uint8_t *out, uint16_t out_size) + uint8_t *out, uint16_t out_size, uint8_t ext_rpm) { - return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size); + return qc_esc_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size, ext_rpm); } -int32_t qc_esc_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, +int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_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) + int32_t fb_id, uint8_t *out, uint16_t out_size, uint8_t ext_rpm) { - uint16_t data[5]; + int16_t data[5]; uint16_t leds = 0; + uint8_t cmd = ESC_PACKET_TYPE_RPM_CMD; + int32_t max = ext_rpm > 0 ? ESC_RPM_MAX_EXT : ESC_RPM_MAX; + int32_t min = ext_rpm > 0 ? ESC_RPM_MIN_EXT : ESC_RPM_MIN; + + // Limit RPMs to prevent overflow when converting to int16_t + + if (rpm0 > max) { rpm0 = max; } if (rpm0 < min) { rpm0 = min; } + + if (rpm1 > max) { rpm1 = max; } if (rpm1 < min) { rpm1 = min; } + + if (rpm2 > max) { rpm2 = max; } if (rpm2 < min) { rpm2 = min; } + + if (rpm3 > max) { rpm3 = max; } if (rpm3 < min) { rpm3 = min; } 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 |= 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); + if (ext_rpm > 0) { + cmd = ESC_PACKET_TYPE_RPM_DIV2_CMD; + data[0] = ((rpm0 / 4) * 2); + data[1] = ((rpm1 / 4) * 2); + data[2] = ((rpm2 / 4) * 2); + data[3] = ((rpm3 / 4) * 2); + data[4] = leds; + + } else { + data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; + } + + //least significant bit is used for feedback request + data[0] &= ~(0x0001); data[1] &= ~(0x0001); data[2] &= ~(0x0001); data[3] &= ~(0x0001); + + if (fb_id == 0) { data[0] |= 0x0001; } if (fb_id == 1) { data[1] |= 0x0001; } + + if (fb_id == 2) { data[2] |= 0x0001; } if (fb_id == 3) { data[3] |= 0x0001; } + + return qc_esc_create_packet(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) diff --git a/src/drivers/actuators/modal_io/qc_esc_packet.h b/src/drivers/actuators/voxl_esc/qc_esc_packet.h similarity index 91% rename from src/drivers/actuators/modal_io/qc_esc_packet.h rename to src/drivers/actuators/voxl_esc/qc_esc_packet.h index 8af2b6ea54..88ecc8e9f9 100644 --- a/src/drivers/actuators/modal_io/qc_esc_packet.h +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.h @@ -52,6 +52,11 @@ extern "C" { #define QC_ESC_LED_GREEN_ON 2 #define QC_ESC_LED_BLUE_ON 4 +// Define RPM command max and min values +#define ESC_RPM_MAX INT16_MAX-1 // 32k +#define ESC_RPM_MIN INT16_MIN+1 // -32k +#define ESC_RPM_MAX_EXT UINT16_MAX-5 // 65k +#define ESC_RPM_MIN_EXT -UINT16_MAX+5 // -65k // Header of the packet. Each packet must start with this header #define ESC_PACKET_HEADER 0xAF @@ -142,6 +147,18 @@ typedef struct { } __attribute__((__packed__)) QC_ESC_FB_RESPONSE_V2; +// Definition of the feedback response packet from ESC, which contains battery voltage and total current +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; //ESC Id (could be used as system ID elsewhere) + uint16_t voltage; //Input voltage (Millivolts) + int16_t current; //Total Current (8mA resolution) + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_FB_POWER_STATUS; + + //------------------------------------------------------------------------- //Below are functions for generating packets that would be outgoing to ESCs //------------------------------------------------------------------------- @@ -197,15 +214,15 @@ int32_t qc_esc_create_pwm_packet4_fb(int16_t pwm0, int16_t pwm1, int16_t pwm2, i // 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, +int32_t qc_esc_create_rpm_packet4(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_t rpm3, uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, - uint8_t *out, uint16_t out_size); + uint8_t *out, uint16_t out_size, uint8_t ext_rpm); -// 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) +// Create a packet for sending closed-loop RPM command (32766 or 65530 max RPM) 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, +int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, int32_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); + int32_t fb_id, uint8_t *out, uint16_t out_size, uint8_t ext_rpm); //------------------------------------------------------------------------- diff --git a/src/drivers/actuators/modal_io/qc_esc_packet_types.h b/src/drivers/actuators/voxl_esc/qc_esc_packet_types.h similarity index 97% rename from src/drivers/actuators/modal_io/qc_esc_packet_types.h rename to src/drivers/actuators/voxl_esc/qc_esc_packet_types.h index 2453d7dbf0..538a8859a7 100644 --- a/src/drivers/actuators/modal_io/qc_esc_packet_types.h +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet_types.h @@ -46,6 +46,7 @@ #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_RPM_DIV2_CMD 7 #define ESC_PACKET_TYPE_RESET_CMD 10 #define ESC_PACKET_TYPE_SET_ID_CMD 11 #define ESC_PACKET_TYPE_SET_DIR_CMD 12 @@ -69,5 +70,6 @@ #define ESC_PACKET_TYPE_TUNE_CONFIG 114 #define ESC_PACKET_TYPE_FB_RESPONSE 128 #define ESC_PACKET_TYPE_VERSION_EXT_RESPONSE 131 +#define ESC_PACKET_TYPE_FB_POWER_STATUS 132 #endif diff --git a/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp similarity index 76% rename from src/drivers/actuators/modal_io/modal_io.cpp rename to src/drivers/actuators/voxl_esc/voxl_esc.cpp index 1e5a2bc08c..88d070a7cf 100644 --- a/src/drivers/actuators/modal_io/modal_io.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -35,33 +35,32 @@ #include -#include "modal_io.hpp" -#include "modal_io_serial.hpp" - -// utility for running on VOXL and using driver as a bridge -#define MODAL_IO_VOXL_BRIDGE_PORT "/dev/ttyS4" +#include "voxl_esc.hpp" +#include "voxl_esc_serial.hpp" // future use: #define MODALAI_PUBLISH_ESC_STATUS 0 const char *_device; -ModalIo::ModalIo() : - OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)), +VoxlEsc::VoxlEsc() : + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL_ESC_DEFAULT_PORT)), + _mixing_output{"VOXL_ESC", VOXL_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), - _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) + _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")), + _battery(1, nullptr, _battery_report_interval, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { - _device = MODAL_IO_DEFAULT_PORT; + _device = VOXL_ESC_DEFAULT_PORT; _mixing_output.setAllFailsafeValues(0); _mixing_output.setAllDisarmedValues(0); _esc_status.timestamp = hrt_absolute_time(); _esc_status.counter = 0; - _esc_status.esc_count = MODAL_IO_OUTPUT_CHANNELS; + _esc_status.esc_count = VOXL_ESC_OUTPUT_CHANNELS; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; - for (unsigned i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (unsigned i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { _esc_status.esc[i].timestamp = 0; _esc_status.esc[i].esc_address = 0; _esc_status.esc[i].esc_rpm = 0; @@ -75,15 +74,12 @@ ModalIo::ModalIo() : _esc_status.esc[i].esc_power = 0; } - _esc_status_pub.advertise(); - qc_esc_packet_init(&_fb_packet); - qc_esc_packet_init(&_uart_bridge_packet); _fb_idx = 0; } -ModalIo::~ModalIo() +VoxlEsc::~VoxlEsc() { _outputs_on = false; @@ -92,16 +88,11 @@ ModalIo::~ModalIo() _uart_port = nullptr; } - if (_uart_port_bridge) { - _uart_port_bridge->uart_close(); - _uart_port_bridge = nullptr; - } - perf_free(_cycle_perf); perf_free(_output_update_perf); } -int ModalIo::init() +int VoxlEsc::init() { /* Getting initial parameter values */ @@ -111,10 +102,15 @@ int ModalIo::init() return ret; } - _uart_port = new ModalIoSerial(); - _uart_port_bridge = new ModalIoSerial(); + _uart_port = new VoxlEscSerial(); memset(&_esc_chans, 0x00, sizeof(_esc_chans)); + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + _version_info[esc_id].sw_version = UINT16_MAX; + _version_info[esc_id].hw_version = UINT16_MAX; + _version_info[esc_id].id = esc_id; + } + //get_instance()->ScheduleOnInterval(10000); //100hz ScheduleNow(); @@ -122,81 +118,82 @@ int ModalIo::init() return 0; } -int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map) +int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) { int ret = PX4_OK; // initialize out - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { params->function_map[i] = (int)OutputFunction::Disabled; params->direction_map[i] = 0; params->motor_map[i] = 0; } - param_get(param_find("MODAL_IO_CONFIG"), ¶ms->config); - param_get(param_find("MODAL_IO_MODE"), ¶ms->mode); - param_get(param_find("MODAL_IO_BAUD"), ¶ms->baud_rate); + param_get(param_find("VOXL_ESC_CONFIG"), ¶ms->config); + param_get(param_find("VOXL_ESC_MODE"), ¶ms->mode); + param_get(param_find("VOXL_ESC_BAUD"), ¶ms->baud_rate); - param_get(param_find("MODAL_IO_T_PERC"), ¶ms->turtle_motor_percent); - param_get(param_find("MODAL_IO_T_DEAD"), ¶ms->turtle_motor_deadband); - param_get(param_find("MODAL_IO_T_EXPO"), ¶ms->turtle_motor_expo); - param_get(param_find("MODAL_IO_T_MINF"), ¶ms->turtle_stick_minf); - param_get(param_find("MODAL_IO_T_COSP"), ¶ms->turtle_cosphi); + param_get(param_find("VOXL_ESC_T_PERC"), ¶ms->turtle_motor_percent); + param_get(param_find("VOXL_ESC_T_DEAD"), ¶ms->turtle_motor_deadband); + param_get(param_find("VOXL_ESC_T_EXPO"), ¶ms->turtle_motor_expo); + param_get(param_find("VOXL_ESC_T_MINF"), ¶ms->turtle_stick_minf); + param_get(param_find("VOXL_ESC_T_COSP"), ¶ms->turtle_cosphi); - param_get(param_find("MODAL_IO_FUNC1"), ¶ms->function_map[0]); - param_get(param_find("MODAL_IO_FUNC2"), ¶ms->function_map[1]); - param_get(param_find("MODAL_IO_FUNC3"), ¶ms->function_map[2]); - param_get(param_find("MODAL_IO_FUNC4"), ¶ms->function_map[3]); + param_get(param_find("VOXL_ESC_FUNC1"), ¶ms->function_map[0]); + param_get(param_find("VOXL_ESC_FUNC2"), ¶ms->function_map[1]); + param_get(param_find("VOXL_ESC_FUNC3"), ¶ms->function_map[2]); + param_get(param_find("VOXL_ESC_FUNC4"), ¶ms->function_map[3]); - param_get(param_find("MODAL_IO_SDIR1"), ¶ms->direction_map[0]); - param_get(param_find("MODAL_IO_SDIR2"), ¶ms->direction_map[1]); - param_get(param_find("MODAL_IO_SDIR3"), ¶ms->direction_map[2]); - param_get(param_find("MODAL_IO_SDIR4"), ¶ms->direction_map[3]); + param_get(param_find("VOXL_ESC_SDIR1"), ¶ms->direction_map[0]); + param_get(param_find("VOXL_ESC_SDIR2"), ¶ms->direction_map[1]); + param_get(param_find("VOXL_ESC_SDIR3"), ¶ms->direction_map[2]); + param_get(param_find("VOXL_ESC_SDIR4"), ¶ms->direction_map[3]); - param_get(param_find("MODAL_IO_RPM_MIN"), ¶ms->rpm_min); - param_get(param_find("MODAL_IO_RPM_MAX"), ¶ms->rpm_max); + param_get(param_find("VOXL_ESC_RPM_MIN"), ¶ms->rpm_min); + param_get(param_find("VOXL_ESC_RPM_MAX"), ¶ms->rpm_max); - param_get(param_find("MODAL_IO_VLOG"), ¶ms->verbose_logging); + param_get(param_find("VOXL_ESC_VLOG"), ¶ms->verbose_logging); + param_get(param_find("VOXL_ESC_PUB_BST"), ¶ms->publish_battery_status); if (params->rpm_min >= params->rpm_max) { - PX4_ERR("Invalid parameter MODAL_IO_RPM_MIN. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); params->rpm_min = 0; ret = PX4_ERROR; } if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) { - PX4_ERR("Invalid parameter MODAL_IO_T_PERC. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); params->turtle_motor_percent = 0; ret = PX4_ERROR; } if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) { - PX4_ERR("Invalid parameter MODAL_IO_T_DEAD. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); params->turtle_motor_deadband = 0; ret = PX4_ERROR; } if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) { - PX4_ERR("Invalid parameter MODAL_IO_T_EXPO. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); params->turtle_motor_expo = 0; ret = PX4_ERROR; } if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) { - PX4_ERR("Invalid parameter MODAL_IO_T_MINF. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); params->turtle_stick_minf = 0.0f; ret = PX4_ERROR; } if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) { - PX4_ERR("Invalid parameter MODAL_IO_T_COSP. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); params->turtle_cosphi = 0.0f; ret = PX4_ERROR; } - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) { - PX4_ERR("Invalid parameter MODAL_IO_FUNCX. Only supports motors 1-4. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); params->function_map[i] = 0; ret = PX4_ERROR; @@ -210,11 +207,11 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map) } } - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { - if (params->motor_map[i] == MODAL_IO_OUTPUT_DISABLED || - params->motor_map[i] < -(MODAL_IO_OUTPUT_CHANNELS) || - params->motor_map[i] > MODAL_IO_OUTPUT_CHANNELS) { - PX4_ERR("Invalid parameter MODAL_IO_MOTORX. Please verify parameters."); + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { + if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED || + params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) || + params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) { + PX4_ERR("Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); params->motor_map[i] = 0; ret = PX4_ERROR; } @@ -227,7 +224,7 @@ int ModalIo::load_params(modal_io_params_t *params, ch_assign_t *map) return ret; } -int ModalIo::task_spawn(int argc, char *argv[]) +int VoxlEsc::task_spawn(int argc, char *argv[]) { int myoptind = 0; int ch; @@ -244,7 +241,7 @@ int ModalIo::task_spawn(int argc, char *argv[]) } } - ModalIo *instance = new ModalIo(); + VoxlEsc *instance = new VoxlEsc(); if (instance) { _object.store(instance); @@ -267,14 +264,31 @@ int ModalIo::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int ModalIo::flush_uart_rx() +int VoxlEsc::flush_uart_rx() { while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} return 0; } -int ModalIo::read_response(Command *out_cmd) +bool VoxlEsc::check_versions_updated() +{ + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + if (_version_info[esc_id].sw_version == UINT16_MAX) { return false; } + } + + // PX4_INFO("Got all ESC Version info!"); + _extended_rpm = true; + _need_version_info = false; + + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) { _extended_rpm = false; } + } + + return true; +} + +int VoxlEsc::read_response(Command *out_cmd) { px4_usleep(_current_cmd.resp_delay_us); @@ -297,7 +311,7 @@ int ModalIo::read_response(Command *out_cmd) return 0; } -int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) +int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) { hrt_abstime tnow = hrt_absolute_time(); @@ -311,13 +325,13 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) uint8_t packet_size = qc_esc_packet_get_size(&_fb_packet); if (packet_type == ESC_PACKET_TYPE_FB_RESPONSE && packet_size == sizeof(QC_ESC_FB_RESPONSE_V2)) { - //PX4_INFO("Got feedback V2 packet!"); + // PX4_INFO("Got feedback V2 packet!"); QC_ESC_FB_RESPONSE_V2 fb; memcpy(&fb, _fb_packet.buffer, packet_size); uint32_t id = (fb.id_state & 0xF0) >> 4; //ID of the ESC based on hardware address - if (id < MODAL_IO_OUTPUT_CHANNELS) { + if (id < VOXL_ESC_OUTPUT_CHANNELS) { int motor_idx = _output_map[id].number - 1; // mapped motor id.. user defined mapping is 1-4, array is 0-3 @@ -335,9 +349,9 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) _esc_chans[id].power_applied = fb.power; _esc_chans[id].state = fb.id_state & 0x0F; _esc_chans[id].cmd_counter = fb.cmd_counter; - _esc_chans[id].voltage = fb.voltage * 0.001; - _esc_chans[id].current = fb.current * 0.008; - _esc_chans[id].temperature = fb.temperature * 0.01; + _esc_chans[id].voltage = fb.voltage * 0.001f; + _esc_chans[id].current = fb.current * 0.008f; + _esc_chans[id].temperature = fb.temperature * 0.01f; _esc_chans[id].feedback_time = tnow; // also update our internal report for logging @@ -382,6 +396,13 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) { QC_ESC_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); + + if (_need_version_info) { + memcpy(&_version_info[ver.id], &ver, sizeof(QC_ESC_VERSION_INFO)); + check_versions_updated(); + break; + } + PX4_INFO("ESC ID: %i", ver.id); PX4_INFO("HW Version: %i", ver.hw_version); PX4_INFO("SW Version: %i", ver.sw_version); @@ -400,83 +421,60 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) PX4_INFO("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); PX4_INFO("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + + } else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) { + QC_ESC_FB_POWER_STATUS packet; + memcpy(&packet, _fb_packet.buffer, packet_size); + + float voltage = packet.voltage * 0.001f; // Voltage is reported at 1 mV resolution + float current = packet.current * 0.008f; // Total current is reported at 8mA resolution + + // Limit the frequency of battery status reports + if (_parameters.publish_battery_status) { + _battery.setConnected(true); + _battery.updateVoltage(voltage); + _battery.updateCurrent(current); + + hrt_abstime current_time = hrt_absolute_time(); + + if ((current_time - _last_battery_report_time) >= _battery_report_interval) { + _last_battery_report_time = current_time; + _battery.updateAndPublishBatteryStatus(current_time); + } + } + } } else { //parser error switch (ret) { case ESC_ERROR_BAD_CHECKSUM: _rx_crc_error_count++; - //PX4_INFO("BAD ESC packet checksum"); + // PX4_INFO("BAD ESC packet checksum"); break; case ESC_ERROR_BAD_LENGTH: - //PX4_INFO("BAD ESC packet length"); + // PX4_INFO("BAD ESC packet length"); break; } } } - /* - 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 < MODAL_IO_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 ModalIo::check_for_esc_timeout() +int VoxlEsc::check_for_esc_timeout() { hrt_abstime tnow = hrt_absolute_time(); - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { // PX4 motor indexed user defined mapping is 1-4, we want to use in bitmask (0-3) uint8_t motor_idx = _output_map[i].number - 1; - if (motor_idx < MODAL_IO_OUTPUT_CHANNELS) { + if (motor_idx < VOXL_ESC_OUTPUT_CHANNELS) { // we are using PX4 motor index in the bitmask if (_esc_status.esc_online_flags & (1 << motor_idx)) { // using index i here for esc_chans enumeration stored in ESC ID order - if ((tnow - _esc_chans[i].feedback_time) > MODAL_IO_DISCONNECT_TIMEOUT_US) { + if ((tnow - _esc_chans[i].feedback_time) > VOXL_ESC_DISCONNECT_TIMEOUT_US) { // stale data, assume offline and clear armed _esc_status.esc_online_flags &= ~(1 << motor_idx); _esc_status.esc_armed_flags &= ~(1 << motor_idx); @@ -489,7 +487,7 @@ int ModalIo::check_for_esc_timeout() } -int ModalIo::send_cmd_thread_safe(Command *cmd) +int VoxlEsc::send_cmd_thread_safe(Command *cmd) { cmd->id = _cmd_id++; _pending_cmd.store(cmd); @@ -504,7 +502,7 @@ int ModalIo::send_cmd_thread_safe(Command *cmd) -int ModalIo::custom_command(int argc, char *argv[]) +int VoxlEsc::custom_command(int argc, char *argv[]) { int myoptind = 0; int ch; @@ -530,7 +528,7 @@ int ModalIo::custom_command(int argc, char *argv[]) /* start the FMU if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { - return ModalIo::task_spawn(argc, argv); + return VoxlEsc::task_spawn(argc, argv); } } @@ -593,7 +591,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } if (!strcmp(verb, "reset")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Reset ESC: %i", esc_id); cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; @@ -605,7 +603,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "version")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { 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; @@ -618,7 +616,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "version-ext")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request extended version for ESC: %i", esc_id); cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; @@ -631,7 +629,7 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "tone")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { 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; @@ -660,12 +658,12 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "rpm")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); - int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; - if (esc_id == 0xFF) { + if (esc_id == 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < VOXL_ESC_OUTPUT_CHANNELS)'. rate_req[0] = rate; rate_req[1] = rate; rate_req[2] = rate; @@ -686,7 +684,8 @@ int ModalIo::custom_command(int argc, char *argv[]) 0, id_fb, cmd.buf, - sizeof(cmd.buf)); + sizeof(cmd.buf), + get_instance()->_extended_rpm); cmd.response = true; cmd.repeats = repeat_count; @@ -705,12 +704,12 @@ int ModalIo::custom_command(int argc, char *argv[]) } } else if (!strcmp(verb, "pwm")) { - if (esc_id < MODAL_IO_OUTPUT_CHANNELS) { + if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); - int16_t rate_req[MODAL_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; - if (esc_id == 0xFF) { + if (esc_id == 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < VOXL_ESC_OUTPUT_CHANNELS)'. rate_req[0] = rate; rate_req[1] = rate; rate_req[2] = rate; @@ -745,7 +744,7 @@ int ModalIo::custom_command(int argc, char *argv[]) return get_instance()->send_cmd_thread_safe(&cmd); } else { - print_usage("Invalid ESC mask, use 1-15"); + print_usage("Invalid ESC ID, use 0-3"); return 0; } } @@ -753,7 +752,7 @@ int ModalIo::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int ModalIo::update_params() +int VoxlEsc::update_params() { int ret = PX4_ERROR; @@ -772,7 +771,7 @@ int ModalIo::update_params() return ret; } -void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control) +void VoxlEsc::update_leds(vehicle_control_mode_s mode, led_control_s control) { int i = 0; uint8_t led_mask = _led_rsc.led_mask; @@ -872,12 +871,12 @@ void ModalIo::update_leds(vehicle_control_mode_s mode, led_control_s control) } } - for (i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { _esc_chans[i].led = led_mask; } } -void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) +void VoxlEsc::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) { bool use_pitch = true; bool use_roll = true; @@ -1052,10 +1051,13 @@ void ModalIo::mix_turtle_mode(uint16_t outputs[MAX_ACTUATORS]) } /* OutputModuleInterface */ -bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - if (num_outputs != MODAL_IO_OUTPUT_CHANNELS) { + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) + //So, if Run() is blocked by a custom command, this function will not be called until Run is running again + + if (num_outputs != VOXL_ESC_OUTPUT_CHANNELS) { return false; } @@ -1064,11 +1066,18 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], mix_turtle_mode(outputs); } - for (int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++) { + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (!_outputs_on || stop_motors) { _esc_chans[i].rate_req = 0; } else { + if (_extended_rpm) { + if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; } + + } else { + if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; } + } + if (!_turtle_mode_en) { _esc_chans[i].rate_req = outputs[i] * _output_map[i].direction; @@ -1079,6 +1088,7 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } } + Command cmd; cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, _esc_chans[1].rate_req, @@ -1090,24 +1100,24 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_chans[3].led, _fb_idx, cmd.buf, - sizeof(cmd.buf)); - + sizeof(cmd.buf), + _extended_rpm); if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { PX4_ERR("Failed to send packet"); return false; } - // round robin - _fb_idx = (_fb_idx + 1) % MODAL_IO_OUTPUT_CHANNELS; + // increment ESC id from which to request feedback in round robin order + _fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS; /* - * 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 + * Here we read and parse response from ESCs. Since the latest command has just been sent out, + * the response packet we may read here is probabaly from previous iteration, but it is totally ok. + * uart_read is non-blocking and we will just parse whatever bytes came in up until this point */ + int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf)); if (res > 0) { @@ -1135,13 +1145,29 @@ bool ModalIo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_status_pub.publish(_esc_status); + // If any extra external modal io data has been received then + // send it over as well + while (_voxl2_io_data_sub.updated()) { + buffer128_s io_data{}; + _voxl2_io_data_sub.copy(&io_data); + + // PX4_INFO("Got Modal IO data: %u bytes", io_data.len); + // PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x", + // io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3], + // io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]); + if (_uart_port->uart_write(io_data.data, io_data.len) != io_data.len) { + PX4_ERR("Failed to send modal io data to esc"); + return false; + } + } + perf_count(_output_update_perf); return true; } -void ModalIo::Run() +void VoxlEsc::Run() { if (should_exit()) { ScheduleClear(); @@ -1164,23 +1190,22 @@ void ModalIo::Run() } } - /* - for (int ii=0; ii<9; ii++) - { - const char * test_str = "Hello World!"; - _uart_port_bridge->uart_write((char*)test_str,12); - px4_usleep(10000); + /* Get ESC FW version info */ + if (_need_version_info) { + for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { + Command cmd; + cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) == cmd.len) { + if (read_response(&_current_cmd) != 0) { PX4_ERR("Failed to parse version request response packet!"); } + + } else { + PX4_ERR("Failed to send version request packet!"); + } } - */ - /* - uint8_t echo_buf[16]; - int bytes_read = _uart_port_bridge->uart_read(echo_buf,sizeof(echo_buf)); - if (bytes_read > 0) - _uart_port_bridge->uart_write(echo_buf,bytes_read); - */ + } - - _mixing_output.update(); + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update output status if armed */ _outputs_on = _mixing_output.armed().armed; @@ -1222,16 +1247,16 @@ void ModalIo::Run() if (!_outputs_on) { - float setpoint = MODAL_IO_MODE_DISABLED_SETPOINT; + float setpoint = VOXL_ESC_MODE_DISABLED_SETPOINT; - if (_parameters.mode == MODAL_IO_MODE_TURTLE_AUX1) { + if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX1) { setpoint = _manual_control_setpoint.aux1; - } else if (_parameters.mode == MODAL_IO_MODE_TURTLE_AUX2) { + } else if (_parameters.mode == VOXL_ESC_MODE_TURTLE_AUX2) { setpoint = _manual_control_setpoint.aux2; } - if (setpoint > MODAL_IO_MODE_THRESHOLD) { + if (setpoint > VOXL_ESC_MODE_THRESHOLD) { _turtle_mode_en = true; } else { @@ -1240,68 +1265,6 @@ void ModalIo::Run() } } - if (_parameters.mode == MODAL_IO_MODE_UART_BRIDGE) { - if (!_uart_port_bridge->is_open()) { - if (_uart_port_bridge->uart_open(MODAL_IO_VOXL_BRIDGE_PORT, 230400) == PX4_OK) { - PX4_INFO("Opened UART ESC Bridge device"); - - } else { - PX4_ERR("Failed openening UART ESC Bridge device"); - return; - } - } - - //uart passthrough test code - //run 9 times because i just don't know how to change update rate of the module from 10hz to 100hz.. - for (int ii = 0; ii < 9; ii++) { - uint8_t uart_buf[128]; - int bytes_read = _uart_port_bridge->uart_read(uart_buf, sizeof(uart_buf)); - - if (bytes_read > 0) { - _uart_port->uart_write(uart_buf, bytes_read); - - for (int i = 0; i < bytes_read; i++) { - int16_t ret = qc_esc_packet_process_char(uart_buf[i], &_uart_bridge_packet); - - if (ret > 0) { - //PX4_INFO("got packet of length %i",ret); - uint8_t packet_type = qc_esc_packet_get_type(&_uart_bridge_packet); - - //uint8_t packet_size = qc_esc_packet_get_size(&_uart_bridge_packet); - //if we received a command for ESC to reset, most likely firmware update is coming, switch to bootloader baud rate - if (packet_type == ESC_PACKET_TYPE_RESET_CMD) { - int bootloader_baud_rate = 230400; - - if (_uart_port->uart_get_baud() != bootloader_baud_rate) { - px4_usleep(5000); - _uart_port->uart_set_baud(bootloader_baud_rate); - } - - } else { - if (_uart_port->uart_get_baud() != _parameters.baud_rate) { - px4_usleep(5000); - _uart_port->uart_set_baud(_parameters.baud_rate); //restore normal baud rate - } - } - } - } - } - - bytes_read = _uart_port->uart_read(uart_buf, sizeof(uart_buf)); - - if (bytes_read > 0) { - _uart_port_bridge->uart_write(uart_buf, bytes_read); - } - - px4_usleep(10000); - } - } - - } else { - if (_uart_port_bridge->is_open()) { - PX4_INFO("Closed UART ESC Bridge device"); - _uart_port_bridge->uart_close(); - } } if (!_outputs_on) { @@ -1364,7 +1327,7 @@ void ModalIo::Run() } -int ModalIo::print_usage(const char *reason) +int VoxlEsc::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -1384,7 +1347,7 @@ $ todo )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("modal_io", "driver"); + PRINT_MODULE_USAGE_NAME("voxl_esc", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Send reset request to ESC"); @@ -1422,7 +1385,7 @@ $ todo return 0; } -int ModalIo::print_status() +int VoxlEsc::print_status() { PX4_INFO("Max update rate: %i Hz", _current_update_rate); PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); @@ -1431,31 +1394,31 @@ int ModalIo::print_status() PX4_INFO(""); - PX4_INFO("Params: MODAL_IO_CONFIG: %" PRId32, _parameters.config); - PX4_INFO("Params: MODAL_IO_BAUD: %" PRId32, _parameters.baud_rate); + PX4_INFO("Params: VOXL_ESC_CONFIG: %" PRId32, _parameters.config); + PX4_INFO("Params: VOXL_ESC_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: MODAL_IO_FUNC1: %" PRId32, _parameters.function_map[0]); - PX4_INFO("Params: MODAL_IO_FUNC2: %" PRId32, _parameters.function_map[1]); - PX4_INFO("Params: MODAL_IO_FUNC3: %" PRId32, _parameters.function_map[2]); - PX4_INFO("Params: MODAL_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + PX4_INFO("Params: VOXL_ESC_FUNC1: %" PRId32, _parameters.function_map[0]); + PX4_INFO("Params: VOXL_ESC_FUNC2: %" PRId32, _parameters.function_map[1]); + PX4_INFO("Params: VOXL_ESC_FUNC3: %" PRId32, _parameters.function_map[2]); + PX4_INFO("Params: VOXL_ESC_FUNC4: %" PRId32, _parameters.function_map[3]); - PX4_INFO("Params: MODAL_IO_SDIR1: %" PRId32, _parameters.direction_map[0]); - PX4_INFO("Params: MODAL_IO_SDIR2: %" PRId32, _parameters.direction_map[1]); - PX4_INFO("Params: MODAL_IO_SDIR3: %" PRId32, _parameters.direction_map[2]); - PX4_INFO("Params: MODAL_IO_SDIR4: %" PRId32, _parameters.direction_map[3]); + PX4_INFO("Params: VOXL_ESC_SDIR1: %" PRId32, _parameters.direction_map[0]); + PX4_INFO("Params: VOXL_ESC_SDIR2: %" PRId32, _parameters.direction_map[1]); + PX4_INFO("Params: VOXL_ESC_SDIR3: %" PRId32, _parameters.direction_map[2]); + PX4_INFO("Params: VOXL_ESC_SDIR4: %" PRId32, _parameters.direction_map[3]); - PX4_INFO("Params: MODAL_IO_RPM_MIN: %" PRId32, _parameters.rpm_min); - PX4_INFO("Params: MODAL_IO_RPM_MAX: %" PRId32, _parameters.rpm_max); + PX4_INFO("Params: VOXL_ESC_RPM_MIN: %" PRId32, _parameters.rpm_min); + PX4_INFO("Params: VOXL_ESC_RPM_MAX: %" PRId32, _parameters.rpm_max); PX4_INFO(""); - for( int i = 0; i < MODAL_IO_OUTPUT_CHANNELS; i++){ + for( int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++){ PX4_INFO("-- ID: %i", i); PX4_INFO(" Motor: %i", _output_map[i].number); PX4_INFO(" Direction: %i", _output_map[i].direction); 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(" Requested: %" PRIi32 " RPM", _esc_chans[i].rate_req); + PX4_INFO(" Measured: %" PRIi32 " RPM", _esc_chans[i].rate_meas); PX4_INFO(" Command Counter: %i", _esc_chans[i].cmd_counter); PX4_INFO(" Voltage: %f VDC", (double)_esc_chans[i].voltage); PX4_INFO(""); @@ -1469,9 +1432,9 @@ int ModalIo::print_status() return 0; } -extern "C" __EXPORT int modal_io_main(int argc, char *argv[]); +extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]); -int modal_io_main(int argc, char *argv[]) +int voxl_esc_main(int argc, char *argv[]) { - return ModalIo::main(argc, argv); + return VoxlEsc::main(argc, argv); } diff --git a/src/drivers/actuators/modal_io/modal_io.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp similarity index 68% rename from src/drivers/actuators/modal_io/modal_io.hpp rename to src/drivers/actuators/voxl_esc/voxl_esc.hpp index 196728aab7..8cc84435fa 100644 --- a/src/drivers/actuators/modal_io/modal_io.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -41,23 +41,26 @@ #include #include +#include + #include #include #include #include #include #include +#include -#include "modal_io_serial.hpp" +#include "voxl_esc_serial.hpp" #include "qc_esc_packet.h" #include "qc_esc_packet_types.h" -class ModalIo : public ModuleBase, public OutputModuleInterface +class VoxlEsc : public ModuleBase, public OutputModuleInterface { public: - ModalIo(); - virtual ~ModalIo(); + VoxlEsc(); + virtual ~VoxlEsc(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -100,56 +103,61 @@ public: int send_cmd_thread_safe(Command *cmd); private: - static constexpr uint32_t MODAL_IO_UART_CONFIG = 1; - static constexpr uint32_t MODAL_IO_DEFAULT_BAUD = 250000; - static constexpr uint16_t MODAL_IO_OUTPUT_CHANNELS = 4; - static constexpr uint16_t MODAL_IO_OUTPUT_DISABLED = 0; + static constexpr uint32_t VOXL_ESC_UART_CONFIG = 1; + static constexpr uint32_t VOXL_ESC_DEFAULT_BAUD = 250000; + static constexpr uint16_t VOXL_ESC_OUTPUT_CHANNELS = 4; + static constexpr uint16_t VOXL_ESC_OUTPUT_DISABLED = 0; - static constexpr uint32_t MODAL_IO_WRITE_WAIT_US = 200; - static constexpr uint32_t MODAL_IO_DISCONNECT_TIMEOUT_US = 500000; + static constexpr uint32_t VOXL_ESC_WRITE_WAIT_US = 200; + static constexpr uint32_t VOXL_ESC_DISCONNECT_TIMEOUT_US = 500000; static constexpr uint16_t DISARMED_VALUE = 0; - static constexpr uint16_t MODAL_IO_PWM_MIN = 0; - static constexpr uint16_t MODAL_IO_PWM_MAX = 800; - static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MIN = 5000; - static constexpr uint16_t MODAL_IO_DEFAULT_RPM_MAX = 17000; + static constexpr uint16_t VOXL_ESC_PWM_MIN = 0; + static constexpr uint16_t VOXL_ESC_PWM_MAX = 800; + static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MIN = 5000; + static constexpr uint16_t VOXL_ESC_DEFAULT_RPM_MAX = 17000; - static constexpr float MODAL_IO_MODE_DISABLED_SETPOINT = -0.1f; - static constexpr float MODAL_IO_MODE_THRESHOLD = 0.0f; + static constexpr float VOXL_ESC_MODE_DISABLED_SETPOINT = -0.1f; + static constexpr float VOXL_ESC_MODE_THRESHOLD = 0.0f; - static constexpr uint32_t MODAL_IO_MODE = 0; - static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX1 = 1; - static constexpr uint32_t MODAL_IO_MODE_TURTLE_AUX2 = 2; - static constexpr uint32_t MODAL_IO_MODE_UART_BRIDGE = 3; + static constexpr uint32_t VOXL_ESC_MODE = 0; + static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1; + static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2; - //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, MODAL_IO_PWM_MAX); } - //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, MODAL_IO_RPM_MAX); } + static constexpr uint16_t VOXL_ESC_EXT_RPM = 39; + static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - + 1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - + 5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) - ModalIoSerial *_uart_port; - ModalIoSerial *_uart_port_bridge; + //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); } + //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); } + + VoxlEscSerial *_uart_port; typedef struct { - int32_t config{MODAL_IO_UART_CONFIG}; - int32_t mode{MODAL_IO_MODE}; + int32_t config{VOXL_ESC_UART_CONFIG}; + int32_t mode{VOXL_ESC_MODE}; int32_t turtle_motor_expo{35}; int32_t turtle_motor_deadband{20}; int32_t turtle_motor_percent{90}; float turtle_stick_minf{0.15f}; float turtle_cosphi{0.99f}; - int32_t baud_rate{MODAL_IO_DEFAULT_BAUD}; - int32_t rpm_min{MODAL_IO_DEFAULT_RPM_MIN}; - int32_t rpm_max{MODAL_IO_DEFAULT_RPM_MAX}; - int32_t function_map[MODAL_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; - int32_t motor_map[MODAL_IO_OUTPUT_CHANNELS] {1, 2, 3, 4}; - int32_t direction_map[MODAL_IO_OUTPUT_CHANNELS] {1, 1, 1, 1}; + int32_t baud_rate{VOXL_ESC_DEFAULT_BAUD}; + int32_t rpm_min{VOXL_ESC_DEFAULT_RPM_MIN}; + int32_t rpm_max{VOXL_ESC_DEFAULT_RPM_MAX}; + int32_t function_map[VOXL_ESC_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t motor_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4}; + int32_t direction_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1}; int32_t verbose_logging{0}; - } modal_io_params_t; + int32_t publish_battery_status{0}; + } voxl_esc_params_t; struct EscChan { - int16_t rate_req; + int32_t rate_req; uint8_t state; - uint16_t rate_meas; + uint32_t rate_meas; uint8_t power_applied; uint8_t led; uint8_t cmd_counter; @@ -167,14 +175,14 @@ private: typedef struct { led_control_s control{}; vehicle_control_mode_s mode{}; - uint8_t led_mask;// TODO led_mask[MODAL_IO_OUTPUT_CHANNELS]; + uint8_t led_mask;// TODO led_mask[VOXL_ESC_OUTPUT_CHANNELS]; bool breath_en; uint8_t breath_counter; bool test; } led_rsc_t; - ch_assign_t _output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; - MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + ch_assign_t _output_map[VOXL_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; + MixingOutput _mixing_output; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; @@ -188,13 +196,19 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; + uORB::Subscription _voxl2_io_data_sub{ORB_ID(voxl2_io_data)}; uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; - modal_io_params_t _parameters; + bool _extended_rpm{false}; + bool _need_version_info{true}; + QC_ESC_VERSION_INFO _version_info[4]; + bool check_versions_updated(); + + voxl_esc_params_t _parameters; int update_params(); - int load_params(modal_io_params_t *params, ch_assign_t *map); + int load_params(voxl_esc_params_t *params, ch_assign_t *map); bool _turtle_mode_en{false}; int32_t _rpm_turtle_min{0}; @@ -205,11 +219,10 @@ private: Command _current_cmd; px4::atomic _pending_cmd{nullptr}; - EscChan _esc_chans[MODAL_IO_OUTPUT_CHANNELS]; + EscChan _esc_chans[VOXL_ESC_OUTPUT_CHANNELS]; Command _esc_cmd; esc_status_s _esc_status; EscPacket _fb_packet; - EscPacket _uart_bridge_packet; led_rsc_t _led_rsc; int _fb_idx; @@ -219,6 +232,10 @@ private: static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; + Battery _battery; + static constexpr unsigned _battery_report_interval{100_ms}; + hrt_abstime _last_battery_report_time; + void update_leds(vehicle_control_mode_s mode, led_control_s control); int read_response(Command *out_cmd); diff --git a/src/drivers/actuators/modal_io/modal_io_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c similarity index 73% rename from src/drivers/actuators/modal_io/modal_io_params.c rename to src/drivers/actuators/voxl_esc/voxl_esc_params.c index 3bb8489855..8c1d03c6e7 100644 --- a/src/drivers/actuators/modal_io/modal_io_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -38,23 +38,23 @@ * * @reboot_required true * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Disabled * @value 1 - VOXL ESC * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(MODAL_IO_CONFIG, 0); +PARAM_DEFINE_INT32(VOXL_ESC_CONFIG, 0); /** * UART ESC baud rate * * Default rate is 250Kbps, which is used in off-the-shelf MoadalAI ESC products. * - * @group MODAL IO + * @group VOXL ESC * @unit bit/s */ -PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000); +PARAM_DEFINE_INT32(VOXL_ESC_BAUD, 250000); /** * Motor mappings for ModalAI ESC @@ -68,33 +68,33 @@ PARAM_DEFINE_INT32(MODAL_IO_BAUD, 250000); // The following are auto generated params from control allocator pattern, put here for reference // Default ESC1 to motor2 -//PARAM_DEFINE_INT32(MODAL_IO_FUNC1, 102); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC1, 102); -//PARAM_DEFINE_INT32(MODAL_IO_FUNC2, 103); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC2, 103); -//PARAM_DEFINE_INT32(MODAL_IO_FUNC3, 101); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC3, 101); -//PARAM_DEFINE_INT32(MODAL_IO_FUNC4, 104); +//PARAM_DEFINE_INT32(VOXL_ESC_FUNC4, 104); /** * UART ESC RPM Min * * Minimum RPM for ESC * - * @group MODAL IO + * @group VOXL ESC * @unit rpm */ -PARAM_DEFINE_INT32(MODAL_IO_RPM_MIN, 5500); +PARAM_DEFINE_INT32(VOXL_ESC_RPM_MIN, 5500); /** * UART ESC RPM Max * * Maximum RPM for ESC * - * @group MODAL IO + * @group VOXL ESC * @unit rpm */ -PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000); +PARAM_DEFINE_INT32(VOXL_ESC_RPM_MAX, 15000); /** * UART ESC Mode @@ -103,7 +103,7 @@ PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000); * * @reboot_required true * - * @group MODAL IO + * @group VOXL ESC * @value 0 - None * @value 1 - Turtle Mode enabled via AUX1 * @value 2 - Turtle Mode enabled via AUX2 @@ -111,108 +111,124 @@ PARAM_DEFINE_INT32(MODAL_IO_RPM_MAX, 15000); * @min 0 * @max 2 */ -PARAM_DEFINE_INT32(MODAL_IO_MODE, 0); +PARAM_DEFINE_INT32(VOXL_ESC_MODE, 0); /** * UART ESC ID 1 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR1, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR1, 0); /** * UART ESC ID 2 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR2, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR2, 0); /** * UART ESC ID 3 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR3, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR3, 0); /** * UART ESC ID 4 Spin Direction Flag * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Default * @value 1 - Reverse */ -PARAM_DEFINE_INT32(MODAL_IO_SDIR4, 0); +PARAM_DEFINE_INT32(VOXL_ESC_SDIR4, 0); /** * UART ESC Turtle Mode Crash Flip Motor Percent * - * @group MODAL IO + * @group VOXL ESC * @min 1 * @max 100 * @decimal 10 * @increment 1 */ -PARAM_DEFINE_INT32(MODAL_IO_T_PERC, 90); +PARAM_DEFINE_INT32(VOXL_ESC_T_PERC, 90); /** * UART ESC Turtle Mode Crash Flip Motor Deadband * - * @group MODAL IO + * @group VOXL ESC * @min 0 * @max 100 * @decimal 10 * @increment 1 */ -PARAM_DEFINE_INT32(MODAL_IO_T_DEAD, 20); +PARAM_DEFINE_INT32(VOXL_ESC_T_DEAD, 20); /** * UART ESC Turtle Mode Crash Flip Motor STICK_MINF * - * @group MODAL IO + * @group VOXL ESC * @min 0.0 * @max 100.0 * @decimal 10 * @increment 1.0 */ -PARAM_DEFINE_FLOAT(MODAL_IO_T_MINF, 0.15); +PARAM_DEFINE_FLOAT(VOXL_ESC_T_MINF, 0.15); /** * UART ESC Turtle Mode Crash Flip Motor expo * - * @group MODAL IO + * @group VOXL ESC * @min 0 * @max 100 * @decimal 10 * @increment 1 */ -PARAM_DEFINE_INT32(MODAL_IO_T_EXPO, 35); +PARAM_DEFINE_INT32(VOXL_ESC_T_EXPO, 35); /** * UART ESC Turtle Mode Cosphi * - * @group MODAL IO + * @group VOXL ESC * @min 0.000 * @max 1.000 * @decimal 10 * @increment 0.001 */ -PARAM_DEFINE_FLOAT(MODAL_IO_T_COSP, 0.990); +PARAM_DEFINE_FLOAT(VOXL_ESC_T_COSP, 0.990); /** * UART ESC verbose logging * * @reboot_required true * - * @group MODAL IO + * @group VOXL ESC * @value 0 - Disabled * @value 1 - Enabled * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(MODAL_IO_VLOG, 0); +PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0); + + +/** + * UART ESC Enable publishing of battery status + * + * Only applicable to ESCs that report total battery voltage and current + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - Disabled + * @value 1 - Enabled + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1); \ No newline at end of file diff --git a/src/drivers/actuators/modal_io/modal_io_serial.cpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp similarity index 93% rename from src/drivers/actuators/modal_io/modal_io_serial.cpp rename to src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp index f4b8076730..1064dc4365 100644 --- a/src/drivers/actuators/modal_io/modal_io_serial.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp @@ -32,20 +32,20 @@ ****************************************************************************/ #include "string.h" -#include "modal_io_serial.hpp" +#include "voxl_esc_serial.hpp" -ModalIoSerial::ModalIoSerial() +VoxlEscSerial::VoxlEscSerial() { } -ModalIoSerial::~ModalIoSerial() +VoxlEscSerial::~VoxlEscSerial() { if (_uart_fd >= 0) { uart_close(); } } -int ModalIoSerial::uart_open(const char *dev, speed_t speed) +int VoxlEscSerial::uart_open(const char *dev, speed_t speed) { if (_uart_fd >= 0) { PX4_ERR("Port in use: %s (%i)", dev, errno); @@ -110,7 +110,7 @@ int ModalIoSerial::uart_open(const char *dev, speed_t speed) return 0; } -int ModalIoSerial::uart_set_baud(speed_t speed) +int VoxlEscSerial::uart_set_baud(speed_t speed) { #ifndef __PX4_QURT @@ -134,7 +134,7 @@ int ModalIoSerial::uart_set_baud(speed_t speed) return -1; } -int ModalIoSerial::uart_close() +int VoxlEscSerial::uart_close() { #ifndef __PX4_QURT @@ -158,7 +158,7 @@ int ModalIoSerial::uart_close() return 0; } -int ModalIoSerial::uart_write(FAR void *buf, size_t len) +int VoxlEscSerial::uart_write(FAR void *buf, size_t len) { if (_uart_fd < 0 || buf == NULL) { PX4_ERR("invalid state for writing or buffer"); @@ -172,7 +172,7 @@ int ModalIoSerial::uart_write(FAR void *buf, size_t len) #endif } -int ModalIoSerial::uart_read(FAR void *buf, size_t len) +int VoxlEscSerial::uart_read(FAR void *buf, size_t len) { if (_uart_fd < 0 || buf == NULL) { PX4_ERR("invalid state for reading or buffer"); diff --git a/src/drivers/actuators/modal_io/modal_io_serial.hpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp similarity index 97% rename from src/drivers/actuators/modal_io/modal_io_serial.hpp rename to src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp index c91f121ca3..99e918886f 100644 --- a/src/drivers/actuators/modal_io/modal_io_serial.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp @@ -43,11 +43,11 @@ #define FAR #endif -class ModalIoSerial +class VoxlEscSerial { public: - ModalIoSerial(); - virtual ~ModalIoSerial(); + VoxlEscSerial(); + virtual ~VoxlEscSerial(); int uart_open(const char *dev, speed_t speed); int uart_set_baud(speed_t speed); diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 2627dcebbb..5fa109d57d 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -74,7 +74,6 @@ VOXLPM::init() _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } /* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */ @@ -82,6 +81,11 @@ VOXLPM::init() return ret; } + // Don't actually publish anything unless we have had a successful probe + if (_ch_type == VOXLPM_CH_TYPE_VBATT) { + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + } + /* If we've probed and succeeded we'll have an accurate address here for the VBat addr */ uint8_t addr = get_device_address(); @@ -420,7 +424,7 @@ VOXLPM::measure_ina231() int16_t vshunt = -1; uint16_t vbus = -1; - uint16_t amps = 0; + int16_t amps = 0; int vshunt_ret = read_reg_buf(INA231_REG_SHUNTVOLTAGE, raw_vshunt, sizeof(raw_vshunt)); int vbus_ret = read_reg_buf(INA231_REG_BUSVOLTAGE, raw_vbus, sizeof(raw_vbus)); diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt new file mode 100644 index 0000000000..9116c91765 --- /dev/null +++ b/src/drivers/voxl2_io/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 2015-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__voxl2_io + MAIN voxl2_io + SRCS + voxl2_io_crc16.c + voxl2_io_crc16.h + voxl2_io_serial.cpp + voxl2_io_serial.hpp + voxl2_io_packet.c + voxl2_io_packet.h + voxl2_io_packet_types.h + voxl2_io.cpp + voxl2_io.hpp + DEPENDS + px4_work_queue + mixer_module + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig new file mode 100644 index 0000000000..15964deeeb --- /dev/null +++ b/src/drivers/voxl2_io/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_VOXL2_IO + bool "voxl2_io" + default n + ---help--- + Enable support for voxl2_io \ No newline at end of file diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml new file mode 100644 index 0000000000..02aa2122e3 --- /dev/null +++ b/src/drivers/voxl2_io/module.yaml @@ -0,0 +1,13 @@ +module_name: VOXL2 IO Output +actuator_output: + config_parameters: + - param: 'VOXL2_IO_MIN' + label: 'PWM min value' + - param: 'VOXL2_IO_MAX' + label: 'PWM max value' + output_groups: + - param_prefix: VOXL2_IO + group_label: 'PWMs' + channel_label: 'PWM Channel' + num_channels: 4 + diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp new file mode 100644 index 0000000000..352b474906 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -0,0 +1,1122 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "voxl2_io.hpp" + +#include + + +Voxl2IO::Voxl2IO() : + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)), + _mixing_output{"VOXL2_IO", VOXL2_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, + _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": module cycle")}, + _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) +{ + _mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS); + _uart_port = new Voxl2IoSerial(); + voxl2_io_packet_init(&_sbus_packet); +} + +Voxl2IO::~Voxl2IO() +{ + /* make sure servos are off */ + stop_all_pwms(); + + if (_uart_port) { + _uart_port->uart_close(); + _uart_port = nullptr; + } + + perf_free(_cycle_perf); + perf_free(_output_update_perf); +} + + +int Voxl2IO::init() +{ + int ret = PX4_OK; + + /* Open serial port in this thread */ + if (!_uart_port->is_open()) { + if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) { + /* Send PWM config to M0065... pwm_min and pwm_max */ + PX4_INFO("Opened UART connection to M0065 device on port %s", _device); + + } else { + PX4_ERR("Failed opening device"); + return PX4_ERROR; + } + } + + /* Verify connectivity and protocol version number */ + if (get_version_info() < 0) { + PX4_ERR("Failed to detect voxl2_io protocol version."); + return PX4_ERROR; + + } else { + if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION + && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) { + PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); + + } else { + PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); + return PX4_ERROR; + } + } + + /* Getting initial parameter values */ + ret = update_params(); + + if (ret != OK) { + return ret; + } + + /* Send PWM MIN/MAX to M0065 */ + update_pwm_config(); + + ScheduleOnInterval(_current_update_interval); + // ScheduleNow(); + return ret; +} + +int Voxl2IO::update_params() +{ + int ret = PX4_ERROR; + + updateParams(); + ret = load_params(&_parameters); + + if (ret == PX4_OK) { + _mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED); + _mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE); + _mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN); + _mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX); + _pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min; + } + + return ret; +} + +int Voxl2IO::load_params(voxl2_io_params_t *params) +{ + int ret = PX4_OK; + int32_t max = params->pwm_max; + int32_t min = params->pwm_min; + + // initialize out + for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { + params->function_map[i] = (int)OutputFunction::Disabled; + } + + /* UART config, PWM mode, and RC protocol*/ + param_get(param_find("VOXL2_IO_BAUD"), ¶ms->baud_rate); + param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); + + /* PWM min, max, and failsafe values*/ + param_get(param_find("VOXL2_IO_MIN"), ¶ms->pwm_min); + param_get(param_find("VOXL2_IO_MAX"), ¶ms->pwm_max); + + /* PWM output functions */ + param_get(param_find("VOXL2_IO_FUNC1"), ¶ms->function_map[0]); + param_get(param_find("VOXL2_IO_FUNC2"), ¶ms->function_map[1]); + param_get(param_find("VOXL2_IO_FUNC3"), ¶ms->function_map[2]); + param_get(param_find("VOXL2_IO_FUNC4"), ¶ms->function_map[3]); + + /* Validate PWM min and max values */ + if (params->pwm_min > params->pwm_max) { + PX4_ERR("Invalid parameter VOXL2_IO_MIN. Please verify parameters."); + params->pwm_min = 0; + ret = PX4_ERROR; + } + + if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) { + PX4_INFO("Updating PWM params load_params"); + update_pwm_config(); + } + + return ret; +} + +void Voxl2IO::update_pwm_config() +{ + Command cmd; + uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast((_parameters.pwm_min & 0xFF00) >> 8), static_cast(_parameters.pwm_min & 0xFF), + static_cast((_parameters.pwm_max & 0xFF00) >> 8), static_cast(_parameters.pwm_max & 0xFF) + }; + cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE, cmd.buf, + sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send config packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } +} + +int Voxl2IO::get_version_info() +{ + int res = 0 ; + int header = -1 ; + int info_packet = -1; + int read_retries = 3; + int read_succeeded = 0; + Command cmd; + + /* Request protocol version info from M0065 */ + cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } + + /* Read response */ + px4_usleep(10000); + memset(&_read_buf, 0x00, READ_BUF_SIZE); + res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + + while (read_retries) { + if (res) { + /* Get index of packer header */ + for (int index = 0; index < READ_BUF_SIZE; ++index) { + if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) { + info_packet = index; + break; + } + + if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { + header = index; + } + } + + /* Try again in a bit if packet header not present yet... */ + if (header == -1 || info_packet == -1) { + if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); } + + if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); } + + read_retries--; + flush_uart_rx(); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + px4_usleep(2000); + } + + continue; + } + + /* Check if we got a valid packet...*/ + if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) { + if (_debug) { + PX4_ERR("Error parsing version info packet"); + PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", + _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], + _read_buf[header + 5], + _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], + _read_buf[header + 11], + _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], + _read_buf[header + 17], + _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], + _read_buf[header + 23], + _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], + _read_buf[header + 29] + ); + } + + read_retries--; + flush_uart_rx(); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + px4_usleep(2000); + } + + break; + + } else { + memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO)); + read_succeeded = 1; + break; + } + + } else { + read_retries--; + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send version info packet"); + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + px4_usleep(2000); + } + } + } + + if (! read_succeeded) { + return -EIO; + } + + return 0; +} + +bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + /* Stop Mixer while ESCs are being calibrated */ + if (_outputs_disabled) { + return 0; + } + + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) + //So, if Run() is blocked by a custom command, this function will not be called until Run is running again + int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int32_t _fb_idx = -1; + + if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) { + PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); + return false; + } + + for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { + // do not run any signal on disabled channels + if (!_mixing_output.isFunctionSet(i)) { + outputs[i] = 0; + } + + if (outputs[i]) { + _pwm_on = true; + } + + if (!_pwm_on || stop_motors) { + _rate_req[i] = 0; + + } else { + _rate_req[i] = outputs[i]; + } + + _pwm_values[i] = _rate_req[i]; + } + + Command cmd; + cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], + _led_req[0], _led_req[1], _led_req[2], _led_req[3], + _fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_pwm_on && _debug) { + PX4_INFO("Mixer outputs"); + PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5], + outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11], + outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17] + ); + + // Debug messages for PWM 400Hz values sent to M0065 + uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1); + uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2); + uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3); + uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; + PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4); + PX4_INFO(""); + } + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet"); + return false; + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } + + perf_count(_output_update_perf); + + return true; +} + +int Voxl2IO::flush_uart_rx() +{ + while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} + + return 0; +} + +static bool valid_port(int port) +{ + if (port == 2 || port == 6 || port == 7) { return true; } + + return false; +} + +int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) +{ + for (int i = 0; i < len; i++) { + int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet); + + if (ret > 0) { + uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { + return 0; + + } else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) { + return 0; + + } else { + return -1; + } + + } else { //parser error + switch (ret) { + case VOXL2_IO_ERROR_BAD_CHECKSUM: + if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); } + + break; + + case VOXL2_IO_ERROR_BAD_LENGTH: + if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); } + + break; + + case VOXL2_IO_ERROR_BAD_HEADER: + if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); } + + break; + + case VOXL2_IO_NO_PACKET: + // if(_pwm_on) PX4_WARN("NO packet"); + break; + + default: + if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); } + + break; + } + + return ret; + } + } + + return 0; +} + +void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi, input_rc_s &input_rc) +{ + // fill rc_in struct for publishing + input_rc.channel_count = raw_rc_count_local; + + if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < input_rc.channel_count; i++) { + input_rc.values[i] = raw_rc_values_local[i]; + + if (raw_rc_values_local[i] != UINT16_MAX) { + valid_chans++; + } + + // once filled, reset values back to default + _raw_rc_values[i] = UINT16_MAX; + } + + input_rc.timestamp = now; + input_rc.timestamp_last_signal = input_rc.timestamp; + input_rc.rc_ppm_frame_length = 0; + + /* fake rssi if no value was provided */ + if (rssi == -1) { + input_rc.rssi = 255; + + } else { + input_rc.rssi = rssi; + } + + if (valid_chans == 0) { + input_rc.rssi = 0; + } + + if (frame_drops) { + _sbus_frame_drops++; + } + + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = input_rc.rc_failsafe; + input_rc.rc_lost_frame_count = _sbus_frame_drops; + input_rc.rc_total_frame_count = ++_sbus_total_frames; +} + +int Voxl2IO::receive_sbus() +{ + int res = 0; + int header = -1; + int read_retries = 3; + int read_succeeded = 0; + voxl2_io_packet_init(&_sbus_packet); + + while (read_retries) { + memset(&_read_buf, 0x00, READ_BUF_SIZE); + res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + + if (res) { + /* Get index of packer header */ + for (int index = 0; index < READ_BUF_SIZE; ++index) { + if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { + header = index; + break; + } + } + + /* Try again in a bit if packet header not present yet... */ + if (header == -1) { + if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); } + + read_retries--; + continue; + } + + /* Check if we got a valid packet...*/ + if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) { + if (_pwm_on && _debug) { + PX4_ERR("Error parsing QC RAW SBUS packet"); + PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", + _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], + _read_buf[header + 5], + _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], + _read_buf[header + 11], + _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], + _read_buf[header + 17], + _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], + _read_buf[header + 23], + _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], + _read_buf[header + 29] + ); + } + + read_retries--; + break; + } + + input_rc_s input_rc; + uint16_t num_values; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); + hrt_abstime now = hrt_absolute_time(); + bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); + + if (rc_updated) { + if (_pwm_on && _debug) { + PX4_INFO("Decoded packet, header pos: %i", header); + PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], + _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], + _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], + _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], + _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], + _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] + ); + } + + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); + + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + _rc_last_valid = input_rc.timestamp; + } + + input_rc.timestamp_last_signal = _rc_last_valid; + _rc_pub.publish(input_rc); + + _bytes_received += res; + _packets_received++; + read_succeeded = 1; + break; + + } else if (_pwm_on && _debug) { + PX4_ERR("Failed to decode SBUS packet, header pos: %i", header); + + if (sbus_frame_drop) { + PX4_WARN("SBUS frame dropped"); + } + + PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]", + _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], + _read_buf[header + 5], + _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], + _read_buf[header + 11], + _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], + _read_buf[header + 17], + _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], + _read_buf[header + 23], + _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], + _read_buf[header + 29] + ); + } + } + + read_retries--; + } + + if (! read_succeeded) { + _new_packet = false; + return -EIO; + } + + _new_packet = true; + return 0; +} + + +void Voxl2IO::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + /* Handle RC */ + if (_rc_mode == RC_MODE::SCAN) { + if (receive_sbus() == PX4_OK) { + PX4_INFO("Found M0065 SBUS RC."); + _rc_mode = RC_MODE::SBUS; + } // Add more cases here for other protocols in the future.. + + } else if (_rc_mode == RC_MODE::SBUS) { + receive_sbus(); + } + + /* Only update outputs if we have new values from RC */ + if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) { + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module + _new_packet = false; + } + + /* update PWM status if armed or if disarmed PWM values are set */ + _pwm_on = _mixing_output.armed().armed; + + /* check for parameter updates */ + if (!_pwm_on && _parameter_update_sub.updated()) { + /* clear update */ + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + /* update parameters from storage */ + update_params(); + } + + /* Don't process commands if pwm on */ + if (!_pwm_on) { + if (_current_cmd.valid()) { + PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us); + flush_uart_rx(); + + do { + PX4_INFO("CMDs left %d", _current_cmd.repeats); + + if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { + if (_current_cmd.repeats == 0) { + _current_cmd.clear(); + } + + + } else { + _bytes_sent += _current_cmd.len; + _packets_sent++; + + 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 Voxl2IO::task_spawn(int argc, char *argv[]) +{ + Voxl2IO *instance = new Voxl2IO(); + + if (instance) { + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + _object.store(instance); + _task_id = task_id_is_work_queue; + argv++; + + while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'v': + PX4_INFO("Verbose mode enabled"); + get_instance()->_debug = true; + break; + + case 'd': + PX4_INFO("M0065 PWM outputs disabled"); + get_instance()->_outputs_disabled = true; + break; + + case 'e': + PX4_INFO("M0065 using external RC"); + get_instance()->_rc_mode = RC_MODE::EXTERNAL; + break; + + case 'p': + if (valid_port(atoi(myoptarg))) { + snprintf(get_instance()->_device, 2, "%s", myoptarg); + + } else { + PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; + } + + break; + + default: + print_usage("Unknown command, parsing flags"); + break; + } + } + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + + +bool Voxl2IO::stop_all_pwms() +{ + int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + uint8_t _fb_idx = 0; + + Command cmd; + cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], + _led_req[0], _led_req[1], _led_req[2], _led_req[3], + _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; + + } else { + _bytes_sent += cmd.len; + _packets_sent++; + } + + return true; +} + +int Voxl2IO::send_cmd_thread_safe(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 Voxl2IO::calibrate_escs() +{ + + /* Disable outputs so Mixer isn't being a PITA while we calibrate */ + _outputs_disabled = true; + + Command cmd; + int32_t fb_idx = -1; + uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0}; + cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf, + sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet"); + _outputs_disabled = false; + return -1; + } + + /* Give user 10 seconds to plug in PWM cable for ESCs */ + PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)"); + hrt_abstime start_cal = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_cal) < 10000000) { + continue; + } + + /* PWM MAX 3 seconds */ + PX4_INFO("Writing PWM MAX for 3 seconds!"); + int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX}; + + if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); } + + int16_t led_cmd[4] {0, 0, 0, 0}; + cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3], + led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], + fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet"); + _outputs_disabled = false; + return -1; + + } else { + cmd.clear(); + } + + hrt_abstime start_pwm_max = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_pwm_max) < 3000000) { + continue; + } + + /* PWM MIN 4 seconds */ + PX4_INFO("Writing PWM MIN for 4 seconds!"); + int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN}; + + if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); } + + cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3], + led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], + fb_idx, cmd.buf, sizeof(cmd.buf)); + + if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet"); + _outputs_disabled = false; + return -1; + } + + hrt_abstime start_pwm_min = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_pwm_min) < 4000000) { + continue; + } + + PX4_INFO("ESC Calibration complete"); + + _outputs_disabled = false; + return 0; +} + +int Voxl2IO::custom_command(int argc, char *argv[]) +{ + int myoptind = 0; + int ch; + const char *myoptarg = nullptr; + + Command cmd; + uint8_t output_channel = 0xF; + int16_t rate = 0; + + uint32_t repeat_count = 100; + uint32_t repeat_delay_us = 10000; + + const char *verb = argv[argc - 1]; + + if ((strcmp(verb, "pwm")) == 0 && argc < 3) { + return print_usage("pwm command: missing args"); + + } else if (argc < 1) { + return print_usage("unknown command: missing args"); + } + + PX4_INFO("Executing the following command: %s", verb); + + /* start the FMU if not running */ + if (!strcmp(verb, "start")) { + if (!is_running()) { + return Voxl2IO::task_spawn(argc, argv); + } + } + + if (!strcmp(verb, "status")) { + if (!is_running()) { + PX4_INFO("Not running"); + return -1; + } + + return get_instance()->print_status(); + } + + if (!is_running()) { + PX4_INFO("Not running"); + return -1; + } + + if (!strcmp(verb, "calibrate_escs")) { + if (get_instance()->_outputs_disabled) { + PX4_WARN("Can't calibrate ESCs while outputs are disabled."); + return -1; + } + + return get_instance()->calibrate_escs(); + } + + while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'c': + output_channel = atoi(myoptarg); + + if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) { + char reason[50]; + sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1); + print_usage(reason); + return 0; + } + + break; + + case 'n': + repeat_count = atoi(myoptarg); + + if (repeat_count < 1) { + print_usage("bad repeat_count"); + return 0; + } + + break; + + case 't': + repeat_delay_us = atoi(myoptarg); + + if (repeat_delay_us < 1) { + print_usage("bad repeat delay"); + return 0; + } + + break; + + case 'r': + rate = atoi(myoptarg); + break; + + case 'p': + if (valid_port(atoi(myoptarg))) { + snprintf(get_instance()->_device, 2, "%s", myoptarg); + + } else { + PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + return 0; + } + + break; + + default: + print_usage("Unknown command, parsing flags"); + return 0; + } + } + + if (!strcmp(verb, "pwm")) { + PX4_INFO("Output channel: %i", output_channel); + PX4_INFO("Repeat count: %i", repeat_count); + PX4_INFO("Repeat delay (us): %i", repeat_delay_us); + PX4_INFO("Rate: %i", rate); + + if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) { + PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate); + int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + uint8_t id_fb = 0; + + if (output_channel == + 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'. + rate_req[0] = rate; + rate_req[1] = rate; + rate_req[2] = rate; + rate_req[3] = rate; + + } else { + rate_req[output_channel] = rate; + id_fb = output_channel; + } + + cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3], + 0, 0, 0, 0, + id_fb, cmd.buf, sizeof(cmd.buf)); + + cmd.response = false; + cmd.repeats = repeat_count; + cmd.resp_delay_us = 1000; + cmd.repeat_delay_us = repeat_delay_us; + cmd.print_feedback = false; + + PX4_INFO("feedback id debug: %i", id_fb); + PX4_INFO("Sending UART M0065 power command %i", rate); + + if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("Failed to send packet: stop PWMs"); + return -1; + + } else { + get_instance()->_bytes_sent += cmd.len; + get_instance()->_packets_sent++; + } + + } else { + print_usage("Invalid Output Channel, use 0-3"); + return 0; + } + } + + return print_usage("unknown custom command"); +} + +int Voxl2IO::print_status() +{ + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); + PX4_INFO("PWM Rate: 400 Hz"); // Only support 400 Hz for now + PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no"); + PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version); + PX4_INFO("RC Type: SBUS"); // Only support SBUS through M0065 for now + PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes"); + PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames); + PX4_INFO("UART port: %s", _device); + PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); + PX4_INFO("Packets sent: %" PRIu32, _packets_sent); + PX4_INFO(""); + PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate); + PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]); + PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]); + PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]); + PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + + perf_print_counter(_cycle_perf); + perf_print_counter(_output_update_perf); + PX4_INFO(""); + _mixing_output.printStatus(); + return 0; +} + +int Voxl2IO::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for driving the output pins. For boards without a separate IO chip +(eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the +px4io driver is used for main ones. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("voxl2_io", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); + PRINT_MODULE_USAGE_PARAM_FLAG('v', "Verbose messages", false); + PRINT_MODULE_USAGE_PARAM_FLAG('d', "Disable PWM", false); + PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); + PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); + PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); + PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); + PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); + PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]); + +int voxl2_io_main(int argc, char *argv[]) +{ + return Voxl2IO::main(argc, argv); +} diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp new file mode 100644 index 0000000000..6ac46251bc --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "voxl2_io_serial.hpp" + +#include "voxl2_io_packet.h" +#include "voxl2_io_packet_types.h" + +using namespace time_literals; + +class Voxl2IO final : public ModuleBase, public OutputModuleInterface +{ +public: + Voxl2IO(); + ~Voxl2IO() override; + + /** @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::print_status() */ + int print_status() override; + + /** @see OutputModuleInterface */ + bool updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + virtual int init(); + + void update_pwm_config(); + int get_version_info(); + + struct Command { + uint16_t id = 0; + uint8_t len = 0; + uint16_t repeats = 0; + uint16_t repeat_delay_us = 2000; + uint8_t retries = 0; + bool response = false; + uint16_t resp_delay_us = 1000; + bool print_feedback = false; + + static const uint8_t BUF_SIZE = 128; + uint8_t buf[BUF_SIZE]; + + bool valid() const { return len > 0; } + void clear() { len = 0; } + }; + + int send_cmd_thread_safe(Command *cmd); + int receive_sbus(); + + void fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi, input_rc_s &input_rc); +private: + void Run() override; + bool stop_all_pwms(); + + /* PWM Parameters */ + static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off + static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes + static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1; + static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; + static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4; + static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0; + + static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200; + static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000; + + static constexpr uint16_t DISARMED_VALUE = 0; + + static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0; + static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800; + static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0; + static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0; + + static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000; + static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000; + static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900; + static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks + + /* SBUS */ + static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30; + static constexpr uint16_t SBUS_PAYLOAD = 3; + + /* M0065 version info */ + static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6; + static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1; + static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35; + VOXL2_IO_VERSION_INFO _version_info; + + /* Module update interval */ + static constexpr unsigned _current_update_interval{4000}; // 250 Hz + + typedef struct { + int32_t config{VOXL2_IO_CONFIG}; + int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD}; + int32_t pwm_min{VOXL2_IO_DEFAULT_MIN}; + int32_t pwm_max{VOXL2_IO_DEFAULT_MAX}; + int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; + int32_t param_rc_input_proto{0}; + int32_t param_rc_rssi_pwm_chan{0}; + int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t verbose_logging{0}; + } voxl2_io_params_t; + voxl2_io_params_t _parameters; + + typedef enum { + PWM_MODE_START = 0, + PWM_MODE_400, + PWM_MODE_END + } PWM_MODE; + + enum RC_MODE { + DISABLED = 0, + SBUS, + SPEKTRUM, + EXTERNAL, + SCAN + } _rc_mode{RC_MODE::SCAN}; + + /* QUP7, VOXL2 J19, /dev/slpi-uart-7*/ + char _device[10] {VOXL2_IO_DEFAULT_PORT}; + Voxl2IoSerial *_uart_port; + + /* Mixer output */ + MixingOutput _mixing_output; + + /* RC input */ + VOXL2_IOPacket _sbus_packet; + uint64_t _rc_last_valid; + uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX}; + unsigned _sbus_frame_drops{0}; + uint16_t _sbus_total_frames{0}; + bool _new_packet{false}; + + /* Publications */ + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + + /* Subscriptions */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + bool _pwm_on{false}; + int32_t _pwm_fullscale{0}; + int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; + bool _outputs_disabled{false}; + + perf_counter_t _cycle_perf; + perf_counter_t _output_update_perf; + + bool _debug{false}; + uint16_t _cmd_id{0}; + Command _current_cmd; + px4::atomic _pending_cmd{nullptr}; + + static const uint8_t READ_BUF_SIZE = 128; + uint8_t _read_buf[READ_BUF_SIZE]; + uint32_t _bytes_sent{0}; + uint32_t _bytes_received{0}; + uint32_t _packets_sent{0}; + uint32_t _packets_received{0}; + + int parse_response(uint8_t *buf, uint8_t len); + int load_params(voxl2_io_params_t *params); + int update_params(); + int flush_uart_rx(); + int calibrate_escs(); +}; diff --git a/src/drivers/voxl2_io/voxl2_io_crc16.c b/src/drivers/voxl2_io/voxl2_io_crc16.c new file mode 100644 index 0000000000..229610b1e0 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_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 "voxl2_io_crc16.h" + +// Look-up table for fast CRC16 computations +const uint16_t voxl2_io_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 voxl2_io_crc16_init() +{ + return 0xFFFF; +} + +uint16_t voxl2_io_crc16_byte(uint16_t prev_crc, const uint8_t new_byte) +{ + uint8_t lut = (prev_crc ^ new_byte) & 0xFF; + return (prev_crc >> 8) ^ voxl2_io_crc16_table[lut]; +} + +uint16_t voxl2_io_crc16(uint16_t prev_crc, uint8_t const *input_buffer, uint16_t input_length) +{ + uint16_t crc = prev_crc; + + while (input_length--) { + crc = voxl2_io_crc16_byte(crc, *input_buffer++); + } + + return crc; +} diff --git a/src/drivers/voxl2_io/voxl2_io_crc16.h b/src/drivers/voxl2_io/voxl2_io_crc16.h new file mode 100644 index 0000000000..c8e320edda --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_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/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c new file mode 100644 index 0000000000..3de26acbfe --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_packet.c @@ -0,0 +1,253 @@ +/**************************************************************************** + * 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 "voxl2_io_packet.h" +#include "voxl2_io_packet_types.h" + +#include + +int32_t voxl2_io_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_VERSION_REQUEST, &id, 1, out, out_size); +} + +int32_t voxl2_io_create_extended_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_VERSION_EXT_REQUEST, &id, 1, out, out_size); +} + +int32_t voxl2_io_create_reset_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + char payload[] = "RESET0"; + payload[5] += id; + + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RESET_CMD, (uint8_t *)payload, 6 /*sizeof(payload)*/, out, out_size); +} + + +int32_t voxl2_io_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 voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_SOUND_CMD, (uint8_t *) & (data[0]), 4, out, out_size); +} + +int32_t voxl2_io_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 voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_LED_CMD, (uint8_t *) & (data[0]), 2, out, out_size); +} + +int32_t voxl2_io_create_set_id_packet(uint8_t id, uint8_t *out, uint16_t out_size) +{ + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_SET_ID_CMD, (uint8_t *)&id, 1, out, out_size); +} + +int32_t voxl2_io_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 voxl2_io_create_pwm_packet4_fb(pwm0, pwm1, pwm2, pwm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t voxl2_io_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 voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + + +int32_t voxl2_io_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 voxl2_io_create_rpm_packet4_fb(rpm0, rpm1, rpm2, rpm3, led0, led1, led2, led3, -1, out, out_size); +} + +int32_t voxl2_io_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 voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); +} + +int32_t voxl2_io_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 = voxl2_io_crc16_init(); + crc = voxl2_io_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 voxl2_io_packet_process_char(uint8_t c, VOXL2_IOPacket *packet) +{ + int16_t ret = VOXL2_IO_NO_PACKET; + + uint16_t chk_comp; + uint16_t chk_rcvd; + + if (packet->len_received >= (sizeof(packet->buffer) - 1)) { + packet->len_received = 0; + } + + //reset the packet and start parsing from beginning if length byte == header + //this can only happen if the packet is de-synced and last char of checksum + //ends up being equal to the header, in that case we can end up in endless loop + //unable to re-sync with the packet + if (packet->len_received == 1 && c == VOXL2_IO_PACKET_HEADER) { + packet->len_received = 0; + } + + switch (packet->len_received) { + case 0: //header + packet->bp = packet->buffer; //reset the pointer for storing data + voxl2_io_packet_checksum_reset(packet); //reset the checksum to starting value + + if (c != VOXL2_IO_PACKET_HEADER) { //check the packet header + packet->len_received = 0; + ret = VOXL2_IO_ERROR_BAD_HEADER; + 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 = VOXL2_IO_ERROR_BAD_LENGTH; + break; + } + + voxl2_io_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) + voxl2_io_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 = voxl2_io_packet_checksum_get(packet); + + memcpy(&chk_rcvd, packet->bp - 2, sizeof(uint16_t)); + + if (chk_comp == chk_rcvd) { ret = packet->len_received; } + + else { ret = VOXL2_IO_ERROR_BAD_CHECKSUM; } + + packet->len_received = 0; + break; + } + + return ret; +} diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h new file mode 100644 index 0000000000..7e6237017f --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_packet.h @@ -0,0 +1,287 @@ +/**************************************************************************** + * 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 Voxl2 IO UART interface + */ + +#ifndef VOXL2_IO_PACKET +#define VOXL2_IO_PACKET + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "voxl2_io_crc16.h" + +// Define byte values that correspond to setting red, greed, and blue LEDs +#define VOXL2_IO_LED_RED_ON 1 +#define VOXL2_IO_LED_GREEN_ON 2 +#define VOXL2_IO_LED_BLUE_ON 4 + + +// Header of the packet. Each packet must start with this header +#define VOXL2_IO_PACKET_HEADER 0xAF + +enum { VOXL2_IO_ERROR_BAD_LENGTH = -3, + VOXL2_IO_ERROR_BAD_HEADER = -2, + VOXL2_IO_ERROR_BAD_CHECKSUM = -1, + VOXL2_IO_NO_PACKET = 0 + }; + +// Defines for the constatnt offsets of different parts of the packet +enum { VOXL2_IO_PACKET_POS_HEADER1 = 0, + VOXL2_IO_PACKET_POS_LENGTH, + VOXL2_IO_PACKET_POS_TYPE, + VOXL2_IO_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[64]; // Buffer to hold incoming data that is being parsed +} VOXL2_IOPacket; + + +// 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__)) VOXL2_IO_VERSION_INFO; + +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; + uint16_t sw_version; + uint16_t hw_version; + uint8_t unique_id[12]; + char firmware_git_version[12]; + char bootloader_git_version[12]; + uint16_t bootloader_version; + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_EXTENDED_VERSION_INFO; + +// Definition of the feedback 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_FB_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__)) VOXL2_IO_FB_RESPONSE; + +// Definition of the feedback 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_FB_RESPONSE + uint8_t id_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 power; // Applied power [0..100] + + uint16_t voltage; // Voltage measured by the ESC in mV + int16_t current; // Current measured by the ESC in 8mA resolution + int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution + + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_FB_RESPONSE_V2; + + +// Definition of the feedback response packet from ESC, which contains battery voltage and total current +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; //ESC Id (could be used as system ID elsewhere) + uint16_t voltage; //Input voltage (Millivolts) + int16_t current; //Total Current (8mA resolution) + uint16_t crc; +} __attribute__((__packed__)) VOXL2_IO_FB_POWER_STATUS; + + +//------------------------------------------------------------------------- +//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 voxl2_io_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 voxl2_io_create_version_request_packet(uint8_t id, uint8_t *out, uint16_t out_size); +int32_t voxl2_io_create_extended_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 voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_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 voxl2_io_packet_process_char(uint8_t c, VOXL2_IOPacket *packet); + +// Get a pointer to the packet type from a pointer to VOXL2_IOPacket +static inline uint8_t voxl2_io_packet_get_type(VOXL2_IOPacket *packet) { return packet->buffer[VOXL2_IO_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 voxl2_io_packet_raw_get_type(uint8_t *packet) { return packet[VOXL2_IO_PACKET_POS_TYPE]; } + +//get a pointer to the packet payload from a pointer to VOXL2_IOPacket +static inline uint8_t *voxl2_io_packet_get_data_ptr(VOXL2_IOPacket *packet) { return &(packet->buffer[VOXL2_IO_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 *voxl2_io_packet_raw_get_data_ptr(uint8_t *packet) { return &(packet[VOXL2_IO_PACKET_POS_DATA]); } + +// Get the total size (length) in bytes of the packet +static inline uint8_t voxl2_io_packet_get_size(VOXL2_IOPacket *packet) { return packet->buffer[VOXL2_IO_PACKET_POS_LENGTH]; } + +// Get checksum of the packet from a pointer to VOXL2_IOPacket +static inline uint16_t voxl2_io_packet_checksum_get(VOXL2_IOPacket *packet) { return packet->crc; } + +// Calculate the checksum of a data array. Used for packet generation / processing +static inline uint16_t voxl2_io_packet_checksum(uint8_t *buf, uint16_t size) +{ + uint16_t crc = voxl2_io_crc16_init(); + return voxl2_io_crc16(crc, buf, size); +} + +// Reset the checksum of the incoming packet. Used internally for packet reception +static inline void voxl2_io_packet_checksum_reset(VOXL2_IOPacket *packet) { packet->crc = voxl2_io_crc16_init(); } + +// Process one character for checksum calculation while receiving a packet (used internally for packet reception) +static inline void voxl2_io_packet_checksum_process_char(VOXL2_IOPacket *packet, uint8_t c) +{ + packet->crc = voxl2_io_crc16_byte(packet->crc, c); +} + + +// Initialize an instance of an VOXL2_IOPacket. This should be called once before using an instance of VOXL2_IOPacket +static inline void voxl2_io_packet_init(VOXL2_IOPacket *packet) +{ + packet->len_received = 0; + packet->len_expected = 0; + packet->bp = 0; + + voxl2_io_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 voxl2_io_packet_reset(VOXL2_IOPacket *packet) +{ + packet->len_received = 0; +} + +#endif //VOXL2_IO_PACKET + +#ifdef __cplusplus +} +#endif diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h new file mode 100644 index 0000000000..150982eca4 --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * 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 VOXL2_IO UART packets + */ + +#ifndef VOXL2_IO_PACKET_TYPES +#define VOXL2_IO_PACKET_TYPES + +#define VOXL2_IO_PACKET_TYPE_VERSION_REQUEST 0 +#define VOXL2_IO_PACKET_TYPE_PWM_CMD 1 +#define VOXL2_IO_PACKET_TYPE_RPM_CMD 2 +#define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3 +#define VOXL2_IO_PACKET_TYPE_STEP_CMD 4 +#define VOXL2_IO_PACKET_TYPE_LED_CMD 5 +#define VOXL2_IO_PACKET_TYPE_RESET_CMD 10 +#define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11 +#define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12 +#define VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST 20 +#define VOXL2_IO_PACKET_TYPE_CONFIG_USER_REQUEST 21 +#define VOXL2_IO_PACKET_TYPE_CONFIG_UART_REQUEST 22 +#define VOXL2_IO_PACKET_TYPE_CONFIG_TUNE_REQUEST 23 +#define VOXL2_IO_PACKET_TYPE_VERSION_EXT_REQUEST 24 + +#define VOXL2_IO_PACKET_TYPE_SET_FEEDBACK_MODE 50 //reserved for future use + +#define VOXL2_IO_PACKET_TYPE_EEPROM_WRITE_UNLOCK 70 +#define VOXL2_IO_PACKET_TYPE_EEPROM_READ_UNLOCK 71 +#define VOXL2_IO_PACKET_TYPE_EEPROM_WRITE 72 + +#define VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE 109 +#define VOXL2_IO_PACKET_TYPE_PARAMS 110 +#define VOXL2_IO_PACKET_TYPE_BOARD_CONFIG 111 +#define VOXL2_IO_PACKET_TYPE_USER_CONFIG 112 +#define VOXL2_IO_PACKET_TYPE_UART_CONFIG 113 +#define VOXL2_IO_PACKET_TYPE_TUNE_CONFIG 114 +#define VOXL2_IO_PACKET_TYPE_FB_RESPONSE 128 +#define VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE 131 +#define VOXL2_IO_PACKET_TYPE_FB_POWER_STATUS 132 +#define VOXL2_IO_PACKET_TYPE_RC_DATA_RAW 133 + +#endif diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c new file mode 100644 index 0000000000..c718a4f62c --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 M0065 baud rate + * + * Default rate is 921600, which is used for communicating with M0065. + * + * @group VOXL2 IO + * @unit bit/s + */ +PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600); + +/** + * M0065 PWM Min + * + * Minimum duration (microseconds) for M0065 PWM + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); + +/** + * M0065 PWM Max + * + * Maximum duration (microseconds) for M0065 PWM + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); + diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp new file mode 100644 index 0000000000..cee0923ece --- /dev/null +++ b/src/drivers/voxl2_io/voxl2_io_serial.cpp @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * 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 "voxl2_io_serial.hpp" + +Voxl2IoSerial::Voxl2IoSerial() +{ +} + +Voxl2IoSerial::~Voxl2IoSerial() +{ + if (_uart_fd >= 0) { + uart_close(); + } +} + +int Voxl2IoSerial::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 */ +#ifdef __PX4_QURT + _uart_fd = qurt_uart_open(dev, speed); +#else + _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); +#endif + + if (_uart_fd < 0) { + PX4_ERR("Error opening port: %s (%i)", dev, errno); + return -1; + } + +#ifndef __PX4_QURT + /* 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; + } + +#endif + + _speed = speed; + + return 0; +} + +int Voxl2IoSerial::uart_set_baud(speed_t speed) +{ +#ifndef __PX4_QURT + + if (_uart_fd < 0) { + return -1; + } + + if (cfsetispeed(&_cfg, speed) < 0) { + return -1; + } + + if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { + return -1; + } + + _speed = speed; + + return 0; +#endif + + return -1; +} + +int Voxl2IoSerial::uart_close() +{ +#ifndef __PX4_QURT + + 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"); + } + +#endif + + _uart_fd = -1; + + return 0; +} + +int Voxl2IoSerial::uart_write(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for writing or buffer"); + return -1; + } + +#ifdef __PX4_QURT + return qurt_uart_write(_uart_fd, (const char *) buf, len); +#else + return write(_uart_fd, buf, len); +#endif +} + +int Voxl2IoSerial::uart_read(FAR void *buf, size_t len) +{ + if (_uart_fd < 0 || buf == NULL) { + PX4_ERR("invalid state for reading or buffer"); + return -1; + } + +#ifdef __PX4_QURT +#define ASYNC_UART_READ_WAIT_US 2000 + // The UART read on SLPI is via an asynchronous service so specify a timeout + // for the return. The driver will poll periodically until the read comes in + // so this may block for a while. However, it will timeout if no read comes in. + return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); +#else + return read(_uart_fd, buf, len); +#endif +} diff --git a/src/modules/muorb/apps/fc_sensor.h b/src/drivers/voxl2_io/voxl2_io_serial.hpp similarity index 62% rename from src/modules/muorb/apps/fc_sensor.h rename to src/drivers/voxl2_io/voxl2_io_serial.hpp index 242a5a708b..638bdef288 100644 --- a/src/modules/muorb/apps/fc_sensor.h +++ b/src/drivers/voxl2_io/voxl2_io_serial.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 ModalAI, Inc. All rights reserved. + * 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 @@ -31,39 +31,39 @@ * ****************************************************************************/ -#ifndef FC_SENSOR_H -#define FC_SENSOR_H +#pragma once -#ifdef __cplusplus -extern "C" { +#include +#include +#include +#include + +#ifdef __PX4_QURT +#include +#define FAR #endif -#include -#include +class Voxl2IoSerial +{ +public: + Voxl2IoSerial(); + virtual ~Voxl2IoSerial(); -typedef void (*fc_receive_cb)(const char *topic, - const uint8_t *data, - uint32_t length_in_bytes); -typedef void (*fc_advertise_cb)(const char *topic); -typedef void (*fc_add_subscription_cb)(const char *topic); -typedef void (*fc_remove_subscription_cb)(const char *topic); + 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; }; + int uart_get_baud() {return _speed; } -typedef struct { - fc_receive_cb rx_callback; - fc_advertise_cb ad_callback; - fc_add_subscription_cb sub_callback; - fc_remove_subscription_cb remove_sub_callback; -} fc_callbacks; +private: + int _uart_fd = -1; -int fc_sensor_initialize(bool enable_debug_messages, fc_callbacks *callbacks); -int fc_sensor_advertise(const char *topic); -int fc_sensor_subscribe(const char *topic); -int fc_sensor_unsubscribe(const char *topic); -int fc_sensor_send_data(const char *topic, - const uint8_t *data, - uint32_t length_in_bytes); -#ifdef __cplusplus -} +#if ! defined(__PX4_QURT) + struct termios _orig_cfg; + struct termios _cfg; #endif -#endif // FC_SENSOR_H + int _speed = -1; +}; diff --git a/src/modules/muorb/apps/CMakeLists.txt b/src/modules/muorb/apps/CMakeLists.txt index 55573cde35..5a608bec53 100644 --- a/src/modules/muorb/apps/CMakeLists.txt +++ b/src/modules/muorb/apps/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( INCLUDES ../test ../aggregator + libfc-sensor-api/inc SRCS uORBAppsProtobufChannel.cpp muorb_main.cpp diff --git a/src/modules/muorb/apps/libfc-sensor-api b/src/modules/muorb/apps/libfc-sensor-api new file mode 160000 index 0000000000..ca16e99074 --- /dev/null +++ b/src/modules/muorb/apps/libfc-sensor-api @@ -0,0 +1 @@ +Subproject commit ca16e99074641a10d153961291243ede7720c2e2 diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp index 9634165479..8669d0650d 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp @@ -113,6 +113,11 @@ void uORB::AppsProtobufChannel::SubscribeCallback(const char *topic) test_flag = true; return; + } else if (strcmp(topic, "CPULOAD") == 0) { + // PX4_ERR("Got CPULOAD subscription"); + // This will happen when a newer PX4 version is talking to a + // SLPI image that doesn't support the CPULOAD request. If the + // SLPI image does support it then we wouldn't get this. } else if (_RxHandler) { pthread_mutex_lock(&_rx_mutex); _SlpiSubscriberCache[topic]++; diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index 64026bfa2f..f358b3f3a5 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -385,8 +385,13 @@ int px4muorb_add_subscriber(const char *topic_name) uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); if (rxHandler) { - channel->AddRemoteSubscriber(topic_name); - // Pick a high message rate of 1000 Hz + if (channel->AddRemoteSubscriber(topic_name)) { + // Only process this subscription if it is the only one for the topic. + // Otherwise it will send some data from the queue and, most likely, + // mess up the queue on the remote side. + return 0; + } + return rxHandler->process_add_subscription(topic_name); } else { @@ -476,3 +481,32 @@ int px4muorb_send_topic_data(const char *topic_name, const uint8_t *data, return -1; } + + +float px4muorb_get_cpu_load(void) +{ + + // Default value to return if the SLPI code doesn't support + // queries for the CPU load + float cpu_load = 0.1f; + + uORB::ProtobufChannel *channel = uORB::ProtobufChannel::GetInstance(); + + if (channel) { + // The method to get the CPU load from the SLPI image is to send + // in the special code string to the add_subscription call. If it + // isn't supported the only return values can be 0 or -1. If it is + // supported then it will be some positive integer. + int16_t int_cpu_load = channel->add_subscription("CPULOAD", 0); + + if (int_cpu_load > 1) { + // Yay! CPU Load query is supported! + cpu_load = (float) int_cpu_load; + } + + } else { + PX4_ERR("Null channel pointer in %s", __FUNCTION__); + } + + return cpu_load; +} diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.hpp b/src/modules/muorb/slpi/uORBProtobufChannel.hpp index ecd1d02780..54ee99ed56 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.hpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.hpp @@ -132,11 +132,13 @@ public: _Aggregator.RegisterSendHandler(func); } - void AddRemoteSubscriber(const std::string &messageName) + int AddRemoteSubscriber(const std::string &messageName) { + int currentRemoteSubscribers; pthread_mutex_lock(&_rx_mutex); - _AppsSubscriberCache[messageName]++; + currentRemoteSubscribers = _AppsSubscriberCache[messageName]++; pthread_mutex_unlock(&_rx_mutex); + return currentRemoteSubscribers; } void RemoveRemoteSubscriber(const std::string &messageName) @@ -214,6 +216,8 @@ extern "C" { int px4muorb_remove_subscriber(const char *name) __EXPORT; int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + + float px4muorb_get_cpu_load(void) __EXPORT; } #endif // _uORBProtobufChannel_hpp_