CoDrone yaw manoeuvre

yaw_manoeuvre.ino

#include <CoDrone.h> 

int init_yaw;
int cmd_yaw;
int err_yaw;

void setup()
{
  CoDrone.begin(115200);
  CoDrone.AutoConnect(NearbyDrone);

  CoDrone.FlightEvent(TakeOff);
  delay(1000);

  AttitudeToSerialMonitor();
  init_yaw = AttitudeYAW;
  cmd_yaw = init_yaw + 90;
}

void loop() 
{
  byte bt1 = digitalRead(11);
  byte bt4 = digitalRead(14);
  byte bt8 = digitalRead(18);
  delay(500);
  
  AttitudeToSerialMonitor();
  err_yaw   = cmd_yaw   - AttitudeYAW;
  delay(500);

  if (PAIRING == true)
  {

    if (err_yaw > 0)
    {
      err_yaw = 1;
    }
    else
    {
      err_yaw = -1;
    }
    
    // (+/-)90 degrees YAW manoeuvre 
    YAW = err_yaw*50;    
    CoDrone.Control();
    delay(2000);

    // throttle reducing; it seems the yaw manoeuvre increases the throttle
    THROTTLE = -10;
    CoDrone.Control();
    delay(2000);
    
  }

  // this is for emergency landing by triggering one of the IR sensors
  // in the smartboard
  if (bt1 || bt4 || bt8)
  {
    CoDrone.FlightEvent(Landing);
    delay(500);
  }
  
  
}

void AttitudeToSerialMonitor()
{
  //---------------------------------------------------------------------------------------------//
  CoDrone.Send_LinkModeBroadcast(LinkBroadcast_Active); //link module mode change => Active

  CoDrone.Request_DroneAttitude(); 
  //CoDrone.Request_ImuRawAndAngle();
  //CoDrone.Request_Pressure();
  while (CoDrone.receiveAttitudeSuccess == 0) //receiveAttitudeSuccess check
  {
    CoDrone.Receive();
  }
  CoDrone.receiveAttitudeSuccess = 0; //receiveAttitudeSuccess flag init

  //---------------------------------------------------------------------------------------------// 
  CoDrone.Send_LinkModeBroadcast(LinkModeMute); //link module mode change => Mute
  delay(20);
}