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

CC-1 Modules/CCAtttitude: Calling the ADXL345 library and getting the accel and

Browse Source 
gyro data into attitude raw.  Hardcoded calibration for now.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2474 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-17 08:45:40 +00:00 committed by peabody124
parent 18deba48c1
commit cf17569de7
3 changed files with 190 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
flight/Modules/CCAttitude/ccattitude.c
View File

@ -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);
}
}

113
flight/PiOS/Common/pios_adxl345.c Normal file
View File

@ -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
}

52
flight/PiOS/inc/pios_adxl345.h Normal file
View File

@ -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