rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Add some drivers to the device specific library for now until the exti problems

Browse Source 
are fixed
This commit is contained in:
James Cotton 2011-11-01 03:19:58 -05:00
parent 2cc0b301ce
commit 9607da9c62
6 changed files with 1782 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

416
flight/PiOS/STM32F4xx/pios_bma180.c Normal file
View 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
View 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
View 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 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-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
View 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
View 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
View 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>&copy; 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****/