File content as of revision 54:a36d39a90c21:

/* File:    Setup.h
 * Author:  Trung Tin Ian HUA
 * Date:    May 2014
 * Purpose: Setup code to initialise all device
 */
#include "setup.h"
#include "tasks.h"

Serial BT(p28, p27);
Serial PC(USBTX, USBRX);
DigitalOut BT_CMD(p29);
//MPU6050 imu(p9, p10);           
MPU6050_NB imu_nb(p9, p10); 
MPL3115A2 altimeter(p9, p10);
AnalogIn voltageSense(p20);
#ifdef ENABLE_COMPASS
HMC5883L compass(p9, p10);
#endif

float KP_YAW_RATE           =   P_Y_RATE;
float KP_PITCH_RATE         =   P_P_RATE;
float KP_ROLL_RATE          =   P_R_RATE;

float TI_YAW_RATE           =   TI_Y_RATE;
float TI_PITCH_RATE         =   TI_P_RATE;
float TI_ROLL_RATE          =   TI_R_RATE;

float KP_PITCH_ATTITUDE     =   P_P_ATTITUDE;
float KP_ROLL_ATTITUDE      =   P_R_ATTITUDE;

float TI_PITCH_ATTITUDE     =   TI_P_ATTITUDE;
float TI_ROLL_ATTITUDE      =   TI_R_ATTITUDE;

PID yawPIDrate(KP_YAW_RATE, TI_Y_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
PID pitchPIDrate(KP_PITCH_RATE, TI_PITCH_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
PID rollPIDrate(KP_ROLL_RATE, TI_ROLL_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);

PID pitchPIDattitude(KP_PITCH_ATTITUDE, TI_PITCH_ATTITUDE, 0.0, TASK2_MASTER_PERIOD/1000.0);
PID rollPIDattitude(KP_ROLL_ATTITUDE, TI_ROLL_ATTITUDE, 0.0, TASK2_MASTER_PERIOD/1000.0);

PwmOut ESC[4] = {p21, p22, p23, p24};



// ==========================
// === MAIN SETUP ROUTINE ===
// ==========================
bool setupALLdevices(void)
{   
    bool error = false;
    box_demo = false;
    mode = ATTITUDE;

    if (!setup_ESC()) {
        PC.printf("ESC FAILED!!!\n");
        error = true;
    }

    if (setup_bt())
        PC.printf("BT established!\n");
    else error = true;

    if (setup_PID()) 
        PC.printf("PID established!\n");
    else error = true;

    if (setup_mpu6050()) 
        PC.printf("MPU6050 established!\n");
    else error = true;

    if (setup_altimeter()) 
        PC.printf("Altimeter established!\n");
     else {
         error = true;
        PC.printf("ALTIMETER FAILED\n");
     }

#ifdef ENABLE_COMPASS
    if (setup_compass())
        PC.printf("Compass established!\n");
    else error = true;
#endif

    return error;
}




// *************************
// *** ESC SETUP ROUTINE ***
// *************************
bool setup_ESC(void)
{
    for (int i = 0; i < 4; i++)
        ESC[i].period_us(ESC_PERIOD_US);

    for (int i = 0; i < 4; i++)
        ESC[i].pulsewidth_us(1000);

    for (int i = 0; i < 4; i++)
        ESCpower[i] = 990;

    armed = false;

    return true;
}

// ****************************************************************
// === BLUETOOTH SETUP ROUTINE ===
// ****************************************************************
bool setup_bt(void)
{
    BT.baud(115200);
    BT_CMD = 0;     // Place bluetooth into normal mode
    BT.printf("Bluetooth online!\n");
    return true;
}

// ****************************************************************
// === PID SETUP ROUTINE ===
// ****************************************************************
bool setup_PID(void)
{
    pitchPIDattitude.setInputLimits(-90.0, 90.0);
    pitchPIDattitude.setOutputLimits(-250, 250.0);
    pitchPIDattitude.setBias(0.0);
    pitchPIDattitude.setMode(AUTO_MODE);

    rollPIDattitude.setInputLimits(-90.0, 90.0);
    rollPIDattitude.setOutputLimits(-250, 250.0);
    rollPIDattitude.setBias(0.0);
    rollPIDattitude.setMode(AUTO_MODE);

    yawPIDrate.setInputLimits(-500.0, 500.0);
    yawPIDrate.setOutputLimits(-200.0, 200.0);
    yawPIDrate.setBias(0.0);
    yawPIDrate.setMode(AUTO_MODE);

    pitchPIDrate.setInputLimits(-500.0, 500.0);
    pitchPIDrate.setOutputLimits(-200.0, 200.0);
    pitchPIDrate.setBias(0.0);
    pitchPIDrate.setMode(AUTO_MODE);

    rollPIDrate.setInputLimits(-500.0, 500.0);
    rollPIDrate.setOutputLimits(-200.0, 200.0);
    rollPIDrate.setBias(0.0);
    rollPIDrate.setMode(AUTO_MODE);

    pitchPIDattitude.reset();
    rollPIDattitude.reset();
    yawPIDrate.reset();
    pitchPIDrate.reset();
    rollPIDrate.reset();

    return true;
}

// ****************************************************************
// === MPU6050 SETUP ROUTINE ===
// ****************************************************************
bool imu_available = false;

/*/ MPU control/status variables: */
bool dmpReady = false;          // set true if DMP init was successful
uint8_t devStatus;              // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;            // expected DMP packet size (default is 42 bytes)

bool setup_mpu6050(void)
{
    MPU6050 imu(p9, p10); 
    
    imu.reset();
    imu.debugSerial.baud(115200);
    wait_ms(5);

    imu.initialize();
    imu_available = imu.testConnection();

    imu.debugSerial.printf("imu status...\t\t\t");
    imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");

    if (imu_available) {
        devStatus = imu.dmpInitialize();
        // supply your own gyro offsets here, scaled for min sensitivity
        //imu.setXGyroOffset(55);
        //imu.setYGyroOffset(3);
        //imu.setZGyroOffset(-2);
        //mpu.setZAccelOffset(16282); // 1688 factory default for my test chip

        if(!devStatus) {
            imu.setDMPEnabled(true);
            while (!imu.getIntDataReadyStatus());
            packetSize = imu.dmpGetFIFOPacketSize();
            imu.debugSerial.printf("Packet Size: %d\n", packetSize);
            dmpReady = true;

            int8_t xgOffsetTC = imu.getXGyroOffset();
            int8_t ygOffsetTC = imu.getYGyroOffset();
            int8_t zgOffsetTC = imu.getZGyroOffset();

            imu.debugSerial.printf("X Offset: %4d Y Offset: %4d Z Offset: %4d\n", xgOffsetTC, ygOffsetTC, zgOffsetTC);

        } else {
            imu.debugSerial.printf("\tDMP setup failed!\n");
            return false;
        }

        imu.resetFIFO();
    } else {
        return false;
    }

    imu.debugSerial.printf("imu setup routine done!");

    return dmpReady;
}

// ****************************************************************
// === MPL3115A2 SETUP ROUTINE ===
// ****************************************************************
bool setup_altimeter(void)
{
    if (altimeter.init())
        return true;
    else
        return false;
}

#ifdef ENABLE_COMPASS
// ****************************************************************
// === HMC5883L SETUP ROUTINE ===
// ****************************************************************
bool setup_compass(void)
{
    compass.setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_75);
    return true;
}
#endif