rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

OP-1658 - Fixes for CC3D and its attitude module

Browse Source
This commit is contained in:
Alessio Morale 2015-01-11 17:09:11 +01:00
parent 4f6c7dcb00
commit 0cf5d163d1
2 changed files with 32 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
flight/modules/Attitude/attitude.c
View File

@ -62,6 +62,10 @@
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "taskinfo.h" #include "taskinfo.h"
#include <pios_sensors.h>
#include <pios_adxl345.h>
#include <pios_mpu6000.h>
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_notify.h> #include <pios_notify.h>
#include <mathmisc.h> #include <mathmisc.h>
@ -160,9 +164,11 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f #define STD_CC_ANALOG_GYRO_GAIN 0.42f
static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL;
// Used to detect CC vs CC3D // Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob; static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02) #define BOARDISCC3D (bdinfo->board_rev == 0x02)
/** /**
* Initialise the module, called on startup * Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed * \returns 0 on success or -1 if initialisation failed
@ -238,7 +244,9 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
if (cc3d) { if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000) #if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
gyro_test = PIOS_MPU6000_Driver.test(0);
mpu6000_data = pios_malloc(sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * 2);
#endif #endif
} else { } else {
#if defined(PIOS_INCLUDE_ADXL345) #if defined(PIOS_INCLUDE_ADXL345)
@ -455,7 +463,6 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
* @param[in] attitudeRaw Populate the UAVO instead of saving right here * @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not * @return 0 if successfull, -1 if not
*/ */
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData) static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{ {
float accels[3] = { 0 }; float accels[3] = { 0 };
@ -465,18 +472,18 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
#if defined(PIOS_INCLUDE_MPU6000) #if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue(); xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0);
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms); BaseType_t ret = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms);
while (ret == pdTRUE) { while (ret == pdTRUE) {
gyros[0] += mpu6000_data.gyro_x; gyros[0] += mpu6000_data->sample[1].x;
gyros[1] += mpu6000_data.gyro_y; gyros[1] += mpu6000_data->sample[1].y;
gyros[2] += mpu6000_data.gyro_z; gyros[2] += mpu6000_data->sample[1].z;
accels[0] += mpu6000_data.accel_x; accels[0] += mpu6000_data->sample[0].x;
accels[1] += mpu6000_data.accel_y; accels[1] += mpu6000_data->sample[0].y;
accels[2] += mpu6000_data.accel_z; accels[2] += mpu6000_data->sample[0].z;
temp += mpu6000_data.temperature; temp += mpu6000_data->temperature;
count++; count++;
// check if further samples are already in queue // check if further samples are already in queue
@ -751,17 +758,19 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max; temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
if (BOARDISCC3D) { if (BOARDISCC3D) {
float scales[2];
PIOS_MPU6000_Driver.get_scale(scales, 2, 0);
accel_bias.X = accelGyroSettings.accel_bias.X; accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y; accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z; accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale(); gyro_scale.X = accelGyroSettings.gyro_scale.X * scales[0];
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale(); gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales[0];
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale(); gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales[0];
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale(); accel_scale.X = accelGyroSettings.accel_scale.X * scales[1];
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale(); accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[1];
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale(); accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[1];
} else { } else {
// Original CC with analog gyros and ADXL accel // Original CC with analog gyros and ADXL accel
accel_bias.X = accelGyroSettings.accel_bias.X; accel_bias.X = accelGyroSettings.accel_bias.X;

6
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -36,6 +36,10 @@
#ifdef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h> #include <pios_instrumentation.h>
#endif #endif
#if defined(PIOS_INCLUDE_ADXL345)
#include <pios_adxl345.h>
#endif
/* /*
* Pull in the board-specific static HW definitions. * Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of * Including .c files is a bit ugly but this allows all of
@ -831,7 +835,7 @@ void PIOS_Board_Init(void)
} }
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure(); PIOS_MPU6000_CONFIG_Configure();
init_test = PIOS_MPU6000_Test(); init_test = !PIOS_MPU6000_Driver.test(0);
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */
break; break;