mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Add some drivers to the device specific library for now until the exti problems
are fixed
This commit is contained in:
parent
2cc0b301ce
commit
9607da9c62
416
flight/PiOS/STM32F4xx/pios_bma180.c
Normal file
416
flight/PiOS/STM32F4xx/pios_bma180.c
Normal file
@ -0,0 +1,416 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_BMA180 BMA180 Functions
|
||||
* @brief Deals with the hardware interface to the BMA180 3-axis accelerometer
|
||||
* @{
|
||||
*
|
||||
* @file pios_bma180.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief PiOS BMA180 digital accelerometer driver.
|
||||
* - Driver for the BMA180 digital accelerometer on the SPI bus.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program 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 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
#include "fifo_buffer.h"
|
||||
|
||||
static uint32_t PIOS_SPI_ACCEL;
|
||||
|
||||
static int32_t PIOS_BMA180_GetReg(uint8_t reg);
|
||||
static int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data);
|
||||
static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw);
|
||||
static int32_t PIOS_BMA180_SetRange(enum bma180_range range);
|
||||
static int32_t PIOS_BMA180_Config();
|
||||
static int32_t PIOS_BMA180_EnableIrq();
|
||||
static void PIOS_BMA180_IRQHandler(void);
|
||||
|
||||
#define PIOS_BMA180_MAX_DOWNSAMPLE 10
|
||||
static int16_t pios_bma180_buffer[PIOS_BMA180_MAX_DOWNSAMPLE * sizeof(struct pios_bma180_data)];
|
||||
static t_fifo_buffer pios_bma180_fifo;
|
||||
|
||||
static const struct pios_bma180_cfg * dev_cfg;
|
||||
static enum bma180_range range;
|
||||
|
||||
#define GRAV 9.81
|
||||
/**
|
||||
* @brief Initialize with good default settings
|
||||
*/
|
||||
int32_t PIOS_BMA180_Init(const struct pios_bma180_cfg * cfg)
|
||||
{
|
||||
dev_cfg = cfg; // store config before enabling interrupt
|
||||
|
||||
fifoBuf_init(&pios_bma180_fifo, (uint8_t *) pios_bma180_buffer, sizeof(pios_bma180_buffer));
|
||||
|
||||
/* Configure EOC pin as input floating */
|
||||
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
|
||||
|
||||
/* Configure the End Of Conversion (EOC) interrupt */
|
||||
SYSCFG_EXTILineConfig(cfg->eoc_exti.port_source, cfg->eoc_exti.pin_source);
|
||||
EXTI_Init(&cfg->eoc_exti.init);
|
||||
|
||||
/* Enable and set EOC EXTI Interrupt to the lowest priority */
|
||||
NVIC_Init(&cfg->eoc_irq.init);
|
||||
|
||||
if(PIOS_BMA180_Config() < 0)
|
||||
return -1;
|
||||
PIOS_DELAY_WaituS(50);
|
||||
PIOS_BMA180_EnableIrq();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Claim the SPI bus for the accel communications and select this chip
|
||||
* @return 0 if successful, -1 if unable to claim bus
|
||||
*/
|
||||
int32_t PIOS_BMA180_ClaimBus()
|
||||
{
|
||||
if(PIOS_SPI_ClaimBus(PIOS_SPI_ACCEL) != 0)
|
||||
return -1;
|
||||
PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0,0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Release the SPI bus for the accel communications and end the transaction
|
||||
* @return 0 if successful
|
||||
*/
|
||||
int32_t PIOS_BMA180_ReleaseBus()
|
||||
{
|
||||
PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0,1);
|
||||
return PIOS_SPI_ReleaseBus(PIOS_SPI_ACCEL);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read a register from BMA180
|
||||
* @returns The register value or -1 if failure to get bus
|
||||
* @param reg[in] Register address to be read
|
||||
*/
|
||||
int32_t PIOS_BMA180_GetReg(uint8_t reg)
|
||||
{
|
||||
uint8_t data;
|
||||
|
||||
if(PIOS_BMA180_ClaimBus() != 0)
|
||||
return -1;
|
||||
|
||||
PIOS_SPI_TransferByte(PIOS_SPI_ACCEL,(0x80 | reg) ); // request byte
|
||||
data = PIOS_SPI_TransferByte(PIOS_SPI_ACCEL,0 ); // receive response
|
||||
|
||||
PIOS_BMA180_ReleaseBus();
|
||||
return data;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Write a BMA180 register. EEPROM must be unlocked before calling this function.
|
||||
* @return none
|
||||
* @param reg[in] address of register to be written
|
||||
* @param data[in] data that is to be written to register
|
||||
*/
|
||||
int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data)
|
||||
{
|
||||
if(PIOS_BMA180_ClaimBus() != 0)
|
||||
return -1;
|
||||
|
||||
PIOS_SPI_TransferByte(PIOS_SPI_ACCEL, 0x7f & reg);
|
||||
PIOS_SPI_TransferByte(PIOS_SPI_ACCEL, data);
|
||||
|
||||
PIOS_BMA180_ReleaseBus();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static int32_t PIOS_BMA180_EnableEeprom() {
|
||||
// Enable EEPROM writing
|
||||
int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0);
|
||||
if(byte < 0)
|
||||
return -1;
|
||||
byte |= 0x10; // Set bit 4
|
||||
if(PIOS_BMA180_SetReg(BMA_CTRREG0,(uint8_t) byte) < 0) // Have to set ee_w to
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int32_t PIOS_BMA180_DisableEeprom() {
|
||||
// Enable EEPROM writing
|
||||
int32_t byte = PIOS_BMA180_GetReg(BMA_CTRREG0);
|
||||
if(byte < 0)
|
||||
return -1;
|
||||
byte |= 0x10; // Set bit 4
|
||||
if(PIOS_BMA180_SetReg(BMA_CTRREG0,(uint8_t) byte) < 0) // Have to set ee_w to
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the default register settings
|
||||
* @return 0 if successful, -1 if not
|
||||
*/
|
||||
static int32_t PIOS_BMA180_Config()
|
||||
{
|
||||
/*
|
||||
0x35 = 0x81 //smp-skip = 1 for less interrupts
|
||||
0x33 = 0x81 //shadow-dis = 1, update MSB and LSB synchronously
|
||||
0x27 = 0x01 //dis-i2c
|
||||
0x21 = 0x02 //new_data_int = 1
|
||||
*/
|
||||
|
||||
PIOS_DELAY_WaituS(20);
|
||||
|
||||
if(PIOS_BMA180_EnableEeprom() < 0)
|
||||
return -1;
|
||||
if(PIOS_BMA180_SetReg(BMA_RESET, BMA_RESET_CODE) < 0)
|
||||
return -1;
|
||||
if(PIOS_BMA180_SetReg(BMA_OFFSET_LSB1, 0x81) < 0)
|
||||
return -1;
|
||||
if(PIOS_BMA180_SetReg(BMA_GAIN_Y, 0x81) < 0)
|
||||
return -1;
|
||||
if(PIOS_BMA180_SetReg(BMA_CTRREG3, 0xFF) < 0)
|
||||
return -1;
|
||||
PIOS_BMA180_SelectBW(BMA_BW_600HZ);
|
||||
PIOS_BMA180_SetRange(BMA_RANGE_8G);
|
||||
|
||||
if(PIOS_BMA180_DisableEeprom() < 0)
|
||||
return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Select the bandwidth the digital filter pass allows.
|
||||
* @return 0 if successful, -1 if not
|
||||
* @param rate[in] Bandwidth setting to be used
|
||||
*
|
||||
* EEPROM must be write-enabled before calling this function.
|
||||
*/
|
||||
static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw)
|
||||
{
|
||||
uint8_t reg;
|
||||
reg = PIOS_BMA180_GetReg(BMA_BW_ADDR);
|
||||
reg = (reg & ~BMA_BW_MASK) | ((bw << BMA_BW_SHIFT) & BMA_BW_MASK);
|
||||
return PIOS_BMA180_SetReg(BMA_BW_ADDR, reg);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Select the full scale acceleration range.
|
||||
* @return 0 if successful, -1 if not
|
||||
* @param rate[in] Range setting to be used
|
||||
*
|
||||
*/
|
||||
static int32_t PIOS_BMA180_SetRange(enum bma180_range new_range)
|
||||
{
|
||||
uint8_t reg;
|
||||
range = new_range;
|
||||
reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR);
|
||||
reg = (reg & ~BMA_RANGE_MASK) | ((range << BMA_RANGE_SHIFT) & BMA_RANGE_MASK);
|
||||
return PIOS_BMA180_SetReg(BMA_RANGE_ADDR, reg);
|
||||
}
|
||||
|
||||
static int32_t PIOS_BMA180_EnableIrq()
|
||||
{
|
||||
|
||||
if(PIOS_BMA180_EnableEeprom() < 0)
|
||||
return -1;
|
||||
|
||||
if(PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT) < 0)
|
||||
return -1;
|
||||
|
||||
if(PIOS_BMA180_DisableEeprom() < 0)
|
||||
return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Connect to the correct SPI bus
|
||||
*/
|
||||
void PIOS_BMA180_Attach(uint32_t spi_id)
|
||||
{
|
||||
PIOS_SPI_ACCEL = spi_id;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read a single set of values from the x y z channels
|
||||
* @param[out] data Int16 array of (x,y,z) sensor values
|
||||
* @returns 0 if successful
|
||||
* @retval -1 unable to claim bus
|
||||
* @retval -2 unable to transfer data
|
||||
*/
|
||||
int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data)
|
||||
{
|
||||
// To save memory use same buffer for in and out but offset by
|
||||
// a byte
|
||||
uint8_t buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
|
||||
uint8_t rec[7] = {0,0,0,0,0,0};
|
||||
|
||||
if(PIOS_BMA180_ClaimBus() != 0)
|
||||
return -1;
|
||||
if(PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],7,NULL) != 0)
|
||||
return -2;
|
||||
PIOS_BMA180_ReleaseBus();
|
||||
|
||||
// | MSB | LSB | 0 | new_data |
|
||||
data->x = ((rec[2] << 8) | rec[1]);
|
||||
data->y = ((rec[4] << 8) | rec[3]);
|
||||
data->z = ((rec[6] << 8) | rec[5]);
|
||||
data->x /= 4;
|
||||
data->y /= 4;
|
||||
data->z /= 4;
|
||||
|
||||
return 0; // return number of remaining entries
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the scale the BMA180 chip is using
|
||||
* @return Scale (m / s^2) / LSB
|
||||
*/
|
||||
float PIOS_BMA180_GetScale()
|
||||
{
|
||||
switch (range) {
|
||||
case BMA_RANGE_1G:
|
||||
return GRAV / 8192.0;
|
||||
case BMA_RANGE_1_5G:
|
||||
return GRAV / 5460.0;
|
||||
case BMA_RANGE_2G:
|
||||
return GRAV / 4096.0;
|
||||
case BMA_RANGE_3G:
|
||||
return GRAV / 2730.0;
|
||||
case BMA_RANGE_4G:
|
||||
return GRAV / 2048.0;
|
||||
case BMA_RANGE_8G:
|
||||
return GRAV / 1024.0;
|
||||
case BMA_RANGE_16G:
|
||||
return GRAV / 512.0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get data from fifo
|
||||
* @param [out] buffer pointer to a @ref pios_bma180_data structure to receive data
|
||||
* @return 0 for success, -1 for failure (no data available)
|
||||
*/
|
||||
int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer)
|
||||
{
|
||||
if(fifoBuf_getUsed(&pios_bma180_fifo) < sizeof(*buffer))
|
||||
return -1;
|
||||
|
||||
fifoBuf_getData(&pios_bma180_fifo, (uint8_t *) buffer, sizeof(*buffer));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Test SPI and chip functionality by reading chip ID register
|
||||
* @return 0 if success, -1 if failure.
|
||||
*
|
||||
*/
|
||||
int32_t PIOS_BMA180_Test()
|
||||
{
|
||||
// Read chip ID then version ID
|
||||
uint8_t buf[3] = {0x80 | BMA_CHIPID_ADDR, 0, 0};
|
||||
uint8_t rec[3] = {0,0, 0};
|
||||
int32_t retval;
|
||||
|
||||
if(PIOS_BMA180_ClaimBus() != 0)
|
||||
return -1;
|
||||
retval = PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL);
|
||||
PIOS_BMA180_ReleaseBus();
|
||||
|
||||
if(retval != 0)
|
||||
return -2;
|
||||
|
||||
struct pios_bma180_data data;
|
||||
if(PIOS_BMA180_ReadAccels(&data) != 0)
|
||||
return -3;
|
||||
|
||||
if(rec[1] != 0x3)
|
||||
return -4;
|
||||
|
||||
if(rec[2] < 0x12)
|
||||
return -5;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static uint8_t pios_bma180_dmabuf[8];
|
||||
/**
|
||||
* @brief Take data from BMA180 read and parse it into structure
|
||||
* Leaving this function here in case I want to go back to using
|
||||
* callback but at higher speed this it is faster to just transfer
|
||||
*/
|
||||
static inline void PIOS_BMA180_SPI_Callback()
|
||||
{
|
||||
// TODO: Make this conversion depend on configuration scale
|
||||
struct pios_bma180_data data;
|
||||
|
||||
// Don't release bus till data has copied
|
||||
PIOS_BMA180_ReleaseBus();
|
||||
|
||||
// Must not return before releasing bus
|
||||
if(fifoBuf_getFree(&pios_bma180_fifo) < sizeof(data))
|
||||
return;
|
||||
|
||||
// Bottom two bits indicate new data and are constant zeros. Don't right
|
||||
// shift because it drops sign bit
|
||||
data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]);
|
||||
data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]);
|
||||
data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]);
|
||||
data.x /= 4;
|
||||
data.y /= 4;
|
||||
data.z /= 4;
|
||||
data.temperature = pios_bma180_dmabuf[7];
|
||||
|
||||
fifoBuf_putData(&pios_bma180_fifo, (uint8_t *) &data, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief IRQ Handler
|
||||
*/
|
||||
const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
|
||||
static void PIOS_BMA180_IRQHandler(void)
|
||||
{
|
||||
// If we can't get the bus then just move on for efficiency
|
||||
if(PIOS_BMA180_ClaimBus() == 0)
|
||||
PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf,
|
||||
sizeof(pios_bma180_dmabuf), NULL);
|
||||
PIOS_BMA180_SPI_Callback();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* The physical IRQ handler
|
||||
* Soon this will be generic in pios_exti and the BMA180 will register
|
||||
* against it
|
||||
*/
|
||||
void EXTI4_IRQHandler(void)
|
||||
{
|
||||
if (EXTI_GetITStatus(dev_cfg->eoc_exti.init.EXTI_Line) != RESET) {
|
||||
PIOS_BMA180_IRQHandler();
|
||||
EXTI_ClearITPendingBit(dev_cfg->eoc_exti.init.EXTI_Line);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
321
flight/PiOS/STM32F4xx/pios_bmp085.c
Normal file
321
flight/PiOS/STM32F4xx/pios_bmp085.c
Normal file
@ -0,0 +1,321 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_BMP085 BMP085 Functions
|
||||
* @brief Hardware functions to deal with the altitude pressure sensor
|
||||
* @{
|
||||
*
|
||||
* @file pios_bmp085.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief BMP085 Pressure Sensor Routines
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************/
|
||||
/*
|
||||
* This program 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 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* Project Includes */
|
||||
// TODO: Clean this up. Getting around old constant.
|
||||
#define PIOS_BMP085_OVERSAMPLING oversampling
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#if defined(PIOS_INCLUDE_BMP085)
|
||||
#if !defined(PIOS_INCLUDE_EXTI)
|
||||
#error PIOS_EXTI Must be included in the project!
|
||||
#endif /* PIOS_INCLUDE_EXTI */
|
||||
|
||||
/* Glocal Variables */
|
||||
ConversionTypeTypeDef CurrentRead;
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
xSemaphoreHandle PIOS_BMP085_EOC;
|
||||
#else
|
||||
int32_t PIOS_BMP085_EOC;
|
||||
#endif
|
||||
|
||||
/* Local Variables */
|
||||
static BMP085CalibDataTypeDef CalibData;
|
||||
|
||||
/* Straight from the datasheet */
|
||||
static int32_t X1, X2, X3, B3, B5, B6, P;
|
||||
static uint32_t B4, B7;
|
||||
static volatile uint16_t RawTemperature;
|
||||
static volatile uint32_t RawPressure;
|
||||
static volatile uint32_t Pressure;
|
||||
static volatile uint16_t Temperature;
|
||||
|
||||
static int32_t PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len);
|
||||
static int32_t PIOS_BMP085_Write(uint8_t address, uint8_t buffer);
|
||||
|
||||
// Move into proper driver structure with cfg stored
|
||||
static uint32_t oversampling;
|
||||
static const struct pios_bmp085_cfg * dev_cfg;
|
||||
|
||||
/**
|
||||
* Initialise the BMP085 sensor
|
||||
*/
|
||||
void PIOS_BMP085_Init(const struct pios_bmp085_cfg * cfg)
|
||||
{
|
||||
PIOS_BMP085_EOC = 0;
|
||||
|
||||
oversampling = cfg->oversampling;
|
||||
dev_cfg = cfg; // Store cfg before enabling interrupt
|
||||
|
||||
/* Configure EOC pin as input floating */
|
||||
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
|
||||
|
||||
/* Configure the End Of Conversion (EOC) interrupt */
|
||||
SYSCFG_EXTILineConfig(cfg->eoc_exti.port_source, cfg->eoc_exti.pin_source);
|
||||
EXTI_Init(&cfg->eoc_exti.init);
|
||||
|
||||
/* Enable and set EOC EXTI Interrupt to the lowest priority */
|
||||
NVIC_Init(&cfg->eoc_irq.init);
|
||||
|
||||
/* Configure anothing GPIO pin pin as output to set address */
|
||||
GPIO_Init(cfg->xclr.gpio, &cfg->xclr.init);
|
||||
GPIO_SetBits(cfg->xclr.gpio,cfg->xclr.init.GPIO_Pin);
|
||||
|
||||
/* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */
|
||||
uint8_t Data[BMP085_CALIB_LEN];
|
||||
bool good_cal = false;
|
||||
while(!good_cal) {
|
||||
good_cal = (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) == 0);
|
||||
// Check none of the calibration is zero to ensure calibration is good
|
||||
for(uint8_t i = 0; i < BMP085_CALIB_LEN; i += 2)
|
||||
good_cal &= (Data[i] != 0) || (Data[i+1] != 0);
|
||||
}
|
||||
|
||||
|
||||
/* Parameters AC1-AC6 */
|
||||
CalibData.AC1 = (Data[0] << 8) | Data[1];
|
||||
CalibData.AC2 = (Data[2] << 8) | Data[3];
|
||||
CalibData.AC3 = (Data[4] << 8) | Data[5];
|
||||
CalibData.AC4 = (Data[6] << 8) | Data[7];
|
||||
CalibData.AC5 = (Data[8] << 8) | Data[9];
|
||||
CalibData.AC6 = (Data[10] << 8) | Data[11];
|
||||
|
||||
/* Parameters B1, B2 */
|
||||
CalibData.B1 = (Data[12] << 8) | Data[13];
|
||||
CalibData.B2 = (Data[14] << 8) | Data[15];
|
||||
|
||||
/* Parameters MB, MC, MD */
|
||||
CalibData.MB = (Data[16] << 8) | Data[17];
|
||||
CalibData.MC = (Data[18] << 8) | Data[19];
|
||||
CalibData.MD = (Data[20] << 8) | Data[21];
|
||||
}
|
||||
|
||||
/**
|
||||
* Start the ADC conversion
|
||||
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
|
||||
* \return 0 for success, -1 for failure (conversion completed and not read)
|
||||
*/
|
||||
int32_t PIOS_BMP085_StartADC(ConversionTypeTypeDef Type)
|
||||
{
|
||||
if(PIOS_BMP085_EOC)
|
||||
return -1;
|
||||
|
||||
/* Start the conversion */
|
||||
if (Type == TemperatureConv) {
|
||||
while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_TEMP_ADDR) != 0)
|
||||
continue;
|
||||
} else if (Type == PressureConv) {
|
||||
while (PIOS_BMP085_Write(BMP085_CTRL_ADDR, BMP085_PRES_ADDR) != 0)
|
||||
continue;
|
||||
}
|
||||
|
||||
CurrentRead = Type;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Read the ADC conversion value (once ADC conversion has completed)
|
||||
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
|
||||
* \return 0 if successfully read the ADC, -1 if failed
|
||||
*/
|
||||
int32_t PIOS_BMP085_ReadADC(void)
|
||||
{
|
||||
uint8_t Data[3];
|
||||
Data[0] = 0;
|
||||
Data[1] = 0;
|
||||
Data[2] = 0;
|
||||
|
||||
if(!PIOS_BMP085_EOC)
|
||||
return -1;
|
||||
|
||||
PIOS_BMP085_EOC = 0;
|
||||
|
||||
/* Read and store the 16bit result */
|
||||
if (CurrentRead == TemperatureConv) {
|
||||
/* Read the temperature conversion */
|
||||
if (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 2) != 0)
|
||||
return -1;
|
||||
|
||||
RawTemperature = ((Data[0] << 8) | Data[1]);
|
||||
|
||||
X1 = (RawTemperature - CalibData.AC6) * CalibData.AC5 >> 15;
|
||||
X2 = ((int32_t) CalibData.MC << 11) / (X1 + CalibData.MD);
|
||||
B5 = X1 + X2;
|
||||
Temperature = (B5 + 8) >> 4;
|
||||
} else {
|
||||
/* Read the pressure conversion */
|
||||
if (PIOS_BMP085_Read(BMP085_ADC_MSB, Data, 3) != 0)
|
||||
return -1;
|
||||
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]) >> (8 - oversampling);
|
||||
|
||||
B6 = B5 - 4000;
|
||||
X1 = (CalibData.B2 * (B6 * B6 >> 12)) >> 11;
|
||||
X2 = CalibData.AC2 * B6 >> 11;
|
||||
X3 = X1 + X2;
|
||||
B3 = ((((int32_t) CalibData.AC1 * 4 + X3) << oversampling) + 2) >> 2;
|
||||
X1 = CalibData.AC3 * B6 >> 13;
|
||||
X2 = (CalibData.B1 * (B6 * B6 >> 12)) >> 16;
|
||||
X3 = ((X1 + X2) + 2) >> 2;
|
||||
B4 = (CalibData.AC4 * (uint32_t) (X3 + 32768)) >> 15;
|
||||
B7 = ((uint32_t) RawPressure - B3) * (50000 >> oversampling);
|
||||
P = B7 < 0x80000000 ? (B7 * 2) / B4 : (B7 / B4) * 2;
|
||||
|
||||
X1 = (P >> 8) * (P >> 8);
|
||||
X1 = (X1 * 3038) >> 16;
|
||||
X2 = (-7357 * P) >> 16;
|
||||
Pressure = P + ((X1 + X2 + 3791) >> 4);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int16_t PIOS_BMP085_GetTemperature(void)
|
||||
{
|
||||
return Temperature;
|
||||
}
|
||||
|
||||
int32_t PIOS_BMP085_GetPressure(void)
|
||||
{
|
||||
return Pressure;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads one or more bytes into a buffer
|
||||
* \param[in] address BMP085 register address (depends on size)
|
||||
* \param[out] buffer destination buffer
|
||||
* \param[in] len number of bytes which should be read
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
*/
|
||||
int32_t PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len)
|
||||
{
|
||||
uint8_t addr_buffer[] = {
|
||||
address,
|
||||
};
|
||||
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = BMP085_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(addr_buffer),
|
||||
.buf = addr_buffer,
|
||||
}
|
||||
,
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = BMP085_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_READ,
|
||||
.len = len,
|
||||
.buf = buffer,
|
||||
}
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
}
|
||||
|
||||
/**
|
||||
* Writes one or more bytes to the BMP085
|
||||
* \param[in] address Register address
|
||||
* \param[in] buffer source buffer
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
*/
|
||||
int32_t PIOS_BMP085_Write(uint8_t address, uint8_t buffer)
|
||||
{
|
||||
uint8_t data[] = {
|
||||
address,
|
||||
buffer,
|
||||
};
|
||||
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = BMP085_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(data),
|
||||
.buf = data,
|
||||
}
|
||||
,
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Run self-test operation.
|
||||
* \return 0 if self-test succeed, -1 if failed
|
||||
*/
|
||||
int32_t PIOS_BMP085_Test()
|
||||
{
|
||||
// TODO: Is there a better way to test this than just checking that pressure/temperature has changed?
|
||||
int32_t cur_value = 0;
|
||||
|
||||
if(PIOS_BMP085_Read(BMP085_ADC_MSB, (uint8_t *) &cur_value, 1) != 0)
|
||||
return -1;
|
||||
return 0;
|
||||
|
||||
cur_value = Temperature;
|
||||
PIOS_BMP085_StartADC(TemperatureConv);
|
||||
PIOS_DELAY_WaitmS(5);
|
||||
PIOS_BMP085_ReadADC();
|
||||
if (cur_value == Temperature)
|
||||
return -1;
|
||||
|
||||
cur_value=Pressure;
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
PIOS_DELAY_WaitmS(26);
|
||||
PIOS_BMP085_ReadADC();
|
||||
if (cur_value == Pressure)
|
||||
return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle external line 2 interrupt requests
|
||||
*/
|
||||
void EXTI2_IRQHandler(void)
|
||||
{
|
||||
if (EXTI_GetITStatus(dev_cfg->eoc_exti.init.EXTI_Line) != RESET) {
|
||||
/* Read the ADC Value */
|
||||
PIOS_BMP085_EOC=1;
|
||||
/* Clear the EXTI line pending bit */
|
||||
EXTI_ClearITPendingBit(dev_cfg->eoc_exti.init.EXTI_Line);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
427
flight/PiOS/STM32F4xx/pios_hmc5883.c
Normal file
427
flight/PiOS/STM32F4xx/pios_hmc5883.c
Normal file
@ -0,0 +1,427 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_HMC5883 HMC5883 Functions
|
||||
* @brief Deals with the hardware interface to the magnetometers
|
||||
* @{
|
||||
*
|
||||
* @file pios_hmc5883.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief HMC5883 Magnetic Sensor Functions from AHRS
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
/*
|
||||
* This program 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 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* Project Includes */
|
||||
#include "pios.h"
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5883)
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Types */
|
||||
|
||||
/* Local Variables */
|
||||
volatile bool pios_hmc5883_data_ready;
|
||||
|
||||
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg);
|
||||
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len);
|
||||
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer);
|
||||
|
||||
static const struct pios_hmc5883_cfg * dev_cfg;
|
||||
|
||||
/**
|
||||
* @brief Initialize the HMC5883 magnetometer sensor.
|
||||
* @return none
|
||||
*/
|
||||
void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg * cfg)
|
||||
{
|
||||
dev_cfg = cfg; // store config before enabling interrupt
|
||||
|
||||
/* Configure EOC pin as input floating */
|
||||
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
|
||||
|
||||
/* Configure the End Of Conversion (EOC) interrupt */
|
||||
SYSCFG_EXTILineConfig(cfg->eoc_exti.port_source, cfg->eoc_exti.pin_source);
|
||||
EXTI_Init(&cfg->eoc_exti.init);
|
||||
|
||||
/* Enable and set EOC EXTI Interrupt to the lowest priority */
|
||||
NVIC_Init(&cfg->eoc_irq.init);
|
||||
|
||||
int32_t val = PIOS_HMC5883_Config(cfg);
|
||||
|
||||
PIOS_Assert(val == 0);
|
||||
|
||||
pios_hmc5883_data_ready = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize the HMC5883 magnetometer sensor
|
||||
* \return none
|
||||
* \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor.
|
||||
*
|
||||
* CTRL_REGA: Control Register A
|
||||
* Read Write
|
||||
* Default value: 0x10
|
||||
* 7:5 0 These bits must be cleared for correct operation.
|
||||
* 4:2 DO2-DO0: Data Output Rate Bits
|
||||
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
|
||||
* ------------------------------------------------------
|
||||
* 0 | 0 | 0 | 0.75
|
||||
* 0 | 0 | 1 | 1.5
|
||||
* 0 | 1 | 0 | 3
|
||||
* 0 | 1 | 1 | 7.5
|
||||
* 1 | 0 | 0 | 15 (default)
|
||||
* 1 | 0 | 1 | 30
|
||||
* 1 | 1 | 0 | 75
|
||||
* 1 | 1 | 1 | Not Used
|
||||
* 1:0 MS1-MS0: Measurement Configuration Bits
|
||||
* MS1 | MS0 | MODE
|
||||
* ------------------------------
|
||||
* 0 | 0 | Normal
|
||||
* 0 | 1 | Positive Bias
|
||||
* 1 | 0 | Negative Bias
|
||||
* 1 | 1 | Not Used
|
||||
*
|
||||
* CTRL_REGB: Control RegisterB
|
||||
* Read Write
|
||||
* Default value: 0x20
|
||||
* 7:5 GN2-GN0: Gain Configuration Bits.
|
||||
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
|
||||
* | | | Range[Ga] | [LSB/mGa] |
|
||||
* ------------------------------------------------------
|
||||
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF8000x07FF (-2048:2047)
|
||||
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF8000x07FF (-2048:2047)
|
||||
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF8000x07FF (-2048:2047)
|
||||
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF8000x07FF (-2048:2047)
|
||||
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF8000x07FF (-2048:2047)
|
||||
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF8000x07FF (-2048:2047)
|
||||
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF8000x07FF (-2048:2047)
|
||||
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF8000x07FF (-2048:2047)
|
||||
* |Not recommended|
|
||||
*
|
||||
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
|
||||
*
|
||||
* _MODE_REG: Mode Register
|
||||
* Read Write
|
||||
* Default value: 0x02
|
||||
* 7:2 0 These bits must be cleared for correct operation.
|
||||
* 1:0 MD1-MD0: Mode Select Bits
|
||||
* MS1 | MS0 | MODE
|
||||
* ------------------------------
|
||||
* 0 | 0 | Continuous-Conversion Mode.
|
||||
* 0 | 1 | Single-Conversion Mode
|
||||
* 1 | 0 | Negative Bias
|
||||
* 1 | 1 | Sleep Mode
|
||||
*/
|
||||
static uint8_t CTRLB = 0x00;
|
||||
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg)
|
||||
{
|
||||
uint8_t CTRLA = 0x00;
|
||||
uint8_t MODE = 0x00;
|
||||
CTRLB = 0;
|
||||
|
||||
CTRLA |= (uint8_t) (cfg->M_ODR | cfg->Meas_Conf);
|
||||
CTRLB |= (uint8_t) (cfg->Gain);
|
||||
MODE |= (uint8_t) (cfg->Mode);
|
||||
|
||||
// CRTL_REGA
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0)
|
||||
return -1;
|
||||
|
||||
// CRTL_REGB
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0)
|
||||
return -1;
|
||||
|
||||
// Mode register
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0)
|
||||
return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read current X, Z, Y values (in that order)
|
||||
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
|
||||
* \return 0 for success or -1 for failure
|
||||
*/
|
||||
int32_t PIOS_HMC5883_ReadMag(int16_t out[3])
|
||||
{
|
||||
pios_hmc5883_data_ready = false;
|
||||
uint8_t buffer[6];
|
||||
int32_t temp;
|
||||
int32_t sensitivity;
|
||||
|
||||
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
switch (CTRLB & 0xE0) {
|
||||
case 0x00:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga;
|
||||
break;
|
||||
case 0x20:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga;
|
||||
break;
|
||||
case 0x40:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga;
|
||||
break;
|
||||
case 0x60:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga;
|
||||
break;
|
||||
case 0x80:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga;
|
||||
break;
|
||||
case 0xA0:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga;
|
||||
break;
|
||||
case 0xC0:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga;
|
||||
break;
|
||||
case 0xE0:
|
||||
sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga;
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
temp = ((int16_t) ((uint16_t) buffer[2 * i] << 8)
|
||||
+ buffer[2 * i + 1]) * 1000 / sensitivity;
|
||||
out[i] = temp;
|
||||
}
|
||||
// Data reads out as X,Z,Y
|
||||
temp = out[2];
|
||||
out[2] = out[1];
|
||||
out[1] = temp;
|
||||
|
||||
// This should not be necessary but for some reason it is coming out of continuous conversion mode
|
||||
PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Read the identification bytes from the HMC5883 sensor
|
||||
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
|
||||
* \return 0 if successful, -1 if not
|
||||
*/
|
||||
uint8_t PIOS_HMC5883_ReadID(uint8_t out[4])
|
||||
{
|
||||
uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3);
|
||||
out[3] = '\0';
|
||||
return retval;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Tells whether new magnetometer readings are available
|
||||
* \return true if new data is available
|
||||
* \return false if new data is not available
|
||||
*/
|
||||
bool PIOS_HMC5883_NewDataAvailable(void)
|
||||
{
|
||||
return (pios_hmc5883_data_ready);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reads one or more bytes into a buffer
|
||||
* \param[in] address HMC5883 register address (depends on size)
|
||||
* \param[out] buffer destination buffer
|
||||
* \param[in] len number of bytes which should be read
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
* \return -2 if unable to claim i2c device
|
||||
*/
|
||||
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t * buffer, uint8_t len)
|
||||
{
|
||||
uint8_t addr_buffer[] = {
|
||||
address,
|
||||
};
|
||||
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = PIOS_HMC5883_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(addr_buffer),
|
||||
.buf = addr_buffer,
|
||||
}
|
||||
,
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = PIOS_HMC5883_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_READ,
|
||||
.len = len,
|
||||
.buf = buffer,
|
||||
}
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Writes one or more bytes to the HMC5883
|
||||
* \param[in] address Register address
|
||||
* \param[in] buffer source buffer
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
* \return -2 if unable to claim i2c device
|
||||
*/
|
||||
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer)
|
||||
{
|
||||
uint8_t data[] = {
|
||||
address,
|
||||
buffer,
|
||||
};
|
||||
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = PIOS_HMC5883_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(data),
|
||||
.buf = data,
|
||||
}
|
||||
,
|
||||
};
|
||||
;
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Run self-test operation. Do not call this during operational use!!
|
||||
* \return 0 if success, -1 if test failed
|
||||
*/
|
||||
int32_t PIOS_HMC5883_Test(void)
|
||||
{
|
||||
int32_t failed = 0;
|
||||
uint8_t registers[3] = {0,0,0};
|
||||
uint8_t status;
|
||||
uint8_t ctrl_a_read;
|
||||
uint8_t ctrl_b_read;
|
||||
uint8_t mode_read;
|
||||
int16_t values[3];
|
||||
|
||||
|
||||
|
||||
/* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */
|
||||
char id[4];
|
||||
PIOS_HMC5883_ReadID((uint8_t *)id);
|
||||
if((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) // Expect H43
|
||||
return -1;
|
||||
|
||||
/* Backup existing configuration */
|
||||
if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A,registers,3) != 0)
|
||||
return -1;
|
||||
|
||||
/* Stop the device and read out last value */
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0)
|
||||
return -1;
|
||||
if( PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status,1) != 0)
|
||||
return -1;
|
||||
if (PIOS_HMC5883_ReadMag(values) != 0)
|
||||
return -1;
|
||||
|
||||
/*
|
||||
* Put HMC5883 into self test mode
|
||||
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
|
||||
* and then placing the mode register into single-measurement mode. This causes the HMC5883
|
||||
* to create an artificial magnetic field of ~1.1 Gauss.
|
||||
*
|
||||
* If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB
|
||||
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
|
||||
*
|
||||
* Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode.
|
||||
*/
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0)
|
||||
return -1;
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0)
|
||||
return -1;
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0)
|
||||
return -1;
|
||||
|
||||
/* Must wait for value to be updated */
|
||||
PIOS_DELAY_WaitmS(200);
|
||||
|
||||
if (PIOS_HMC5883_ReadMag(values) != 0)
|
||||
return -1;
|
||||
|
||||
/*
|
||||
if(abs(values[0] - 766) > 20)
|
||||
failed |= 1;
|
||||
if(abs(values[1] - 766) > 20)
|
||||
failed |= 1;
|
||||
if(abs(values[2] - 713) > 20)
|
||||
failed |= 1;
|
||||
*/
|
||||
|
||||
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read,1);
|
||||
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read,1);
|
||||
PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read,1);
|
||||
PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status,1);
|
||||
|
||||
|
||||
/* Restore backup configuration */
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0)
|
||||
return -1;
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0)
|
||||
return -1;
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0)
|
||||
return -1;
|
||||
|
||||
return failed;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief IRQ Handler
|
||||
*/
|
||||
void PIOS_HMC5883_IRQHandler(void)
|
||||
{
|
||||
pios_hmc5883_data_ready = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* The physical IRQ handler
|
||||
* Soon this will be generic in pios_exti and the BMA180 will register
|
||||
* against it. Right now this is crap!
|
||||
*/
|
||||
void EXTI9_5_IRQHandler(void)
|
||||
{
|
||||
if (EXTI_GetITStatus(dev_cfg->eoc_exti.init.EXTI_Line) != RESET) {
|
||||
PIOS_HMC5883_IRQHandler();
|
||||
EXTI_ClearITPendingBit(dev_cfg->eoc_exti.init.EXTI_Line);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif /* PIOS_INCLUDE_HMC5883 */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
133
flight/PiOS/STM32F4xx/pios_iap.c
Normal file
133
flight/PiOS/STM32F4xx/pios_iap.c
Normal file
@ -0,0 +1,133 @@
|
||||
/*!
|
||||
* @File iap.c
|
||||
* @Brief
|
||||
*
|
||||
* Created on: Sep 6, 2010
|
||||
* Author: joe
|
||||
*/
|
||||
|
||||
|
||||
/****************************************************************************************
|
||||
* Header files
|
||||
****************************************************************************************/
|
||||
#include <pios.h>
|
||||
|
||||
/****************************************************************************************
|
||||
* Private Definitions/Macros
|
||||
****************************************************************************************/
|
||||
|
||||
/* these definitions reside here for protection and privacy. */
|
||||
#define IAP_MAGIC_WORD_1 0x1122
|
||||
#define IAP_MAGIC_WORD_2 0xAA55
|
||||
|
||||
#define IAP_REQLOC_1 BKP_DR1
|
||||
#define IAP_CRCLOC_LOW BKP_DR2
|
||||
#define IAP_CRCLOC_UPPER BKP_DR3
|
||||
#define IAP_PORTLOC BKP_DR4
|
||||
#define IAP_REQLOC_2 BKP_RR5
|
||||
|
||||
#define IAP_UPLOAD_REQ_1 0x20AA
|
||||
#define IAP_UPLOAD_REQ_2 0x2055
|
||||
#define IAP_DNLOAD_REQ_1 0x30AA
|
||||
#define IAP_DNLOAD_REQ_2 0x3055
|
||||
|
||||
#define UPPERWORD16(lw) (uint16_t)((uint32_t)(lw)>>16)
|
||||
#define LOWERWORD16(lw) (uint16_t)((uint32_t)(lw)&0x0000ffff)
|
||||
#define UPPERBYTE(w) (uint8_t)((w)>>8)
|
||||
#define LOWERBYTE(w) (uint8_t)((w)&0x00ff)
|
||||
|
||||
/****************************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************************/
|
||||
|
||||
/****************************************************************************************
|
||||
* Private (static) Data
|
||||
****************************************************************************************/
|
||||
|
||||
/****************************************************************************************
|
||||
* Public/Global Data
|
||||
****************************************************************************************/
|
||||
|
||||
/*!
|
||||
* \brief PIOS_IAP_Init - performs required initializations for iap module.
|
||||
* \param none.
|
||||
* \return none.
|
||||
* \retval none.
|
||||
*
|
||||
* Created: Sep 8, 2010 10:10:48 PM by joe
|
||||
*/
|
||||
void PIOS_IAP_Init( void )
|
||||
{
|
||||
#if 0
|
||||
/* Enable CRC clock */
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
|
||||
|
||||
/* Enable PWR and BKP clock */
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE);
|
||||
|
||||
/* Enable write access to Backup domain */
|
||||
PWR_BackupAccessCmd(ENABLE);
|
||||
|
||||
/* Clear Tamper pin Event(TE) pending flag */
|
||||
BKP_ClearFlag();
|
||||
#endif
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Determines if an In-Application-Programming request has been made.
|
||||
* \param *comm - Which communication stream to use for the IAP (USB, Telemetry, I2C, SPI, etc)
|
||||
* \return TRUE - if correct sequence found, along with 'comm' updated.
|
||||
* FALSE - Note that 'comm' will have an invalid comm identifier.
|
||||
* \retval
|
||||
*
|
||||
*/
|
||||
uint32_t PIOS_IAP_CheckRequest( void )
|
||||
{
|
||||
#if 0
|
||||
uint32_t retval = FALSE;
|
||||
uint16_t reg1;
|
||||
uint16_t reg2;
|
||||
|
||||
reg1 = BKP_ReadBackupRegister( MAGIC_REG_1 );
|
||||
reg2 = BKP_ReadBackupRegister( MAGIC_REG_2 );
|
||||
|
||||
if( reg1 == IAP_MAGIC_WORD_1 && reg2 == IAP_MAGIC_WORD_2 ) {
|
||||
// We have a match.
|
||||
retval = TRUE;
|
||||
} else {
|
||||
retval = FALSE;
|
||||
}
|
||||
return retval;
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Sets the 1st word of the request sequence.
|
||||
* \param n/a
|
||||
* \return n/a
|
||||
* \retval
|
||||
*/
|
||||
void PIOS_IAP_SetRequest1(void)
|
||||
{
|
||||
#if 0
|
||||
BKP_WriteBackupRegister( MAGIC_REG_1, IAP_MAGIC_WORD_1);
|
||||
#endif
|
||||
}
|
||||
|
||||
void PIOS_IAP_SetRequest2(void)
|
||||
{
|
||||
#if 0
|
||||
BKP_WriteBackupRegister( MAGIC_REG_2, IAP_MAGIC_WORD_2);
|
||||
#endif
|
||||
}
|
||||
|
||||
void PIOS_IAP_ClearRequest(void)
|
||||
{
|
||||
#if 0
|
||||
BKP_WriteBackupRegister( MAGIC_REG_1, 0);
|
||||
BKP_WriteBackupRegister( MAGIC_REG_2, 0);
|
||||
#endif
|
||||
}
|
397
flight/PiOS/STM32F4xx/pios_imu3000.c
Normal file
397
flight/PiOS/STM32F4xx/pios_imu3000.c
Normal file
@ -0,0 +1,397 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @{
|
||||
* @addtogroup PIOS_IMU3000 IMU3000 Functions
|
||||
* @brief Deals with the hardware interface to the 3-axis gyro
|
||||
* @{
|
||||
*
|
||||
* @file pios_IMU3000.c
|
||||
* @author David "Buzz" Carlson (buzz@chebuzz.com)
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief IMU3000 3-axis gyor functions from INS
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
/*
|
||||
* This program 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 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* Project Includes */
|
||||
#include "pios.h"
|
||||
|
||||
#if defined(PIOS_INCLUDE_IMU3000)
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
#define DEG_TO_RAD (M_PI / 180.0)
|
||||
static void PIOS_IMU3000_Config(struct pios_imu3000_cfg const * cfg);
|
||||
static int32_t PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len);
|
||||
static int32_t PIOS_IMU3000_Write(uint8_t address, uint8_t buffer);
|
||||
|
||||
#define PIOS_IMU3000_MAX_DOWNSAMPLE 10
|
||||
static int16_t pios_imu3000_buffer[PIOS_IMU3000_MAX_DOWNSAMPLE * sizeof(struct pios_imu3000_data)];
|
||||
static t_fifo_buffer pios_imu3000_fifo;
|
||||
|
||||
volatile bool imu3000_first_read = true;
|
||||
volatile bool imu3000_configured = false;
|
||||
volatile bool imu3000_cb_ready = true;
|
||||
|
||||
static struct pios_imu3000_cfg const * cfg;
|
||||
|
||||
/**
|
||||
* @brief Initialize the IMU3000 3-axis gyro sensor.
|
||||
* @return none
|
||||
*/
|
||||
void PIOS_IMU3000_Init(const struct pios_imu3000_cfg * new_cfg)
|
||||
{
|
||||
cfg = new_cfg;
|
||||
|
||||
fifoBuf_init(&pios_imu3000_fifo, (uint8_t *) pios_imu3000_buffer, sizeof(pios_imu3000_buffer));
|
||||
|
||||
/* Configure EOC pin as input floating */
|
||||
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
|
||||
|
||||
/* Configure the End Of Conversion (EOC) interrupt */
|
||||
SYSCFG_EXTILineConfig(cfg->eoc_exti.port_source, cfg->eoc_exti.pin_source);
|
||||
EXTI_Init(&cfg->eoc_exti.init);
|
||||
|
||||
/* Enable and set EOC EXTI Interrupt to the lowest priority */
|
||||
NVIC_Init(&cfg->eoc_irq.init);
|
||||
|
||||
/* Configure the IMU3000 Sensor */
|
||||
PIOS_IMU3000_Config(cfg);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize the IMU3000 3-axis gyro sensor
|
||||
* \return none
|
||||
* \param[in] PIOS_IMU3000_ConfigTypeDef struct to be used to configure sensor.
|
||||
*
|
||||
*/
|
||||
static void PIOS_IMU3000_Config(struct pios_imu3000_cfg const * cfg)
|
||||
{
|
||||
imu3000_first_read = true;
|
||||
imu3000_cb_ready = true;
|
||||
|
||||
// Reset chip and fifo
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, 0x01 | 0x02) != 0);
|
||||
PIOS_DELAY_WaituS(20);
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, 0x00) != 0);
|
||||
|
||||
// FIFO storage
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_FIFO_EN_REG, cfg->Fifo_store) != 0);
|
||||
|
||||
// Sample rate divider
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_SMPLRT_DIV_REG, cfg->Smpl_rate_div) != 0) ;
|
||||
|
||||
// Digital low-pass filter and scale
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_DLPF_CFG_REG, cfg->filter | (cfg->range << 3)) != 0) ;
|
||||
|
||||
// Interrupt configuration
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, cfg->User_ctl) != 0) ;
|
||||
|
||||
// Interrupt configuration
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ;
|
||||
|
||||
// Interrupt configuration
|
||||
while (PIOS_IMU3000_Write(PIOS_IMU3000_INT_CFG_REG, cfg->Interrupt_cfg) != 0) ;
|
||||
|
||||
imu3000_configured = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read current X, Z, Y values (in that order)
|
||||
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
|
||||
* \returns The number of samples remaining in the fifo
|
||||
*/
|
||||
int32_t PIOS_IMU3000_ReadGyros(struct pios_imu3000_data * data)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
if(PIOS_IMU3000_Read(PIOS_IMU3000_GYRO_X_OUT_MSB, (uint8_t *) buf, sizeof(buf)) < 0)
|
||||
return -1;
|
||||
data->x = buf[0] << 8 | buf[1];
|
||||
data->y = buf[2] << 8 | buf[3];
|
||||
data->z = buf[4] << 8 | buf[5];
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Read the identification bytes from the IMU3000 sensor
|
||||
* \return ID read from IMU3000 or -1 if failure
|
||||
*/
|
||||
int32_t PIOS_IMU3000_ReadID()
|
||||
{
|
||||
uint8_t imu3000_id;
|
||||
if(PIOS_IMU3000_Read(0x00, (uint8_t *) &imu3000_id, 1) != 0)
|
||||
return -1;
|
||||
return imu3000_id;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Reads the data from the IMU3000 FIFO
|
||||
* \param[out] buffer destination buffer
|
||||
* \param[in] len maximum number of bytes which should be read
|
||||
* \note This returns the data as X, Y, Z the temperature
|
||||
* \return number of bytes transferred if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
*/
|
||||
int32_t PIOS_IMU3000_ReadFifo(struct pios_imu3000_data * buffer)
|
||||
{
|
||||
if(fifoBuf_getUsed(&pios_imu3000_fifo) < sizeof(*buffer))
|
||||
return -1;
|
||||
|
||||
fifoBuf_getData(&pios_imu3000_fifo, (uint8_t *) buffer, sizeof(*buffer));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reads one or more bytes from IMU3000 into a buffer
|
||||
* \param[in] address IMU3000 register address (depends on size)
|
||||
* \param[out] buffer destination buffer
|
||||
* \param[in] len number of bytes which should be read
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
* \return -2 if unable to claim i2c device
|
||||
*/
|
||||
static int32_t PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len)
|
||||
{
|
||||
uint8_t addr_buffer[] = {
|
||||
address,
|
||||
};
|
||||
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = PIOS_IMU3000_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(addr_buffer),
|
||||
.buf = addr_buffer,
|
||||
}
|
||||
,
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = PIOS_IMU3000_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_READ,
|
||||
.len = len,
|
||||
.buf = buffer,
|
||||
}
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
}
|
||||
|
||||
// Must allocate on stack to be persistent
|
||||
static uint8_t cb_addr_buffer[] = {
|
||||
0,
|
||||
};
|
||||
static struct pios_i2c_txn cb_txn_list[] = {
|
||||
{
|
||||
.addr = PIOS_IMU3000_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(cb_addr_buffer),
|
||||
.buf = cb_addr_buffer,
|
||||
}
|
||||
,
|
||||
{
|
||||
.addr = PIOS_IMU3000_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_READ,
|
||||
.len = 0,
|
||||
.buf = 0,
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
static int32_t PIOS_IMU3000_Read_Callback(uint8_t address, uint8_t * buffer, uint8_t len, void *callback)
|
||||
{
|
||||
cb_addr_buffer[0] = address;
|
||||
cb_txn_list[0].info = __func__,
|
||||
cb_txn_list[1].info = __func__;
|
||||
cb_txn_list[1].len = len;
|
||||
cb_txn_list[1].buf = buffer;
|
||||
|
||||
return PIOS_I2C_Transfer_Callback(PIOS_I2C_GYRO_ADAPTER, cb_txn_list, NELEMENTS(cb_txn_list), callback);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Writes one or more bytes to the IMU3000
|
||||
* \param[in] address Register address
|
||||
* \param[in] buffer source buffer
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
* \return -2 if unable to claim i2c device
|
||||
*/
|
||||
static int32_t PIOS_IMU3000_Write(uint8_t address, uint8_t buffer)
|
||||
{
|
||||
uint8_t data[] = {
|
||||
address,
|
||||
buffer,
|
||||
};
|
||||
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = PIOS_IMU3000_I2C_ADDR,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = sizeof(data),
|
||||
.buf = data,
|
||||
}
|
||||
,
|
||||
};
|
||||
|
||||
return PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list, NELEMENTS(txn_list));
|
||||
}
|
||||
|
||||
float PIOS_IMU3000_GetScale()
|
||||
{
|
||||
switch (cfg->range) {
|
||||
case PIOS_IMU3000_SCALE_250_DEG:
|
||||
return DEG_TO_RAD / 131.0;
|
||||
case PIOS_IMU3000_SCALE_500_DEG:
|
||||
return DEG_TO_RAD / 65.5;
|
||||
case PIOS_IMU3000_SCALE_1000_DEG:
|
||||
return DEG_TO_RAD / 32.8;
|
||||
case PIOS_IMU3000_SCALE_2000_DEG:
|
||||
return DEG_TO_RAD / 16.4;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/**
|
||||
* @brief Run self-test operation.
|
||||
* \return 0 if test succeeded
|
||||
* \return non-zero value if test succeeded
|
||||
*/
|
||||
uint8_t PIOS_IMU3000_Test(void)
|
||||
{
|
||||
/* Verify that ID matches (IMU3000 ID is 0x69) */
|
||||
int32_t imu3000_id = PIOS_IMU3000_ReadID();
|
||||
if(imu3000_id < 0)
|
||||
return -1;
|
||||
|
||||
if(imu3000_id != PIOS_IMU3000_I2C_ADDR)
|
||||
return -2;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static uint8_t imu3000_read_buffer[sizeof(struct pios_imu3000_data) + 2]; // Right now using ,Y,Z,fifo_footer
|
||||
static void imu3000_callback()
|
||||
{
|
||||
struct pios_imu3000_data data;
|
||||
|
||||
if(fifoBuf_getFree(&pios_imu3000_fifo) < sizeof(data))
|
||||
goto out;
|
||||
|
||||
if(imu3000_first_read) {
|
||||
data.temperature = imu3000_read_buffer[0] << 8 | imu3000_read_buffer[1];
|
||||
data.x = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3];
|
||||
data.y = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5];
|
||||
data.z = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7];
|
||||
|
||||
imu3000_first_read = false;
|
||||
} else {
|
||||
// First two bytes are left over fifo from last call
|
||||
data.temperature = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3];
|
||||
data.x = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5];
|
||||
data.y = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7];
|
||||
data.z = imu3000_read_buffer[8] << 8 | imu3000_read_buffer[9];
|
||||
}
|
||||
|
||||
fifoBuf_putData(&pios_imu3000_fifo, (uint8_t *) &data, sizeof(data));
|
||||
|
||||
out:
|
||||
imu3000_cb_ready = true;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief IRQ Handler
|
||||
*/
|
||||
uint32_t imu3000_irq = 0;
|
||||
uint16_t fifo_level;
|
||||
uint8_t fifo_level_data[2];
|
||||
uint32_t cb_not_ready = 0;
|
||||
void PIOS_IMU3000_IRQHandler(void)
|
||||
{
|
||||
|
||||
imu3000_irq++;
|
||||
|
||||
if(!imu3000_configured)
|
||||
return;
|
||||
|
||||
//PIOS_Assert(imu3000_cb_ready);
|
||||
if(!imu3000_cb_ready) {
|
||||
PIOS_LED_Toggle(LED2);
|
||||
cb_not_ready++;
|
||||
return;
|
||||
}
|
||||
|
||||
// If at least one read doesnt succeed then the irq not reset and we will stall
|
||||
while(PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_CNT_MSB, (uint8_t *) &fifo_level_data, sizeof(fifo_level_data)) != 0)
|
||||
PIOS_DELAY_WaituS(10);
|
||||
|
||||
fifo_level = (fifo_level_data[0] << 8) + fifo_level_data[1];
|
||||
|
||||
PIOS_DELAY_WaituS(10);
|
||||
|
||||
if(imu3000_first_read) {
|
||||
// Stupid system for IMU3000. If first read from buffer then we will read 4 sensors without fifo
|
||||
// footer. After this we will read out a fifo footer
|
||||
if(fifo_level < sizeof(imu3000_read_buffer))
|
||||
return;
|
||||
|
||||
imu3000_cb_ready = false;
|
||||
|
||||
// Leave footer in buffer
|
||||
PIOS_IMU3000_Read_Callback(PIOS_IMU3000_FIFO_REG, imu3000_read_buffer, sizeof(imu3000_read_buffer) - 2, imu3000_callback);
|
||||
|
||||
} else {
|
||||
// Stupid system for IMU3000. Ensure something is left in buffer
|
||||
if(fifo_level < (sizeof(imu3000_read_buffer) + 2))
|
||||
return;
|
||||
|
||||
imu3000_cb_ready = false;
|
||||
|
||||
// Leave footer in buffer
|
||||
PIOS_IMU3000_Read_Callback(PIOS_IMU3000_FIFO_REG, imu3000_read_buffer, sizeof(imu3000_read_buffer), imu3000_callback);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* The physical IRQ handler
|
||||
* Soon this will be generic in pios_exti and the BMA180 will register
|
||||
* against it. Right now this is crap!
|
||||
*/
|
||||
void EXTI1_IRQHandler(void)
|
||||
{
|
||||
if (EXTI_GetITStatus(EXTI_Line1) != RESET)
|
||||
{
|
||||
PIOS_IMU3000_IRQHandler();
|
||||
EXTI_ClearITPendingBit(EXTI_Line1);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
88
flight/PiOS/inc/stm32f4xx_conf.h
Normal file
88
flight/PiOS/inc/stm32f4xx_conf.h
Normal file
@ -0,0 +1,88 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file Project/STM32F4xx_StdPeriph_Template/stm32f4xx_conf.h
|
||||
* @author MCD Application Team
|
||||
* @version V0.0.4
|
||||
* @date 13-January-2011
|
||||
* @brief Library configuration file.
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
|
||||
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
|
||||
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
|
||||
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
|
||||
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
|
||||
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
|
||||
*
|
||||
* <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __STM32F4xx_CONF_H
|
||||
#define __STM32F4xx_CONF_H
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
/* Uncomment the line below to enable peripheral header file inclusion */
|
||||
#include "stm32f4xx_adc.h"
|
||||
//#include "stm32f4xx_can.h"
|
||||
#include "stm32f4xx_crc.h"
|
||||
//#include "stm32f2xx_cryp.h"
|
||||
#include "stm32f4xx_dac.h"
|
||||
//#include "stm32f4xx_dbgmcu.h"
|
||||
//#include "stm32f4xx_dcmi.h"
|
||||
#include "stm32f4xx_dma.h"
|
||||
#include "stm32f4xx_exti.h"
|
||||
#include "stm32f4xx_flash.h"
|
||||
//#include "stm32f4xx_fsmc.h"
|
||||
//#include "stm32f4xx_hash.h"
|
||||
#include "stm32f4xx_gpio.h"
|
||||
#include "stm32f4xx_i2c.h"
|
||||
//#include "stm32f4xx_iwdg.h"
|
||||
#include "stm32f4xx_pwr.h"
|
||||
#include "stm32f4xx_rcc.h"
|
||||
//#include "stm32f4xx_rng.h"
|
||||
#include "stm32f4xx_rtc.h"
|
||||
//#include "stm32f4xx_sdio.h"
|
||||
#include "stm32f4xx_spi.h"
|
||||
#include "stm32f4xx_syscfg.h"
|
||||
#include "stm32f4xx_tim.h"
|
||||
#include "stm32f4xx_usart.h"
|
||||
#include "stm32f4xx_wwdg.h"
|
||||
#include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
|
||||
/* If an external clock source is used, then the value of the following define
|
||||
should be set to the value of the external clock source, else, if no external
|
||||
clock is used, keep this define commented */
|
||||
/*#define I2S_EXTERNAL_CLOCK_VAL 12288000 */ /* Value of the external clock in Hz */
|
||||
|
||||
|
||||
/* Uncomment the line below to expanse the "assert_param" macro in the
|
||||
Standard Peripheral Library drivers code */
|
||||
/* #define USE_FULL_ASSERT 1 */
|
||||
|
||||
/* Exported macro ------------------------------------------------------------*/
|
||||
#ifdef USE_FULL_ASSERT
|
||||
|
||||
/**
|
||||
* @brief The assert_param macro is used for function's parameters check.
|
||||
* @param expr: If expr is false, it calls assert_failed function
|
||||
* which reports the name of the source file and the source
|
||||
* line number of the call that failed.
|
||||
* If expr is true, it returns no value.
|
||||
* @retval None
|
||||
*/
|
||||
#define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void assert_failed(uint8_t* file, uint32_t line);
|
||||
#else
|
||||
#define assert_param(expr) ((void)0)
|
||||
#endif /* USE_FULL_ASSERT */
|
||||
|
||||
#endif /* __STM32F4xx_CONF_H */
|
||||
|
||||
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
|
Loading…
x
Reference in New Issue
Block a user