diff --git a/flight/Modules/CCAttitude/ccattitude.c b/flight/Modules/CCAttitude/ccattitude.c index 24d227047..be11e51dc 100644 --- a/flight/Modules/CCAttitude/ccattitude.c +++ b/flight/Modules/CCAttitude/ccattitude.c @@ -56,6 +56,9 @@ #define STACK_SIZE_BYTES 340 #define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define UPDATE_RATE 10 /* ms */ +#define GYRO_NEUTRAL 1665 +#define GYRO_SCALE 0.014f // Private types // Private variables @@ -88,59 +91,41 @@ static void CCAttitudeTask(void *parameters) // Keep flash CS pin high while talking accel PIOS_FLASH_DISABLE; - uint8_t out_buf[16]; - uint8_t in_buf[16]; - - out_buf[0] = 0x2C; // configure rate - out_buf[1] = 0x0C; // 200 Hz BW (400 Hz clock), not low power - - out_buf[2] = 0x2D; // select power control - out_buf[3] = 0x08; // active - - out_buf[4] = 0x38; // configure FIFO - out_buf[5] = 0x90; // stream mode, watermark at 16 samples - - out_buf[6] = 0x31; // data format - out_buf[7] = 0x02; // 8 g range, 4-wire spi - - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0); - PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,out_buf,in_buf,8,NULL); - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,1); + PIOS_ADXL345_Init(); // Main task loop while (1) { //PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS); - //int16_t accel_x,accel_y,accel_z; - out_buf[0] = 0x32 | 0x80 | 0x40; // Multibyte read starting at X0 - out_buf[1] = 0; - out_buf[2] = 0; - out_buf[3] = 0; - out_buf[4] = 0; - out_buf[5] = 0; - out_buf[6] = 0; - - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0); - PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,out_buf,in_buf,7,NULL); - PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,1); + struct pios_adxl345_data accel_data; // TODO: register the adc callback, push the data onto a queue (safe for thread) // with the queue ISR version AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); - attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = PIOS_ADC_PinGet(0); - attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = PIOS_ADC_PinGet(1); - attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = PIOS_ADC_PinGet(2); - attitudeRaw.accels[ATTITUDERAW_ACCELS_X] = (in_buf[2] << 8) + in_buf[1]; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] = (in_buf[4] << 8) + in_buf[3]; - attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] = (in_buf[6] << 8) + in_buf[5]; - attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = (float) attitudeRaw.accels[ATTITUDERAW_ACCELS_X] / 256 * 9.81; - attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = (float) attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] / 256 * 9.81; - attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = (float) attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] / 256 * 9.81; + + attitudeRaw.gyros[ATTITUDERAW_GYROS_X] = PIOS_ADC_PinGet(0); + attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] = PIOS_ADC_PinGet(1); + attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] = PIOS_ADC_PinGet(2); + + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_X] - GYRO_NEUTRAL) * GYRO_SCALE; + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_Y] - GYRO_NEUTRAL) * GYRO_SCALE; + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = (attitudeRaw.gyros[ATTITUDERAW_GYROS_Z] - GYRO_NEUTRAL) * GYRO_SCALE; + + attitudeRaw.gyrotemp[0] = PIOS_ADXL345_Read(&accel_data); + attitudeRaw.gyrotemp[1] = PIOS_ADC_PinGet(3); + + attitudeRaw.accels[ATTITUDERAW_ACCELS_X] = accel_data.x; + attitudeRaw.accels[ATTITUDERAW_ACCELS_Y] = accel_data.y; + attitudeRaw.accels[ATTITUDERAW_ACCELS_Z] = accel_data.z; + + attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = (float) accel_data.x * 0.004f * 9.81; + attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = (float) accel_data.y * 0.004f * 9.81; + attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = (float) accel_data.z * 0.004f * 9.81; AttitudeRawSet(&attitudeRaw); /* Wait for the next update interval */ - vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS); + vTaskDelayUntil(&lastSysTime, UPDATE_RATE / portTICK_RATE_MS); } } diff --git a/flight/PiOS/Common/pios_adxl345.c b/flight/PiOS/Common/pios_adxl345.c new file mode 100644 index 000000000..545579598 --- /dev/null +++ b/flight/PiOS/Common/pios_adxl345.c @@ -0,0 +1,113 @@ +/* + * pios_adxl345.c + * OpenPilotOSX + * + * Created by James Cotton on 1/16/11. + * Copyright 2011 OpenPilot. All rights reserved. + * + */ + +#include "pios.h" + +/** + * @brief Claim the SPI bus for the accel communications and select this chip + */ +void PIOS_ADXL345_ClaimBus() +{ + // TODO: Semaphore to lock bus + PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0); + PIOS_DELAY_WaituS(1); + +} + +/** + * @brief Release the SPI bus for the accel communications and end the transaction + */ +void PIOS_ADXL345_ReleaseBus() +{ + // TODO: Release semaphore + PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,1); + PIOS_DELAY_WaituS(1); +} + +/** + * @brief Select the sampling rate of the chip + * + * This also puts it into high power mode + */ +void PIOS_ADXL345_SelectRate(uint8_t rate) +{ + uint8_t out[2] = {ADXL_RATE_ADDR, rate & 0x0F}; + PIOS_ADXL345_ClaimBus(); + PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,out,NULL,sizeof(out),NULL); + PIOS_ADXL345_ReleaseBus(); +} + +/** + * @brief Set the range of the accelerometer and set the data to be right justified + * with sign extension. Also keep device in 4 wire mode. + */ +void PIOS_ADXL345_SetRange(uint8_t range) +{ + uint8_t out[2] = {ADXL_FORMAT_ADDR, (range & 0x03) | ADXL_FULL_RES | ADXL_4WIRE}; + PIOS_ADXL345_ClaimBus(); + PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,out,NULL,sizeof(out),NULL); + PIOS_ADXL345_ReleaseBus(); +} + +/** + * @brief Set the fifo depth that triggers an interrupt. This will be matched to the oversampling + */ +void PIOS_ADXL345_FifoDepth(uint8_t depth) +{ + uint8_t out[2] = {ADXL_FIFO_ADDR, (depth & 0x1f) | ADXL_FIFO_STREAM}; + PIOS_ADXL345_ClaimBus(); + PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,out,NULL,sizeof(out),NULL); + PIOS_ADXL345_ReleaseBus(); +} + +/** + * @brief Enable measuring. This also disables the activity sensors (tap or free fall) + */ +void PIOS_ADXL345_SetMeasure(uint8_t enable) +{ + uint8_t out[2] = {ADXL_POWER_ADDR, ADXL_MEAURE}; + PIOS_ADXL345_ClaimBus(); + PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,out,NULL,sizeof(out),NULL); + PIOS_ADXL345_ReleaseBus(); +} + +/** + * @brief Initialize with good default settings + */ +void PIOS_ADXL345_Init() +{ + PIOS_ADXL345_ReleaseBus(); + PIOS_ADXL345_SelectRate(ADXL_RATE_400); + PIOS_ADXL345_SetRange(ADXL_RANGE_8G); + PIOS_ADXL345_FifoDepth(16); + PIOS_ADXL345_SetMeasure(1); +} + +/** + * @brief Read a single set of values from the x y z channels + * @returns The number of samples remaining in the fifo + */ +uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data) +{ + // 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}; + buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT ; // Multibyte read starting at X0 + + PIOS_ADXL345_ClaimBus(); + PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],9,NULL); + PIOS_ADXL345_ReleaseBus(); + + data->x = rec[1] + (rec[2] << 8); + data->y = rec[3] + (rec[4] << 8); + data->z = rec[5] + (rec[6] << 8); + + return rec[8] & 0x7F; // return number of remaining entries +} \ No newline at end of file diff --git a/flight/PiOS/inc/pios_adxl345.h b/flight/PiOS/inc/pios_adxl345.h new file mode 100644 index 000000000..c2a08b856 --- /dev/null +++ b/flight/PiOS/inc/pios_adxl345.h @@ -0,0 +1,52 @@ +/* + * pios_adxl345.h + * OpenPilotOSX + * + * Created by James Cotton on 1/16/11. + * Copyright 2011 OpenPilot. All rights reserved. + * + */ + +#ifndef PIOS_ADXL345_H +#define PIOS_ADXL345_H + +// Defined by data rate, not BW + +#define ADXL_READ_BIT 0x80 +#define ADXL_MULTI_BIT 0x40 + +#define ADXL_X0_ADDR 0x32 + +#define ADXL_RATE_ADDR 0x2C +#define ADXL_RATE_100 0x0A +#define ADXL_RATE_200 0x0B +#define ADXL_RATE_400 0x0C + +#define ADXL_POWER_ADDR 0x2D +#define ADXL_MEAURE 0x08 + +#define ADXL_FORMAT_ADDR 0x31 +#define ADXL_FULL_RES 0x08 +#define ADXL_4WIRE 0x00 +#define ADXL_RANGE_2G 0x00 +#define ADXL_RANGE_4G 0x01 +#define ADXL_RANGE_8G 0x02 +#define ADXL_RANGE_16G 0x03 + +#define ADXL_FIFO_ADDR 0x38 +#define ADXL_FIFO_STREAM 0x80 + + +struct pios_adxl345_data { + int16_t x; + int16_t y; + int16_t z; +}; + +void PIOS_ADXL345_SelectRate(uint8_t rate); +void PIOS_ADXL345_SetRange(uint8_t range); +void PIOS_ADXL345_FifoDepth(uint8_t depth); +void PIOS_ADXL345_Init(); +uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data); + +#endif \ No newline at end of file