rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

OP-1535 - change sensor rate to 1KHz with software downsample to 500Hz

Browse Source 
Allow per board SPI prescaler settings
This commit is contained in:
Alessio Morale 2014-10-07 11:40:39 +02:00
parent c1062124af
commit 9350bd65d9
7 changed files with 83 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/modules/Attitude/attitude.c
View File

@ -82,7 +82,7 @@ PERF_DEFINE_COUNTER(counterAtt);
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 4
#define SENSOR_PERIOD 2
#define UPDATE_RATE 25.0f
// Interval in number of sample to recalculate temp bias
@ -230,12 +230,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Set critical error and wait until the accel is producing data
while (PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
bool cc3d = BOARDISCC3D;
if (cc3d) {
@ -244,6 +238,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
#endif
} else {
#if defined(PIOS_INCLUDE_ADXL345)
// Set critical error and wait until the accel is producing data
while (PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
accel_test = PIOS_ADXL345_Test();
#endif
@ -265,7 +264,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
settingsUpdatedCb(AttitudeSettingsHandle());
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
portTickType lastSysTime = xTaskGetTickCount();
// Main task loop
while (1) {
FlightStatusData flightStatus;
@ -325,6 +324,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PERF_MEASURE_PERIOD(counterPeriod);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
}
}
@ -462,7 +462,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
#if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue();
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD);
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
while (ret == pdTRUE) {
gyros[0] += mpu6000_data.gyro_x;
gyros[1] += mpu6000_data.gyro_y;

16
flight/modules/Sensors/sensors.c
View File

@ -74,7 +74,15 @@
#define ZERO_ROT_ANGLE 0.00001f
// Private types
#define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterGyroSamples);
PERF_DEFINE_COUNTER(counterSensorPeriod);
// Counters:
// - 0x53000001 Sensor fetch rate(period)
// - 0x53000002 number of gyro samples read for each loop
// Private functions
static void SensorsTask(void *parameters);
@ -230,7 +238,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
vTaskDelay(10);
}
}
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
@ -343,6 +352,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accel_samples++;
}
PERF_MEASURE_PERIOD(counterSensorPeriod);
PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
@ -454,7 +466,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
lastSysTime = xTaskGetTickCount();
}
}

58
flight/pios/common/pios_mpu6000.c
View File

@ -41,7 +41,7 @@ enum pios_mpu6000_dev_magic {
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
};
#define PIOS_MPU6000_MAX_DOWNSAMPLE 2
#define PIOS_MPU6000_MAX_DOWNSAMPLE 5
struct mpu6000_dev {
uint32_t spi_id;
uint32_t slave_num;
@ -63,6 +63,7 @@ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
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);
static void PIOS_MPU6000_SetSpeed(const bool fast);
#define GRAV 9.81f
@ -129,9 +130,7 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios
dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
@ -234,11 +233,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
return -1;
}
// TODO: check that changing the SPI clock speed is safe
// to do here given that interrupts are enabled and the bus has
// not been claimed/is not claimed during this call
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
;
@ -267,7 +261,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
dev->accel_range = accelRange;
#endif
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
return 0;
}
@ -275,7 +268,7 @@ int32_t PIOS_MPU6000_ConfigureRanges(
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_MPU6000_ClaimBus()
static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
@ -283,17 +276,28 @@ static int32_t PIOS_MPU6000_ClaimBus()
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -2;
}
PIOS_MPU6000_SetSpeed(fast_spi);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
static void PIOS_MPU6000_SetSpeed(const bool fast)
{
if (fast) {
PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->fast_prescaler);
} else {
PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->std_prescaler);
}
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
*/
static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken, bool fast_spi)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
@ -301,6 +305,7 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2;
}
PIOS_MPU6000_SetSpeed(fast_spi);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
@ -342,7 +347,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{
uint8_t data;
if (PIOS_MPU6000_ClaimBus() != 0) {
if (PIOS_MPU6000_ClaimBus(false) != 0) {
return -1;
}
@ -363,7 +368,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
*/
static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
{
if (PIOS_MPU6000_ClaimBus() != 0) {
if (PIOS_MPU6000_ClaimBus(false) != 0) {
return -1;
}
@ -393,7 +398,7 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
uint8_t rec[7];
if (PIOS_MPU6000_ClaimBus() != 0) {
if (PIOS_MPU6000_ClaimBus(true) != 0) {
return -1;
}
@ -504,7 +509,7 @@ static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
/* Interrupt Status register can be read at high SPI clock speed */
uint8_t data;
if (PIOS_MPU6000_ClaimBusISR(woken) != 0) {
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
@ -525,29 +530,16 @@ static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
{
int32_t result = 0;
/* Register writes must be at < 1MHz SPI clock.
* Speed can only be changed when SPI bus semaphore is held, but
* device chip select must not be enabled, so we use the direct
* SPI bus claim call here */
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
/* Reduce SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
/* Enable chip select */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
/* Reset FIFO. */
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
result = -2;
} else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
result = -2;
}
/* Disable chip select. */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
/* Increase SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Release the SPI bus semaphore. */
PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
PIOS_MPU6000_ReleaseBusISR(woken);
return result;
}
@ -562,7 +554,7 @@ static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
uint8_t mpu6000_rec_buf[3];
if (PIOS_MPU6000_ClaimBusISR(woken) != 0) {
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
@ -630,7 +622,7 @@ bool PIOS_MPU6000_IRQHandler(void)
return woken;
}
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
if (PIOS_MPU6000_ClaimBusISR(&woken, true) != 0) {
return woken;
}
@ -650,7 +642,7 @@ bool PIOS_MPU6000_IRQHandler(void)
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
if (PIOS_MPU6000_ClaimBusISR(&woken, true) != 0) {
return woken;
}

2
flight/pios/inc/pios_mpu6000.h
View File

@ -159,6 +159,8 @@ struct pios_mpu6000_cfg {
enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_filter filter;
enum pios_mpu6000_orientation orientation;
SPIPrescalerTypeDef fast_prescaler;
SPIPrescalerTypeDef std_prescaler;
};
/* Public Functions */

18
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -119,18 +119,20 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
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 16 for 500 Hz
// Clock at 8 khz, downsampled by 8 for 1000 Hz
.Smpl_rate_div_no_dlp = 15,
// Clock at 1 khz, downsampled by 2 for 500 Hz
// Clock at 1 khz, downsampled by 2 for 1000 Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG
.orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_16,
.std_prescaler = PIOS_SPI_PRESCALER_64
};
#endif /* PIOS_INCLUDE_MPU6000 */

20
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -192,18 +192,20 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
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 12 for 666Hz
.Smpl_rate_div_no_dlp = 11,
// with dlp on output rate is 500Hz
// Clock at 8 khz, downsampled by 8 for 1000Hz
.Smpl_rate_div_no_dlp = 15,
// with dlp on output rate is 1000Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG
.orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_16,
.std_prescaler = PIOS_SPI_PRESCALER_256
};
#endif /* PIOS_INCLUDE_MPU6000 */

22
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -222,18 +222,20 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
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 12 for 666Hz
.Smpl_rate_div_no_dlp = 11,
// with dlp on output rate is 500Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
// Clock at 8 khz, downsampled by 8 for 1000Hz
.Smpl_rate_div_no_dlp = 7,
// with dlp on output rate is 1000Hz
.Smpl_rate_div_dlp = 0,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_0DEG
.orientation = PIOS_MPU6000_TOP_0DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_16,
.std_prescaler = PIOS_SPI_PRESCALER_256
};
#endif /* PIOS_INCLUDE_MPU6000 */