mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
add config file for new board umarim and a first driver for the
integrated imu
This commit is contained in:
Gautier Hattenberger
@@ -0,0 +1,15 @@
|
||||
|
||||
IMU_UMARIM_CFLAGS = -DUSE_IMU
|
||||
IMU_UMARIM_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_umarim.h\"
|
||||
|
||||
IMU_UMARIM_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
|
||||
$(SRC_SUBSYSTEMS)/imu/imu_umarim.c
|
||||
|
||||
|
||||
IMU_UMARIM_CFLAGS += -DUSE_I2C
|
||||
IMU_UMARIM_CFLAGS += -DUSE_I2C1
|
||||
IMU_UMARIM_CFLAGS += -DIMU_UMARIM_I2C_DEVICE=i2c1
|
||||
|
||||
ap.CFLAGS += $(IMU_UMARIM_CFLAGS)
|
||||
ap.srcs += $(IMU_UMARIM_SRCS)
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
#
|
||||
# unami_1.0.makefile
|
||||
#
|
||||
# prototype for Umarim board
|
||||
#
|
||||
|
||||
ARCH=lpc21
|
||||
|
||||
BOARD=umarim
|
||||
BOARD_VERSION=1.0
|
||||
|
||||
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
|
||||
|
||||
ifndef FLASH_MODE
|
||||
FLASH_MODE = IAP
|
||||
endif
|
||||
|
||||
|
||||
LPC21ISP_BAUD = 38400
|
||||
LPC21ISP_XTAL = 12000
|
||||
|
||||
|
||||
### default settings
|
||||
ifndef GPS_PORT
|
||||
GPS_PORT = UART0
|
||||
endif
|
||||
ifndef GPS_BAUD
|
||||
GPS_BAUD = B38400
|
||||
endif
|
||||
GPS_LED = 2
|
||||
|
||||
ifndef MODEM_PORT
|
||||
MODEM_PORT = UART1
|
||||
endif
|
||||
ifndef MODEM_BAUD
|
||||
MODEM_BAUD = B57600
|
||||
endif
|
||||
|
||||
ADC_GENERIC_NB_SAMPLES = 16
|
||||
|
||||
# All targets on the Umarim board run on the same processor achitecture
|
||||
$(TARGET).ARCHDIR = $(ARCH)
|
||||
|
||||
ACTUATORS = actuators_4017
|
||||
@@ -0,0 +1,68 @@
|
||||
#ifndef CONFIG_UMARIM_V1_0_H
|
||||
#define CONFIG_UMARIM_V1_0_H
|
||||
|
||||
/* Master oscillator freq. */
|
||||
#define FOSC (12000000)
|
||||
|
||||
/* PLL multiplier */
|
||||
#define PLL_MUL (5)
|
||||
|
||||
/* CPU clock freq. */
|
||||
#define CCLK (FOSC * PLL_MUL)
|
||||
|
||||
/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2 */
|
||||
#define PBSD_BITS 0x02
|
||||
#define PBSD_VAL 2
|
||||
|
||||
/* Peripheral bus clock freq. */
|
||||
#define PCLK (CCLK / PBSD_VAL)
|
||||
|
||||
/* Onboard LEDs */
|
||||
#define LED_1_BANK 1
|
||||
#define LED_1_PIN 25
|
||||
|
||||
#define LED_2_BANK 1
|
||||
#define LED_2_PIN 24
|
||||
|
||||
/* P0.5 aka MAT0.1 */
|
||||
#define SERVO_CLOCK_PIN 5
|
||||
#define SERVO_CLOCK_PINSEL PINSEL0
|
||||
#define SERVO_CLOCK_PINSEL_VAL 0x02
|
||||
#define SERVO_CLOCK_PINSEL_BIT 10
|
||||
/* p1.20 */
|
||||
#define SERVO_RESET_PIN 20
|
||||
|
||||
/* PPM : rc rx on P0.28 ( CAP0.2 ) */
|
||||
#define PPM_PINSEL PINSEL1
|
||||
#define PPM_PINSEL_VAL 0x02
|
||||
#define PPM_PINSEL_BIT 24
|
||||
#define PPM_CRI TIR_CR2I
|
||||
#define PPM_CCR_CRF TCCR_CR2_F
|
||||
#define PPM_CCR_CRR TCCR_CR2_R
|
||||
#define PPM_CCR_CRI TCCR_CR2_I
|
||||
#define PPM_CR T0CR2
|
||||
|
||||
/* ADC */
|
||||
|
||||
/* battery */
|
||||
#define ADC_CHANNEL_VSUPPLY AdcBank0(2)
|
||||
#ifndef USE_AD0
|
||||
#define USE_AD0
|
||||
#endif
|
||||
#define USE_AD0_2
|
||||
|
||||
#define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
|
||||
|
||||
/* SPI (SSP) */
|
||||
#define SPI_SELECT_SLAVE0_PORT 0
|
||||
#define SPI_SELECT_SLAVE0_PIN 20
|
||||
|
||||
#define SPI1_DRDY_PINSEL PINSEL1
|
||||
#define SPI1_DRDY_PINSEL_BIT 0
|
||||
#define SPI1_DRDY_PINSEL_VAL 1
|
||||
#define SPI1_DRDY_EINT 0
|
||||
#define SPI1_DRDY_VIC_IT VIC_EINT0
|
||||
|
||||
#define BOARD_HAS_BARO
|
||||
|
||||
#endif /* CONFIG_UMARIM_V1_0_H */
|
||||
@@ -1,9 +1,11 @@
|
||||
#ifndef ADXL345_H
|
||||
#define ADXL345_H
|
||||
|
||||
/* default I2C address */
|
||||
#define ADXL345_ADDR 0xA6
|
||||
#define ADXL345_ADDR_ALT 0x3A
|
||||
|
||||
/* Registers */
|
||||
#define ADXL345_REG_BW_RATE 0x2C
|
||||
#define ADXL345_REG_POWER_CTL 0x2D
|
||||
#define ADXL345_REG_INT_ENABLE 0x2E
|
||||
|
||||
@@ -2,7 +2,8 @@
|
||||
#define ITG3200
|
||||
|
||||
/* default I2C address */
|
||||
#define ITG3200_ADDR 0xD0
|
||||
#define ITG3200_ADDR 0xD0
|
||||
#define ITG3200_ADDR_ALT 0xD1
|
||||
|
||||
/* Registers */
|
||||
#define ITG3200_REG_WHO_AM_I 0X00
|
||||
|
||||
@@ -0,0 +1,214 @@
|
||||
/*
|
||||
* Copyright (C) 2011 Gautier Hattenberger
|
||||
* Derived from Aspirin and ppzuavimu drivers
|
||||
*
|
||||
* 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 "subsystems/imu/imu_umarim.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
#include "led.h"
|
||||
|
||||
// Downlink
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "messages.h"
|
||||
#include "downlink.h"
|
||||
|
||||
#ifndef DOWNLINK_DEVICE
|
||||
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
|
||||
#endif
|
||||
|
||||
|
||||
// Peripherials
|
||||
#include "peripherals/itg3199.h"
|
||||
#include "peripherals/hmc5843.h"
|
||||
|
||||
// Results
|
||||
volatile bool_t gyr_valid;
|
||||
volatile bool_t acc_valid;
|
||||
|
||||
// Communication
|
||||
struct i2c_transaction imu_umarim_itg3200;
|
||||
struct i2c_transaction imu_umarim_adxl345;
|
||||
|
||||
#ifndef PERIODIC_FREQUENCY
|
||||
#define PERIODIC_FREQUENCY 60
|
||||
#endif
|
||||
|
||||
void imu_impl_init(void)
|
||||
{
|
||||
/////////////////////////////////////////////////////////////////////
|
||||
// ITG3200
|
||||
imu_umarim_itg3200.type = I2CTransTx;
|
||||
imu_umarim_itg3200.slave_addr = ITG3200_ADDR_ALT;
|
||||
imu_umarim_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 */
|
||||
imu_umarim_itg3200.buf[1] = (0x03<<3) | (0x04<<0);
|
||||
# warning ITG3200 read at 50Hz
|
||||
#else
|
||||
# if PERIODIC_FREQUENCY == 120
|
||||
# warning ITG3200 read at 100Hz
|
||||
/* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */
|
||||
imu_umarim_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
|
||||
imu_umarim_itg3200.len_w = 2;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200);
|
||||
while(imu_umarim_itg3200.status == I2CTransPending);
|
||||
|
||||
/* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */
|
||||
imu_umarim_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV;
|
||||
#if PERIODIC_FREQUENCY == 60
|
||||
imu_umarim_itg3200.buf[1] = 19; // 50Hz
|
||||
#else
|
||||
imu_umarim_itg3200.buf[1] = 9; // 100Hz
|
||||
#endif
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200);
|
||||
while(imu_umarim_itg3200.status == I2CTransPending);
|
||||
|
||||
/* switch to gyroX clock */
|
||||
imu_umarim_itg3200.buf[0] = ITG3200_REG_PWR_MGM;
|
||||
imu_umarim_itg3200.buf[1] = 0x01;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200);
|
||||
while(imu_umarim_itg3200.status == I2CTransPending);
|
||||
|
||||
/* no interrupts for now, but set data ready interrupt to enable reading status bits */
|
||||
imu_umarim_itg3200.buf[0] = ITG3200_REG_INT_CFG;
|
||||
imu_umarim_itg3200.buf[1] = 0x00;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200);
|
||||
while(imu_umarim_itg3200.status == I2CTransPending);
|
||||
|
||||
/////////////////////////////////////////////////////////////////////
|
||||
// ADXL345
|
||||
|
||||
/* set data rate to 100Hz */
|
||||
imu_umarim_adxl345.slave_addr = ADXL345_ADDR_ALT;
|
||||
imu_umarim_adxl345.type = I2CTransTx;
|
||||
imu_umarim_adxl345.buf[0] = ADXL345_REG_BW_RATE;
|
||||
#if PERIODIC_FREQUENCY == 60
|
||||
imu_umarim_adxl345.buf[1] = 0x09; // normal power and 50Hz sampling, 50Hz BW
|
||||
#else
|
||||
imu_umarim_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW
|
||||
#endif
|
||||
imu_umarim_adxl345.len_w = 2;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_adxl345);
|
||||
while(imu_umarim_adxl345.status == I2CTransPending);
|
||||
|
||||
/* switch to measurement mode */
|
||||
imu_umarim_adxl345.type = I2CTransTx;
|
||||
imu_umarim_adxl345.buf[0] = ADXL345_REG_POWER_CTL;
|
||||
imu_umarim_adxl345.buf[1] = 1<<3;
|
||||
imu_umarim_adxl345.len_w = 2;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_adxl345);
|
||||
while(imu_umarim_adxl345.status == I2CTransPending);
|
||||
|
||||
/* Set range to 16g but keeping full resolution of 3.9 mV/g */
|
||||
imu_umarim_adxl345.type = I2CTransTx;
|
||||
imu_umarim_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
|
||||
imu_umarim_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
|
||||
imu_umarim_adxl345.len_w = 2;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_adxl345);
|
||||
while(imu_umarim_adxl345.status == I2CTransPending);
|
||||
|
||||
}
|
||||
|
||||
void imu_periodic( void )
|
||||
{
|
||||
// Start reading the latest gyroscope data
|
||||
if (imu_umarim_itg3200.status = I2CTransDone) {
|
||||
imu_umarim_itg3200.type = I2CTransTxRx;
|
||||
imu_umarim_itg3200.len_r = 9;
|
||||
imu_umarim_itg3200.len_w = 1;
|
||||
imu_umarim_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE, &imu_umarim_itg3200);
|
||||
}
|
||||
|
||||
// Start reading the latest accelerometer data
|
||||
if (imu_umarim_adxl345.status = I2CTransDone) {
|
||||
imu_umarim_adxl345.type = I2CTransTxRx;
|
||||
imu_umarim_adxl345.len_r = 6;
|
||||
imu_umarim_adxl345.len_w = 1;
|
||||
imu_umarim_adxl345.buf[0] = ADXL345_REG_DATA_X0;
|
||||
i2c_submit(&IMU_UMARIM_I2C_DEVICE, &imu_umarim_adxl345);
|
||||
}
|
||||
|
||||
//RunOnceEvery(10,imu_umarim_module_downlink_raw());
|
||||
}
|
||||
|
||||
void imu_umarim_module_downlink_raw( void )
|
||||
{
|
||||
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
|
||||
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
|
||||
}
|
||||
|
||||
void imu_umarim_module_event( void )
|
||||
{
|
||||
int32_t x, y, z;
|
||||
|
||||
// If the itg3200 I2C transaction has succeeded: convert the data
|
||||
if (imu_umarim_itg3200.status == I2CTransSuccess) {
|
||||
#define ITG_STA_DAT_OFFSET 3
|
||||
x = (int16_t) ((imu_umarim_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | imu_umarim_itg3200.buf[1+ITG_STA_DAT_OFFSET]);
|
||||
y = (int16_t) ((imu_umarim_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | imu_umarim_itg3200.buf[3+ITG_STA_DAT_OFFSET]);
|
||||
z = (int16_t) ((imu_umarim_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | imu_umarim_itg3200.buf[5+ITG_STA_DAT_OFFSET]);
|
||||
|
||||
// Is this is new data
|
||||
if (imu_umarim_itg3200.buf[0] & 0x01) {
|
||||
gyr_valid = TRUE;
|
||||
}
|
||||
|
||||
RATES_ASSIGN(imu.gyro_unscaled,
|
||||
(IMU_GYRO_P_SIGN * x),
|
||||
(IMU_GYRO_Q_SIGN * y),
|
||||
(IMU_GYRO_R_SIGN * z));
|
||||
|
||||
imu_umarim_itg3200.status = I2CTransDone;
|
||||
}
|
||||
// Reset status if transaction fails
|
||||
if (imu_umarim_itg3200.status = I2CTransFail) {
|
||||
imu_umarim_itg3200.status = I2CTransDone;
|
||||
}
|
||||
|
||||
|
||||
// If the adxl345 I2C transaction has succeeded: convert the data
|
||||
if (imu_umarim_adxl345.status == I2CTransSuccess)
|
||||
{
|
||||
x = (int16_t) ((imu_umarim_adxl345.buf[1] << 8) | imu_umarim_adxl345.buf[0]);
|
||||
y = (int16_t) ((imu_umarim_adxl345.buf[3] << 8) | imu_umarim_adxl345.buf[2]);
|
||||
z = (int16_t) ((imu_umarim_adxl345.buf[5] << 8) | imu_umarim_adxl345.buf[4]);
|
||||
|
||||
VECT3_ASSIGN(imu.accel_unscaled,
|
||||
(IMU_ACCEL_X_SIGN * x),
|
||||
(IMU_ACCEL_Y_SIGN * y),
|
||||
(IMU_ACCEL_Z_SIGN * z));
|
||||
|
||||
acc_valid = TRUE;
|
||||
imu_umarim_adxl345.status = I2CTransDone;
|
||||
}
|
||||
// Reset status if transaction fails
|
||||
if (imu_umarim_adxl345.status = I2CTransFail) {
|
||||
imu_umarim_adxl345.status = I2CTransDone;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Copyright (C) 2011 Gautier Hattenberger
|
||||
* Derived from Aspirin and ppzuavimu drivers
|
||||
*
|
||||
* 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 IMU_UMARIM_H
|
||||
#define IMU_UMARIM_H
|
||||
|
||||
#include "std.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
// Default configuration
|
||||
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
|
||||
#define IMU_GYRO_P_SIGN 1
|
||||
#define IMU_GYRO_Q_SIGN 1
|
||||
#define IMU_GYRO_R_SIGN 1
|
||||
#endif
|
||||
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
|
||||
#define IMU_ACCEL_X_SIGN 1
|
||||
#define IMU_ACCEL_Y_SIGN 1
|
||||
#define IMU_ACCEL_Z_SIGN 1
|
||||
#endif
|
||||
|
||||
extern volatile bool_t gyr_valid;
|
||||
extern volatile bool_t acc_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) { \
|
||||
imu_umarim_event(); \
|
||||
if (gyr_valid) { \
|
||||
gyr_valid = FALSE; \
|
||||
_gyro_handler(); \
|
||||
} \
|
||||
if (acc_valid) { \
|
||||
acc_valid = FALSE; \
|
||||
_accel_handler(); \
|
||||
} \
|
||||
}
|
||||
|
||||
/* Own Extra Functions */
|
||||
extern void imu_umarim_event( void );
|
||||
|
||||
#endif // PPZUAVIMU_H
|
||||
Reference in New Issue
Block a user