File content as of revision 122:68efdee114fa:

#include "global.hpp"
using namespace std;

void setup()
{
    sd.baud(115200);
    sd.printf("\r\nFlight Start\r\n");
    twelite.baud(38400);
    twelite.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.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};
    if(hilFlag==true){
        for(int i = 0;i<3;i++){
            magMin[i] =  -1000.0f;
            magMax[i] =  1000.0f;
        }
    }
    magCalibrator.setExtremes(magMin,magMax);
    
    pitchPID.setSetPoint(0.0);
    pitchratePID.setSetPoint(0.0); 
    rollPID.setSetPoint(0.0); 
    rollratePID.setSetPoint(0.0); 
    pitchPID.setBias(0.0);
    pitchratePID.setBias(0.0); 
    rollPID.setBias(0.0); 
    rollratePID.setBias(0.0); 
    pitchPID.setOutputLimits(-1.0,1.0);
    pitchratePID.setOutputLimits(-1.0,1.0);
    rollPID.setOutputLimits(-1.0,1.0); 
    rollratePID.setOutputLimits(-1.0,1.0);
    pitchPID.setInputLimits(-M_PI,M_PI);
    pitchratePID.setInputLimits(-M_PI,M_PI);
    rollPID.setInputLimits(-M_PI,M_PI); 
    rollratePID.setInputLimits(-M_PI,M_PI);
    
    servoRight.period_us(15000.0);
    servoLeft.period_us(15000.0);
    servoThrust.period_us(15000.0);
    servoRight.pulsewidth_us(1500.0);
    servoLeft.pulsewidth_us(1500.0); 
    servoThrust.pulsewidth_us(1100.0);
    
    autopilot.set_dest(destination.x, destination.y);
    autopilot.set_turn(turn_center.x, turn_center.y, turn_radius);
}

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