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

CC-2 Complimentary filter working on copter contol, nice attitude

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2475 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-17 08:45:43 +00:00 committed by peabody124
parent cf17569de7
commit 56dd0569b8
2 changed files with 85 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/CopterControl/Makefile
View File

@ -223,6 +223,7 @@ SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c

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

@ -51,14 +51,18 @@
#include "pios.h"
#include "ccattitude.h"
#include "attituderaw.h"
#include "attitudeactual.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 340
#define STACK_SIZE_BYTES 740
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_RATE 10 /* ms */
#define GYRO_NEUTRAL 1665
#define GYRO_SCALE 0.014f
#define GYRO_SCALE 0.010f
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
// Private types
// Private variables
@ -66,6 +70,8 @@ static xTaskHandle taskHandle;
// Private functions
static void CCAttitudeTask(void *parameters);
void updateSensors();
void updateAttitude();
/**
* Initialise the module, called on startup
@ -97,19 +103,31 @@ static void CCAttitudeTask(void *parameters)
while (1) {
//PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS);
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
updateSensors();
updateAttitude();
/* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, UPDATE_RATE / portTICK_RATE_MS);
}
}
void updateSensors()
{
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
struct pios_adxl345_data accel_data;
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_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);
@ -123,13 +141,54 @@ static void CCAttitudeTask(void *parameters)
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, UPDATE_RATE / portTICK_RATE_MS);
}
}
#define UPDATE_FRAC 0.99f
void updateAttitude()
{
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
static portTickType lastSysTime = 0;
static portTickType thisSysTime;
float accel_pitch, accel_roll;
float dT;
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Convert into radians
attitudeActual.Roll = attitudeActual.Roll * M_PI / 180;
attitudeActual.Pitch = attitudeActual.Pitch * M_PI / 180;
attitudeActual.Yaw = attitudeActual.Yaw * M_PI / 180;
// Integrate gyros
attitudeActual.Roll = PI_MOD(attitudeActual.Roll + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] * dT);
attitudeActual.Pitch = PI_MOD(attitudeActual.Pitch + attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] * dT);
attitudeActual.Yaw += fmod(attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] * dT, 2 * M_PI);
// Compute gravity sense of ground
accel_roll = atan2(-attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y],
-attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z]);
accel_pitch = atan2(attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X],
-attitudeRaw.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z]);
// Compute quaternion
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
// Weighted average and back into degrees
attitudeActual.Roll = (UPDATE_FRAC * attitudeActual.Roll + (1-UPDATE_FRAC) * accel_roll) * 180 / M_PI;
attitudeActual.Pitch = (UPDATE_FRAC * attitudeActual.Pitch + (1-UPDATE_FRAC) * accel_pitch) * 180 / M_PI;
attitudeActual.Yaw = attitudeActual.Yaw * 180 / M_PI;
AttitudeActualSet(&attitudeActual);
}
/**
* @}
* @}