Diff: RTOS-Setup/src/setup.cpp

Revision:

55:fc7cc7a4821c

Parent:

54:a36d39a90c21

diff -r a36d39a90c21 -r fc7cc7a4821c RTOS-Setup/src/setup.cpp
--- a/RTOS-Setup/src/setup.cpp	Mon May 19 16:05:11 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,239 +0,0 @@
-/* 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