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

CC-1 Modules/CCAttitude: Initial reads from accelerometer

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2472 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-17 08:45:34 +00:00 committed by peabody124
parent 2e3c20b12f
commit 211585c7d7
2 changed files with 44 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/CopterControl/System/pios_board.c
View File

@ -102,7 +102,7 @@ const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32,
},
.use_crc = TRUE,
.dma = {

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

@ -85,9 +85,44 @@ static void CCAttitudeTask(void *parameters)
// AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
// 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);
// 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);
// TODO: register the adc callback, push the data onto a queue (safe for thread)
// with the queue ISR version
@ -96,10 +131,16 @@ static void CCAttitudeTask(void *parameters)
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;
AttitudeRawSet(&attitudeRaw);
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, 100 / portTICK_RATE_MS);
vTaskDelayUntil(&lastSysTime, 10 / portTICK_RATE_MS);
}
}