/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Main task loop
while (1) {
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
init = 1;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
}
}
static void settingsUpdatedCb(UAVObjEvent * ev)
{
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
/* When the revo calibration is updated, update the GyroBias object */
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
GyrosBiasSet(&gyrosBias);
gyroBiasSettingsUpdated = true;
// In case INS currently running
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
}
if(ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
alt = homeLocation.Altitude;
T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f;
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle())
AttitudeSettingsGet(&attitudeSettings);
if (ev == NULL || ev->obj == RevoSettingsHandle())
RevoSettingsGet(&revoSettings);
}
void AttitudeSettingsFilterChoiceGet( uint8_t *NewFilterChoice )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewFilterChoice, offsetof( AttitudeSettingsData, FilterChoice), sizeof(uint8_t));
}
void AttitudeSettingsBiasCorrectGyroGet( uint8_t *NewBiasCorrectGyro )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewBiasCorrectGyro, offsetof( AttitudeSettingsData, BiasCorrectGyro), sizeof(uint8_t));
}
void AttitudeSettingsZeroDuringArmingGet( uint8_t *NewZeroDuringArming )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewZeroDuringArming, offsetof( AttitudeSettingsData, ZeroDuringArming), sizeof(uint8_t));
}
void AttitudeSettingsBoardRotationGet( int16_t *NewBoardRotation )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewBoardRotation, offsetof( AttitudeSettingsData, BoardRotation), 3*sizeof(int16_t));
}
void AttitudeSettingsYawBiasRateGet( float *NewYawBiasRate )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewYawBiasRate, offsetof( AttitudeSettingsData, YawBiasRate), sizeof(float));
}
void AttitudeSettingsVertPositionTauGet( float *NewVertPositionTau )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewVertPositionTau, offsetof( AttitudeSettingsData, VertPositionTau), sizeof(float));
}
void AttitudeSettingsAccelTauGet( float *NewAccelTau )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewAccelTau, offsetof( AttitudeSettingsData, AccelTau), sizeof(float));
}
void AttitudeSettingsAccelKiSet( float *NewAccelKi )
{
UAVObjSetDataField(AttitudeSettingsHandle(), (void*)NewAccelKi, offsetof( AttitudeSettingsData, AccelKi), sizeof(float));
}
void AttitudeSettingsMagKiGet( float *NewMagKi )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewMagKi, offsetof( AttitudeSettingsData, MagKi), sizeof(float));
}
void AttitudeSettingsTrimFlightGet( uint8_t *NewTrimFlight )
{
UAVObjGetDataField(AttitudeSettingsHandle(), (void*)NewTrimFlight, offsetof( AttitudeSettingsData, TrimFlight), sizeof(uint8_t));
}
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
enum complimentary_filter_status complimentary_filter_status;
complimentary_filter_status = CF_POWERON;
uint32_t arming_count = 0;
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (complimentary_filter_status == CF_POWERON) {
complimentary_filter_status = (xTaskGetTickCount() > 1000) ?
CF_INITIALIZING : CF_POWERON;
} else if(complimentary_filter_status == CF_INITIALIZING &&
(xTaskGetTickCount() < 7000) &&
(xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 0.1f + 0.1f * (xTaskGetTickCount() < 4000);
accelKi = 0.1;
yawBiasRate = 0.1;
accel_filter_enabled = false;
} else if (zero_during_arming &&
(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
// Use a rapidly decrease accelKp to force the attitude to snap back
// to level and then converge more smoothly
if (arming_count < 20)
accelKp = 1.0f;
else if (accelKp > 0.1f)
accelKp -= 0.01f;
arming_count++;
accelKi = 0.1f;
yawBiasRate = 0.1f;
accel_filter_enabled = false;
// Indicate arming so that after arming it reloads
// the normal settings
if (complimentary_filter_status != CF_ARMING) {
accumulate_gyro_zero();
complimentary_filter_status = CF_ARMING;
accumulating_gyro = true;
}
} else if (complimentary_filter_status == CF_ARMING ||
complimentary_filter_status == CF_INITIALIZING) {
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
if(accel_alpha > 0.0f)
accel_filter_enabled = true;
// If arming that means we were accumulating gyro
// samples. Compute new bias.
if (complimentary_filter_status == CF_ARMING) {
accumulate_gyro_compute();
accumulating_gyro = false;
arming_count = 0;
}
// Indicate normal mode to prevent rerunning this code
complimentary_filter_status = CF_NORMAL;
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AccelsData accels;
GyrosData gyros;
int32_t retval = 0;
retval = updateSensorsCC3D(&accels, &gyros);
// During power on set to angle from accel
if (complimentary_filter_status == CF_POWERON) {
float RPY[3];
float theta = atan2f(accels.x, -accels.z);
RPY[1] = theta * RAD2DEG;
RPY[0] = atan2f(-accels.y, -accels.z / cosf(theta)) * RAD2DEG;
RPY[2] = 0;
RPY2Quaternion(RPY, q);
}
// Only update attitude when sensor data is good
if (retval != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Do not update attitude data in simulation mode
if (!AttitudeActualReadOnly())
updateAttitude(&accels, &gyros);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
}
