dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VOXL2 specific drivers, modules, and miscellaneous support files (#22588)

This commit is contained in:
Eric Katzfey 2024-01-18 09:14:17 -08:00 committed by GitHub
parent b60e73c76f
commit 2b69a3d290
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
96 changed files with 8266 additions and 678 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -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

2
boards/modalai/fc-v1/default.px4board
View File

@ -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

4
boards/modalai/fc-v1/init/rc.board_sensors
View File

@ -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

2
boards/modalai/fc-v1/src/board_config.h
View File

@ -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

16
boards/modalai/fc-v2/default.px4board
View File

@ -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

4
boards/modalai/fc-v2/init/rc.board_sensors
View File

@ -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

2
boards/modalai/fc-v2/src/board_config.h
View File

@ -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

5
boards/modalai/voxl2-io/default.px4board Normal file
View File

@ -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

13
boards/modalai/voxl2-io/firmware.prototype Normal file
View File

@ -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
}

141
boards/modalai/voxl2-io/nuttx-config/include/board.h Normal file
View File

@ -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 <gnutt@nuttx.org>
* 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 <nuttx/config.h>
#ifndef __ASSEMBLY__
# include <stdint.h>
#endif
#include <stm32.h>
/************************************************************************************
* 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 */

61
boards/modalai/voxl2-io/nuttx-config/nsh/defconfig Normal file
View File

@ -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

131
boards/modalai/voxl2-io/nuttx-config/scripts/script.ld Normal file
View File

@ -0,0 +1,131 @@
/****************************************************************************
* configs/px4io-v2/scripts/ld.script
*
* Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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) }
}

42
boards/modalai/voxl2-io/src/CMakeLists.txt Normal file
View File

@ -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
)

157
boards/modalai/voxl2-io/src/board_config.h Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <nuttx/compiler.h>
#include <stdint.h>
/******************************************************************************
* 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 <px4_platform_common/board_common.h>

133
boards/modalai/voxl2-io/src/init.c Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <stdbool.h>
#include <stdio.h>
#include <debug.h>
#include <errno.h>
#include <syslog.h>
#include <nuttx/board.h>
#include <stm32.h>
#include "board_config.h"
#include <arch/board/board.h>
/****************************************************************************
* 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);
}

54
boards/modalai/voxl2-io/src/timer_config.cpp Normal file
View File

@ -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 <px4_arch/io_timer_hw_description.h>
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);

53
boards/modalai/voxl2-slpi/default.px4board
View File

@ -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

8
boards/modalai/voxl2-slpi/src/CMakeLists.txt
View File

@ -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)

19
boards/modalai/voxl2-slpi/src/board_config.h
View File

@ -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

49
boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/CMakeLists.txt Normal file
View File

@ -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
)

1064
boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

41
boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/CMakeLists.txt Normal file
View File

@ -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
)

383
boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/dsp_sbus.cpp Normal file
View File

@ -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 <string>
#include <px4_log.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <uORB/PublicationMulti.hpp>
#include <drivers/device/qurt/uart.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/input_rc.h>
#include <lib/parameters/param.h>
#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<input_rc_s> _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<void *>(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 <number> 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;
}

405
boards/modalai/voxl2-slpi/src/drivers/dsp_sbus/protocol.h Normal file
View File

@ -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 <inttypes.h>
/**
* @file protocol.h
*
* PX4IO interface protocol.
*
* @author Lorenz Meier <lorenz@px4.io>
*
* 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;
}

41
boards/modalai/voxl2-slpi/src/drivers/elrs_led/CMakeLists.txt Normal file
View File

@ -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
)

317
boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp Normal file
View File

@ -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 <string>
#include <px4_log.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <uORB/Subscription.hpp>
#include <drivers/device/qurt/uart.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <lib/parameters/param.h>
#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<ControllerInput, std::string> 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 <number> uart port number");
PX4_INFO("Options: -o <number> LEDs off button");
PX4_INFO("Options: -l <number> Overt LEDs on button");
PX4_INFO("Options: -i <number> IR LEDs on button");
PX4_INFO("Options: -d <number> 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;
}

113
boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h Normal file
View File

@ -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 <string>
#include <map>
#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<ControllerInput, std::string> &map, const std::string &value)
{
for (const auto &pair : map) {
if (pair.second == value) {
return pair.first;
}
}
return ControllerInput::DEFAULT;
}

47
boards/modalai/voxl2-slpi/src/drivers/ghst_rc/CMakeLists.txt Normal file
View File

@ -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
)

5
boards/modalai/voxl2-slpi/src/drivers/ghst_rc/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_GHST_RC
bool "ghst_rc"
default n
---help---
Enable support for ghst rc

312
boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp Normal file
View File

@ -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 <lib/rc/common_rc.h>
#include "ghst_rc.hpp"
#include <lib/rc/ghst.hpp>
#include <px4_log.h>
#include <drivers/device/qurt/uart.h>
#include <termios.h>
#include <fcntl.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>
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", "<file:dev>", "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);
}

126
boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/input_rc.h>
// telemetry
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>
#define GHST_MAX_NUM_CHANNELS (16)
class GhstRc : public ModuleBase<GhstRc>, 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_s> _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<px4::params::RC_GHST_TEL_EN>) _param_rc_ghst_tel_en
// )
};

11
boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml Normal file
View File

@ -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.

152
boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.cpp
View File

@ -650,8 +650,7 @@ bool ICM42688P::FIFORead(const hrt_abstime &timestamp_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<int32_t>(x);
}
void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo)
void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)

10
boards/modalai/voxl2-slpi/src/drivers/icm42688p/ICM42688P.hpp
View File

@ -50,6 +50,7 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/imu_server.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <memory>
@ -147,7 +148,7 @@ private:
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
void FIFOReset();
void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA &fifo);
void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_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_s> _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_s> _imu_server_pub{ORB_ID(imu_server)};
};

9
boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp
View File

@ -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

8
boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp
View File

@ -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 {

4
boards/modalai/voxl2/cmake/link_libraries.cmake
View File

@ -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}
)

44
boards/modalai/voxl2/default.px4board
View File

@ -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

10
boards/modalai/voxl2/scripts/build-deps.sh Executable file
View File

@ -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 ../../../../../..

2
boards/modalai/voxl2/scripts/build-slpi.sh
View File

@ -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 ***"

7
boards/modalai/voxl2/scripts/clean.sh
View File

@ -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

9
boards/modalai/voxl2/scripts/install-voxl-bins.sh Executable file
View File

@ -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

9
boards/modalai/voxl2/scripts/install-voxl.sh
View File

@ -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"

13
boards/modalai/voxl2/scripts/patch-gazebo.sh Executable file
View File

@ -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 -

3
boards/modalai/voxl2/src/CMakeLists.txt
View File

@ -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)

3
boards/modalai/voxl2/src/board_config.h
View File

@ -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"

41
boards/modalai/voxl2/src/drivers/apps_sbus/CMakeLists.txt Normal file
View File

@ -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
)

453
boards/modalai/voxl2/src/drivers/apps_sbus/apps_sbus.cpp Normal file
View File

@ -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 <string>
#include <px4_log.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <uORB/PublicationMulti.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/topics/input_rc.h>
#include <lib/parameters/param.h>
#include <poll.h>
#include <termios.h>
#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<input_rc_s> _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<void *>(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 <number> 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;
}

405
boards/modalai/voxl2/src/drivers/apps_sbus/protocol.h Normal file
View File

@ -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 <inttypes.h>
/**
* @file protocol.h
*
* PX4IO interface protocol.
*
* @author Lorenz Meier <lorenz@px4.io>
*
* 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;
}

24
boards/modalai/voxl2/target/voxl-px4
View File

@ -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

39
boards/modalai/voxl2/target/voxl-px4-hitl Executable file
View File

@ -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

111
boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config Executable file
View File

@ -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

101
boards/modalai/voxl2/target/voxl-px4-hitl-start Executable file
View File

@ -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=$(</etc/version)
M0052SUBSTRING="M0052"
if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
PLATFORM="M0052"
fi
fi
# We can only run on M0052, M0054, or M0104 so exit with error if that is not the case
if [ $PLATFORM = "M0052" ]; then
/bin/echo "Running on M0052"
elif [ $PLATFORM = "M0054" ]; then
/bin/echo "Running on M0054"
elif [ $PLATFORM = "M0104" ]; then
/bin/echo "Running on M0104"
else
/bin/echo "Error, cannot determine platform!"
exit 0
fi
# Sleep a little here. A lot happens when the uorb and muorb start
# and we need to make sure that it all completes successfully to avoid
# any possible race conditions.
/bin/sleep 1
if [ ! -f /data/px4/param/hitl_parameters ]; then
echo "[INFO] Setting default parameters for PX4 on voxl"
. /etc/modalai/voxl-px4-hitl-set-default-parameters.config
/bin/sync
else
param select /data/px4/param/hitl_parameters
param load
fi
# Start logging and use timestamps for log files when possible.
# Add the "-e" option to start logging immediately. Default is
# to log only when armed. Caution must be used with the "-e" option
# because if power is removed without stopping the logger gracefully then
# the log file may be corrupted.
logger start -e -t -b 200
/bin/sleep 1
# Start all of the processing modules on DSP
qshell sensors start -h
qshell ekf2 start
qshell mc_pos_control start
qshell mc_att_control start
qshell mc_rate_control start
qshell mc_hover_thrust_estimator start
qshell mc_autotune_attitude_control start
qshell land_detector start multicopter
qshell manual_control start
qshell control_allocator start
qshell rc_update start
qshell commander start -h
qshell commander mode posctl
qshell load_mon start
# This is needed for altitude and position hold modes
qshell flight_mode_manager start
/bin/sleep 1
# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
# Start microdds_client for ros2 offboard messages from agent over localhost
microdds_client start -t udp -h 127.0.0.1 -p 8888
qshell pwm_out_sim start -m hil
qshell dsp_hitl start -g -m
# start the onboard fast link to connect to voxl-mavlink-server
mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard
# slow down some of the fastest streams
mavlink stream -u 14556 -s HIGHRES_IMU -r 10
mavlink stream -u 14556 -s ATTITUDE -r 10
mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10
mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30
# start the slow normal mode for voxl-mavlink-server to forward to GCS
mavlink start -x -u 14558 -o 14559 -r 100000 -n lo
/bin/sleep 1
mavlink boot_complete

41
boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config
View File

@ -12,9 +12,8 @@ param set IMU_GYRO_RATEMAX 800
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_EV_CTRL 15
param set EKF2_EV_DELAY 5
param set EKF2_GPS_CTRL 0
param set EKF2_AID_MASK 280
param set EKF2_ABL_LIM 0.8
param set EKF2_TAU_POS 0.25
param set EKF2_TAU_VEL 0.25
@ -79,22 +78,32 @@ param set MPC_TKO_RAMP_T 1.50
param set MPC_TKO_SPEED 1.50
# Set the ESC outputs to function as motors
param set MODAL_IO_FUNC1 101
param set MODAL_IO_FUNC2 103
param set MODAL_IO_FUNC3 104
param set MODAL_IO_FUNC4 102
param set VOXL_ESC_FUNC1 101
param set VOXL_ESC_FUNC2 103
param set VOXL_ESC_FUNC3 104
param set VOXL_ESC_FUNC4 102
param set MODAL_IO_SDIR1 0
param set MODAL_IO_SDIR2 0
param set MODAL_IO_SDIR3 0
param set MODAL_IO_SDIR4 0
param set VOXL_ESC_SDIR1 0
param set VOXL_ESC_SDIR2 0
param set VOXL_ESC_SDIR3 0
param set VOXL_ESC_SDIR4 0
param set MODAL_IO_CONFIG 1
param set MODAL_IO_REV 0
param set MODAL_IO_MODE 0
param set MODAL_IO_BAUD 2000000
param set MODAL_IO_RPM_MAX 10500
param set MODAL_IO_RPM_MIN 1000
param set VOXL_ESC_CONFIG 1
param set VOXL_ESC_REV 0
param set VOXL_ESC_MODE 0
param set VOXL_ESC_BAUD 2000000
param set VOXL_ESC_RPM_MAX 10500
param set VOXL_ESC_RPM_MIN 1000
# Set the Voxl2 IO outputs to function as motors
param set VOXL2_IO_FUNC1 101
param set VOXL2_IO_FUNC2 102
param set VOXL2_IO_FUNC3 103
param set VOXL2_IO_FUNC4 104
param set VOXL2_IO_BAUD 921600
param set VOXL2_IO_MIN 1000
param set VOXL2_IO_MAX 2000
# Some parameters for control allocation
param set CA_ROTOR_COUNT 4

180
boards/modalai/voxl2/target/voxl-px4-start
View File

@ -6,7 +6,9 @@
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 "EXTRA STEPS:"
for i in "${EXTRA_STEPS[@]}"
@ -24,6 +26,32 @@ if [ ! -z $MINIMAL_PX4 ]; then
exit 0
fi
# 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=$(</etc/version)
M0052SUBSTRING="M0052"
if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
PLATFORM="M0052"
fi
fi
# We can only run on M0052, M0054, or M0104 so exit with error if that is not the case
if [ $PLATFORM = "M0052" ]; then
/bin/echo "Running on M0052"
elif [ $PLATFORM = "M0054" ]; then
/bin/echo "Running on M0054"
elif [ $PLATFORM = "M0104" ]; then
/bin/echo "Running on M0104"
else
/bin/echo "Error, cannot determine platform!"
exit 0
fi
# Sleep a little here. A lot happens when the uorb and muorb start
# and we need to make sure that it all completes successfully to avoid
# any possible race conditions.
@ -41,49 +69,40 @@ param load
# the log file may be corrupted.
logger start -t
/bin/sleep 1
# IMU (accelerometer / gyroscope)
if [ "$IMU_ROTATION" != "NONE" ]; then
/bin/echo "Starting IMU driver with rotation: $IMU_ROTATION"
qshell icm42688p start -s -R $IMU_ROTATION
if [ "$PLATFORM" == "M0104" ]; then
/bin/echo "Starting IMU driver with rotation 12"
qshell icm42688p start -s -R 12
else
/bin/echo "Starting IMU driver with no rotation"
qshell icm42688p start -s
fi
/bin/sleep 1
# Start Invensense ICP 101xx barometer built on to VOXL 2
qshell icp101xx start -I -b 5
# Magnetometer
if [ "$GPS" == "MATEK" ]; then
# Use this line for the magnetometer in the Matek Systems M8Q-5883 module
/bin/echo "Starting Mateksys M8Q-5883 magnetometer"
qshell qmc5883l start -R 10 -X -b 1
elif [ "$GPS" == "HOLYBRO" ]; then
# Use this line for the magnetometer in the Holybro GPS module
/bin/echo "Starting Holybro magnetometer"
qshell ist8310 start -R 10 -X -b 1
fi
# Auto detect the magnetometer. If one or both of these devices
# are not connected it will fail but not cause any harm.
/bin/echo "Looking for qmc5883l magnetometer"
qshell qmc5883l start -R 10 -X -b 1
/bin/echo "Looking for ist8310 magnetometer"
qshell ist8310 start -R 10 -X -b 1
# GPS
if [ "$GPS" == "MATEK" ]; then
# Use this gps start line instead for Matek Systems M8Q-5883 module
/bin/echo "Starting Mateksys M8Q-5883 GPS"
# GPS and magnetometer
if [ "$GPS" != "NONE" ]; then
# On M0052 the GPS driver runs on the apps processor
if [ $PLATFORM = "M0052" ]; then
gps start -d /dev/ttyHS2
# On M0054 and M0104 the GPS driver runs on SLPI DSP
else
qshell gps start
elif [ "$GPS" == "HOLYBRO" ]; then
# Only the newer Holybro unit is supported on M0054
/bin/echo "Starting Holybro GPS"
qshell gps start -b 115200
fi
fi
# LED driver for the Pixhawk 4 GPS module
if [ "$GPS" == "HOLYBRO" ]; then
/bin/echo "Starting Holybro LED driver"
qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56
fi
# Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will
# fail but not cause any harm.
/bin/echo "Looking for ncp5623c RGB LED"
qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56
# We do not change the value of SYS_AUTOCONFIG but if it does not
# show up as used then it is not reported to QGC and we get a
@ -91,10 +110,23 @@ fi
param touch SYS_AUTOCONFIG
# ESC driver
qshell modal_io start
if [ "$ESC" == "VOXL_ESC" ]; then
/bin/echo "Starting VOXL ESC driver"
qshell voxl_esc start
elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then
if [ "$RC" == "M0065_SBUS" ]; then
/bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
qshell voxl2_io start
else
/bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
qshell voxl2_io start -e
fi
else
/bin/echo "No ESC type specified, not starting an ESC driver"
fi
/bin/sleep 1
# RC driver
if [ "$RC" == "FAKE_RC_INPUT" ]; then
/bin/echo "Starting fake RC driver"
qshell rc_controller start
@ -106,13 +138,46 @@ elif [ "$RC" == "CRSF_MAV" ]; then
qshell mavlink_rc_in start -m -p 7 -b 115200
elif [ "$RC" == "SPEKTRUM" ]; then
/bin/echo "Starting Spektrum RC"
# On M0052 the RC driver runs on the apps processor
if [ $PLATFORM = "M0052" ]; then
rc_input start -d /dev/ttyHS1
# On M0054 and M0104 the RC driver runs on SLPI DSP
else
qshell spektrum_rc start
fi
elif [ "$RC" == "GHST" ]; then
/bin/echo "Starting GHST RC driver"
qshell ghst_rc start -d 7
elif [ "$RC" == "M0065_SBUS" ]; then
if [ $PLATFORM = "M0052" ]; then
apps_sbus start
elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
qshell dsp_sbus start
retVal=$?
if [ $retVal -ne 0 ]; then
/bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
/bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
qshell voxl2_io start -d -p 7
fi
else
/bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
fi
fi
/bin/sleep 1
if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then
# Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
qshell lightware_laser_serial start -d 7
fi
# APM power monitor
qshell voxlpm start -X -b 2
if [ "$POWER_MANAGER" == "VOXLPM" ]; then
# APM power monitor
qshell voxlpm start -X -b 2
fi
# Optional distance sensor on spare i2c
# qshell vl53l0x start -X -b 4
# qshell vl53l1x start -X -b 4
# Start all of the processing modules on DSP
qshell sensors start
@ -121,32 +186,53 @@ qshell mc_pos_control start
qshell mc_att_control start
qshell mc_rate_control start
qshell mc_hover_thrust_estimator start
qshell mc_autotune_attitude_control start
qshell land_detector start multicopter
qshell manual_control start
qshell control_allocator start
qshell rc_update start
qshell commander start
qshell commander mode manual
qshell load_mon start
/bin/sleep 1
# Only start the rc_update module if an actual RC driver
# is publishing input_rc topics. Otherwise for external RC
# over Mavlink this isn't needed.
if [ "$RC" != "EXTERNAL" ]; then
qshell rc_update start
fi
qshell commander start
# This is needed for altitude and position hold modes
qshell flight_mode_manager start
# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
# This is needed for altitude and position hold modes
flight_mode_manager start
# This bridge allows raw data packets to be sent over UART to the ESC
voxl2_io_bridge start
# Start microdds_client for ros2 offboard messages from agent over localhost
microdds_client start -t udp -h 127.0.0.1 -p 8888
# On M0052 there is only one IMU. So, PX4 needs to
# publish IMU samples externally for VIO to use.
if [ $PLATFORM = "M0052" ]; then
imu_server start
fi
# start the onboard fast link to connect to voxl-mavlink-server
mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard
# slow down some of the fastest streams in onboard mode
# slow down some of the fastest streams
mavlink stream -u 14556 -s HIGHRES_IMU -r 10
mavlink stream -u 14556 -s ATTITUDE -r 10
mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10
# speed up rc_channels
mavlink stream -u 14556 -s RC_CHANNELS -r 50
mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30
mavlink stream -u 14556 -s SCALED_PRESSURE -r 10
/bin/sleep 1
# start the slow normal mode for voxl-mavlink-server to forward to GCS
mavlink start -x -u 14558 -o 14559 -r 100000 -n lo
mavlink boot_complete

9
msg/Buffer128.msg Normal file
View File

@ -0,0 +1,9 @@
uint64 timestamp # time since system start (microseconds)
uint8 len # length of data
uint32 MAX_BUFLEN = 128
uint8[128] data # data
# TOPICS voxl2_io_data

1
msg/CMakeLists.txt
View File

@ -53,6 +53,7 @@ set(msg_files
ArmingCheckRequest.msg
AutotuneAttitudeControlStatus.msg
BatteryStatus.msg
Buffer128.msg
ButtonEvent.msg
CameraCapture.msg
CameraStatus.msg

12
platforms/posix/cmake/Toolchain-aarch64-linux-gnu.cmake
View File

@ -2,16 +2,8 @@
set(triple aarch64-linux-gnu)
if("${PX4_BOARD}" MATCHES "modalai_voxl2")
set(CMAKE_LIBRARY_ARCHITECTURE ${ARM_CROSS_GCC_ROOT}/bin/${triple})
set(TOOLCHAIN_PREFIX ${ARM_CROSS_GCC_ROOT}/bin/${triple})
set(ARM_CROSS_GCC_ROOT $ENV{ARM_CROSS_GCC_ROOT})
set(HEXAGON_ARM_SYSROOT $ENV{HEXAGON_ARM_SYSROOT})
set(CMAKE_EXE_LINKER_FLAGS "-Wl,-gc-sections -Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib/aarch64-linux-gnu -Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib/aarch64-linux-gnu")
else()
set(CMAKE_LIBRARY_ARCHITECTURE ${triple})
set(TOOLCHAIN_PREFIX ${triple})
endif()
set(CMAKE_LIBRARY_ARCHITECTURE ${triple})
set(TOOLCHAIN_PREFIX ${triple})
set(CMAKE_SYSTEM_NAME Linux)
set(CMAKE_SYSTEM_PROCESSOR arm)

3
platforms/qurt/cmake/qurt_reqs.cmake
View File

@ -121,7 +121,8 @@ exec_program(${CMAKE_CXX_COMPILER} ${CMAKE_CURRENT_SOURCE_DIR} ARGS -print-file-
set(EXTRA_LIBS ${EXTRA_LIBS} ${LIBM})
# Full optimization and Link Time Optimization (LTO)
set(QURT_OPTIMIZATION_FLAGS -O3 -flto)
# set(QURT_OPTIMIZATION_FLAGS -O3 -flto)
set(QURT_OPTIMIZATION_FLAGS -O3) # LTO takes a lot of extra time and doesn't really help much...
# Flags we pass to the C compiler
list2string(CFLAGS

37
platforms/qurt/src/px4/common/include/px4_platform/cpuload.h Normal file
View File

@ -0,0 +1,37 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
extern "C" float px4muorb_get_cpu_load(void);

8
platforms/qurt/src/px4/drv_hrt.cpp
View File

@ -116,16 +116,10 @@ hrt_abstime hrt_absolute_time()
int hrt_set_absolute_time_offset(int32_t time_diff_us)
{
dsp_offset = time_diff_us;
// dsp_offset = time_diff_us;
return 0;
}
hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
{
hrt_abstime delta = hrt_absolute_time() - *then;
return delta;
}
void hrt_store_absolute_time(volatile hrt_abstime *t)
{
*t = hrt_absolute_time();

8
platforms/qurt/src/px4/tasks.cpp
View File

@ -161,6 +161,9 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma
return -1;
}
char *charPointer = const_cast<char *>(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;
}
}

5
src/drivers/actuators/modal_io/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig DRIVERS_ACTUATORS_MODAL_IO
bool "modal_io"
default n
---help---
Enable support for modal_io

26
src/drivers/actuators/modal_io/module.yaml
View File

@ -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

13
src/drivers/actuators/modal_io/CMakeLists.txt → src/drivers/actuators/voxl_esc/CMakeLists.txt
View File

@ -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

5
src/drivers/actuators/voxl_esc/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_ACTUATORS_VOXL_ESC
bool "voxl_esc"
default n
---help---
Enable support for voxl_esc

0
src/drivers/actuators/modal_io/crc16.c → src/drivers/actuators/voxl_esc/crc16.c
View File

0
src/drivers/actuators/modal_io/crc16.h → src/drivers/actuators/voxl_esc/crc16.h
View File

41
src/drivers/actuators/voxl_esc/module.yaml Normal file
View File

@ -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

52
src/drivers/actuators/modal_io/qc_esc_packet.c → src/drivers/actuators/voxl_esc/qc_esc_packet.c
View File

@ -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 |= (led1 & 0b00000111) << 3;
leds |= ((uint16_t)(led2 & 0b00000111)) << 6;
leds |= ((uint16_t)(led3 & 0b00000111)) << 9;
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;
return qc_esc_create_packet(ESC_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
}
//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)

27
src/drivers/actuators/modal_io/qc_esc_packet.h → src/drivers/actuators/voxl_esc/qc_esc_packet.h
View File

@ -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);
//-------------------------------------------------------------------------

2
src/drivers/actuators/modal_io/qc_esc_packet_types.h → src/drivers/actuators/voxl_esc/qc_esc_packet_types.h
View File

@ -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

465
src/drivers/actuators/modal_io/modal_io.cpp → src/drivers/actuators/voxl_esc/voxl_esc.cpp
View File

@ -35,33 +35,32 @@
#include <px4_platform_common/getopt.h>
#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"), &params->config);
param_get(param_find("MODAL_IO_MODE"), &params->mode);
param_get(param_find("MODAL_IO_BAUD"), &params->baud_rate);
param_get(param_find("VOXL_ESC_CONFIG"), &params->config);
param_get(param_find("VOXL_ESC_MODE"), &params->mode);
param_get(param_find("VOXL_ESC_BAUD"), &params->baud_rate);
param_get(param_find("MODAL_IO_T_PERC"), &params->turtle_motor_percent);
param_get(param_find("MODAL_IO_T_DEAD"), &params->turtle_motor_deadband);
param_get(param_find("MODAL_IO_T_EXPO"), &params->turtle_motor_expo);
param_get(param_find("MODAL_IO_T_MINF"), &params->turtle_stick_minf);
param_get(param_find("MODAL_IO_T_COSP"), &params->turtle_cosphi);
param_get(param_find("VOXL_ESC_T_PERC"), &params->turtle_motor_percent);
param_get(param_find("VOXL_ESC_T_DEAD"), &params->turtle_motor_deadband);
param_get(param_find("VOXL_ESC_T_EXPO"), &params->turtle_motor_expo);
param_get(param_find("VOXL_ESC_T_MINF"), &params->turtle_stick_minf);
param_get(param_find("VOXL_ESC_T_COSP"), &params->turtle_cosphi);
param_get(param_find("MODAL_IO_FUNC1"), &params->function_map[0]);
param_get(param_find("MODAL_IO_FUNC2"), &params->function_map[1]);
param_get(param_find("MODAL_IO_FUNC3"), &params->function_map[2]);
param_get(param_find("MODAL_IO_FUNC4"), &params->function_map[3]);
param_get(param_find("VOXL_ESC_FUNC1"), &params->function_map[0]);
param_get(param_find("VOXL_ESC_FUNC2"), &params->function_map[1]);
param_get(param_find("VOXL_ESC_FUNC3"), &params->function_map[2]);
param_get(param_find("VOXL_ESC_FUNC4"), &params->function_map[3]);
param_get(param_find("MODAL_IO_SDIR1"), &params->direction_map[0]);
param_get(param_find("MODAL_IO_SDIR2"), &params->direction_map[1]);
param_get(param_find("MODAL_IO_SDIR3"), &params->direction_map[2]);
param_get(param_find("MODAL_IO_SDIR4"), &params->direction_map[3]);
param_get(param_find("VOXL_ESC_SDIR1"), &params->direction_map[0]);
param_get(param_find("VOXL_ESC_SDIR2"), &params->direction_map[1]);
param_get(param_find("VOXL_ESC_SDIR3"), &params->direction_map[2]);
param_get(param_find("VOXL_ESC_SDIR4"), &params->direction_map[3]);
param_get(param_find("MODAL_IO_RPM_MIN"), &params->rpm_min);
param_get(param_find("MODAL_IO_RPM_MAX"), &params->rpm_max);
param_get(param_find("VOXL_ESC_RPM_MIN"), &params->rpm_min);
param_get(param_find("VOXL_ESC_RPM_MAX"), &params->rpm_max);
param_get(param_find("MODAL_IO_VLOG"), &params->verbose_logging);
param_get(param_find("VOXL_ESC_VLOG"), &params->verbose_logging);
param_get(param_find("VOXL_ESC_PUB_BST"), &params->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);
}

101
src/drivers/actuators/modal_io/modal_io.hpp → src/drivers/actuators/voxl_esc/voxl_esc.hpp
View File

@ -41,23 +41,26 @@
#include <px4_log.h>
#include <px4_platform_common/module.h>
#include <battery/battery.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/buffer128.h>
#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<ModalIo>, public OutputModuleInterface
class VoxlEsc : public ModuleBase<VoxlEsc>, 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<actuator_outputs_s> _outputs_debug_pub{ORB_ID(actuator_outputs_debug)};
uORB::Publication<esc_status_s> _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<Command *> _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);

84
src/drivers/actuators/modal_io/modal_io_params.c → src/drivers/actuators/voxl_esc/voxl_esc_params.c
View File

@ -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);

16
src/drivers/actuators/modal_io/modal_io_serial.cpp → src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp
View File

@ -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");

6
src/drivers/actuators/modal_io/modal_io_serial.hpp → src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp
View File

@ -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);

8
src/drivers/power_monitor/voxlpm/voxlpm.cpp
View File

@ -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));

52
src/drivers/voxl2_io/CMakeLists.txt Normal file
View File

@ -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
)

5
src/drivers/voxl2_io/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_VOXL2_IO
bool "voxl2_io"
default n
---help---
Enable support for voxl2_io

13
src/drivers/voxl2_io/module.yaml Normal file
View File

@ -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

1122
src/drivers/voxl2_io/voxl2_io.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

228
src/drivers/voxl2_io/voxl2_io.hpp Normal file
View File

@ -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 <float.h>
#include <math.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <lib/rc/sbus.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <uORB/Subscription.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/input_rc.h>
#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<Voxl2IO>, 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<input_rc_s> _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<Command *> _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();
};

95
src/drivers/voxl2_io/voxl2_io_crc16.c Normal file
View File

@ -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 <stdint.h>
#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;
}

65
src/drivers/voxl2_io/voxl2_io_crc16.h Normal file
View File

@ -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 <stdint.h>
// 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_

253
src/drivers/voxl2_io/voxl2_io_packet.c Normal file
View File

@ -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 <string.h>
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;
}

287
src/drivers/voxl2_io/voxl2_io_packet.h Normal file
View File

@ -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 <stdint.h>
#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

75
src/drivers/voxl2_io/voxl2_io_packet_types.h Normal file
View File

@ -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

66
src/drivers/voxl2_io/voxl2_io_params.c Normal file
View File

@ -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);

191
src/drivers/voxl2_io/voxl2_io_serial.cpp Normal file
View File

@ -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
}

58
src/modules/muorb/apps/fc_sensor.h → src/drivers/voxl2_io/voxl2_io_serial.hpp
View File

@ -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 <px4_log.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#ifdef __PX4_QURT
#include <drivers/device/qurt/uart.h>
#define FAR
#endif
#include <stdint.h>
#include <stdbool.h>
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;
};

1
src/modules/muorb/apps/CMakeLists.txt
View File

@ -39,6 +39,7 @@ px4_add_module(
INCLUDES
../test
../aggregator
libfc-sensor-api/inc
SRCS
uORBAppsProtobufChannel.cpp
muorb_main.cpp

1
src/modules/muorb/apps/libfc-sensor-api Submodule

@ -0,0 +1 @@
Subproject commit ca16e99074641a10d153961291243ede7720c2e2

5
src/modules/muorb/apps/uORBAppsProtobufChannel.cpp
View File

@ -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]++;

38
src/modules/muorb/slpi/uORBProtobufChannel.cpp
View File

@ -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;
}

8
src/modules/muorb/slpi/uORBProtobufChannel.hpp
View File

@ -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_