/**
* 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);
}
}
/**
* 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);
}
}
}
