[NEW] - bmi088 i2c drivers and crazyflie 2.1 conf

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 <densht@gmail.com>
+#
+# @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

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

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)

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

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

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

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

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

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

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"

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 <gnutt@nuttx.org>
+ *
+ * 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) }
+}

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

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

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 <px4_arch/i2c_hw_description.h>
+
+constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
+	initI2CBusExternal(1),
+	initI2CBusInternal(3),
+};

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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/board.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/spi/spi.h>
+
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_board_led.h>
+
+#include <systemlib/px4_macros.h>
+#include <px4_platform_common/init.h>
+
+/****************************************************************************
+ * 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;
+}

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 <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * 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);
+	}
+}

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 <px4_platform_common/px4_config.h>
+#include <px4_platform/gpio.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+#include <unistd.h>
+
+#include <nuttx/spi/spi.h>
+#include <arch/board/board.h>
+#include <systemlib/px4_macros.h>
+#include <nuttx/mmcsd.h>
+
+#include <arm_arch.h>
+#include <chip.h>
+#include <stm32_gpio.h>
+#include "board_config.h"
+
+
+#include <px4_arch/spi_hw_description.h>
+#include <drivers/drv_sensor.h>
+#include <nuttx/spi/spi.h>
+
+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;
+}

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

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 <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <arm_arch.h>
+#include <stm32.h>
+#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);
+}
+

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

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

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 <px4_platform_common/defines.h>
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+
+#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

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 <string.h>
+
+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

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 <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+
+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

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 <densht@gmail.com>
+ */
+
+
+#include <px4_platform_common/defines.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <poll.h>
+#include <termios.h>
+
+
+#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;
+}

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 <stdint.h>
+
+
+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

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 <cstring>
+
+
+
+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);
+}

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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/defines.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <string.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <termios.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <mqueue.h>
+
+#include <drivers/drv_rc_input.h>
+#include <drivers/drv_board_led.h>
+
+#include <systemlib/err.h>
+
+#include <board_config.h>
+
+#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;
+}

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 <stdint.h>
+
+#include <battery/battery.h>
+
+#include <drivers/device/device.h>
+#include "ringbuffer.h"
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/input_rc.h>
+
+#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<input_rc_s>		_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();
+};

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 <cstring>
+
+
+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);
+}

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 <densht@gmail.com>
+ */
+
+/**
+ * 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

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

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 <drivers/drv_hrt.h>
+#include <lib/drivers/device/i2c.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
+
+class BMI088 : public device::I2C, public I2CSPIDriver<BMI088>
+{
+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<uint32_t> _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
+
+};

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 <ecl/geo/geo.h> // 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<uint8_t>(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<uint8_t>(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 &reg_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 &reg_cfg : _register_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	ConfigureAccel();
+
+	return success;
+}
+
+int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<BMI088_Accelerometer *>(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 &reg_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<uint8_t>(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 &timestamp_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 &timestamp_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<uint8_t>(Register::FIFO_LENGTH_0)};
+	data_i[0] = static_cast<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(Register::FIFO_LENGTH_0)};
+	data_i[0] = static_cast<uint8_t>(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<uint8_t>(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 &timestamp_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<uint8_t>(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 &timestamp_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<uint8_t>(Register::FIFO_LENGTH_0)};
+	data_i[0] = static_cast<uint8_t>(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<uint8_t>(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

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 <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+
+#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<uint8_t>(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 &reg_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 &timestamp_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 &timestamp_sample);
+	bool NormalRead(const hrt_abstime &timestamp_sample);
+	bool FIFOReadTest(const hrt_abstime &timestamp_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<uint8_t>(_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

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 <px4_platform/board_dma_alloc.h>
+
+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 &reg_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 &reg_cfg : _register_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	ConfigureGyro();
+
+	return success;
+}
+
+int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<BMI088_Gyroscope *>(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 &reg_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<uint8_t>(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<uint8_t>(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 &timestamp_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 &timestamp_sample) {
+	float x = 0;
+	float y = 0;
+	float z = 0;
+	uint8_t buffer[6] = {0};
+	uint8_t cmd[1] = {static_cast<uint8_t>(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 &timestamp_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<uint8_t>(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<uint8_t>(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

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 <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+
+#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<uint8_t>(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 &reg_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 &timestamp_sample, uint8_t samples);
+	void FIFOReset();
+
+	bool SelfTest();
+	bool NormalRead(const hrt_abstime &timestamp_sample);
+	bool FIFOReadTest(const hrt_abstime &timestamp_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<uint8_t>(_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

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

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

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

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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#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;
+}