/*
* From Roll-Pitch Gyro Drift Compensation, Rev 3. William Premerlani, 2012.
*/
static void rollPitch_drift_GPS(float Rbe[3][3], float accels_e_int[3],
float delT_between_updates, float *errRollPitch_b)
{
float errRollPitch_e[3];
float dGPSdt_e[3];
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
dGPSdt_e[0] = (gpsVelocity.North - drft->GPSV_old[0]) / delT_between_updates;
dGPSdt_e[1] = (gpsVelocity.East - drft->GPSV_old[1]) / delT_between_updates;
dGPSdt_e[2] = -GRAVITY + (gpsVelocity.Down - drft->GPSV_old[2]) / delT_between_updates;
drft->GPSV_old[0] = gpsVelocity.North;
drft->GPSV_old[1] = gpsVelocity.East;
drft->GPSV_old[2] = gpsVelocity.Down;
float normdGPSdt_e = VectorMagnitude(dGPSdt_e);
//Take cross product of integrated accelerometer measurements with integrated earth frame accelerations. We should be using normalized dGPSdt, but we perform that calculation in the following line(s).
CrossProduct((const float *)accels_e_int, (const float *)dGPSdt_e, errRollPitch_e);
//Scale cross product
errRollPitch_e[0] /= (normdGPSdt_e * delT_between_updates);
errRollPitch_e[1] /= (normdGPSdt_e * delT_between_updates);
errRollPitch_e[2] /= (normdGPSdt_e * delT_between_updates);
//Rotate earth drift error back into body frame;
rot_mult(Rbe, errRollPitch_e, errRollPitch_b, FALSE);
}
/**
* @brief Apply calibration and rotation to the raw gyro data
* @param[in] gyros The raw gyro data
*/
static void update_gyros(struct pios_sensor_gyro_data *gyros, GyrosData * gyrosData)
{
static float gyro_temp_bias[3] = {0,0,0};
// Scale the gyros
float gyros_out[3] = {gyros->x * sensorSettings.GyroScale[0],
gyros->y * sensorSettings.GyroScale[1],
gyros->z * sensorSettings.GyroScale[2]
};
// Update the bias due to the temperature
updateTemperatureComp(gyrosData->temperature, gyro_temp_bias);
// Apply temperature bias correction before the rotation
if (bias_correct_gyro) {
gyros_out[0] -= gyro_temp_bias[0];
gyros_out[1] -= gyro_temp_bias[1];
gyros_out[2] -= gyro_temp_bias[2];
}
// When computing the bias accumulate samples
accumulate_gyro(gyros_out);
if (rotate) {
float gyros[3];
rot_mult(Rsb, gyros_out, gyros, true);
gyrosData->x = gyros[0];
gyrosData->y = gyros[1];
gyrosData->z = gyros[2];
} else {
gyrosData->x = gyros_out[0];
gyrosData->y = gyros_out[1];
gyrosData->z = gyros_out[2];
}
if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
gyrosData->x -= gyro_correct_int[0];
gyrosData->y -= gyro_correct_int[1];
gyrosData->z -= gyro_correct_int[2];
}
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += gyrosData->z * yawBiasRate;
gyrosData->temperature = gyros->temperature;
}
void auxmagsupport_publish_samples(float mags[3], uint8_t status)
{
float mag_out[3];
mags[0] -= mag_bias[0];
mags[1] -= mag_bias[1];
mags[2] -= mag_bias[2];
rot_mult(mag_transform, mags, mag_out);
AuxMagSensorData data;
data.x = mag_out[0];
data.y = mag_out[1];
data.z = mag_out[2];
data.Status = status;
AuxMagSensorSet(&data);
}
/**
* @brief Apply calibration and rotation to the raw accel data
* @param[in] accels The raw accel data
*/
static void update_accels(struct pios_sensor_accel_data *accels, AccelsData * accelsData)
{
// Average and scale the accels before rotation
float accels_out[3] = {accels->x * sensorSettings.AccelScale[0] - sensorSettings.AccelBias[0],
accels->y * sensorSettings.AccelScale[1] - sensorSettings.AccelBias[1],
accels->z * sensorSettings.AccelScale[2] - sensorSettings.AccelBias[2]
};
if (rotate) {
float accel_rotated[3];
rot_mult(Rsb, accels_out, accel_rotated, true);
accelsData->x = accel_rotated[0];
accelsData->y = accel_rotated[1];
accelsData->z = accel_rotated[2];
} else {
accelsData->x = accels_out[0];
accelsData->y = accels_out[1];
accelsData->z = accels_out[2];
}
accelsData->temperature = accels->temperature;
}
static void SensorsTask(void *parameters)
{
portTickType lastSysTime;
uint32_t accel_samples = 0;
uint32_t gyro_samples = 0;
int32_t accel_accum[3] = {0, 0, 0};
int32_t gyro_accum[3] = {0,0,0};
float gyro_scaling = 0;
float accel_scaling = 0;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
UAVObjEvent ev;
settingsUpdatedCb(&ev);
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();
#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();
#else
mag_test = 0;
#endif
if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
vTaskDelay(10);
}
}
// Main task loop
lastSysTime = xTaskGetTickCount();
bool error = false;
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false;
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
}
for (int i = 0; i < 3; i++) {
accel_accum[i] = 0;
gyro_accum[i] = 0;
}
accel_samples = 0;
gyro_samples = 0;
AccelsData accelsData;
GyrosData gyrosData;
switch(bdinfo->board_rev) {
case 0x01: // L3GD20 + BMA180 board
#if defined(PIOS_INCLUDE_BMA180)
{
struct pios_bma180_data accel;
int32_t read_good;
int32_t count;
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[1] += accel.x;
accel_accum[0] += 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[1] += gyro.gyro_x;
gyro_accum[0] += gyro.gyro_y;
gyro_accum[2] -= gyro.gyro_z;
gyro_scaling = PIOS_L3GD20_GetScale();
// Get temp from last reading
gyrosData.temperature = gyro.temperature;
}
#endif
break;
case 0x02: // MPU6000 board
case 0x03: // MPU6000 board
#if defined(PIOS_INCLUDE_MPU6000)
{
struct pios_mpu6000_data mpu6000_data;
xQueueHandle queue = PIOS_MPU6000_GetQueue();
while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY)
{
gyro_accum[0] += mpu6000_data.gyro_x;
gyro_accum[1] += mpu6000_data.gyro_y;
gyro_accum[2] += mpu6000_data.gyro_z;
accel_accum[0] += mpu6000_data.accel_x;
accel_accum[1] += mpu6000_data.accel_y;
accel_accum[2] += mpu6000_data.accel_z;
gyro_samples ++;
accel_samples ++;
}
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
continue;
}
gyro_scaling = PIOS_MPU6000_GetScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
default:
PIOS_DEBUG_Assert(0);
}
// Scale the accels
float accels[3] = {(float) accel_accum[0] / accel_samples,
(float) accel_accum[1] / accel_samples,
(float) accel_accum[2] / accel_samples};
float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]};
if (rotate) {
rot_mult(R, accels_out, accels);
accelsData.x = accels[0];
accelsData.y = accels[1];
accelsData.z = accels[2];
} else {
accelsData.x = accels_out[0];
accelsData.y = accels_out[1];
accelsData.z = accels_out[2];
}
AccelsSet(&accelsData);
// Scale the gyros
float gyros[3] = {(float) gyro_accum[0] / gyro_samples,
(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling,
gyros[1] * gyro_scaling,
gyros[2] * gyro_scaling};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
gyrosData.y = gyros[1];
gyrosData.z = gyros[2];
} else {
gyrosData.x = gyros_out[0];
gyrosData.y = gyros_out[1];
gyrosData.z = gyros_out[2];
}
if (bias_correct_gyro) {
// Apply bias correction to the gyros from the state estimator
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x -= gyrosBias.x;
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)
#if defined(PIOS_INCLUDE_HMC5883)
MagnetometerData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
float mags[3] = {-(float) values[1] * mag_scale[0] - mag_bias[0],
-(float) values[0] * mag_scale[1] - mag_bias[1],
-(float) values[2] * mag_scale[2] - mag_bias[2]};
if (rotate) {
float mag_out[3];
rot_mult(R, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
} else {
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
// Correct for mag bias and update if the rate is non zero
if(cal.MagBiasNullingRate > 0)
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
lastSysTime = xTaskGetTickCount();
}
}
static void updateSensors(AttitudeRawData * attitudeRaw)
{
struct pios_adxl345_data accel_data;
float gyro[4];
// Only wait the time for two nominal updates before setting an alarm
if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
}
// First sample is temperature
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
int32_t x = 0;
int32_t y = 0;
int32_t z = 0;
uint8_t i = 0;
uint8_t samples_remaining;
do {
i++;
samples_remaining = PIOS_ADXL345_Read(&accel_data);
x += accel_data.x;
y += -accel_data.y;
z += -accel_data.z;
} while ( (i < 32) && (samples_remaining > 0) );
attitudeRaw->gyrotemp[0] = samples_remaining;
attitudeRaw->gyrotemp[1] = i;
float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
if(rotate) {
// TODO: rotate sensors too so stabilization is well behaved
float vec_out[3];
rot_mult(R, accel, vec_out);
attitudeRaw->accels[0] = vec_out[0];
attitudeRaw->accels[1] = vec_out[1];
attitudeRaw->accels[2] = vec_out[2];
rot_mult(R, attitudeRaw->gyros, vec_out);
attitudeRaw->gyros[0] = vec_out[0];
attitudeRaw->gyros[1] = vec_out[1];
attitudeRaw->gyros[2] = vec_out[2];
} else {
attitudeRaw->accels[0] = accel[0];
attitudeRaw->accels[1] = accel[1];
attitudeRaw->accels[2] = accel[2];
}
// Scale accels and correct bias
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * 0.004f * 9.81f;
if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
}
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
}
/*
* Although yaw correction is done in horizontal plane, it is
* computed in 3 dimensions, just in case we change our minds later.
*/
void yaw_drift_MagGPS(float Rbe[3][3], bool gpsNewData_flag, bool magNewData_flag, float *errYaw_b)
{
#if defined (PIOS_INCLUDE_GPS) || defined (PIOS_INCLUDE_MAGNETOMETER)
// Form the horizontal direction over ground based on rmat
float horizDirOverGndRmat[3];
//Define forward vector in earth frame, Rbe' * [1;0;0], and eliminate vertical component from vector
horizDirOverGndRmat[0] = Rbe[0][0];
horizDirOverGndRmat[1] = Rbe[0][1];
horizDirOverGndRmat[2] = 0;
#if defined (PIOS_INCLUDE_GPS)
if (gpsNewData_flag) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
if (fabs(gpsVelocity.North) > GPS_SPEED_MIN
|| fabs(gpsVelocity.East) > GPS_SPEED_MIN) {
float errorGPSYaw_e;
float errorGPSYaw_b[3];
float normGPS =
sqrtf(gpsVelocity.North * gpsVelocity.North +
gpsVelocity.East * gpsVelocity.East);
float horizDirOverGndGPS[3] = { gpsVelocity.North / normGPS, gpsVelocity.East / normGPS, 0 }; //Normalized vector
// vector cross product to get the rotation error in ground frame. However, save several processor cycles by
// condensing the math to take advantage of the cross products null entries on the x and y elements.
// errorGPSYaw_e = horizDirOverGndRmat X horizDirOverGndGPS
errorGPSYaw_e =
horizDirOverGndRmat[0] * horizDirOverGndGPS[1] -
horizDirOverGndGPS[0] * horizDirOverGndRmat[1];
// Rotate error to body frame. Again, take advantage of the yaw error vector [0;0;errorGPSYaw_e]'s null entries
// errorGPSYaw_b = Rbe * errorGPSYaw_e;
errorGPSYaw_b[0] = Rbe[0][2] * errorGPSYaw_e;
errorGPSYaw_b[1] = Rbe[1][2] * errorGPSYaw_e;
errorGPSYaw_b[2] = Rbe[2][2] * errorGPSYaw_e;
//Sum error with existing error
errYaw_b[0] += errorGPSYaw_b[0] * GPS_YAW_KP;
errYaw_b[1] += errorGPSYaw_b[1] * GPS_YAW_KP;
errYaw_b[2] += errorGPSYaw_b[2] * GPS_YAW_KP;
}
}
#endif
// TODO: Create a flag to indicate whether mag data is present (or check for updates of MagnetometerData)
#if defined (PIOS_INCLUDE_MAGNETOMETER)
if (magNewData_flag) {
MagnetometerData magnetometerData;
MagnetometerGet(&magnetometerData);
float errorMagYaw_e;
float errorMagYaw_b[3];
float horizDirOverGndMag[3];
float mags_b[3] = { magnetometerData.x, magnetometerData.y, magnetometerData.z };
// Rotate magnetometer to earth frame and eliminate vertical component
rot_mult(Rbe, mags_b, horizDirOverGndMag, true);
horizDirOverGndMag[2] = 0;
//Compute norm
float normMag = VectorMagnitude(horizDirOverGndMag);
//Normalize mag_vector. Recall that horizDirOverGndMag[2]=0
horizDirOverGndMag[0] /= normMag;
horizDirOverGndMag[1] /= normMag;
// vector cross product to get the rotation error in ground frame. However, save several processor cycles by
// condensing the math to take advantage of the cross products null entries on the x and y elements.
// errorMagYaw_e = horizDirOverGndRmat X horizDirOverGndMag
errorMagYaw_e = horizDirOverGndRmat[0] * horizDirOverGndMag[1] -
horizDirOverGndMag[0] * horizDirOverGndRmat[1];
// Rotate error to body frame. Again, take advantage of the yaw error vector [0;0;errorGPSYaw_e]'s null entries
// errorMagYaw_b = Rbe * errorMagYaw_e;
errorMagYaw_b[0] = Rbe[0][2] * errorMagYaw_e;
errorMagYaw_b[1] = Rbe[1][2] * errorMagYaw_e;
errorMagYaw_b[2] = Rbe[2][2] * errorMagYaw_e;
errYaw_b[0] += errorMagYaw_b[0] * MAG_YAW_KP;
errYaw_b[1] += errorMagYaw_b[1] * MAG_YAW_KP;
errYaw_b[2] += errorMagYaw_b[2] * MAG_YAW_KP;
}
#endif
#endif
}
/*
* Correct sensor drift, using the DCM approach from W. Premerlani et. al
*/
void Premerlani_GPS(float *accels, float *gyros, float Rbe[3][3], const float delT, bool GPS_Drift_Compensation, GlobalAttitudeVariables *glblAtt, float *omegaCorrP)
{
float errYaw_b[3] = { 0, 0, 0 };
float errRollPitch_b[3] = { 0, 0, 0 };
float normOmegaScalar = VectorMagnitude(gyros);
//Correct roll-pitch drift via GPS and accelerometer
//The math is derived from Roll-Pitch Gyro Drift Compensation, Rev.3, by W. Premerlani
#if defined (PIOS_INCLUDE_GPS)
if (drft->gpsPresent_flag && GPS_Drift_Compensation) {
float accels_e[3];
//Rotate accelerometer readings into Earth frame. Note that we need to take the transpose of Rbe.
rot_mult(Rbe, accels, accels_e, TRUE);
//Integrate accelerometer measurements in Earth frame
drft->accels_e_integrator[0] += accels_e[0] * delT;
drft->accels_e_integrator[1] += accels_e[1] * delT;
drft->accels_e_integrator[2] += accels_e[2] * delT;
drft->delT_between_GPS += delT;
//Check if the GPS has new information.
if (!
(drft->
gpsVelocityDataConsumption_flag & GPS_CONSUMED_BY_RPY)) {
//Compute drift correction, errRollPitch_b, from GPS
rollPitch_drift_GPS(Rbe, drft->accels_e_integrator,
drft->delT_between_GPS,
errRollPitch_b);
//Reset integrator
memset(drft->accels_e_integrator, 0,
sizeof(drft->accels_e_integrator));
//Mark GPS data as consumed by this function
drft->gpsVelocityDataConsumption_flag |=
GPS_CONSUMED_BY_RPY;
drft->delT_between_GPS = 0;
}
}
#endif
if (!GPS_Drift_Compensation) {
#if defined (PIOS_INCLUDE_GPS) && 0 || defined (PIOS_INCLUDE_MAGNETOMETER)
if (!(drft->gpsVelocityDataConsumption_flag & GPS_CONSUMED_BY_Y)) {
// We're actually using new GPS data here, but it's already been stored in old by the previous function
yaw_drift_MagGPS(Rbe, true, drft->magNewData_flag, errYaw_b);
// Mark GPS data as consumed by this function
drft->gpsVelocityDataConsumption_flag |= GPS_CONSUMED_BY_Y;
} else {
// In addition to calculating the roll-pitch-yaw error, we can calculate yaw drift, errYaw_b, based on GPS and attitude data
// We're actually using new GPS data here, but it's already been stored in old by the previous function
yaw_drift_MagGPS(Rbe, false, drft->magNewData_flag, errYaw_b);
}
// Reset flag. Not the best place to do it, but it's messy anywhere else
if (drft->magNewData_flag) {
drft->magNewData_flag = false;
}
#endif
//In addition, we can calculate roll-pitch error with only the aid of an accelerometer
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualData airspeedActualData;
AirspeedActualGet(&airspeedActualData);
float airspeed_tas = airspeedActualData.TrueAirspeed;
#else
float airspeed_tas = 0;
#endif
rollPitch_drift_accel(accels, gyros, Rbe, airspeed_tas,
errRollPitch_b);
}
// Calculate gyro drift, based on all errors
gyro_drift(gyros, errYaw_b, errRollPitch_b, normOmegaScalar, delT, omegaCorrP, drft->omegaCorrI);
//Calculate final drift response
gyros[0] += omegaCorrP[0] + drft->omegaCorrI[0];
gyros[1] += omegaCorrP[1] + drft->omegaCorrI[1];
gyros[2] += omegaCorrP[2] + drft->omegaCorrI[2];
//Add 0.0001% of proportional error back into gyroscope bias offset. This keeps DC elements out of the raw gyroscope data.
glblAtt->gyro_correct_int[0] += omegaCorrP[0] / 1000000.0f;
glblAtt->gyro_correct_int[1] += omegaCorrP[1] / 1000000.0f;
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
glblAtt->gyro_correct_int[2] += -gyros[2] * glblAtt->yawBiasRate;
}
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE )
{
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()){
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
return -1;
MagnetometerData magData;
MagnetometerGet(&magData);
#else
MagnetometerData magData;
magData.x = 100;
magData.y = 0;
magData.z = 0;
#endif
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
timeval = PIOS_DELAY_GetRaw();
return 0;
}
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
magKp = 0.01f;
init = 1;
}
GyrosGet(&gyrosData);
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
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]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
// Account for accel magnitude
accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
accel_mag = sqrtf(accel_mag);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
{
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagnetometerData mag;
Quaternion2R(q, Rbe);
MagnetometerGet(&mag);
// If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi;
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.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] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.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
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NED[0];
positionActual.East = NED[1];
positionActual.Down = NED[2];
PositionActualSet(&positionActual);
}
if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) {
// Transform the GPS position into NED coordinates
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gpsVelocity.North;
velocityActual.East = gpsVelocity.East;
velocityActual.Down = gpsVelocity.Down;
VelocityActualSet(&velocityActual);
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
return 0;
}
static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
SensorSettingsGet(&sensorSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0025f;
if(attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false;
} else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
accel_filter_enabled = true;
}
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
gyro_correct_int[0] = 0;
gyro_correct_int[1] = 0;
gyro_correct_int[2] = 0;
// Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
rotate = 0;
// Shouldn't be used but to be safe
float rotationQuat[4] = {1,0,0,0};
Quaternion2R(rotationQuat, Rsb);
} else {
float rotationQuat[4];
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL] / 100.0f,
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH] / 100.0f,
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] / 100.0f
};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, Rsb);
rotate = 1;
}
if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) {
trim_accels[0] = 0;
trim_accels[1] = 0;
trim_accels[2] = 0;
trim_samples = 0;
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
// Get sensor data mean
float a_body[3] = { trim_accels[0] / trim_samples,
trim_accels[1] / trim_samples,
trim_accels[2] / trim_samples
};
// Inverse rotation of sensor data, from body frame into sensor frame
float a_sensor[3];
rot_mult(Rsb, a_body, a_sensor, false);
// Temporary variables
float psi, theta, phi;
psi = attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] * DEG2RAD / 100.0f;
float cP = cosf(psi);
float sP = sinf(psi);
// In case psi is too small, we have to use a different equation to solve for theta
if (fabsf(psi) > PI / 2)
theta = atanf((a_sensor[1] + cP * (sP * a_sensor[0] -
cP * a_sensor[1])) / (sP * a_sensor[2]));
else
theta = atanf((a_sensor[0] - sP * (sP * a_sensor[0] -
cP * a_sensor[1])) / (cP * a_sensor[2]));
phi = atan2f((sP * a_sensor[0] - cP * a_sensor[1]) / GRAVITY,
(a_sensor[2] / cosf(theta) / GRAVITY));
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL] = phi * RAD2DEG * 100.0f;
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH] = theta * RAD2DEG * 100.0f;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
}
}