mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-29 14:52:12 +01:00
Get the MPU6000 CC3D working. Right now the ADC system is commented out, which
will break regular CC.
This commit is contained in:
James Cotton
parent
a0b7453580
commit
1415728762
@ -215,7 +215,7 @@ SRC += $(PIOSSTM32F10X)/pios_led.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_delay.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_adc.c
|
||||
#SRC += $(PIOSSTM32F10X)/pios_adc.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_servo.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_i2c.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
@ -248,8 +248,7 @@ SRC += $(PIOSCOMMON)/pios_crc.c
|
||||
SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c
|
||||
SRC += $(PIOSCOMMON)/pios_flash_jedec.c
|
||||
SRC += $(PIOSCOMMON)/pios_adxl345.c
|
||||
SRC += $(PIOSCOMMON)/pios_l3gd20.c
|
||||
SRC += $(PIOSCOMMON)/pios_bma180.c
|
||||
SRC += $(PIOSCOMMON)/pios_mpu6000.c
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
#SRC += $(PIOSCOMMON)/pios_i2c_esc.c
|
||||
#SRC += $(PIOSCOMMON)/pios_bmp085.c
|
||||
|
||||
@ -34,7 +34,7 @@
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_ADC
|
||||
//#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
//#if defined(USE_I2C)
|
||||
//#define PIOS_INCLUDE_I2C
|
||||
@ -77,11 +77,8 @@
|
||||
|
||||
#define PIOS_INCLUDE_ADXL345
|
||||
#define PIOS_INCLUDE_FLASH
|
||||
#define PIOS_INCLUDE_BMA180
|
||||
#define PIOS_INCLUDE_L3GD20
|
||||
/*
|
||||
#define PIOS_INCLUDE_BMP085
|
||||
*/
|
||||
#define PIOS_INCLUDE_MPU6000
|
||||
#define PIOS_MPU6000_ACCEL
|
||||
|
||||
/* A really shitty setting saving implementation */
|
||||
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
|
||||
|
||||
@ -68,52 +68,12 @@ uint32_t pios_com_gps_id;
|
||||
uint32_t pios_com_bridge_id;
|
||||
|
||||
/**
|
||||
* Configuration for the BMA180 chip
|
||||
* Configuration for MPU6000 chip
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
#include "pios_bma180.h"
|
||||
static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = {
|
||||
.vector = PIOS_BMA180_IRQHandler,
|
||||
.line = EXTI_Line13,
|
||||
.pin = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.irq = {
|
||||
.init = {
|
||||
.NVIC_IRQChannel = EXTI15_10_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.exti = {
|
||||
.init = {
|
||||
.EXTI_Line = EXTI_Line13, // matches above GPIO pin
|
||||
.EXTI_Mode = EXTI_Mode_Interrupt,
|
||||
.EXTI_Trigger = EXTI_Trigger_Rising,
|
||||
.EXTI_LineCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
};
|
||||
static const struct pios_bma180_cfg pios_bma180_cfg = {
|
||||
.exti_cfg = &pios_exti_bma180_cfg,
|
||||
.bandwidth = BMA_BW_300HZ,
|
||||
.range = BMA_RANGE_8G,
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_BMA180 */
|
||||
|
||||
/**
|
||||
* Configuration for L3GD20 chip
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
#include "pios_l3gd20.h"
|
||||
static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = {
|
||||
.vector = PIOS_L3GD20_IRQHandler,
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
#include "pios_mpu6000.h"
|
||||
static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
|
||||
.vector = PIOS_MPU6000_IRQHandler,
|
||||
.line = EXTI_Line3,
|
||||
.pin = {
|
||||
.gpio = GPIOA,
|
||||
@ -141,11 +101,19 @@ static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = {
|
||||
},
|
||||
};
|
||||
|
||||
static const struct pios_l3gd20_cfg pios_l3gd20_cfg = {
|
||||
.exti_cfg = &pios_exti_l3gd20_cfg,
|
||||
.range = PIOS_L3GD20_SCALE_500_DEG,
|
||||
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
|
||||
.exti_cfg = &pios_exti_mpu6000_cfg,
|
||||
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
|
||||
// Clock at 8 khz, downsampled by 8 for 1khz
|
||||
.Smpl_rate_div = 7,
|
||||
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
|
||||
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
|
||||
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN,
|
||||
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
|
||||
.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
|
||||
.filter = PIOS_MPU6000_LOWPASS_256_HZ
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_L3GD20 */
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
static const struct flashfs_cfg flashfs_w25x_cfg = {
|
||||
.table_magic = 0x85FB3C35,
|
||||
@ -160,12 +128,25 @@ static const struct pios_flash_jedec_cfg flash_w25x_cfg = {
|
||||
.chip_erase = 0x60
|
||||
};
|
||||
|
||||
static const struct flashfs_cfg flashfs_m25p_cfg = {
|
||||
.table_magic = 0x85FB3D35,
|
||||
.obj_magic = 0x3015A371,
|
||||
.obj_table_start = 0x00000010,
|
||||
.obj_table_end = 0x00010000,
|
||||
.sector_size = 0x00010000,
|
||||
};
|
||||
|
||||
static const struct pios_flash_jedec_cfg flash_m25p_cfg = {
|
||||
.sector_erase = 0xD8,
|
||||
.chip_erase = 0xC7
|
||||
};
|
||||
#include <pios_board_info.h>
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
int32_t init_test;
|
||||
void PIOS_Board_Init(void) {
|
||||
|
||||
/* Delay system */
|
||||
@ -206,9 +187,19 @@ void PIOS_Board_Init(void) {
|
||||
}
|
||||
|
||||
#endif
|
||||
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg);
|
||||
|
||||
PIOS_FLASHFS_Init(&flashfs_w25x_cfg);
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01: // Revision 1
|
||||
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg);
|
||||
PIOS_FLASHFS_Init(&flashfs_w25x_cfg);
|
||||
break;
|
||||
case 0x02: // Revision 2
|
||||
PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 0, &flash_m25p_cfg);
|
||||
PIOS_FLASHFS_Init(&flashfs_m25p_cfg);
|
||||
break;
|
||||
default:
|
||||
PIOS_DEBUG_Assert(0);
|
||||
}
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
@ -709,7 +700,9 @@ void PIOS_Board_Init(void) {
|
||||
switch(bdinfo->board_rev) {
|
||||
case 0x01:
|
||||
// Revision 1 with invensense gyros, start the ADC
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
PIOS_ADC_Init();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_ADXL345)
|
||||
PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0);
|
||||
#endif
|
||||
@ -718,19 +711,16 @@ void PIOS_Board_Init(void) {
|
||||
// Revision 2 with L3GD20 gyros, start a SPI interface and connect to it
|
||||
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
|
||||
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
// Set up the SPI interface to the serial flash
|
||||
if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
|
||||
PIOS_Assert(PIOS_L3GD20_Test() == 0);
|
||||
#endif /* PIOS_INCLUDE_L3GD20 */
|
||||
PIOS_MPU6000_Attach(pios_spi_gyro_id);
|
||||
PIOS_MPU6000_Init(&pios_mpu6000_cfg);
|
||||
init_test = PIOS_MPU6000_Test();
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
PIOS_BMA180_Init(pios_spi_flash_accel_id, 0, &pios_bma180_cfg);
|
||||
PIOS_Assert(PIOS_BMA180_Test() == 0);
|
||||
#endif /* PIOS_INCLUDE_BMA180 */
|
||||
break;
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
|
||||
@ -63,6 +63,7 @@
|
||||
#define STACK_SIZE_BYTES 540
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
|
||||
#define SENSOR_PERIOD 2
|
||||
#define UPDATE_RATE 25.0f
|
||||
#define GYRO_NEUTRAL 1665
|
||||
|
||||
@ -183,22 +184,21 @@ static void AttitudeTask(void *parameters)
|
||||
bool cc3d = bdinfo->board_rev == 0x02;
|
||||
|
||||
if(cc3d) {
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
accel_test = PIOS_BMA180_Test();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
gyro_test = PIOS_L3GD20_Test();
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
gyro_test = PIOS_MPU6000_Test();
|
||||
#endif
|
||||
} else {
|
||||
#if defined(PIOS_INCLUDE_ADXL345)
|
||||
accel_test = PIOS_ADXL345_Test();
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
// Create queue for passing gyro data, allow 2 back samples in case
|
||||
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
|
||||
PIOS_Assert(gyro_queue != NULL);
|
||||
PIOS_ADC_SetQueue(gyro_queue);
|
||||
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
|
||||
#endif
|
||||
|
||||
}
|
||||
// Force settings update to make sure rotation loaded
|
||||
@ -245,7 +245,6 @@ static void AttitudeTask(void *parameters)
|
||||
else {
|
||||
// Only update attitude when sensor data is good
|
||||
updateAttitude(&accels, &gyros);
|
||||
AccelsSet(&accels);
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
}
|
||||
@ -345,9 +344,8 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
|
||||
// and make it average zero (weakly)
|
||||
gyro_correct_int[2] += - gyros->z * yawBiasRate;
|
||||
|
||||
gyros_passed[0] = gyros->x;
|
||||
gyros_passed[1] = gyros->y;
|
||||
gyros_passed[2] = gyros->z;
|
||||
GyrosSet(gyros);
|
||||
AccelsSet(accels);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -357,111 +355,94 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros)
|
||||
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
|
||||
* @return 0 if successfull, -1 if not
|
||||
*/
|
||||
struct pios_bma180_data accel;
|
||||
struct pios_l3gd20_data gyro;
|
||||
struct pios_mpu6000_data gyro;
|
||||
static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
{
|
||||
static portTickType lastSysTime;
|
||||
uint32_t accel_samples = 0;
|
||||
int32_t accel_accum[3] = {0, 0, 0};
|
||||
float gyro_scaling = 1;
|
||||
float gyros_accum[3] = {0, 0, 0};
|
||||
float gyro_accum[3] = {0, 0, 0};
|
||||
float accel_scaling = 1;
|
||||
uint32_t gyro_samples;
|
||||
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
|
||||
struct pios_l3gd20_data gyro;
|
||||
gyro_scaling = PIOS_L3GD20_GetScale();
|
||||
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
|
||||
|
||||
if(xQueueReceive(gyro_queue, (void *) &gyro, 3) == errQUEUE_EMPTY) {
|
||||
// Unfortunately if the L3GD20 ever misses getting read, then it will not
|
||||
// trigger more interrupts. In this case we must force a read to kickstart
|
||||
// it.
|
||||
PIOS_L3GD20_ReadGyros(&gyro);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
gyrosData->x = gyro.gyro_x * gyro_scaling;
|
||||
gyrosData->y = -gyro.gyro_y * gyro_scaling;
|
||||
gyrosData->z = -gyro.gyro_z * gyro_scaling;
|
||||
gyrosData->temperature = 0;
|
||||
GyrosSet(gyrosData);
|
||||
|
||||
gyros_accum[0] = gyrosData->x;
|
||||
gyros_accum[1] = gyrosData->y;
|
||||
gyros_accum[2] = gyrosData->z;
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
|
||||
if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
uint32_t count = 0;
|
||||
int32_t read_good = 0;
|
||||
int32_t error = false;
|
||||
|
||||
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error)
|
||||
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
|
||||
if (error)
|
||||
return -1;
|
||||
}
|
||||
|
||||
gyrosData->x = gyro.gyro_x * gyro_scaling;
|
||||
gyrosData->y = -gyro.gyro_y * gyro_scaling;
|
||||
gyrosData->z = -gyro.gyro_z * gyro_scaling;
|
||||
gyrosData->temperature = 0;
|
||||
|
||||
gyros_accum[0] += gyrosData->x;
|
||||
gyros_accum[1] += gyrosData->y;
|
||||
gyros_accum[2] += gyrosData->z;
|
||||
|
||||
if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
gyrosData->x = gyro.gyro_x * gyro_scaling;
|
||||
gyrosData->y = -gyro.gyro_y * gyro_scaling;
|
||||
gyrosData->z = -gyro.gyro_z * gyro_scaling;
|
||||
gyrosData->temperature = 0;
|
||||
GyrosSet(gyrosData);
|
||||
|
||||
gyros_accum[0] += gyrosData->x;
|
||||
gyros_accum[1] += gyrosData->y;
|
||||
gyros_accum[2] += gyrosData->z;
|
||||
|
||||
gyros_passed[0] = gyros_accum[0] / 3;
|
||||
gyros_passed[1] = gyros_accum[1] / 3;
|
||||
gyros_passed[2] = gyros_accum[2] / 3;
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
struct pios_bma180_data accel;
|
||||
accel_samples = 0;
|
||||
bool error = false;
|
||||
int32_t accel_read_good;
|
||||
|
||||
while((accel_read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
|
||||
error = ((xTaskGetTickCount() - lastSysTime) > 5) ? true : error;
|
||||
if (error) {
|
||||
// Unfortunately if the BMA180 ever misses getting read, then it will not
|
||||
// trigger more interrupts. In this case we must force a read to kickstart
|
||||
// it.
|
||||
PIOS_BMA180_ReadAccels(&accel);
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
return -1;
|
||||
}
|
||||
while(accel_read_good == 0) {
|
||||
accel_samples++;
|
||||
while(read_good == 0) {
|
||||
count++;
|
||||
|
||||
accel_accum[0] += accel.x;
|
||||
accel_accum[1] += accel.y;
|
||||
accel_accum[2] += accel.z;
|
||||
gyro_accum[0] += gyro.gyro_x;
|
||||
gyro_accum[1] += gyro.gyro_y;
|
||||
gyro_accum[2] += gyro.gyro_z;
|
||||
|
||||
accel_read_good = PIOS_BMA180_ReadFifo(&accel);
|
||||
accel_accum[0] += gyro.accel_x;
|
||||
accel_accum[1] += gyro.accel_y;
|
||||
accel_accum[2] += gyro.accel_z;
|
||||
|
||||
read_good = PIOS_MPU6000_ReadFifo(&gyro);
|
||||
}
|
||||
accel_scaling = PIOS_BMA180_GetScale();
|
||||
gyro_samples = count;
|
||||
gyro_scaling = PIOS_MPU6000_GetScale();
|
||||
|
||||
accel_samples = count;
|
||||
accel_scaling = PIOS_MPU6000_GetAccelScale();
|
||||
#endif
|
||||
|
||||
float accels[3] = {(float) -accel_accum[1] / accel_samples, (float) -accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
|
||||
|
||||
// Get temp from last reading
|
||||
gyrosData->temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
|
||||
accelsData->temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
|
||||
|
||||
// Scale the accels
|
||||
float accels[3] = {-(float) accel_accum[1] / accel_samples,
|
||||
-(float) accel_accum[0] / accel_samples,
|
||||
-(float) accel_accum[2] / accel_samples};
|
||||
|
||||
// Scale the gyros
|
||||
float gyros[3] = {-(float) gyro_accum[1] / gyro_samples,
|
||||
-(float) gyro_accum[0] / gyro_samples,
|
||||
-(float) gyro_accum[2] / gyro_samples};
|
||||
|
||||
if(rotate) {
|
||||
// TODO: rotate sensors too so stabilization is well behaved
|
||||
float vec_out[3];
|
||||
rot_mult(R, accels, vec_out);
|
||||
accels[0] = vec_out[0];
|
||||
accels[1] = vec_out[1];
|
||||
accels[2] = vec_out[2];
|
||||
rot_mult(R, gyros, vec_out);
|
||||
gyros[0] = vec_out[0];
|
||||
gyros[1] = vec_out[1];
|
||||
gyros[2] = vec_out[2];
|
||||
}
|
||||
|
||||
accelsData->x = (accels[0] - accelbias[0]) * accel_scaling;
|
||||
accelsData->y = (accels[1] - accelbias[1]) * accel_scaling;
|
||||
accelsData->z = (accels[2] - accelbias[2]) * accel_scaling;
|
||||
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
|
||||
AccelsSet(&accelsData);
|
||||
|
||||
gyrosData->x = gyros[0] * gyro_scaling;
|
||||
gyrosData->y = gyros[1] * gyro_scaling;
|
||||
gyrosData->z = gyros[2] * gyro_scaling;
|
||||
|
||||
if(bias_correct_gyro) {
|
||||
// Applying integral component here so it can be seen on the gyros and correct bias
|
||||
gyrosData->x += gyro_correct_int[0];
|
||||
gyrosData->y += gyro_correct_int[1];
|
||||
gyrosData->z += gyro_correct_int[2];
|
||||
}
|
||||
|
||||
GyrosSet(gyrosData);
|
||||
AccelsSet(accelsData);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -475,7 +456,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
// Bad practice to assume structure order, but saves memory
|
||||
float * gyros = gyros_passed;
|
||||
float * gyros = &gyrosData->x;
|
||||
float * accels = &accelsData->x;
|
||||
|
||||
float grot[3];
|
||||
|
||||
@ -45,7 +45,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg);
|
||||
static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
|
||||
static int32_t PIOS_MPU6000_GetReg(uint8_t address);
|
||||
|
||||
#define PIOS_MPU6000_MAX_DOWNSAMPLE 100
|
||||
#define PIOS_MPU6000_MAX_DOWNSAMPLE 10
|
||||
static int16_t pios_mpu6000_buffer[PIOS_MPU6000_MAX_DOWNSAMPLE * sizeof(struct pios_mpu6000_data)];
|
||||
static t_fifo_buffer pios_mpu6000_fifo;
|
||||
|
||||
@ -66,9 +66,9 @@ void PIOS_MPU6000_Init(const struct pios_mpu6000_cfg * new_cfg)
|
||||
fifoBuf_init(&pios_mpu6000_fifo, (uint8_t *) pios_mpu6000_buffer, sizeof(pios_mpu6000_buffer));
|
||||
|
||||
/* Configure the MPU6000 Sensor */
|
||||
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_256);
|
||||
PIOS_SPI_SetClockSpeed(pios_spi_gyro, PIOS_SPI_PRESCALER_256);
|
||||
PIOS_MPU6000_Config(cfg);
|
||||
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_8);
|
||||
PIOS_SPI_SetClockSpeed(pios_spi_gyro, PIOS_SPI_PRESCALER_16);
|
||||
|
||||
/* Set up EXTI line */
|
||||
PIOS_EXTI_Init(cfg->exti_cfg);
|
||||
@ -295,10 +295,11 @@ float PIOS_MPU6000_GetAccelScale()
|
||||
* \return 0 if test succeeded
|
||||
* \return non-zero value if test succeeded
|
||||
*/
|
||||
int32_t mpu6000_id;
|
||||
uint8_t PIOS_MPU6000_Test(void)
|
||||
{
|
||||
/* Verify that ID matches (MPU6000 ID is 0x69) */
|
||||
int32_t mpu6000_id = PIOS_MPU6000_ReadID();
|
||||
mpu6000_id = PIOS_MPU6000_ReadID();
|
||||
if(mpu6000_id < 0)
|
||||
return -1;
|
||||
|
||||
@ -371,9 +372,9 @@ void PIOS_MPU6000_IRQHandler(void)
|
||||
}
|
||||
|
||||
PIOS_MPU6000_ReleaseBus();
|
||||
|
||||
|
||||
struct pios_mpu6000_data data;
|
||||
|
||||
|
||||
if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) {
|
||||
mpu6000_fifo_full++;
|
||||
return;
|
||||
@ -395,8 +396,6 @@ void PIOS_MPU6000_IRQHandler(void)
|
||||
|
||||
PIOS_MPU6000_ReleaseBus();
|
||||
|
||||
struct pios_mpu6000_data data;
|
||||
|
||||
if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) {
|
||||
mpu6000_fifo_full++;
|
||||
return;
|
||||
|
||||
@ -88,7 +88,7 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = {
|
||||
.SPI_CRCPolynomial = 7,
|
||||
.SPI_CPOL = SPI_CPOL_High,
|
||||
.SPI_CPHA = SPI_CPHA_2Edge,
|
||||
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, /* 10 Mhz */
|
||||
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */
|
||||
},
|
||||
.use_crc = false,
|
||||
.dma = {
|
||||
@ -390,6 +390,7 @@ void PIOS_SPI_flash_accel_irq_handler(void)
|
||||
/*
|
||||
* ADC system
|
||||
*/
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
#include "pios_adc_priv.h"
|
||||
extern void PIOS_ADC_handler(void);
|
||||
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
|
||||
@ -437,6 +438,7 @@ uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs);
|
||||
void PIOS_ADC_handler() {
|
||||
PIOS_ADC_DMA_Handler();
|
||||
}
|
||||
#endif
|
||||
|
||||
#include "pios_tim_priv.h"
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user