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

Update the bootloader hw version for the mpu6000 board and make the sensor code

Browse Source 
and initialization respond appropriately.  Enable hte L3GD20 and BMA180.
Let the board type determine which is used.
This commit is contained in:
James Cotton 2012-03-21 02:00:32 -05:00
parent e7ca02c038
commit 2f8049e20a
4 changed files with 159 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -60,6 +60,8 @@
#include "baroaltitude.h"
#include "CoordinateConversions.h"
#include <pios_board_info.h>
// Private constants
#define STACK_SIZE_BYTES 1540
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
@ -152,8 +154,8 @@ static void SensorsTask(void *parameters)
uint32_t gyro_samples;
int32_t accel_accum[3] = {0, 0, 0};
int32_t gyro_accum[3] = {0,0,0};
float gyro_scaling;
float accel_scaling;
float gyro_scaling = 0;
float accel_scaling = 0;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
@ -161,17 +163,27 @@ static void SensorsTask(void *parameters)
UAVObjEvent ev;
settingsUpdatedCb(&ev);
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
#if !defined(PIOS_INCLUDE_BMA180)
accel_test = gyro_test;
#endif
#elif defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
const struct pios_board_info * bdinfo = &pios_board_info_blob;
switch(bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
#endif
#if defined(PIOS_INCLUDE_BMA180)
accel_test = PIOS_BMA180_Test();
accel_test = PIOS_BMA180_Test();
#endif
break;
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
accel_test = gyro_test;
#endif
break;
default:
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_HMC5883)
mag_test = PIOS_HMC5883_Test();
#endif
@ -219,121 +231,121 @@ static void SensorsTask(void *parameters)
accel_samples = 0;
gyro_samples = 0;
// Make sure we get one sample
#if !defined(PIOS_MPU6000_ACCEL)
struct pios_bma180_data accel;
AccelsData accelsData;
GyrosData gyrosData;
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstarts
// it.
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
continue;
}
while(read_good == 0) {
count++;
accel_accum[0] += accel.x;
accel_accum[1] += accel.y;
accel_accum[2] += accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
switch(bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
#if defined(PIOS_INCLUDE_BMA180)
{
struct pios_bma180_data accel;
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not
// trigger more interrupts. In this case we must force a read to kickstarts
// it.
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
continue;
}
while(read_good == 0) {
count++;
accel_accum[0] += accel.x;
accel_accum[1] += accel.y;
accel_accum[2] += accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
// Get temp from last reading
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
}
#endif
#if defined(PIOS_INCLUDE_L3GD20)
{
struct pios_l3gd20_data gyro;
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
error = true;
continue;
}
gyro_samples = 1;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
// Using MPU6000 gyro and possibly accel
// Get temp from last reading
gyrosData.temperature = gyro.temperature;
}
#endif
break;
case 0x02: // MPU6000 board
#if defined(PIOS_INCLUDE_MPU6000)
struct pios_mpu6000_data gyro;
{
struct pios_mpu6000_data gyro;
count = 0;
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error)
continue;
while(read_good == 0) {
count++;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
accel_accum[0] += gyro.accel_x;
accel_accum[1] += gyro.accel_y;
accel_accum[2] += gyro.accel_z;
read_good = PIOS_MPU6000_ReadFifo(&gyro);
}
gyro_samples = count;
gyro_scaling = PIOS_MPU6000_GetScale();
accel_samples = count;
accel_scaling = PIOS_MPU6000_GetAccelScale();
count = 0;
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error)
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
if (error)
continue;
while(read_good == 0) {
count++;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
#if defined(PIOS_MPU6000_ACCEL)
accel_accum[0] += gyro.accel_x;
accel_accum[1] += gyro.accel_y;
accel_accum[2] += gyro.accel_z;
#endif
read_good = PIOS_MPU6000_ReadFifo(&gyro);
}
gyro_samples = count;
gyro_scaling = PIOS_MPU6000_GetScale();
#if defined(PIOS_MPU6000_ACCEL)
accel_samples = count;
accel_scaling = PIOS_MPU6000_GetAccelScale();
#endif
// Using L3DG20 gyro
#elif defined(PIOS_INCLUDE_L3GD20)
struct pios_l3gd20_data gyro;
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) {
error = true;
continue;
// Get temp from last reading
gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
accelsData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
default:
PIOS_DEBUG_Assert(0);
}
gyro_samples = 1;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
#else
//#error No gyro defined
struct gyro_data {float x; float y; float z; float temperature;} gyro;
gyro_scaling = 0;
gyro_samples = 1;
#endif
float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
// Not the swaping of channel orders
#if defined(PIOS_MPU6000_ACCEL)
accel_scaling = PIOS_MPU6000_GetAccelScale();
#else
accel_scaling = PIOS_BMA180_GetScale();
#endif
AccelsData accelsData; // Skip get as we set all the fields
// Scale the accels
float accels[3] = {(float) accel_accum[1] / accel_samples,
(float) accel_accum[0] / accel_samples,
-(float) accel_accum[2] / accel_samples};
accelsData.x = accels[0] * accel_scaling * accel_scale[0] - accel_bias[0];
accelsData.y = accels[1] * accel_scaling * accel_scale[1] - accel_bias[1];
accelsData.z = accels[2] * accel_scaling * accel_scale[2] - accel_bias[2];
#if defined(BMA180)
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
#elif defined(PIOS_MPU6000_ACCEL)
accelsData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
#endif
accelsData.temperature =
AccelsSet(&accelsData);
float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples};
GyrosData gyrosData; // Skip get as we set all the fields
// Scale the gyros
float gyros[3] = {(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[0] / gyro_samples,
-(float) gyro_accum[2] / gyro_samples};
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;
#else
gyrosData.temperature = gyro.temperature;
#endif
if (bias_correct_gyro) {
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;
@ -342,17 +354,14 @@ static void SensorsTask(void *parameters)
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
}
GyrosSet(&gyrosData);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
bool mag_updated = false;
MagnetometerData mag;
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
mag_updated = true;
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
mag.x = values[1] * mag_scale[0] - mag_bias[0];
@ -363,59 +372,18 @@ static void SensorsTask(void *parameters)
}
#endif
// For debugging purposes here we can output all of the sensors. Do it as a single transaction
// so the message isn't split if anything else is writing to it
if(pios_com_aux_id != 0) {
uint32_t message_size = 3;
uint8_t message[200] = {0xff, (lastSysTime & 0xff00) >> 8, lastSysTime & 0x00ff};
// Add accel data
memcpy(&message[message_size], &accelsData.x, sizeof(accelsData.x) * 3);
message_size += sizeof(accelsData.x) * 3;
// Add gyro data with temp
memcpy(&message[message_size], &gyrosData, sizeof(gyrosData));
message_size += sizeof(gyrosData);
if(mag_updated) { // Add mag data
message[message_size] = 0x01; // Indicate mag data here
message_size++;
memcpy(&message[message_size], &mag, sizeof(mag));
message_size += sizeof(mag);
}
if(gps_updated) { // Add GPS data
gps_updated = false;
GPSPositionData gps;
GPSPositionGet(&gps);
message[message_size] = 0x02; // Indicate gps data here
message_size++;
memcpy(&message[message_size], &gps, sizeof(gps));
message_size += sizeof(gps);
}
if(baro_updated) { // Add baro data
baro_updated = false;
BaroAltitudeData baro;
BaroAltitudeGet(&baro);
message[message_size] = 0x03; // Indicate mag data here
message_size++;
memcpy(&message[message_size], &baro, sizeof(baro));
message_size += sizeof(baro);
}
PIOS_COM_SendBufferNonBlocking(pios_com_aux_id, message, message_size);
}
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
// For L3GD20 which runs at 760 then one cycle per sample
#if defined(PIOS_INCLUDE_MPU6000) && !defined(PIOS_INCLUDE_L3GD20)
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
#else
lastSysTime = xTaskGetTickCount();
#endif
switch(bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
lastSysTime = xTaskGetTickCount();
break;
case 0x02:
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
break;
default:
PIOS_DEBUG_Assert(0);
}
}
}

4
flight/Revolution/System/inc/pios_config.h
View File

@ -57,11 +57,11 @@
#define PIOS_INCLUDE_WDG
/* Select the sensors to include */
//#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
//#define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_MS5611
//#define PIOS_INCLUDE_HCSR04
#define PIOS_FLASH_ON_ACCEL /* true for second revo */

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

@ -325,7 +325,12 @@ static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
#include <pios_board_info.h>
void PIOS_Board_Init(void) {
const struct pios_board_info * bdinfo = &pios_board_info_blob;
/* Delay system */
PIOS_DELAY_Init();
@ -787,7 +792,7 @@ void PIOS_Board_Init(void) {
PIOS_DEBUG_Assert(0);
}
PIOS_DELAY_WaitmS(500);
PIOS_DELAY_WaitmS(50);
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
@ -797,20 +802,26 @@ void PIOS_Board_Init(void) {
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id);
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Attach(pios_spi_gyro_id);
PIOS_MPU6000_Init(&pios_mpu6000_cfg);
#elif defined(PIOS_INCLUDE_L3GD20)
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
#else
PIOS_Assert(0);
switch(bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
#endif
#if defined(PIOS_INCLUDE_BMA180)
PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0);
PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0);
#endif
break;
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Attach(pios_spi_gyro_id);
PIOS_MPU6000_Init(&pios_mpu6000_cfg);
#endif
break;
default:
PIOS_DEBUG_Assert(0);
}
}

2
make/boards/revolution/board-info.mk
View File

@ -1,5 +1,5 @@
BOARD_TYPE := 0x09
BOARD_REVISION := 0x01
BOARD_REVISION := 0x02
BOOTLOADER_VERSION := 0x01
HW_TYPE := 0x00