Flipping

When the drone gets crashed, then if we execute the code by going into the developer mode, then the drone will automatically level itself by returning into its original position.

// Do not remove the include below
#include "PlutoPilot.h"
#include "Motor.h"
#include "Control.h"
#include "Estimate.h"
#include "Sensor.h"
#include "Utils.h"
#include "User.h"

int angle;

#define ABS(x) ((x) > 0 ? (x) : -(x))


//The setup function is called once at Pluto's hardware startup
void plutoInit()
{
Motor.initReversibleMotors();
// Add your hardware initialization code here
}



//The function is called once before plutoLoop when you activate Developer Mode
void onLoopStart()
{
LED.flightStatus(DEACTIVATE);
Motor.setDirection(M1,ANTICLOCK_WISE);
Motor.setDirection(M2,CLOCK_WISE);
Motor.setDirection(M3,ANTICLOCK_WISE);
Motor.setDirection(M4,CLOCK_WISE);
  // do your one time stuff here

}



// The loop function is called in an endless loop
void plutoLoop()
{
angle = Angle.get(AG_ROLL);
Monitor.println("Angle is: ", angle);
if (!FlightStatus.check(FS_ARMED))
{
if (ABS(angle)<1800 && ABS(angle)>800)
{
Motor.set(M1, 2000);
Motor.set(M2, 2000);
}
else
{
Motor.set(M1, 1000);
Motor.set(M2, 1000);
Motor.set(M3, 1000);
Motor.set(M4, 1000);
}
}

//Add your repeated code here

}



//The function is called once after plutoLoop when you deactivate Developer Mode
void onLoopFinish()
{
Motor.set(M1, 1000);
Motor.set(M2, 1000);
Motor.set(M3, 1000);
Motor.set(M4, 1000);
Motor.setDirection(M1,CLOCK_WISE);
Motor.setDirection(M2,ANTICLOCK_WISE);
Motor.setDirection(M3,CLOCK_WISE);
Motor.setDirection(M1,ANTICLOCK_WISE);

LED.flightStatus(ACTIVATE);


// do your cleanup stuffs here

}