rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Getting data from gyro.

Browse Source
This commit is contained in:
James Cotton 2012-01-08 14:25:11 +01:00
parent 147cd7eede
commit 35025303cd
4 changed files with 52 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/Modules/Sensors/sensors.c
View File

@ -244,10 +244,10 @@ static void SensorsTask(void *parameters)
#endif
// Using L3DG20 gyro
#elif 0 && defined(PIOS_INCLUDE_L3GD20)
#elif defined(PIOS_INCLUDE_L3GD20)
struct pios_l3gd20_data gyro;
count = 0;
while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0);
/*while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0);
while(read_good == 0) {
count++;
@ -256,7 +256,14 @@ static void SensorsTask(void *parameters)
gyro_accum[2] += gyro.gyro_z;
read_good = PIOS_L3GD20_ReadFifo(&gyro);
}
} */
PIOS_L3GD20_ReadGyros(&gyro);
gyro_accum[0] = gyro.gyro_x;
gyro_accum[1] = gyro.gyro_y;
gyro_accum[2] = gyro.gyro_z;
count = 1;
gyro_samples = count;
gyro_scaling = PIOS_L3GD20_GetScale();
@ -287,8 +294,11 @@ static void SensorsTask(void *parameters)
gyrosData.x = gyros[0] * gyro_scaling;
gyrosData.y = gyros[1] * gyro_scaling;
gyrosData.z = gyros[2] * gyro_scaling;
#if defined(PIOS_INCLUDE_MPU6000)
gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
gyrosData.temperature = PIOS_L3GD20_ReadID();
#else
gyrosData.temperature = gyro.temperature;
#endif
if (bias_correct_gyro) {
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;

109
flight/PiOS/STM32F4xx/pios_l3gd20.c
View File

@ -66,7 +66,7 @@ void PIOS_L3GD20_Init(const struct pios_l3gd20_cfg * new_cfg)
/* Configure the MPU6050 Sensor */
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_256);
PIOS_L3GD20_Config(cfg);
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_8);
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_16);
/* Configure EOC pin as input floating */
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
@ -88,9 +88,25 @@ void PIOS_L3GD20_Init(const struct pios_l3gd20_cfg * new_cfg)
*/
static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg)
{
PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST |
PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN |
// This register enables the channels and sets the bandwidth
PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST |
PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN |
PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN);
// Disable the high pass filters
PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0);
// Set int2 to go high on data ready
PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08);
// Select SPI interface, 500 deg/s, endianness?
PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, 0x10);
// Enable FIFO, disable HPF
PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40);
// Fifo stream mode
PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40);
}
/**
@ -175,22 +191,29 @@ static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \returns The number of samples remaining in the fifo
*/
uint32_t l3gd20_irq = 0;
int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * data)
{
uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
if(PIOS_L3GD20_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0)
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
data->gyro_x = 0;
data->gyro_y = 0;
data->gyro_z = 0;
data->temperature = 0;
return -2;
}
PIOS_L3GD20_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
data->gyro_z = rec[5] << 8 | rec[6];
memcpy((uint8_t *) &(data->gyro_x), &rec[1], 6);
data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP);
return 0;
}
@ -287,86 +310,26 @@ static int32_t PIOS_L3GD20_FifoDepth(void)
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
uint32_t l3gd20_irq = 0;
int32_t l3gd20_count;
uint32_t l3gd20_fifo_full = 0;
uint8_t l3gd20_last_read_count = 0;
uint32_t l3gd20_fails = 0;
uint32_t l3gd20_interval_us;
uint32_t l3gd20_time_us;
uint32_t l3gd20_transfer_size;
void PIOS_L3GD20_IRQHandler(void)
{
/* static uint32_t timeval;
l3gd20_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if(!l3gd20_configured)
return;
l3gd20_count = PIOS_L3GD20_FifoDepth();
if(l3gd20_count < sizeof(struct pios_l3gd20_data))
return;
if(PIOS_L3GD20_ClaimBus() != 0)
return;
uint8_t l3gd20_send_buf[1+sizeof(struct pios_l3gd20_data)] = {PIOS_L3GD20_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t l3gd20_rec_buf[1+sizeof(struct pios_l3gd20_data)];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &l3gd20_send_buf[0], &l3gd20_rec_buf[0], sizeof(l3gd20_send_buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
l3gd20_fails++;
return;
}
PIOS_L3GD20_ReleaseBus();
/*
struct pios_l3gd20_data data;
PIOS_L3GD20_ReadGyros(&data);
data.temperature = l3gd20_irq;
if(fifoBuf_getFree(&pios_l3gd20_fifo) < sizeof(data)) {
l3gd20_fifo_full++;
return;
}
// In the case where extras samples backed up grabbed an extra
if (l3gd20_count >= (sizeof(data) * 2)) {
if(PIOS_L3GD20_ClaimBus() != 0)
return;
uint8_t l3gd20_send_buf[1+sizeof(struct pios_l3gd20_data)] = {PIOS_L3GD20_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t l3gd20_rec_buf[1+sizeof(struct pios_l3gd20_data)];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &l3gd20_send_buf[0], &l3gd20_rec_buf[0], sizeof(l3gd20_send_buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
l3gd20_fails++;
return;
}
PIOS_L3GD20_ReleaseBus();
struct pios_l3gd20_data data;
if(fifoBuf_getFree(&pios_l3gd20_fifo) < sizeof(data)) {
l3gd20_fifo_full++;
return;
}
}
data.temperature = l3gd20_rec_buf[1] << 8 | l3gd20_rec_buf[2];
data.gyro_x = l3gd20_rec_buf[3] << 8 | l3gd20_rec_buf[4];
data.gyro_y = l3gd20_rec_buf[5] << 8 | l3gd20_rec_buf[6];
data.gyro_z = l3gd20_rec_buf[7] << 8 | l3gd20_rec_buf[8];
fifoBuf_putData(&pios_l3gd20_fifo, (uint8_t *) &data, sizeof(data));
*/
l3gd20_irq++;
l3gd20_time_us = PIOS_DELAY_DiffuS(timeval);
*/
}
#endif /* L3GD20 */

5
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -8,9 +8,8 @@
/* Begin PBXFileReference section */
65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = "<group>"; };
6502584212CA4D2600583CDF /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps13state.c; path = ../../AHRS/insgps13state.c; sourceTree = SOURCE_ROOT; };
650387E414B52B680045AFE4 /* pios_l3gd20.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_l3gd20.c; sourceTree = "<group>"; };
650387E514B52B860045AFE4 /* pios_l3gd20.h */ = {isa = PBXFileReference; fileEncoding = 4; path = pios_l3gd20.h; sourceTree = "<group>"; };
650387E514B52B860045AFE4 /* pios_l3gd20.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_l3gd20.h; sourceTree = "<group>"; };
65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = "<group>"; };
650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = "<group>"; };
650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = "<group>"; };
@ -82,7 +81,6 @@
65345C871288668B00A5E4E8 /* guidancesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = guidancesettings.xml; sourceTree = "<group>"; };
6534B5571474F78B003DF47C /* pios_mpu6000.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_mpu6000.h; sourceTree = "<group>"; };
6534B5581474F7B1003DF47C /* pios_mpu6000.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_mpu6000.c; sourceTree = "<group>"; };
6534B559147634FF003DF47C /* pios_flash_m25p16.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_flash_m25p16.h; sourceTree = "<group>"; };
6534B55A14763566003DF47C /* pios_flash_m25p16.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_flash_m25p16.c; sourceTree = "<group>"; };
6534B55B1476D3A8003DF47C /* pios_ms5611.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_ms5611.h; sourceTree = "<group>"; };
6536D47B1307962C0042A298 /* stabilizationdesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = stabilizationdesired.xml; sourceTree = "<group>"; };
@ -8445,7 +8443,6 @@
65E8F03E11EFF25C00BBF654 /* pios_debug.h */,
65E8F03F11EFF25C00BBF654 /* pios_delay.h */,
65E8F04011EFF25C00BBF654 /* pios_exti.h */,
6534B559147634FF003DF47C /* pios_flash_m25p16.h */,
6512D60512ED4CA2008175E5 /* pios_flash_w25x.h */,
65FF4D61137EFA4F00146BE4 /* pios_flashfs_objlist.h */,
65FA9B7C14709E9E0019A260 /* pios_gcsrcvr_priv.h */,

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

@ -1709,7 +1709,7 @@ void PIOS_Board_Init(void) {
#elif defined(PIOS_INCLUDE_L3GD20)
PIOS_L3GD20_Attach(pios_spi_gyro_id);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
// PIOS_L3GD20_Init(&pios_l3gd20_cfg);
PIOS_L3GD20_Init(&pios_l3gd20_cfg);
#else
PIOS_Assert(0);
#endif