diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 new file mode 100644 index 0000000000..968f1f6a76 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -0,0 +1,81 @@ +#!/bin/sh +# +# @name Crazyflie 2.1 +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Dennis Shtatov +# +# @board px4_fmu-v2 exclude +# @board px4_fmu-v3 exclude +# @board px4_fmu-v4 exclude +# @board px4_fmu-v4pro exclude +# @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude +# +. ${R}etc/init.d/rc.mc_defaults + +set MIXER quad_x_cw +set PWM_OUT 1234 +if [ $AUTOCNF = yes ] +then + param set SYS_MC_EST_GROUP 3 + param set SYS_HAS_MAG 0 + + param set BAT_N_CELLS 1 + param set BAT1_N_CELLS 1 + param set BAT1_SOURCE 1 + + param set CBRK_SUPPLY_CHK 894281 + param set CBRK_USB_CHK 197848 + param set COM_RC_IN_MODE 1 + + param set IMU_GYRO_CUTOFF 100 + param set IMU_ACCEL_CUTOFF 30 + + param set MC_AIRMODE 1 + param set IMU_DGYRO_CUTOFF 70 + param set MC_PITCHRATE_D 0.002 + param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_P 0.07 + param set MC_PITCH_P 6.5 + param set MC_ROLLRATE_D 0.002 + param set MC_ROLLRATE_I 0.2 + param set MC_ROLLRATE_P 0.07 + param set MC_ROLL_P 6.5 + param set MC_YAW_P 3 + + param set MPC_THR_HOVER 0.7 + param set MPC_THR_MAX 1 + param set MPC_Z_P 1.5 + param set MPC_Z_VEL_P_ACC 8 + param set MPC_Z_VEL_I_ACC 6 + param set MPC_HOLD_MAX_XY 0.1 + param set MPC_MAX_FLOW_HGT 3 + #param set IMU_GYRO_NF_FREQ 10 + #param set IMU_GYRO_NF_BW 20 + + param set NAV_RCL_ACT 3 + + param set PWM_DISARMED 0 + param set PWM_MIN 0 + param set PWM_MAX 255 + + # Run the motors at 328.125 kHz (recommended) + param set PWM_RATE 3921 + + param set SDLOG_PROFILE 1 + + param set SENS_FLOW_MINRNG 0.05 + + +fi + +set PWM_DISARMED none +set PWM_MAX none +set PWM_MIN none + +syslink start +mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index ee7765e993..9debc7d5c4 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -91,6 +91,7 @@ px4_add_romfs_files( 4250_teal 4500_clover4 4900_crazyflie + 4901_crazyflie21 # [5000, 5999] Quadrotor +" 5001_quad_+ diff --git a/boards/bitcraze/crazyflie21/CMakeLists.txt b/boards/bitcraze/crazyflie21/CMakeLists.txt new file mode 100644 index 0000000000..4fda6f229e --- /dev/null +++ b/boards/bitcraze/crazyflie21/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(syslink) diff --git a/boards/bitcraze/crazyflie21/default.cmake b/boards/bitcraze/crazyflie21/default.cmake new file mode 100644 index 0000000000..600c9dad8b --- /dev/null +++ b/boards/bitcraze/crazyflie21/default.cmake @@ -0,0 +1,64 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR bitcraze + MODEL crazyflie 2.1 + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + CONSTRAINED_FLASH + DRIVERS + distance_sensor/vl53l1x + gps + optical_flow/pmw3901 + pwm_out + imu/bosch/bmi088/bmi088_i2c + barometer/bmp388 + MODULES + attitude_estimator_q + #camera_feedback + commander + dataman + ekf2 + events + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + navigator + rc_update + sensors + #temperature_compensation + SYSTEMCMDS + bl_update + dmesg + dumpfile + esc_calib + hardfault_log + i2cdetect + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + system_time + top + topic_listener + tune_control + usb_connected + ver + work_queue + ) diff --git a/boards/bitcraze/crazyflie21/firmware.prototype b/boards/bitcraze/crazyflie21/firmware.prototype new file mode 100644 index 0000000000..f1121a6269 --- /dev/null +++ b/boards/bitcraze/crazyflie21/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 14, + "magic": "Crazyflie21", + "description": "Firmware for the Crazyflie 2.1", + "image": "", + "build_time": 0, + "summary": "CRAZYFLIE 2.1", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1032192, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/bitcraze/crazyflie21/init/rc.board_defaults b/boards/bitcraze/crazyflie21/init/rc.board_defaults new file mode 100644 index 0000000000..38e6641756 --- /dev/null +++ b/boards/bitcraze/crazyflie21/init/rc.board_defaults @@ -0,0 +1,19 @@ +#!/bin/sh +# +# Bitcraze Crazyflie specific board sensors init +#------------------------------------------------------------------------------ + +# system_power unavailable +param set CBRK_SUPPLY_CHK 894281 + +if param compare SYS_AUTOSTART 0 +then + param set SYS_AUTOSTART 4901 + set AUTOCNF yes +fi + +if [ $AUTOCNF = yes ] +then + # don't probe external I2C + param set SENS_EXT_I2C_PRB 0 +fi diff --git a/boards/bitcraze/crazyflie21/init/rc.board_mavlink b/boards/bitcraze/crazyflie21/init/rc.board_mavlink new file mode 100644 index 0000000000..ae5089a30f --- /dev/null +++ b/boards/bitcraze/crazyflie21/init/rc.board_mavlink @@ -0,0 +1,7 @@ +#!/bin/sh +# +# Bitcraze Crazyflie specific board MAVLink startup script. +#------------------------------------------------------------------------------ + +# Start MAVLink on the USB port +mavlink start -d /dev/ttyACM0 diff --git a/boards/bitcraze/crazyflie21/init/rc.board_sensors b/boards/bitcraze/crazyflie21/init/rc.board_sensors new file mode 100644 index 0000000000..75a6c1b10a --- /dev/null +++ b/boards/bitcraze/crazyflie21/init/rc.board_sensors @@ -0,0 +1,14 @@ +#!/bin/sh +# +# Bitcraze Crazyflie specific board sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# Internal I2C bus +bmp388 -I -a 0x77 start +bmi088_i2c -A -R 0 -I -a 0x18 start +bmi088_i2c -G -R 0 -I -a 0x69 start + +# Optical flow deck +vl53l1x start -X diff --git a/boards/bitcraze/crazyflie21/nuttx-config/include/board.h b/boards/bitcraze/crazyflie21/nuttx-config/include/board.h new file mode 100644 index 0000000000..d3c2f95256 --- /dev/null +++ b/boards/bitcraze/crazyflie21/nuttx-config/include/board.h @@ -0,0 +1,223 @@ +/************************************************************************************ + * configs/crazyflie/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __CONFIG_CRAZYFLIE_INCLUDE_BOARD_H +#define __CONFIG_CRAZYFLIE_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The Crazyflie 2.0 uses a 8MHz crystal connected to the HSE. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 8MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8-11 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_TIM8_FREQUENCY STM32_APB2_TIM8_CLKIN +#define BOARD_TIM9_FREQUENCY STM32_APB2_TIM9_CLKIN +#define BOARD_TIM10_FREQUENCY STM32_APB2_TIM10_CLKIN +#define BOARD_TIM11_FREQUENCY STM32_APB2_TIM11_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 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + * + */ +/* E_TX2 / E_RX2 */ +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* E_TX1 / E_RX1 */ +#define GPIO_USART3_RX GPIO_USART3_RX_2 +#define GPIO_USART3_TX GPIO_USART3_TX_2 + +/* NRF51 via syslink */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* UART DMA configuration for USART6 */ +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN7) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) + +/* + * SPI + * + * E_MISO, E_MOSI, E_SCK exposed on headers + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +/* XXX since we allocate the HP work stack from CCM RAM on normal system startup, + SPI1 will never run in DMA mode - so we can just give it a random config here. + What we really need to do is to make DMA configurable per channel, and always + disable it for SPI1. */ +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 + +#endif /* __CONFIG_CRAZYFLIE_INCLUDE_BOARD_H */ diff --git a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..43e3346bba --- /dev/null +++ b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig @@ -0,0 +1,190 @@ +# +# 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_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_SPI_CALLBACK is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../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_STM32F405RG=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0016 +CONFIG_CDCACM_PRODUCTSTR="PX4 Crazyflie v2.0" +CONFIG_CDCACM_RXBUFSIZE=300 +CONFIG_CDCACM_TXBUFSIZE=1000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="Bitcraze AB" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_FAT_DIRECT_RETRY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_EXCLUDE_BLOCKS=y +CONFIG_FS_PROCFS_EXCLUDE_MEMINFO=y +CONFIG_FS_PROCFS_EXCLUDE_MOUNT=y +CONFIG_FS_PROCFS_EXCLUDE_USAGE=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_AT24XX=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_NFILE_DESCRIPTORS=12 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_EXPORT=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MB=y +CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_FATDEVNO=0 +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=1 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=196608 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C3=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=300 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=300 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=128 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=64 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/bitcraze/crazyflie21/nuttx-config/scripts/script.ld b/boards/bitcraze/crazyflie21/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..83d5c12a93 --- /dev/null +++ b/boards/bitcraze/crazyflie21/nuttx-config/scripts/script.ld @@ -0,0 +1,152 @@ +/**************************************************************************** + * configs/crazyflie/scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000: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. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +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) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.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 + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/bitcraze/crazyflie21/src/CMakeLists.txt b/boards/bitcraze/crazyflie21/src/CMakeLists.txt new file mode 100644 index 0000000000..d967b47016 --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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 + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c +) +target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + px4_layer +) diff --git a/boards/bitcraze/crazyflie21/src/board_config.h b/boards/bitcraze/crazyflie21/src/board_config.h new file mode 100644 index 0000000000..0e6da91b13 --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/board_config.h @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * PX4-CRAZYFLIE internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* Crazyflie GPIOs **********************************************************************************/ +/* LEDs */ + + +/* Radio TX indicator */ +#define GPIO_LED_RED_L (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN0) +/* Radio RX indicator */ +#define GPIO_LED_GREEN_L (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_GREEN_R (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN2) +#define GPIO_LED_RED_R (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) + +/* PX4: armed state indicator ; Stock FW: Blinking while charging */ +#define GPIO_LED_BLUE_L (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN2) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN + +#define LED_TX 4 +#define LED_RX 5 + +#define GPIO_FSYNC_MPU9250 (GPIO_OUTPUT|GPIO_PORTC|GPIO_PIN14) // Needs to be set low +#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN13) + + +#define GPIO_NRF_TXEN (GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTA|GPIO_PIN4) + +#define GPIO_I2C4_DRDY1_BMP388 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5) + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ONBOARD_HZ 400000 +#define PX4_I2C_BUS_EXPANSION_HZ 400000 + +#define PX4_I2C_BUS_MTD 1 + +#define BOARD_NUMBER_I2C_BUSES 3 +#define BOARD_I2C_BUS_CLOCK_INIT {PX4_I2C_BUS_ONBOARD_HZ, 100000, PX4_I2C_BUS_EXPANSION_HZ} + + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_MPU9250 0x69 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS 0 + +/* Tone alarm output : These are only applicable when the buzzer deck is attached */ +#define TONE_ALARM_TIMER 5 /* timer 5 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN2) +#define GPIO_TONE_ALARM_NEG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) + +/* PWM +*/ + +#define DIRECT_PWM_OUTPUT_CHANNELS 4 + +/* This board overrides the defaults by providing + * PX4_PWM_ALTERNATE_RANGES and a replacement set of + * constants + */ + + +/* PWM directly wired to transistor. Duty cycle directly corresponds to power + * So we need to override the defaults + */ + +#define PX4_PWM_ALTERNATE_RANGES +#define PWM_LOWEST_MIN 0 +#define PWM_MOTOR_OFF 0 +#define PWM_DEFAULT_MIN 20 +#define PWM_HIGHEST_MIN 0 +#define PWM_HIGHEST_MAX 255 +#define PWM_DEFAULT_MAX 255 +#define PWM_LOWEST_MAX 255 +#define PWM_DEFAULT_TRIM 1500 + + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_CONSOLE_BUFFER_SIZE (1024*3) + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: board_peripheral_reset + * + * Description: + * Called to reset the periferal bus + * + ****************************************************************************************************/ + +#define board_peripheral_reset(ms) + +/**************************************************************************************************** + * Name: stm32_usbinitialize + * + * Description: + * Called to configure USB IO. + * + ****************************************************************************************************/ + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: board_i2c_initialize + * + * Description: + * Called to set I2C bus frequencies. + * + ****************************************************************************/ + +int board_i2c_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +/************************************************************************************ + * Name: stm32_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +extern int stm32_spi_bus_initialize(void); + + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/bitcraze/crazyflie21/src/i2c.cpp b/boards/bitcraze/crazyflie21/src/i2c.cpp new file mode 100644 index 0000000000..bb73d27b0a --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(3), +}; diff --git a/boards/bitcraze/crazyflie21/src/init.c b/boards/bitcraze/crazyflie21/src/init.c new file mode 100644 index 0000000000..97e05c090e --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/init.c @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * 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 crazyflie_init.c + * + * Crazyflie specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * 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 LEDs */ + + board_autoled_initialize(); + + /* configure SPI interfaces */ + + stm32_spiinitialize(); + + stm32_usbinitialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + led_off(LED_TX); + led_off(LED_RX); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_SPI + int ret = stm32_spi_bus_initialize(); + + if (ret != OK) { + return ret; + } + +#endif + + return OK; +} diff --git a/boards/bitcraze/crazyflie21/src/led.c b/boards/bitcraze/crazyflie21/src/led.c new file mode 100644 index 0000000000..b2ea621131 --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/led.c @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file crazyflie_led.c + * + * Crazyflie LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE_L, // Indexed by LED_BLUE + GPIO_LED_RED_R, // Indexed by LED_RED, LED_AMBER + 0, // Indexed by LED_SAFETY + GPIO_LED_GREEN_R, // Indexed by LED_GREEN + GPIO_LED_RED_L, // Indexed by LED_TX + GPIO_LED_GREEN_L // Indexed by LED_RX +}; + + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l]) { + px4_arch_configgpio(g_ledmap[l]); + } + } +} + +__EXPORT void led_on(int led) +{ + /* Pull down to switch on */ + px4_arch_gpiowrite(g_ledmap[led], g_ledmap[led] & GPIO_OUTPUT_SET ? true : false); +} + +__EXPORT void led_off(int led) +{ + /* Pull up to switch off */ + px4_arch_gpiowrite(g_ledmap[led], g_ledmap[led] & GPIO_OUTPUT_SET ? false : true); +} + +__EXPORT void led_toggle(int led) +{ + if (px4_arch_gpioread(g_ledmap[led])) { + px4_arch_gpiowrite(g_ledmap[led], false); + + } else { + px4_arch_gpiowrite(g_ledmap[led], true); + } +} diff --git a/boards/bitcraze/crazyflie21/src/spi.cpp b/boards/bitcraze/crazyflie21/src/spi.cpp new file mode 100644 index 0000000000..8cee5d6ab3 --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/spi.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include "board_config.h" + + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_FLOW_DEVTYPE_PMW3901, SPI::CS{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortC, GPIO::Pin12}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin5}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +/************************************************************************************ + * Name: stm32_spi_bus_initialize + * + * Description: + * Called to configure SPI buses on PX4FMU board. + * + ************************************************************************************/ +static struct spi_dev_s *spi_expansion; + +__EXPORT int stm32_spi_bus_initialize(void) +{ + /* Configure SPI-based devices */ + + /* Get the external SPI port */ + spi_expansion = stm32_spibus_initialize(1); + + if (!spi_expansion) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); + return -ENODEV; + } + +#ifdef CONFIG_MMCSD + int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_expansion); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); + return -ENODEV; + } + +#endif + + return OK; +} diff --git a/boards/bitcraze/crazyflie21/src/timer_config.cpp b/boards/bitcraze/crazyflie21/src/timer_config.cpp new file mode 100644 index 0000000000..33532b0530 --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/timer_config.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +/* IO Timers normally free-run at 1MHz + * Here we make adjustments to the Frequency that sets the timer's prescale + * so that the prescale is set to 0 + */ +#define TIMx_CLKIN 1000000 + +static inline constexpr io_timers_t initIOTimerOverrideClockFreq(Timer::Timer timer) +{ + io_timers_t ret = initIOTimer(timer); + ret.clock_freq = TIMx_CLKIN; + return ret; +} + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimerOverrideClockFreq(Timer::Timer2), + initIOTimerOverrideClockFreq(Timer::Timer4), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortB, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/bitcraze/crazyflie21/src/usb.c b/boards/bitcraze/crazyflie21/src/usb.c new file mode 100644 index 0000000000..af187d8df6 --- /dev/null +++ b/boards/bitcraze/crazyflie21/src/usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 crazyflie_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4-STM32F4Discovery board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} + diff --git a/boards/bitcraze/crazyflie21/syslink/CMakeLists.txt b/boards/bitcraze/crazyflie21/syslink/CMakeLists.txt new file mode 100644 index 0000000000..ebf6c04c08 --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__syslink + MAIN syslink + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + ringbuffer.cpp + syslink_main.cpp + syslink_bridge.cpp + syslink_memory.cpp + syslink.c + DEPENDS + battery + ) diff --git a/boards/bitcraze/crazyflie21/syslink/crtp.h b/boards/bitcraze/crazyflie21/syslink/crtp.h new file mode 100644 index 0000000000..a874659a25 --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/crtp.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + + +#include + +#define CRTP_PORT_CONSOLE 0x00 +#define CRTP_PORT_PARAM 0x02 +#define CRTP_PORT_COMMANDER 0x03 +#define CRTP_PORT_MEM 0x04 +#define CRTP_PORT_LOG 0x05 + +#define CRTP_PORT_MAVLINK 0x08 // Non-standard port for transmitting mavlink messages + +#define CRTP_PORT_PLATFORM 0x0D +#define CRTP_PORT_DEBUG 0x0E +#define CRTP_PORT_LINK 0x0F + +#define CRTP_NULL(x) (((x).header & 0xf3) == 0xf3) + +// 1 byte header + 31 bytes data = 32 (max ESB packet size) +// With the NRF51, this could be increased to ~250, but the Crazyradio PA uses a NRF24 which can't do this +#define CRTP_MAX_DATA_SIZE 31 + +typedef struct { + uint8_t size; // Total size of this message, including the header (placed here to overlap with syslink length field) + union { + uint8_t header; + struct { + uint8_t channel : 2; + uint8_t link : 2; + uint8_t port : 4; + }; + }; + + uint8_t data[CRTP_MAX_DATA_SIZE]; +} __attribute__((packed)) crtp_message_t; + + +typedef struct { + float roll; // -20 to 20 + float pitch; + float yaw; // -150 to 150 + uint16_t thrust; +} crtp_commander; diff --git a/boards/bitcraze/crazyflie21/syslink/drv_deck.h b/boards/bitcraze/crazyflie21/syslink/drv_deck.h new file mode 100644 index 0000000000..2b21ae5f3f --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/drv_deck.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Definitions for crazyflie related drivers */ + +#ifndef _DRV_CRAZYFLIE_H +#define _DRV_CRAZYFLIE_H + +#include +#include +#include + + +#define DECK_DEVICE_PATH "/dev/deck" + + + +/* structure of the data stored in deck memory */ +typedef struct { + uint8_t header; // Should be 0xEB + uint32_t pins; + uint8_t vendorId; + uint8_t productId; + uint8_t crc; + uint8_t data[104]; + +} __attribute__((packed)) deck_descriptor_t; + + + +/* + * ioctl() definitions + */ + +#define _DECKIOCBASE (0x4100) +#define _DECKIOC(_n) (_PX4_IOC(_DECKIOCBASE, _n)) + +/** get the number of connected deck memory devices */ +#define DECKIOGNUM _DECKIOC(0) + +/** set the index of the current deck memory being accessed */ +#define DECKIOSNUM _DECKIOC(1) + +#define DECKIOID _DECKIOC(2) + + + + +#endif diff --git a/boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp b/boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp new file mode 100644 index 0000000000..92c9fcf753 --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/ringbuffer.cpp @@ -0,0 +1,418 @@ +/**************************************************************************** + * + * Copyright (C) 2013-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. + * + ****************************************************************************/ + +/** + * @file ringbuffer.cpp + * + * A flexible ringbuffer class. + */ + +#include "ringbuffer.h" +#include + +namespace ringbuffer +{ + +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), + _item_size(item_size), + _buf(new char[(_num_items + 1) * item_size]), + _head(_num_items), + _tail(_num_items) +{} + +RingBuffer::~RingBuffer() +{ + delete[] _buf; +} + +unsigned +RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _num_items : (index - 1); +} + +bool +RingBuffer::empty() +{ + return _tail == _head; +} + +bool +RingBuffer::full() +{ + return _next(_head) == _tail; +} + +unsigned +RingBuffer::size() +{ + return (_buf != nullptr) ? _num_items : 0; +} + +void +RingBuffer::flush() +{ + while (!empty()) { + get(nullptr); + } +} + +bool +RingBuffer::put(const void *val, size_t val_size) +{ + unsigned next = _next(_head); + + if (next != _tail) { + if ((val_size == 0) || (val_size > _item_size)) { + val_size = _item_size; + } + + memcpy(&_buf[_head * _item_size], val, val_size); + _head = next; + return true; + + } else { + return false; + } +} + +bool +RingBuffer::put(int8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::force(const void *val, size_t val_size) +{ + bool overwrote = false; + + for (;;) { + if (put(val, val_size)) { + break; + } + + get(nullptr); + overwrote = true; + } + + return overwrote; +} + +bool +RingBuffer::force(int8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +// FIXME - clang crashes on this get() call +#ifdef __PX4_QURT +#define __PX4_SBCAP my_sync_bool_compare_and_swap +static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) +{ + if (*a == b) { + *a = c; + return true; + } + + return false; +} + +#else +#define __PX4_SBCAP __sync_bool_compare_and_swap +#endif +bool +RingBuffer::get(void *val, size_t val_size) +{ + if (_tail != _head) { + unsigned candidate; + unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) { + val_size = _item_size; + } + + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + if (val != nullptr) { + memcpy(val, &_buf[candidate * _item_size], val_size); + } + + /* if the tail pointer didn't change, we got our item */ + } while (!__PX4_SBCAP(&_tail, candidate, next)); + + return true; + + } else { + return false; + } +} + +bool +RingBuffer::get(int8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) +{ + return get(&val, sizeof(val)); +} + +unsigned +RingBuffer::space() +{ + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); +} + +unsigned +RingBuffer::count() +{ + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ + return _num_items - space(); +} + +bool +RingBuffer::resize(unsigned new_size) +{ + char *old_buffer; + char *new_buffer = new char [(new_size + 1) * _item_size]; + + if (new_buffer == nullptr) { + return false; + } + + old_buffer = _buf; + _buf = new_buffer; + _num_items = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; +} + +void +RingBuffer::print_info(const char *name) +{ + printf("%s %u/%lu (%u/%u @ %p)\n", + name, + _num_items, + (unsigned long)_num_items * _item_size, + _head, + _tail, + _buf); +} + +} // namespace ringbuffer diff --git a/boards/bitcraze/crazyflie21/syslink/ringbuffer.h b/boards/bitcraze/crazyflie21/syslink/ringbuffer.h new file mode 100644 index 0000000000..04bd9fa973 --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/ringbuffer.h @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (C) 2013-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. + * + ****************************************************************************/ + +/** + * @file ringbuffer.h + * + * A flexible ringbuffer class. + */ + +#pragma once + +#include +#include +#include + +namespace ringbuffer __EXPORT +{ + +class RingBuffer +{ +public: + RingBuffer(unsigned num_items, size_t item_size); + virtual ~RingBuffer(); + + /** + * Put an item into the buffer. + * + * @param val Item to put + * @return true if the item was put, false if the buffer is full + */ + bool put(const void *val, size_t val_size = 0); + + bool put(int8_t val); + bool put(uint8_t val); + bool put(int16_t val); + bool put(uint16_t val); + bool put(int32_t val); + bool put(uint32_t val); + bool put(int64_t val); + bool put(uint64_t val); + bool put(float val); + bool put(double val); + + /** + * Force an item into the buffer, discarding an older item if there is not space. + * + * @param val Item to put + * @return true if an item was discarded to make space + */ + bool force(const void *val, size_t val_size = 0); + + bool force(int8_t val); + bool force(uint8_t val); + bool force(int16_t val); + bool force(uint16_t val); + bool force(int32_t val); + bool force(uint32_t val); + bool force(int64_t val); + bool force(uint64_t val); + bool force(float val); + bool force(double val); + + /** + * Get an item from the buffer. + * + * @param val Item that was gotten + * @return true if an item was got, false if the buffer was empty. + */ + bool get(void *val, size_t val_size = 0); + + bool get(int8_t &val); + bool get(uint8_t &val); + bool get(int16_t &val); + bool get(uint16_t &val); + bool get(int32_t &val); + bool get(uint32_t &val); + bool get(int64_t &val); + bool get(uint64_t &val); + bool get(float &val); + bool get(double &val); + + /* + * Get the number of slots free in the buffer. + * + * @return The number of items that can be put into the buffer before + * it becomes full. + */ + unsigned space(void); + + /* + * Get the number of items in the buffer. + * + * @return The number of items that can be got from the buffer before + * it becomes empty. + */ + unsigned count(void); + + /* + * Returns true if the buffer is empty. + */ + bool empty(); + + /* + * Returns true if the buffer is full. + */ + bool full(); + + /* + * Returns the capacity of the buffer, or zero if the buffer could + * not be allocated. + */ + unsigned size(); + + /* + * Empties the buffer. + */ + void flush(); + + /* + * resize the buffer. This is unsafe to be called while + * a producer or consuming is running. Caller is responsible + * for any locking needed + * + * @param new_size new size for buffer + * @return true if the resize succeeds, false if + * not (allocation error) + */ + bool resize(unsigned new_size); + + /* + * printf() some info on the buffer + */ + void print_info(const char *name); + +private: + unsigned _num_items; + const size_t _item_size; + char *_buf; + volatile unsigned _head; /**< insertion point in _item_size units */ + volatile unsigned _tail; /**< removal point in _item_size units */ + + unsigned _next(unsigned index); + + /* we don't want this class to be copied */ + RingBuffer(const RingBuffer &); + RingBuffer operator=(const RingBuffer &); +}; + +} // namespace ringbuffer diff --git a/boards/bitcraze/crazyflie21/syslink/syslink.c b/boards/bitcraze/crazyflie21/syslink/syslink.c new file mode 100644 index 0000000000..d9e40d137f --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink.c @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file syslink.c + * + * Crazyflie Syslink protocol implementation + * + * @author Dennis Shtatnov + */ + + +#include +#include +#include +#include +#include +#include +#include + + +#include "syslink.h" + + +const char *syslink_magic = "\xbc\xcf"; + +void syslink_parse_init(syslink_parse_state *state) +{ + state->state = SYSLINK_STATE_START; + state->index = 0; +} + + +int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg) +{ + + switch (state->state) { + case SYSLINK_STATE_START: + if (c == syslink_magic[state->index]) { + state->index++; + + } else { + state->index = 0; + } + + if (syslink_magic[state->index] == '\x00') { + state->state = SYSLINK_STATE_TYPE; + } + + break; + + case SYSLINK_STATE_TYPE: + msg->type = c; + state->state = SYSLINK_STATE_LENGTH; + break; + + case SYSLINK_STATE_LENGTH: + msg->length = c; + + if (c > SYSLINK_MAX_DATA_LEN) { // Too long + state->state = SYSLINK_STATE_START; + + } else { + state->state = c > 0 ? SYSLINK_STATE_DATA : SYSLINK_STATE_CKSUM; + } + + state->index = 0; + break; + + case SYSLINK_STATE_DATA: + msg->data[state->index++] = c; + + if (state->index >= msg->length) { + state->state = SYSLINK_STATE_CKSUM; + state->index = 0; + syslink_compute_cksum(msg); + } + + break; + + case SYSLINK_STATE_CKSUM: + if (c != msg->cksum[state->index]) { + PX4_INFO("Bad checksum"); + state->state = SYSLINK_STATE_START; + state->index = 0; + break; + } + + state->index++; + + if (state->index >= (int)sizeof(msg->cksum)) { + state->state = SYSLINK_STATE_START; + state->index = 0; + return 1; + } + + + break; + } + + return 0; + +} + +/* + Computes Fletcher 8bit checksum per RFC1146 + A := A + D[i] + B := B + A +*/ +void syslink_compute_cksum(syslink_message_t *msg) +{ + uint8_t a = 0, b = 0; + uint8_t *Di = (uint8_t *)msg, *end = Di + (2 + msg->length) * sizeof(uint8_t); + + while (Di < end) { + a = a + *Di; + b = b + a; + ++Di; + } + + msg->cksum[0] = a; + msg->cksum[1] = b; +} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink.h b/boards/bitcraze/crazyflie21/syslink/syslink.h new file mode 100644 index 0000000000..d0ccb14c4b --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink.h @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + + +extern const char *syslink_magic; + +#define SYSLINK_GROUP 0xF0 + +#define SYSLINK_RADIO 0x00 +#define SYSLINK_RADIO_RAW 0x00 +#define SYSLINK_RADIO_CHANNEL 0x01 +#define SYSLINK_RADIO_DATARATE 0x02 +#define SYSLINK_RADIO_CONTWAVE 0x03 +#define SYSLINK_RADIO_RSSI 0x04 +#define SYSLINK_RADIO_ADDRESS 0x05 + +#define SYSLINK_PM 0x10 +#define SYSLINK_PM_SOURCE 0x10 +#define SYSLINK_PM_ONOFF_SWITCHOFF 0x11 +#define SYSLINK_PM_BATTERY_VOLTAGE 0x12 +#define SYSLINK_PM_BATTERY_STATE 0x13 +#define SYSLINK_PM_BATTERY_AUTOUPDATE 0x14 + +#define SYSLINK_OW 0x20 +#define SYSLINK_OW_SCAN 0x20 +#define SYSLINK_OW_GETINFO 0x21 +#define SYSLINK_OW_READ 0x22 +#define SYSLINK_OW_WRITE 0x23 + +// Limited by the CRTP packet which is limited by the ESB protocol used by the NRF +#define SYSLINK_MAX_DATA_LEN 32 + +#define SYSLINK_RADIO_RATE_250K 0 +#define SYSLINK_RADIO_RATE_1M 1 +#define SYSLINK_RADIO_RATE_2M 2 + + +typedef struct { + uint8_t type; + uint8_t length; + uint8_t data[SYSLINK_MAX_DATA_LEN]; + uint8_t cksum[2]; +} __attribute__((packed)) syslink_message_t; + + +#define OW_SIZE 112 +#define OW_READ_BLOCK 29 +#define OW_WRITE_BLOCK 26 // TODO: Use even, but can be up to 27 + +typedef struct { + uint8_t nmems; +} __attribute__((packed)) syslink_ow_scan_t; + +typedef struct { + uint8_t family; // Should by 0x0D for most chips + uint8_t sn[6]; + uint8_t crc; +} __attribute__((packed)) syslink_ow_id_t; + +typedef struct { + uint8_t idx; + uint8_t id[8]; +} __attribute__((packed)) syslink_ow_getinfo_t; + +typedef struct { + uint8_t idx; + uint16_t addr; + uint8_t data[OW_READ_BLOCK]; +} __attribute__((packed)) syslink_ow_read_t; + +typedef struct { + uint8_t idx; + uint16_t addr; + uint16_t length; + uint8_t data[OW_WRITE_BLOCK]; +} __attribute__((packed)) syslink_ow_write_t; + + +typedef enum { + SYSLINK_STATE_START = 0, + SYSLINK_STATE_TYPE, + SYSLINK_STATE_LENGTH, + SYSLINK_STATE_DATA, + SYSLINK_STATE_CKSUM +} SYSLINK_STATE; + +typedef struct { + SYSLINK_STATE state; + int index; + +} syslink_parse_state; + + +#ifdef __cplusplus +extern "C" { +#endif + +extern void syslink_parse_init(syslink_parse_state *state); + +// Returns true if a full message was parsed +extern int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg); + +extern void syslink_compute_cksum(syslink_message_t *msg); + +#ifdef __cplusplus +} +#endif diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp b/boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp new file mode 100644 index 0000000000..7c7ac1e21a --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink_bridge.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file syslink_bridge.cpp + * + * Character device for talking to the radio as a plain serial port + */ + +#include "syslink_main.h" +#include + + + +SyslinkBridge::SyslinkBridge(Syslink *link) : + CDev("/dev/bridge0"), + _link(link), + _readbuffer(16, sizeof(crtp_message_t)) +{ + _msg_to_send.header = 0; + _msg_to_send.size = sizeof(_msg_to_send.header); + _msg_to_send.port = CRTP_PORT_MAVLINK; + _msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1; // to send 30 bytes of data + //ideally _msg_to_send.data size should be CRTP_MAX_DATA_SIZE but cfbridge does not receive 31 bytes of data due to a bug somewhere +} + +int +SyslinkBridge::init() +{ + int ret = CDev::init(); + + /* if init failed, bail now */ + if (ret != OK) { + PX4_DEBUG("CDev init failed"); + return ret; + } + + + return ret; +} + +pollevent_t +SyslinkBridge::poll_state(struct file *filp) +{ + pollevent_t state = 0; + + if (!_readbuffer.empty()) { + state |= POLLIN; + } + + if (_link->_writebuffer.space() > 0) { + state |= POLLOUT; + } + + return state; +} + +ssize_t +SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + int nread = 0; + crtp_message_t msg; + + while (!_readbuffer.empty() && buflen >= sizeof(CRTP_MAX_DATA_SIZE)) { + _readbuffer.get(&msg, sizeof(msg)); + int size = msg.size - sizeof(msg.header); + memcpy(buffer, &msg.data, size); + + nread += size; + buffer += size; + buflen -= size; + } + + return nread; +} + +ssize_t +SyslinkBridge::write(struct file *filp, const char *buffer, size_t buflen) +{ + int buflen_rem = buflen; + + while (buflen_rem > 0) { + + int datasize = MIN(_msg_to_send_size_remaining, buflen_rem); + _msg_to_send.size += datasize; + memcpy(&_msg_to_send.data[CRTP_MAX_DATA_SIZE - 1 - _msg_to_send_size_remaining], buffer, datasize); + + buffer += datasize; + _msg_to_send_size_remaining -= datasize; + buflen_rem -= datasize; + + + if (_msg_to_send_size_remaining == 0) { + + if (_link->_writebuffer.force(&_msg_to_send, sizeof(crtp_message_t))) { + PX4_WARN("write buffer overflow"); + } + + _msg_to_send.size = sizeof(_msg_to_send.header); + _msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1; + } + } + + return buflen; +} + +int +SyslinkBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + // All termios commands should be silently ignored as they are handled + + switch (cmd) { + + case FIONSPACE: + *((int *) arg) = _link->_writebuffer.space() * CRTP_MAX_DATA_SIZE; + return 0; + + default: + /* give it to the superclass */ + CDev::ioctl(filp, cmd, arg); + return 0; + } +} + + +void +SyslinkBridge::pipe_message(crtp_message_t *msg) +{ + _readbuffer.force(msg, sizeof(msg->size) + msg->size); + poll_notify(POLLIN); +} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie21/syslink/syslink_main.cpp new file mode 100644 index 0000000000..b6e7b06aaf --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink_main.cpp @@ -0,0 +1,865 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file syslink_main.cpp + * Entry point for syslink module used to communicate with the NRF module on a Crazyflie + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include "crtp.h" +#include "syslink_main.h" +#include "drv_deck.h" + + + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); } + + +Syslink *g_syslink = nullptr; + +Syslink::Syslink() : + pktrate(0), + nullrate(0), + rxrate(0), + txrate(0), + _syslink_task(-1), + _task_running(false), + _bootloader_mode(false), + _count(0), + _null_count(0), + _count_in(0), + _count_out(0), + _lasttime(0), + _lasttxtime(0), + _lastrxtime(0), + _fd(0), + _queue(2, sizeof(syslink_message_t)), + _writebuffer(16, sizeof(crtp_message_t)), + _rssi(RC_INPUT_RSSI_MAX), + _bstate(BAT_DISCHARGING) +{ + px4_sem_init(&memory_sem, 0, 0); + /* memory_sem use case is a signal */ + px4_sem_setprotocol(&memory_sem, SEM_PRIO_NONE); +} + + +int +Syslink::start() +{ + _task_running = true; + _syslink_task = px4_task_spawn_cmd( + "syslink", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1500, + Syslink::task_main_trampoline, + NULL + ); + + return 0; +} + + +int +Syslink::set_datarate(uint8_t datarate) +{ + syslink_message_t msg; + msg.type = SYSLINK_RADIO_DATARATE; + msg.length = 1; + msg.data[0] = datarate; + return send_message(&msg); +} + +int +Syslink::set_channel(uint8_t channel) +{ + syslink_message_t msg; + msg.type = SYSLINK_RADIO_CHANNEL; + msg.length = 1; + msg.data[0] = channel; + return send_message(&msg); +} + +int +Syslink::set_address(uint64_t addr) +{ + syslink_message_t msg; + msg.type = SYSLINK_RADIO_ADDRESS; + msg.length = 5; + memcpy(&msg.data, &addr, 5); + return send_message(&msg); +} + +int +Syslink::send_queued_raw_message() +{ + if (_writebuffer.empty()) { + return 0; + } + + _lasttxtime = hrt_absolute_time(); + + syslink_message_t msg; + msg.type = SYSLINK_RADIO_RAW; + + _count_out++; + + _writebuffer.get(&msg.length, sizeof(crtp_message_t)); + + return send_message(&msg); +} + + +void +Syslink::update_params(bool force_set) +{ + param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN"); + param_t _param_radio_rate = param_find("SLNK_RADIO_RATE"); + param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1"); + param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2"); + + + // reading parameter values into temp variables + + int32_t channel, rate, addr1, addr2; + uint64_t addr = 0; + + param_get(_param_radio_channel, &channel); + param_get(_param_radio_rate, &rate); + param_get(_param_radio_addr1, &addr1); + param_get(_param_radio_addr2, &addr2); + + memcpy(&addr, &addr2, 4); + memcpy(((char *)&addr) + 4, &addr1, 4); + + + hrt_abstime t = hrt_absolute_time(); + + // request updates if needed + + if (channel != this->_channel || force_set) { + this->_channel = channel; + set_channel(channel); + this->_params_update[0] = t; + this->_params_ack[0] = 0; + } + + if (rate != this->_rate || force_set) { + this->_rate = rate; + set_datarate(rate); + this->_params_update[1] = t; + this->_params_ack[1] = 0; + } + + if (addr != this->_addr || force_set) { + this->_addr = addr; + set_address(addr); + this->_params_update[2] = t; + this->_params_ack[2] = 0; + } +} + +// 1M 8N1 serial connection to NRF51 +int +Syslink::open_serial(const char *dev) +{ +#ifndef B1000000 +#define B1000000 1000000 +#endif + + int rate = B1000000; + + // open uart + int fd = px4_open(dev, O_RDWR | O_NOCTTY); + int termios_state = -1; + + if (fd < 0) { + PX4_ERR("failed to open uart device!"); + return -1; + } + + // set baud rate + struct termios config; + tcgetattr(fd, &config); + + // clear ONLCR flag (which appends a CR for every LF) + config.c_oflag = 0; + config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // Disable hardware flow control + config.c_cflag &= ~CRTSCTS; + + + /* Set baud rate */ + if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) { + warnx("ERR SET BAUD %s: %d\n", dev, termios_state); + px4_close(fd); + return -1; + } + + if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) { + PX4_WARN("ERR SET CONF %s\n", dev); + px4_close(fd); + return -1; + } + + return fd; +} + + + +int +Syslink::task_main_trampoline(int argc, char *argv[]) +{ + g_syslink->task_main(); + return 0; +} + +void +Syslink::task_main() +{ + _bridge = new SyslinkBridge(this); + _bridge->init(); + + _memory = new SyslinkMemory(this); + _memory->init(); + + _battery.reset(); + + + // int ret; + + /* Open serial port */ + const char *dev = "/dev/ttyS2"; + _fd = open_serial(dev); + + if (_fd < 0) { + err(1, "can't open %s", dev); + return; + } + + + /* Set non-blocking */ + /* + int flags = fcntl(_fd, F_GETFL, 0); + fcntl(_fd, F_SETFL, flags | O_NONBLOCK); + */ + + px4_arch_configgpio(GPIO_NRF_TXEN); + + px4_pollfd_struct_t fds[2]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + fds[1].fd = _params_sub; + fds[1].events = POLLIN; + + int error_counter = 0; + + char buf[64]; + int nread; + + syslink_parse_state state; + syslink_message_t msg; + + syslink_parse_init(&state); + + //setup initial parameters + update_params(true); + + while (_task_running) { + int poll_ret = px4_poll(fds, 2, 500); + + /* handle the poll result */ + if (poll_ret == 0) { + /* timeout: this means none of our providers is giving us data */ + + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (error_counter < 10 || error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("[syslink] ERROR return value from poll(): %d" + , poll_ret); + } + + error_counter++; + + } else { + if (fds[0].revents & POLLIN) { + if ((nread = read(_fd, buf, sizeof(buf))) < 0) { + continue; + } + + for (int i = 0; i < nread; i++) { + if (syslink_parse_char(&state, buf[i], &msg)) { + handle_message(&msg); + } + } + } + + if (fds[1].revents & POLLIN) { + parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + update_params(false); + } + } + + } + + close(_fd); +} + +void +Syslink::handle_message(syslink_message_t *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - _lasttime > 1000000) { + pktrate = _count; + rxrate = _count_in; + txrate = _count_out; + nullrate = _null_count; + + _lasttime = t; + _count = 0; + _null_count = 0; + _count_in = 0; + _count_out = 0; + } + + _count++; + + if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) { + // When the power button is hit + } else if (msg->type == SYSLINK_PM_BATTERY_STATE) { + + if (msg->length != 9) { + return; + } + + uint8_t flags = msg->data[0]; + int charging = flags & 1; + int powered = flags & 2; + + float vbat; //, iset; + memcpy(&vbat, &msg->data[1], sizeof(float)); + //memcpy(&iset, &msg->data[5], sizeof(float)); + + _battery.updateBatteryStatus(t, vbat, -1, true, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); + + + // Update battery charge state + if (charging) { + _bstate = BAT_CHARGING; + } + + /* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */ + else if (powered && !charging && vbat > 3.7f) { + _bstate = BAT_CHARGED; + + } else { + _bstate = BAT_DISCHARGING; + } + + } else if (msg->type == SYSLINK_RADIO_RSSI) { + uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm + _rssi = 140 - rssi * 100 / (100 - 40); + + } else if (msg->type == SYSLINK_RADIO_RAW) { + handle_raw(msg); + _lastrxtime = t; + + } else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) { + handle_radio(msg); + + } else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) { + memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t)); + px4_sem_post(&memory_sem); + + } else { + PX4_INFO("GOT %d", msg->type); + } + + //Send queued messages + if (!_queue.empty()) { + _queue.get(msg, sizeof(syslink_message_t)); + send_message(msg); + } + + + float p = (t % 500000) / 500000.0f; + + /* Use LED_GREEN for charging indicator */ + if (_bstate == BAT_CHARGED) { + led_on(LED_GREEN); + + } else if (_bstate == BAT_CHARGING && p < 0.25f) { + led_on(LED_GREEN); + + } else { + led_off(LED_GREEN); + } + + /* Alternate RX/TX LEDS when transfering */ + bool rx = t - _lastrxtime < 200000, + tx = t - _lasttxtime < 200000; + + + if (rx && p < 0.25f) { + led_on(LED_RX); + + } else { + led_off(LED_RX); + } + + if (tx && p > 0.5f && p > 0.75f) { + led_on(LED_TX); + + } else { + led_off(LED_TX); + } + + + // resend parameters if they haven't been acknowledged + if (_params_ack[0] == 0 && t - _params_update[0] > 10000) { + set_channel(_channel); + + } else if (_params_ack[1] == 0 && t - _params_update[1] > 10000) { + set_datarate(_rate); + + } else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) { + set_address(_addr); + } +} + +void +Syslink::handle_radio(syslink_message_t *sys) +{ + hrt_abstime t = hrt_absolute_time(); + + // record acknowlegements to parameter messages + if (sys->type == SYSLINK_RADIO_CHANNEL) { + _params_ack[0] = t; + + } else if (sys->type == SYSLINK_RADIO_DATARATE) { + _params_ack[1] = t; + + } else if (sys->type == SYSLINK_RADIO_ADDRESS) { + _params_ack[2] = t; + } +} + +void +Syslink::handle_raw(syslink_message_t *sys) +{ + crtp_message_t *c = (crtp_message_t *) &sys->length; + + if (CRTP_NULL(*c)) { + if (c->size >= 3) { + handle_bootloader(sys); + } + + _null_count++; + + } else if (c->port == CRTP_PORT_COMMANDER) { + + crtp_commander *cmd = (crtp_commander *) &c->data[0]; + + input_rc_s rc = {}; + + rc.timestamp = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp; + rc.channel_count = 5; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + rc.rssi = _rssi; + + + double pitch = cmd->pitch, roll = cmd->roll, yaw = cmd->yaw; + + /* channels (scaled to rc limits) */ + rc.values[0] = pitch * 500 / 20 + 1500; + rc.values[1] = roll * 500 / 20 + 1500; + rc.values[2] = yaw * 500 / 150 + 1500; + rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000; + rc.values[4] = 1000; // Dummy channel as px4 needs at least 5 + + _rc_pub.publish(rc); + + } else if (c->port == CRTP_PORT_MAVLINK) { + _count_in++; + /* Pipe to Mavlink bridge */ + _bridge->pipe_message(c); + + } else { + handle_raw_other(sys); + } + + // Block all non-requested messaged in bootloader mode + if (_bootloader_mode) { + return; + } + + // Allow one raw message to be sent from the queue + send_queued_raw_message(); +} + +void +Syslink::handle_bootloader(syslink_message_t *sys) +{ + // Minimal bootloader emulation for being detectable + // To the bitcraze utilities, the STM32 will appear to have no flashable pages + // Upon receiving a bootloader message, all outbound packets are blocked in 'bootloader mode' due to how fragile the aforementioned utilities are to extra packets + + crtp_message_t *c = (crtp_message_t *) &sys->length; + + uint8_t target = c->data[0]; + uint8_t cmd = c->data[1]; + + if (target != 0xFF) { // CF2 STM32 target + return; + } + + _bootloader_mode = true; + + if (cmd == 0x10) { // GET_INFO + + c->size = 1 + 23; + memset(&c->data[2], 0, 21); + c->data[22] = 0x10; // Protocol version + send_message(sys); + } +} + +void +Syslink::handle_raw_other(syslink_message_t *sys) +{ + // This function doesn't actually do anything + // It is just here to return null responses to most standard messages + + crtp_message_t *c = (crtp_message_t *) &sys->length; + + if (c->port == CRTP_PORT_LOG) { + + PX4_INFO("Log: %d %d", c->channel, c->data[0]); + + if (c->channel == 0) { // Table of Contents Access + + uint8_t cmd = c->data[0]; + + if (cmd == 0) { // GET_ITEM + //int id = c->data[1]; + memset(&c->data[2], 0, 3); + c->data[2] = 1; // type + c->size = 1 + 5; + send_message(sys); + + } else if (cmd == 1) { // GET_INFO + memset(&c->data[1], 0, 7); + c->size = 1 + 8; + send_message(sys); + } + + } else if (c->channel == 1) { // Log control + + uint8_t cmd = c->data[0]; + + PX4_INFO("Responding to cmd: %d", cmd); + c->data[2] = 0; // Success + c->size = 3 + 1; + + // resend message + send_message(sys); + + } else if (c->channel == 2) { // Log data + + } + } else if (c->port == CRTP_PORT_MEM) { + if (c->channel == 0) { // Info + int cmd = c->data[0]; + + if (cmd == 1) { // GET_NBR_OF_MEMS + c->data[1] = 0; + c->size = 2 + 1; + + // resend message + send_message(sys); + } + } + + } else if (c->port == CRTP_PORT_PARAM) { + if (c->channel == 0) { // TOC Access + // uint8_t msgId = c->data[0]; + + c->data[1] = 0; // Last parameter (id = 0) + memset(&c->data[2], 0, 10); + c->size = 1 + 8; + send_message(sys); + } + + else if (c->channel == 1) { // Param read + // 0 is ok + c->data[1] = 0; // value + c->size = 1 + 2; + send_message(sys); + } + + } else { + PX4_INFO("Got raw: %d", c->port); + } +} + +int +Syslink::send_bytes(const void *data, size_t len) +{ + // TODO: This could be way more efficient + // Using interrupts/DMA/polling would be much better + for (size_t i = 0; i < len; i++) { + // Block until we can send a byte + while (px4_arch_gpioread(GPIO_NRF_TXEN)) ; + + write(_fd, ((const char *)data) + i, 1); + } + + return 0; +} + +int +Syslink::send_message(syslink_message_t *msg) +{ + syslink_compute_cksum(msg); + send_bytes(syslink_magic, 2); + send_bytes(&msg->type, sizeof(msg->type)); + send_bytes(&msg->length, sizeof(msg->length)); + send_bytes(&msg->data, msg->length); + send_bytes(&msg->cksum, sizeof(msg->cksum)); + return 0; +} + + +namespace syslink +{ + +void usage(); +void start(); +void status(); +void test(); +void attached(int pid); + +void usage() +{ + warnx("missing command: try 'start', 'status', 'attached', 'test'"); +} + +void start() +{ + if (g_syslink != nullptr) { + printf("Already started\n"); + exit(1); + } + + g_syslink = new Syslink(); + g_syslink->start(); + + // Wait for task and bridge to start + usleep(5000); + + + warnx("Started syslink on /dev/ttyS2\n"); + exit(0); + +} + +void status() +{ + if (g_syslink == nullptr) { + printf("Please start syslink first\n"); + exit(1); + } + + printf("Connection activity:\n"); + printf("- total rx: %d p/s\n", g_syslink->pktrate); + printf("- radio rx: %d p/s (%d null)\n", g_syslink->rxrate, g_syslink->nullrate); + printf("- radio tx: %d p/s\n\n", g_syslink->txrate); + + printf("Parameter Status:\n"); + const char *goodParam = "good"; + const char *badParam = "fail!"; + printf("- channel: %s\n", g_syslink->is_good(0) ? goodParam : badParam); + printf("- data rate: %s\n", g_syslink->is_good(1) != 0 ? goodParam : badParam); + printf("- address: %s\n\n", g_syslink->is_good(2) != 0 ? goodParam : badParam); + + + int deckfd = open(DECK_DEVICE_PATH, O_RDONLY); + int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks); + printf("Deck scan: (found %d)\n", ndecks); + + for (int i = 0; i < ndecks; i++) { + ioctl(deckfd, DECKIOSNUM, (unsigned long) &i); + + uint8_t *id; + int idlen = ioctl(deckfd, DECKIOID, (unsigned long) &id); + + // TODO: Validate the ID + printf("%i: ROM ID: ", i); + + for (int idi = 0; idi < idlen; idi++) { + printf("%02X", id[idi]); + } + + deck_descriptor_t desc; + read(deckfd, &desc, sizeof(desc)); + + printf(", VID: %02X , PID: %02X\n", desc.header, desc.vendorId, desc.productId); + + // Print pages of memory + for (size_t di = 0; di < sizeof(desc); di++) { + if (di % 16 == 0) { + printf("\n"); + } + + printf("%02X ", ((uint8_t *)&desc)[di]); + + } + + printf("\n\n"); + } + + close(deckfd); + exit(0); +} + +void attached(int pid) +{ + bool found = false; + + int deckfd = open(DECK_DEVICE_PATH, O_RDONLY); + int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks); + + for (int i = 0; i < ndecks; i++) { + ioctl(deckfd, DECKIOSNUM, (unsigned long) &i); + + deck_descriptor_t desc; + read(deckfd, &desc, sizeof(desc)); + + if (desc.productId == pid) { + found = true; + break; + } + } + + close(deckfd); + exit(found ? 1 : 0); +} + +void test() +{ + // TODO: Ensure battery messages are recent + // TODO: Read and write from memory to ensure it is working +} + +} // namespace syslink + +int syslink_main(int argc, char *argv[]) +{ + if (argc < 2) { + syslink::usage(); + exit(1); + } + + const char *verb = argv[1]; + + if (!strcmp(verb, "start")) { + syslink::start(); + } + + if (!strcmp(verb, "status")) { + syslink::status(); + } + + if (!strcmp(verb, "attached")) { + if (argc != 3) { + errx(1, "usage: syslink attached [deck_pid]"); + } + + int pid = atoi(argv[2]); + syslink::attached(pid); + } + + if (!strcmp(verb, "test")) { + syslink::test(); + } + + syslink::usage(); + exit(1); + + return 0; +} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_main.h b/boards/bitcraze/crazyflie21/syslink/syslink_main.h new file mode 100644 index 0000000000..875aa0347d --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink_main.h @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#include +#include "ringbuffer.h" + +#include +#include +#include +#include + +#include "syslink.h" +#include "crtp.h" + +using namespace time_literals; + +#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) + +typedef enum { + BAT_DISCHARGING = 0, + BAT_CHARGING = 1, + BAT_CHARGED = 2 +} battery_state; + + +class SyslinkBridge; +class SyslinkMemory; + +class Syslink +{ +public: + Syslink(); + + int start(); + + int set_datarate(uint8_t datarate); + int set_channel(uint8_t channel); + int set_address(uint64_t addr); + + int is_good(int i) { return _params_ack[i] != 0; } + + int pktrate; + int nullrate; + int rxrate; + int txrate; + +private: + friend class SyslinkBridge; + friend class SyslinkMemory; + + int open_serial(const char *dev); + + void handle_message(syslink_message_t *msg); + void handle_raw(syslink_message_t *sys); + void handle_radio(syslink_message_t *sys); + void handle_bootloader(syslink_message_t *sys); + + // Handles other types of messages that we don't really care about, but + // will be maintained with the bare minimum implementation for supporting + // other crazyflie libraries + void handle_raw_other(syslink_message_t *sys); + + int send_bytes(const void *data, size_t len); + + // Checksums and transmits a syslink message + int send_message(syslink_message_t *msg); + + int send_queued_raw_message(); + + void update_params(bool force_set); + + int _syslink_task; + bool _task_running; + bool _bootloader_mode; + + int _count; + int _null_count; + int _count_in; + int _count_out; + hrt_abstime _lasttime; + hrt_abstime _lasttxtime; // Last time a radio message was sent + hrt_abstime _lastrxtime; // Last time a radio message was recieved + + int _fd; + + // For receiving raw syslink messages to send from other processes + ringbuffer::RingBuffer _queue; + + // Stores data that was needs to be written as a raw message + ringbuffer::RingBuffer _writebuffer; + SyslinkBridge *_bridge; + SyslinkMemory *_memory; + + int _params_sub; + + // Current parameter values + int32_t _channel, _rate; + uint64_t _addr; + hrt_abstime _params_update[3]; // Time at which the parameters were updated + hrt_abstime _params_ack[3]; // Time at which the parameters were acknowledged by the nrf module + + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; + + // nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS + static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms; + Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US}; + + int32_t _rssi; + battery_state _bstate; + + px4_sem_t memory_sem; + + syslink_message_t memory_msg; + + static int task_main_trampoline(int argc, char *argv[]); + + void task_main(); +}; + + +class SyslinkBridge : public cdev::CDev +{ + +public: + SyslinkBridge(Syslink *link); + virtual ~SyslinkBridge() = default; + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + // Makes the message available for reading to processes reading from the bridge + void pipe_message(crtp_message_t *msg); + +protected: + virtual pollevent_t poll_state(struct file *filp); + +private: + Syslink *_link; + + // Stores data that was received from syslink but not yet read by another driver + ringbuffer::RingBuffer _readbuffer; + + crtp_message_t _msg_to_send; + int _msg_to_send_size_remaining; +}; + + +class SyslinkMemory : public cdev::CDev +{ + +public: + SyslinkMemory(Syslink *link); + virtual ~SyslinkMemory() = default; + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + friend class Syslink; + + Syslink *_link; + + int _activeI; + + syslink_message_t msgbuf; + + uint8_t scan(); + void getinfo(int i); + + int read(int i, uint16_t addr, char *buf, int length); + int write(int i, uint16_t addr, const char *buf, int length); + + void sendAndWait(); +}; diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp b/boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp new file mode 100644 index 0000000000..4473339557 --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink_memory.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include "syslink_main.h" + +#include "drv_deck.h" + +#include + + +SyslinkMemory::SyslinkMemory(Syslink *link) : + CDev(DECK_DEVICE_PATH), + _link(link), + _activeI(0) +{ +} + + +int +SyslinkMemory::init() +{ + int ret = CDev::init(); + + /* if init failed, bail now */ + if (ret != OK) { + PX4_DEBUG("CDev init failed"); + return ret; + } + + + return ret; +} + +ssize_t +SyslinkMemory::read(struct file *filp, char *buffer, size_t buflen) +{ + return read(_activeI, 0, buffer, buflen); +} + +ssize_t +SyslinkMemory::write(struct file *filp, const char *buffer, size_t buflen) +{ + // For now, unsupported + return -1; +// return buflen; +} + +int +SyslinkMemory::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case DECKIOGNUM: + *((int *) arg) = scan(); + return 0; + + case DECKIOSNUM: + _activeI = *((int *) arg); + return 0; + + case DECKIOID: { + syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data; + getinfo(_activeI); + *((uint8_t **)arg) = data->id; + return 8; + } + + default: + CDev::ioctl(filp, cmd, arg); + return 0; + } +} + + +uint8_t +SyslinkMemory::scan() +{ + syslink_ow_scan_t *data = (syslink_ow_scan_t *) &msgbuf.data; + msgbuf.type = SYSLINK_OW_SCAN; + msgbuf.length = 0; + sendAndWait(); + + return data->nmems; +} + +void +SyslinkMemory::getinfo(int i) +{ + syslink_ow_getinfo_t *data = (syslink_ow_getinfo_t *) &msgbuf.data; + msgbuf.type = SYSLINK_OW_GETINFO; + msgbuf.length = 1; + data->idx = i; + sendAndWait(); +} + +int +SyslinkMemory::read(int i, uint16_t addr, char *buf, int length) +{ + syslink_ow_read_t *data = (syslink_ow_read_t *) &msgbuf.data; + msgbuf.type = SYSLINK_OW_READ; + + int nread = 0; + + while (nread < length) { + + msgbuf.length = 3; + data->idx = i; + data->addr = addr; + sendAndWait(); + + // Number of bytes actually read + int n = MIN(length - nread, msgbuf.length - 3); + + if (n == 0) { + break; + } + + memcpy(buf, data->data, n); + nread += n; + buf += n; + addr += n; + } + + return nread; +} + +int +SyslinkMemory::write(int i, uint16_t addr, const char *buf, int length) +{ + // TODO: Unimplemented + return -1; +} + +void +SyslinkMemory::sendAndWait() +{ + // TODO: Force the syslink thread to wake up + _link->_queue.force(&msgbuf, sizeof(msgbuf)); + px4_sem_wait(&_link->memory_sem); +} diff --git a/boards/bitcraze/crazyflie21/syslink/syslink_params.c b/boards/bitcraze/crazyflie21/syslink/syslink_params.c new file mode 100644 index 0000000000..921ec014ae --- /dev/null +++ b/boards/bitcraze/crazyflie21/syslink/syslink_params.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file syslink_params.c + * + * Parameters defined by the syslink module and the exposed NRF51 radio + * + * @author Dennis Shtatnov + */ + +/** + * Operating channel of the NRF51 + * + * @min 0 + * @max 125 + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_CHAN, 80); + +/** + * Operating datarate of the NRF51 + * + * @min 0 + * @max 2 + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 2); + +/** + * Operating address of the NRF51 (most significant byte) + * + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_ADDR1, 231); // 0xE7 + +/** + * Operating address of the NRF51 (least significant 4 bytes) + * + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, 3890735079); // 0xE7E7E7E7 diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp new file mode 100644 index 0000000000..8b476c45a6 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 "BMI088.hpp" + +#include "BMI088_Accelerometer.hpp" +#include "BMI088_Gyroscope.hpp" + +I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + BMI088 *instance = nullptr; + + if (cli.type == DRV_ACC_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(iterator.configuredBusOption(), iterator.bus(), + cli.i2c_address, cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + + } else if (cli.type == DRV_GYR_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(iterator.configuredBusOption(), iterator.bus(), + cli.i2c_address, cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + } + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +BMI088::BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, + enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) : + I2C(devtype, name, bus, device, frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype), + _drdy_gpio(drdy_gpio) +{ +} + +int BMI088::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + int res = Reset() ? 0 : -1; + _state = STATE::SELFTEST; + + return res; +} + +bool BMI088::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp new file mode 100644 index 0000000000..1acf72e4e5 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } + +class BMI088 : public device::I2C, public I2CSPIDriver +{ +public: + BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode, + uint32_t frequency, spi_drdy_gpio_t drdy_gpio); + + virtual ~BMI088() = default; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + virtual void RunImpl() = 0; + + int init() override; + virtual void print_status() = 0; + +protected: + + bool Reset(); + + const spi_drdy_gpio_t _drdy_gpio; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + int _overflow_data_size_count{0}; + int _overflow_fifo_max_samples_count{0}; + int _fifo_read_error_count{0}; + int _empty_count{0}; + int _total_failure_count{0}; + + + px4::atomic _drdy_fifo_read_samples{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + SELFTEST, + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + }; + + STATE _state{STATE::SELFTEST}; + + uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval + +}; diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp new file mode 100644 index 0000000000..9a484995b7 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp @@ -0,0 +1,962 @@ +/**************************************************************************** + * + * 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 "BMI088_Accelerometer.hpp" + +#include // CONSTANTS_ONE_G + +using namespace time_literals; + +namespace Bosch::BMI088::Accelerometer +{ + +BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, + int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : + BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), + _px4_accel(get_device_id(), rotation) +{ + if (drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); + } + + ConfigureSampleRate(1600); +} + +BMI088_Accelerometer::~BMI088_Accelerometer() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +void BMI088_Accelerometer::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI088_Accelerometer::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +uint8_t BMI088_Accelerometer::RegisterRead(Register reg) +{ + uint8_t add = static_cast(reg); + uint8_t cmd[2] = {add, 0}; + transfer(&cmd[0], 1, &cmd[1], 1); + return cmd[1]; +} + +uint8_t BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t add = static_cast(reg); + uint8_t cmd[2] = { add, value}; + return transfer(cmd, sizeof(cmd), nullptr, 0); +} + +int BMI088_Accelerometer::probe() +{ + const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID); + + if (ACC_CHIP_ID != ID) { + DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID); + return PX4_ERROR; + } + PX4_WARN("Probe success, ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID); + + return PX4_OK; +} + +void BMI088_Accelerometer::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::SELFTEST: + //PX4_WARN("Selftest state"); + //SelfTest(); + _state = STATE::RESET; + ScheduleDelayed(10_ms); + break; + + case STATE::RESET: + // ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor + RegisterWrite(Register::ACC_SOFTRESET, 0xB6); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + + + ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value. + + break; + + case STATE::WAIT_FOR_RESET: + if (RegisterRead(Register::ACC_CHIP_ID) == ID) { + // ACC_PWR_CONF: Power on sensor + RegisterWrite(Register::ACC_PWR_CONF, 0); + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_READ: { + //FIFOReadTest(now); + NormalRead(now); + } + break; + } +} + +void BMI088_Accelerometer::ConfigureAccel() +{ + //PX4_WARN("ConfigureAccel"); + const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0); + + switch (ACC_RANGE) { + case acc_range_3g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(3.f); + break; + + case acc_range_6g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(6.f); + break; + + case acc_range_12g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(12.f); + break; + + case acc_range_24g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(24.f); + break; + } +} + +void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 800; // default to 800 Hz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + PX4_WARN("_fifo_empty_interval_us %d", _fifo_empty_interval_us); + _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); + + PX4_WARN("_fifo_samples %d", _fifo_samples); + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE); + + PX4_WARN("_fifo_empty_interval_us %d", _fifo_empty_interval_us); + //PX4_WARN("_fifo_samples %d", _fifo_samples); + ConfigureFIFOWatermark(_fifo_samples); +} + +void BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO_WTM: 13 bit FIFO watermark level value + // unit of the fifo watermark is one byte + const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); + + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_WTM_0) { + // fifo_water_mark[7:0] + r.set_bits = fifo_watermark_threshold & 0x00FF; + r.clear_bits = ~r.set_bits; + + } else if (r.reg == Register::FIFO_WTM_1) { + // fifo_water_mark[12:8] + r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8; + r.clear_bits = ~r.set_bits; + } + } +} + +bool BMI088_Accelerometer::Configure() +{ + + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + ConfigureAccel(); + + return success; +} + +int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void BMI088_Accelerometer::DataReady() +{ + uint32_t expected = 0; + + if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { + ScheduleNow(); + } +} + +bool BMI088_Accelerometer::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool BMI088_Accelerometer::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI088_Accelerometer::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +uint16_t BMI088_Accelerometer::FIFOReadCount() +{ + // FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte + uint8_t fifo_len_buf[2] {}; + fifo_len_buf[0] = static_cast(Register::FIFO_LENGTH_0) | DIR_READ; + // fifo_len_buf[1] dummy byte + + if (transfer(&fifo_len_buf[0], 1, &fifo_len_buf[0], 2) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + const uint8_t FIFO_LENGTH_0 = fifo_len_buf[0]; // fifo_byte_counter[7:0] + const uint8_t FIFO_LENGTH_1 = fifo_len_buf[1] & 0x3F; // fifo_byte_counter[13:8] + + return combine(FIFO_LENGTH_1, FIFO_LENGTH_0); +} + +bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE); + + if (transfer((uint8_t *)&buffer, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + //PX4_WARN("Accel transfer success"); + const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0); + + // An empty FIFO corresponds to 0x8000 + if (fifo_byte_counter == 0x8000) { + perf_count(_fifo_empty_perf); + return false; + + } else if (fifo_byte_counter >= FIFO::SIZE) { + perf_count(_fifo_overflow_perf); + return false; + } + + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT; + + // first find all sensor data frames in the buffer + uint8_t *data_buffer = (uint8_t *)&buffer.f[0]; + unsigned fifo_buffer_index = 0; // start of buffer + + while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) { + // look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2 + switch (data_buffer[fifo_buffer_index] & 0xFC) { + case FIFO::header::sensor_data_frame: { + // Acceleration sensor data frame + // Frame length: 7 bytes (1 byte header + 6 bytes payload) + + FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index]; + const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB); + const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB); + const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB); + + // 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.x[accel.samples] = accel_x; + accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.samples++; + + fifo_buffer_index += 7; // move forward to next record + } + break; + + case FIFO::header::skip_frame: + // Skip Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("Skip Frame"); + fifo_buffer_index += 2; + break; + + case FIFO::header::sensor_time_frame: + // Sensortime Frame + // Frame length: 4 bytes (1 byte header + 3 bytes payload) + PX4_DEBUG("Sensortime Frame"); + fifo_buffer_index += 4; + break; + + case FIFO::header::FIFO_input_config_frame: + // FIFO input config Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("FIFO input config Frame"); + fifo_buffer_index += 2; + break; + + case FIFO::header::sample_drop_frame: + // Sample drop Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("Sample drop Frame"); + fifo_buffer_index += 2; + break; + + default: + fifo_buffer_index++; + break; + } + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + _px4_accel.updateFIFO(accel); + return true; + } + + return false; +} + +bool BMI088_Accelerometer::SimpleFIFORead(const hrt_abstime ×tamp_sample) +{ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT; + + int fifo_fill_level = 0; + + uint8_t data_o[2] = { 0, 0 }; + uint8_t data_i[1] = {static_cast(Register::FIFO_LENGTH_0)}; + data_i[0] = static_cast(Register::FIFO_LENGTH_0); + + transfer(&data_i[0], 1, &data_o[0], 2); + fifo_fill_level = data_o[0] + 256 * data_o[1]; + //PX4_WARN("fifo_fill_level %d", fifo_fill_level); + + //uint8_t custom_size = fifo_fill_level * 7; + uint8_t custom_size = fifo_fill_level; + uint8_t buffer_data[custom_size] = {0}; + uint8_t buffer[1] = {0}; + buffer[0] = static_cast(Register::FIFO_DATA); + if(fifo_fill_level > 0){ + transfer(&buffer[0], 1, &buffer_data[0], custom_size); + struct FIFO::bmi08x_sensor_data bmi08x_accel; + + /* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */ + for(int i = 1; i < custom_size;) + { + /* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */ + if(buffer_data[i] == (0x84 & 0x8c)) + { + UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]); + //PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i/6, (double)bmi08x_accel.x, (double)bmi08x_accel.y, (double)bmi08x_accel.z); + + accel.x[accel.samples] = bmi08x_accel.x; + accel.y[accel.samples] = bmi08x_accel.y; + accel.z[accel.samples] = bmi08x_accel.z; + accel.samples++; + i += 7; + } + else{ + i++; + } + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + _px4_accel.updateFIFO(accel); + return true; + } + } + return false; +} + +void BMI088_Accelerometer::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E). + RegisterWrite(Register::ACC_SOFTRESET, 0xB0); + + // reset while FIFO is disabled + _drdy_fifo_read_samples.store(0); +} + +void BMI088_Accelerometer::UpdateTemperature() +{ + // stored in an 11-bit value in 2’s complement format + uint8_t temperature_buf[4] {}; + temperature_buf[0] = static_cast(Register::TEMP_MSB) | ACC_I2C_ADDR_PRIMARY; + // temperature_buf[1] dummy byte + + if (transfer(&temperature_buf[0], 1, &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const uint8_t TEMP_MSB = temperature_buf[2]; + const uint8_t TEMP_LSB = temperature_buf[3]; + + // Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data + uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32); + int16_t Temp_int11 = 0; + + if (Temp_uint11 > 1023) { + Temp_int11 = Temp_uint11 - 2048; + + } else { + Temp_int11 = Temp_uint11; + } + + float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C + + if (PX4_ISFINITE(temperature)) { + _px4_accel.set_temperature(temperature); + + } else { + perf_count(_bad_transfer_perf); + } +} + +bool BMI088_Accelerometer::SelfTest() { + PX4_WARN("Running self-test with datasheet recomended steps(page 17)"); + // Reset + PX4_WARN("Reseting the sensor"); + if(RegisterWrite(Register::ACC_SOFTRESET, 0xB6) == PX4_OK){ + PX4_WARN("Reset success"); + } + usleep(100000); + PX4_WARN("Accel on"); + if(RegisterWrite(Register::ACC_PWR_CTRL, 0x04) == PX4_OK){ + PX4_WARN("Accel on success"); + } + usleep(100000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + Configure(); + usleep(1000000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID); + PX4_WARN("ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID); + usleep(30000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + if(RegisterWrite(Register::ACC_PWR_CONF, 0) == PX4_OK){ + PX4_WARN("Start sensor success"); + PX4_WARN("ACC_PWR_CONF(0): 0x%02x", RegisterRead(Register::ACC_PWR_CONF)); + } + usleep(2000000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + + if(RegisterWrite(Register::ACC_RANGE, 0x03) == PX4_OK){ + PX4_WARN("Range set success"); + PX4_WARN("ACC_RANGE(0x03): 0x%02x", RegisterRead(Register::ACC_RANGE)); + } + usleep(100000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + + if(RegisterWrite(Register::ACC_CONF, 0xA7) == PX4_OK){ + PX4_WARN("Conf set success"); + PX4_WARN("ACC_CONF(0xA7): 0x%02x", RegisterRead(Register::ACC_CONF)); + } + usleep(100000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + + // Positive sel-test polarity + if(RegisterWrite(Register::ACC_SELF_TEST, 0x0D) == PX4_OK){ + PX4_WARN("Self-test positive mode set success"); + PX4_WARN("ACC_SELF_TEST(0x0D): 0x%02x", RegisterRead(Register::ACC_SELF_TEST)); + } + usleep(100000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + + float *accel_mss = ReadAccelDataFIFO(); + PX4_WARN("Positive value"); + PX4_WARN("X %f", (double)accel_mss[0]); + PX4_WARN("Y %f", (double)accel_mss[1]); + PX4_WARN("Z %f", (double)accel_mss[2]); + + // Negative sel-test polarity + if(RegisterWrite(Register::ACC_SELF_TEST, 0x09) == PX4_OK){ + PX4_WARN("Self-test negative mode set success"); + PX4_WARN("ACC_SELF_TEST(0x09): 0x%02x", RegisterRead(Register::ACC_SELF_TEST)); + } + usleep(600000); + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + float *accel_mss2 = ReadAccelDataFIFO(); + PX4_WARN("Negative value"); + PX4_WARN("X %f", (double)accel_mss2[0]); + PX4_WARN("Y %f", (double)accel_mss2[1]); + PX4_WARN("Z %f", (double)accel_mss2[2]); + + // Calculate difference between positive and negative sef-test response + float diff_x = accel_mss[0] - accel_mss2[0]; + float diff_y = accel_mss[1] - accel_mss2[1]; + float diff_z = accel_mss[2] - accel_mss2[2]; + + PX4_WARN("Diff value"); + PX4_WARN("diff_x %f", (double)diff_x); + PX4_WARN("diff_y %f", (double)diff_y); + PX4_WARN("diff_z %f", (double)diff_z); + + + if(diff_x >= 1000){ + PX4_WARN("X Axis self-test success"); + } + if(diff_y >= 1000){ + PX4_WARN("Y Axis self-test success"); + } + if(diff_z >= 500){ + PX4_WARN("Z Axis self-test success"); + } + + + // Disable self-test + RegisterWrite(Register::ACC_SELF_TEST, 0x00); + usleep(60000); + + PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg()); + // Reset + //PX4_WARN("Reseting the sensor again"); + //RegisterWrite(Register::ACC_SOFTRESET, 0xB6); + //usleep(100000); + return true; +} + +float * BMI088_Accelerometer::ReadAccelData() +{ + uint8_t cmd[1] = {0x12}; + + uint8_t buf[6] = {0, 0, 0, 0, 0, 0}; + uint8_t* buffer = buf; + + int16_t accel[3]; + + if(transfer(&cmd[0], 1, buffer, sizeof(buf)) == PX4_OK) { + PX4_WARN("ReadAccelData transfer success"); + } + for(uint8_t i = 0; i < sizeof(buf); i++){ + PX4_WARN("buf[%d]: %f", i, (double)buf[i]); + } + + accel[0] = (buf[1] << 8) | buf[0]; + accel[1] = (buf[3] << 8) | buf[2]; + accel[2] = (buf[5] << 8) | buf[4]; + + float *accel_mss = new float[3]; + + accel_mss[0] = (float) accel[0] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f; + accel_mss[1] = (float) accel[1] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f; + accel_mss[2] = (float) accel[2] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f; + + return accel_mss; +} + +float * BMI088_Accelerometer::ReadAccelDataFIFO() +{ + float *accel_mg = new float[3]; + struct FIFO::bmi08x_sensor_data bmi08x_accel; + uint8_t buffer[50] = {0}; + + PX4_WARN("FIFO mode is stop-at-full"); + /* Desired FIFO mode is stop-at-full: set bit #0 to 1 in 0x48. Bit #1 must always be one! */ + buffer[0] = 0x00 | 0x02; + RegisterWrite(Register::FIFO_CONFIG_0, buffer[0]); + PX4_WARN("FIFO_CONFIG_0(0x%02x): 0x%02x",buffer[0], RegisterRead(Register::FIFO_CONFIG_0)); + + PX4_WARN("Downsampling factor 2**4 = 16"); + /* Downsampling factor 2**4 = 16: write 4 into bit #4-6 of reg. 0x45. Bit #7 must always be one! */ + buffer[0] = 0x10 | 0x80; + RegisterWrite(Register::FIFO_DOWN_SAMPLING, buffer[0]); + PX4_WARN("FIFO_DOWN_SAMPLING(0x%02x): 0x%02x",buffer[0], RegisterRead(Register::FIFO_DOWN_SAMPLING)); + + /* Set water mark to 42 bytes (aka 6 frames, each 7 bytes: 1 byte header + 6 bytes accel data) */ + // uint16_t wml = 42; + // buffer[0] = (uint8_t) wml & 0xff; + // buffer[1] = (uint8_t) (wml >> 8) & 0xff; + // uint8_t add = static_cast(Register::FIFO_WTM_0); + // uint8_t cmd[3] = { add, buffer[0], buffer[1]}; + // transfer(cmd, sizeof(cmd), nullptr, 0); + // PX4_WARN("FIFO_WTM_0(0x%02x): 0x%02x",cmd[0], RegisterRead(Register::FIFO_WTM_0)); + + /* Enable the actual FIFO functionality: write 0x50 to 0x49. Bit #4 must always be one! */ + buffer[0] = 0x10 | 0x40; + RegisterWrite(Register::FIFO_CONFIG_1, buffer[0]); + PX4_WARN("FIFO_CONFIG_1(0x%02x): 0x%02x",buffer[0], RegisterRead(Register::FIFO_CONFIG_1)); + usleep(1000000); + + int fifo_fill_level = 0; + + uint8_t data_o[2] = { 0, 0 }; + uint8_t data_i[1] = {static_cast(Register::FIFO_LENGTH_0)}; + data_i[0] = static_cast(Register::FIFO_LENGTH_0); + + transfer(&data_i[0], 1, &data_o[0], 2); + fifo_fill_level = data_o[0] + 256 * data_o[1]; + PX4_WARN("fifo_fill_level %d", fifo_fill_level); + + // while(fifo_fill_level < wml) + // { + // transfer(&data_i[0], 1, &data_o[0], 2); + // fifo_fill_level = data_o[0] + 256 * data_o[1]; + // PX4_WARN("fifo_fill_level %d", fifo_fill_level); + // } + + uint8_t custom_size = 42; + uint8_t buffer_data[custom_size] = {0}; + buffer[0] = static_cast(Register::FIFO_DATA); + bmi08x_accel.x=10; + PX4_WARN("bmi08x_accel %d", bmi08x_accel.x); + transfer(&buffer[0], 1, &buffer_data[0], custom_size); + + /* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */ + for(int i = 1; i < custom_size;) + { + /* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */ + if(buffer_data[i] == (0x84 & 0x8c)) + { + UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]); + PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i/6, (double)bmi08x_accel.x, (double)bmi08x_accel.y, (double)bmi08x_accel.z); + accel_mg[0] = bmi08x_accel.x; + accel_mg[1] = bmi08x_accel.y; + accel_mg[2] = bmi08x_accel.z; + float* data_in_mg = SensorDataTomg(accel_mg); + PX4_WARN("Frame mg: %03d ax:%f ay:%f az:%f", i/6, (double)data_in_mg[0], (double)data_in_mg[1], (double)data_in_mg[2]); + i += 7; + } + else{ + i++; + } + } + return accel_mg; +} + +uint8_t BMI088_Accelerometer::CheckSensorErrReg(){ + return RegisterRead(Register::ACC_ERR_REG); +} + +void BMI088_Accelerometer::UnpackSensorData(struct FIFO::bmi08x_sensor_data *sens_data, uint8_t *buffer) +{ + uint16_t data_lsb; + uint16_t data_msb; + uint16_t start_idx = 0; + /* Gyro raw x data */ + data_lsb = buffer[start_idx++]; + data_msb = buffer[start_idx++]; + sens_data->x = (int16_t)((data_msb << 8) | data_lsb); + /* Gyro raw y data */ + data_lsb = buffer[start_idx++]; + data_msb = buffer[start_idx++]; + sens_data->y = (int16_t)((data_msb << 8) | data_lsb); + /* Gyro raw z data */ + data_lsb = buffer[start_idx++]; + data_msb = buffer[start_idx++]; + sens_data->z = (int16_t)((data_msb << 8) | data_lsb); +} + +float* BMI088_Accelerometer::SensorDataTomg(float* data) +{ + data[0] = (float) data[0] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f; + data[1] = (float) data[1] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f; + data[2] = (float) data[2] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f; + return data; +} + +bool BMI088_Accelerometer::NormalRead(const hrt_abstime ×tamp_sample) { + const int16_t tX[3] = {1, 0, 0}; + const int16_t tY[3] = {0, -1, 0}; + const int16_t tZ[3] = {0, 0, -1}; + + float x = 0; + float y = 0; + float z = 0; + uint8_t buffer[6] = {0}; + uint8_t cmd[1] = {static_cast(Register::ACC_READ)}; + + transfer(&cmd[0], 1, &buffer[0], 6); + + uint8_t RATE_X_LSB = buffer[0]; + uint8_t RATE_X_MSB = buffer[1]; + + uint8_t RATE_Y_LSB = buffer[2]; + uint8_t RATE_Y_MSB = buffer[3]; + + uint8_t RATE_Z_LSB = buffer[4]; + uint8_t RATE_Z_MSB = buffer[5]; + + const int16_t accel_x = combine(RATE_X_MSB, RATE_X_LSB); + const int16_t accel_y = combine(RATE_Y_MSB, RATE_Y_LSB); + const int16_t accel_z = combine(RATE_Z_MSB, RATE_Z_LSB); + + // 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) + x = accel_x * tX[0] + accel_y * tX[1] + accel_z * tX[2]; + y = accel_x * tY[0] + accel_y * tY[1] + accel_z * tY[2]; + z = accel_x * tZ[0] + accel_y * tZ[1] + accel_z * tZ[2]; + + //PX4_WARN("x: %f | y: %f | z: %f", (double)x, (double)y ,(double)z); + _px4_accel.update(timestamp_sample, x, y, z); + + return true; +} + +bool BMI088_Accelerometer::FIFOReadTest(const hrt_abstime ×tamp_sample){ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT; + + int fifo_fill_level = 0; + + uint8_t data_o[2] = { 0, 0 }; + uint8_t data_i[1] = {static_cast(Register::FIFO_LENGTH_0)}; + data_i[0] = static_cast(Register::FIFO_LENGTH_0); + + transfer(&data_i[0], 1, &data_o[0], 2); + fifo_fill_level = data_o[0] + (data_o[1]<<8); + if (fifo_fill_level & 0x8000) { + //PX4_WARN("fifo_fill_level & 0x8000"); + // empty + return false; + } + int n_frames_to_read = 5; + // don't read more than 8 frames at a time + if (fifo_fill_level > n_frames_to_read*7) { + fifo_fill_level = n_frames_to_read*7; + } + + if (fifo_fill_level == 0) { + //PX4_WARN("fifo_fill_level == 0"); + return false; + } + + uint8_t data[fifo_fill_level]; + data[0] = static_cast(Register::FIFO_DATA); + if(transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK) { + //PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK"); + return false; + } + const uint8_t *p = &data[0]; + + while (fifo_fill_level >= 7) { + /* + the fifo frames are variable length, with the frame type in the first byte + */ + uint8_t frame_len = 2; + switch (p[0] & 0xFC) { + case 0x84: { + // accel frame + frame_len = 7; + const uint8_t *d = p+1; + int16_t xyz[3] { + int16_t(uint16_t(d[0] | (d[1]<<8))), + int16_t(uint16_t(d[2] | (d[3]<<8))), + int16_t(uint16_t(d[4] | (d[5]<<8)))}; + + + const int16_t tX[3] = {1, 0, 0}; + const int16_t tY[3] = {0, -1, 0}; + const int16_t tZ[3] = {0, 0, -1}; + + float x = 0; + float y = 0; + float z = 0; + + x = xyz[0] * tX[0] + xyz[1] * tX[1] + xyz[2] * tX[2]; + y = xyz[0] * tY[0] + xyz[1] * tY[1] + xyz[2] * tY[2]; + z = xyz[0] * tZ[0] + xyz[1] * tZ[1] + xyz[2] * tZ[2]; + + accel.x[accel.samples] = x; + accel.y[accel.samples] = y; + accel.z[accel.samples] = z; + accel.samples++; + + break; + } + case 0x40: + // skip frame + frame_len = 2; + break; + case 0x44: + // sensortime frame + frame_len = 4; + break; + case 0x48: + // fifo config frame + frame_len = 2; + break; + case 0x50: + // sample drop frame + frame_len = 2; + break; + } + p += frame_len; + fifo_fill_level -= frame_len; + } + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + //PX4_WARN("accel.samples: %d", accel.samples); + _px4_accel.updateFIFO(accel); + return true; + } + return true; +} +} // namespace Bosch::BMI088::Accelerometer diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp new file mode 100644 index 0000000000..3b54f3f31b --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "BMI088.hpp" + +#include + +#include "Bosch_BMI088_Accelerometer_Registers.hpp" + +namespace Bosch::BMI088::Accelerometer +{ + +class BMI088_Accelerometer : public BMI088 +{ +public: + BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ~BMI088_Accelerometer() override; + + void RunImpl() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + // static constexpr uint32_t RATE{1600}; // 1600 Hz + static constexpr uint32_t RATE{1600}; // 1600 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::FIFO_LENGTH_0)}; + uint8_t dummy{0}; + uint8_t FIFO_LENGTH_0{0}; + uint8_t FIFO_LENGTH_1{0}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // Transfer data without length + struct FIFOTransferBufferWithoutLength { + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Configure(); + void ConfigureAccel(); + void ConfigureSampleRate(int sample_rate = 0); + void ConfigureFIFOWatermark(uint8_t samples); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + uint8_t RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void UpdateTemperature(); + void UnpackSensorData(struct FIFO::bmi08x_sensor_data *sens_data, uint8_t *buffer); + bool SelfTest(); + float* ReadAccelData(); + float* ReadAccelDataFIFO(); + float* SensorDataTomg(float* data); + uint8_t CheckSensorErrReg(); + bool SimpleFIFORead(const hrt_abstime ×tamp_sample); + bool NormalRead(const hrt_abstime ×tamp_sample); + bool FIFOReadTest(const hrt_abstime ×tamp_sample); + + PX4Accelerometer _px4_accel; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")}; + perf_counter_t _drdy_missed_perf{nullptr}; + + uint8_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{10}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::ACC_PWR_CONF, 0, ACC_PWR_CONF_BIT::acc_pwr_save }, // + { Register::ACC_PWR_CTRL, ACC_PWR_CTRL_BIT::acc_enable, 0 }, + { Register::ACC_CONF, ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 }, + { Register::ACC_RANGE, ACC_RANGE_BIT::acc_range_24g, 0 }, + { Register::FIFO_WTM_0, 0, 0 }, + { Register::FIFO_WTM_1, 0, 0 }, + { Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 }, + { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 }, + { Register::INT1_IO_CONF, INT1_IO_CONF_BIT::int1_out, 0 }, + { Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0}, + }; +}; + +} // namespace Bosch::BMI088::Accelerometer diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp new file mode 100644 index 0000000000..e93f957601 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp @@ -0,0 +1,509 @@ +/**************************************************************************** + * + * 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 "BMI088_Gyroscope.hpp" + +#include + +using namespace time_literals; + +namespace Bosch::BMI088::Gyroscope +{ + +BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, + int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : + BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), + _px4_gyro(get_device_id(), rotation) +{ + if (drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); + } + + ConfigureSampleRate(2000); +} + +BMI088_Gyroscope::~BMI088_Gyroscope() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +void BMI088_Gyroscope::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI088_Gyroscope::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +int BMI088_Gyroscope::probe() +{ + const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID); + + if (chipid != ID) { + DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid); + return PX4_ERROR; + } + + return PX4_OK; +} + +void BMI088_Gyroscope::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + + case STATE::SELFTEST: + //SelfTest(); + _state = STATE::RESET; + ScheduleDelayed(1_ms); + break; + + case STATE::RESET: + // GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor. + // Following a delay of 30 ms, all configuration settings are overwritten with their reset value. + RegisterWrite(Register::GYRO_SOFTRESET, 0xB6); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(30_ms); + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) { + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(1_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_READ: { + //FIFOReadTest(now); + NormalRead(now); + } + break; + } +} + +void BMI088_Gyroscope::ConfigureGyro() +{ + const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0); + + switch (GYRO_RANGE) { + case gyro_range_2000_dps: + _px4_gyro.set_scale(math::radians(1.f / 16.384f)); + _px4_gyro.set_range(math::radians(2000.f)); + break; + + case gyro_range_1000_dps: + _px4_gyro.set_scale(math::radians(1.f / 32.768f)); + _px4_gyro.set_range(math::radians(1000.f)); + break; + + case gyro_range_500_dps: + _px4_gyro.set_scale(math::radians(1.f / 65.536f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case gyro_range_250_dps: + _px4_gyro.set_scale(math::radians(1.f / 131.072f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case gyro_range_125_dps: + _px4_gyro.set_scale(math::radians(1.f / 262.144f)); + _px4_gyro.set_range(math::radians(125.f)); + break; + } +} + +void BMI088_Gyroscope::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 2000; // default to 1000 Hz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE); + + ConfigureFIFOWatermark(_fifo_samples); +} + +void BMI088_Gyroscope::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO watermark threshold + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_CONFIG_0) { + r.set_bits = samples; + r.clear_bits = ~r.set_bits; + } + } +} + +bool BMI088_Gyroscope::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + ConfigureGyro(); + + return success; +} + +int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void BMI088_Gyroscope::DataReady() +{ + uint32_t expected = 0; + + if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { + ScheduleNow(); + } +} + +bool BMI088_Gyroscope::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool BMI088_Gyroscope::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI088_Gyroscope::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t BMI088_Gyroscope::RegisterRead(Register reg) +{ + uint8_t add = static_cast(reg); + uint8_t cmd[2] = {add, 0}; + transfer(&cmd[0], 1, &cmd[1], 1); + return cmd[1]; +} + +void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t add = static_cast(reg); + uint8_t cmd[2] = {add, value}; + transfer(cmd, sizeof(cmd), nullptr, 0); +} + +void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + //PX4_WARN("Estimated transfer size: %d", transfer_size); + if (transfer((uint8_t *)&buffer, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = buffer.f[i]; + + const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB); + const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB); + const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB); + + // 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) + gyro.x[i] = gyro_x; + gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + } + + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + _px4_gyro.updateFIFO(gyro); + + return true; +} + +void BMI088_Gyroscope::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer. + RegisterWrite(Register::FIFO_CONFIG_0, 0); + + // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1 + RegisterWrite(Register::FIFO_CONFIG_1, 0); + + // reset while FIFO is disabled + _drdy_fifo_read_samples.store(0); + + // FIFO_CONFIG_0: restore FIFO watermark + // FIFO_CONFIG_1: re-enable FIFO + for (const auto &r : _register_cfg) { + if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) { + RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); + } + } +} + +bool BMI088_Gyroscope::SelfTest() { + //Datasheet page 17 self test + + //Set bit0 to enable built in self test + RegisterWrite(Register::SELF_TEST, 0x01); + usleep(10000); + uint8_t res = 0; + uint8_t test_res = false; + while(true){ + res = RegisterRead(Register::SELF_TEST); + if((res & 0x02) == 0x02){ + if((res & 0x04) == 0x00) { + PX4_WARN("Gyro Self-test success"); + test_res = true; + } else { + PX4_WARN("Gyro Self-test error"); + } + break; + } + } + + RegisterWrite(Register::SELF_TEST, 0x00); + return test_res; +} + +bool BMI088_Gyroscope::NormalRead(const hrt_abstime ×tamp_sample) { + float x = 0; + float y = 0; + float z = 0; + uint8_t buffer[6] = {0}; + uint8_t cmd[1] = {static_cast(Register::READ_GYRO)}; + + transfer(&cmd[0], 1, &buffer[0], 6); + + uint8_t RATE_X_LSB = buffer[0]; + uint8_t RATE_X_MSB = buffer[1]; + uint8_t RATE_Y_LSB = buffer[2]; + uint8_t RATE_Y_MSB = buffer[3]; + uint8_t RATE_Z_LSB = buffer[4]; + uint8_t RATE_Z_MSB = buffer[5]; + + const int16_t gyro_x = combine(RATE_X_MSB, RATE_X_LSB); + const int16_t gyro_y = combine(RATE_Y_MSB, RATE_Y_LSB); + const int16_t gyro_z = combine(RATE_Z_MSB, RATE_Z_LSB); + + // 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) + x = gyro_x; + y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + + _px4_gyro.update(timestamp_sample, x, y, z); + + return true; +} + +bool BMI088_Gyroscope::FIFOReadTest(const hrt_abstime ×tamp_sample) +{ + uint8_t n_frames; + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = 0; + gyro.dt = FIFO_SAMPLE_DT; + + uint8_t data_i[1] = {static_cast(Register::FIFO_STATUS)}; + + transfer(&data_i[0], 1, &n_frames, 1); + + n_frames &= 0x7F; + + int n_frames_to_read = 5; + // don't read more than 8 frames at a time + if (n_frames > n_frames_to_read) { + n_frames = n_frames_to_read; + } + + if (n_frames == 0) { + return false; + } + + uint8_t data[6*n_frames]; + data[0] = static_cast(Register::FIFO_DATA); + if(transfer(&data[0], 1, &data[0], 6*n_frames) != PX4_OK) { + //PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK"); + return false; + } + for (uint8_t i = 0; i < n_frames; i++) { + const uint8_t *d = &data[i*6]; + int16_t xyz[3] { + int16_t(uint16_t(d[0] | d[1]<<8)), + int16_t(uint16_t(d[2] | d[3]<<8)), + int16_t(uint16_t(d[4] | d[5]<<8)) }; + + gyro.x[i] = xyz[0]; + gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1]; + gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2]; + gyro.samples++; + } + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (gyro.samples > 0) { + //PX4_WARN("accel.samples: %d", accel.samples); + _px4_gyro.updateFIFO(gyro); + return true; + } + return true; + +} +} // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp new file mode 100644 index 0000000000..5e5a2936fa --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "BMI088.hpp" + +#include + +#include "Bosch_BMI088_Gyroscope_Registers.hpp" + +namespace Bosch::BMI088::Gyroscope +{ + +class BMI088_Gyroscope : public BMI088 +{ +public: + BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ~BMI088_Gyroscope() override; + + void RunImpl() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr uint32_t RATE{400}; // 2000 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::FIFO_DATA) | DIR_READ}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Configure(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate = 0); + void ConfigureFIFOWatermark(uint8_t samples); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + bool SelfTest(); + bool NormalRead(const hrt_abstime ×tamp_sample); + bool FIFOReadTest(const hrt_abstime ×tamp_sample); + PX4Gyroscope _px4_gyro; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")}; + perf_counter_t _drdy_missed_perf{nullptr}; + + uint8_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{8}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::GYRO_RANGE, GYRO_RANGE_BIT::gyro_range_2000_dps, 0 }, + { Register::GYRO_BANDWIDTH, 0, GYRO_BANDWIDTH_BIT::gyro_bw_2000_Hz}, + { Register::GYRO_INT_CTRL, GYRO_INT_CTRL_BIT::fifo_en, 0 }, + { Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl }, + { Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 }, + { Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 }, + { Register::FIFO_CONFIG_0, 0, 0 }, // fifo_water_mark_level_trigger_retain<6:0> + { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::FIFO_MODE_STREAM, 0 }, + }; +}; + +} // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp new file mode 100644 index 0000000000..415b04811d --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +namespace Bosch::BMI088::Accelerometer +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface + +static constexpr uint32_t I2C_400_SPEED = 400 * 1000; // 400kHz I2C interface +static constexpr uint32_t I2C_200_SPEED = 200 * 1000; // 200kHz I2C interface +static constexpr uint32_t I2C_100_SPEED = 100 * 1000; // 100kHz I2C interface + +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t ID = 0x1E; + +static constexpr uint8_t ACC_I2C_ADDR_PRIMARY = 0x18; +static constexpr uint8_t ACC_I2C_ADDR_SECONDARY = 0x19; + +enum class Register : uint8_t { + ACC_CHIP_ID = 0x00, + ACC_ERR_REG = 0x02, + + TEMP_MSB = 0x22, + TEMP_LSB = 0x23, + + FIFO_LENGTH_0 = 0x24, + FIFO_LENGTH_1 = 0x25, + + FIFO_DATA = 0x26, + + ACC_CONF = 0x40, + ACC_RANGE = 0x41, + + FIFO_DOWN_SAMPLING = 0x45, + FIFO_WTM_0 = 0x46, + FIFO_WTM_1 = 0x47, + FIFO_CONFIG_0 = 0x48, + FIFO_CONFIG_1 = 0x49, + + INT1_IO_CONF = 0x53, + + INT1_INT2_MAP_DATA = 0x58, + + ACC_PWR_CONF = 0x7C, + ACC_PWR_CTRL = 0x7D, + ACC_SOFTRESET = 0x7E, + ACC_SELF_TEST = 0x6D, + ACC_I2C_ADDR_PRIMARY_REG = 0x6D, + ACC_I2C_ADDR_SECONDARY_REG = 0x19, + ACC_READ = 0x12 +}; + +// ACC_CONF +enum ACC_CONF_BIT : uint8_t { + // [7:4] acc_bwp + acc_bwp_Normal = Bit7 | Bit5, // Filter setting normal + // [7:4] acc_bwp + acc_bwp_osr_4 = Bit7, // OSR4 + // [3:0] acc_odr + acc_odr_1600 = Bit3 | Bit2, // ODR 1600 Hz + // [3:0] acc_odr + acc_odr_12_5 = Bit2 | Bit0, // ODR 12.5 Hz + // [3:0] acc_odr + acc_odr_100 = Bit3, // ODR 100 Hz +}; + +// ACC_RANGE +enum ACC_RANGE_BIT : uint8_t { + acc_range_3g = 0, // ±3g + acc_range_6g = Bit0, // ±6g + acc_range_12g = Bit1, // ±12g + acc_range_24g = Bit1 | Bit0, // ±24g +}; + +// FIFO_CONFIG_0 +enum FIFO_CONFIG_0_BIT : uint8_t { + BIT1_ALWAYS = Bit1, // This bit must always be ‘1’. + FIFO_mode = Bit0, +}; + +// FIFO_CONFIG_1 +enum FIFO_CONFIG_1_BIT : uint8_t { + Acc_en = Bit6, + BIT4_ALWAYS = Bit4, // This bit must always be ‘1’. +}; + +// INT1_IO_CONF +enum INT1_IO_CONF_BIT : uint8_t { + int1_in = Bit4, + int1_out = Bit3, + + int1_lvl = Bit1, +}; + +// INT1_INT2_MAP_DATA +enum INT1_INT2_MAP_DATA_BIT : uint8_t { + int2_drdy = Bit6, + int2_fwm = Bit5, + int2_ffull = Bit4, + + int1_drdy = Bit2, + int1_fwm = Bit1, + int1_ffull = Bit0, +}; + +// ACC_PWR_CONF +enum ACC_PWR_CONF_BIT : uint8_t { + acc_pwr_save = 0x03 +}; + +// ACC_PWR_CTRL +enum ACC_PWR_CTRL_BIT : uint8_t { + acc_enable = 0x04 +}; + +namespace FIFO +{ + + +// 1. Acceleration sensor data frame - Frame length: 7 bytes (1 byte header + 6 bytes payload) +// Payload: the next bytes contain the sensor data in the same order as defined in the register map (addresses 0x12 – 0x17). +// 2. Skip Frame - Frame length: 2 bytes (1 byte header + 1 byte payload) +// Payload: one byte containing the number of skipped frames. When more than 0xFF frames have been skipped, 0xFF is returned. +// 3. Sensortime Frame - Frame length: 4 bytes (1 byte header + 3 bytes payload) +// Payload: Sensortime (content of registers 0x18 – 0x1A), taken when the last byte of the last frame is read. + +struct DATA { + uint8_t Header; + uint8_t ACC_X_LSB; + uint8_t ACC_X_MSB; + uint8_t ACC_Y_LSB; + uint8_t ACC_Y_MSB; + uint8_t ACC_Z_LSB; + uint8_t ACC_Z_MSB; +}; +static constexpr size_t SIZE = 1024; +//static constexpr size_t SIZE = sizeof(DATA) * 10; +static_assert(sizeof(DATA) == 7); + +enum header : uint8_t { + sensor_data_frame = 0b10000100, + skip_frame = 0b01000000, + sensor_time_frame = 0b01000100, + FIFO_input_config_frame = 0b01001000, + sample_drop_frame = 0b01010000, +}; +struct bmi08x_sensor_data +{ + /*! X-axis sensor data */ + int16_t x; + + /*! Y-axis sensor data */ + int16_t y; + + /*! Z-axis sensor data */ + int16_t z; +}; +} // namespace FIFO +} // namespace Bosch::BMI088::Accelerometer diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp new file mode 100644 index 0000000000..a3b4530c1c --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +namespace Bosch::BMI088::Gyroscope +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface + +static constexpr uint32_t I2C_400_SPEED = 400 * 1000; // 400kHz I2C interface +static constexpr uint32_t I2C_200_SPEED = 200 * 1000; // 200kHz I2C interface +static constexpr uint32_t I2C_100_SPEED = 100 * 1000; // 100kHz I2C interface + +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t ID = 0x0F; + +static constexpr uint8_t GYRO_I2C_ADDR_PRIMARY = 0x68; +static constexpr uint8_t GYRO_I2C_ADDR_SECONDARY = 0x69; + +enum class Register : uint8_t { + GYRO_CHIP_ID = 0x00, + + FIFO_STATUS = 0x0E, + GYRO_RANGE = 0x0F, + GYRO_BANDWIDTH = 0x10, + + GYRO_SOFTRESET = 0x14, + GYRO_INT_CTRL = 0x15, + INT3_INT4_IO_CONF = 0x16, + INT3_INT4_IO_MAP = 0x18, + + FIFO_WM_ENABLE = 0x1E, + + FIFO_CONFIG_0 = 0x3D, + FIFO_CONFIG_1 = 0x3E, + FIFO_DATA = 0x3F, + SELF_TEST = 0x3C, + READ_GYRO = 0x02, +}; + +// FIFO_STATUS +enum FIFO_STATUS_BIT : uint8_t { + Fifo_overrun = Bit7, + Fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + +// GYRO_RANGE +enum GYRO_RANGE_BIT : uint8_t { + gyro_range_2000_dps = 0x00, // ±2000 + gyro_range_1000_dps = 0x01, // ±1000 + gyro_range_500_dps = 0x02, // ±500 + gyro_range_250_dps = 0x04, // ±250 + gyro_range_125_dps = 0x05, // ±125 +}; + +// GYRO_BANDWIDTH +enum GYRO_BANDWIDTH_BIT : uint8_t { + gyro_bw_100_Hz = Bit2 | Bit1 | Bit0, + gyro_bw_200_Hz = Bit4, + gyro_bw_2000_Hz = 0x00, +}; + +// GYRO_INT_CTRL +enum GYRO_INT_CTRL_BIT : uint8_t { + data_en = Bit7, + fifo_en = Bit6, +}; + +// INT3_INT4_IO_CONF +enum INT3_INT4_IO_CONF_BIT : uint8_t { + Int3_od = Bit1, // ‘0’ Push-pull + Int3_lvl = Bit0, // ‘0’ Active low +}; + +// INT3_INT4_IO_MAP +enum INT3_INT4_IO_MAP_BIT : uint8_t { + Int4_data = Bit7, + Int4_fifo = Bit5, + Int3_fifo = Bit2, + Int3_data = Bit0, +}; + +// FIFO_WM_ENABLE +enum FIFO_WM_ENABLE_BIT : uint8_t { + fifo_wm_enable = Bit7 | Bit3, + fifo_wm_disable = Bit3, +}; + +// FIFO_CONFIG_1 +enum FIFO_CONFIG_1_BIT : uint8_t { + FIFO_MODE = Bit6, + FIFO_MODE_STREAM = Bit7, +}; + + +namespace FIFO +{ +struct DATA { + uint8_t RATE_X_LSB; + uint8_t RATE_X_MSB; + uint8_t RATE_Y_LSB; + uint8_t RATE_Y_MSB; + uint8_t RATE_Z_LSB; + uint8_t RATE_Z_MSB; +}; +static_assert(sizeof(DATA) == 6); + +// 100 frames of data in FIFO mode +static constexpr size_t SIZE = sizeof(DATA) * 100; + +} // namespace FIFO +} // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt b/src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt new file mode 100644 index 0000000000..a86ef7637e --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2019-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__imu__bosch__bmi088_i2c + MAIN bmi088_i2c + COMPILE_FLAGS + SRCS + Bosch_BMI088_Accelerometer_Registers.hpp + Bosch_BMI088_Gyroscope_Registers.hpp + + BMI088.cpp + BMI088.hpp + BMI088_Accelerometer.cpp + BMI088_Accelerometer.hpp + BMI088_Gyroscope.cpp + BMI088_Gyroscope.hpp + + bmi088_i2c_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp new file mode 100644 index 0000000000..270292e52d --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include "BMI088.hpp" + +void BMI088::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmi088_i2c", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true); + PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmi088_i2c_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMI088; + BusCLIArguments cli{true, true}; + cli.i2c_address = 0x18; + cli.default_i2c_frequency = 400 * 1000; + cli.default_spi_frequency = 400 * 1000; + + + while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) { + switch (ch) { + case 'A': + cli.type = DRV_ACC_DEVTYPE_BMI088; + cli.i2c_address = 0x18; + break; + + case 'G': + cli.type = DRV_GYR_DEVTYPE_BMI088; + cli.i2c_address = 0x69; + break; + + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + BusInstanceIterator iterator(MODULE_NAME, cli, cli.type); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + PX4_WARN("print_usage1"); + ThisDriver::print_usage(); + return -1; +}