CrazyFlie源码学习2-Stabilizer任务

static void stabilizerTask(void* param){  RPYType rollType;  RPYType pitchType;  RPYType yawType;  uint32_t attitudeCounter = 0;  uint32_t altHoldCounter = 0;  uint32_t lastWakeTime;  float yawRateAngle = 0;  vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);  //Wait for the system to be fully started to start stabilization loop  systemWaitStart();  lastWakeTime = xTaskGetTickCount ();  while(1)  {    vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz    // Magnetometer not yet used more then for logging.    imu9Read(&gyro, &acc, &mag);    if (imu6IsCalibrated())    {      commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);      commanderGetRPYType(&rollType, &pitchType, &yawType);      // Rate-controled YAW is moving YAW angle setpoint      if (yawType == RATE) {        yawRateAngle -= eulerYawDesired/500.0;        while (yawRateAngle > 180.0)          yawRateAngle -= 360.0;        while (yawRateAngle < -180.0)          yawRateAngle += 360.0;        eulerYawDesired = -yawRateAngle;      }      // 250HZ      if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)      {        sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);        sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);        accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z);        accMAG = (acc.x*acc.x) + (acc.y*acc.y) + (acc.z*acc.z);        // Estimate speed from acc (drifts)        vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT;        // Adjust yaw if configured to do so        stabilizerYawModeUpdate();        controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,                                     eulerRollDesired, eulerPitchDesired, -eulerYawDesired,                                     &rollRateDesired, &pitchRateDesired, &yawRateDesired);         attitudeCounter = 0;        /* Call out after performing attitude updates, if any functions would like to use the calculated values. */        stabilizerPostAttitudeUpdateCallOut();      }      // 100HZ      if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER))      {        stabilizerAltHoldUpdate();        altHoldCounter = 0;      }      if (rollType == RATE)      {        rollRateDesired = eulerRollDesired;      }      if (pitchType == RATE)      {        pitchRateDesired = eulerPitchDesired;      }      // TODO: Investigate possibility to subtract gyro drift.      controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z,                               rollRateDesired, pitchRateDesired, yawRateDesired);      controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);      if (!altHold || !imuHasBarometer())      {        // Use thrust from controller if not in altitude hold mode        commanderGetThrust(&actuatorThrust);      }      else      {        // Added so thrust can be set to 0 while in altitude hold mode after disconnect        commanderWatchdog();      }      /* Call out before performing thrust updates, if any functions would like to influence the thrust. */      stabilizerPreThrustUpdateCallOut();      if (actuatorThrust > 0)      {#if defined(TUNE_ROLL)        distributePower(actuatorThrust, actuatorRoll, 0, 0);#elif defined(TUNE_PITCH)        distributePower(actuatorThrust, 0, actuatorPitch, 0);#elif defined(TUNE_YAW)        distributePower(actuatorThrust, 0, 0, -actuatorYaw);#else        distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);#endif      }      else      {        distributePower(0, 0, 0, 0);        controllerResetAllPID();        // Reset the calculated YAW angle for rate control        yawRateAngle = eulerYawActual;      }    }  }}static void stabilizerPreAltHoldComputeThrustCallOut(void){  /* Code that shall run BEFORE each altHold thrust computation, should be placed here. */#if defined(SITAW_ENABLED)  /*