File content as of revision 144:b3a713b4f1c4:

#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.0);
    pitchPID.setBias(0.0);
    pitchPID.setOutputLimits(-1.0,1.0);
    pitchPID.setInputLimits(-M_PI,M_PI);
    
    pitchratePID.setSetPoint(0.0); 
    pitchratePID.setBias(0.0); 
    pitchratePID.setOutputLimits(-1.0,1.0);
    pitchratePID.setInputLimits(-M_PI,M_PI);
    
    rollPID.setSetPoint(0.0); 
    rollPID.setBias(0.0); 
    rollPID.setOutputLimits(-1.0,1.0);
    rollPID.setInputLimits(-M_PI,M_PI); 
     
    rollratePID.setSetPoint(0.0);
    rollratePID.setBias(0.0);
    rollratePID.setOutputLimits(-1.0,1.0);
    rollratePID.setInputLimits(-M_PI,M_PI);
    
    yawratePID.setSetPoint(0.0); 
    yawratePID.setBias(0.0); 
    yawratePID.setOutputLimits(-1.0,1.0);
    yawratePID.setInputLimits(-M_PI,M_PI);
    
    vxPID.setSetPoint(0.0f); 
    vxPID.setBias(0.0f); 
    vxPID.setOutputLimits(-1.0f,1.0);
    vxPID.setInputLimits(-5.0f,5.0f);
    
    vyPID.setSetPoint(0.0f); 
    vyPID.setBias(0.0f); 
    vyPID.setOutputLimits(-1.0f,1.0);
    vyPID.setInputLimits(-5.0f,5.0f);
    
    vzPID.setSetPoint(0.0f); 
    vzPID.setBias(0.0f); 
    vzPID.setOutputLimits(-1.0f,1.0);
    vzPID.setInputLimits(-1.0f,1.0f);
    
    
    motor1.period_us(15000.0);
    motor2.period_us(15000.0);
    motor3.period_us(15000.0);
    motor4.period_us(15000.0);
    motor5.period_us(15000.0);
    motor6.period_us(15000.0);

    motor1.pulsewidth_us(motorPwmMin);
    motor2.pulsewidth_us(motorPwmMin);
    motor3.pulsewidth_us(motorPwmMin);
    motor4.pulsewidth_us(motorPwmMin);
    motor5.pulsewidth_us(motorPwmMin);
    motor6.pulsewidth_us(motorPwmMin);
    
}

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