// Automatic Acrobatic Procedure (AAP) v1 : Roll flip
// State machine aproach:
//    Some states are fixed commands (for a fixed time)
//    Some states are fixed commands (until some IMU condition)
//    Some states include controls inside
void AAP_v1()
{
  #define AAP_THR_INC 180
  #define AAP_THR_TIME 200
  #define AAP_THR_DEC 90
  #define AAP_ROLL_VALUE 120  //up to 125

  // State machine
  switch (AAP_status){
    case 0: 
      AAP_count=0;                                        // Step 1 : Initialize
      AAP_status++;
      break;
    case 1: 
      if (AAP_count < 95)                                // Step 2 : Increase throttle to start maneuver
      {
        ch_throttle = Initial_Throttle + AAP_THR_INC;     // fixed throttle command
        Attitude_control_v4(0,0,ToDeg(yaw));              // Control : Stabilize on roll, pitch, yaw
        AAP_count++;
      }
      else
        {
        AAP_status++;
        AAP_count=0;
        }
      break;
    case 2: 
      if (roll < 0.7)                                     // Step 3 : ROLL (until we reach a certain angle [45º])  
      {
        ch_throttle = Initial_Throttle - AAP_THR_DEC;
        control_pitch = 0;
        control_roll = AAP_ROLL_VALUE;      // fixed command on roll
        control_yaw = 0;
      }
      else
        AAP_status++;
      break;
    case 3: 
      if ((roll >= 0.7)||(roll<-0.7))                      // Step 4 : CONTINUE ROLL (until we reach a certain angle [-45º])
      {
        ch_throttle = Initial_Throttle - AAP_THR_DEC;
        control_pitch = 0;
        control_roll = AAP_ROLL_VALUE;
        control_yaw = 0;
      }
      else
        AAP_status++;
      break;
    case 4: 
      if (AAP_count < 90)                                  // Step 5 : Increase throttle to stop the descend
      {
        ch_throttle = Initial_Throttle + AAP_THR_INC;
        //Attitude_control_v4(0,0,ToDeg(yaw));              // Stabilize on roll, pitch, yaw
        //We are doing Stable mode externally
        AAP_count++;
      }
      else
      {
        AAP_status++;
        AAP_count=0;
      }
      break;
    default: ;                                             // Step 5 : Finish : continue with Stable mode + altitude hold
     // We are doing Stable mode and altitude hold externally
  }  
}