Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 11:37:06 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #2881 from fvantienen/cleanup_imu

Browse Source 
[imu] Cleanup
This commit is contained in:
Gautier Hattenberger
2022-05-19 23:54:31 +02:00
committed by GitHub
parent 3211fa011c dcca44e591
commit bb0a27187e
45 changed files with 0 additions and 3960 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/Makefile.swing
-37
View File
@@ -1,37 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
#
# Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
# Copyright (C) 2017 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/>.
#
#
# This Makefile uses the generic Makefile.arm-linux and adds upload rules for the Parrot Swing
#
include $(PAPARAZZI_SRC)/conf/Makefile.arm-linux
DRONE = $(PAPARAZZI_SRC)/sw/tools/parrot/swing.py
# Program the device and start it.
upload program: $(OBJDIR)/$(TARGET).elf
$(Q)$(PREFIX)-strip $(OBJDIR)/$(TARGET).elf
$(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR)
# Listing of phony targets.
.PHONY : upload program
conf/airframes/examples/swing.xml
-213
View File
@@ -1,213 +0,0 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="swing">
<firmware name="rotorcraft">
<configure name="PREFIX" value="/opt/arm-2012.03/bin/arm-none-linux-gnueabi"/>
<target name="ap" board="swing"/>
<target name="nps" board="pc">
<module name="fdm" type="jsbsim"/>
</target>
<!--define name="USE_SONAR" value="TRUE"/-->
<module name="radio_control" type="datalink"/>
<module name="telemetry" type="transparent_udp"/>
<module name="motor_mixing"/>
<module name="actuators" type="swing"/>
<module name="imu" type="swing"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins"/>
<module name="air_data"/>
</firmware>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<servos>
<servo name="BOTTOM_RIGHT" no="0" min="0" neutral="15" max="511"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="15" max="511"/>
<servo name="BOTTOM_LEFT" no="2" min="0" neutral="15" max="511"/>
<servo name="TOP_LEFT" no="3" min="0" neutral="15" max="511"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<define name="MAX_SATURATION_OFFSET" value="3*MAX_PPRZ"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="ROLL_COEF" value="{ -81, -81, 81, 81}"/>
<define name="PITCH_COEF" value="{ 243, -243, 243, -243}"/>
<define name="YAW_COEF" value="{ 128, -128, -128, 128}"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[0]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[2]"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="18"/>
<define name="MAG_Y_NEUTRAL" value="157"/>
<define name="MAG_Z_NEUTRAL" value="49"/>
<define name="MAG_X_SENS" value="10.5007722373" integer="16"/>
<define name="MAG_Y_SENS" value="11.1147400462" integer="16"/>
<define name="MAG_Z_SENS" value="11.6479371722" integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
<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>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
</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="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="650"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="650"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="300"/>
<define name="PSI_DGAIN" value="250"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="0"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</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_STARTUP" value="AP_MODE_KILL"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_Z_HOLD"/>
</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.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
</airframe>
conf/boards/krooz_sd.makefile
-56
View File
@@ -1,56 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
#
# krooz_sd.makefile
#
#
#
BOARD=krooz
BOARD_VERSION=sd
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
ARCH=stm32
ARCH_L=f4
HARD_FLOAT=yes
ARCH_DIR=stm32
SRC_ARCH=arch/$(ARCH_DIR)
$(TARGET).ARCHDIR = $(ARCH)
# not needed?
$(TARGET).OOCD_INTERFACE=ftdi/flossjtag
#$(TARGET).OOCD_INTERFACE=ftdi/jtagkey
$(TARGET).LDSCRIPT=$(SRC_ARCH)/krooz.ld
# -----------------------------------------------------------------------
# default flash mode is via usb dfu bootloader (luftboot)
# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL
FLASH_MODE ?= DFU
DFU_ADDR = 0x8004000
DFU_PRODUCT = any
#
#
# some default values shared between different firmwares
#
#
#
# default LED configuration
#
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= 2
GPS_LED ?= none
SYS_TIME_LED ?= 1
#
# default uart configuration
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART1
MODEM_PORT ?= UART5
MODEM_BAUD ?= B57600
GPS_PORT ?= UART3
GPS_BAUD ?= B38400
conf/boards/swing.makefile
-51
View File
@@ -1,51 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
#
# swing.makefile
#
# http://wiki.paparazziuav.org/wiki/Swing
#
BOARD=swing
BOARD_CFG=\"boards/$(BOARD).h\"
ARCH=linux
$(TARGET).ARCHDIR = $(ARCH)
# include conf/Makefile.swing (with specific upload rules) instead of only Makefile.linux:
ap.MAKEFILE = swing
FLOAT_ABI =
ARCH_CFLAGS = -march=armv5
# -----------------------------------------------------------------------
USER=foobar
HOST?=192.168.4.1
SUB_DIR=paparazzi
FTP_DIR=/data/edu
TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
# -----------------------------------------------------------------------
# The datalink default uses UDP
MODEM_HOST ?= 192.168.4.255
# The GPS sensor is connected internally
GPS_PORT ?= UART1
GPS_BAUD ?= B230400
# handle linux signals by hand
$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL -D_GNU_SOURCE
# board specific init function
$(TARGET).srcs += $(SRC_BOARD)/board.c
# Link static (Done for GLIBC)
$(TARGET).CFLAGS += -DLINUX_LINK_STATIC
$(TARGET).LDFLAGS += -static
# -----------------------------------------------------------------------
# default LED configuration
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= none
GPS_LED ?= none
SYS_TIME_LED ?= 0
conf/firmwares/baro_board.makefile
-12
View File
@@ -36,10 +36,6 @@ else ifeq ($(BOARD), disco)
BARO_BOARD_SRCS += peripherals/ms5611_i2c.c BARO_BOARD_SRCS += peripherals/ms5611_i2c.c
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
# Swing baro
else ifeq ($(BOARD), swing)
BARO_BOARD_SRCS += $(SRC_BOARD)/baro_board.c
# Lisa/M baro # Lisa/M baro
else ifeq ($(BOARD), lisa_m) else ifeq ($(BOARD), lisa_m)
ifeq ($(BOARD_VERSION), 1.0) ifeq ($(BOARD_VERSION), 1.0)
@@ -172,14 +168,6 @@ LIA_BARO ?= BARO_MS5611_SPI
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
endif endif
# krooz baro
else ifeq ($(BOARD), krooz)
BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2
BARO_BOARD_CFLAGS += -DBB_MS5611_SLAVE_ADDR=0xEC
BARO_BOARD_SRCS += peripherals/ms5611.c
BARO_BOARD_SRCS += peripherals/ms5611_i2c.c
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
# PX4FMU # PX4FMU
else ifeq ($(BOARD),$(filter $(BOARD),px4fmu)) else ifeq ($(BOARD),$(filter $(BOARD),px4fmu))
include $(CFG_SHARED)/spi_master.makefile include $(CFG_SHARED)/spi_master.makefile
conf/flash_modes.xml
-3
View File
@@ -4,7 +4,6 @@
<mode name="SERIAL (stm32)"> <mode name="SERIAL (stm32)">
<variable name="FLASH_MODE" value="SERIAL"/> <variable name="FLASH_MODE" value="SERIAL"/>
<boards> <boards>
<board name="krooz_sd"/>
<board name="lia_.*"/> <board name="lia_.*"/>
<board name="lisa_[ms]_.*"/> <board name="lisa_[ms]_.*"/>
<board name="elle*"/> <board name="elle*"/>
@@ -14,7 +13,6 @@
<mode name="USB DFU (stm32_mem)"> <mode name="USB DFU (stm32_mem)">
<variable name="FLASH_MODE" value="DFU"/> <variable name="FLASH_MODE" value="DFU"/>
<boards> <boards>
<board name="krooz_sd"/>
<board name="li[s]?a_[m]?_.*"/> <board name="li[s]?a_[m]?_.*"/>
<board name="li[s]?a_mx_.*"/> <board name="li[s]?a_mx_.*"/>
<board name="px4fmu_.*"/> <board name="px4fmu_.*"/>
@@ -109,7 +107,6 @@
<mode name="JTAG (OpenOCD)"> <mode name="JTAG (OpenOCD)">
<variable name="FLASH_MODE" value="JTAG"/> <variable name="FLASH_MODE" value="JTAG"/>
<boards> <boards>
<board name="krooz_sd"/>
<board name="li[s]?a_[lm]?_.*"/> <board name="li[s]?a_[lm]?_.*"/>
<board name="elle*"/> <board name="elle*"/>
</boards> </boards>
conf/modules/actuators_swing.xml
-20
View File
@@ -1,20 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="actuators_swing" dir="actuators" task="actuators">
<doc>
<description>
Actuators Driver for Swing
</description>
</doc>
<dep>
<depends>actuators</depends>
<provides>actuators</provides>
</dep>
<header>
<file name="actuators.h" dir="boards/swing"/>
</header>
<makefile target="ap">
<file name="actuators.c" dir="$(SRC_BOARD)"/>
</makefile>
</module>
conf/modules/ahrs_gx3.xml
-39
View File
@@ -1,39 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ahrs_gx3" dir="ahrs" task="estimation">
<doc>
<description>
AHRS driver for GX3.
</description>
<configure name="GX3_PORT" value="UART3" description="uart for GX3"/>
<configure name="GX3_BAUD" value="B921600" description="GX3 uart baud rate"/>
</doc>
<dep>
<depends>uart</depends>
<provides>ahrs,imu,mag</provides>
</dep>
<autoload name="ahrs_sim"/>
<header>
<file name="ahrs.h"/>
</header>
<init fun="ahrs_init()"/>
<makefile target="ap">
<configure name="GX3_PORT" default="UART3" case="upper|lower"/>
<configure name="GX3_BAUD" default="B921600"/>
<define name="USE_$(GX3_PORT_UPPER)"/>
<define name="$(GX3_PORT_UPPER)_BAUD" value="$(GX3_BAUD)"/>
<define name="USE_AHRS"/>
<define name="USE_IMU"/>
<file name="ahrs.c"/>
<file name="imu.c" dir="modules/imu"/>
<file name="ahrs_gx3.c"/>
<test>
<define name="GX3_PORT" value="uart3"/>
<define name="USE_UART3"/>
<define name="PRIMARY_AHRS" value="ahrs_gx3"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_gx3.h" type="string"/>
</test>
</makefile>
</module>
conf/modules/baro_board.xml
-8
View File
@@ -150,14 +150,6 @@ LIA_BARO ?= BARO_MS5611_SPI
endif endif
# krooz baro
else ifeq ($(BOARD), krooz)
BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2
BARO_BOARD_CFLAGS += -DBB_MS5611_SLAVE_ADDR=0xEC
BARO_BOARD_SRCS += peripherals/ms5611.c
BARO_BOARD_SRCS += peripherals/ms5611_i2c.c
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
# PX4FMU # PX4FMU
else ifeq ($(BOARD),$(filter $(BOARD),px4fmu)) else ifeq ($(BOARD),$(filter $(BOARD),px4fmu))
include $(CFG_SHARED)/spi_master.makefile include $(CFG_SHARED)/spi_master.makefile
conf/modules/boards/krooz_sd.xml
-14
View File
@@ -1,14 +0,0 @@
<!DOCTYPE module SYSTEM "../module.dtd">
<module name="krooz_sd" dir="boards">
<doc>
<description>
Specific configuration for Krooz SD
</description>
</doc>
<dep>
<depends>i2c,baro_board</depends>
</dep>
<makefile target="!sim|nps|fbw"/>
</module>
conf/modules/boards/swing.xml
-17
View File
@@ -1,17 +0,0 @@
<!DOCTYPE module SYSTEM "../module.dtd">
<module name="swing" dir="boards">
<doc>
<description>
Specific configuration for Parrot Swing
</description>
</doc>
<dep>
<depends>baro_board_common</depends>
<provides>baro</provides>
</dep>
<makefile target="!sim|nps|fbw">
<file name="baro_board.c" dir="$(SRC_BOARD)"/>
</makefile>
</module>
conf/modules/imu_drotek_10dof_v2.xml
-51
View File
@@ -1,51 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_drotek_10dof_v2" dir="imu" task="sensors">
<doc>
<description>
Drotek 10DOF V2 IMU via I2C.
MPU60x0 via I2C.
HMC58xx via I2C.
</description>
<configure name="DROTEK_2_I2C_DEV" value="i2c2" description="I2C device to use"/>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="2358"/>
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<define name="MAG_X_SENS" value="3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value="3.607713" integer="16"/>
<define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
</section>
</doc>
<dep>
<depends>i2c,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_sim"/>
<autoload name="imu_nps"/>
<header>
<file name="imu_drotek_10dof_v2.h"/>
</header>
<init fun="imu_drotek2_init()"/>
<periodic fun="imu_drotek2_periodic()"/>
<event fun="imu_drotek2_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="DROTEK_2_I2C_DEV" default="i2c2" case="lower|upper"/>
<define name="DROTEK_2_I2C_DEV" value="$(DROTEK_2_I2C_DEV_LOWER)"/>
<define name="USE_$(DROTEK_2_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="imu/imu_drotek_10dof_v2.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_drotek_10dof_v2.c"/>
<test>
<define name="IMU_TYPE_H" value="imu/imu_drotek_10dof_v2.h" type="string"/>
<define name="USE_I2C1"/>
<define name="DROTEK_2_I2C_DEV" value="i2c1"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
</test>
</makefile>
</module>
conf/modules/imu_gl1.xml
-46
View File
@@ -1,46 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_gl1" dir="imu" task="sensors">
<doc>
<description>
IMU from Goodluckbuy.
- Accelerometer: ADXL345 via I2C.
- Gyroscope: L3G4200 via I2C.
- Magnetometer: HMC58xx via I2C.
</description>
<configure name="GL1_I2C_DEV" value="i2c2" description="I2C device to use"/>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="2358"/>
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<define name="MAG_X_SENS" value="3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value="3.607713" integer="16"/>
<define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
</section>
</doc>
<dep>
<depends>i2c,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<header>
<file name="imu_gl1.h"/>
</header>
<init fun="imu_gl1_init()"/>
<periodic fun="imu_gl1_periodic()"/>
<event fun="imu_gl1_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="GL1_I2C_DEV" default="i2c2" case="lower|upper"/>
<define name="GL1_I2C_DEV" value="$(GL1_I2C_DEV_LOWER)"/>
<define name="USE_$(GL1_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_gl1.h" type="string"/>
<file name="adxl345_i2c.c" dir="peripherals"/>
<file name="l3g4200.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_gl1.c"/>
</makefile>
</module>
conf/modules/imu_krooz_sd.xml
-44
View File
@@ -1,44 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_krooz_sd" dir="imu" task="sensors">
<doc>
<description>
IMU on KroozSD board.
MPU60x0 and HMC58xx via I2C2.
</description>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="2358"/>
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<define name="MAG_X_SENS" value="3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value="3.607713" integer="16"/>
<define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
</section>
</doc>
<dep>
<depends>i2c,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<header>
<file name="imu_krooz.h" dir="boards/krooz"/>
</header>
<init fun="imu_krooz_init()"/>
<periodic fun="imu_krooz_periodic()"/>
<event fun="imu_krooz_event()"/>
<makefile target="!sim|nps|fbw">
<define name="IMU_KROOZ_I2C_DEV" value="i2c2"/>
<define name="USE_I2C2"/>
<define name="I2C2_CLOCK_SPEED" value="400000"/>
<define name="IMU_TYPE_H" value="boards/krooz/imu_krooz.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_krooz.c" dir="boards/krooz"/>
<file_arch name="imu_krooz_sd_arch.c"/>
</makefile>
</module>
conf/modules/imu_krooz_sd_memsic.xml
-50
View File
@@ -1,50 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_krooz_sd_memsic" dir="imu" task="sensors">
<doc>
<description>
IMU on KroozSD board.
MPU60x0 via SPI2.
HMC58xx via I2C2.
</description>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="2358"/>
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<define name="MAG_X_SENS" value="3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value="3.607713" integer="16"/>
<define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
</section>
</doc>
<dep>
<depends>spi_master,i2c,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<header>
<file name="imu_krooz.h" dir="boards/krooz"/>
</header>
<init fun="imu_krooz_init()"/>
<periodic fun="imu_krooz_periodic()"/>
<event fun="imu_krooz_event()"/>
<makefile target="!sim|nps|fbw">
<define name="IMU_KROOZ_I2C_DEV" value="i2c2"/>
<define name="USE_I2C2"/>
<define name="I2C2_CLOCK_SPEED" value="400000"/>
<define name="IMU_KROOZ_SPI_DEV" value="spi2"/>
<define name="USE_SPI2"/>
<define name="USE_SPI_SLAVE1"/>
<define name="USE_SPI_SLAVE2"/>
<define name="IMU_KROOZ_SPI_SLAVE_IDX" value="1"/>
<define name="IMU_TYPE_H" value="boards/krooz/imu_krooz.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_krooz_memsic.c" dir="boards/krooz"/>
<file_arch name="imu_krooz_sd_arch.c"/>
</makefile>
</module>
conf/modules/imu_pprzuav.xml
-46
View File
@@ -1,46 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_pprzuav" dir="imu" task="sensors">
<doc>
<description>
PPZUAV IMU.
- Accelerometer: ADXL345 via I2C
- Gyroscope: ITG3200 via I2C
- Magnetometer: HMC58xx via I2C
</description>
<configure name="IMU_PPZUAV_I2C_DEV" value="i2c2" description="I2C device to use"/>
<section name="IMU" prefix="IMU_">
<define name="MAG_X_NEUTRAL" value="2358"/>
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<define name="MAG_X_SENS" value="3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value="3.607713" integer="16"/>
<define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
</section>
</doc>
<dep>
<depends>i2c,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<header>
<file name="imu_ppzuav.h"/>
</header>
<init fun="imu_ppzuav_init()"/>
<periodic fun="imu_ppzuav_periodic()"/>
<event fun="imu_ppzuav_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="IMU_PPZUAV_I2C_DEV" default="i2c2" case="lower|upper"/>
<define name="IMU_PPZUAV_I2C_DEV" value="$(IMU_PPZUAV_I2C_DEV_LOWER)"/>
<define name="USE_$(IMU_PPZUAV_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="imu/imu_ppzuav.h" type="string"/>
<file name="adxl345_i2c.c" dir="peripherals"/>
<file name="itg3200.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_ppzuav.c"/>
</makefile>
</module>
conf/modules/imu_swing.xml
-35
View File
@@ -1,35 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_swing" dir="imu" task="sensors">
<doc>
<description>
Driver for IMU on the Parrot Swing drone.
- Accelerometer/Gyroscope: MPU6000 via I2C0
</description>
</doc>
<dep>
<depends>i2c,imu_common</depends>
<provides>imu</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<!--autoload name="sonar_bebop"/-->
<header>
<file name="imu_swing.h"/>
</header>
<init fun="imu_swing_init()"/>
<periodic fun="imu_swing_periodic()"/>
<event fun="imu_swing_event()"/>
<makefile target="!sim|nps|fbw">
<define name="USE_I2C0"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_swing.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="imu_swing.c"/>
<test>
<define name="IMU_TYPE_H" value="modules/imu/imu_swing.h" type="string"/>
<define name="USE_I2C0"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
</test>
</makefile>
</module>
conf/modules/imu_um6.xml
-34
View File
@@ -1,34 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_um6" dir="imu" task="sensors">
<doc>
<description>
CHR-UM6 IMU.
</description>
<configure name="UM6_PORT" value="uart3" description="UART to use"/>
<configure name="UM6_BAUD" value="B115200" description="Baud rate"/>
</doc>
<dep>
<depends>uart,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<header>
<file name="imu_um6.h"/>
</header>
<init fun="imu_um6_init()"/>
<periodic fun="imu_um6_periodic()"/>
<event fun="imu_um6_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="UM6_PORT" default="uart3" case="lower|upper"/>
<configure name="UM6_BAUD" default="B115200"/>
<define name="USE_$(UM6_PORT_UPPER)"/>
<define name="$(UM6_PORT_UPPER)_BAUD" value="$(UM6_BAUD)"/>
<define name="UM6_LINK" value="$(UM6_PORT_LOWER)"/>
<define name="IMU_TYPE_H" value="imu/imu_um6.h" type="string"/>
<file name="imu_um6.c"/>
</makefile>
</module>
sw/airborne/arch/stm32/modules/imu/imu_krooz_sd_arch.c
-39
View File
@@ -1,39 +0,0 @@
#include "modules/imu/imu.h"
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/exti.h>
#include <libopencm3/cm3/nvic.h>
#include "modules/imu/imu_krooz_sd_arch.h"
void imu_krooz_sd_arch_init(void)
{
rcc_periph_clock_enable(RCC_SYSCFG);
rcc_periph_clock_enable(RCC_GPIOB);
rcc_periph_clock_enable(RCC_GPIOB);
gpio_mode_setup(GPIOB, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO5);
gpio_mode_setup(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO6);
nvic_enable_irq(NVIC_EXTI9_5_IRQ);
exti_select_source(EXTI5, GPIOB);
exti_select_source(EXTI6, GPIOC);
exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING);
exti_set_trigger(EXTI6, EXTI_TRIGGER_FALLING);
exti_enable_request(EXTI5);
exti_enable_request(EXTI6);
nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0F);
}
void exti9_5_isr(void)
{
/* clear EXTI */
if (EXTI_PR & EXTI6) {
exti_reset_request(EXTI6);
imu_krooz.hmc_eoc = true;
}
if (EXTI_PR & EXTI5) {
exti_reset_request(EXTI5);
imu_krooz.mpu_eoc = true;
}
}
sw/airborne/arch/stm32/modules/imu/imu_krooz_sd_arch.h
-8
View File
@@ -1,8 +0,0 @@
#ifndef IMU_KROOZ_SD_ARCH_H
#define IMU_KROOZ_SD_ARCH_H
#include "modules/imu/imu.h"
void imu_krooz_sd_arch_init(void);
#endif /* IMU_KROOZ_SD_ARCH_H */
sw/airborne/boards/krooz/baro_board.h
-17
View File
@@ -1,17 +0,0 @@
/*
* board specific interface for the KroozSD board
*
* It uses the modules/sensors/baro_ms5611_i2c.c driver
*/
#ifndef BOARDS_KROOZ_BARO_H
#define BOARDS_KROOZ_BARO_H
// only for printing the baro type during compilation
#define BARO_BOARD BARO_BOARD_MS5611_I2C
extern void baro_event(void);
#define BaroEvent baro_event
#endif /* BOARDS_KROOZ_SD_BARO_H */
sw/airborne/boards/krooz/imu_krooz.c
-169
View File
@@ -1,169 +0,0 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file boards/krooz/imu_krooz.c
*
* Driver for the IMU on the KroozSD board.
*
* Invensense MPU-6050
* Honeywell HMC-5883
*/
#include <math.h>
#include "boards/krooz/imu_krooz.h"
#include "modules/imu/imu_krooz_sd_arch.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "filters/median_filter.h"
#include "modules/core/abi.h"
// Downlink
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define KROOZ_SMPLRT_DIV 1
#endif
PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
struct MedianFilter3Int median_accel;
#endif
struct MedianFilter3Int median_mag;
void imu_krooz_init(void)
{
/////////////////////////////////////////////////////////////////////
// MPU-60X0
mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
// change the default configuration
imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
imu_krooz.mpu.config.drdy_int_enable = true;
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
// Init median filters
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel, MEDIAN_DEFAULT_SIZE);
#endif
InitMedianFilterVect3Int(median_mag, MEDIAN_DEFAULT_SIZE);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.hmc_eoc = false;
imu_krooz.mpu_eoc = false;
imu_krooz_sd_arch_init();
}
void imu_krooz_periodic(void)
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized) {
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
}
if (!imu_krooz.hmc.initialized) {
hmc58xx_start_configure(&imu_krooz.hmc);
}
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb,
imu_krooz.rates_sum.r / imu_krooz.meas_nb);
VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb,
imu_krooz.accel_sum.z / imu_krooz.meas_nb);
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
uint32_t now_ts = get_sys_time_usec();
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
//RunOnceEvery(10,imu_krooz_downlink_raw());
}
void imu_krooz_downlink_raw(void)
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q,
&imu.gyro_unscaled.r);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y,
&imu.accel_unscaled.z);
}
void imu_krooz_event(void)
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = false;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = false;
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = false;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag);
}
}
sw/airborne/boards/krooz/imu_krooz.h
-99
View File
@@ -1,99 +0,0 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file boards/krooz/imu_krooz.h
*
* Driver for the IMU on the KroozSD board.
*
* Invensense MPU-6050
*/
#ifndef IMU_KROOZ_H
#define IMU_KROOZ_H
#include "std.h"
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
#ifndef KROOZ_ACCEL_RANGE
#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
#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 MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_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 MPU60X0_ACCEL_SENS[KROOZ_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[KROOZ_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[KROOZ_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][1]
#endif
#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
#define IMU_KROOZ_ACCEL_AVG_FILTER 15
#endif
struct ImuKrooz {
volatile bool mpu_eoc;
volatile bool hmc_eoc;
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
struct Int32Rates rates_sum;
struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb;
struct Int32Vect3 accel_filtered;
int32_t temperature;
};
extern struct ImuKrooz imu_krooz;
extern void imu_krooz_init(void);
extern void imu_krooz_periodic(void);
extern void imu_krooz_event(void);
extern void imu_krooz_downlink_raw(void);
#endif // IMU_KROOZ_H
sw/airborne/boards/krooz/imu_krooz_memsic.c
-228
View File
@@ -1,228 +0,0 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file boards/krooz/imu_krooz_memsic.c
*
* Driver for the IMU on the KroozSD Big Rotorcraft Edition board.
*
* Invensense MPU-6050
* Memsic MXR9500 with AD7689
* Honeywell HMC-5883
*/
#include <math.h>
#include "boards/krooz/imu_krooz_memsic.h"
#include "modules/imu/imu_krooz_sd_arch.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "filters/median_filter.h"
#include "mcu_periph/sys_time.h"
#include "modules/core/abi.h"
#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define KROOZ_SMPLRT_DIV 1
#endif
PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
#ifndef KROOZ_ACCEL_RANGE
#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
#endif
PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
struct MedianFilter3Int median_accel;
#endif
struct MedianFilter3Int median_mag;
static uint32_t ad7689_event_timer;
static uint8_t axis_cnt;
static uint8_t axis_nb;
void imu_krooz_init(void)
{
/////////////////////////////////////////////////////////////////////
// MPU-60X0
mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
// change the default configuration
imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
imu_krooz.mpu.config.drdy_int_enable = true;
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
// Init median filters
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel, MEDIAN_DEFAULT_SIZE);
#endif
InitMedianFilterVect3Int(median_mag, MEDIAN_DEFAULT_SIZE);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.hmc_eoc = false;
imu_krooz.mpu_eoc = false;
imu_krooz.ad7689_trans.slave_idx = IMU_KROOZ_SPI_SLAVE_IDX;
imu_krooz.ad7689_trans.select = SPISelectUnselect;
imu_krooz.ad7689_trans.cpol = SPICpolIdleLow;
imu_krooz.ad7689_trans.cpha = SPICphaEdge1;
imu_krooz.ad7689_trans.dss = SPIDss8bit;
imu_krooz.ad7689_trans.bitorder = SPIMSBFirst;
imu_krooz.ad7689_trans.cdiv = SPIDiv16;
imu_krooz.ad7689_trans.output_length = sizeof(imu_krooz.ad7689_spi_tx_buffer);
imu_krooz.ad7689_trans.output_buf = (uint8_t *) imu_krooz.ad7689_spi_tx_buffer;
imu_krooz.ad7689_trans.input_length = sizeof(imu_krooz.ad7689_spi_rx_buffer);
imu_krooz.ad7689_trans.input_buf = (uint8_t *) imu_krooz.ad7689_spi_rx_buffer;
imu_krooz.ad7689_trans.before_cb = NULL;
imu_krooz.ad7689_trans.after_cb = NULL;
axis_cnt = 0;
axis_nb = 2;
imu_krooz_sd_arch_init();
}
void imu_krooz_periodic(void)
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized) {
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
}
if (!imu_krooz.hmc.initialized) {
hmc58xx_start_configure(&imu_krooz.hmc);
}
uint32_t now_ts = get_sys_time_usec();
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
imu_krooz.rates_sum.p / imu_krooz.meas_nb,
imu_krooz.rates_sum.r / imu_krooz.meas_nb);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
RunOnceEvery(128, {axis_nb = 5;});
}
void imu_krooz_event(void)
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = false;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = false;
}
if (SysTimeTimer(ad7689_event_timer) > 215) {
SysTimeTimerStart(ad7689_event_timer);
if (axis_cnt < axis_nb) {
axis_cnt++;
} else {
axis_cnt = 0;
}
imu_krooz.ad7689_trans.output_buf[0] =
axis_cnt <= 2 ? 0xF0 | (axis_cnt << 1) : (axis_cnt >= 4 ? 0xF0 | ((axis_cnt - 3) << 1) : 0xB0);
imu_krooz.ad7689_trans.output_buf[1] = 0x44;
spi_submit(&(IMU_KROOZ_SPI_DEV), &imu_krooz.ad7689_trans);
}
if (imu_krooz.ad7689_trans.status == SPITransSuccess) {
imu_krooz.ad7689_trans.status = SPITransDone;
uint16_t buf = (imu_krooz.ad7689_trans.input_buf[0] << 8) | imu_krooz.ad7689_trans.input_buf[1];
switch (axis_cnt) {
case 0:
case 3:
imu_krooz.accel_sum.x += (int32_t)buf;
imu_krooz.meas_nb_acc.x++;
break;
case 1:
case 4:
imu_krooz.accel_sum.y += (int32_t)buf;
imu_krooz.meas_nb_acc.y++;
break;
case 2:
imu_krooz.accel_sum.z += (int32_t)buf;
imu_krooz.meas_nb_acc.z++;
break;
case 5:
imu_krooz.temperature = (imu_krooz.temperature * 4 + (int32_t)buf) / 5;
//imu.temperature = 33000 * imu_krooz.temp / 65536 - 2400;
axis_nb = 2;
break;
default:
axis_cnt = 0;
break;
}
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = false;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, get_sys_time_usec(), &imu.mag);
}
}
sw/airborne/boards/krooz/imu_krooz_memsic.h
-110
View File
@@ -1,110 +0,0 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file boards/krooz/imu_krooz_memsic.h
*
* Driver for the IMU on the KroozSD Big Rotorcraft Edition board.
*
* Invensense MPU-6050
* Memsic MXR9500 with AD7689
* Honeywell HMC-5883
*/
#ifndef IMU_KROOZ_H
#define IMU_KROOZ_H
#include "std.h"
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
#include "mcu_periph/spi.h"
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#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 MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#endif
/** default accel sensitivy using 16 bit AD7689 adc
* MXR9500 with 1.5g has 21845 LSB/g
* sens = 9.81 [m/s^2] / 21845 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 0.9197
#define IMU_ACCEL_X_SENS_NUM 9197
#define IMU_ACCEL_X_SENS_DEN 10000
#define IMU_ACCEL_Y_SENS 0.9197
#define IMU_ACCEL_Y_SENS_NUM 9197
#define IMU_ACCEL_Y_SENS_DEN 10000
#define IMU_ACCEL_Z_SENS 0.9197
#define IMU_ACCEL_Z_SENS_NUM 9197
#define IMU_ACCEL_Z_SENS_DEN 10000
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 32768
#define IMU_ACCEL_Y_NEUTRAL 32768
#define IMU_ACCEL_Z_NEUTRAL 32768
#endif
#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
#define IMU_KROOZ_ACCEL_AVG_FILTER 15
#endif
struct ImuKrooz {
volatile bool mpu_eoc;
volatile bool hmc_eoc;
struct Mpu60x0_I2c mpu;
struct spi_transaction ad7689_trans;
volatile uint8_t ad7689_spi_tx_buffer[2];
volatile uint8_t ad7689_spi_rx_buffer[2];
struct Hmc58xx hmc;
struct Int32Rates rates_sum;
struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb;
struct Uint8Vect3 meas_nb_acc;
struct Int32Vect3 accel_filtered;
int32_t temperature;
};
extern struct ImuKrooz imu_krooz;
extern void imu_krooz_init(void);
extern void imu_krooz_periodic(void);
extern void imu_krooz_event(void);
extern void imu_krooz_downlink_raw(void);
#endif // IMU_KROOZ_H
sw/airborne/boards/swing.h
-64
View File
@@ -1,64 +0,0 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef CONFIG_SWING
#define CONFIG_SWING
#define BOARD_SWING
#include "std.h"
/** uart connected to GPS internally */
#define UART1_DEV /dev/ttyPA1
#define GPS_UBX_ENABLE_NMEA_DATA_MASK 0xff
/** FTDI cable for stereoboard or external GPS */
#define UART2_DEV /dev/ttyUSB0
/* Default actuators driver */
#define DEFAULT_ACTUATORS "boards/swing/actuators.h"
#define ActuatorDefaultSet(_x,_y) ActuatorsSwingSet(_x,_y)
#define ActuatorsDefaultInit() ActuatorsSwingInit()
#define ActuatorsDefaultCommit() ActuatorsSwingCommit()
/* by default activate onboard baro */
#ifndef USE_BARO_BOARD
#define USE_BARO_BOARD 1
#endif
/* The ADC from the sonar */
#if USE_ADC0
#define ADC0_ID 0
#define ADC0_CHANNELS 2
#define ADC0_CHANNELS_CNT 1
#define ADC0_BUF_LENGTH 8192
#endif
/* The SPI from the sonar */
#if USE_SPI0
#define SPI0_MODE 0
#define SPI0_BITS_PER_WORD 8
#define SPI0_MAX_SPEED_HZ 320000
#endif
#endif /* CONFIG_SWING */
sw/airborne/boards/swing/actuators.c
-126
View File
@@ -1,126 +0,0 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file boards/swing/actuators.c
* Actuator driver for the swing
*/
#include <sys/ioctl.h>
#include <fcntl.h>
/* build ioctl unique identifiers for R/W operations */
#define PWM_MAGIC 'p'
typedef struct { unsigned int val[4]; } __attribute__ ((packed)) pwm_delos_quadruplet;
#define PWM_DELOS_SET_RATIOS _IOR(PWM_MAGIC, 9, pwm_delos_quadruplet*)
#define PWM_DELOS_SET_SPEEDS _IOR(PWM_MAGIC, 10, pwm_delos_quadruplet*)
#define PWM_DELOS_SET_CTRL _IOR(PWM_MAGIC, 11, unsigned int)
#define PWM_DELOS_REQUEST _IO(PWM_MAGIC, 12)
#define PWM_NB_BITS (9)
/* PWM can take value between 0 and 511 */
#ifndef PWM_TOTAL_RANGE
#define PWM_TOTAL_RANGE (1<<PWM_NB_BITS)
#endif
#define PWM_REG_RATIO_PRECISION_MASK (PWM_NB_BITS<<16)
#define PWM_REG_SATURATION (PWM_REG_RATIO_PRECISION_MASK|PWM_TOTAL_RANGE)
#include "modules/actuators/actuators.h"
#include "modules/actuators/motor_mixing.h"
#include "actuators.h"
#include "autopilot.h"
struct ActuatorsSwing actuators_swing;
static int actuators_fd;
// Start/stop PWM
enum {
SiP6_PWM0_START = (1<<0),
SiP6_PWM1_START = (1<<1),
SiP6_PWM2_START = (1<<2),
SiP6_PWM3_START = (1<<3),
};
void actuators_swing_init(void)
{
actuators_fd = open("/dev/pwm", O_RDWR);
pwm_delos_quadruplet m = {{ 1, 1, 1, 1 }};
int ret __attribute__((unused)) = ioctl(actuators_fd, PWM_DELOS_SET_SPEEDS, &m);
#if ACTUATORS_SWING_DEBUG
printf("Return Speeds: %d\n", ret);
#endif
actuators_swing_commit();
unsigned int control_reg = (SiP6_PWM0_START|SiP6_PWM1_START|SiP6_PWM2_START|SiP6_PWM3_START);
ret = ioctl(actuators_fd, PWM_DELOS_SET_CTRL, &control_reg);
#if ACTUATORS_SWING_DEBUG
printf("Return control: %d\n", ret);
#endif
}
void actuators_swing_commit(void)
{
pwm_delos_quadruplet m;
m.val[0] = actuators_swing.rpm_ref[0] & 0xffff;
m.val[1] = actuators_swing.rpm_ref[1] & 0xffff;
m.val[2] = actuators_swing.rpm_ref[2] & 0xffff;
m.val[3] = actuators_swing.rpm_ref[3] & 0xffff;
if( actuators_swing.rpm_ref[0] > (PWM_TOTAL_RANGE) ) { m.val[0] = PWM_REG_SATURATION; }
if( actuators_swing.rpm_ref[1] > (PWM_TOTAL_RANGE) ) { m.val[1] = PWM_REG_SATURATION; }
if( actuators_swing.rpm_ref[2] > (PWM_TOTAL_RANGE) ) { m.val[2] = PWM_REG_SATURATION; }
if( actuators_swing.rpm_ref[3] > (PWM_TOTAL_RANGE) ) { m.val[3] = PWM_REG_SATURATION; }
if( actuators_swing.rpm_ref[0] < 0 ) { m.val[0] = 0; }
if( actuators_swing.rpm_ref[1] < 0 ) { m.val[1] = 0; }
if( actuators_swing.rpm_ref[2] < 0 ) { m.val[2] = 0; }
if( actuators_swing.rpm_ref[3] < 0 ) { m.val[3] = 0; }
/* The upper 16-bit word of the ratio register contains the number
* of bits used to code the ratio command */
m.val[0] |= PWM_REG_RATIO_PRECISION_MASK;
m.val[1] |= PWM_REG_RATIO_PRECISION_MASK;
m.val[2] |= PWM_REG_RATIO_PRECISION_MASK;
m.val[3] |= PWM_REG_RATIO_PRECISION_MASK;
int ret __attribute__((unused)) = ioctl(actuators_fd, PWM_DELOS_SET_RATIOS, &m);
#if ACTUATORS_SWING_DEBUG
RunOnceEvery(512, printf("Return ratios: %d (ratios: %d %d %d %d, pwm: %d %d %d %d\n",
ret,
m.val[0], m.val[1], m.val[2], m.val[3],
actuators_swing.rpm_ref[0],
actuators_swing.rpm_ref[1],
actuators_swing.rpm_ref[2],
actuators_swing.rpm_ref[3])
);
#endif
}
sw/airborne/boards/swing/actuators.h
-46
View File
@@ -1,46 +0,0 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file boards/swing/actuators.h
* Actuator driver for the swing
*/
#ifndef ACTUATORS_SWING_H_
#define ACTUATORS_SWING_H_
#include <stdint.h>
struct ActuatorsSwing {
uint16_t rpm_ref[4]; ///< Reference RPM
};
#define ActuatorsSwingSet(_i, _v) { actuators_swing.rpm_ref[_i] = _v; }
#define ActuatorsSwingCommit() actuators_swing_commit();
#define ActuatorsSwingInit() actuators_swing_init();
extern struct ActuatorsSwing actuators_swing;
extern void actuators_swing_commit(void);
extern void actuators_swing_init(void);
#endif /* ACTUATORS_SWING_H_ */
sw/airborne/boards/swing/baro_board.c
-104
View File
@@ -1,104 +0,0 @@
/*
* Copyright (C) 2017 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 boards/swing/baro_board.c
* Paparazzi Swing Baro Sensor implementation.
* Sensor is LPS22HB (I2C) from ST but is accessed through sysfs interface
*/
#include "modules/sensors/baro.h"
#include "modules/core/abi.h"
#include "baro_board.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <linux/input.h>
static bool baro_swing_available;
static int32_t baro_swing_raw;
static pthread_mutex_t baro_swing_mutex = PTHREAD_MUTEX_INITIALIZER;
/**
* Check baro thread
* TODO something better ?
*/
static void *baro_read(void *data __attribute__((unused)))
{
struct input_event ev;
ssize_t n;
int fd_sonar = open("/dev/input/baro_event", O_RDONLY);
if (fd_sonar == -1) {
printf("Unable to open baro event to read pressure\n");
return NULL;
}
while (TRUE) {
/* Check new pressure */
n = read(fd_sonar, &ev, sizeof(ev));
if (n == sizeof(ev) && ev.type == EV_ABS && ev.code == ABS_PRESSURE) {
pthread_mutex_lock(&baro_swing_mutex);
baro_swing_available = true;
baro_swing_raw = ev.value;
pthread_mutex_unlock(&baro_swing_mutex);
}
// Wait 100ms
//usleep(100000);
}
return NULL;
}
void baro_init(void)
{
baro_swing_available = false;
baro_swing_raw = 0;
/* Start baro reading thread */
pthread_t baro_thread;
if (pthread_create(&baro_thread, NULL, baro_read, NULL) != 0) {
printf("[swing_board] Could not create baro reading thread!\n");
}
pthread_setname_np(baro_thread, "pprz_baro_thread");
}
void baro_periodic(void) {}
void baro_event(void)
{
pthread_mutex_lock(&baro_swing_mutex);
if (baro_swing_available) {
// From datasheet: raw_pressure / 4096 -> pressure in hPa
// send data in Pa
uint32_t now_ts = get_sys_time_usec();
float pressure = 100.f * ((float)baro_swing_raw) / 4096.f;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, now_ts, pressure);
baro_swing_available = false;
}
pthread_mutex_unlock(&baro_swing_mutex);
}
sw/airborne/boards/swing/baro_board.h
-38
View File
@@ -1,38 +0,0 @@
/*
* Copyright (C) 2017 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 boards/swing/baro_board.h
* Paparazzi Swing Baro Sensor implementation.
* Sensor is LPS22HB (I2C) from ST but is accessed through sysfs interface
*/
#ifndef BOARDS_SWING_BARO_H
#define BOARDS_SWING_BARO_H
// only for printing the baro type during compilation
#ifndef BARO_BOARD
#define BARO_BOARD BARO_SWING
#endif
extern void baro_event(void);
#define BaroEvent baro_event
#endif /* BOARDS_SWING_BARO_H */
sw/airborne/boards/swing/board.c
-131
View File
@@ -1,131 +0,0 @@
/*
* Copyright (C) 2017 The Paparazzi Team
*
* 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 boards/swing/board.c
*
* Swing specific board initialization function.
*
*/
#include "boards/swing.h"
#include "mcu.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <linux/input.h>
#include "modules/energy/electrical.h"
/**
* Battery reading thread
*/
static void *bat_read(void *data __attribute__((unused)))
{
FILE *fp;
char path[16];
while (TRUE) {
/* Open the command for reading. */
fp = popen("cat /sys/devices/platform/p6-spi.2/spi2.0/vbat", "r");
if (fp == NULL) {
printf("Failed to read battery\n");
} else {
/* Read the output a line at a time - output it. */
while (fgets(path, sizeof(path) - 1, fp) != NULL) {
int raw_bat = atoi(path);
// from /bin/mcu_vbat.sh: MILLIVOLTS_VALUE=$(( ($RAW_VALUE * 4250) / 1023 ))
electrical.vsupply = (float)((raw_bat * 4250) / 1023) / 1000.f;
}
/* close */
pclose(fp);
}
// Wait 100ms
// reading is done at 10Hz like the electrical_periodic from rotorcraft main program
usleep(100000);
}
return NULL;
}
/**
* Check button thread
*/
static void *button_read(void *data __attribute__((unused)))
{
struct input_event ev;
ssize_t n;
/* Open power button event sysfs file */
int fd_button = open("/dev/input/pm_mcu_event", O_RDONLY);
if (fd_button == -1) {
printf("Unable to open mcu_event to read power button state\n");
return NULL;
}
while (TRUE) {
/* Check power button (read is blocking) */
n = read(fd_button, &ev, sizeof(ev));
if (n == sizeof(ev) && ev.type == EV_KEY && ev.code == KEY_POWER && ev.value > 0) {
printf("Stopping Paparazzi from power button and rebooting\n");
usleep(1000);
int ret __attribute__((unused)) = system("reboot.sh");
exit(0);
}
}
return NULL;
}
void board_init(void)
{
/*
* Stop original processes using pstop/ptart commands
* Don't kill to avoid automatic restart
*
*/
int ret __attribute__((unused));
ret = system("pstop delosd");
ret = system("pstop dragon-prog");
usleep(50000); /* Give 50ms time to end on a busy system */
/* Start bat reading thread */
pthread_t bat_thread;
if (pthread_create(&bat_thread, NULL, bat_read, NULL) != 0) {
printf("[swing_board] Could not create battery reading thread!\n");
}
pthread_setname_np(bat_thread, "pprz_bat_thread");
/* Start button reading thread */
pthread_t button_thread;
if (pthread_create(&button_thread, NULL, button_read, NULL) != 0) {
printf("[swing_board] Could not create button reading thread!\n");
}
pthread_setname_np(button_thread, "pprz_button_thread");
}
void board_init2(void)
{
}
sw/airborne/modules/ahrs/ahrs_gx3.c
-379
View File
@@ -1,379 +0,0 @@
/*
* Copyright (C) 2013 Michal Podhradsky
* Utah State University, http://aggieair.usu.edu/
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file ahrs_gx3.c
*
* Driver for Microstrain GX3 IMU/AHRS subsystem
*
* Takes care of configuration of the IMU, communication and parsing
* the received packets. See GX3 datasheet for configuration options.
*
* @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*/
#include "modules/ahrs/ahrs_gx3.h"
// for ahrs_register_impl
#include "modules/ahrs/ahrs.h"
#include "modules/core/abi.h"
#define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
/**
* Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
* Positive pitch : nose up
* Positive roll : right wing down
* Positive yaw : clockwise
*/
struct AhrsGX3 ahrs_gx3;
static inline bool gx3_verify_chk(volatile uint8_t *buff_add);
static inline float bef(volatile uint8_t *c);
/* Big Endian to Float */
static inline float bef(volatile uint8_t *c)
{
float f;
int8_t *p;
p = ((int8_t *)&f) + 3;
*p-- = *c++;
*p-- = *c++;
*p-- = *c++;
*p = *c;
return f;
}
static inline bool gx3_verify_chk(volatile uint8_t *buff_add)
{
uint16_t i, chk_calc;
chk_calc = 0;
for (i = 0; i < GX3_MSG_LEN - 2; i++) {
chk_calc += (uint8_t) * buff_add++;
}
return (chk_calc == ((((uint16_t) * buff_add) << 8) + (uint8_t) * (buff_add + 1)));
}
void ahrs_gx3_align(void)
{
ahrs_gx3.is_aligned = false;
//make the gyros zero, takes 10s (specified in Byte 4 and 5)
uart_put_byte(&GX3_PORT, 0, 0xcd);
uart_put_byte(&GX3_PORT, 0, 0xc1);
uart_put_byte(&GX3_PORT, 0, 0x29);
uart_put_byte(&GX3_PORT, 0, 0x27);
uart_put_byte(&GX3_PORT, 0, 0x10);
ahrs_gx3.is_aligned = true;
}
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_gx3(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GX3_INFO(trans, dev, AC_ID,
&ahrs_gx3.freq,
&ahrs_gx3.packet.chksm_error,
&ahrs_gx3.packet.hdr_error,
&ahrs_gx3.chksm);
}
#endif
/*
* GX3 can be set up during the startup, or it can be configured to
* start sending data automatically after power up.
*/
void imu_gx3_init(void)
{
// Initialize variables
ahrs_gx3.is_aligned = false;
// Initialize packet
ahrs_gx3.packet.status = GX3PacketWaiting;
ahrs_gx3.packet.msg_idx = 0;
ahrs_gx3.packet.msg_available = false;
ahrs_gx3.packet.chksm_error = 0;
ahrs_gx3.packet.hdr_error = 0;
// It is necessary to wait for GX3 to power up for proper initialization
for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY * 2; startup_counter++) {
__asm("nop");
}
#ifdef GX3_INITIALIZE_DURING_STARTUP
#pragma message "GX3 initializing"
/*
// FOR NON-CONTINUOUS MODE UNCOMMENT THIS
//4 byte command for non-Continous Mode so we can set the other settings
uart_put_byte(&GX3_PORT, 0, 0xc4);
uart_put_byte(&GX3_PORT, 0, 0xc1);
uart_put_byte(&GX3_PORT, 0, 0x29);
uart_put_byte(&GX3_PORT, 0, 0x00); // stop
*/
//Sampling Settings (0xDB)
uart_put_byte(&GX3_PORT, 0, 0xdb); //set update speed
uart_put_byte(&GX3_PORT, 0, 0xa8);
uart_put_byte(&GX3_PORT, 0, 0xb9);
//set rate of IMU link, is 1000/IMU_DIV
#define IMU_DIV1 0
#define IMU_DIV2 2
#define ACC_FILT_DIV 2
#define MAG_FILT_DIV 30
#ifdef GX3_SAVE_SETTINGS
uart_put_byte(&GX3_PORT, 0, 0x02);//set params and save them in non-volatile memory
#else
uart_put_byte(&GX3_PORT, 0, 0x02); //set and don't save
#endif
uart_put_byte(&GX3_PORT, 0, IMU_DIV1);
uart_put_byte(&GX3_PORT, 0, IMU_DIV2);
uart_put_byte(&GX3_PORT, 0, 0b00000000); //set options byte 8 - GOOD
uart_put_byte(&GX3_PORT, 0, 0b00000011); //set options byte 7 - GOOD
//0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data,
// 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved,
// 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation
// 12 - no quaternion calculation, 13-15 reserved
uart_put_byte(&GX3_PORT, 0, ACC_FILT_DIV);
uart_put_byte(&GX3_PORT, 0, MAG_FILT_DIV); //mag window filter size == 33hz
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 10); // Up Compensation in secs, def=10s
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 10); // North Compensation in secs
uart_put_byte(&GX3_PORT, 0, 0x00); //power setting = 0, high power/bw
uart_put_byte(&GX3_PORT, 0, 0x00); //rest of the bytes are 0
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 0x00);
// OPTIONAL: realign up and north
/*
uart_put_byte(&GX3_PORT, 0, 0xdd);
uart_put_byte(&GX3_PORT, 0, 0x54);
uart_put_byte(&GX3_PORT, 0, 0x4c);
uart_put_byte(&GX3_PORT, 0, 3);
uart_put_byte(&GX3_PORT, 0, 10);
uart_put_byte(&GX3_PORT, 0, 10);
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 0x00);
uart_put_byte(&GX3_PORT, 0, 0x00);
*/
//Another wait loop for proper GX3 init
for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY; startup_counter++) {
__asm("nop");
}
#ifdef GX3_SET_WAKEUP_MODE
//Mode Preset (0xD5)
uart_put_byte(&GX3_PORT, 0, 0xD5);
uart_put_byte(&GX3_PORT, 0, 0xBA);
uart_put_byte(&GX3_PORT, 0, 0x89);
uart_put_byte(&GX3_PORT, 0, 0x02); // wake up in continuous mode
//Continuous preset (0xD6)
uart_put_byte(&GX3_PORT, 0, 0xD6);
uart_put_byte(&GX3_PORT, 0, 0xC6);
uart_put_byte(&GX3_PORT, 0, 0x6B);
uart_put_byte(&GX3_PORT, 0, 0xc8); // accel, gyro, R
#endif
//4 byte command for Continous Mode
uart_put_byte(&GX3_PORT, 0, 0xc4);
uart_put_byte(&GX3_PORT, 0, 0xc1);
uart_put_byte(&GX3_PORT, 0, 0x29);
uart_put_byte(&GX3_PORT, 0, 0xc8); // accel,gyro, R
// Reset gyros to zero
ahrs_gx3_align();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GX3_INFO, send_gx3);
#endif
}
void imu_gx3_periodic(void)
{
/* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW
uart_put_byte(&GX3_PORT, 0, 0xc8); // accel,gyro, R
*/
}
void gx3_packet_read_message(void)
{
ahrs_gx3.accel.x = bef(&ahrs_gx3.packet.msg_buf[1]);
ahrs_gx3.accel.y = bef(&ahrs_gx3.packet.msg_buf[5]);
ahrs_gx3.accel.z = bef(&ahrs_gx3.packet.msg_buf[9]);
ahrs_gx3.rate.p = bef(&ahrs_gx3.packet.msg_buf[13]);
ahrs_gx3.rate.q = bef(&ahrs_gx3.packet.msg_buf[17]);
ahrs_gx3.rate.r = bef(&ahrs_gx3.packet.msg_buf[21]);
ahrs_gx3.rmat.m[0] = bef(&ahrs_gx3.packet.msg_buf[25]);
ahrs_gx3.rmat.m[1] = bef(&ahrs_gx3.packet.msg_buf[29]);
ahrs_gx3.rmat.m[2] = bef(&ahrs_gx3.packet.msg_buf[33]);
ahrs_gx3.rmat.m[3] = bef(&ahrs_gx3.packet.msg_buf[37]);
ahrs_gx3.rmat.m[4] = bef(&ahrs_gx3.packet.msg_buf[41]);
ahrs_gx3.rmat.m[5] = bef(&ahrs_gx3.packet.msg_buf[45]);
ahrs_gx3.rmat.m[6] = bef(&ahrs_gx3.packet.msg_buf[49]);
ahrs_gx3.rmat.m[7] = bef(&ahrs_gx3.packet.msg_buf[53]);
ahrs_gx3.rmat.m[8] = bef(&ahrs_gx3.packet.msg_buf[57]);
ahrs_gx3.time = (uint32_t)(ahrs_gx3.packet.msg_buf[61] << 24 |
ahrs_gx3.packet.msg_buf[62] << 16 |
ahrs_gx3.packet.msg_buf[63] << 8 |
ahrs_gx3.packet.msg_buf[64]);
ahrs_gx3.chksm = GX3_CHKSM(ahrs_gx3.packet.msg_buf);
ahrs_gx3.freq = 62500.0 / (float)(ahrs_gx3.time - ahrs_gx3.ltime);
ahrs_gx3.ltime = ahrs_gx3.time;
// Acceleration
VECT3_SMUL(ahrs_gx3.accel, ahrs_gx3.accel, 9.80665); // Convert g into m/s2
// for compatibility with fixed point interface
ACCELS_BFP_OF_REAL(imu.accel, ahrs_gx3.accel);
// Rates
// for compatibility with fixed point interface
RATES_BFP_OF_REAL(imu.gyro, ahrs_gx3.rate);
struct FloatRates body_rate;
/* compute body rates */
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
float_rmat_ratemult(&body_rate, body_to_imu_rmat, &ahrs_gx3.rate);
/* Set state */
stateSetBodyRates_f(&body_rate);
// Attitude
struct FloatRMat ltp_to_body_rmat;
float_rmat_comp(&ltp_to_body_rmat, &ahrs_gx3.rmat, body_to_imu_rmat);
#if AHRS_USE_GPS_HEADING && USE_GPS
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
float course_f = (float)DegOfRad(gps.course / 1e7);
if (course_f > 180.0) {
course_f -= 360.0;
}
ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else // !AHRS_USE_GPS_HEADING
#ifdef IMU_MAG_OFFSET
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
ltp_to_body_eulers.psi -= ahrs_gx3.mag_offset;
stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else
stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
#endif // IMU_MAG_OFFSET
#endif // !AHRS_USE_GPS_HEADING
}
/* GX3 Packet Collection */
void gx3_packet_parse(uint8_t c)
{
switch (ahrs_gx3.packet.status) {
case GX3PacketWaiting:
ahrs_gx3.packet.msg_idx = 0;
if (c == GX3_HEADER) {
ahrs_gx3.packet.status++;
ahrs_gx3.packet.msg_buf[ahrs_gx3.packet.msg_idx] = c;
ahrs_gx3.packet.msg_idx++;
} else {
ahrs_gx3.packet.hdr_error++;
}
break;
case GX3PacketReading:
ahrs_gx3.packet.msg_buf[ahrs_gx3.packet.msg_idx] = c;
ahrs_gx3.packet.msg_idx++;
if (ahrs_gx3.packet.msg_idx == GX3_MSG_LEN) {
if (gx3_verify_chk(ahrs_gx3.packet.msg_buf)) {
ahrs_gx3.packet.msg_available = true;
} else {
ahrs_gx3.packet.msg_available = false;
ahrs_gx3.packet.chksm_error++;
}
ahrs_gx3.packet.status = 0;
}
break;
default:
ahrs_gx3.packet.status = 0;
ahrs_gx3.packet.msg_idx = 0;
break;
}
}
void ahrs_gx3_init(void)
{
/* set ltp_to_imu so that body is zero */
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
QUAT_COPY(ahrs_gx3.ltp_to_imu_quat, *body_to_imu_quat);
#ifdef IMU_MAG_OFFSET
ahrs_gx3.mag_offset = IMU_MAG_OFFSET;
#else
ahrs_gx3.mag_offset = 0.0;
#endif
ahrs_gx3.is_aligned = false;
}
void ahrs_gx3_register(void)
{
ahrs_gx3_init();
/// @todo: provide enable function
ahrs_register_impl(NULL);
}
/* no scaling */
void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
void ahrs_gx3_publish_imu(void)
{
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(IMU_GX3_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_GX3_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_GX3_ID, now_ts, &imu.mag);
}
static inline void ReadGX3Buffer(void)
{
while (uart_char_available(&GX3_PORT) && !ahrs_gx3.packet.msg_available) {
gx3_packet_parse(uart_getch(&GX3_PORT));
}
}
void imu_gx3_event(void)
{
if (uart_char_available(&GX3_PORT)) {
ReadGX3Buffer();
}
if (ahrs_gx3.packet.msg_available) {
gx3_packet_read_message();
ahrs_gx3_publish_imu();
ahrs_gx3.packet.msg_available = false;
}
}
sw/airborne/modules/ahrs/ahrs_gx3.h
-100
View File
@@ -1,100 +0,0 @@
/*
* Copyright (C) 2013 Michal Podhradsky
* Utah State University, http://aggieair.usu.edu/
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file ahrs_gx3.h
*
* Driver for Microstrain GX3 IMU/AHRS subsystem
*
* Takes care of configuration of the IMU, communication and parsing
* the received packets. See GX3 datasheet for configuration options.
*
* @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*/
#ifndef AHRS_GX3_H
#define AHRS_GX3_H
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "modules/ins/ins.h"
#include "modules/gps/gps.h"
#include "mcu_periph/uart.h"
#include "state.h"
#include "led.h"
#define GX3_MAX_PAYLOAD 128
#define GX3_MSG_LEN 67
#define GX3_HEADER 0xC8
#define GX3_MIN_FREQ 300
#define IMU_GX3_LONG_DELAY 4000000
extern void gx3_packet_read_message(void);
extern void gx3_packet_parse(uint8_t c);
struct GX3Packet {
bool msg_available;
uint32_t chksm_error;
uint32_t hdr_error;
uint8_t msg_buf[GX3_MAX_PAYLOAD];
uint8_t status;
uint8_t msg_idx;
};
enum GX3PacketStatus {
GX3PacketWaiting,
GX3PacketReading
};
//AHRS
struct AhrsGX3 {
struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as quaternions
float mag_offset; ///< Difference between true and magnetic north
struct GX3Packet packet; ///< Packet struct
float freq; ///< data frequency
uint16_t chksm; ///< aux variable for checksum
uint32_t time; ///< GX3 time stamp
uint32_t ltime; ///< aux time stamp
struct FloatVect3 accel; ///< measured acceleration in IMU frame
struct FloatRates rate; ///< measured angular rates in IMU frame
struct FloatRMat rmat; ///< measured attitude in IMU frame (rotational matrix)
bool is_aligned;
};
extern struct AhrsGX3 ahrs_gx3;
#ifndef PRIMARY_AHRS
#define PRIMARY_AHRS ahrs_gx3
#endif
extern void ahrs_gx3_init(void);
extern void ahrs_gx3_align(void);
extern void ahrs_gx3_register(void);
extern void ahrs_gx3_publish_imu(void);
extern void imu_gx3_init(void);
extern void imu_gx3_periodic(void);
extern void imu_gx3_event(void);
#endif /* AHRS_GX3_H*/
sw/airborne/modules/imu/imu_drotek_10dof_v2.c
-167
View File
@@ -1,167 +0,0 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_drotek_10dof_v2.c
*
* Driver for the Drotek 10DOF V2 IMU.
* MPU6050 + HMC5883 + MS5611
* Reading the baro is not part of the IMU driver.
*
* By default the axes orientation should be as printed on the pcb,
* meaning z-axis pointing down if ICs are facing down.
* The orientation can be switched so that the IMU can be mounted ICs facing up
* by defining IMU_DROTEK_2_ORIENTATION_IC_UP.
*/
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
#if !defined DROTEK_2_LOWPASS_FILTER && !defined DROTEK_2_SMPLRT_DIV
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
*/
#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define DROTEK_2_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#elif PERIODIC_FREQUENCY == 512
/* Accelerometer: Bandwidth 260Hz, Delay 0ms
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
*/
#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define DROTEK_2_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#endif
#endif
PRINT_CONFIG_VAR(DROTEK_2_SMPLRT_DIV)
PRINT_CONFIG_VAR(DROTEK_2_LOWPASS_FILTER)
PRINT_CONFIG_VAR(DROTEK_2_GYRO_RANGE)
PRINT_CONFIG_VAR(DROTEK_2_ACCEL_RANGE)
#ifndef DROTEK_2_MPU_I2C_ADDR
#define DROTEK_2_MPU_I2C_ADDR MPU60X0_ADDR_ALT
#endif
PRINT_CONFIG_VAR(DROTEK_2_MPU_I2C_ADDR)
#ifndef DROTEK_2_HMC_I2C_ADDR
#define DROTEK_2_HMC_I2C_ADDR HMC58XX_ADDR
#endif
PRINT_CONFIG_VAR(DROTEK_2_HMC_I2C_ADDR)
struct ImuDrotek2 imu_drotek2;
void imu_drotek2_init(void)
{
/* MPU-60X0 */
mpu60x0_i2c_init(&imu_drotek2.mpu, &(DROTEK_2_I2C_DEV), DROTEK_2_MPU_I2C_ADDR);
// change the default configuration
imu_drotek2.mpu.config.smplrt_div = DROTEK_2_SMPLRT_DIV;
imu_drotek2.mpu.config.dlpf_cfg = DROTEK_2_LOWPASS_FILTER;
imu_drotek2.mpu.config.gyro_range = DROTEK_2_GYRO_RANGE;
imu_drotek2.mpu.config.accel_range = DROTEK_2_ACCEL_RANGE;
/* HMC5883 magnetometer */
hmc58xx_init(&imu_drotek2.hmc, &(DROTEK_2_I2C_DEV), DROTEK_2_HMC_I2C_ADDR);
/* mag is declared as slave to call the configure function,
* regardless if it is an actual MPU slave or passthrough
*/
imu_drotek2.mpu.config.nb_slaves = 1;
/* set callback function to configure mag */
imu_drotek2.mpu.config.slaves[0].configure = &imu_drotek2_configure_mag_slave;
// use hmc mag via passthrough
imu_drotek2.mpu.config.i2c_bypass = true;
}
void imu_drotek2_periodic(void)
{
// Start reading the latest gyroscope data
mpu60x0_i2c_periodic(&imu_drotek2.mpu);
// Read HMC58XX at ~50Hz (main loop for rotorcraft: 512Hz)
if (imu_drotek2.mpu.config.initialized) {
RunOnceEvery(10, hmc58xx_read(&imu_drotek2.hmc));
}
}
void imu_drotek2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
/* change orientation, so if ICs face up, z-axis is down */
imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
/* default orientation as should be printed on the pcb, z-down, ICs down */
RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif
imu_drotek2.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
hmc58xx_event(&imu_drotek2.hmc);
if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
imu_drotek2.hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
}
}
/** callback function to configure hmc5883 mag
* @return TRUE if mag configuration finished
*/
bool imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)),
void *mpu __attribute__((unused)))
{
hmc58xx_start_configure(&imu_drotek2.hmc);
if (imu_drotek2.hmc.initialized) {
return true;
} else {
return false;
}
}
sw/airborne/modules/imu/imu_drotek_10dof_v2.h
-87
View File
@@ -1,87 +0,0 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_drotek_10dof_v2.h
*
* Driver for the Drotek 10DOF V2 IMU.
* MPU6050 + HMC5883 + MS5611
*/
#ifndef IMU_DROTEK_10DOF_V2_H
#define IMU_DROTEK_10DOF_V2_H
#include "std.h"
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
#ifndef DROTEK_2_GYRO_RANGE
#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
#ifndef DROTEK_2_ACCEL_RANGE
#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#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 MPU60X0_GYRO_SENS[DROTEK_2_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[DROTEK_2_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[DROTEK_2_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DROTEK_2_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 MPU60X0_ACCEL_SENS[DROTEK_2_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[DROTEK_2_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[DROTEK_2_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][1]
#endif
struct ImuDrotek2 {
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
};
extern struct ImuDrotek2 imu_drotek2;
extern void imu_drotek2_init(void);
extern void imu_drotek2_periodic(void);
extern void imu_drotek2_event(void);
extern bool imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
#endif /* IMU_DROTEK_10DOF_V2_H */
sw/airborne/modules/imu/imu_gl1.c
-156
View File
@@ -1,156 +0,0 @@
/*
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
* 2013 Felix Ruess <felix.ruess@gmail.com>
* 2013 Eduardo Lavratti <agressiva@hotmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_gl1.c
* Driver for I2C IMU using L3G4200, ADXL345, HMC5883 and BMP085.
*/
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
PRINT_CONFIG_VAR(GL1_I2C_DEV)
/** adxl345 accelerometer output rate, lowpass is set to half of rate */
#ifndef GL1_ACCEL_RATE
# if PERIODIC_FREQUENCY <= 60
# define GL1_ACCEL_RATE ADXL345_RATE_50HZ
# elif PERIODIC_FREQUENCY <= 120
# define GL1_ACCEL_RATE ADXL345_RATE_100HZ
# else
# define GL1_ACCEL_RATE ADXL345_RATE_200HZ
# endif
#endif
PRINT_CONFIG_VAR(GL1_ACCEL_RATE)
/** gyro internal lowpass frequency */
#ifndef GL1_GYRO_LOWPASS
# if PERIODIC_FREQUENCY <= 60
# define GL1_GYRO_LOWPASS L3G4200_DLPF_1
# elif PERIODIC_FREQUENCY <= 120
# define GL1_GYRO_LOWPASS L3G4200_DLPF_2
# else
# define GL1_GYRO_LOWPASS L3G4200_DLPF_3
# endif
#endif
PRINT_CONFIG_VAR(GL1_GYRO_LOWPASS)
/** gyro sample rate divider */
#ifndef GL1_GYRO_SMPLRT
# if PERIODIC_FREQUENCY <= 60
# define GL1_GYRO_SMPLRT L3G4200_DR_100Hz
PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
# else
# define GL1_GYRO_SMPLRT L3G4200_DR_100Hz
PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
# endif
#endif
PRINT_CONFIG_VAR(GL1_GYRO_SMPLRT)
#ifdef GL1_GYRO_SCALE
# define L3G4200_SCALE GL1_GYRO_SCALE
# else
# define L3G4200_SCALE L3G4200_SCALE_2000
#endif
PRINT_CONFIG_VAR(L3G4200_SCALE)
struct ImuGL1I2c imu_gl1;
void imu_gl1_init(void)
{
/* Set accel configuration */
adxl345_i2c_init(&imu_gl1.acc_adxl, &(GL1_I2C_DEV), ADXL345_ADDR);
// set the data rate
imu_gl1.acc_adxl.config.rate = GL1_ACCEL_RATE;
/// @todo drdy int handling for adxl345
//imu_aspirin.acc_adxl.config.drdy_int_enable = true;
/* Gyro configuration and initalization */
l3g4200_init(&imu_gl1.gyro_l3g, &(GL1_I2C_DEV), L3G4200_ADDR_ALT);
/* change the default config */
// output data rate, bandwidth, enable axis (0x1f = 100 ODR, 25hz) (0x5f = 200hz ODR, 25hz)
imu_gl1.gyro_l3g.config.ctrl_reg1 = ((GL1_GYRO_SMPLRT << 6) | (GL1_GYRO_LOWPASS << 4) | 0xf);
// senstivity
imu_gl1.gyro_l3g.config.ctrl_reg4 = (L3G4200_SCALE << 4) | 0x00;
// filter config
imu_gl1.gyro_l3g.config.ctrl_reg5 = 0x00; // only first LPF active
/* initialize mag and set default options */
hmc58xx_init(&imu_gl1.mag_hmc, &(GL1_I2C_DEV), HMC58XX_ADDR);
imu_gl1.mag_hmc.type = HMC_TYPE_5883;
}
void imu_gl1_periodic(void)
{
adxl345_i2c_periodic(&imu_gl1.acc_adxl);
// Start reading the latest gyroscope data
l3g4200_periodic(&imu_gl1.gyro_l3g);
// Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
RunOnceEvery(10, hmc58xx_periodic(&imu_gl1.mag_hmc));
}
void imu_gl1_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_i2c_event(&imu_gl1.acc_adxl);
if (imu_gl1.acc_adxl.data_available) {
VECT3_COPY(imu.accel_unscaled, imu_gl1.acc_adxl.data.vect);
imu.accel_unscaled.x = imu_gl1.acc_adxl.data.vect.y;
imu.accel_unscaled.y = imu_gl1.acc_adxl.data.vect.x;
imu.accel_unscaled.z = -imu_gl1.acc_adxl.data.vect.z;
imu_gl1.acc_adxl.data_available = false;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_GL1_ID, now_ts, &imu.accel);
}
/* If the lg34200 I2C transaction has succeeded: convert the data */
l3g4200_event(&imu_gl1.gyro_l3g);
if (imu_gl1.gyro_l3g.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_gl1.gyro_l3g.data.rates);
imu.gyro_unscaled.p = imu_gl1.gyro_l3g.data.rates.q;
imu.gyro_unscaled.q = imu_gl1.gyro_l3g.data.rates.p;
imu.gyro_unscaled.r = -imu_gl1.gyro_l3g.data.rates.r;
imu_gl1.gyro_l3g.data_available = false;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_GL1_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
hmc58xx_event(&imu_gl1.mag_hmc);
if (imu_gl1.mag_hmc.data_available) {
// VECT3_COPY(imu.mag_unscaled, imu_gl1.mag_hmc.data.vect);
imu.mag_unscaled.y = imu_gl1.mag_hmc.data.vect.x;
imu.mag_unscaled.x = imu_gl1.mag_hmc.data.vect.y;
imu.mag_unscaled.z = -imu_gl1.mag_hmc.data.vect.z;
imu_gl1.mag_hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_GL1_ID, now_ts, &imu.mag);
}
}
sw/airborne/modules/imu/imu_gl1.h
-55
View File
@@ -1,55 +0,0 @@
/*
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
* 2013 Felix Ruess <felix.ruess@gmail.com>
* 2013 Eduardo Lavratti <agressiva@hotmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_gl1.h
* Interface for I2c IMU using using L3G4200, ADXL345, HMC5883 and BMP085.
*/
#ifndef IMU_GL1_H
#define IMU_GL1_H
#include "generated/airframe.h"
#include "modules/imu/imu.h"
/* include default GL1 sensitivity/channel definitions */
#include "modules/imu/imu_gl1_defaults.h"
#include "peripherals/l3g4200.h"
#include "peripherals/hmc58xx.h"
#include "peripherals/adxl345_i2c.h"
struct ImuGL1I2c {
struct Adxl345_I2c acc_adxl;
struct L3g4200 gyro_l3g;
struct Hmc58xx mag_hmc;
};
extern struct ImuGL1I2c imu_gl1;
extern void imu_gl1_init(void);
extern void imu_gl1_periodic(void);
extern void imu_gl1_event(void);
#endif /* IMU_GL1_H */
sw/airborne/modules/imu/imu_gl1_defaults.h
-77
View File
@@ -1,77 +0,0 @@
/*
* Copyright (C) 2010-2013 The Paparazzi Team
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_gl1_defaults.h
* Default sensitivity definitions for IMU GL1.
*/
#ifndef IMU_GL1_DEFAULTS_H
#define IMU_GL1_DEFAULTS_H
#include "generated/airframe.h"
/** default gyro sensitivy and neutral from the datasheet
* L3G4200 has 8.75 LSB/(deg/s)
* sens = 1/xxx * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/xxx * pi/180 * 4096 = ?????
*
* 250deg = 114.28 = 0.625
* 500deg = 57.14 = 1.25
* 2000deg = 14.28 = 5.006
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS 5.006
#define IMU_GYRO_P_SENS_NUM 2503
#define IMU_GYRO_P_SENS_DEN 500
#define IMU_GYRO_Q_SENS 5.006
#define IMU_GYRO_Q_SENS_NUM 2503
#define IMU_GYRO_Q_SENS_DEN 500
#define IMU_GYRO_R_SENS 5.006
#define IMU_GYRO_R_SENS_NUM 2503
#define IMU_GYRO_R_SENS_DEN 500
#endif
/** default accel sensitivy from the ADXL345 datasheet
* sensitivity of x & y axes depends on supply voltage:
* - 256 LSB/g @ 2.5V
* - 265 LSB/g @ 3.3V
* z sensitivity stays at 256 LSB/g
* fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC
* x/y sens = 9.81 / 265 * 1024 = 37.91
* z sens = 9.81 / 256 * 1024 = 39.24
*
* what about the offset at 3.3V?
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS 37.91
#define IMU_ACCEL_X_SENS_NUM 3791
#define IMU_ACCEL_X_SENS_DEN 100
#define IMU_ACCEL_Y_SENS 37.91
#define IMU_ACCEL_Y_SENS_NUM 3791
#define IMU_ACCEL_Y_SENS_DEN 100
#define IMU_ACCEL_Z_SENS 39.24
#define IMU_ACCEL_Z_SENS_NUM 3924
#define IMU_ACCEL_Z_SENS_DEN 100
#endif
#endif /* IMU_GL1_DEFAULTS_H */
sw/airborne/modules/imu/imu_ppzuav.c
-147
View File
@@ -1,147 +0,0 @@
/*
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
* 2013 Felix Ruess <felix.ruess@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_ppzuav.h
*
* Driver for the PPZUAV IMU.
*
* 9DoM IMU with ITG-3200, ADXL345 and HMC5843, all via I2C.
*/
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
/* i2c default suitable for Tiny/Twog */
#ifndef IMU_PPZUAV_I2C_DEV
#define IMU_PPZUAV_I2C_DEV i2c0
#endif
/** adxl345 accelerometer output rate, lowpass is set to half of rate */
#ifndef IMU_PPZUAV_ACCEL_RATE
# if PERIODIC_FREQUENCY <= 60
# define IMU_PPZUAV_ACCEL_RATE ADXL345_RATE_50HZ
# elif PERIODIC_FREQUENCY <= 120
# define IMU_PPZUAV_ACCEL_RATE ADXL345_RATE_100HZ
# else
# define IMU_PPZUAV_ACCEL_RATE ADXL345_RATE_200HZ
# endif
#endif
PRINT_CONFIG_VAR(IMU_PPZUAV_ACCEL_RATE)
/** gyro internal lowpass frequency */
#ifndef IMU_PPZUAV_GYRO_LOWPASS
# if PERIODIC_FREQUENCY <= 60
# define IMU_PPZUAV_GYRO_LOWPASS ITG3200_DLPF_20HZ
# elif PERIODIC_FREQUENCY <= 120
# define IMU_PPZUAV_GYRO_LOWPASS ITG3200_DLPF_42HZ
# else
# define IMU_PPZUAV_GYRO_LOWPASS ITG3200_DLPF_98HZ
# endif
#endif
PRINT_CONFIG_VAR(IMU_PPZUAV_GYRO_LOWPASS)
/** gyro sample rate divider */
#ifndef IMU_PPZUAV_GYRO_SMPLRT_DIV
# if PERIODIC_FREQUENCY <= 60
# define IMU_PPZUAV_GYRO_SMPLRT_DIV 19
PRINT_CONFIG_MSG("Gyro output rate is 50Hz")
# else
# define IMU_PPZUAV_GYRO_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
# endif
#endif
PRINT_CONFIG_VAR(IMU_PPZUAV_GYRO_SMPLRT_DIV)
struct ImuPpzuav imu_ppzuav;
void imu_ppzuav_init(void)
{
/* Set accel configuration */
adxl345_i2c_init(&imu_ppzuav.acc_adxl, &(IMU_PPZUAV_I2C_DEV), ADXL345_ADDR);
/* set the data rate */
imu_ppzuav.acc_adxl.config.rate = IMU_PPZUAV_ACCEL_RATE;
/* Gyro configuration and initalization */
itg3200_init(&imu_ppzuav.gyro_itg, &(IMU_PPZUAV_I2C_DEV), ITG3200_ADDR);
/* change the default config */
imu_ppzuav.gyro_itg.config.smplrt_div = IMU_PPZUAV_GYRO_SMPLRT_DIV;
imu_ppzuav.gyro_itg.config.dlpf_cfg = IMU_PPZUAV_GYRO_LOWPASS;
/* initialize mag and set default options */
hmc58xx_init(&imu_ppzuav.mag_hmc, &(IMU_PPZUAV_I2C_DEV), HMC58XX_ADDR);
/* set the type to the old HMC5843 */
imu_ppzuav.mag_hmc.type = HMC_TYPE_5843;
}
void imu_ppzuav_periodic(void)
{
adxl345_i2c_periodic(&imu_ppzuav.acc_adxl);
// Start reading the latest gyroscope data
itg3200_periodic(&imu_ppzuav.gyro_itg);
// Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
RunOnceEvery(10, hmc58xx_periodic(&imu_ppzuav.mag_hmc));
}
void imu_ppzuav_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_i2c_event(&imu_ppzuav.acc_adxl);
if (imu_ppzuav.acc_adxl.data_available) {
imu.accel_unscaled.x = -imu_ppzuav.acc_adxl.data.vect.x;
imu.accel_unscaled.y = imu_ppzuav.acc_adxl.data.vect.y;
imu.accel_unscaled.z = -imu_ppzuav.acc_adxl.data.vect.z;
imu_ppzuav.acc_adxl.data_available = false;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_PPZUAV_ID, now_ts, &imu.accel);
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
itg3200_event(&imu_ppzuav.gyro_itg);
if (imu_ppzuav.gyro_itg.data_available) {
imu.gyro_unscaled.p = -imu_ppzuav.gyro_itg.data.rates.p;
imu.gyro_unscaled.q = imu_ppzuav.gyro_itg.data.rates.q;
imu.gyro_unscaled.r = -imu_ppzuav.gyro_itg.data.rates.r;
imu_ppzuav.gyro_itg.data_available = false;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_PPZUAV_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
hmc58xx_event(&imu_ppzuav.mag_hmc);
if (imu_ppzuav.mag_hmc.data_available) {
imu.mag_unscaled.x = -imu_ppzuav.mag_hmc.data.vect.y;
imu.mag_unscaled.y = -imu_ppzuav.mag_hmc.data.vect.x;
imu.mag_unscaled.z = -imu_ppzuav.mag_hmc.data.vect.z;
imu_ppzuav.mag_hmc.data_available = false;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_PPZUAV_ID, now_ts, &imu.mag);
}
}
sw/airborne/modules/imu/imu_ppzuav.h
-94
View File
@@ -1,94 +0,0 @@
/*
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
* 2013 Felix Ruess <felix.ruess@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_ppzuav.h
*
* Interface and defaults for the PPZUAV IMU.
*
* 9DoM IMU with ITG-3200, ADXL345 and HMC5843, all via I2C.
*/
#ifndef IMU_PPZUAV_H
#define IMU_PPZUAV_H
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "peripherals/itg3200.h"
#include "peripherals/hmc58xx.h"
#include "peripherals/adxl345_i2c.h"
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
/** default gyro sensitivy and neutral from the datasheet
* ITG3200 has 14.375 LSB/(deg/s)
* sens = 1/14.375 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/14.375 * pi/180 * 4096 = 4.973126
*/
#define IMU_GYRO_P_SENS 4.973
#define IMU_GYRO_P_SENS_NUM 4973
#define IMU_GYRO_P_SENS_DEN 1000
#define IMU_GYRO_Q_SENS 4.973
#define IMU_GYRO_Q_SENS_NUM 4973
#define IMU_GYRO_Q_SENS_DEN 1000
#define IMU_GYRO_R_SENS 4.973
#define IMU_GYRO_R_SENS_NUM 4973
#define IMU_GYRO_R_SENS_DEN 1000
#endif
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
/** default accel sensitivy from the ADXL345 datasheet
* sensitivity of x & y axes depends on supply voltage:
* - 256 LSB/g @ 2.5V
* - 265 LSB/g @ 3.3V
* z sensitivity stays at 256 LSB/g
* fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC
* x/y sens = 9.81 / 265 * 1024 = 37.91
* z sens = 9.81 / 256 * 1024 = 39.24
*/
#define IMU_ACCEL_X_SENS 37.91
#define IMU_ACCEL_X_SENS_NUM 3791
#define IMU_ACCEL_X_SENS_DEN 100
#define IMU_ACCEL_Y_SENS 37.91
#define IMU_ACCEL_Y_SENS_NUM 3791
#define IMU_ACCEL_Y_SENS_DEN 100
#define IMU_ACCEL_Z_SENS 39.24
#define IMU_ACCEL_Z_SENS_NUM 3924
#define IMU_ACCEL_Z_SENS_DEN 100
#endif
struct ImuPpzuav {
struct Adxl345_I2c acc_adxl;
struct Itg3200 gyro_itg;
struct Hmc58xx mag_hmc;
};
extern struct ImuPpzuav imu_ppzuav;
extern void imu_ppzuav_init(void);
extern void imu_ppzuav_periodic(void);
extern void imu_ppzuav_event(void);
#endif /* IMU_PPZUAV_H */
sw/airborne/modules/imu/imu_swing.c
-118
View File
@@ -1,118 +0,0 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_swing.c
* Driver for the Swing accelerometer and gyroscope
*/
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/i2c.h"
/* defaults suitable for Swing */
#ifndef SWING_MPU_I2C_DEV
#define SWING_MPU_I2C_DEV i2c0
#endif
PRINT_CONFIG_VAR(SWING_MPU_I2C_DEV)
#if !defined SWING_LOWPASS_FILTER && !defined SWING_SMPLRT_DIV
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
*/
#define SWING_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define SWING_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#elif PERIODIC_FREQUENCY == 512
/* Accelerometer: Bandwidth 260Hz, Delay 0ms
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
*/
#define SWING_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define SWING_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#endif
#endif
PRINT_CONFIG_VAR(SWING_SMPLRT_DIV)
PRINT_CONFIG_VAR(SWING_LOWPASS_FILTER)
PRINT_CONFIG_VAR(SWING_GYRO_RANGE)
PRINT_CONFIG_VAR(SWING_ACCEL_RANGE)
/** Basic Navstik IMU data */
struct ImuSwing imu_swing;
/**
* Navstik IMU initializtion of the MPU-60x0 and HMC58xx
*/
void imu_swing_init(void)
{
/* MPU-60X0 */
mpu60x0_i2c_init(&imu_swing.mpu, &(SWING_MPU_I2C_DEV), MPU60X0_ADDR);
imu_swing.mpu.config.smplrt_div = SWING_SMPLRT_DIV;
imu_swing.mpu.config.dlpf_cfg = SWING_LOWPASS_FILTER;
imu_swing.mpu.config.gyro_range = SWING_GYRO_RANGE;
imu_swing.mpu.config.accel_range = SWING_ACCEL_RANGE;
}
/**
* Handle all the periodic tasks of the Navstik IMU components.
* Read the MPU60x0 every periodic call
*/
void imu_swing_periodic(void)
{
// Start reading the latest gyroscope data
mpu60x0_i2c_periodic(&imu_swing.mpu);
}
/**
* Handle all the events of the Navstik IMU components.
* When there is data available convert it to the correct axis and save it in the imu structure.
*/
void imu_swing_event(void)
{
uint32_t now_ts = get_sys_time_usec();
/* MPU-60x0 event taks */
mpu60x0_i2c_event(&imu_swing.mpu);
if (imu_swing.mpu.data_available) {
/* default orientation of the MPU is upside down and in plane mode
* turn it to have rotorcraft mode by default */
RATES_ASSIGN(imu.gyro_unscaled,
-imu_swing.mpu.data_rates.rates.r,
-imu_swing.mpu.data_rates.rates.q,
-imu_swing.mpu.data_rates.rates.p);
VECT3_ASSIGN(imu.accel_unscaled,
-imu_swing.mpu.data_accel.vect.z,
-imu_swing.mpu.data_accel.vect.y,
-imu_swing.mpu.data_accel.vect.x);
imu_swing.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
}
}
sw/airborne/modules/imu/imu_swing.h
-83
View File
@@ -1,83 +0,0 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* Copyright (C) 2017 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/imu/imu_swing.h
* Interface for the Swing accelerometer and gyroscope
*/
#ifndef IMU_SWING_H
#define IMU_SWING_H
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#ifndef SWING_GYRO_RANGE
#define SWING_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
#ifndef SWING_ACCEL_RANGE
#define SWING_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#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 MPU60X0_GYRO_SENS[SWING_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[SWING_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[SWING_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[SWING_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[SWING_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 MPU60X0_ACCEL_SENS[SWING_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[SWING_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[SWING_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[SWING_ACCEL_RANGE][1]
#endif
/** Everything that is in the swing IMU */
struct ImuSwing {
struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device
};
extern struct ImuSwing imu_swing;
extern void imu_swing_init(void);
extern void imu_swing_periodic(void);
extern void imu_swing_event(void);
#endif /* IMU_SWING_H */
sw/airborne/modules/imu/imu_um6.c
-375
View File
@@ -1,375 +0,0 @@
/*
* Copyright (C) 2013 Michal Podhradsky
* Utah State University, http://aggieair.usu.edu/
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file imu_um6.c
*
* Driver for CH Robotics UM6 IMU/AHRS subsystem
*
* Takes care of configuration of the IMU, communication and parsing
* the received packets. See UM6 datasheet for configuration options.
* Should be used with ahrs_extern_euler AHRS subsystem.
*
* @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*/
#include "modules/imu/imu_um6.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
struct UM6Packet UM6_packet;
uint8_t buf_out[IMU_UM6_BUFFER_LENGTH];
uint16_t data_chk;
uint8_t PacketLength;
uint8_t PacketType;
uint8_t PacketAddr;
enum UM6Status UM6_status;
uint16_t chk_calc;
uint16_t chk_rec;
struct FloatVect3 UM6_accel;
struct FloatRates UM6_rate;
struct FloatVect3 UM6_mag;
struct FloatEulers UM6_eulers;
struct FloatQuat UM6_quat;
inline void UM6_imu_align(void);
inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length);
inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length);
inline bool UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length);
inline bool UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length)
{
chk_rec = (packet_buffer[packet_length - 2] << 8) | packet_buffer[packet_length - 1];
chk_calc = UM6_calculate_checksum(packet_buffer, packet_length - 2);
return (chk_calc == chk_rec);
}
inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length)
{
uint16_t chk = 0;
for (int i = 0; i < packet_length; i++) {
chk += packet_buffer[i];
}
return chk;
}
inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length)
{
for (int i = 0; i < packet_length; i++) {
uart_put_byte(&(UM6_LINK), 0, packet_buffer[i]);
}
}
void UM6_imu_align(void)
{
UM6_status = UM6Uninit;
// Acceleration vector realign
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0;
buf_out[4] = IMU_UM6_SET_MAG_REF;
data_chk = UM6_calculate_checksum(buf_out, 5);
buf_out[5] = (uint8_t)(data_chk >> 8);
buf_out[6] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 7);
// Magnetic realign
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0;
buf_out[4] = IMU_UM6_SET_ACCEL_REF;
data_chk = UM6_calculate_checksum(buf_out, 5);
buf_out[5] = (uint8_t)(data_chk >> 8);
buf_out[6] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 7);
// Zero gyros 0xAC, takes around 3s
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0;
buf_out[4] = IMU_UM6_ZERO_GYROS_CMD;
data_chk = UM6_calculate_checksum(buf_out, 5);
buf_out[5] = (uint8_t)(data_chk >> 8);
buf_out[6] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 7);
// Reset EKF 0xAD
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0;
buf_out[4] = IMU_UM6_RESET_EKF_CMD;
data_chk = UM6_calculate_checksum(buf_out, 5);
buf_out[5] = (uint8_t)(data_chk >> 8);
buf_out[6] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 7);
UM6_status = UM6Running;
}
void imu_um6_init(void)
{
// Initialize variables
UM6_status = UM6Uninit;
// Initialize packet
UM6_packet.status = UM6PacketWaiting;
UM6_packet.msg_idx = 0;
UM6_packet.msg_available = false;
UM6_packet.chksm_error = 0;
UM6_packet.hdr_error = 0;
// Communication register 0x00
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0x80; // has data, zeros
buf_out[4] = IMU_UM6_COMMUNICATION_REG; // communication register address
buf_out[5] = 0b01000110; // B3 - broadcast enabled, processed data enabled (acc, gyro, mag)
buf_out[6] = 0b10000000; // B2 - no euler angles, quaternions enabled
buf_out[7] = 0b00101101; // B1 - baud 115200
buf_out[8] = 100; // B0 - broadcast rate, 1=20Hz, 100=130Hz, 255=300Hz
data_chk = UM6_calculate_checksum(buf_out, 9);
buf_out[9] = (uint8_t)(data_chk >> 8);
buf_out[10] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 11);
// Config register 0x01
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0x80; // has data, zeros
buf_out[4] = IMU_UM6_MISC_CONFIG_REG; // config register address
buf_out[5] = 0b11110000; // B3 - mag, accel updates enabled, use quaternions
buf_out[6] = 0; // B2 - RES
buf_out[7] = 0; // B1 - RES
buf_out[8] = 0; // B0 - RES
data_chk = UM6_calculate_checksum(buf_out, 9);
buf_out[9] = (uint8_t)(data_chk >> 8);
buf_out[10] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 11);
/*
// Get firmware
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0;
buf_out[4] = IMU_UM6_GET_FIRMWARE_CMD;
data_chk = UM6_calculate_checksum(buf_out, 5);
buf_out[5] = (uint8_t)(data_chk >> 8);
buf_out[6] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 7);
*/
UM6_imu_align();
}
void imu_um6_periodic(void)
{
/* We would request for data here - optional
//GET_DATA command 0xAE
buf_out[0] = 's';
buf_out[1] = 'n';
buf_out[2] = 'p';
buf_out[3] = 0; // zeros
buf_out[4] = 0xAE; // GET_DATA command
data_chk = UM6_calculate_checksum(buf_out, 5);
buf_out[5] = (uint8_t)(data_chk >> 8);
buf_out[6] = (uint8_t)data_chk;
UM6_send_packet(buf_out, 7);
*/
}
void UM6_packet_read_message(void)
{
if ((UM6_status == UM6Running) && PacketLength > 11) {
switch (PacketAddr) {
case IMU_UM6_GYRO_PROC:
UM6_rate.p =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
UM6_rate.q =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
UM6_rate.r =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
RATES_BFP_OF_REAL(imu.gyro, UM6_rate);
break;
case IMU_UM6_ACCEL_PROC:
UM6_accel.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
UM6_accel.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
UM6_accel.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
break;
case IMU_UM6_MAG_PROC:
UM6_mag.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
UM6_mag.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
UM6_mag.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
// Assume the same units for magnetic field
// Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
MAGS_BFP_OF_REAL(imu.mag, UM6_mag);
break;
case IMU_UM6_EULER:
UM6_eulers.phi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
UM6_eulers.theta =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
UM6_eulers.psi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
break;
case IMU_UM6_QUAT:
UM6_quat.qi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
UM6_quat.qx =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
UM6_quat.qy =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
UM6_quat.qz =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
break;
default:
break;
}
}
}
/* UM6 Packet Collection */
void UM6_packet_parse(uint8_t c)
{
switch (UM6_packet.status) {
case UM6PacketWaiting:
UM6_packet.msg_idx = 0;
if (c == 's') {
UM6_packet.status = UM6PacketReadingS;
UM6_packet.msg_buf[UM6_packet.msg_idx] = c;
UM6_packet.msg_idx++;
} else {
UM6_packet.hdr_error++;
UM6_packet.status = UM6PacketWaiting;
}
break;
case UM6PacketReadingS:
if (c == 'n') {
UM6_packet.status = UM6PacketReadingN;
UM6_packet.msg_buf[UM6_packet.msg_idx] = c;
UM6_packet.msg_idx++;
} else {
UM6_packet.hdr_error++;
UM6_packet.status = UM6PacketWaiting;
}
break;
case UM6PacketReadingN:
if (c == 'p') {
UM6_packet.status = UM6PacketReadingPT;
UM6_packet.msg_buf[UM6_packet.msg_idx] = c;
UM6_packet.msg_idx++;
} else {
UM6_packet.hdr_error++;
UM6_packet.status = UM6PacketWaiting;
}
break;
case UM6PacketReadingPT:
PacketType = c;
UM6_packet.msg_buf[UM6_packet.msg_idx] = c;
UM6_packet.msg_idx++;
UM6_packet.status = UM6PacketReadingAddr;
if ((PacketType & 0xC0) == 0xC0) {
PacketLength = 4 * ((PacketType >> 2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data
} else if ((PacketType & 0xC0) == 0x80) {
PacketLength = 11; // Not batch, has 4 bytes of data
} else if ((PacketType & 0xC0) == 0x00) {
PacketLength = 7; // Not batch, no data
}
break;
case UM6PacketReadingAddr:
PacketAddr = c;
UM6_packet.msg_buf[UM6_packet.msg_idx] = c;
UM6_packet.msg_idx++;
UM6_packet.status = UM6PacketReadingData;
break;
case UM6PacketReadingData:
UM6_packet.msg_buf[UM6_packet.msg_idx] = c;
UM6_packet.msg_idx++;
if (UM6_packet.msg_idx == PacketLength) {
if (UM6_verify_chk(UM6_packet.msg_buf, PacketLength)) {
UM6_packet.msg_available = true;
} else {
UM6_packet.msg_available = false;
UM6_packet.chksm_error++;
}
UM6_packet.status = UM6PacketWaiting;
}
break;
default:
UM6_packet.status = UM6PacketWaiting;
UM6_packet.msg_idx = 0;
break;
}
}
/* no scaling */
void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
void imu_um6_publish(void)
{
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(IMU_UM6_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_UM6_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_UM6_ID, now_ts, &imu.mag);
}
sw/airborne/modules/imu/imu_um6.h
-119
View File
@@ -1,119 +0,0 @@
/*
* Copyright (C) 2013 Michal Podhradsky
* Utah State University, http://aggieair.usu.edu/
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file imu_um6.h
*
* Driver for CH Robotics UM6 IMU/AHRS subsystem
*
* Takes care of configuration of the IMU, communication and parsing
* the received packets. See UM6 datasheet for configuration options.
* Should be used with ahrs_extern_euler AHRS subsystem.
*
* @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*/
#ifndef IMU_UM6_H
#define IMU_UM6_H
#include "generated/airframe.h"
#include "modules/imu/imu.h"
#include "mcu_periph/uart.h"
#include "modules/ahrs/ahrs.h"
#define IMU_UM6_BUFFER_LENGTH 32
#define IMU_UM6_DATA_OFFSET 5
#define IMU_UM6_LONG_DELAY 4000000
#define IMU_UM6_COMMUNICATION_REG 0x00
#define IMU_UM6_MISC_CONFIG_REG 0x01
#define IMU_UM6_GET_FIRMWARE_CMD 0xAA
#define IMU_UM6_ZERO_GYROS_CMD 0xAC
#define IMU_UM6_RESET_EKF_CMD 0xAD
#define IMU_UM6_GET_DATA 0xAE
#define IMU_UM6_SET_ACCEL_REF 0xAF
#define IMU_UM6_SET_MAG_REF 0xB0
#define IMU_UM6_GYRO_PROC 0x5C
#define IMU_UM6_ACCEL_PROC 0x5E
#define IMU_UM6_MAG_PROC 0x60
#define IMU_UM6_EULER 0x62
#define IMU_UM6_QUAT 0x64
extern void UM6_packet_read_message(void);
extern void UM6_packet_parse(uint8_t c);
extern void imu_um6_publish(void);
extern struct UM6Packet UM6_packet;
extern uint8_t PacketLength;
extern uint8_t PacketType;
extern uint8_t PacketAddr;
extern uint16_t chk_calc;
extern uint16_t chk_rec;
extern enum UM6Status UM6_status;
extern volatile uint8_t UM6_imu_available;
extern struct FloatEulers UM6_eulers;
extern struct FloatQuat UM6_quat;
struct UM6Packet {
bool msg_available;
uint32_t chksm_error;
uint32_t hdr_error;
uint8_t msg_buf[IMU_UM6_BUFFER_LENGTH];
uint8_t status;
uint8_t msg_idx;
};
enum UM6PacketStatus {
UM6PacketWaiting,
UM6PacketReadingS,
UM6PacketReadingN,
UM6PacketReadingPT,
UM6PacketReadingAddr,
UM6PacketReadingData
};
enum UM6Status {
UM6Uninit,
UM6Running
};
extern void imu_um6_init(void);
extern void imu_um6_periodic(void);
static inline void imu_um6_event(void)
{
if (uart_char_available(&(UM6_LINK))) {
while (uart_char_available(&(UM6_LINK)) && !UM6_packet.msg_available) {
UM6_packet_parse(uart_getch(&(UM6_LINK)));
}
if (UM6_packet.msg_available) {
UM6_packet.msg_available = false;
UM6_packet_read_message();
imu_um6_publish();
}
}
}
#endif /* IMU_UM6_H*/
sw/tools/parrot/swing.py
-48
View File
@@ -1,48 +0,0 @@
#!/usr/bin/env python
#
# Copyright (C) 2012-2017 The Paparazzi Team
#
# 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/>.
#
from __future__ import print_function
from parrot_utils import ParrotUtils
class Swing(ParrotUtils):
uav_name = 'Swing'
address = '192.168.4.1'
version_file = None
upload_path = '/data/edu/'
prompt = '$ '
def uav_status(self):
print('Parrot version:\t\t' + str(self.check_version()))
def init_extra_parser(self):
# nothing here
pass
def parse_extra_args(self, args):
# nothing here
pass
if __name__ == "__main__":
swing = Swing()
swing.parse_args()
exit(0)