fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 21:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

[NEW] - bmi088 i2c drivers and crazyflie 2.1 conf

Browse Source
This commit is contained in:
TheLegendaryJedi
2021-01-20 19:51:45 +01:00
committed by Lorenz Meier
parent d7812f83f1
commit 40f971c082
41 changed files with 6686 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/bitcraze/crazyflie21/CMakeLists.txt
+34
View File
@@ -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
+64
View File
@@ -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
+13
View File
@@ -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
+19
View File
@@ -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
+7
View File
@@ -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
+14
View File
@@ -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
+223
View File
@@ -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
+190
View File
@@ -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
+152
View File
@@ -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
+47
View File
@@ -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
+233
View File
@@ -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
+39
View File
@@ -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
+173
View File
@@ -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
+103
View File
@@ -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
+99
View File
@@ -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
+62
View File
@@ -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
+108
View File
@@ -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
+47
View File
@@ -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
+77
View File
@@ -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
+79
View File
@@ -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
+418
View File
@@ -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
+179
View File
@@ -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
+153
View File
@@ -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
+141
View File
@@ -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
+162
View File
@@ -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
+865
View File
@@ -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
+216
View File
@@ -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
+170
View File
@@ -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
+72
View File
@@ -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