rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-378: Get BMA180 interrupt working. Data gets pushed onto a fifo that is

Browse Source 
pulled from in the INS code.
This commit is contained in:
James Cotton 2011-05-30 03:40:51 -05:00
parent ca2205d35f
commit b6a8293f69
7 changed files with 183 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/INS/inc/ins.h
View File

@ -56,11 +56,6 @@ struct mag_sensor {
//! Contains the data from the accelerometer
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;

27
flight/INS/ins.c
View File

@ -443,6 +443,9 @@ int16_t accel_data_glob[3];
int16_t gyro_data_glob[3];
int16_t mag_data_glob[3];
uint32_t pin;
int16_t accel[3];
int main()
{
gps_data.quality = -1;
@ -597,18 +600,34 @@ int main()
*
* This function will act as the HAL for the new INS sensors
*/
uint16_t accel_samples = 0;
bool get_accel_gyro_data()
{
int16_t accel[3];
int32_t accel_accum[3] = {0, 0, 0};
accel_samples = 0;
int16_t gyro[3];
PIOS_BMA180_ReadAccels(accel);
t_fifo_buffer * accel_fifo = PIOS_BMA180_GetFifo();
while(fifoBuf_getUsed(accel_fifo) < sizeof(accel));
while(fifoBuf_getUsed(accel_fifo) >= sizeof(accel)) {
accel_samples++;
fifoBuf_getData(accel_fifo, (uint8_t *) accel, sizeof(accel));
accel_accum[0] += accel[0];
accel_accum[1] += accel[1];
accel_accum[2] += accel[2];
}
accel[0] = accel_accum[0] / accel_samples;
accel[1] = accel_accum[1] / accel_samples;
accel[2] = accel_accum[2] / accel_samples;
PIOS_IMU3000_ReadGyros(gyro);
// Not the swaping of channel orders
accel_data.filtered.x = accel[0] * 0.0025;
accel_data.filtered.y = accel[1] * 0.0025;
accel_data.filtered.z = accel[2] * 0.0025;
accel_data.filtered.x = accel[0] * PIOS_BMA180_GetScale();
accel_data.filtered.y = accel[1] * PIOS_BMA180_GetScale();
accel_data.filtered.z = accel[2] * PIOS_BMA180_GetScale();
gyro_data.filtered.x = -gyro[1] * 0.00763 * DEG_TO_RAD;;
gyro_data.filtered.y = -gyro[0] * 0.00763 * DEG_TO_RAD;;
gyro_data.filtered.z = -gyro[2] * 0.00763 * DEG_TO_RAD;;

1
flight/INS/pios_board.c
View File

@ -590,6 +590,7 @@ void PIOS_Board_Init(void) {
}
PIOS_BMA180_Attach(pios_spi_accel_id);
PIOS_BMA180_Init();
/* Set up the SPI interface to the OP board */

6
flight/PiOS/Boards/STM3210E_INS.h
View File

@ -195,9 +195,9 @@ extern uint32_t pios_com_aux_id;
// GPIO
// Not used, but pios_gpio.c expects something
//-------------------------
#define PIOS_GPIO_1_PORT GPIOA
#define PIOS_GPIO_1_PIN GPIO_Pin_1
#define PIOS_GPIO_1_GPIO_CLK RCC_APB2Periph_GPIOA
#define PIOS_GPIO_1_PORT GPIOA
#define PIOS_GPIO_1_PIN GPIO_Pin_1
#define PIOS_GPIO_1_GPIO_CLK RCC_APB2Periph_GPIOA
#define PIOS_GPIO_PORTS { PIOS_GPIO_1_PORT }
#define PIOS_GPIO_PINS { PIOS_GPIO_1_PIN }

100
flight/PiOS/Common/pios_bma180.c
View File

@ -31,16 +31,23 @@
*/
#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(uint8_t bw);
static int32_t PIOS_BMA180_SetRange(uint8_t range);
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();
volatile bool pios_bma180_data_ready = false;
#define PIOS_BMA180_MAX_DOWNSAMPLE 10
static int16_t pios_bma180_buffer[PIOS_BMA180_MAX_DOWNSAMPLE * 3];
static t_fifo_buffer pios_bma180_fifo;
/**
* @brief Initialize with good default settings
*/
@ -75,8 +82,12 @@ void PIOS_BMA180_Init()
pios_bma180_data_ready = false;
PIOS_BMA180_SelectBW(BMA_BW_150HZ);
fifoBuf_init(&pios_bma180_fifo, (uint8_t *) pios_bma180_buffer, sizeof(pios_bma180_buffer));
PIOS_BMA180_Config();
PIOS_BMA180_SelectBW(BMA_BW_600HZ);
PIOS_BMA180_SetRange(BMA_RANGE_8G);
PIOS_DELAY_WaituS(50);
PIOS_BMA180_EnableIrq();
}
@ -140,6 +151,52 @@ int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data)
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
*/
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;
return 0;
}
/**
* @brief Select the bandwidth the digital filter pass allows.
* @return 0 if successful, -1 if not
@ -147,7 +204,7 @@ int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data)
*
* EEPROM must be write-enabled before calling this function.
*/
static int32_t PIOS_BMA180_SelectBW(uint8_t bw)
static int32_t PIOS_BMA180_SelectBW(enum bma180_bandwidth bw)
{
uint8_t reg;
reg = PIOS_BMA180_GetReg(BMA_BW_ADDR);
@ -161,7 +218,7 @@ static int32_t PIOS_BMA180_SelectBW(uint8_t bw)
* @param rate[in] Range setting to be used
*
*/
static int32_t PIOS_BMA180_SetRange(uint8_t range)
static int32_t PIOS_BMA180_SetRange(enum bma180_range range)
{
uint8_t reg;
reg = PIOS_BMA180_GetReg(BMA_RANGE_ADDR);
@ -171,7 +228,17 @@ static int32_t PIOS_BMA180_SetRange(uint8_t range)
static int32_t PIOS_BMA180_EnableIrq()
{
return PIOS_BMA180_SetReg(BMA_CTRREG3, BMA_NEW_DAT_INT);
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;
}
/**
@ -217,11 +284,17 @@ int32_t PIOS_BMA180_ReadAccels(int16_t * data)
* @brief Returns the scale the BMA180 chip is using
* @return Scale (m / s^2) / LSB
*/
float PIOS_BMA_GetScale()
float PIOS_BMA180_GetScale()
{
return 1;
return 9.81 / 1024;
}
t_fifo_buffer * PIOS_BMA180_GetFifo()
{
return &pios_bma180_fifo;
}
/**
* @brief Test SPI and chip functionality by reading chip ID register
* @return 0 if success, -1 if failure.
@ -255,12 +328,21 @@ int32_t PIOS_BMA180_Test()
return 0;
}
uint32_t pios_bma180_count = 0;
/**
* @brief IRQ Handler
*/
void PIOS_BMA180_IRQHandler(void)
{
pios_bma180_data_ready = true;
int16_t accels[3];
pios_bma180_count++;
if(PIOS_BMA180_ReadAccels(accels) < 0)
return;
if(fifoBuf_getFree(&pios_bma180_fifo) < sizeof(accels))
return;
fifoBuf_putData(&pios_bma180_fifo, accels, sizeof(accels));
}
/**

43
flight/PiOS/inc/pios_bma180.h
View File

@ -30,6 +30,8 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "fifo_buffer.h"
#ifndef PIOS_BMA180_H
#define PIOS_BMA180_H
@ -42,32 +44,37 @@
#define BMA_WE_ADDR 0x0D
#define BMA_BW_ADDR 0x20
#define BMA_RANGE_ADDR 0x35
#define BMA_OFFSET_LSB1 0x35
#define BMA_GAIN_Y 0x33
#define BMA_CTRREG3 0x21
#define BMA_CTRREG0 0x0D
/* Accel range */
#define BMA_RANGE_MASK 0x0E
#define BMA_RANGE_SHIFT 1
#define BMA_RANGE_1G 0x00 // +/- 1G ADC resolution 0.13 mg/LSB
#define BMA_RANGE_1_5G 0x01 // +/- 1.5G ADC resolution 0.19 mg/LSB
#define BMA_RANGE_2G 0x02 // +/- 2G ADC resolution 0.25 mg/LSB *** default ***
#define BMA_RANGE_3G 0x03 // +/- 3G ADC resolution 0.38 mg/LSB
#define BMA_RANGE_4G 0x04 // +/- 4G ADC resolution 0.50 mg/LSB
#define BMA_RANGE_8G 0x05 // +/- 8G ADC resolution 0.99 mg/LSB
#define BMA_RANGE_16G 0x06 // +/- 16G ADC resolution 1.98 mg/LSB
enum bma180_range { BMA_RANGE_1G = 0x00,
BMA_RANGE_1_5G = 0x01,
BMA_RANGE_2G = 0x02,
BMA_RANGE_3G = 0x03,
BMA_RANGE_4G = 0x04,
BMA_RANGE_8G = 0x05,
BMA_RANGE_16G = 0x06
};
/* Measurement bandwidth */
#define BMA_BW_MASK 0xF0
#define BMA_BW_SHIFT 4
#define BMA_BW_10HZ 0x00
#define BMA_BW_20HZ 0x01
#define BMA_BW_40HZ 0x02
#define BMA_BW_75HZ 0x03
#define BMA_BW_150HZ 0x04 // *** default ***
#define BMA_BW_300HZ 0x05
#define BMA_BW_600HZ 0x06
#define BMA_BW_1200HZ 0x07
#define BMA_BW_HP1HZ 0x08 // High-pass, 1Hz
#define BMA_BW_BP0_300HZ 0x09 // Band-pass, 0.3Hz-300Hz
enum bma180_bandwidth { BMA_BW_10HZ = 0x00,
BMA_BW_20HZ = 0x01,
BMA_BW_40HZ = 0x02,
BMA_BW_75HZ = 0x03,
BMA_BW_150HZ = 0x04,
BMA_BW_300HZ = 0x05,
BMA_BW_600HZ = 0x06,
BMA_BW_1200HZ =0x07,
BMA_BW_HP1HZ = 0x08, // High-pass, 1 Hz
BMA_BW_BP0_300HZ = 0x09 // Band-pass, 0.3Hz-300Hz
};
#define BMA_NEW_DAT_INT 0x02
@ -80,9 +87,11 @@ struct pios_bma180_data {
/* Public Functions */
void PIOS_BMA180_Attach(uint32_t spi_id);
void PIOS_BMA180_Init();
float PIOS_BMA180_GetScale();
int32_t PIOS_BMA180_ReadAccels(int16_t * data);
int32_t PIOS_BMA180_Test();
void PIOS_BMA180_IRQHandler(void);
t_fifo_buffer * PIOS_BMA180_GetFifo();
#endif /* PIOS_BMA180_H */

39
flight/Project/gdb/ins Normal file
View File

@ -0,0 +1,39 @@
define connect
target remote localhost:3334
monitor cortex_m3 vector_catch all
file ./build/ins/INS.elf
# file ./build/bl_ins/BL_INS.elf
end
#monitor reset halt
define hook-step
monitor cortex_m3 maskisr on
end
define hookpost-step
monitor cortex_m3 maskisr off
end
define hook-stepi
monitor cortex_m3 maskisr on
end
define hookpost-stepi
monitor cortex_m3 maskisr off
end
define hook-next
monitor cortex_m3 maskisr on
end
define hookpost-next
monitor cortex_m3 maskisr off
end
define hook-finish
monitor cortex_m3 maskisr on
end
define hookpost-finish
monitor cortex_m3 maskisr off
end