Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 08:22:43 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #2481 from paparazzi/crazyflie_2.1-integration

Browse Source 
Crazyflie 2.1 integration
This commit is contained in:
Gautier Hattenberger
2020-01-13 23:32:22 +01:00
committed by GitHub
parent c5a122ec65 f646737d61
commit 3c411d6cf0
38 changed files with 5697 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
+219
View File
@@ -0,0 +1,219 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<!-- Crazyflie 2.1 -->
<airframe name="Crazyflie2">
<firmware name="rotorcraft">
<configure name="PERIODIC_FREQUENCY" value="1000"/>
<configure name="RTOS_DEBUG" value="0"/>
<target name="ap" board="crazyflie_2.1">
<module name="gps" type="datalink"/>
</target>
<target name="nps" board="pc">
<module name="gps" type="ublox"/>
<module name="fdm" type="jsbsim"/>
</target>
<module name="motor_mixing"/>
<module name="actuators" type="pwm"/>
<module name="syslink_dl"/>
<module name="radio_control" type="datalink"/>
<module name="telemetry" type="transparent"/>
<module name="imu" type="bmi088_i2c">
<configure name="IMU_BMI088_I2C_DEV" value="i2c3"/>
<define name="IMU_BMI088_GYRO_I2C_ADDR" value="BMI088_GYRO_ADDR_ALT"/>
<define name="IMU_BMI088_Y_SIGN" value="-1"/>
<define name="IMU_BMI088_Z_SIGN" value="-1"/>
</module>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="madgwick">
<configure name="AHRS_PROPAGATE_FREQUENCY" value="1000"/>
<!--configure name="USE_MAGNETOMETER" value="FALSE"/-->
</module>
<module name="ins"/>
<module name="sys_mon"/>
<module name="air_data"/>
<module name="baro_bmp3">
<configure name="BMP3_I2C_DEV" value="i2c3"/>
<define name="AIR_DATA_BARO_ABS_ID" value="BARO_BMP3_SENDER_ID"/>
<define name="BMP3_SLAVE_ADDR" value="BMP3_I2C_ADDR_ALT"/>
</module>
</firmware>
<firmware name="test_chibios">
<target name="test_sys_time_timer" board="crazyflie_2.1"/>
<target name="test_led" board="crazyflie_2.1"/>
<target name="test_sys_gpio" board="crazyflie_2.1"/>
</firmware>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<servos driver="Pwm">
<servo name="FRONT_LEFT" no="4" min="0" neutral="20" max="255"/>
<servo name="FRONT_RIGHT" no="1" min="0" neutral="20" max="255"/>
<servo name="BACK_RIGHT" no="2" min="0" neutral="20" max="255"/>
<servo name="BACK_LEFT" no="3" min="0" neutral="20" max="255"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TYPE" value="QUAD_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot.motors_on,FALSE,values)"/>
<set servo="FRONT_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="FRONT_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BACK_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BACK_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="-19"/>
<define name="ACCEL_Y_NEUTRAL" value="-12"/>
<define name="ACCEL_Z_NEUTRAL" value="58"/>
<define name="ACCEL_X_SENS" value="1.83754430011" integer="16"/>
<define name="ACCEL_Y_SENS" value="1.8445793586" integer="16"/>
<define name="ACCEL_Z_SENS" value="1.84794644385" integer="16"/>
<!-- replace this with your own calibration -->
<!--define name="MAG_X_NEUTRAL" value="-179"/>
<define name="MAG_Y_NEUTRAL" value="-21"/>
<define name="MAG_Z_NEUTRAL" value="79"/>
<define name="MAG_X_SENS" value="4.17334785618" integer="16"/>
<define name="MAG_Y_SENS" value="3.98885954135" integer="16"/>
<define name="MAG_Z_SENS" value="4.40442339014" integer="16"/-->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
<!-- Use GPS heading instead of magneto -->
<define name="USE_GPS_HEADING" value="1"/>
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="250" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="300"/>
<define name="PHI_DGAIN" value="200"/>
<define name="PHI_IGAIN" value="100"/>
<define name="THETA_PGAIN" value="300"/>
<define name="THETA_DGAIN" value="200"/>
<define name="THETA_IGAIN" value="100"/>
<define name="PSI_PGAIN" value="400"/>
<define name="PSI_DGAIN" value="200"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive estimation -->
<!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="70"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="3.1" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.6" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.2" unit="V"/>
</section>
</airframe>
conf/boards/crazyflie_2.1.makefile
+87
View File
@@ -0,0 +1,87 @@
# Hey Emacs, this is a -*- makefile -*-
#
# crazyflie_2.1.makefile
#
# https://www.bitcraze.io/crazyflie-2-1/
#
# based on STM32F4
# only compatible with PPRZ/ChibiOS
#
BOARD=crazyflie
BOARD_VERSION=2.1
BOARD_DIR=$(BOARD)/chibios/v$(BOARD_VERSION)
BOARD_CFG=\"boards/$(BOARD_DIR)/$(BOARD).h\"
ARCH=chibios
## FPU on F4
USE_FPU=softfp
$(TARGET).CFLAGS += -DSTM32F4 -DPPRZLINK_ENABLE_FD -DUSE_HARD_FAULT_RECOVERY
##############################################################################
# Architecture or project specific options
#
# Define project name here (target)
PROJECT = $(TARGET)
# Project specific files and paths (see Makefile.chibios for details)
CHIBIOS_BOARD_PLATFORM = STM32F4xx/platform.mk
CHIBIOS_BOARD_LINKER = STM32F405xG.ld
CHIBIOS_BOARD_STARTUP = startup_stm32f4xx.mk
##############################################################################
# Compiler settings
#
MCU = cortex-m4
# -----------------------------------------------------------------------
# default flash mode is via usb dfu bootloader (luftboot)
# other possibilities: DFU-UTIL, SWD, JTAG_BMP, STLINK, SERIAL
FLASH_MODE ?= DFU-UTIL
# while the crazyflie doesn't have luftboot, it has a bootloader until same offset
# see also https://wiki.bitcraze.io/projects:crazyflie2:development:dfu
HAS_LUFTBOOT ?= 1
ifeq (,$(findstring $(HAS_LUFTBOOT),0 FALSE))
$(TARGET).CFLAGS+=-DLUFTBOOT
$(TARGET).LDFLAGS+=-Wl,-Ttext=0x8004000
DFU_ADDR = 0x8004000
endif
#
# default LED configuration
#
RADIO_CONTROL_LED ?= 3
BARO_LED ?= none
AHRS_ALIGNER_LED ?= 2
GPS_LED ?= 4
SYS_TIME_LED ?= 1
CHARGING_LED ?= 5
#
# default uart configuration
#
# Over NRF/Syslink
MODEM_PORT ?= syslink
# External port
#MODEM_PORT ?= UART3
#MODEM_BAUD ?= B57600
#
# default actuator configuration
#
# you can use different actuators by adding a configure option to your firmware section
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
conf/flash_modes.xml
+3
View File
@@ -61,6 +61,7 @@
<board name="openpilot_revo.*"/>
<board name="chimera_.*"/>
<board name="tawaki_.*"/>
<board name="crazyflie_.*"/>
</boards>
</mode>
<mode name="STLink (SWD)">
@@ -75,6 +76,7 @@
<board name="openpilot_revo.*"/>
<board name="chimera_.*"/>
<board name="tawaki_.*"/>
<board name="crazyflie_.*"/>
</boards>
</mode>
<mode name="BlackMagic Probe (SWD)">
@@ -95,6 +97,7 @@
<board name="chimera_.*"/>
<board name="tawaki_.*"/>
<board name="vms_ecu_.*"/>
<board name="crazyflie_.*"/>
</boards>
</mode>
<mode name="BlackMagic Probe (SWD_NOPWR)">
conf/modules/ahrs_madgwick.xml
+54
View File
@@ -0,0 +1,54 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ahrs_madgwick" dir="ahrs">
<doc>
<description>
AHRS using IMU (accel, gyro) only with Madwick implementation.
</description>
<configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
<define name="AHRS_MADWICK_IMU_ID" value="ABI_BROADCAST" description="ABI sender id of IMU to use"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="madwick">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ahrs_madgwick.reset" shortname="reset"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="ahrs.h" dir="subsystems"/>
</header>
<makefile target="!sim|fbw">
<configure name="USE_MAGNETOMETER" default="0"/>
<define name="USE_MAGNETOMETER" cond="ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))"/>
<define name="AHRS_ALIGNER_LED" value="$(AHRS_ALIGNER_LED)" cond="ifneq ($(AHRS_ALIGNER_LED),none)"/>
<define name="USE_AHRS"/>
<define name="USE_AHRS_ALIGNER"/>
<file name="ahrs.c" dir="subsystems"/>
<file name="ahrs_aligner.c" dir="subsystems/ahrs"/>
<file name="ahrs_madgwick.c"/>
<file name="ahrs_madgwick_wrapper.c"/>
<raw>
ifdef SECONDARY_AHRS
ifneq (,$(findstring $(SECONDARY_AHRS), madwick))
# this is the secondary AHRS
$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"modules/ahrs/ahrs_madgwick_wrapper.h\"
$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_madgwick
else
# this is the primary AHRS
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_madgwick_wrapper.h\"
$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_madgwick
endif
else
# plain old single AHRS usage
$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"modules/ahrs/ahrs_madgwick_wrapper.h\"
endif
</raw>
</makefile>
</module>
conf/modules/imu_bmi088_i2c.xml
+41
View File
@@ -0,0 +1,41 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_bmi088_i2c" dir="imu">
<doc>
<description>
IMU with BMI088 via I2C.
</description>
<configure name="IMU_BMI088_I2C_DEV" value="i2c3" description="I2C device to use for BMI088"/>
<define name="IMU_BMI088_GYRO_RANGE" value="BMI088_GYRO_RANGE_1000" description="gyroscope range setting"/>
<define name="IMU_BMI088_GYRO_ODR" value="BMI088_GYRO_ODR_400_BW_47" description="gyroscope output data rate"/>
<define name="IMU_BMI088_GYRO_I2C_ADDR" value="BMI088_GYRO_ADDR|BMI088_GYRO_ADDR_ALT" description="I2C address of the gyro"/>
<define name="IMU_BMI088_ACCEL_RANGE" value="BMI088_ACCEL_RANGE_6G" description="accelerometer range setting"/>
<define name="IMU_BMI088_ACCEL_ODR" value="BMI088_ACCEL_ODR_400" description="accelerometer output data rate"/>
<define name="IMU_BMI088_ACCEL_I2C_ADDR" value="BMI088_ACCEL_ADDR|BMI088_ACCEL_ADDR_ALT" description="I2C address of the accelerometer"/>
<define name="IMU_BMI088_CHAN_X" value="0" description="channel index"/>
<define name="IMU_BMI088_CHAN_Y" value="1" description="channel index"/>
<define name="IMU_BMI088_CHAN_Z" value="2" description="channel index"/>
</doc>
<autoload name="imu_common"/>
<autoload name="imu_nps"/>
<header>
<file name="imu_bmi088_i2c.h"/>
</header>
<init fun="imu_bmi088_init()"/>
<periodic fun="imu_bmi088_periodic()"/>
<event fun="imu_bmi088_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="IMU_BMI088_I2C_DEV" default="i2c3" case="lower|upper"/>
<define name="IMU_BMI088_I2C_DEV" value="$(IMU_BMI088_I2C_DEV_LOWER)"/>
<define name="USE_$(IMU_BMI088_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_bmi088_i2c.h" type="string"/>
<file name="bmi088.c" dir="peripherals"/>
<file name="bmi088_i2c.c" dir="peripherals"/>
<file name="imu_bmi088_i2c.c"/>
</makefile>
</module>
conf/modules/syslink_dl.xml
+32
View File
@@ -0,0 +1,32 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="syslink" dir="datalink/bitcraze" task="datalink">
<doc>
<description>
Bitcraze syslink module
</description>
<configure name="SYSLINK_PORT" value="UARTX" description="Select syslink port"/>
<configure name="SYSLINK_BAUD" value="B1000000" description="Syslink baudrate"/>
</doc>
<autoload name="telemetry" type="nps"/>
<autoload name="telemetry" type="sim"/>
<header>
<file name="syslink_dl.h"/>
</header>
<init fun="syslink_dl_init()"/>
<periodic fun="syslink_dl_periodic()" freq="2"/>
<event fun="syslink_dl_event()"/>
<makefile target="!fbw|sim|nps|hitl">
<configure name="SYSLINK_PORT" default="uart6" case="upper|lower"/>
<configure name="SYSLINK_BAUD" default="B1000000" case="upper|lower"/>
<define name="SYSLINK_DEV" value="$(SYSLINK_PORT_LOWER)"/>
<define name="USE_$(SYSLINK_PORT_UPPER)"/>
<define name="$(SYSLINK_PORT_UPPER)_BAUD" value="$(SYSLINK_BAUD)"/>
<define name="USE_UART_SOFT_FLOW_CONTROL"/>
<define name="USE_SYSLINK"/>
<define name="CHARGING_LED" value="$(CHARGING_LED)" cond="ifneq ($(CHARGING_LED),none)"/>
<file name="syslink_dl.c"/>
<file name="syslink.c"/>
</makefile>
</module>
conf/tools/crazyradio2ivy.xml
+2
View File
@@ -0,0 +1,2 @@
<program command="sw/ground_segment/python/bitcraze/crazyradio2ivy.py" name="Crazyradio/IVY bridge" />
sw/airborne/arch/chibios/STM32F405xG.ld
+86
View File
@@ -0,0 +1,86 @@
/*
ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
/*
* STM32F405xG memory setup.
* Note: Use of ram1 and ram2 is mutually exclusive with use of ram0.
*/
MEMORY
{
flash0 : org = 0x08004000, len = 1M
flash1 : org = 0x00000000, len = 0
flash2 : org = 0x00000000, len = 0
flash3 : org = 0x00000000, len = 0
flash4 : org = 0x00000000, len = 0
flash5 : org = 0x00000000, len = 0
flash6 : org = 0x00000000, len = 0
flash7 : org = 0x00000000, len = 0
ram0 : org = 0x20000000, len = 128k /* SRAM1 + SRAM2 */
ram1 : org = 0x20000000, len = 112k /* SRAM1 */
ram2 : org = 0x2001C000, len = 16k /* SRAM2 */
ram3 : org = 0x00000000, len = 0
ram4 : org = 0x10000000, len = 64k /* CCM SRAM */
ram5 : org = 0x40024000, len = 4k /* BCKP SRAM */
ram6 : org = 0x00000000, len = 0
ram7 : org = 0x00000000, len = 0
}
/* For each data/text section two region are defined, a virtual region
and a load region (_LMA suffix).*/
/* Flash region to be used for exception vectors.*/
REGION_ALIAS("VECTORS_FLASH", flash0);
REGION_ALIAS("VECTORS_FLASH_LMA", flash0);
/* Flash region to be used for constructors and destructors.*/
REGION_ALIAS("XTORS_FLASH", flash0);
REGION_ALIAS("XTORS_FLASH_LMA", flash0);
/* Flash region to be used for code text.*/
REGION_ALIAS("TEXT_FLASH", flash0);
REGION_ALIAS("TEXT_FLASH_LMA", flash0);
/* Flash region to be used for read only data.*/
REGION_ALIAS("RODATA_FLASH", flash0);
REGION_ALIAS("RODATA_FLASH_LMA", flash0);
/* Flash region to be used for various.*/
REGION_ALIAS("VARIOUS_FLASH", flash0);
REGION_ALIAS("VARIOUS_FLASH_LMA", flash0);
/* Flash region to be used for RAM(n) initialization data.*/
REGION_ALIAS("RAM_INIT_FLASH_LMA", flash0);
/* RAM region to be used for Main stack. This stack accommodates the processing
of all exceptions and interrupts.*/
REGION_ALIAS("MAIN_STACK_RAM", ram0);
/* RAM region to be used for the process stack. This is the stack used by
the main() function.*/
REGION_ALIAS("PROCESS_STACK_RAM", ram0);
/* RAM region to be used for data segment.*/
REGION_ALIAS("DATA_RAM", ram0);
REGION_ALIAS("DATA_RAM_LMA", flash0);
/* RAM region to be used for BSS segment.*/
REGION_ALIAS("BSS_RAM", ram0);
/* RAM region to be used for the default heap.*/
REGION_ALIAS("HEAP_RAM", ram0);
/* Generic rules inclusion.*/
INCLUDE rules.ld
sw/airborne/arch/chibios/mcu_periph/uart_arch.c
+48 -26
View File
@@ -51,8 +51,12 @@ struct SerialInit {
semaphore_t *tx_sem;
mutex_t *rx_mtx;
mutex_t *tx_mtx;
ioportid_t cts_port;
uint16_t cts_pin;
};
#define SERIAL_INIT_NULL { NULL, NULL, NULL, NULL, NULL, 0, 0 }
/**
* RX handler
*/
@@ -80,19 +84,32 @@ static void handle_uart_rx(struct uart_periph *p)
static void handle_uart_tx(struct uart_periph *p)
{
// check if more data to send
// TODO send by block with sdWrite (be careful with circular buffer)
// not compatible with soft flow control
struct SerialInit *init_struct = (struct SerialInit *)(p->init_struct);
chSemWait(init_struct->tx_sem);
p->tx_running = true;
while (p->tx_insert_idx != p->tx_extract_idx) {
#if USE_UART_SOFT_FLOW_CONTROL
if (init_struct->cts_port != 0) {
// wait for CTS line to be set to send next byte
while (gpio_get(init_struct->cts_port, init_struct->cts_pin) == 1) ;
}
#endif
uint8_t data = p->tx_buf[p->tx_extract_idx];
p->tx_running = true;
sdWrite((SerialDriver *)p->reg_addr, &data, sizeof(data));
p->tx_running = false;
// TODO send by block (be careful with circular buffer)
sdPut((SerialDriver *)p->reg_addr, data);
chMtxLock(init_struct->tx_mtx);
p->tx_extract_idx++;
p->tx_extract_idx %= UART_TX_BUFFER_SIZE;
chMtxUnlock(init_struct->tx_mtx);
#if USE_UART_SOFT_FLOW_CONTROL
if (init_struct->cts_port != 0) {
// wait for physical transfer to be completed
while ((((SerialDriver *)p->reg_addr)->usart->SR & USART_SR_TC) == 0) ;
}
#endif
}
p->tx_running = false;
}
#if USE_UART1
@@ -116,7 +133,7 @@ static SerialConfig usart1_config = {
0 /* USART CR3 */
};
static struct SerialInit uart1_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart1_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART1_RX
@@ -218,7 +235,7 @@ static SerialConfig usart2_config = {
#endif
};
static struct SerialInit uart2_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart2_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART2_RX
@@ -277,13 +294,13 @@ void uart2_init(void)
uart2_init_struct.rx_mtx = &uart2_rx_mtx;
uart2_init_struct.rx_sem = &uart2_rx_sem;
chThdCreateStatic(wa_thd_uart2_rx, sizeof(wa_thd_uart2_rx),
NORMALPRIO, thd_uart2_rx, NULL);
NORMALPRIO + 1, thd_uart2_rx, NULL);
#endif
#if USE_UART2_TX
uart2_init_struct.tx_mtx = &uart2_tx_mtx;
uart2_init_struct.tx_sem = &uart2_tx_sem;
chThdCreateStatic(wa_thd_uart2_tx, sizeof(wa_thd_uart2_tx),
NORMALPRIO, thd_uart2_tx, NULL);
NORMALPRIO + 1, thd_uart2_tx, NULL);
#endif
}
@@ -310,7 +327,7 @@ static SerialConfig usart3_config = {
0 /* USART CR3 */
};
static struct SerialInit uart3_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart3_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART3_RX
@@ -369,13 +386,13 @@ void uart3_init(void)
uart3_init_struct.rx_mtx = &uart3_rx_mtx;
uart3_init_struct.rx_sem = &uart3_rx_sem;
chThdCreateStatic(wa_thd_uart3_rx, sizeof(wa_thd_uart3_rx),
NORMALPRIO, thd_uart3_rx, NULL);
NORMALPRIO + 1, thd_uart3_rx, NULL);
#endif
#if USE_UART3_TX
uart3_init_struct.tx_mtx = &uart3_tx_mtx;
uart3_init_struct.tx_sem = &uart3_tx_sem;
chThdCreateStatic(wa_thd_uart3_tx, sizeof(wa_thd_uart3_tx),
NORMALPRIO, thd_uart3_tx, NULL);
NORMALPRIO + 1, thd_uart3_tx, NULL);
#endif
}
@@ -402,7 +419,7 @@ static SerialConfig usart4_config = {
0 /* USART CR3 */
};
static struct SerialInit uart4_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart4_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART4_RX
@@ -461,13 +478,13 @@ void uart4_init(void)
uart4_init_struct.rx_mtx = &uart4_rx_mtx;
uart4_init_struct.rx_sem = &uart4_rx_sem;
chThdCreateStatic(wa_thd_uart4_rx, sizeof(wa_thd_uart4_rx),
NORMALPRIO, thd_uart4_rx, NULL);
NORMALPRIO + 1, thd_uart4_rx, NULL);
#endif
#if USE_UART4_TX
uart4_init_struct.tx_mtx = &uart4_tx_mtx;
uart4_init_struct.tx_sem = &uart4_tx_sem;
chThdCreateStatic(wa_thd_uart4_tx, sizeof(wa_thd_uart4_tx),
NORMALPRIO, thd_uart4_tx, NULL);
NORMALPRIO + 1, thd_uart4_tx, NULL);
#endif
}
@@ -494,7 +511,7 @@ static SerialConfig usart5_config = {
0 /* USART CR3 */
};
static struct SerialInit uart5_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart5_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART5_RX
@@ -553,13 +570,13 @@ void uart5_init(void)
uart5_init_struct.rx_mtx = &uart5_rx_mtx;
uart5_init_struct.rx_sem = &uart5_rx_sem;
chThdCreateStatic(wa_thd_uart5_rx, sizeof(wa_thd_uart5_rx),
NORMALPRIO, thd_uart5_rx, NULL);
NORMALPRIO + 1, thd_uart5_rx, NULL);
#endif
#if USE_UART5_TX
uart5_init_struct.tx_mtx = &uart5_tx_mtx;
uart5_init_struct.tx_sem = &uart5_tx_sem;
chThdCreateStatic(wa_thd_uart5_tx, sizeof(wa_thd_uart5_tx),
NORMALPRIO, thd_uart5_tx, NULL);
NORMALPRIO + 1, thd_uart5_tx, NULL);
#endif
}
@@ -586,7 +603,7 @@ static SerialConfig usart6_config = {
0 /* USART CR3 */
};
static struct SerialInit uart6_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart6_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART6_RX
@@ -645,13 +662,18 @@ void uart6_init(void)
uart6_init_struct.rx_mtx = &uart6_rx_mtx;
uart6_init_struct.rx_sem = &uart6_rx_sem;
chThdCreateStatic(wa_thd_uart6_rx, sizeof(wa_thd_uart6_rx),
NORMALPRIO, thd_uart6_rx, NULL);
NORMALPRIO + 1, thd_uart6_rx, NULL);
#endif
#if USE_UART6_TX
uart6_init_struct.tx_mtx = &uart6_tx_mtx;
uart6_init_struct.tx_sem = &uart6_tx_sem;
chThdCreateStatic(wa_thd_uart6_tx, sizeof(wa_thd_uart6_tx),
NORMALPRIO, thd_uart6_tx, NULL);
NORMALPRIO + 1, thd_uart6_tx, NULL);
#endif
#if defined UART6_GPIO_CTS && defined UART6_GPIO_PORT_CTS
uart6_init_struct.cts_pin = UART6_GPIO_CTS;
uart6_init_struct.cts_port = UART6_GPIO_PORT_CTS;
#endif
}
@@ -678,7 +700,7 @@ static SerialConfig usart7_config = {
0 /* USART CR3 */
};
static struct SerialInit uart7_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart7_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART7_RX
@@ -737,13 +759,13 @@ void uart7_init(void)
uart7_init_struct.rx_mtx = &uart7_rx_mtx;
uart7_init_struct.rx_sem = &uart7_rx_sem;
chThdCreateStatic(wa_thd_uart7_rx, sizeof(wa_thd_uart7_rx),
NORMALPRIO, thd_uart7_rx, NULL);
NORMALPRIO + 1, thd_uart7_rx, NULL);
#endif
#if USE_UART7_TX
uart7_init_struct.tx_mtx = &uart7_tx_mtx;
uart7_init_struct.tx_sem = &uart7_tx_sem;
chThdCreateStatic(wa_thd_uart7_tx, sizeof(wa_thd_uart7_tx),
NORMALPRIO, thd_uart7_tx, NULL);
NORMALPRIO + 1, thd_uart7_tx, NULL);
#endif
}
@@ -770,7 +792,7 @@ static SerialConfig usart8_config = {
0 /* USART CR3 */
};
static struct SerialInit uart8_init_struct = { NULL, NULL, NULL, NULL, NULL };
static struct SerialInit uart8_init_struct = SERIAL_INIT_NULL;
// Threads RX and TX
#if USE_UART8_RX
@@ -829,13 +851,13 @@ void uart8_init(void)
uart8_init_struct.rx_mtx = &uart8_rx_mtx;
uart8_init_struct.rx_sem = &uart8_rx_sem;
chThdCreateStatic(wa_thd_uart8_rx, sizeof(wa_thd_uart8_rx),
NORMALPRIO, thd_uart8_rx, NULL);
NORMALPRIO + 1, thd_uart8_rx, NULL);
#endif
#if USE_UART8_TX
uart8_init_struct.tx_mtx = &uart8_tx_mtx;
uart8_init_struct.tx_sem = &uart8_tx_sem;
chThdCreateStatic(wa_thd_uart8_tx, sizeof(wa_thd_uart8_tx),
NORMALPRIO, thd_uart8_tx, NULL);
NORMALPRIO + 1, thd_uart8_tx, NULL);
#endif
}
sw/airborne/arch/chibios/mcu_periph/uart_arch.h
+1
View File
@@ -48,6 +48,7 @@
#define B115200 115200
#define B230400 230400
#define B921600 921600
#define B1000000 1000000
#define B1500000 1500000
#define B3000000 3000000
#define UART_SPEED(_def) _def
sw/airborne/arch/chibios/subsystems/actuators/actuators_pwm_arch.c
+1 -1
View File
@@ -44,7 +44,7 @@
* in milliseconds to microseconds (required by pwmEnableChannel())
*/
#ifndef PWM_CMD_TO_US
#define PWM_CMD_TO_US(_t) (1000000 * _t / PWM_FREQUENCY)
#define PWM_CMD_TO_US(_t) (PWM_FREQUENCY * _t / 1000000)
#endif
int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
sw/airborne/boards/crazyflie/chibios/v2.1/board.c
+263
View File
@@ -0,0 +1,263 @@
/*
ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
/*
* This file has been automatically generated using ChibiStudio board
* generator plugin. Do not edit manually.
*/
#include "hal.h"
#include "stm32_gpio.h"
/*===========================================================================*/
/* Driver local definitions. */
/*===========================================================================*/
/*===========================================================================*/
/* Driver exported variables. */
/*===========================================================================*/
/*===========================================================================*/
/* Driver local variables and types. */
/*===========================================================================*/
/**
* @brief Type of STM32 GPIO port setup.
*/
typedef struct {
uint32_t moder;
uint32_t otyper;
uint32_t ospeedr;
uint32_t pupdr;
uint32_t odr;
uint32_t afrl;
uint32_t afrh;
} gpio_setup_t;
/**
* @brief Type of STM32 GPIO initialization data.
*/
typedef struct {
#if STM32_HAS_GPIOA || defined(__DOXYGEN__)
gpio_setup_t PAData;
#endif
#if STM32_HAS_GPIOB || defined(__DOXYGEN__)
gpio_setup_t PBData;
#endif
#if STM32_HAS_GPIOC || defined(__DOXYGEN__)
gpio_setup_t PCData;
#endif
#if STM32_HAS_GPIOD || defined(__DOXYGEN__)
gpio_setup_t PDData;
#endif
#if STM32_HAS_GPIOE || defined(__DOXYGEN__)
gpio_setup_t PEData;
#endif
#if STM32_HAS_GPIOF || defined(__DOXYGEN__)
gpio_setup_t PFData;
#endif
#if STM32_HAS_GPIOG || defined(__DOXYGEN__)
gpio_setup_t PGData;
#endif
#if STM32_HAS_GPIOH || defined(__DOXYGEN__)
gpio_setup_t PHData;
#endif
#if STM32_HAS_GPIOI || defined(__DOXYGEN__)
gpio_setup_t PIData;
#endif
#if STM32_HAS_GPIOJ || defined(__DOXYGEN__)
gpio_setup_t PJData;
#endif
#if STM32_HAS_GPIOK || defined(__DOXYGEN__)
gpio_setup_t PKData;
#endif
} gpio_config_t;
/**
* @brief STM32 GPIO static initialization data.
*/
static const gpio_config_t gpio_default_config = {
#if STM32_HAS_GPIOA
{VAL_GPIOA_MODER, VAL_GPIOA_OTYPER, VAL_GPIOA_OSPEEDR, VAL_GPIOA_PUPDR,
VAL_GPIOA_ODR, VAL_GPIOA_AFRL, VAL_GPIOA_AFRH},
#endif
#if STM32_HAS_GPIOB
{VAL_GPIOB_MODER, VAL_GPIOB_OTYPER, VAL_GPIOB_OSPEEDR, VAL_GPIOB_PUPDR,
VAL_GPIOB_ODR, VAL_GPIOB_AFRL, VAL_GPIOB_AFRH},
#endif
#if STM32_HAS_GPIOC
{VAL_GPIOC_MODER, VAL_GPIOC_OTYPER, VAL_GPIOC_OSPEEDR, VAL_GPIOC_PUPDR,
VAL_GPIOC_ODR, VAL_GPIOC_AFRL, VAL_GPIOC_AFRH},
#endif
#if STM32_HAS_GPIOD
{VAL_GPIOD_MODER, VAL_GPIOD_OTYPER, VAL_GPIOD_OSPEEDR, VAL_GPIOD_PUPDR,
VAL_GPIOD_ODR, VAL_GPIOD_AFRL, VAL_GPIOD_AFRH},
#endif
#if STM32_HAS_GPIOE
{VAL_GPIOE_MODER, VAL_GPIOE_OTYPER, VAL_GPIOE_OSPEEDR, VAL_GPIOE_PUPDR,
VAL_GPIOE_ODR, VAL_GPIOE_AFRL, VAL_GPIOE_AFRH},
#endif
#if STM32_HAS_GPIOF
{VAL_GPIOF_MODER, VAL_GPIOF_OTYPER, VAL_GPIOF_OSPEEDR, VAL_GPIOF_PUPDR,
VAL_GPIOF_ODR, VAL_GPIOF_AFRL, VAL_GPIOF_AFRH},
#endif
#if STM32_HAS_GPIOG
{VAL_GPIOG_MODER, VAL_GPIOG_OTYPER, VAL_GPIOG_OSPEEDR, VAL_GPIOG_PUPDR,
VAL_GPIOG_ODR, VAL_GPIOG_AFRL, VAL_GPIOG_AFRH},
#endif
#if STM32_HAS_GPIOH
{VAL_GPIOH_MODER, VAL_GPIOH_OTYPER, VAL_GPIOH_OSPEEDR, VAL_GPIOH_PUPDR,
VAL_GPIOH_ODR, VAL_GPIOH_AFRL, VAL_GPIOH_AFRH},
#endif
#if STM32_HAS_GPIOI
{VAL_GPIOI_MODER, VAL_GPIOI_OTYPER, VAL_GPIOI_OSPEEDR, VAL_GPIOI_PUPDR,
VAL_GPIOI_ODR, VAL_GPIOI_AFRL, VAL_GPIOI_AFRH},
#endif
#if STM32_HAS_GPIOJ
{VAL_GPIOJ_MODER, VAL_GPIOJ_OTYPER, VAL_GPIOJ_OSPEEDR, VAL_GPIOJ_PUPDR,
VAL_GPIOJ_ODR, VAL_GPIOJ_AFRL, VAL_GPIOJ_AFRH},
#endif
#if STM32_HAS_GPIOK
{VAL_GPIOK_MODER, VAL_GPIOK_OTYPER, VAL_GPIOK_OSPEEDR, VAL_GPIOK_PUPDR,
VAL_GPIOK_ODR, VAL_GPIOK_AFRL, VAL_GPIOK_AFRH}
#endif
};
/*===========================================================================*/
/* Driver local functions. */
/*===========================================================================*/
static void gpio_init(stm32_gpio_t *gpiop, const gpio_setup_t *config) {
gpiop->OTYPER = config->otyper;
gpiop->OSPEEDR = config->ospeedr;
gpiop->PUPDR = config->pupdr;
gpiop->ODR = config->odr;
gpiop->AFRL = config->afrl;
gpiop->AFRH = config->afrh;
gpiop->MODER = config->moder;
}
static void stm32_gpio_init(void) {
/* Enabling GPIO-related clocks, the mask comes from the
registry header file.*/
rccResetAHB1(STM32_GPIO_EN_MASK);
rccEnableAHB1(STM32_GPIO_EN_MASK, true);
/* Initializing all the defined GPIO ports.*/
#if STM32_HAS_GPIOA
gpio_init(GPIOA, &gpio_default_config.PAData);
#endif
#if STM32_HAS_GPIOB
gpio_init(GPIOB, &gpio_default_config.PBData);
#endif
#if STM32_HAS_GPIOC
gpio_init(GPIOC, &gpio_default_config.PCData);
#endif
#if STM32_HAS_GPIOD
gpio_init(GPIOD, &gpio_default_config.PDData);
#endif
#if STM32_HAS_GPIOE
gpio_init(GPIOE, &gpio_default_config.PEData);
#endif
#if STM32_HAS_GPIOF
gpio_init(GPIOF, &gpio_default_config.PFData);
#endif
#if STM32_HAS_GPIOG
gpio_init(GPIOG, &gpio_default_config.PGData);
#endif
#if STM32_HAS_GPIOH
gpio_init(GPIOH, &gpio_default_config.PHData);
#endif
#if STM32_HAS_GPIOI
gpio_init(GPIOI, &gpio_default_config.PIData);
#endif
#if STM32_HAS_GPIOJ
gpio_init(GPIOJ, &gpio_default_config.PJData);
#endif
#if STM32_HAS_GPIOK
gpio_init(GPIOK, &gpio_default_config.PKData);
#endif
}
/*===========================================================================*/
/* Driver interrupt handlers. */
/*===========================================================================*/
/*===========================================================================*/
/* Driver exported functions. */
/*===========================================================================*/
/**
* @brief Early initialization code.
* @details GPIO ports and system clocks are initialized before everything
* else.
*/
void __early_init(void) {
stm32_gpio_init();
stm32_clock_init();
}
#if HAL_USE_SDC || defined(__DOXYGEN__)
/**
* @brief SDC card detection.
*/
bool sdc_lld_is_card_inserted(SDCDriver *sdcp) {
(void)sdcp;
return !palReadLine(LINE_SDIO_DETECT);
}
/**
* @brief SDC card write protection detection.
*/
bool sdc_lld_is_write_protected(SDCDriver *sdcp) {
(void)sdcp;
return false;
}
#endif /* HAL_USE_SDC */
#if HAL_USE_MMC_SPI || defined(__DOXYGEN__)
/**
* @brief MMC_SPI card detection.
*/
bool mmc_lld_is_card_inserted(MMCDriver *mmcp) {
(void)mmcp;
/* TODO: Fill the implementation.*/
return true;
}
/**
* @brief MMC_SPI card write protection detection.
*/
bool mmc_lld_is_write_protected(MMCDriver *mmcp) {
(void)mmcp;
/* TODO: Fill the implementation.*/
return false;
}
#endif
/**
* @brief Board-specific initialization code.
* @todo Add your board-specific code, if any.
*/
void boardInit(void) {
}
sw/airborne/boards/crazyflie/chibios/v2.1/board.cfg
+96
View File
@@ -0,0 +1,96 @@
MCU_MODEL = STM32F405RGTx
CHIBIOS_VERSION = 3.0
HEADER
/*
* Board identifier.
*/
#define BOARD_ST_CRAZYFLIE
#define BOARD_NAME "Bitcraze Crazyflie 2.1"
/*
* Board oscillators-related settings.
* NOTE: LSE fitted.
*/
#if !defined(STM32_LSECLK)
#define STM32_LSECLK 32768
#endif
#if !defined(STM32_HSECLK)
#define STM32_HSECLK 8000000
#endif
/*
* Board voltages.
* Required for performance limits calculation.
*/
#define STM32_VDD 300
/*
* MCU type as defined in the ST header file stm32f4xx.h.
*/
#define STM32F405xx
/*
* AHB_CLK
*/
#define AHB_CLK STM32_HCLK
CONFIG
# PIN NAME PERIPH_TYPE AF_NUMBER or
# PIN NAME FUNCTION PUSHPULL|OPENDRAIN PIN_SPEED PULLUP|PULLDOWN|FLOATING LEVEL_LOW|LEVEL_HIGH AF_NUMBER
# simplified config for INPUT and OUTPUT
# PIN NAME INPUT PULLUP|PULLDOWN|FLOATING
# PIN NAME OUTPUT PUSHPULL|OPENDRAIN PIN_SPEED PULLUP|PULLDOWN|FLOATING LEVEL_LOW|LEVEL_HIGH
#
# PIN_SPEED : SPEED_VERYLOW, SPEED_LOW, SPEED_MEDIUM, SPEED_HIGH
# PERIPH_TYPE : SYS, ADC, PWM, ICU, I2C, SPI, UART, OTG, ETH, SDIO, SDIOCK,
#
# SYSTEM
A13 STM_SWIO SYS AF:SYS_JTMS-SWDIO
A14 STM_SWCLK SYS AF:SYS_JTCK-SWCLK
H00 OSC_IN SYS AF:RCC_OSC_IN
H01 OSC_OUT SYS AF:RCC_OSC_OUT
DEFAULT INPUT PUSHPULL SPEED_VERYLOW PULLDOWN LEVEL_LOW AF0
# ACTIVE PINS
PA01 MOTOR1 PWM AF:TIM2_CH2
PA02 E_TX2 UART AF:USART2_TX
PA03 E_RX2 UART AF:USART2_RX
PA04 NRF_FLOW_CTRL INPUT FLOATING
PA05 E_SCK SPI AF:SPI1_SCK
PA06 E_MISO SPI AF:SPI1_MISO
PA07 E_MOSI SPI AF:SPI1_MOSI
PA08 IMU_SCL I2C AF:I2C3_SCL
PA10 USB_ID OTG AF:USB_OTG_FS_ID
PA11 USB_DM OTG AF:USB_OTG_FS_DM
PA12 USB_DP OTG AF:USB_OTG_FS_DP
PA15 MOTOR3 PWM AF:TIM2_CH1
PB02 BOOT1 INPUT FLOATING
#PB03 STM_SW0 INPUT FLOATING FIXME in system section ?
PB04 E_CS1 INPUT FLOATING
PB05 E_CS2 INPUT FLOATING
PB06 E_SCL I2C AF:I2C1_SCL
PB07 E_SDA I2C AF:I2C1_SDA
PB08 E_CS3 INPUT FLOATING
PB09 MOTOR4 PWM AF:TIM4_CH4
PB11 MOTOR2 PWM AF:TIM2_CH4
PB13 NRF_SWCLK INPUT FLOATING
PB15 NRF_SWIO INPUT FLOATING
PC00 LED_RED_L LED
PC01 LED_GREEN_L LED
PC02 LED_GREEN_R LED
PC03 LED_RED_R LED
PC06 NRF_TX UART AF:USART6_TX
PC07 NRF_RX UART AF:USART6_RX
PC09 IMU_SDA I2C AF:I2C3_SDA
PC10 E_TX1 UART AF:USART3_TX
PC11 E_RX1 UART AF:USART3_RX
PC12 E_CS0 INPUT FLOATING
PD02 LED_BLUE_L LED
sw/airborne/boards/crazyflie/chibios/v2.1/board.h
+1680
View File
File diff suppressed because it is too large Load Diff
sw/airborne/boards/crazyflie/chibios/v2.1/board.mk
+20
View File
@@ -0,0 +1,20 @@
#
# ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Required include directories
BOARDINC = $(CHIBIOS_BOARD_DIR)
# List of all the board related files.
BOARDSRC = ${BOARDINC}/board.c
sw/airborne/boards/crazyflie/chibios/v2.1/crazyflie.h
+357
View File
@@ -0,0 +1,357 @@
#ifndef CONFIG_CRAZYFLIE_2_1_H
#define CONFIG_CRAZYFLIE_2_1_H
#define BOARD_CRAZYFLIE
/**
* ChibiOS board file
*/
#include "boards/crazyflie/chibios/v2.1/board.h"
/**
* PPRZ definitions
*/
/*
* AHB_CLK
*/
#define AHB_CLK STM32_HCLK
/*
* LEDs
*/
#ifndef USE_LED_1
#define USE_LED_1 1
#endif
#define LED_1_GPIO LED_RED_R_PORT
#define LED_1_GPIO_PIN LED_RED_R
#define LED_1_GPIO_ON gpio_clear
#define LED_1_GPIO_OFF gpio_set
#ifndef USE_LED_2
#define USE_LED_2 1
#endif
#define LED_2_GPIO LED_RED_L_PORT
#define LED_2_GPIO_PIN LED_RED_L
#define LED_2_GPIO_ON gpio_clear
#define LED_2_GPIO_OFF gpio_set
#ifndef USE_LED_3
#define USE_LED_3 1
#endif
#define LED_3_GPIO LED_GREEN_R_PORT
#define LED_3_GPIO_PIN LED_GREEN_R
#define LED_3_GPIO_ON gpio_clear
#define LED_3_GPIO_OFF gpio_set
#ifndef USE_LED_4
#define USE_LED_4 1
#endif
#define LED_4_GPIO LED_GREEN_L_PORT
#define LED_4_GPIO_PIN LED_GREEN_L
#define LED_4_GPIO_ON gpio_clear
#define LED_4_GPIO_OFF gpio_set
#ifndef USE_LED_5
#define USE_LED_5 1
#endif
#define LED_5_GPIO LED_BLUE_L_PORT
#define LED_5_GPIO_PIN LED_BLUE_L
#define LED_5_GPIO_ON gpio_set
#define LED_5_GPIO_OFF gpio_clear
/*
* ADCs
*/
// TODO for AUX
// No VBAT monitoring ?
/*
* PWM defines
*/
// SRVa connectors, activated in PWM mode by default
#ifndef USE_PWM1
#define USE_PWM1 1
#endif
#if USE_PWM1
#define PWM_SERVO_1 1
#define PWM_SERVO_1_GPIO MOTOR1_PORT
#define PWM_SERVO_1_PIN MOTOR1
#define PWM_SERVO_1_AF AF_MOTOR1
#define PWM_SERVO_1_DRIVER PWMD2
#define PWM_SERVO_1_CHANNEL 1
#define PWM_SERVO_1_ACTIVE PWM_OUTPUT_ACTIVE_HIGH
#else
#define PWM_SERVO_1_ACTIVE PWM_OUTPUT_DISABLED
#endif
#ifndef USE_PWM2
#define USE_PWM2 1
#endif
#if USE_PWM2
#define PWM_SERVO_2 2
#define PWM_SERVO_2_GPIO MOTOR2_PORT
#define PWM_SERVO_2_PIN MOTOR2
#define PWM_SERVO_2_AF AF_MOTOR2
#define PWM_SERVO_2_DRIVER PWMD2
#define PWM_SERVO_2_CHANNEL 3
#define PWM_SERVO_2_ACTIVE PWM_OUTPUT_ACTIVE_HIGH
#else
#define PWM_SERVO_2_ACTIVE PWM_OUTPUT_DISABLED
#endif
#ifndef USE_PWM3
#define USE_PWM3 1
#endif
#if USE_PWM3
#define PWM_SERVO_3 3
#define PWM_SERVO_3_GPIO MOTOR3_PORT
#define PWM_SERVO_3_PIN MOTOR3
#define PWM_SERVO_3_AF AF_MOTOR3
#define PWM_SERVO_3_DRIVER PWMD2
#define PWM_SERVO_3_CHANNEL 0
#define PWM_SERVO_3_ACTIVE PWM_OUTPUT_ACTIVE_HIGH
#else
#define PWM_SERVO_3_ACTIVE PWM_OUTPUT_DISABLED
#endif
#ifndef USE_PWM4
#define USE_PWM4 1
#endif
#if USE_PWM4
#define PWM_SERVO_4 4
#define PWM_SERVO_4_GPIO MOTOR4_PORT
#define PWM_SERVO_4_PIN MOTOR4
#define PWM_SERVO_4_AF AF_MOTOR4
#define PWM_SERVO_4_DRIVER PWMD4
#define PWM_SERVO_4_CHANNEL 3
#define PWM_SERVO_4_ACTIVE PWM_OUTPUT_ACTIVE_HIGH
#else
#define PWM_SERVO_4_ACTIVE PWM_OUTPUT_DISABLED
#endif
// servo index starting at 1 + regular servos + aux servos
// so NB = 1+4
#define ACTUATORS_PWM_NB 5
// PWM control of brushed motors
// Freq = 84 MHz (corresponding to prescaler of 0)
// Period = 256 (corresponding to 8bit resolution for command at ~328 kHz))
// as indicated in Crazyflie source code, 328 kHz offers better natural filtering
// than 128 kHz
// It is also needed to redefined PWM_CMD_TO_US to get the proper converstion
// from command to clock pulses number
#define PWM_CMD_TO_US(_t) (_t)
#ifdef STM32_PWM_USE_TIM2
#define PWM_CONF_TIM2 STM32_PWM_USE_TIM2
#else
#define PWM_CONF_TIM2 1
#endif
#define PWM_CONF2_DEF { \
84000000, \
256, \
NULL, \
{ \
{ PWM_SERVO_3_ACTIVE, NULL }, \
{ PWM_SERVO_1_ACTIVE, NULL }, \
{ PWM_OUTPUT_DISABLED, NULL }, \
{ PWM_SERVO_2_ACTIVE, NULL }, \
}, \
0, \
0 \
}
#ifdef STM32_PWM_USE_TIM4
#define PWM_CONF_TIM4 STM32_PWM_USE_TIM4
#else
#define PWM_CONF_TIM4 1
#endif
#define PWM_CONF4_DEF { \
84000000, \
256, \
NULL, \
{ \
{ PWM_OUTPUT_DISABLED, NULL }, \
{ PWM_OUTPUT_DISABLED, NULL }, \
{ PWM_OUTPUT_DISABLED, NULL }, \
{ PWM_SERVO_4_ACTIVE, NULL }, \
}, \
0, \
0 \
}
/**
* UART2 E_TX2
*/
#define UART2_GPIO_PORT_TX E_TX2_PORT
#define UART2_GPIO_TX E_TX2
#define UART2_GPIO_PORT_RX E_RX2_PORT
#define UART2_GPIO_RX E_RX2
#define UART2_GPIO_AF AF_E_RX2
#ifndef UART2_HW_FLOW_CONTROL
#define UART2_HW_FLOW_CONTROL FALSE
#endif
/**
* UART3 E_TX1
*/
#define UART3_GPIO_PORT_TX E_TX1_PORT
#define UART3_GPIO_TX E_TX1
#define UART3_GPIO_PORT_RX E_RX1_PORT
#define UART3_GPIO_RX E_RX1
#define UART3_GPIO_AF AF_E_RX1
/**
* UART6 NRF
*/
#define UART6_GPIO_PORT_TX NRF_TX_PORT
#define UART6_GPIO_TX NRF_TX
#define UART6_GPIO_PORT_RX NRF_RX_PORT
#define UART6_GPIO_RX NRF_RX
#define UART6_GPIO_AF AF_NRF_RX
#define UART6_GPIO_PORT_CTS NRF_FLOW_CTRL_PORT
#define UART6_GPIO_CTS NRF_FLOW_CTRL
/**
* I2C defines
*/
// Digital noise filter: 0 disabled, [0x1 - 0xF] enable up to n t_I2CCLK
#define STM32_CR1_DNF(n) ((n & 0x0f) << 8)
// Timing register
#define I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR (STM32_TIMINGR_PRESC(0U) | \
STM32_TIMINGR_SCLDEL(10U) | STM32_TIMINGR_SDADEL(0U) | \
STM32_TIMINGR_SCLH(34U) | STM32_TIMINGR_SCLL(86U))
#define I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR (STM32_TIMINGR_PRESC(1U) | \
STM32_TIMINGR_SCLDEL(9U) | STM32_TIMINGR_SDADEL(0U) | \
STM32_TIMINGR_SCLH(105U) | STM32_TIMINGR_SCLL(153U))
// Internal I2C (IMU, baro)
#ifndef I2C3_CLOCK_SPEED
#define I2C3_CLOCK_SPEED 400000
#endif
#if I2C3_CLOCK_SPEED == 400000
#define I2C3_DUTY_CYCLE FAST_DUTY_CYCLE_2
#elif I2C3_CLOCK_SPEED == 100000
#define I2C3_DUTY_CYCLE STD_DUTY_CYCLE
#else
#error "Invalid I2C3 clock speed"
#endif
#define I2C3_CFG_DEF { \
OPMODE_I2C, \
I2C3_CLOCK_SPEED, \
I2C3_DUTY_CYCLE, \
}
// External I2C
#ifndef I2C1_CLOCK_SPEED
#define I2C1_CLOCK_SPEED 400000
#endif
#if I2C1_CLOCK_SPEED == 400000
#define I2C1_DUTY_CYCLE FAST_DUTY_CYCLE_2
#elif I2C1_CLOCK_SPEED == 100000
#define I2C1_DUTY_CYCLE STD_DUTY_CYCLE
#else
#error "Invalid I2C1 clock speed"
#endif
#define I2C1_CFG_DEF { \
OPMODE_I2C, \
I2C1_CLOCK_SPEED, \
I2C1_DUTY_CYCLE, \
}
/*
#ifndef I2C3_CLOCK_SPEED
#define I2C3_CLOCK_SPEED 400000
#endif
#if I2C3_CLOCK_SPEED == 400000
#define I2C3_CFG_DEF { \
.timingr = I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#elif I2C3_CLOCK_SPEED == 100000
#define I2C3_CFG_DEF { \
.timingr = I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#else
#error "Unknown I2C3 clock speed"
#endif
// External I2C
#ifndef I2C1_CLOCK_SPEED
#define I2C1_CLOCK_SPEED 400000
#endif
#if I2C1_CLOCK_SPEED == 400000
#define I2C1_CFG_DEF { \
.timingr = I2C_FAST_400KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#elif I2C1_CLOCK_SPEED == 100000
#define I2C1_CFG_DEF { \
.timingr = I2C_STD_100KHZ_DNF0_100NS_PCLK54MHZ_TIMINGR, \
.cr1 = STM32_CR1_DNF(0), \
.cr2 = 0 \
}
#else
#error "Unknown I2C2 clock speed"
#endif
*/
/**
* SPI Config
*/
// External SPI
#define SPI1_GPIO_AF AF_E_SCK
#define SPI1_GPIO_PORT_MISO E_MISO_PORT
#define SPI1_GPIO_MISO E_MISO
#define SPI1_GPIO_PORT_MOSI E_MOSI_PORT
#define SPI1_GPIO_MOSI E_MOSI
#define SPI1_GPIO_PORT_SCK E_SCK_PORT
#define SPI1_GPIO_SCK E_SCK
#define SPI_SELECT_SLAVE0_PORT E_CS0_PORT
#define SPI_SELECT_SLAVE0_PIN E_CS0
#define SPI_SELECT_SLAVE1_PORT E_CS1_PORT
#define SPI_SELECT_SLAVE1_PIN E_CS1
#define SPI_SELECT_SLAVE2_PORT E_CS2_PORT
#define SPI_SELECT_SLAVE2_PIN E_CS2
#define SPI_SELECT_SLAVE3_PORT E_CS3_PORT
#define SPI_SELECT_SLAVE3_PIN E_CS3
/**
* Baro
*
* Apparently needed for backwards compatibility
* with the ancient onboard baro boards
*/
#ifndef USE_BARO_BOARD
#define USE_BARO_BOARD 0
#endif
/*
* Actuators for fixedwing
*/
/* Default actuators driver */
#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
#define ActuatorsDefaultInit() ActuatorsPwmInit()
#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
#endif /* CONFIG_TAWAKI_1_00_H */
sw/airborne/boards/crazyflie/chibios/v2.1/mcuconf.h
+362
View File
@@ -0,0 +1,362 @@
/*
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#ifndef _MCUCONF_H_
#define _MCUCONF_H_
/*
* STM32F4xx drivers configuration.
* The following settings override the default settings present in
* the various device driver implementation headers.
* Note that the settings for each driver only have effect if the whole
* driver is enabled in halconf.h.
*
* IRQ priorities:
* 15...0 Lowest...Highest.
*
* DMA priorities:
* 0...3 Lowest...Highest.
*/
#define STM32F4xx_MCUCONF
/*
* HAL driver system settings.
*/
#define STM32_NO_INIT FALSE
#define STM32_HSI_ENABLED TRUE
#define STM32_LSI_ENABLED FALSE
#define STM32_HSE_ENABLED TRUE
#define STM32_LSE_ENABLED FALSE
#define STM32_CLOCK48_REQUIRED TRUE
#define STM32_SW STM32_SW_PLL
#define STM32_PLLSRC STM32_PLLSRC_HSE
#define STM32_PLLM_VALUE 8
#define STM32_PLLN_VALUE 336
#define STM32_PLLP_VALUE 2
#define STM32_PLLQ_VALUE 7
#define STM32_HPRE STM32_HPRE_DIV1
#define STM32_PPRE1 STM32_PPRE1_DIV4
#define STM32_PPRE2 STM32_PPRE2_DIV2
#define STM32_RTCSEL STM32_RTCSEL_HSEDIV
#define STM32_RTCPRE_VALUE 8
#define STM32_MCO1SEL STM32_MCO1SEL_HSI
#define STM32_MCO1PRE STM32_MCO1PRE_DIV1
#define STM32_MCO2SEL STM32_MCO2SEL_SYSCLK
#define STM32_MCO2PRE STM32_MCO2PRE_DIV5
#define STM32_I2SSRC STM32_I2SSRC_CKIN
#define STM32_PLLI2SN_VALUE 192
#define STM32_PLLI2SR_VALUE 5
#define STM32_PVD_ENABLE FALSE
#define STM32_PLS STM32_PLS_LEV0
#define STM32_BKPRAM_ENABLE FALSE
/*
* ADC driver system settings.
*/
#define STM32_ADC_ADCPRE ADC_CCR_ADCPRE_DIV8
#define STM32_ADC_USE_ADC1 TRUE
#define STM32_ADC_USE_ADC2 FALSE
#define STM32_ADC_USE_ADC3 FALSE
#define STM32_ADC_ADC1_DMA_STREAM STM32_DMA_STREAM_ID(2, 4)
#define STM32_ADC_ADC2_DMA_STREAM STM32_DMA_STREAM_ID(2, 2)
#define STM32_ADC_ADC3_DMA_STREAM STM32_DMA_STREAM_ID(2, 1)
#define STM32_ADC_ADC1_DMA_PRIORITY 2
#define STM32_ADC_ADC2_DMA_PRIORITY 2
#define STM32_ADC_ADC3_DMA_PRIORITY 2
#define STM32_ADC_IRQ_PRIORITY 6
#define STM32_ADC_ADC1_DMA_IRQ_PRIORITY 6
#define STM32_ADC_ADC2_DMA_IRQ_PRIORITY 6
#define STM32_ADC_ADC3_DMA_IRQ_PRIORITY 6
/*
* CAN driver system settings.
*/
#define STM32_CAN_USE_CAN1 FALSE
#define STM32_CAN_USE_CAN2 FALSE
#define STM32_CAN_CAN1_IRQ_PRIORITY 11
#define STM32_CAN_CAN2_IRQ_PRIORITY 11
/*
* DAC driver system settings.
*/
#define STM32_DAC_DUAL_MODE FALSE
#define STM32_DAC_USE_DAC1_CH1 FALSE
#define STM32_DAC_USE_DAC1_CH2 FALSE
#define STM32_DAC_DAC1_CH1_IRQ_PRIORITY 10
#define STM32_DAC_DAC1_CH2_IRQ_PRIORITY 10
#define STM32_DAC_DAC1_CH1_DMA_PRIORITY 2
#define STM32_DAC_DAC1_CH2_DMA_PRIORITY 2
#define STM32_DAC_DAC1_CH1_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
#define STM32_DAC_DAC1_CH2_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
/*
* EXT driver system settings.
*/
#define STM32_EXT_EXTI0_IRQ_PRIORITY 6
#define STM32_EXT_EXTI1_IRQ_PRIORITY 6
#define STM32_EXT_EXTI2_IRQ_PRIORITY 6
#define STM32_EXT_EXTI3_IRQ_PRIORITY 6
#define STM32_EXT_EXTI4_IRQ_PRIORITY 6
#define STM32_EXT_EXTI5_9_IRQ_PRIORITY 6
#define STM32_EXT_EXTI10_15_IRQ_PRIORITY 6
#define STM32_EXT_EXTI16_IRQ_PRIORITY 6
#define STM32_EXT_EXTI17_IRQ_PRIORITY 15
#define STM32_EXT_EXTI18_IRQ_PRIORITY 6
#define STM32_EXT_EXTI19_IRQ_PRIORITY 6
#define STM32_EXT_EXTI20_IRQ_PRIORITY 6
#define STM32_EXT_EXTI21_IRQ_PRIORITY 15
#define STM32_EXT_EXTI22_IRQ_PRIORITY 15
/*
* GPT driver system settings.
*/
#define STM32_GPT_USE_TIM1 FALSE
#define STM32_GPT_USE_TIM2 FALSE
#define STM32_GPT_USE_TIM3 FALSE
#define STM32_GPT_USE_TIM4 FALSE
#define STM32_GPT_USE_TIM5 FALSE
#define STM32_GPT_USE_TIM6 FALSE
#define STM32_GPT_USE_TIM7 FALSE
#define STM32_GPT_USE_TIM8 FALSE
#define STM32_GPT_USE_TIM9 FALSE
#define STM32_GPT_USE_TIM11 FALSE
#define STM32_GPT_USE_TIM12 FALSE
#define STM32_GPT_USE_TIM14 FALSE
#define STM32_GPT_TIM1_IRQ_PRIORITY 7
#define STM32_GPT_TIM2_IRQ_PRIORITY 7
#define STM32_GPT_TIM3_IRQ_PRIORITY 7
#define STM32_GPT_TIM4_IRQ_PRIORITY 7
#define STM32_GPT_TIM5_IRQ_PRIORITY 7
#define STM32_GPT_TIM6_IRQ_PRIORITY 7
#define STM32_GPT_TIM7_IRQ_PRIORITY 7
#define STM32_GPT_TIM8_IRQ_PRIORITY 7
#define STM32_GPT_TIM9_IRQ_PRIORITY 7
#define STM32_GPT_TIM11_IRQ_PRIORITY 7
#define STM32_GPT_TIM12_IRQ_PRIORITY 7
#define STM32_GPT_TIM14_IRQ_PRIORITY 7
/*
* I2C driver system settings.
*/
#if USE_I2C1
#define STM32_I2C_USE_I2C1 TRUE
#else
#define STM32_I2C_USE_I2C1 FALSE
#endif
#define STM32_I2C_USE_I2C2 FALSE
#if USE_I2C3
#define STM32_I2C_USE_I2C3 TRUE
#else
#define STM32_I2C_USE_I2C3 FALSE
#endif
#define STM32_I2C_BUSY_TIMEOUT 50
#define STM32_I2C_I2C1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0)
#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
#define STM32_I2C_I2C2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
#define STM32_I2C_I2C2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
#define STM32_I2C_I2C3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
#define STM32_I2C_I2C3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
#define STM32_I2C_I2C1_IRQ_PRIORITY 5
#define STM32_I2C_I2C2_IRQ_PRIORITY 5
#define STM32_I2C_I2C3_IRQ_PRIORITY 5
#define STM32_I2C_I2C1_DMA_PRIORITY 3
#define STM32_I2C_I2C2_DMA_PRIORITY 3
#define STM32_I2C_I2C3_DMA_PRIORITY 3
#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure")
/*
* ICU driver system settings.
*/
#define STM32_ICU_USE_TIM1 FALSE
#define STM32_ICU_USE_TIM2 FALSE
#define STM32_ICU_USE_TIM3 FALSE
#define STM32_ICU_USE_TIM4 FALSE
#define STM32_ICU_USE_TIM5 FALSE
#define STM32_ICU_USE_TIM8 FALSE
#define STM32_ICU_USE_TIM9 TRUE
#define STM32_ICU_TIM1_IRQ_PRIORITY 7
#define STM32_ICU_TIM2_IRQ_PRIORITY 7
#define STM32_ICU_TIM3_IRQ_PRIORITY 7
#define STM32_ICU_TIM4_IRQ_PRIORITY 7
#define STM32_ICU_TIM5_IRQ_PRIORITY 7
#define STM32_ICU_TIM8_IRQ_PRIORITY 7
#define STM32_ICU_TIM9_IRQ_PRIORITY 7
/*
* MAC driver system settings.
*/
#define STM32_MAC_TRANSMIT_BUFFERS 2
#define STM32_MAC_RECEIVE_BUFFERS 4
#define STM32_MAC_BUFFERS_SIZE 1522
#define STM32_MAC_PHY_TIMEOUT 100
#define STM32_MAC_ETH1_CHANGE_PHY_STATE TRUE
#define STM32_MAC_ETH1_IRQ_PRIORITY 13
#define STM32_MAC_IP_CHECKSUM_OFFLOAD 0
/*
* PWM driver system settings.
*/
#define STM32_PWM_USE_ADVANCED FALSE
#define STM32_PWM_USE_TIM1 FALSE // enable for WS2812
#ifndef STM32_PWM_USE_TIM2
#define STM32_PWM_USE_TIM2 TRUE
#endif
#define STM32_PWM_USE_TIM3 FALSE
#ifndef STM32_PWM_USE_TIM4
#define STM32_PWM_USE_TIM4 TRUE
#endif
#define STM32_PWM_USE_TIM5 FALSE
#define STM32_PWM_USE_TIM8 FALSE
#define STM32_PWM_USE_TIM9 FALSE
#define STM32_PWM_TIM1_IRQ_PRIORITY 7
#define STM32_PWM_TIM2_IRQ_PRIORITY 7
#define STM32_PWM_TIM3_IRQ_PRIORITY 7
#define STM32_PWM_TIM4_IRQ_PRIORITY 7
#define STM32_PWM_TIM5_IRQ_PRIORITY 7
#define STM32_PWM_TIM8_IRQ_PRIORITY 7
#define STM32_PWM_TIM9_IRQ_PRIORITY 7
/*
* SERIAL driver system settings.
*/
#define STM32_SERIAL_USE_USART1 FALSE
#if USE_UART2
#define STM32_SERIAL_USE_USART2 TRUE
#else
#define STM32_SERIAL_USE_USART2 FALSE
#endif
#if USE_UART3
#define STM32_SERIAL_USE_USART3 TRUE
#else
#define STM32_SERIAL_USE_USART3 FALSE
#endif
#define STM32_SERIAL_USE_UART4 FALSE
#define STM32_SERIAL_USE_UART5 FALSE
#if USE_UART6
#define STM32_SERIAL_USE_USART6 TRUE
#else
#define STM32_SERIAL_USE_USART6 FALSE
#endif
#define STM32_SERIAL_USART1_PRIORITY 12
#define STM32_SERIAL_USART2_PRIORITY 12
#define STM32_SERIAL_USART3_PRIORITY 12
#define STM32_SERIAL_UART4_PRIORITY 12
#define STM32_SERIAL_UART5_PRIORITY 12
#define STM32_SERIAL_USART6_PRIORITY 12
/*
* SPI driver system settings.
*/
#if USE_SPI1
#define STM32_SPI_USE_SPI1 TRUE
#else
#define STM32_SPI_USE_SPI1 FALSE
#endif
#define STM32_SPI_USE_SPI2 FALSE
#define STM32_SPI_USE_SPI3 FALSE
#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0)
#define STM32_SPI_SPI1_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 5)
#define STM32_SPI_SPI2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
#define STM32_SPI_SPI2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
#define STM32_SPI_SPI3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0)
#define STM32_SPI_SPI3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
#define STM32_SPI_SPI1_DMA_PRIORITY 1
#define STM32_SPI_SPI2_DMA_PRIORITY 1
#define STM32_SPI_SPI3_DMA_PRIORITY 1
#define STM32_SPI_SPI1_IRQ_PRIORITY 10
#define STM32_SPI_SPI2_IRQ_PRIORITY 10
#define STM32_SPI_SPI3_IRQ_PRIORITY 10
#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure")
/*
* ST driver system settings.
*/
#define STM32_ST_IRQ_PRIORITY 8
#define STM32_ST_USE_TIMER 2
/*
* UART driver system settings.
*/
#define STM32_UART_USE_USART1 FALSE
#define STM32_UART_USE_USART2 FALSE
#define STM32_UART_USE_USART3 FALSE
#define STM32_UART_USE_UART4 FALSE
#define STM32_UART_USE_UART5 FALSE
#define STM32_UART_USE_USART6 FALSE
#define STM32_UART_USART1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 5) // Not used: conflict SPI1
#define STM32_UART_USART1_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 7)
#define STM32_UART_USART2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 5)
#define STM32_UART_USART2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
#define STM32_UART_USART3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 1)
#define STM32_UART_USART3_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
#define STM32_UART_UART4_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
#define STM32_UART_UART4_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 4)
#define STM32_UART_UART5_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0)
#define STM32_UART_UART5_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
#define STM32_UART_USART6_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 2)
#define STM32_UART_USART6_TX_DMA_STREAM STM32_DMA_STREAM_ID(2, 6)
#define STM32_UART_USART1_IRQ_PRIORITY 12
#define STM32_UART_USART2_IRQ_PRIORITY 12
#define STM32_UART_USART3_IRQ_PRIORITY 12
#define STM32_UART_UART4_IRQ_PRIORITY 12
#define STM32_UART_UART5_IRQ_PRIORITY 12
#define STM32_UART_USART6_IRQ_PRIORITY 12
#define STM32_UART_USART1_DMA_PRIORITY 1
#define STM32_UART_USART2_DMA_PRIORITY 0
#define STM32_UART_USART3_DMA_PRIORITY 0
#define STM32_UART_UART4_DMA_PRIORITY 0
#define STM32_UART_UART5_DMA_PRIORITY 0
#define STM32_UART_USART6_DMA_PRIORITY 0
#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure")
/*
* USB driver system settings.
*/
#define STM32_USB_USE_OTG1 TRUE // FS, DFU_BOOT
#define STM32_USB_USE_OTG2 FALSE // HS
#define STM32_USB_OTG1_IRQ_PRIORITY 14
#define STM32_USB_OTG2_IRQ_PRIORITY 14
#define STM32_USB_OTG1_RX_FIFO_SIZE 512
#define STM32_USB_OTG2_RX_FIFO_SIZE 512
#define STM32_USB_OTG_THREAD_PRIO HIGHPRIO
#define STM32_USB_OTG_THREAD_STACK_SIZE 256
#define STM32_USB_OTGFIFO_FILL_BASEPRI 0
/*
* SDC driver system settings.
*/
#define STM32_SDC_SDIO_DMA_STREAM STM32_DMA_STREAM_ID(2, 3)
#define STM32_SDC_SDIO_DMA_PRIORITY 3
#define STM32_SDC_SDIO_IRQ_PRIORITY 9
#define STM32_SDC_CLOCK_ACTIVATION_DELAY 10
#define STM32_SDC_SDIO_UNALIGNED_SUPPORT FALSE
#define STM32_SDC_WRITE_TIMEOUT_MS 250
#define STM32_SDC_READ_TIMEOUT_MS 15
/*
sdlog message buffer and queue configuration
*/
#define SDLOG_QUEUE_BUCKETS 1024
#define SDLOG_MAX_MESSAGE_LEN 300
#define SDLOG_NUM_FILES 2
#define SDLOG_ALL_BUFFERS_SIZE (SDLOG_NUM_FILES*8*1024)
#endif /* _MCUCONF_H_ */
sw/airborne/modules/ahrs/ahrs_madgwick.c
+156
View File
@@ -0,0 +1,156 @@
/*
* Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ahrs/ahrs_madgwick.h
* AHRS using Madgwick implementation
*
* See:
* https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
*/
#include "modules/ahrs/ahrs_madgwick.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
#ifndef AHRS_MADGWICK_BETA
#define AHRS_MADGWICK_BETA 0.1f // 2 * proportional gain
#endif
struct AhrsMadgwick ahrs_madgwick;
/* init state and measurements */
static inline void init_state(void)
{
float_quat_identity(&ahrs_madgwick.quat);
FLOAT_RATES_ZERO(ahrs_madgwick.bias);
FLOAT_VECT3_ZERO(ahrs_madgwick.accel);
}
void ahrs_madgwick_init(void)
{
// init state and measurements
init_state();
ahrs_madgwick.is_aligned = false;
ahrs_madgwick.reset = false;
}
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
{
/* Compute an initial orientation from accel directly as quaternion */
ahrs_float_get_quat_from_accel(&ahrs_madgwick.quat, lp_accel);
/* use average gyro as initial value for bias */
ahrs_madgwick.bias = *lp_gyro;
// ins and ahrs are now running
ahrs_madgwick.is_aligned = true;
}
void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt)
{
struct FloatQuat qdot;
// realign all the filter if needed
// a complete init cycle is required
if (ahrs_madgwick.reset) {
ahrs_madgwick.reset = false;
ahrs_madgwick.is_aligned = false;
init_state();
}
// unbias and rotate gyro
struct FloatRates gyro_unbiased;
RATES_DIFF(gyro_unbiased, *gyro, ahrs_madgwick.bias);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_madgwick.body_to_imu);
float_rmat_transp_ratemult(&ahrs_madgwick.rates, body_to_imu_rmat, &gyro_unbiased);
// Rate of change of quaternion from gyroscope
float_quat_derivative(&qdot, &ahrs_madgwick.rates, &ahrs_madgwick.quat);
// compute accel norm
float norm = float_vect3_norm(&ahrs_madgwick.accel);
// Compute feedback only if accelerometer measurement valid
// (avoids NaN in accelerometer normalisation)
if (norm > 0.01f) {
// Normalise accelerometer measurement
// direction of accel is inverted to comply with filter original frame
struct FloatVect3 a;
VECT3_SDIV(a, ahrs_madgwick.accel, -norm);
// Auxiliary variables to avoid repeated arithmetic
const float q1 = ahrs_madgwick.quat.qx;
const float q2 = ahrs_madgwick.quat.qy;
const float q3 = ahrs_madgwick.quat.qz;
const float _2q0 = 2.0f * ahrs_madgwick.quat.qi;
const float _2q1 = 2.0f * ahrs_madgwick.quat.qx;
const float _2q2 = 2.0f * ahrs_madgwick.quat.qy;
const float _2q3 = 2.0f * ahrs_madgwick.quat.qz;
const float _4q0 = 4.0f * ahrs_madgwick.quat.qi;
const float _4q1 = 4.0f * ahrs_madgwick.quat.qx;
const float _4q2 = 4.0f * ahrs_madgwick.quat.qy;
const float _8q1 = 8.0f * ahrs_madgwick.quat.qx;
const float _8q2 = 8.0f * ahrs_madgwick.quat.qy;
const float q0q0 = ahrs_madgwick.quat.qi * ahrs_madgwick.quat.qi;
const float q1q1 = ahrs_madgwick.quat.qx * ahrs_madgwick.quat.qx;
const float q2q2 = ahrs_madgwick.quat.qy * ahrs_madgwick.quat.qy;
const float q3q3 = ahrs_madgwick.quat.qz * ahrs_madgwick.quat.qz;
// Gradient decent algorithm corrective step
const float s0 = _4q0 * q2q2 + _2q2 * a.x + _4q0 * q1q1 - _2q1 * a.y;
const float s1 = _4q1 * q3q3 - _2q3 * a.x + 4.0f * q0q0 * q1 - _2q0 * a.y - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * a.z;
const float s2 = 4.0f * q0q0 * q2 + _2q0 * a.x + _4q2 * q3q3 - _2q3 * a.y - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * a.z;
const float s3 = 4.0f * q1q1 * q3 - _2q1 * a.x + 4.0f * q2q2 * q3 - _2q2 * a.y;
const float beta_inv_grad_norm = AHRS_MADGWICK_BETA / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
// Apply feedback step
qdot.qi -= s0 * beta_inv_grad_norm;
qdot.qx -= s1 * beta_inv_grad_norm;
qdot.qy -= s2 * beta_inv_grad_norm;
qdot.qz -= s3 * beta_inv_grad_norm;
}
// Integrate rate of change of quaternion to yield quaternion
ahrs_madgwick.quat.qi += qdot.qi * dt;
ahrs_madgwick.quat.qx += qdot.qx * dt;
ahrs_madgwick.quat.qy += qdot.qy * dt;
ahrs_madgwick.quat.qz += qdot.qz * dt;
// Normalise quaternion
float_quat_normalize(&ahrs_madgwick.quat);
}
void ahrs_madgwick_update_accel(struct FloatVect3* accel)
{
ahrs_madgwick.accel = *accel;
}
void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i)
{
orientationSetQuat_f(&ahrs_madgwick.body_to_imu, q_b2i);
if (!ahrs_madgwick.is_aligned) {
/* Set ltp_to_imu so that body is zero */
ahrs_madgwick.quat = *q_b2i;
}
}
sw/airborne/modules/ahrs/ahrs_madgwick.h
+57
View File
@@ -0,0 +1,57 @@
/*
* Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ahrs/ahrs_madgwick.h
* AHRS using Madgwick implementation
*
* See:
* https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
*/
#ifndef AHRS_MADGWICK_H
#define AHRS_MADGWICK_H
#include "subsystems/ahrs.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
/** Madgwick filter structure
*/
struct AhrsMadgwick {
struct FloatQuat quat; ///< Estimated attitude (quaternion)
struct FloatRates rates; ///< Measured gyro rates
struct FloatRates bias; ///< Gyro bias (from alignment)
struct FloatVect3 accel; ///< Measured accelerometers
struct OrientationReps body_to_imu; ///< body_to_imu rotation
bool reset; ///< flag to request reset/reinit the filter
bool is_aligned; ///< aligned flag
};
extern struct AhrsMadgwick ahrs_madgwick;
extern void ahrs_madgwick_init(void);
extern void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel);
extern void ahrs_madgwick_propagate(struct FloatRates* gyro, float dt);
extern void ahrs_madgwick_update_accel(struct FloatVect3* accel);
#endif /* AHRS_MADGWICK_H */
sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
+222
View File
@@ -0,0 +1,222 @@
/*
* Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ahrs/ahrs_madgwick_wrapper.c
*
* Paparazzi specific wrapper to run Madgwick ahrs filter.
*/
#include "modules/ahrs/ahrs_madgwick_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "state.h"
#ifndef AHRS_MADGWICK_OUTPUT_ENABLED
#define AHRS_MADGWICK_OUTPUT_ENABLED TRUE
#endif
PRINT_CONFIG_VAR(AHRS_MADGWICK_OUTPUT_ENABLED)
/** if TRUE with push the estimation results to the state interface */
static bool ahrs_madgwick_output_enabled;
/** last gyro msg timestamp */
static uint32_t ahrs_madgwick_last_stamp = 0;
static uint8_t ahrs_madgwick_id = AHRS_COMP_ID_MADGWICK;
static void compute_body_orientation_and_rates(void);
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
/* compute eulers in int (IMU frame) */
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_madgwick.quat);
struct Int32Eulers eulers_imu;
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
float_eulers_of_quat(&ltp_to_body_euler, &ltp_to_body_quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&eulers_imu.phi,
&eulers_imu.theta,
&eulers_imu.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
&ahrs_madgwick_id);
}
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
uint16_t val = 0;
if (!ahrs_madgwick.is_aligned) { mde = 2; }
uint32_t t_diff = get_sys_time_usec() - ahrs_madgwick_last_stamp;
/* set lost if no new gyro measurements for 50ms */
if (t_diff > 50000) { mde = 5; }
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_madgwick_id, &mde, &val);
}
#endif
/*
* ABI bindings
*/
/** IMU (gyro, accel) */
#ifndef AHRS_MADGWICK_IMU_ID
#define AHRS_MADGWICK_IMU_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(AHRS_MADGWICK_IMU_ID)
/** magnetometer */
#ifndef AHRS_MADGWICK_MAG_ID
#define AHRS_MADGWICK_MAG_ID AHRS_MADGWICK_IMU_ID
#endif
PRINT_CONFIG_VAR(AHRS_MADGWICK_MAG_ID)
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
/**
* Call ahrs_madgwick_propagate on new gyro measurements.
* Since acceleration measurement is also needed for propagation,
* use the last stored accel from #ahrs_madgwick_accel.
*/
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Rates *gyro)
{
struct FloatRates gyro_f;
RATES_FLOAT_OF_BFP(gyro_f, *gyro);
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS Madgwick propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_madgwick.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_madgwick_propagate(&gyro_f, dt);
compute_body_orientation_and_rates();
}
last_stamp = stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS Madgwick propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
if (ahrs_madgwick.is_aligned) {
ahrs_madgwick_propagate(&gyro_f, dt);
compute_body_orientation_and_rates();
}
#endif
ahrs_madgwick_last_stamp = stamp;
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
if (ahrs_madgwick.is_aligned) {
struct FloatVect3 accel_f;
ACCELS_FLOAT_OF_BFP(accel_f, *accel);
ahrs_madgwick_update_accel(&accel_f);
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag __attribute__((unused)))
{
if (!ahrs_madgwick.is_aligned) {
/* convert to float */
struct FloatRates gyro_f;
RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
struct FloatVect3 accel_f;
ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
ahrs_madgwick_align(&gyro_f, &accel_f);
compute_body_orientation_and_rates();
}
}
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
struct FloatQuat *q_b2i_f)
{
ahrs_madgwick_set_body_to_imu_quat(q_b2i_f);
}
static bool ahrs_madgwick_enable_output(bool enable)
{
ahrs_madgwick_output_enabled = enable;
return ahrs_madgwick_output_enabled;
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static void compute_body_orientation_and_rates(void)
{
if (ahrs_madgwick_output_enabled) {
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
/* Set state */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
/* compute body rates */
stateSetBodyRates_f(&ahrs_madgwick.rates);
}
}
void ahrs_madgwick_register(void)
{
ahrs_madgwick_output_enabled = AHRS_MADGWICK_OUTPUT_ENABLED;
ahrs_madgwick_init();
ahrs_register_impl(ahrs_madgwick_enable_output);
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(AHRS_MADGWICK_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(AHRS_MADGWICK_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
#endif
}
sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.h
+39
View File
@@ -0,0 +1,39 @@
/*
* Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ahrs/ahrs_madgwick_wrapper.h
*
* Paparazzi specific wrapper to run Madgwick ahrs filter.
*/
#ifndef AHRS_MADGWICK_WRAPPER_H
#define AHRS_MADGWICK_WRAPPER_H
#include "modules/ahrs/ahrs_madgwick.h"
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_madgwick
#endif
extern void ahrs_madgwick_register(void);
#endif /* AHRS_MADGWICK_WRAPPER_H */
sw/airborne/modules/datalink/bitcraze/crtp.h
+75
View File
@@ -0,0 +1,75 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/datalink/bitcraze/crtp.h
*
* CRTP protocol for communication with bitcraze/crazyflie modems
*
* based on PX4 implementation
*/
#ifndef CRTP_H
#define CRTP_H
#include "std.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_PPRZLINK 0x09 // Non-standard port for transmitting pprzlink 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;
#endif
sw/airborne/modules/datalink/bitcraze/syslink.c
+125
View File
@@ -0,0 +1,125 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/datalink/bitcraze/syslink.c
*
* Syslink protocol for communication with bitcraze/crazyflie NRF mcu
*
* based on PX4 implementation
*/
#include "syslink.h"
const char *syslink_stx = "\xbc\xcf";
void syslink_parse_init(syslink_parse_state *state)
{
state->state = SYSLINK_STATE_START;
state->index = 0;
}
bool syslink_parse_char(syslink_parse_state *state, uint8_t c, syslink_message_t *msg)
{
switch (state->state) {
case SYSLINK_STATE_START:
if (c == syslink_stx[state->index]) {
state->index++;
} else {
state->index = 0;
}
if (syslink_stx[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]) {
// fail 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 true; // message is correct, return true
}
break;
}
return false;
}
/*
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;
}
sw/airborne/modules/datalink/bitcraze/syslink.h
+119
View File
@@ -0,0 +1,119 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/datalink/bitcraze/syslink.h
*
* Syslink protocol for communication with bitcraze/crazyflie NRF mcu
*
* based on PX4 implementation
*/
#ifndef SYSLINK_H
#define SYSLINK_H
#include <stdint.h>
#include <stdbool.h>
#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;
typedef enum {
SYSLINK_STATE_START = 0,
SYSLINK_STATE_TYPE,
SYSLINK_STATE_LENGTH,
SYSLINK_STATE_DATA,
SYSLINK_STATE_CKSUM
} syslink_state_t;
typedef struct {
syslink_state_t state;
int index;
} syslink_parse_state;
extern const char *syslink_stx;
#ifdef __cplusplus
extern "C" {
#endif
/** Init syslink parser
*
* @param state pointer to state structure
*/
extern void syslink_parse_init(syslink_parse_state *state);
/** Parse one byte
*
* @param state pointer to state structure
* @param c next byte to parse
* @param msg pointer to message structure
* @return true if a full message was parsed
*/
extern bool syslink_parse_char(syslink_parse_state *state, uint8_t c, syslink_message_t *msg);
/** Compute syslink checksum
*
* @param msg pointer to message structure
*/
extern void syslink_compute_cksum(syslink_message_t *msg);
#ifdef __cplusplus
}
#endif
#endif
sw/airborne/modules/datalink/bitcraze/syslink_dl.c
+422
View File
@@ -0,0 +1,422 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/datalink/bitcraze/syslink_dl.c
*
* Syslink protocol handling and functionalities
*
*/
#include "modules/datalink/bitcraze/syslink_dl.h"
#include "subsystems/electrical.h"
#include "mcu_periph/uart.h"
#include <string.h>
#include "led.h"
struct syslink_dl syslink;
/** Protect syslink TX with Mutex when using RTOS */
#include "pprz_mutex.h"
PPRZ_MUTEX(syslink_tx_mtx);
/** Send a syslink message
*/
static void send_message(syslink_message_t *msg)
{
syslink_compute_cksum(msg);
uint8_t buf[sizeof(syslink_message_t)];
buf[0] = syslink_stx[0];
buf[1] = syslink_stx[1];
buf[2] = msg->type;
buf[3] = msg->length;
for (int i = 0; i < msg->length; i++) {
buf[4+i] = msg->data[i];
}
buf[msg->length+4] = msg->cksum[0];
buf[msg->length+5] = msg->cksum[1];
uart_put_buffer(&(SYSLINK_DEV), 0, buf, msg->length+6);
//uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)syslink_stx, 2);
//uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->type), sizeof(msg->type));
//uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->length), sizeof(msg->length));
//uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->data), msg->length);
//uart_put_buffer(&(SYSLINK_DEV), 0, (uint8_t*)(&msg->cksum), sizeof(msg->cksum));
}
/**
* Handle battery message
*/
static void handle_battery(syslink_message_t *msg)
{
if (msg->length != 9) {
return;
}
// check flag
uint8_t flags = msg->data[0];
syslink.charging = (bool) (flags & 1);
syslink.powered = (bool) (flags & 2);
// update voltage
float vbat;
memcpy(&vbat, &msg->data[1], sizeof(float));
if (syslink.powered && vbat > 3.f) {
electrical.vsupply = vbat; // remove 0 reading when powered on USB ?
} else if (!syslink.powered) {
electrical.vsupply = vbat; // running on battery
}
}
/**
* Handle various raw messages
*/
static void handle_raw_other(syslink_message_t *msg)
{
// This function doesn't actually do anything
// It is just here to return null responses to most standard messages
// Setup order from the cflib-python is
// - platform link source (name, protocol version)
// - log TOC
// - mem update
// - param TOC
// -> call connected callback and start getting data
// - update param if needed
crtp_message_t *c = (crtp_message_t *) &msg->length;
if (c->port == CRTP_PORT_LOG) {
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(msg);
} else if (cmd == 1) { // GET_INFO
memset(&c->data[1], 0, 7);
c->size = 1 + 8;
send_message(msg);
}
}
else if (c->channel == 1) { // Log control
c->data[2] = 0; // Success
c->size = 3 + 1;
// resend message
send_message(msg);
}
else if (c->channel == 2) { // Log data
// nothing
}
}
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(msg);
}
}
}
else if (c->port == CRTP_PORT_PARAM) {
if (c->channel == 0) { // TOC Access
c->data[1] = 0; // Last parameter (id = 0)
memset(&c->data[2], 0, 10);
c->size = 1 + 8;
send_message(msg);
}
else if (c->channel == 1) { // Param read
// 0 is ok
c->data[1] = 0; // value
c->size = 1 + 2;
send_message(msg);
}
}
else if (c->port == CRTP_PORT_LINK) {
if (c->channel == 0) { // Echo
send_message(msg);
}
else if (c->channel == 1) { // Reply platform name
c->size = CRTP_MAX_DATA_SIZE;
bzero(c->data, CRTP_MAX_DATA_SIZE);
strcpy((char *)c->data, "Bitcraze PPRZ");
send_message(msg);
}
}
else {
// TODO handle error ?
}
}
/**
* Handle raw datalink
*
* From Bitcraze documentation:
*
* This packet carries the raw radio packet. The NRF51 acts as a radio bridge.
* Because the NRF51 does not have much memory and the STM32 is capable of bursting
* a lot of data a flow control rules has been made: The STM32 is allowed to send
* a RADIO_RAW packet only when one RADIO_RAW packet has been received.
*
* The NRF51 is regularly sending CRTP NULL packet or empty packets to the STM32
* to get the communication working both ways.
*
* Note: So far RADIO_RAW is the only syslink packet that has flow control constrain,
* all other packets can be sent full duplex at any moment.
*/
static void handle_raw(syslink_message_t *msg)
{
crtp_message_t *c = (crtp_message_t *) &msg->length;
if (CRTP_NULL(*c)) {
if (c->size >= 3) {
//handle_bootloader(sys);
}
}
else if (c->port == CRTP_PORT_COMMANDER) {
//crtp_commander *cmd = (crtp_commander *) &c->data[0];
// TODO set RC from cmd message
}
else if (c->port == CRTP_PORT_PPRZLINK) {
// fill rx buffer with CRTP data
uint8_t data_size = c->size;
uint16_t available = SYSLINK_RX_BUF_LEN - syslink.device.char_available(&syslink);
if (available > data_size) {
// enough room to add new bytes
if ((uint16_t)syslink.rx_insert_idx + (uint16_t)data_size < SYSLINK_RX_BUF_LEN) {
// copy in one block
memcpy(&syslink.rx_buf[syslink.rx_insert_idx], c->data, data_size);
} else {
// copy in two parts
uint16_t split = SYSLINK_RX_BUF_LEN - syslink.rx_insert_idx;
memcpy(&syslink.rx_buf[syslink.rx_insert_idx], c->data, split);
memcpy(&syslink.rx_buf[0], &c->data[split], data_size - split);
}
syslink.rx_insert_idx = (syslink.rx_insert_idx + data_size) % SYSLINK_RX_BUF_LEN;
}
}
else {
handle_raw_other(msg);
}
// send next raw message if fifo is not empty
if (syslink.tx_extract_idx != syslink.tx_insert_idx) {
PPRZ_MUTEX_LOCK(syslink_tx_mtx);
syslink_message_t msg_raw;
msg_raw.type = SYSLINK_RADIO_RAW;
memcpy(&msg_raw.length, &syslink.msg_tx[syslink.tx_extract_idx], sizeof(crtp_message_t));
send_message(&msg_raw);
// move fifo indexes
syslink.tx_extract_idx++;
if (syslink.tx_extract_idx == CRTP_BUF_LEN) {
syslink.tx_extract_idx = 0;
}
PPRZ_MUTEX_UNLOCK(syslink_tx_mtx);
}
}
static void handle_radio(syslink_message_t *msg)
{
if (msg->type == SYSLINK_RADIO_RSSI) {
uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
syslink.rssi = 140 - rssi * 100 / (100 - 40);
}
else if (msg->type == SYSLINK_RADIO_CHANNEL) {
// ack radio channel
}
else if (msg->type == SYSLINK_RADIO_DATARATE) {
// ack radio datarate
}
else if (msg->type == SYSLINK_RADIO_ADDRESS) {
// ack radio address
}
}
/** New RX message
*/
static void handle_new_msg(syslink_message_t *msg)
{
if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
// power button is hit
// don't do anything for now ?
}
else if (msg->type == SYSLINK_PM_BATTERY_STATE) {
handle_battery(msg);
}
else if (msg->type == SYSLINK_RADIO_RAW) {
handle_raw(msg);
}
else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
handle_radio(msg);
}
else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
// one-wire memory access
// don't do anything for now
}
else {
// handle errors ?
}
}
/**
* Implementation of syslink as generic device
*/
// check free space: nb of CRTP slots x space in a slot
static bool syslink_check_free_space(struct syslink_dl *s, long *fd UNUSED, uint16_t len)
{
int16_t slots = s->tx_extract_idx - s->tx_insert_idx;
if (slots <= 0) {
slots += CRTP_BUF_LEN;
}
uint16_t space = (uint16_t)(CRTP_MAX_DATA_SIZE * (slots - 1)) >= len;
if (space > 0) {
PPRZ_MUTEX_LOCK(syslink_tx_mtx);
}
return space;
}
// implementation of put_buffer, fill CRTP slots
static void syslink_put_buffer(struct syslink_dl *s, long fd UNUSED, const uint8_t *data, uint16_t len)
{
uint16_t buf_rem = len;
// fill slots until they are full
// we assume that the available space have been check before
while (buf_rem > 0) {
// get current slot
crtp_message_t *c = &s->msg_tx[s->tx_insert_idx];
if (c->size < CRTP_MAX_DATA_SIZE) {
// fill current buffer
uint16_t data_size = Min(buf_rem, CRTP_MAX_DATA_SIZE - c->size);
memcpy(&c->data[c->size - sizeof(c->header)], &data[len - buf_rem], data_size);
c->size += data_size;
buf_rem -= data_size;
}
else {
// start a new slot
uint8_t tmp = (s->tx_insert_idx + 1) % CRTP_BUF_LEN;
if (tmp == s->tx_extract_idx) {
// no more slots
// this should be be possible when check_free_space is called before
return;
}
crtp_message_t *c = &s->msg_tx[tmp];
uint16_t data_size = Min(buf_rem, CRTP_MAX_DATA_SIZE);
memcpy(c->data, &data[len - buf_rem], data_size);
c->size = sizeof(c->header) + data_size;
buf_rem -= data_size;
s->tx_insert_idx = tmp;
}
}
}
// implementation of put_byte using put_buffer
static void syslink_put_byte(struct syslink_dl *s, long fd, const uint8_t b)
{
syslink_put_buffer(s, fd, &b, 1);
}
// send_message is not needed as messages are stored in a fifo
static void syslink_send_message(struct syslink_dl *s UNUSED, long fd UNUSED)
{
PPRZ_MUTEX_UNLOCK(syslink_tx_mtx); // release mutex
}
static uint8_t syslink_getch(struct syslink_dl *s)
{
uint8_t ret = s->rx_buf[s->rx_extract_idx];
s->rx_extract_idx = (s->rx_extract_idx + 1) % SYSLINK_RX_BUF_LEN;
return ret;
}
static uint16_t syslink_char_available(struct syslink_dl *s)
{
int16_t available = s->rx_insert_idx - s->rx_extract_idx;
if (available < 0) {
available += SYSLINK_RX_BUF_LEN;
}
return (uint16_t)available;
}
/** Init function */
void syslink_dl_init(void)
{
syslink_parse_init(&syslink.state);
syslink.tx_insert_idx = 0;
syslink.tx_extract_idx = 0;
syslink.rssi = 0;
syslink.charging = false;
syslink.powered = false;
for (int i = 0; i < CRTP_BUF_LEN; i++) {
// prepare raw pprzlink datalink headers
syslink.msg_tx[i].header = 0;
syslink.msg_tx[i].port = CRTP_PORT_PPRZLINK;
syslink.msg_tx[i].channel = i % 4;
syslink.msg_tx[i].size = sizeof(syslink.msg_tx[i].header);
}
// generic device
syslink.device.periph = (void *)(&syslink);
syslink.device.check_free_space = (check_free_space_t) syslink_check_free_space;
syslink.device.put_byte = (put_byte_t) syslink_put_byte;
syslink.device.put_buffer = (put_buffer_t) syslink_put_buffer;
syslink.device.send_message = (send_message_t) syslink_send_message;
syslink.device.char_available = (char_available_t) syslink_char_available;
syslink.device.get_byte = (get_byte_t) syslink_getch;
// init mutex if needed
PPRZ_MUTEX_INIT(syslink_tx_mtx);
}
/** Periodic function */
void syslink_dl_periodic(void)
{
#ifdef CHARGING_LED
if (syslink.charging) {
LED_TOGGLE(CHARGING_LED);
}
else if (syslink.powered) {
LED_ON(CHARGING_LED);
}
else {
LED_OFF(CHARGING_LED);
}
#endif
}
/** Datalink event */
void syslink_dl_event(void)
{
while (uart_char_available(&(SYSLINK_DEV))) {
uint8_t ch = uart_getch(&(SYSLINK_DEV));
if (syslink_parse_char(&syslink.state, ch, &syslink.msg_rx)) {
handle_new_msg(&syslink.msg_rx);
}
}
}
sw/airborne/modules/datalink/bitcraze/syslink_dl.h
+70
View File
@@ -0,0 +1,70 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/datalink/bitcraze/syslink_dl.h
*
* Syslink protocol handling and functionalities
*
*/
#ifndef SYSLINK_DL_H
#define SYSLINK_DL_H
#include "modules/datalink/bitcraze/syslink.h"
#include "modules/datalink/bitcraze/crtp.h"
#include "pprzlink/pprzlink_device.h"
#define CRTP_BUF_LEN 16
#define SYSLINK_RX_BUF_LEN 256
struct syslink_dl {
// syslink structures
syslink_parse_state state; ///< syslink parser state
syslink_message_t msg_rx; ///< last received syslink message
crtp_message_t msg_tx[CRTP_BUF_LEN]; ///< queued crtp packets to be send
uint8_t tx_insert_idx; ///< tx insert index
uint8_t tx_extract_idx; ///< tx extract index
uint8_t rx_buf[SYSLINK_RX_BUF_LEN]; ///< received pprzlink bytes from syslink/crtp
uint8_t rx_insert_idx; ///< rx insert index
uint8_t rx_extract_idx; ///< rx extract index
// generic device to use pprzlink over syslink
struct link_device device;
// crazyflie state
uint8_t rssi;
bool charging; ///< battery charging
bool powered; ///< USB powered
};
extern struct syslink_dl syslink;
/** Init function */
extern void syslink_dl_init(void);
/** Periodic function */
extern void syslink_dl_periodic(void);
/** Datalink event */
extern void syslink_dl_event(void);
#endif
sw/airborne/modules/datalink/pprz_dl.h
+3
View File
@@ -35,6 +35,9 @@
#if USE_UDP
#include "mcu_periph/udp.h"
#endif
#if USE_SYSLINK
#include "modules/datalink/bitcraze/syslink_dl.h"
#endif
/** PPRZ transport structure */
extern struct pprz_transport pprz_tp;
sw/airborne/modules/imu/imu_bmi088_i2c.c
+145
View File
@@ -0,0 +1,145 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/imu/imu_bmi088_i2c.c
*
* IMU driver for the BMI088 using I2C
*
*/
#include "subsystems/imu.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/abi.h"
PRINT_CONFIG_VAR(IMU_BMI088_GYRO_RANGE)
PRINT_CONFIG_VAR(IMU_BMI088_ACCEL_RANGE)
#ifndef IMU_BMI088_GYRO_ODR
#define IMU_BMI088_GYRO_ODR BMI088_GYRO_ODR_1000_BW_116
#endif
PRINT_CONFIG_VAR(IMU_BMI088_GYRO_ODR)
#ifndef IMU_BMI088_ACCEL_ODR
#define IMU_BMI088_ACCEL_ODR BMI088_ACCEL_ODR_1600
#endif
PRINT_CONFIG_VAR(IMU_BMI088_ACCEL_ODR)
#ifndef IMU_BMI088_ACCEL_BW
#define IMU_BMI088_ACCEL_BW BMI088_ACCEL_BW_OSR4
#endif
PRINT_CONFIG_VAR(IMU_BMI088_ACCEL_BW)
#ifndef IMU_BMI088_GYRO_I2C_ADDR
#define IMU_BMI088_GYRO_I2C_ADDR BMI088_GYRO_ADDR
#endif
PRINT_CONFIG_VAR(IMU_BMI088_GYRO_I2C_ADDR)
#ifndef IMU_BMI088_ACCEL_I2C_ADDR
#define IMU_BMI088_ACCEL_I2C_ADDR BMI088_ACCEL_ADDR
#endif
PRINT_CONFIG_VAR(IMU_BMI088_ACCEL_I2C_ADDR)
// Default channels order
#ifndef IMU_BMI088_CHAN_X
#define IMU_BMI088_CHAN_X 0
#endif
PRINT_CONFIG_VAR(IMU_BMI088_CHAN_X)
#ifndef IMU_BMI088_CHAN_Y
#define IMU_BMI088_CHAN_Y 1
#endif
PRINT_CONFIG_VAR(IMU_BMI088_CHAN_Y)
#ifndef IMU_BMI088_CHAN_Z
#define IMU_BMI088_CHAN_Z 2
#endif
PRINT_CONFIG_VAR(IMU_BMI088_CHAN_Z)
#ifndef IMU_BMI088_X_SIGN
#define IMU_BMI088_X_SIGN 1
#endif
PRINT_CONFIG_VAR(IMU_BMI088_X_SIGN)
#ifndef IMU_BMI088_Y_SIGN
#define IMU_BMI088_Y_SIGN 1
#endif
PRINT_CONFIG_VAR(IMU_BMI088_Y_SIGN)
#ifndef IMU_BMI088_Z_SIGN
#define IMU_BMI088_Z_SIGN 1
#endif
PRINT_CONFIG_VAR(IMU_BMI088_Z_SIGN)
struct ImuBmi088 imu_bmi088;
void imu_bmi088_init(void)
{
/* BMI088 */
bmi088_i2c_init(&imu_bmi088.bmi, &(IMU_BMI088_I2C_DEV), IMU_BMI088_GYRO_I2C_ADDR, IMU_BMI088_ACCEL_I2C_ADDR);
// change the default configuration
imu_bmi088.bmi.config.gyro_range = IMU_BMI088_GYRO_RANGE;
imu_bmi088.bmi.config.gyro_odr = IMU_BMI088_GYRO_ODR;
imu_bmi088.bmi.config.accel_range = IMU_BMI088_ACCEL_RANGE;
imu_bmi088.bmi.config.accel_odr = IMU_BMI088_ACCEL_ODR;
imu_bmi088.bmi.config.accel_bw = IMU_BMI088_ACCEL_BW;
}
void imu_bmi088_periodic(void)
{
bmi088_i2c_periodic(&imu_bmi088.bmi);
}
void imu_bmi088_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the BMI088 I2C transaction has succeeded: convert the data
bmi088_i2c_event(&imu_bmi088.bmi);
if (imu_bmi088.bmi.gyro_available) {
struct Int32Rates rates = {
IMU_BMI088_X_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_X]),
IMU_BMI088_Y_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_Y]),
IMU_BMI088_Z_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_Z])
};
// unscaled vector
RATES_COPY(imu.gyro_unscaled, rates);
imu_bmi088.bmi.gyro_available = false;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BMI088_ID, now_ts, &imu.gyro);
}
if (imu_bmi088.bmi.accel_available) {
// set channel order
struct Int32Vect3 accel = {
IMU_BMI088_X_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_X]),
IMU_BMI088_Y_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_Y]),
IMU_BMI088_Z_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
imu_bmi088.bmi.accel_available = false;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BMI088_ID, now_ts, &imu.accel);
}
}
sw/airborne/modules/imu/imu_bmi088_i2c.h
+83
View File
@@ -0,0 +1,83 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/imu/imu_bmi088_i2c.h
*
* IMU driver for the BMI088 using I2C
*
*/
#ifndef IMU_BMI088_I2C_H
#define IMU_BMI088_I2C_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "peripherals/bmi088_i2c.h"
#ifndef IMU_BMI088_GYRO_RANGE
#define IMU_BMI088_GYRO_RANGE BMI088_GYRO_RANGE_1000
#endif
#ifndef IMU_BMI088_ACCEL_RANGE
#define IMU_BMI088_ACCEL_RANGE BMI088_ACCEL_RANGE_6G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1]
#endif
struct ImuBmi088 {
struct Bmi088_I2c bmi;
};
extern struct ImuBmi088 imu_bmi088;
extern void imu_bmi088_init(void);
extern void imu_bmi088_periodic(void);
extern void imu_bmi088_event(void);
#endif /* IMU_BMI088_I2C_H */
sw/airborne/peripherals/bmi088.c
+115
View File
@@ -0,0 +1,115 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/bmi088.c
*
* BMI088 driver common functions (I2C and SPI).
*
* Still needs the either I2C or SPI specific implementation.
*/
#include "peripherals/bmi088.h"
const float BMI088_GYRO_SENS[5] = {
BMI088_GYRO_SENS_2000,
BMI088_GYRO_SENS_1000,
BMI088_GYRO_SENS_500,
BMI088_GYRO_SENS_250,
BMI088_GYRO_SENS_125,
};
const int32_t BMI088_GYRO_SENS_FRAC[5][2] = {
{ BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_DEN },
{ BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_DEN },
{ BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_DEN },
{ BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_DEN },
{ BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_DEN },
};
const float BMI088_ACCEL_SENS[4] = {
BMI088_ACCEL_SENS_3G,
BMI088_ACCEL_SENS_6G,
BMI088_ACCEL_SENS_12G,
BMI088_ACCEL_SENS_24G
};
const int32_t BMI088_ACCEL_SENS_FRAC[4][2] = {
{ BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_DEN },
{ BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_DEN },
{ BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_DEN },
{ BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_DEN }
};
void bmi088_set_default_config(struct Bmi088Config *c)
{
c->gyro_range = BMI088_DEFAULT_GYRO_RANGE;
c->gyro_odr = BMI088_DEFAULT_GYRO_ODR;
c->accel_range = BMI088_DEFAULT_ACCEL_RANGE;
c->accel_odr = BMI088_DEFAULT_ACCEL_ODR;
c->accel_bw = BMI088_DEFAULT_ACCEL_BW;
}
void bmi088_send_config(Bmi088ConfigSet bmi_set, void *bmi, struct Bmi088Config *config)
{
switch (config->init_status) {
case BMI088_CONF_ACCEL_RANGE:
/* configure accelerometer range */
bmi_set(bmi, BMI088_ACCEL_RANGE, config->accel_range, BMI088_CONFIG_ACCEL);
config->init_status++;
break;
case BMI088_CONF_ACCEL_ODR:
/* configure accelerometer odr and bw */
bmi_set(bmi, BMI088_ACCEL_CONF, ((config->accel_bw << 4) | config->accel_odr), BMI088_CONFIG_ACCEL);
config->init_status++;
break;
case BMI088_CONF_ACCEL_PWR_CONF:
/* switch on accel */
bmi_set(bmi, BMI088_ACCEL_PWR_CONF, BMI088_ACCEL_ACTIVE, BMI088_CONFIG_ACCEL);
config->init_status++;
break;
case BMI088_CONF_ACCEL_PWR_CTRL:
/* switch on accel */
bmi_set(bmi, BMI088_ACCEL_PWR_CTRl, BMI088_ACCEL_POWER_ON, BMI088_CONFIG_ACCEL);
config->init_status++;
break;
case BMI088_CONF_GYRO_RANGE:
/* configure gyro range */
bmi_set(bmi, BMI088_GYRO_RANGE, config->gyro_range, BMI088_CONFIG_GYRO);
config->init_status++;
break;
case BMI088_CONF_GYRO_ODR:
/* configure gyro odr */
bmi_set(bmi, BMI088_GYRO_BAND_WIDTH, config->gyro_odr, BMI088_CONFIG_GYRO);
config->init_status++;
break;
case BMI088_CONF_GYRO_PWR:
/* switch on gyro */
bmi_set(bmi, BMI088_GYRO_LPM_1, BMI088_GYRO_NORMAL, BMI088_CONFIG_GYRO);
config->init_status++;
break;
case BMI088_CONF_DONE:
config->initialized = true;
break;
default:
break;
}
}
sw/airborne/peripherals/bmi088.h
+128
View File
@@ -0,0 +1,128 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/bmi088.h
*
* BMI088 driver common interface (I2C and SPI).
*/
#ifndef BMI088_H
#define BMI088_H
#include "std.h"
/* Include address and register definition */
#include "peripherals/bmi088_regs.h"
/// Default gyro full scale range +- 1000°/s
#define BMI088_DEFAULT_GYRO_RANGE BMI088_GYRO_RANGE_1000
/// Default gyro output rate
#define BMI088_DEFAULT_GYRO_ODR BMI088_GYRO_ODR_400_BW_47
/// Default accel full scale range +- 6g
#define BMI088_DEFAULT_ACCEL_RANGE BMI088_ACCEL_RANGE_6G
/// Default accel output rate
#define BMI088_DEFAULT_ACCEL_ODR BMI088_ACCEL_ODR_400
/// Default accel bandwidth
#define BMI088_DEFAULT_ACCEL_BW BMI088_ACCEL_BW_NORMAL
/** default gyro sensitivy from the datasheet
* sens = 1/ [LSB/(deg/s)] * pi/180 * 2^INT32_RATE_FRAC
* ex: BMI with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
*/
#define BMI088_GYRO_SENS_125 0.272442 // FIXME
#define BMI088_GYRO_SENS_125_NUM 19327
#define BMI088_GYRO_SENS_125_DEN 17735
#define BMI088_GYRO_SENS_250 0.544883
#define BMI088_GYRO_SENS_250_NUM 19327
#define BMI088_GYRO_SENS_250_DEN 35470
#define BMI088_GYRO_SENS_500 1.08977
#define BMI088_GYRO_SENS_500_NUM 57663
#define BMI088_GYRO_SENS_500_DEN 52913
#define BMI088_GYRO_SENS_1000 2.17953
#define BMI088_GYRO_SENS_1000_NUM 18271
#define BMI088_GYRO_SENS_1000_DEN 8383
#define BMI088_GYRO_SENS_2000 4.35906
#define BMI088_GYRO_SENS_2000_NUM 36542
#define BMI088_GYRO_SENS_2000_DEN 8383
// Get default sensitivity from a table
extern const float BMI088_GYRO_SENS[5];
// Get default sensitivity numerator and denominator from a table
extern const int32_t BMI088_GYRO_SENS_FRAC[5][2];
/** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
* ex: BMI with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
// FIXME
#define BMI088_ACCEL_SENS_3G 0.919687
#define BMI088_ACCEL_SENS_3G_NUM 9197
#define BMI088_ACCEL_SENS_3G_DEN 10000
#define BMI088_ACCEL_SENS_6G 1.83937
#define BMI088_ACCEL_SENS_6G_NUM 18394
#define BMI088_ACCEL_SENS_6G_DEN 10000
#define BMI088_ACCEL_SENS_12G 3.67875
#define BMI088_ACCEL_SENS_12G_NUM 36787
#define BMI088_ACCEL_SENS_12G_DEN 10000
#define BMI088_ACCEL_SENS_24G 7.3575
#define BMI088_ACCEL_SENS_24G_NUM 7357
#define BMI088_ACCEL_SENS_24G_DEN 1000
// Get default sensitivity from a table
extern const float BMI088_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
extern const int32_t BMI088_ACCEL_SENS_FRAC[4][2];
enum Bmi088ConfStatus {
BMI088_CONF_UNINIT,
BMI088_CONF_ACCEL_RANGE,
BMI088_CONF_ACCEL_ODR,
BMI088_CONF_ACCEL_PWR_CONF,
BMI088_CONF_ACCEL_PWR_CTRL,
BMI088_CONF_GYRO_RANGE,
BMI088_CONF_GYRO_ODR,
BMI088_CONF_GYRO_PWR,
BMI088_CONF_DONE
};
#define BMI088_CONFIG_ACCEL 0
#define BMI088_CONFIG_GYRO 1
/// Configuration function prototype
typedef void (*Bmi088ConfigSet)(void *bmi, uint8_t _reg, uint8_t _val, uint8_t _type);
struct Bmi088Config {
enum Bmi088GyroRanges gyro_range; ///< deg/s Range
enum Bmi088GyroODR gyro_odr; ///< output data rate
enum Bmi088AccelRanges accel_range; ///< g Range
enum Bmi088AccelODR accel_odr; ///< output data rate
enum Bmi088AccelBW accel_bw; ///< bandwidth
enum Bmi088ConfStatus init_status; ///< init status
bool initialized; ///< config done flag
};
extern void bmi088_set_default_config(struct Bmi088Config *c);
/// Configuration sequence called once before normal use
extern void bmi088_send_config(Bmi088ConfigSet bmi_set, void *bmi, struct Bmi088Config *config);
#endif // BMI088_H
sw/airborne/peripherals/bmi088_i2c.c
+158
View File
@@ -0,0 +1,158 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/bmi088_i2c.c
*
* Driver for the BMI088 using I2C.
*
*/
#include "peripherals/bmi088_i2c.h"
void bmi088_i2c_init(struct Bmi088_I2c *bmi, struct i2c_periph *i2c_p, uint8_t gyro_addr, uint8_t accel_addr)
{
/* set i2c_peripheral */
bmi->i2c_p = i2c_p;
/* slave address */
bmi->gyro_trans.slave_addr = gyro_addr;
bmi->accel_trans.slave_addr = accel_addr;
/* set inital status: Success or Done */
bmi->gyro_trans.status = I2CTransDone;
bmi->accel_trans.status = I2CTransDone;
/* set default BMI088 config options */
bmi088_set_default_config(&(bmi->config));
bmi->gyro_available = false;
bmi->accel_available = false;
bmi->config.initialized = false;
bmi->config.init_status = BMI088_CONF_UNINIT;
}
static void bmi088_i2c_write_to_reg(void *bmi, uint8_t _reg, uint8_t _val, uint8_t _type)
{
struct Bmi088_I2c *bmi_i2c = (struct Bmi088_I2c *)(bmi);
if (_type == BMI088_CONFIG_ACCEL) {
bmi_i2c->accel_trans.buf[0] = _reg;
bmi_i2c->accel_trans.buf[1] = _val;
i2c_transmit(bmi_i2c->i2c_p, &(bmi_i2c->accel_trans), bmi_i2c->accel_trans.slave_addr, 2);
} else if (_type == BMI088_CONFIG_GYRO) {
bmi_i2c->gyro_trans.buf[0] = _reg;
bmi_i2c->gyro_trans.buf[1] = _val;
i2c_transmit(bmi_i2c->i2c_p, &(bmi_i2c->gyro_trans), bmi_i2c->gyro_trans.slave_addr, 2);
}
}
// Configuration function called once before normal use
void bmi088_i2c_start_configure(struct Bmi088_I2c *bmi)
{
if (bmi->config.init_status == BMI088_CONF_UNINIT) {
bmi->config.init_status++;
if (bmi->accel_trans.status == I2CTransSuccess || bmi->accel_trans.status == I2CTransDone) {
bmi088_send_config(bmi088_i2c_write_to_reg, (void *)bmi, &(bmi->config));
}
}
}
void bmi088_i2c_read(struct Bmi088_I2c *bmi)
{
if (bmi->config.initialized &&
bmi->gyro_trans.status == I2CTransDone &&
bmi->accel_trans.status == I2CTransDone) {
/* read gyro */
bmi->gyro_trans.buf[0] = BMI088_GYRO_RATE_X_LSB;
i2c_transceive(bmi->i2c_p, &(bmi->gyro_trans), bmi->gyro_trans.slave_addr, 1, 6); //9);
/* read accel */
bmi->accel_trans.buf[0] = BMI088_ACCEL_X_LSB;
i2c_transceive(bmi->i2c_p, &(bmi->accel_trans), bmi->accel_trans.slave_addr, 1, 6); // 12);
}
}
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
void bmi088_i2c_event(struct Bmi088_I2c *bmi)
{
if (bmi->config.initialized) {
// Check gyro read
if (bmi->gyro_trans.status == I2CTransFailed) {
bmi->gyro_trans.status = I2CTransDone;
} else if (bmi->gyro_trans.status == I2CTransSuccess) {
// Successfull reading
//if (bit_is_set(bmi->gyro_trans.buf[8], 7)) {
// new data
bmi->data_rates.rates.p = Int16FromBuf(bmi->gyro_trans.buf, 0);
bmi->data_rates.rates.q = Int16FromBuf(bmi->gyro_trans.buf, 2);
bmi->data_rates.rates.r = Int16FromBuf(bmi->gyro_trans.buf, 4);
bmi->gyro_available = true;
//}
bmi->gyro_trans.status = I2CTransDone;
}
// Check accel read
if (bmi->accel_trans.status == I2CTransFailed) {
bmi->accel_trans.status = I2CTransDone;
} else if (bmi->accel_trans.status == I2CTransSuccess) {
// Successfull reading
//if (bit_is_set(bmi->accel_trans.buf[11], 7)) {
// new data
bmi->data_accel.vect.x = Int16FromBuf(bmi->accel_trans.buf, 0);
bmi->data_accel.vect.y = Int16FromBuf(bmi->accel_trans.buf, 2);
bmi->data_accel.vect.z = Int16FromBuf(bmi->accel_trans.buf, 4);
bmi->accel_available = true;
//}
bmi->accel_trans.status = I2CTransDone;
}
} else if (bmi->config.init_status != BMI088_CONF_UNINIT) { // Configuring but not yet initialized
if (bmi->config.init_status <= BMI088_CONF_ACCEL_PWR_CTRL) {
// Accel config not finished
switch (bmi->accel_trans.status) {
case I2CTransFailed:
bmi->config.init_status--; // Retry config (TODO max retry)
/* Falls through. */
case I2CTransSuccess:
case I2CTransDone:
bmi088_send_config(bmi088_i2c_write_to_reg, (void *)bmi, &(bmi->config));
break;
default:
break;
}
}
else {
// gyro config not finished
switch (bmi->gyro_trans.status) {
case I2CTransFailed:
bmi->config.init_status--; // Retry config (TODO max retry)
/* Falls through. */
case I2CTransSuccess:
case I2CTransDone:
bmi088_send_config(bmi088_i2c_write_to_reg, (void *)bmi, &(bmi->config));
if (bmi->config.initialized) {
bmi->gyro_trans.status = I2CTransDone;
}
break;
default:
break;
}
}
}
}
sw/airborne/peripherals/bmi088_i2c.h
+70
View File
@@ -0,0 +1,70 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/bmi088_i2c.h
*
* Driver for the BMI088 using I2C.
*/
#ifndef BMI088_I2C_H
#define BMI088_I2C_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "mcu_periph/i2c.h"
/* Include common BMI088 options and definitions */
#include "peripherals/bmi088.h"
struct Bmi088_I2c {
struct i2c_periph *i2c_p;
struct i2c_transaction gyro_trans; ///< i2c transaction for gyro
struct i2c_transaction accel_trans; ///< i2c transaction for accel
volatile bool gyro_available; ///< gyro data ready flag
volatile bool accel_available; ///< accel data ready flag
union {
struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
int16_t value[3]; ///< accel data values accessible by channel index
} data_accel;
union {
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
int16_t value[3]; ///< rates data values accessible by channel index
} data_rates;
struct Bmi088Config config;
};
// Functions
extern void bmi088_i2c_init(struct Bmi088_I2c *bmi, struct i2c_periph *i2c_p, uint8_t gyro_addr, uint8_t accel_addr);
extern void bmi088_i2c_start_configure(struct Bmi088_I2c *bmi);
extern void bmi088_i2c_read(struct Bmi088_I2c *bmi);
extern void bmi088_i2c_event(struct Bmi088_I2c *bmi);
/// convenience function: read or start configuration if not already initialized
static inline void bmi088_i2c_periodic(struct Bmi088_I2c *bmi)
{
if (bmi->config.initialized) {
bmi088_i2c_read(bmi);
} else {
bmi088_i2c_start_configure(bmi);
}
}
#endif // BMI088_I2C_H
sw/airborne/peripherals/bmi088_regs.h
+180
View File
@@ -0,0 +1,180 @@
/*
* Copyright (C) 2019 Gautier Hattenberger <gaurier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/bmi088_regs.h
*
* Register and address definitions for BMI088.
*/
#ifndef BMI088_REGS_H
#define BMI088_REGS_H
/* default I2C address */
#define BMI088_ACCEL_ADDR (0x18<<1)
#define BMI088_ACCEL_ADDR_ALT (0x19<<1)
#define BMI088_ACCEL_CHIP_ID 0x00 // Default value 0x1E
#define BMI088_ACCEL_ERR_REG 0x02
#define BMI088_ACCEL_STATUS 0x03
#define BMI088_ACCEL_X_LSB 0x12
#define BMI088_ACCEL_X_MSB 0x13
#define BMI088_ACCEL_Y_LSB 0x14
#define BMI088_ACCEL_Y_MSB 0x15
#define BMI088_ACCEL_Z_LSB 0x16
#define BMI088_ACCEL_Z_MSB 0x17
#define BMI088_ACCEL_SENSOR_TIME_0 0x18
#define BMI088_ACCEL_SENSOR_TIME_1 0x19
#define BMI088_ACCEL_SENSOR_TIME_2 0x1A
#define BMI088_ACCEL_INT_STAT_1 0x1D
#define BMI088_ACCEL_TEMP_MSB 0x22
#define BMI088_ACCEL_TEMP_LSB 0x23
#define BMI088_ACCEL_CONF 0x40
#define BMI088_ACCEL_RANGE 0x41
#define BMI088_ACCEL_INT1_IO_CTRL 0x53
#define BMI088_ACCEL_INT2_IO_CTRL 0x54
#define BMI088_ACCEL_INT_MAP_DATA 0x58
#define BMI088_ACCEL_SELF_TEST 0x6D
#define BMI088_ACCEL_PWR_CONF 0x7C
#define BMI088_ACCEL_PWR_CTRl 0x7D
#define BMI088_ACCEL_SOFT_RESET 0x7E
#define BMI088_GYRO_ADDR (0x68<<1)
#define BMI088_GYRO_ADDR_ALT (0x69<<1)
#define BMI088_GYRO_CHIP_ID 0x00 // Default value 0x0F
#define BMI088_GYRO_RATE_X_LSB 0x02
#define BMI088_GYRO_RATE_X_MSB 0x03
#define BMI088_GYRO_RATE_Y_LSB 0x04
#define BMI088_GYRO_RATE_Y_MSB 0x05
#define BMI088_GYRO_RATE_Z_LSB 0x06
#define BMI088_GYRO_RATE_Z_MSB 0x07
#define BMI088_GYRO_INT_STAT_1 0x0A
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_BAND_WIDTH 0x10
#define BMI088_GYRO_LPM_1 0x11
#define BMI088_GYRO_SOFT_RESET 0x14
#define BMI088_GYRO_INT_CTRL 0x15
#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_SELF_TEST 0x3C
/** Accel output range
*/
enum Bmi088AccelRanges
{
BMI088_ACCEL_RANGE_3G = 0x00,
BMI088_ACCEL_RANGE_6G = 0x01,
BMI088_ACCEL_RANGE_12G = 0x02,
BMI088_ACCEL_RANGE_24G = 0x03,
};
/** Accel outpur data rate
*/
enum Bmi088AccelODR
{
BMI088_ACCEL_ODR_12 = 0x05,
BMI088_ACCEL_ODR_25 = 0x06,
BMI088_ACCEL_ODR_50 = 0x07,
BMI088_ACCEL_ODR_100 = 0x08,
BMI088_ACCEL_ODR_200 = 0x09,
BMI088_ACCEL_ODR_400 = 0x0A,
BMI088_ACCEL_ODR_800 = 0x0B,
BMI088_ACCEL_ODR_1600 = 0x0C,
};
/** Accel bandwith
*/
enum Bmi088AccelBW
{
BMI088_ACCEL_BW_OSR4 = 0x08,
BMI088_ACCEL_BW_OSR2 = 0x09,
BMI088_ACCEL_BW_NORMAL = 0x0A,
};
/** Accel power type
*/
enum Bmi088AccelPowerType
{
BMI088_ACCEL_ACTIVE = 0x00,
BMI088_ACCEL_SUSPEND = 0x03,
};
/** Accel power control
*/
enum Bmi088AccelPowerCtrl
{
BMI088_ACCEL_POWER_OFF = 0x00,
BMI088_ACCEL_POWER_ON = 0x04,
};
/** Gyro output range
*/
enum Bmi088GyroRanges
{
BMI088_GYRO_RANGE_2000 = 0x00,
BMI088_GYRO_RANGE_1000 = 0x01,
BMI088_GYRO_RANGE_500 = 0x02,
BMI088_GYRO_RANGE_250 = 0x03,
BMI088_GYRO_RANGE_125 = 0x04,
};
/** Gyro output data rate and bandwidth
*/
enum Bmi088GyroODR
{
BMI088_GYRO_ODR_2000_BW_532 = 0x00,
BMI088_GYRO_ODR_2000_BW_230 = 0x01,
BMI088_GYRO_ODR_1000_BW_116 = 0x02,
BMI088_GYRO_ODR_400_BW_47 = 0x03,
BMI088_GYRO_ODR_200_BW_23 = 0x04,
BMI088_GYRO_ODR_100_BW_12 = 0x05,
BMI088_GYRO_ODR_200_BW_64 = 0x06,
BMI088_GYRO_ODR_100_BW_32 = 0x07,
};
/** Gyro power type
*/
enum Bmi088GyroPowerType
{
BMI088_GYRO_NORMAL = 0x00,
BMI088_GYRO_SUSPEND = 0x80,
BMI088_GYRO_DEEP_SUSPEND = 0x20,
};
#endif /* BMI088_REGS_H */
sw/airborne/subsystems/abi_sender_ids.h
+4
View File
@@ -323,6 +323,10 @@
#define IMU_VECTORNAV_ID 18
#endif
#ifndef IMU_BMI088_ID
#define IMU_BMI088_ID 19
#endif
// prefiltering with OneEuro filter
#ifndef IMU_F1E_ID
#define IMU_F1E_ID 30
sw/airborne/subsystems/ahrs.h
+1
View File
@@ -42,6 +42,7 @@
#define AHRS_COMP_ID_CHIMU 10
#define AHRS_COMP_ID_VECTORNAV 11
#define AHRS_COMP_ID_EKF2 12
#define AHRS_COMP_ID_MADGWICK 13
/* include actual (primary) implementation header */
#ifdef AHRS_TYPE_H
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: d9173a6dc9...3da2eefa3d
sw/ground_segment/python/bitcraze/crazyradio2ivy.py
Executable
+172
View File
@@ -0,0 +1,172 @@
#!/usr/bin/env python3
"""
Bridge a Crazyflie connected to a Crazyradio to the Ivy software bus
with support of PPRZLINK messages
Requires 'pip3 install cflib'
As the ESB protocol works using PTX and PRX (Primary Transmitter/Reciever)
modes. Thus, data is only recieved as a response to a sent packet.
So, we need to constantly poll the receivers for bidirectional communication.
@author: Dennis Shtatnov (densht@gmail.com)
Gautier Hattenberger for Paparazzi UAV support
"""
# import struct
from os import path, getenv
import logging
import sys
import time
from cflib.crtp.crtpstack import CRTPPacket
from cflib.crtp import RadioDriver
# if PAPARAZZI_HOME not set, then assume the tree containing this
# file is a reasonable substitute
PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
sys.path.append(PPRZ_HOME + "/var/lib/python")
from pprzlink.ivy import IvyMessagesInterface
from pprzlink.pprz_transport import PprzTransport
from pprzlink.message import PprzMessage
import pprzlink.messages_xml_map as messages_xml_map
CRTP_PORT_PPRZLINK = 9
# Only output errors from the logging framework
#logging.basicConfig(level=logging.DEBUG)
logging.basicConfig(level=logging.ERROR)
class RadioBridge:
def __init__(self, link_uri, msg_class='telemetry', verbose=False):
""" Initialize and run with the specified link_uri """
self.verbose = verbose
# Ivy interface and stream parser
self._ivy = IvyMessagesInterface("cf2ivy")
self._transport = PprzTransport(msg_class)
# Create a Crazyradio
self._driver = RadioDriver()
self._driver.connect(link_uri, self._link_quality_cb, self._link_error_cb)
if self.verbose:
print('Connecting to %s' % link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
# Bind to all messages from ac_id
def _forward_to_cf(ac_id, msg):
try:
data = self._transport.pack_pprz_msg(0, msg) # sender_id 0 = GCS
for i in range(0, len(data), 30):
pk = CRTPPacket()
pk.port = CRTP_PORT_PPRZLINK
pk.data = data[i:(i+30)]
self._driver.send_packet(pk)
if self.verbose:
print('Forward message', msg.name)
except:
if self.verbose:
print('Forward error for', ac_id)
messages_datalink = messages_xml_map.get_msgs("datalink")
for msg in messages_datalink:
self._ivy.subscribe(_forward_to_cf, PprzMessage("datalink", msg))
def shutdown(self):
if self.verbose:
print('closing cf2ivy interfaces')
self._ivy.shutdown()
self._driver.close()
def run(self):
pk = self._driver.receive_packet(0.1) # wait for next message with timeout
if pk is not None:
self._got_packet(pk)
def _got_packet(self, pk):
if pk.port == CRTP_PORT_PPRZLINK:
for c in pk.data:
if self._transport.parse_byte(bytes([c])):
(sender_id, _, _, msg) = self._transport.unpack()
if self.verbose:
print("Got message {} from {}".format(msg.name, sender_id))
# Forward message to Ivy bus
if self.is_connected:
self._ivy.send(msg, sender_id=sender_id)
def _link_quality_cb(self, quality):
pass
def _link_error_cb(self, msg):
if self.verbose:
print("Link error: {}".format(msg))
if __name__ == '__main__':
from argparse import ArgumentParser
parser = ArgumentParser(description="Crazyradio link for paparazzi")
parser.add_argument('-a','--address', default=None, help="URI address of Crazyflie")
parser.add_argument('-c','--chanel', default='80', help="URI chanel of Crazyflie (full URI will be 'radio://0/%(default)/2M'")
parser.add_argument('-u','--uri', default=None, help="URI of Crazyflie (chanel option will not be effective)")
parser.add_argument('-b','--bus', default=None, help="Ivy bus [default to system IVY bus]")
parser.add_argument('-s','--scan', action='store_true', help="Scan available Crazyflie at startup")
parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages")
args = parser.parse_args()
if args.scan:
import cflib.crtp
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.radiodriver.set_retries_before_disconnect(1500)
cflib.crtp.radiodriver.set_retries(3)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
if args.address is not None:
address = int(args.address, 16)
else:
address = None # equivalent to default 0xE7E7E7E7E7
available = cflib.crtp.scan_interfaces(address)
if len(available) > 0:
print('Crazyflies found:')
for i in available:
print(' ',i[0])
else:
print('No radio. Leaving')
sys.exit(1)
bridge = None
link_uri = None
if args.uri is not None:
link_uri = args.uri
else:
link_uri = 'radio://0/' + args.chanel + '/2M'
try:
# Start radio to ivy bridge
bridge = RadioBridge(link_uri, verbose=args.verbose)
# The Crazyflie lib doesn't contain anything to keep the application alive,
# so this is where your application should do something. In our case we
# are just waiting until we are disconnected.
while bridge.is_connected:
bridge.run()
except KeyboardInterrupt:
pass
except Exception as e:
print("Failing with error:", e)
if bridge is not None:
bridge.is_connected = False
bridge.shutdown()
time.sleep(1)
sys.exit()