YMFC-AL implementation in mbed.

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
iforce2d
Date:
Fri Oct 04 17:04:41 2019 +0000
Parent:
0:f76c26307f9a
Commit message:
First commit

Changed in this revision

PPM.cpp Show annotated file Show diff for this revision Revisions of this file
PPM.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r f76c26307f9a -r 411d267f9d32 PPM.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PPM.cpp	Fri Oct 04 17:04:41 2019 +0000
@@ -0,0 +1,27 @@
+#include "mbed.h"
+#include "PPM.h"
+ 
+PPM::PPM(PinName pin): ppm(pin)
+{
+    for (int i = 0; i < NUM_CHANNELS; i++)
+        channels[i] = 1500;
+    currentChannel = 0;    
+    timer.start();
+    ppm.rise( callback(this, &PPM::rise) );
+}
+            
+void PPM::rise()
+{
+    uint16_t time = timer.read_us();
+    timer.reset();
+    
+    if ( time > 2500 )
+    {
+       currentChannel = 0;
+    }
+    else if ( currentChannel < NUM_CHANNELS )
+    {
+        channels[currentChannel] = time;
+        currentChannel++;
+    }
+}
\ No newline at end of file
diff -r f76c26307f9a -r 411d267f9d32 PPM.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PPM.h	Fri Oct 04 17:04:41 2019 +0000
@@ -0,0 +1,20 @@
+#ifndef PPM_IN
+#define PPM_IN
+ 
+class PPM
+{
+    public:    
+        static const uint8_t NUM_CHANNELS = 6;
+        uint16_t channels[NUM_CHANNELS];
+        
+        PPM(PinName pin);
+        
+        void rise();
+ 
+    protected:
+        InterruptIn ppm;
+        Timer timer;
+        uint8_t currentChannel;
+};
+ 
+#endif
\ No newline at end of file
diff -r f76c26307f9a -r 411d267f9d32 main.cpp
--- a/main.cpp	Thu Feb 14 17:24:48 2013 +0000
+++ b/main.cpp	Fri Oct 04 17:04:41 2019 +0000
@@ -1,25 +1,483 @@
 #include "mbed.h"
- 
-// Read temperature from LM75BD
+#include "PPM.h"
+
+I2C i2c(PB_11, PB_10);  // sda, scl
+Serial pc(PA_9, PA_10, 115200); // tx, rx
+DigitalOut led_green(PB_3);
+DigitalOut led_red(PB_4);
+PPM ppm(PA_0);
+
+DigitalOut motorPin1(PA_8);
+DigitalOut motorPin2(PA_11);
+DigitalOut motorPin3(PB_6);
+DigitalOut motorPin4(PB_7);
+DigitalOut motorPin5(PB_8);
+DigitalOut motorPin6(PB_9);
+
+DigitalOut buzzer(PA_12);
+
+#define ROLL  0
+#define PITCH 1
+#define YAW   2
+
+#define RC_ROLL      (ppm.channels[0])
+#define RC_PITCH     (ppm.channels[1])
+#define RC_THROTTLE  (ppm.channels[2])
+#define RC_YAW       (ppm.channels[3])
+#define RC_DEADBAND_US 8
+
+#define GYRO_CALIB_SAMPLES 4000
+#define MPU6050 (0x68 << 1)
+
+float pid_p_gain_roll = 1.3;               //Gain setting for the roll P-controller
+float pid_i_gain_roll = 0.04;              //Gain setting for the roll I-controller
+float pid_d_gain_roll = 18.0;              //Gain setting for the roll D-controller
+int pid_max_roll = 400;                    //Maximum output of the PID-controller (+/-)
+
+float pid_p_gain_pitch = pid_p_gain_roll;  //Gain setting for the pitch P-controller.
+float pid_i_gain_pitch = pid_i_gain_roll;  //Gain setting for the pitch I-controller.
+float pid_d_gain_pitch = pid_d_gain_roll;  //Gain setting for the pitch D-controller.
+int pid_max_pitch = pid_max_roll;          //Maximum output of the PID-controller (+/-)
+
+float pid_p_gain_yaw = 4.0;                //Gain setting for the pitch P-controller. //4.0
+float pid_i_gain_yaw = 0.02;               //Gain setting for the pitch I-controller. //0.02
+float pid_d_gain_yaw = 0.0;                //Gain setting for the pitch D-controller.
+int pid_max_yaw = 400;                     //Maximum output of the PID-controller (+/-)
+
+int cal_int;
+double gyro_cal[3];
+int16_t acc_raw[3], gyro_raw[3];
+float acc_smoothed[3], gyro_smoothed[3];
+
+Timer armingTimer;
+
+Timer timer;
+
+float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
+float acc_total_vector;
+
+float roll_level_adjust, pitch_level_adjust;
+float pid_i_mem_roll, pid_roll_setpoint, pid_output_roll, pid_last_roll_d_error;
+float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_pitch, pid_last_pitch_d_error;
+float pid_i_mem_yaw, pid_yaw_setpoint, pid_output_yaw, pid_last_yaw_d_error;
+
+int constrain( int val, int lower, int upper )
+{
+    if ( val < lower ) return lower;
+    if ( val > upper ) return upper;
+    return val;
+}
+
+void beepStart() 
+{
+    buzzer = 0;
+}
+
+void beepStop() 
+{
+    buzzer = 1;
+}
+
+void beep(int ms) 
+{
+    beepStart();
+    wait_ms(ms);
+    beepStop();
+}
+
+void setIMURegisters() 
+{    
+    char cmd[2];
+    
+    cmd[0] = 0x6B;                                                          //We want to write to the PWR_MGMT_1 register (6B hex)
+    cmd[1] = 0x00;                                                          //Set the register bits as 00000000 to activate the gyro
+    i2c.write(MPU6050, cmd, 2);
+    
+    cmd[0] = 0x1B;                                                          //We want to write to the GYRO_CONFIG register (1B hex)
+    cmd[1] = 0x08;                                                          //Set the register bits as 00001000 (500dps full scale)
+    i2c.write(MPU6050, cmd, 2);
+    
+    cmd[0] = 0x1C;                                                          //We want to write to the ACCEL_CONFIG register (1A hex)
+    cmd[1] = 0x10;                                                          //Set the register bits as 00010000 (+/- 8g full scale range)
+    i2c.write(MPU6050, cmd, 2);
+    
+    //Let's perform a random register check to see if the values are written correct
+    cmd[0] = 0x1B;                                                          //Start reading at register 0x1B
+    i2c.write(MPU6050, cmd, 1);    
+    i2c.read(MPU6050, cmd, 1);
+    if (cmd[0] != 0x08) {                                                   //Check if the value is 0x08
+        led_red = 0;                                                        //Turn on the warning led
+        pc.printf("Gyro init failed! (%d)\n", cmd[0]);
+        while (true)
+            wait(1);                                                      //Stay in this loop for ever
+    }
+    
+    cmd[0] = 0x1A;                                                          //We want to write to the CONFIG register (1A hex)
+    cmd[1] = 0x03;                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
+    i2c.write(MPU6050, cmd, 2);
+}
+
+void readIMU(bool calibrated = true)
+{    
+    char cmd[14];
 
-I2C i2c(p28, p27);
+    cmd[0] = 0x3B;                                                          //Start reading at register 43h and auto increment with every read.
+    i2c.write(MPU6050, cmd, 1);
+    i2c.read(MPU6050, cmd, 14);                                             //Read 14 bytes from the gyro.
+    
+    acc_raw[0] = (cmd[0] << 8) | cmd[1];                               //Add the low and high byte to the acc_x variable.
+    acc_raw[1] = (cmd[2] << 8) | cmd[3];                               //Add the low and high byte to the acc_y variable.
+    acc_raw[2] = (cmd[4] << 8) | cmd[5];                               //Add the low and high byte to the acc_z variable.
+    
+    gyro_raw[0] = (cmd[8] << 8) |  cmd[9];                              //Read high and low part of the angular data.
+    gyro_raw[1] = (cmd[10] << 8) | cmd[11];                              //Read high and low part of the angular data.
+    gyro_raw[2] = (cmd[12] << 8) | cmd[13];                              //Read high and low part of the angular data.
+    
+    if ( calibrated ) {
+        for (int i = 0; i < 3; i++)
+            gyro_raw[i] -= gyro_cal[i];                                       
+    }
+}
+
+void calibrateGyro() 
+{
+    for (int i = 0; i < 3; i++) 
+        gyro_cal[i] = 0;
+    
+    for (int s = 0; s < GYRO_CALIB_SAMPLES; s++) {                            //Take GYRO_CALIB_SAMPLES readings for calibration.
+        if (s % 50 == 0)
+            led_green = 1 - led_green;                                          //Change the led status to indicate calibration.
+        readIMU(false);                                                        //Read the gyro output.
+        for (int i = 0; i < 3; i++)
+            gyro_cal[i] += gyro_raw[i];                                       //Ad roll value to gyro_roll_cal.
+        wait_us(3);                                                               //Wait 3 milliseconds before the next loop.
+    }
+    
+    for (int i = 0; i < 3; i++)
+        gyro_cal[i] /= (float)GYRO_CALIB_SAMPLES;                                                 //Divide the roll total by GYRO_CALIB_SAMPLES.
+}
 
-const int addr = 0x90;
+bool checkArmStateChange(bool armed) 
+{
+    static int armingState = 0;
+    
+    if ( RC_THROTTLE > 1100 )
+        return false;
+    
+    if ( ! armed && RC_YAW > 1900 ) {
+        if ( armingState == 0 ) {
+            armingState = 1;
+            armingTimer.reset();
+            armingTimer.start();
+            pc.printf("Started arming\n");
+        }
+        else {
+            uint16_t time = armingTimer.read_ms();
+            if ( time > 2000 ) {
+                armed = true;
+                armingState = 0;
+                pc.printf("ARMED\n");
+                //beep(300);
+                return true;
+            }
+        }
+    }
+    else if ( armed && RC_YAW < 1100 ) {
+        if ( armingState == 0 ) {
+            armingState = -1;
+            armingTimer.reset();
+            armingTimer.start();
+            pc.printf("Started disarming\n");
+        }
+        else {
+            uint16_t time = armingTimer.read_ms();
+            if ( time > 2000 ) {
+                armed = false;
+                armingState = 0;
+                pc.printf("DISARMED\n");
+                //beep(50);
+                //wait_ms(50);
+                //beep(50);
+                return false;
+            }
+        }
+    }
+    else {
+        armingState = 0;
+        armingTimer.reset();
+    }
+    
+    return armed;
+}
+
+bool getAutoLevelState()
+{
+    return ppm.channels[5] > 1600;
+}
+
+int afterDeadband(int in)
+{
+    int lower = 1500 - RC_DEADBAND_US;
+    int upper = 1500 + RC_DEADBAND_US;
+    if ( in < lower )
+        return in - lower;
+    if ( in > upper )
+        return in - upper;
+    return 0;
+}
+
+void resetPID()
+{
+    pid_i_mem_roll = 0;
+    pid_last_roll_d_error = 0;
+    pid_i_mem_pitch = 0;
+    pid_last_pitch_d_error = 0;
+    pid_i_mem_yaw = 0;
+    pid_last_yaw_d_error = 0;
+}
+
+void calculatePID()
+{
+    //Roll calculations
+    float pid_error_temp = gyro_smoothed[ROLL] - pid_roll_setpoint;
+    pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
+    if(pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
+    else if(pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;
+    
+    pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
+    if(pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
+    else if(pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;
+    
+    pid_last_roll_d_error = pid_error_temp;
+    
+    //Pitch calculations
+    pid_error_temp = gyro_smoothed[PITCH] - pid_pitch_setpoint;
+    pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
+    if(pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
+    else if(pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;
+    
+    pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
+    if(pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
+    else if(pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;
+    
+    pid_last_pitch_d_error = pid_error_temp;
+    
+    //Yaw calculations
+    pid_error_temp = gyro_smoothed[YAW] - pid_yaw_setpoint;
+    pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
+    if(pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
+    else if(pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;
+    
+    pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error);
+    if(pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
+    else if(pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;
+    
+    pid_last_yaw_d_error = pid_error_temp;
+}
 
 int main() {
-    char cmd[2];
-    while (1) {
-        cmd[0] = 0x01;
-        cmd[1] = 0x00;
-        i2c.write(addr, cmd, 2);
- 
-        wait(0.5);
- 
-        cmd[0] = 0x00;
-        i2c.write(addr, cmd, 1);
-        i2c.read(addr, cmd, 2);
- 
-        float tmp = (float((cmd[0]<<8)|cmd[1]) / 256.0);
-        printf("Temp = %.2f\n", tmp);
+    
+    i2c.frequency(400000);
+    
+    buzzer = 1;
+    led_red = 1;
+    led_green = 1;
+    
+    motorPin1 = 0;
+    motorPin2 = 0;
+    motorPin3 = 0;
+    motorPin4 = 0;
+    motorPin5 = 0;
+    motorPin6 = 0;
+    
+    armingTimer.reset();
+    
+    beep(50);
+        
+    pc.printf("setIMURegisters\n");    
+    setIMURegisters();
+    
+    pc.printf("calibrateGyro\n");
+    calibrateGyro();
+    
+    pc.printf("Finished gyro calibration (%f, %f, %f)\n", gyro_cal[0], gyro_cal[1], gyro_cal[2]);
+    
+    timer.reset();
+    timer.start();
+    
+    for (int i = 0; i < 3; i++)
+        gyro_smoothed[i] = 0;
+    angle_pitch = 0;
+    angle_roll = 0;
+        
+    beep(50);
+    wait_ms(50);
+    beep(50);
+    
+    bool armed = false;
+    bool armedLastTime = armed;
+    
+    bool autoLevel = getAutoLevelState();
+    bool autoLevelLastTime = autoLevel;
+    
+    unsigned long oneShotCounter = 0;
+    
+    unsigned long loopCounter = 0;
+    
+    float initialAccUptake = 0.8;
+    
+    while (true) {
+        
+        //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
+        //pc.printf("Accel (%f, %f)\n", angle_pitch, angle_roll);
+        
+        armed = checkArmStateChange(armed);
+        led_green = ! armed;
+        
+        if ( ! armedLastTime && armed )
+            resetPID();
+        if ( armed != armedLastTime ) {
+            oneShotCounter = 20;
+            beepStart();
+        }
+        armedLastTime = armed;
+        
+        autoLevel = getAutoLevelState();
+        if ( autoLevel != autoLevelLastTime ) {
+            oneShotCounter = 4;
+            beepStart();
+        }
+        autoLevelLastTime = autoLevel;
+        
+        if ( oneShotCounter > 0 ) {
+            if ( --oneShotCounter == 0 ) {
+                beepStop();
+            }
+        }
+        
+        const float uptake = 0.3;
+        for (int i = 0; i < 3; i++)
+            gyro_smoothed[i] = (gyro_smoothed[i] * (1-uptake)) + ((gyro_raw[i] / 65.5) * uptake);   //Gyro pid input is deg/sec.
+            
+            
+        //Gyro angle calculations
+        //0.0000611 = 1 / (250Hz / 65.5)
+        angle_pitch += gyro_raw[ROLL] * 0.0000611;                                    //Calculate the traveled pitch angle and add this to the angle_pitch variable.
+        angle_roll += gyro_raw[PITCH] * 0.0000611;                                      //Calculate the traveled roll angle and add this to the angle_roll variable.
+        
+        //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
+        float new_angle_pitch = angle_pitch + angle_roll * sin(gyro_raw[YAW] * 0.000001066);                  //If the IMU has yawed transfer the roll angle to the pitch angel.
+        angle_roll -= angle_pitch * sin(gyro_raw[YAW] * 0.000001066);                  //If the IMU has yawed transfer the pitch angle to the roll angel.
+        angle_pitch = new_angle_pitch;
+        
+        //Accelerometer angle calculations
+        acc_total_vector = sqrtf((acc_raw[0]*acc_raw[0])+(acc_raw[1]*acc_raw[1])+(acc_raw[2]*acc_raw[2]));       //Calculate the total accelerometer vector.
+        
+        if (abs(acc_raw[PITCH]) < acc_total_vector) {                                        //Prevent the asin function to produce a NaN
+            angle_pitch_acc = asin((float)acc_raw[PITCH]/acc_total_vector)* 57.296;          //Calculate the pitch angle.
+        }
+        if (abs(acc_raw[ROLL]) < acc_total_vector) {                                        //Prevent the asin function to produce a NaN
+            angle_roll_acc = asin((float)acc_raw[ROLL]/acc_total_vector)* -57.296;          //Calculate the roll angle.
+        }
+        
+        //Place the MPU-6050 spirit level and note the values in the following two lines for calibration.
+        angle_pitch_acc -= 0.0;                                                   //Accelerometer calibration value for pitch.
+        angle_roll_acc -= 0.0;                                                    //Accelerometer calibration value for roll.
+        
+        initialAccUptake *= 0.98;
+        
+        float acc_uptake = 0.0004 + initialAccUptake;
+        angle_pitch = angle_pitch * (1-acc_uptake) + angle_pitch_acc * acc_uptake;            //Correct the drift of the gyro pitch angle with the accelerometer pitch angle.
+        angle_roll = angle_roll * (1-acc_uptake) + angle_roll_acc * acc_uptake;               //Correct the drift of the gyro roll angle with the accelerometer roll angle.
+        
+        pitch_level_adjust = angle_pitch * 15;                                    //Calculate the pitch angle correction
+        roll_level_adjust = angle_roll * 15;                                      //Calculate the roll angle correction
+        
+        if ( ! autoLevel ){                                                          //If the quadcopter is not in auto-level mode
+            pitch_level_adjust = 0;                                                 //Set the pitch angle correction to zero.
+            roll_level_adjust = 0;                                                  //Set the roll angle correcion to zero.
+        }
+            
+           
+           
+        //The PID set point in degrees per second is determined by the roll receiver input.
+        //In the case of deviding by 3 the max roll rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
+        pid_roll_setpoint = afterDeadband(RC_ROLL);        
+        pid_roll_setpoint -= roll_level_adjust;                                   //Subtract the angle correction from the standardized receiver roll input value.
+        pid_roll_setpoint /= 3.0;                                                 //Divide the setpoint for the PID roll controller by 3 to get angles in degrees.
+        
+        //The PID set point in degrees per second is determined by the pitch receiver input.
+        //In the case of deviding by 3 the max pitch rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
+        pid_pitch_setpoint = afterDeadband(RC_PITCH);        
+        pid_pitch_setpoint -= pitch_level_adjust;                                  //Subtract the angle correction from the standardized receiver pitch input value.
+        pid_pitch_setpoint /= 3.0;                                                 //Divide the setpoint for the PID pitch controller by 3 to get angles in degrees.
+        
+        //The PID set point in degrees per second is determined by the yaw receiver input.
+        //In the case of dividing by 3 the max yaw rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
+        pid_yaw_setpoint = 0;
+        //We need a little dead band of 16us for better results.
+        if (RC_THROTTLE > 1050) { //Do not yaw when turning off the motors.
+            pid_yaw_setpoint = afterDeadband(RC_YAW) / 3.0;
+        }
+        
+        calculatePID();           
+           
+        int pwm[6];
+        
+        if (armed) {                                                          //The motors are started.
+            int throttle = RC_THROTTLE;
+            if (throttle > 1800) 
+                throttle = 1800;                                   //We need some room to keep full control at full throttle.
+            pwm[0] = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
+            pwm[1] = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
+            pwm[2] = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
+            pwm[3] = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)
+            pwm[4] = 1000;
+            pwm[5] = 1000;
+            
+            for (int i = 0; i < 6; i++)
+                pwm[i] = constrain( pwm[i], 1100, 2000);                                         //Keep the motors running.
+        }        
+        else {
+            for (int i = 0; i < 6; i++)
+                pwm[i] = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-1.
+        }
+    
+    
+    
+        
+        while ( timer.read_us() < 4000 )
+            ; // wait
+            
+        motorPin1 = motorPin2 = motorPin3 = motorPin4 = 1;
+        timer.reset();
+        
+        readIMU();
+        
+        if ( loopCounter++ % 20 == 0 ) {
+            //pc.printf("%d %d %d %d\n", pwm[0], pwm[1], pwm[2], pwm[3]);
+            pc.printf("%f %f\n", angle_roll, angle_pitch );
+            //pc.printf("%d\n", pwm[0] );
+            //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
+            //pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]);
+        }
+        
+        bool doneOutput = false;
+        while ( ! doneOutput ) {
+            int now = timer.read_us();
+            int c = 0;
+            if ( now >= pwm[0] ) { motorPin1 = 0; c++; }
+            if ( now >= pwm[1] ) { motorPin2 = 0; c++; }
+            if ( now >= pwm[2] ) { motorPin3 = 0; c++; }
+            if ( now >= pwm[3] ) { motorPin4 = 0; c++; }
+            if ( now >= pwm[4] ) { motorPin5 = 0; c++; }
+            if ( now >= pwm[5] ) { motorPin6 = 0; c++; }
+            doneOutput = c >= 6;
+        }
+        
     }
-}
\ No newline at end of file
+    
+    /*while (true) {
+        pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]);
+    }*/
+}
diff -r f76c26307f9a -r 411d267f9d32 mbed.bld
--- a/mbed.bld	Thu Feb 14 17:24:48 2013 +0000
+++ b/mbed.bld	Fri Oct 04 17:04:41 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/0954ebd79f59
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file