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