File content as of revision 25:a7cfe421cb4a:

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

float KP_PITCH_STABLE = 0.8;    // 0.5;
float KP_ROLL_STABLE = 0.8;     // 0.5;
float KP_YAW_RATE = 8.8;        // 8.5 for RATE MODE
float KP_PITCH_RATE = 7.6;      // 8.5 for RATE MODE
float KP_ROLL_RATE = 7.6;       // 8.5 for RATE MODE

float PID_TI_RATE = 0.0;
float PID_TI_STABLE = 0.0;

#include "setup.h"
#include "tasks.h"

Serial BT(p28, p27);
DigitalOut BT_CMD(p29);
MPU6050 imu(p9, p10);           // MPU6050-Tilty SDA, SCL (18, 19)
MPL3115A2 altimeter(p9, p10);
#ifdef ENABLE_COMPASS
HMC5883L compass(p9, p10);
#endif

PID pitchPIDstable(KP_PITCH_STABLE, PID_TI_STABLE, 0.0, TASK1_PERIOD/1000.0);
PID rollPIDstable(KP_ROLL_STABLE, PID_TI_STABLE, 0.0, TASK1_PERIOD/1000.0);

PID yawPIDrate(KP_YAW_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
PID pitchPIDrate(KP_PITCH_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);
PID rollPIDrate(KP_ROLL_RATE, PID_TI_RATE, 0.0, TASK2_PERIOD/1000.0);

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



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

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

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

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

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

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

#ifdef ENABLE_COMPASS
    if (setup_compass())
        imu.debugSerial.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(0);

    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)
{
    pitchPIDstable.setInputLimits(-90.0, 90.0);
    pitchPIDstable.setOutputLimits(-250, 250.0);
    pitchPIDstable.setBias(0.0);
    pitchPIDstable.setMode(AUTO_MODE);

    rollPIDstable.setInputLimits(-90.0, 90.0);
    rollPIDstable.setOutputLimits(-250, 250.0);
    rollPIDstable.setBias(0.0);
    rollPIDstable.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);
    
    pitchPIDstable.reset();
    rollPIDstable.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)
{
    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