mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 20:36:06 +08:00
[subsystems][imu] cleaned up imu_ppzuav driver no modules for imu_ppzuav and imu_aspirin_i2c anymore
This commit is contained in:
Felix Ruess
@@ -203,8 +203,6 @@
|
||||
-->
|
||||
<!-- <load name="digital_cam_i2c.xml"/> -->
|
||||
|
||||
<!-- <load name="ins_ppzuavimu.xml" /> -->
|
||||
|
||||
<!--
|
||||
<load name="digital_cam.xml" >
|
||||
<define name="DC_SHUTTER_LED" value="3"/>
|
||||
|
||||
@@ -1,4 +1,2 @@
|
||||
include $(CFG_FIXEDWING)/ahrs_int_cmpl_quat.makefile
|
||||
|
||||
$(warning The ahrs_ic subsystem has been renamed, please replace <subsystem name="ahrs" type="ic"/> with <subsystem name="ahrs" type="int_cmpl_quat"/> in your airframe file.)
|
||||
$(error The ahrs_ic subsystem has been renamed, please replace <subsystem name="ahrs" type="ic"/> with <subsystem name="ahrs" type="int_cmpl_quat"/> in your airframe file.)
|
||||
|
||||
|
||||
@@ -1,2 +1,2 @@
|
||||
#generic i2c driver
|
||||
$(warning The i2c subsystem does not have to be specified explicitly anymore, it is always included now. Please remove the line <subsystem name="i2c"/> from the firmware section of your airframe file!)
|
||||
$(error The i2c subsystem does not have to be specified explicitly anymore, it is always included now. Remove the line <subsystem name="i2c"/> from the firmware section of your airframe file!)
|
||||
|
||||
@@ -1,5 +1,3 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
|
||||
include $(CFG_FIXEDWING)/imu_ppzuav.makefile
|
||||
$(error The imu aspirin_i2c subsystem was rewritten, please replace <subsystem name="imu" type="aspirin_i2c"/> with type="aspirin_i2c_v1.0" or type="aspirin_i2c_v1.5" in your airframe file.)
|
||||
|
||||
ap.CFLAGS += -DASPIRIN_IMU
|
||||
|
||||
@@ -1,19 +1,21 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
|
||||
IMU_PPZUAV_CFLAGS = -DUSE_IMU
|
||||
IMU_PPZUAV_CFLAGS += -DIMU_TYPE_H=\"modules/sensors/imu_ppzuav.h\"
|
||||
IMU_PPZUAV_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ppzuav.h\"
|
||||
|
||||
IMU_PPZUAV_SRCS = $(SRC_SUBSYSTEMS)/imu.c
|
||||
IMU_PPZUAV_SRCS += modules/sensors/imu_ppzuav.c
|
||||
|
||||
IMU_PPZUAV_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_ppzuav.c
|
||||
IMU_PPZUAV_SRCS += peripherals/adxl345_i2c.c
|
||||
IMU_PPZUAV_SRCS += peripherals/itg3200.c
|
||||
IMU_PPZUAV_SRCS += peripherals/hmc58xx.c
|
||||
|
||||
IMU_PPZUAV_CFLAGS += -DUSE_I2C
|
||||
ifeq ($(ARCH), stm32)
|
||||
IMU_PPZUAV_CFLAGS += -DUSE_I2C2
|
||||
IMU_PPZUAV_CFLAGS += -DPPZUAVIMU_I2C_DEV=i2c2
|
||||
IMU_PPZUAV_CFLAGS += -DIMU_PPZUAV_I2C_DEV=i2c2
|
||||
else ifeq ($(ARCH), lpc21)
|
||||
IMU_PPZUAV_CFLAGS += -DUSE_I2C0
|
||||
IMU_PPZUAV_CFLAGS += -DPPZUAVIMU_I2C_DEV=i2c0
|
||||
IMU_PPZUAV_CFLAGS += -DIMU_PPZUAV_I2C_DEV=i2c0
|
||||
endif
|
||||
|
||||
ap.CFLAGS += $(IMU_PPZUAV_CFLAGS)
|
||||
|
||||
@@ -1,28 +0,0 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="imu_aspirin_i2c" dir="sensors">
|
||||
<doc>
|
||||
<description>Aspirin IMU (all I2C version)</description>
|
||||
</doc>
|
||||
<!-- <depend conflict="ins" -->
|
||||
<header>
|
||||
<file name="imu_ppzuav.h"/>
|
||||
</header>
|
||||
|
||||
<!-- default imu stuff -->
|
||||
<init fun="imu_impl_init()"/>
|
||||
<periodic fun="imu_periodic()" freq="60"/>
|
||||
<!-- ImuEvent called directly from main_ap -->
|
||||
|
||||
<!-- extras to become a usefull module -->
|
||||
<periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
|
||||
<event fun="ppzuavimu_module_event()"/>
|
||||
|
||||
<makefile>
|
||||
<file name="imu_ppzuav.c"/>
|
||||
<define name="PPZUAVIMU_I2C_DEV" value="i2c0" />
|
||||
<define name="USE_I2C" />
|
||||
<define name="USE_I2C0" />
|
||||
<define name="ASPIRIN_IMU" />
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -1,27 +0,0 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="imu_ppzuav" dir="sensors">
|
||||
<doc>
|
||||
<description></description>
|
||||
</doc>
|
||||
<!-- <depend conflict="ins" -->
|
||||
<header>
|
||||
<file name="imu_ppzuav.h"/>
|
||||
</header>
|
||||
|
||||
<!-- default imu stuff -->
|
||||
<init fun="imu_impl_init()"/>
|
||||
<periodic fun="imu_periodic()" freq="60"/>
|
||||
<!-- ImuEvent called directly from main_ap -->
|
||||
|
||||
<!-- extras to become a usefull module -->
|
||||
<periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
|
||||
<event fun="ppzuavimu_module_event()"/>
|
||||
|
||||
<makefile target="ap">
|
||||
<file name="imu_ppzuav.c"/>
|
||||
<define name="PPZUAVIMU_I2C_DEV" value="i2c0" />
|
||||
<define name="USE_I2C" />
|
||||
<define name="USE_I2C0" />
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -1,17 +0,0 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="ins_ppzuavimu" dir="ins">
|
||||
<doc>
|
||||
<description></description>
|
||||
</doc>
|
||||
|
||||
<header>
|
||||
<file name="imu_ppzuav.h"/>
|
||||
</header>
|
||||
|
||||
<makefile target="ap">
|
||||
<raw>
|
||||
$(error The ins_ppzuavimu module has been renamed, please replace the name="ins_ppzuavimu.xml" with name="imu_ppzuav.xml" in the load tag of your airframe file modules section.)
|
||||
</raw>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -1,273 +0,0 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "imu_ppzuav.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
#include "led.h"
|
||||
|
||||
// Set SPI_CS High
|
||||
#include "mcu_periph/gpio_arch.h"
|
||||
|
||||
// Downlink
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
|
||||
#ifndef DOWNLINK_DEVICE
|
||||
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||
#endif
|
||||
|
||||
|
||||
// Peripherials
|
||||
#define HMC5843_NO_IRQ
|
||||
#include "peripherals/itg3200_regs.h"
|
||||
#include "peripherals/adxl345_regs.h"
|
||||
#include "peripherals/hmc5843_regs.h"
|
||||
|
||||
// Results
|
||||
volatile bool_t mag_valid;
|
||||
volatile bool_t gyr_valid;
|
||||
volatile bool_t acc_valid;
|
||||
|
||||
// Communication
|
||||
struct i2c_transaction ppzuavimu_hmc5843;
|
||||
struct i2c_transaction ppzuavimu_itg3200;
|
||||
struct i2c_transaction ppzuavimu_adxl345;
|
||||
|
||||
// Standalone option: run module only
|
||||
#ifndef IMU_TYPE_H
|
||||
struct Imu imu;
|
||||
#endif
|
||||
|
||||
#ifndef PERIODIC_FREQUENCY
|
||||
#define PERIODIC_FREQUENCY 60
|
||||
#endif
|
||||
|
||||
void imu_impl_init(void)
|
||||
{
|
||||
GPIO_ARCH_SET_SPI_CS_HIGH();
|
||||
|
||||
/////////////////////////////////////////////////////////////////////
|
||||
// ITG3200
|
||||
ppzuavimu_itg3200.type = I2CTransTx;
|
||||
ppzuavimu_itg3200.slave_addr = ITG3200_ADDR;
|
||||
ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS;
|
||||
#if PERIODIC_FREQUENCY == 60
|
||||
/* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */
|
||||
ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
|
||||
# pragma message "ITG3200 read at 50Hz."
|
||||
#else
|
||||
# if PERIODIC_FREQUENCY == 120
|
||||
# pragma message "ITG3200 read at 100Hz."
|
||||
/* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
|
||||
ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0);
|
||||
# else
|
||||
# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
|
||||
# endif
|
||||
#endif
|
||||
ppzuavimu_itg3200.len_w = 2;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_itg3200);
|
||||
while(ppzuavimu_itg3200.status == I2CTransPending);
|
||||
|
||||
/* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
|
||||
ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
|
||||
#if PERIODIC_FREQUENCY == 60
|
||||
ppzuavimu_itg3200.buf[1] = 19; // 50Hz
|
||||
#else
|
||||
ppzuavimu_itg3200.buf[1] = 9; // 100Hz
|
||||
#endif
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_itg3200);
|
||||
while(ppzuavimu_itg3200.status == I2CTransPending);
|
||||
|
||||
/* switch to gyroX clock */
|
||||
ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
|
||||
ppzuavimu_itg3200.buf[1] = 0x01;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_itg3200);
|
||||
while(ppzuavimu_itg3200.status == I2CTransPending);
|
||||
|
||||
/* no interrupts for now, but set data ready interrupt to enable reading status bits */
|
||||
ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG;
|
||||
ppzuavimu_itg3200.buf[1] = 0x01;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_itg3200);
|
||||
while(ppzuavimu_itg3200.status == I2CTransPending);
|
||||
|
||||
/////////////////////////////////////////////////////////////////////
|
||||
// ADXL345
|
||||
|
||||
/* set data rate to 100Hz */
|
||||
ppzuavimu_adxl345.slave_addr = ADXL345_ADDR;
|
||||
ppzuavimu_adxl345.type = I2CTransTx;
|
||||
ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE;
|
||||
#if PERIODIC_FREQUENCY == 60
|
||||
ppzuavimu_adxl345.buf[1] = ADXL345_RATE_50HZ; // normal power and 50Hz sampling, 25Hz BW
|
||||
#else
|
||||
ppzuavimu_adxl345.buf[1] = ADXL345_RATE_100HZ; // normal power and 100Hz sampling, 50Hz BW
|
||||
#endif
|
||||
ppzuavimu_adxl345.len_w = 2;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_adxl345);
|
||||
while(ppzuavimu_adxl345.status == I2CTransPending);
|
||||
|
||||
/* switch to measurement mode */
|
||||
ppzuavimu_adxl345.type = I2CTransTx;
|
||||
ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
|
||||
ppzuavimu_adxl345.buf[1] = 1<<3;
|
||||
ppzuavimu_adxl345.len_w = 2;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_adxl345);
|
||||
while(ppzuavimu_adxl345.status == I2CTransPending);
|
||||
|
||||
/* Set range to 16g but keeping full resolution of 3.9 mV/g */
|
||||
ppzuavimu_adxl345.type = I2CTransTx;
|
||||
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
|
||||
ppzuavimu_adxl345.buf[1] = ADXL345_FULL_RES | ADXL345_RANGE_16G;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_adxl345);
|
||||
while(ppzuavimu_adxl345.status == I2CTransPending);
|
||||
|
||||
/////////////////////////////////////////////////////////////////////
|
||||
// HMC5843
|
||||
ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
|
||||
ppzuavimu_hmc5843.type = I2CTransTx;
|
||||
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
|
||||
ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
|
||||
ppzuavimu_hmc5843.len_w = 2;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
|
||||
while(ppzuavimu_hmc5843.status == I2CTransPending);
|
||||
|
||||
ppzuavimu_hmc5843.type = I2CTransTx;
|
||||
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
|
||||
ppzuavimu_hmc5843.buf[1] = 0x01<<5;
|
||||
ppzuavimu_hmc5843.len_w = 2;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
|
||||
while(ppzuavimu_hmc5843.status == I2CTransPending);
|
||||
|
||||
ppzuavimu_hmc5843.type = I2CTransTx;
|
||||
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
|
||||
ppzuavimu_hmc5843.buf[1] = 0x00;
|
||||
ppzuavimu_hmc5843.len_w = 2;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
|
||||
while(ppzuavimu_hmc5843.status == I2CTransPending);
|
||||
|
||||
}
|
||||
|
||||
void imu_periodic( void )
|
||||
{
|
||||
// Start reading the latest gyroscope data
|
||||
ppzuavimu_itg3200.type = I2CTransTxRx;
|
||||
ppzuavimu_itg3200.len_r = 9;
|
||||
ppzuavimu_itg3200.len_w = 1;
|
||||
ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_itg3200);
|
||||
|
||||
// Start reading the latest accelerometer data
|
||||
ppzuavimu_adxl345.type = I2CTransTxRx;
|
||||
ppzuavimu_adxl345.len_r = 6;
|
||||
ppzuavimu_adxl345.len_w = 1;
|
||||
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345);
|
||||
|
||||
// Start reading the latest magnetometer data
|
||||
#if PERIODIC_FREQUENCY > 60
|
||||
RunOnceEvery(2,{
|
||||
#endif
|
||||
ppzuavimu_hmc5843.type = I2CTransTxRx;
|
||||
ppzuavimu_hmc5843.len_r = 6;
|
||||
ppzuavimu_hmc5843.len_w = 1;
|
||||
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
|
||||
i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843);
|
||||
#if PERIODIC_FREQUENCY > 60
|
||||
});
|
||||
#endif
|
||||
//RunOnceEvery(10,ppzuavimu_module_downlink_raw());
|
||||
}
|
||||
|
||||
void ppzuavimu_module_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);
|
||||
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
|
||||
}
|
||||
|
||||
void ppzuavimu_module_event( void )
|
||||
{
|
||||
int32_t x, y, z;
|
||||
|
||||
// If the itg3200 I2C transaction has succeeded: convert the data
|
||||
if (ppzuavimu_itg3200.status == I2CTransSuccess)
|
||||
{
|
||||
#define ITG_STA_DAT_OFFSET 3
|
||||
x = (int16_t) ((ppzuavimu_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[1+ITG_STA_DAT_OFFSET]);
|
||||
y = (int16_t) ((ppzuavimu_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[3+ITG_STA_DAT_OFFSET]);
|
||||
z = (int16_t) ((ppzuavimu_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[5+ITG_STA_DAT_OFFSET]);
|
||||
|
||||
// Is this is new data
|
||||
if (ppzuavimu_itg3200.buf[0] & 0x01)
|
||||
{
|
||||
//LED_ON(3);
|
||||
gyr_valid = TRUE;
|
||||
//LED_OFF(3);
|
||||
}
|
||||
else
|
||||
{
|
||||
}
|
||||
|
||||
// Signs depend on the way sensors are soldered on the board: so they are hardcoded
|
||||
#ifdef ASPIRIN_IMU
|
||||
RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z);
|
||||
#else // PPZIMU
|
||||
RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z);
|
||||
#endif
|
||||
|
||||
ppzuavimu_itg3200.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data
|
||||
}
|
||||
|
||||
// If the adxl345 I2C transaction has succeeded: convert the data
|
||||
if (ppzuavimu_adxl345.status == I2CTransSuccess)
|
||||
{
|
||||
x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
|
||||
y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
|
||||
z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
|
||||
|
||||
#ifdef ASPIRIN_IMU
|
||||
VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z);
|
||||
#else // PPZIMU
|
||||
VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z);
|
||||
#endif
|
||||
|
||||
acc_valid = TRUE;
|
||||
ppzuavimu_adxl345.status = I2CTransDone;
|
||||
}
|
||||
|
||||
// If the hmc5843 I2C transaction has succeeded: convert the data
|
||||
if (ppzuavimu_hmc5843.status == I2CTransSuccess)
|
||||
{
|
||||
x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]);
|
||||
y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
|
||||
z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
|
||||
|
||||
#ifdef ASPIRIN_IMU
|
||||
VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z);
|
||||
#else // PPZIMU
|
||||
VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z);
|
||||
#endif
|
||||
|
||||
mag_valid = TRUE;
|
||||
ppzuavimu_hmc5843.status = I2CTransDone;
|
||||
}
|
||||
}
|
||||
@@ -1,57 +0,0 @@
|
||||
/*
|
||||
* 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 PPZUAVIMU_H
|
||||
#define PPZUAVIMU_H
|
||||
|
||||
#include "std.h"
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
extern volatile bool_t gyr_valid;
|
||||
extern volatile bool_t acc_valid;
|
||||
extern volatile bool_t mag_valid;
|
||||
|
||||
/* must be defined in order to be IMU code: declared in imu.h
|
||||
extern void imu_impl_init(void);
|
||||
extern void imu_periodic(void);
|
||||
*/
|
||||
|
||||
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
|
||||
ppzuavimu_module_event(); \
|
||||
if (gyr_valid) { \
|
||||
gyr_valid = FALSE; \
|
||||
_gyro_handler(); \
|
||||
} \
|
||||
if (acc_valid) { \
|
||||
acc_valid = FALSE; \
|
||||
_accel_handler(); \
|
||||
} \
|
||||
if (mag_valid) { \
|
||||
mag_valid = FALSE; \
|
||||
_mag_handler(); \
|
||||
} \
|
||||
}
|
||||
|
||||
/* Own Extra Functions */
|
||||
extern void ppzuavimu_module_event( void );
|
||||
extern void ppzuavimu_module_downlink_raw( void );
|
||||
|
||||
|
||||
#endif // PPZUAVIMU_H
|
||||
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* 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 subsystems/imu/imu_ppzuav.h
|
||||
*
|
||||
* Driver for the PPZUAV IMU.
|
||||
*
|
||||
* 9DoM IMU with ITG-3200, ADXL345 and HMC5843, all via I2C.
|
||||
*/
|
||||
|
||||
#include "subsystems/imu.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
|
||||
INFO("Gyro output rate is 50Hz")
|
||||
# else
|
||||
# define IMU_PPZUAV_GYRO_SMPLRT_DIV 9
|
||||
INFO("Gyro output rate is 100Hz")
|
||||
# endif
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(IMU_PPZUAV_GYRO_SMPLRT_DIV)
|
||||
|
||||
|
||||
struct ImuPpzuav imu_ppzuav;
|
||||
|
||||
void imu_impl_init(void)
|
||||
{
|
||||
imu_ppzuav.accel_valid = FALSE;
|
||||
imu_ppzuav.gyro_valid = FALSE;
|
||||
imu_ppzuav.mag_valid = FALSE;
|
||||
|
||||
/* 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_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)
|
||||
{
|
||||
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_ppzuav.accel_valid = TRUE;
|
||||
}
|
||||
|
||||
/* 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_ppzuav.gyro_valid = TRUE;
|
||||
}
|
||||
|
||||
/* 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_ppzuav.mag_valid = TRUE;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* 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 subsystems/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 "subsystems/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 {
|
||||
volatile uint8_t accel_valid;
|
||||
volatile uint8_t gyro_valid;
|
||||
volatile uint8_t mag_valid;
|
||||
struct Adxl345_I2c acc_adxl;
|
||||
struct Itg3200 gyro_itg;
|
||||
struct Hmc58xx mag_hmc;
|
||||
};
|
||||
|
||||
extern struct ImuPpzuav imu_ppzuav;
|
||||
|
||||
extern void imu_ppzuav_event(void);
|
||||
|
||||
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
|
||||
imu_ppzuav_event();
|
||||
if (imu_ppzuav.gyro_valid) {
|
||||
imu_ppzuav.gyro_valid = FALSE;
|
||||
_gyro_handler();
|
||||
}
|
||||
if (imu_ppzuav.accel_valid) {
|
||||
imu_ppzuav.accel_valid = FALSE;
|
||||
_accel_handler();
|
||||
}
|
||||
if (imu_ppzuav.mag_valid) {
|
||||
imu_ppzuav.mag_valid = FALSE;
|
||||
_mag_handler();
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* IMU_PPZUAV_H */
|
||||
Reference in New Issue
Block a user