File content as of revision 143:53808e4e684c:

#include "global.hpp"
using namespace std;

void setup()
{
    //sd.baud(115200);
    //sd.printf("\r\nFlight Start\r\n");
    //twelite.baud(38400);
    twelite.serial.printf("\r\nTelemetory Start\r\n");
    uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G);
    //printf("%x\n", reg);
    if (!lps.init()){
        twelite.serial.printf("Failed to autodetect pressure sensor!\r\n");
        while (1);
    }
    lps.enableDefault();
    gps.Attach();
    wait_ms(100);
    
    SensorAlignmentAG << 1.0f, 0.0f, 0.0f,
                         0.0f, 1.0f, 0.0f,
                         0.0f, 0.0f, -1.0f;
    SensorAlignmentMAG << -1.0f, 0.0f, 0.0f,
                           0.0f, 1.0f, 0.0f,
                           0.0f, 0.0f, -1.0f; 
    float magMin[3] =  {-392.590332, -85.194908, -155.781174};
    float magMax[3] =  {182.602386, 530.811523, 365.834625};
    magCalibrator.setExtremes(magMin,magMax);
    
    pitchPID.setSetPoint(0.0f);
    pitchratePID.setSetPoint(0.0f); 
    rollPID.setSetPoint(0.0f); 
    rollratePID.setSetPoint(0.0f); 
    pitchPID.setBias(0.0f);
    pitchratePID.setBias(0.0f); 
    rollPID.setBias(0.0f); 
    rollratePID.setBias(0.0f); 
    pitchPID.setOutputLimits(-1.0f,1.0f);
    pitchratePID.setOutputLimits(-1.0f,1.0f);
    rollPID.setOutputLimits(-1.0f,1.0f); 
    rollratePID.setOutputLimits(-1.0f,1.0f);
    pitchPID.setInputLimits(-M_PI_F,M_PI_F);
    pitchratePID.setInputLimits(-M_PI_F,M_PI_F);
    rollPID.setInputLimits(-M_PI_F,M_PI_F); 
    rollratePID.setInputLimits(-M_PI_F,M_PI_F);
    
    servoRight.period_us(15000.0f);
    servoLeft.period_us(15000.0f);
    servoThrust.period_us(15000.0f);
    servoRight.pulsewidth_us(1500.0f);
    servoLeft.pulsewidth_us(1500.0f); 
    servoThrust.pulsewidth_us(1100.0f);
    
    autopilot.set_dest(0.0f, 50.0f);
    autopilot.set_turn(0.0f, 50.0f, 50.0f);
    autopilot.set_alt(30.0f, 10.0f);
    autopilot.set_climb(3.0f*M_PI_F/180.0f, 0.0f);
    
}

void calibrate()
{
    while(1)
    {
        wait(1000);
    }
}