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

OP-1227 Fix high CPU usage: Reduce Accel rate to 1600Hz, optimize sample reading, explicitly set bus speed

Browse Source 
Note: Counter 0xA3450001 is the time it takes to transfer the requested number of samples
This commit is contained in:
Alessio Morale 2014-06-08 21:33:18 +02:00
parent c9d97639b0
commit a0d68c6afb
2 changed files with 58 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
flight/pios/common/pios_adxl345.c
View File

@ -41,6 +41,12 @@ struct adxl345_dev {
uint32_t slave_num;
enum pios_adxl345_dev_magic magic;
};
#undef PIOS_INCLUDE_INSTRUMENTATION
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
static int8_t counterUpd;
// Counter 0xA3450001 time it takes to transfer the requested amount of samples using PIOS_ADXL345_ReadAndAccumulateSamples
#endif
// ! Global structure for this device device
static struct adxl345_dev *dev;
@ -59,6 +65,9 @@ static struct adxl345_dev *PIOS_ADXL345_alloc(void)
{
struct adxl345_dev *adxl345_dev;
#ifdef PIOS_INCLUDE_INSTRUMENTATION
counterUpd = PIOS_Instrumentation_CreateCounter(0xA3450001);
#endif
adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev));
if (!adxl345_dev) {
return NULL;
@ -99,7 +108,7 @@ static int32_t PIOS_ADXL345_ClaimBus()
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -2;
}
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_8);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
@ -237,7 +246,7 @@ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num)
dev->slave_num = slave_num;
PIOS_ADXL345_ReleaseBus();
PIOS_ADXL345_SelectRate(ADXL_RATE_3200);
PIOS_ADXL345_SelectRate(ADXL_RATE_1600);
PIOS_ADXL345_SetRange(ADXL_RANGE_8G);
PIOS_ADXL345_FifoDepth(16);
PIOS_ADXL345_SetMeasure(1);
@ -299,6 +308,7 @@ int32_t PIOS_ADXL345_FifoElements()
return rec[1] & 0x3f;
}
/**
* @brief Read a single set of values from the x y z channels
* @returns The number of samples remaining in the fifo
@ -312,13 +322,11 @@ uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data)
if (PIOS_ADXL345_ClaimBus() != 0) {
return -2;
}
// To save memory use same buffer for in and out but offset by
// a byte
uint8_t buf[9] = { 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t rec[9] = { 0, 0, 0, 0, 0, 0, 0, 0 };
uint8_t buf[10] = { 0 };
uint8_t *rec = buf + 1;
buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0
if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 9, NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
@ -333,4 +341,47 @@ uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data)
return rec[8] & 0x7F; // return number of remaining entries
}
/**
* @brief Read a single set of values from the x y z channels
* @returns 0 if successful
*/
int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uint8_t samples)
{
if (PIOS_ADXL345_Validate(dev) != 0) {
return -1;
}
if (PIOS_ADXL345_ClaimBus() != 0) {
return -2;
}
data->x = 0;
data->y = 0;
data->z = 0;
// To save memory use same buffer for in and out but offset by
// a byte
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeStart(counterUpd);
#endif
for (uint8_t i = 0; i < samples; i++) {
uint8_t buf[7] = { 0 };
uint8_t *rec = &buf[1];
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0
if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 7, NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
data->x += rec[1] + (rec[2] << 8);
data->y += rec[3] + (rec[4] << 8);
data->z += rec[5] + (rec[6] << 8);
}
uint8_t r = PIOS_SPI_TransferByte(dev->spi_id, ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT);
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeEnd(counterUpd);
#endif
PIOS_ADXL345_ReleaseBus();
return r & 0x7F; // return number of remaining entries
}
#endif /* PIOS_INCLUDE_ADXL345 */

1
flight/pios/inc/pios_adxl345.h
View File

@ -72,6 +72,7 @@ int32_t PIOS_ADXL345_SelectRate(uint8_t rate);
int32_t PIOS_ADXL345_SetRange(uint8_t range);
int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num);
uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data);
int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uint8_t samples);
int32_t PIOS_ADXL345_FifoElements();
int32_t PIOS_ADXL345_Test();