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

Make the revo attitude now use the separate gyro object. CopterControl

Browse Source 
attitude still needs an update.
This commit is contained in:
James Cotton 2011-12-12 13:28:04 -06:00
parent d22e0e66dd
commit 53cb5b67d1
2 changed files with 67 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
flight/Modules/Attitude/revolution/attitude.c
View File

@ -55,7 +55,6 @@
#include "gyros.h" #include "gyros.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "baroaltitude.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
@ -80,10 +79,8 @@ const uint32_t SENSOR_QUEUE_SIZE = 10;
static void SensorTask(void *parameters); static void SensorTask(void *parameters);
static void AttitudeTask(void *parameters); static void AttitudeTask(void *parameters);
static float gyro_correct_int[3] = {0,0,0}; static int32_t updateSensors();
static int32_t updateAttitudeComplimentary();
static int8_t updateSensors(AttitudeRawData *);
static void updateAttitude();
static void settingsUpdatedCb(UAVObjEvent * objEv); static void settingsUpdatedCb(UAVObjEvent * objEv);
static float accelKi = 0; static float accelKi = 0;
@ -94,7 +91,10 @@ static int16_t accelbias[3];
static float R[3][3]; static float R[3][3];
static int8_t rotate = 0; static int8_t rotate = 0;
static bool zero_during_arming = false; static bool zero_during_arming = false;
// These values are initialized by settings but can be updated by the attitude algorithm
static bool bias_correct_gyro = true; static bool bias_correct_gyro = true;
static float gyro_bias[3] = {0,0,0};
/** /**
* API for sensor fusion algorithms: * API for sensor fusion algorithms:
@ -133,9 +133,10 @@ int32_t AttitudeStart(void)
int32_t AttitudeInitialize(void) int32_t AttitudeInitialize(void)
{ {
AttitudeActualInitialize(); AttitudeActualInitialize();
AttitudeRawInitialize(); GyrosInitialize();
AccelsInitialize();
MagnetometerInitialize();
AttitudeSettingsInitialize(); AttitudeSettingsInitialize();
BaroAltitudeInitialize();
// Initialize quaternion // Initialize quaternion
AttitudeActualData attitude; AttitudeActualData attitude;
@ -147,9 +148,9 @@ int32_t AttitudeInitialize(void)
AttitudeActualSet(&attitude); AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs // Cannot trust the values to init right above if BL runs
gyro_correct_int[0] = 0; gyro_bias[0] = 0;
gyro_correct_int[1] = 0; gyro_bias[1] = 0;
gyro_correct_int[2] = 0; gyro_bias[2] = 0;
for(uint8_t i = 0; i < 3; i++) for(uint8_t i = 0; i < 3; i++)
for(uint8_t j = 0; j < 3; j++) for(uint8_t j = 0; j < 3; j++)
@ -218,15 +219,10 @@ static void SensorTask(void *parameters)
init = 1; init = 1;
} }
// Update the sensor readings
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
if(updateSensors(&attitudeRaw) != 0) if(updateSensors() != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else { else {
// Only update attitude when sensor data is good
AttitudeRawSet(&attitudeRaw);
// TODO: Push data onto queue // TODO: Push data onto queue
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
} }
@ -251,7 +247,7 @@ static void AttitudeTask(void *parameters)
while (1) { while (1) {
// This function blocks on data queue // This function blocks on data queue
updateAttitude(); updateAttitudeComplimentary();
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
} }
@ -262,7 +258,6 @@ uint32_t accel_samples;
uint32_t gyro_samples; uint32_t gyro_samples;
struct pios_bma180_data accel; struct pios_bma180_data accel;
struct pios_mpu6000_data gyro; struct pios_mpu6000_data gyro;
AttitudeRawData raw;
int32_t accel_accum[3] = {0, 0, 0}; int32_t accel_accum[3] = {0, 0, 0};
int32_t gyro_accum[3] = {0,0,0}; int32_t gyro_accum[3] = {0,0,0};
float scaling; float scaling;
@ -272,7 +267,7 @@ float scaling;
* @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 int8_t updateSensors(AttitudeRawData * attitudeRaw) static int32_t updateSensors()
{ {
int32_t read_good; int32_t read_good;
int32_t count; int32_t count;
@ -302,16 +297,15 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
// Not the swaping of channel orders // Not the swaping of channel orders
scaling = PIOS_BMA180_GetScale(); scaling = PIOS_BMA180_GetScale();
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (accels[0] - accelbias[0]) * scaling; AccelsData accelsData; // Skip get as we set all the fields
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (accels[1] - accelbias[1]) * scaling; accelsData.x = (accels[0] - accelbias[0]) * scaling;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (accels[2] - accelbias[2]) * scaling; accelsData.y = (accels[1] - accelbias[1]) * scaling;
accelsData.z = (accels[2] - accelbias[2]) * scaling;
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
AccelsSet(&accelsData);
// Push the data onto the queue for attitude to consume // Push the data onto the queue for attitude to consume
struct accel_data accel_data; if(xQueueSendToBack(accelQueue, (void *) &accelsData, 0) != pdTRUE) {
accel_data.x = attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
accel_data.y = attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
accel_data.z = attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
if(xQueueSendToBack(accelQueue, (void *) &accel_data, 0) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} }
@ -332,67 +326,54 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples}; float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples};
scaling = PIOS_MPU6000_GetScale(); scaling = PIOS_MPU6000_GetScale();
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = gyros[0] * scaling; GyrosData gyrosData; // Skip get as we set all the fields
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = gyros[1] * scaling; gyrosData.x = gyros[0] * scaling;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = gyros[2] * scaling; gyrosData.y = gyros[1] * scaling;
gyrosData.z = gyros[2] * scaling;
gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
// Don't set yet. We push raw data to queue but then bias correct for other modules
// Push the data onto the queue for attitude to consume // Push the data onto the queue for attitude to consume
struct gyro_data gyro_data; if(xQueueSendToBack(gyroQueue, (void *) &gyrosData, 0) != pdTRUE) {
gyro_data.x = attitudeRaw->gyros[ATTITUDERAW_GYROS_X];
gyro_data.y = attitudeRaw->gyros[ATTITUDERAW_GYROS_Y];
gyro_data.z = attitudeRaw->gyros[ATTITUDERAW_GYROS_Z];
if(xQueueSendToBack(gyroQueue, (void *) &gyro_data, 0) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} }
// From data sheet 35 deg C corresponds to -13200, and 280 LSB per C // Apply bias correction to the gyros
attitudeRaw->temperature[ATTITUDERAW_TEMPERATURE_GYRO] = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
// From the data sheet 25 deg C corresponds to 2 and 2 LSB per C
attitudeRaw->temperature[ATTITUDERAW_TEMPERATURE_ACCEL] = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
if(bias_correct_gyro) { if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias gyrosData.x += gyro_bias[0];
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0]; gyrosData.y += gyro_bias[1];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1]; gyrosData.z += gyro_bias[2];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
} }
GyrosSet(&gyrosData);
// Hack for tweaking gyro gains with the old settings
scaling = gyroGain / 0.42;
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] *= scaling;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] *= scaling;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] *= scaling;
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try // Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly) // and make it average zero (weakly)
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate; gyro_bias[2] += - gyrosData.z * yawBiasRate;
if (PIOS_HMC5883_NewDataAvailable()) { if (PIOS_HMC5883_NewDataAvailable()) {
int16_t values[3]; int16_t values[3];
PIOS_HMC5883_ReadMag(values); PIOS_HMC5883_ReadMag(values);
attitudeRaw->magnetometers[ATTITUDERAW_MAGNETOMETERS_X] = -values[0]; MagnetometerData mag; // Skip get as we set all the fields
attitudeRaw->magnetometers[ATTITUDERAW_MAGNETOMETERS_Y] = -values[1]; mag.x = -values[0];
attitudeRaw->magnetometers[ATTITUDERAW_MAGNETOMETERS_Z] = -values[2]; mag.y = -values[1];
mag.z = -values[2];
MagnetometerSet(&mag);
} }
AttitudeRawSet(&raw);
return 0; return 0;
} }
float accel_mag; float accel_mag;
float qmag; float qmag;
static void updateAttitude() static int32_t updateAttitudeComplimentary()
{ {
struct gyro_data gyro_data; GyrosData gyrosData;
struct accel_data accel_data; AccelsData accelsData;
if(xQueueReceive(gyroQueue, (void *) &gyro_data, 10 / portTICK_RATE_MS) != pdTRUE || if(xQueueReceive(gyroQueue, (void *) &gyrosData, 10 / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, (void *) &accel_data, 10 / portTICK_RATE_MS) != pdTRUE) { xQueueReceive(accelQueue, (void *) &accelsData, 10 / portTICK_RATE_MS) != pdTRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return; return -1;
} }
static int32_t timeval; static int32_t timeval;
@ -414,32 +395,32 @@ static void updateAttitude()
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accel_data.x, (const float *) grot, accel_err); CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
// Account for accel magnitude // Account for accel magnitude
accel_mag = accel_data.x*accel_data.x + accel_data.y*accel_data.y + accel_data.z*accel_data.z; accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
accel_mag = sqrtf(accel_mag); accel_mag = sqrtf(accel_mag);
accel_err[0] /= accel_mag; accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag; accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag; accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi; gyro_bias[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi; gyro_bias[1] += accel_err[1] * accelKi;
// Correct rates based on error, integral component dealt with in updateSensors // Correct rates based on error, integral component dealt with in updateSensors
gyro_data.x += accel_err[0] * accelKp / dT; gyrosData.x += accel_err[0] * accelKp / dT;
gyro_data.y += accel_err[1] * accelKp / dT; gyrosData.y += accel_err[1] * accelKp / dT;
gyro_data.z += accel_err[2] * accelKp / dT; gyrosData.z += accel_err[2] * accelKp / dT;
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; float qdot[4];
qdot[0] = (-q[1] * gyro_data.x - q[2] * gyro_data.y - q[3] * gyro_data.z) * dT * F_PI / 180 / 2; qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[1] = (q[0] * gyro_data.x - q[3] * gyro_data.y + q[2] * gyro_data.z) * dT * F_PI / 180 / 2; qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[2] = (q[3] * gyro_data.x + q[0] * gyro_data.y - q[1] * gyro_data.z) * dT * F_PI / 180 / 2; qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[3] = (-q[2] * gyro_data.x + q[1] * gyro_data.y + q[0] * gyro_data.z) * dT * F_PI / 180 / 2; qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
// Take a time step // Take a time step
q[0] = q[0] + qdot[0]; q[0] = q[0] + qdot[0];
@ -479,6 +460,7 @@ static void updateAttitude()
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
return 0;
} }
static void settingsUpdatedCb(UAVObjEvent * objEv) { static void settingsUpdatedCb(UAVObjEvent * objEv) {
@ -498,9 +480,9 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; gyro_bias[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; gyro_bias[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; gyro_bias[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
// Indicates not to expend cycles on rotation // Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&

14
flight/Modules/Stabilization/stabilization.c
View File

@ -38,7 +38,7 @@
#include "ratedesired.h" #include "ratedesired.h"
#include "stabilizationdesired.h" #include "stabilizationdesired.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attituderaw.h" #include "gyros.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro #include "manualcontrol.h" // Just to get a macro
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
@ -124,7 +124,7 @@ int32_t StabilizationInitialize()
// Listen for updates. // Listen for updates.
// AttitudeActualConnectQueue(queue); // AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue); GyrosConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb); StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle()); SettingsUpdatedCb(StabilizationSettingsHandle());
@ -149,7 +149,7 @@ static void stabilizationTask(void* parameters)
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
RateDesiredData rateDesired; RateDesiredData rateDesired;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw; GyrosData gyrosData;
FlightStatusData flightStatus; FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL); SettingsUpdatedCb((UAVObjEvent *) NULL);
@ -176,7 +176,7 @@ static void stabilizationTask(void* parameters)
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw); GyrosGet(&gyrosData);
#if defined(DIAGNOSTICS) #if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
@ -220,9 +220,9 @@ static void stabilizationTask(void* parameters)
#endif #endif
for(uint8_t i = 0; i < MAX_AXES; i++) { gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha); gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
} gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
float *attitudeDesiredAxis = &stabDesired.Roll; float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll;