Matthias Grob / Mbed 2 deprecated FlyBed2

My fully self designed first stable working Quadrocopter Software.

Dependencies:   mbed

Dependents:   fluy343

/media/uploads/maetugr/dsc09031.jpg

main.cpp@10:14390c90c3f5, 2015-08-31 (annotated)

Committer:
maetugr
Date:
Mon Aug 31 20:20:50 2015 +0000
Revision:
10:14390c90c3f5
Parent:
8:e79c7939d6de 
before changing to MPU9250

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 8:e79c7939d6de 1 /* X- Configuration +-Configuration
maetugr 8:e79c7939d6de 2 m0 m3 m1 -- >
maetugr 8:e79c7939d6de 3 \ / | / \ /
maetugr 8:e79c7939d6de 4 / \ m2-------m0 V |
maetugr 8:e79c7939d6de 5 m1 m2 | \
maetugr 8:e79c7939d6de 6 m3 PITCH ROLL*/
maetugr 0:12950aa67f2a 7 #include "mbed.h"
maetugr 0:12950aa67f2a 8 #include "LED.h" // LEDs framework for blinking ;)
maetugr 0:12950aa67f2a 9 #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
maetugr 0:12950aa67f2a 10
maetugr 0:12950aa67f2a 11 #include "IMU_10DOF.h" // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085)
maetugr 0:12950aa67f2a 12 #include "RC_Channel.h" // RemoteControl Channels with PPM
maetugr 0:12950aa67f2a 13 #include "PID.h" // PID Library (slim, self written)
maetugr 5:06e978fd147a 14 #include "Servo.h" // Motor PPM using any DigitalOut Pin
maetugr 0:12950aa67f2a 15
maetugr 0:12950aa67f2a 16 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
maetugr 0:12950aa67f2a 17 #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors
maetugr 2:03e5f7ab473f 18 #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for
maetugr 7:ac2895479e34 19 #define YAWSPEED 1.0 // maximal speed of yaw rotation in degree per Rate
maetugr 0:12950aa67f2a 20 #define AILERON 0 // RC
maetugr 0:12950aa67f2a 21 #define ELEVATOR 1
maetugr 0:12950aa67f2a 22 #define RUDDER 2
maetugr 0:12950aa67f2a 23 #define THROTTLE 3
maetugr 1:5e2b81f2d0b4 24 #define CHANNEL8 4
maetugr 1:5e2b81f2d0b4 25 #define CHANNEL7 5
maetugr 1:5e2b81f2d0b4 26 #define CHANNEL6 6
maetugr 0:12950aa67f2a 27 #define ROLL 0 // Axes
maetugr 0:12950aa67f2a 28 #define PITCH 1
maetugr 0:12950aa67f2a 29 #define YAW 2
maetugr 0:12950aa67f2a 30
maetugr 8:e79c7939d6de 31 #define SQRT2 0.7071067811865
maetugr 8:e79c7939d6de 32
maetugr 7:ac2895479e34 33 //#define CONSTRAIN(VAL,LIMIT) ((VAL)<(-LIMIT)?(-LIMIT):((VAL)>(LIMIT)?(LIMIT):(VAL)))
maetugr 7:ac2895479e34 34
maetugr 5:06e978fd147a 35 bool armed = false; // is for security (when false no motor rotates any more)
maetugr 5:06e978fd147a 36 bool debug = true; // shows if we want output for the computer
maetugr 5:06e978fd147a 37 bool RC_present = false; // shows if an RC is present
maetugr 10:14390c90c3f5 38 float P_R = 2.5, I_R = 3.7, D_R = 0;
maetugr 7:ac2895479e34 39 float P_A = 1.865, I_A = 1.765, D_A = 0;
maetugr 5:06e978fd147a 40 //float P = 13.16, I = 8, D = 2.73; // PID values
maetugr 7:ac2895479e34 41 float PY = 3.2, IY = 0, DY = 0;
maetugr 5:06e978fd147a 42 //float PY = 5.37, IY = 0, DY = 3; // PID values for Yaw
maetugr 2:03e5f7ab473f 43 float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
maetugr 0:12950aa67f2a 44 float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send
maetugr 7:ac2895479e34 45 //float * command_pointer = &D; // TODO: pointer to varible that's going to be changed by UART command
maetugr 5:06e978fd147a 46
maetugr 5:06e978fd147a 47 /*float max[3] = {-10000,-10000,-10000};
maetugr 5:06e978fd147a 48 float min[3] = {10000,10000,10000};*/
maetugr 0:12950aa67f2a 49
maetugr 0:12950aa67f2a 50 LED LEDs;
maetugr 0:12950aa67f2a 51 PC pc(USBTX, USBRX, 921600); // USB
maetugr 1:5e2b81f2d0b4 52 //PC pc(p9, p10, 115200); // Bluetooth
maetugr 0:12950aa67f2a 53 IMU_10DOF IMU(p28, p27);
maetugr 1:5e2b81f2d0b4 54 RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p7,2), RC_Channel(p5,4), RC_Channel(p6,3), RC_Channel(p15,2), RC_Channel(p16,4), RC_Channel(p17,3)}; // no p19/p20 !
maetugr 7:ac2895479e34 55 PID Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
maetugr 7:ac2895479e34 56 PID Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)};
maetugr 5:06e978fd147a 57 Servo ESC[] = {Servo(p21,PPM_FREQU), Servo(p22,PPM_FREQU), Servo(p23,PPM_FREQU), Servo(p24,PPM_FREQU)}; // use any DigitalOit Pin
maetugr 0:12950aa67f2a 58
maetugr 1:5e2b81f2d0b4 59 extern "C" void mbed_reset();
maetugr 1:5e2b81f2d0b4 60
maetugr 0:12950aa67f2a 61 void executer() {
maetugr 1:5e2b81f2d0b4 62 char command = pc.getc();
maetugr 1:5e2b81f2d0b4 63 if (command == 'X')
maetugr 1:5e2b81f2d0b4 64 mbed_reset();
maetugr 5:06e978fd147a 65 if (command == '-')
maetugr 5:06e978fd147a 66 debug = !debug;
maetugr 2:03e5f7ab473f 67 if (command == 'A') {
maetugr 2:03e5f7ab473f 68 IMU.Acc.calibrate(100,0.05);
maetugr 2:03e5f7ab473f 69 pc.printf("\r\n***A***%.3f,%.3f,%.3f***\r\n", IMU.Acc.offset[ROLL], IMU.Acc.offset[PITCH], IMU.Acc.offset[YAW]);
maetugr 2:03e5f7ab473f 70 wait(10);
maetugr 2:03e5f7ab473f 71 }
maetugr 2:03e5f7ab473f 72 if (command == 'C') {
maetugr 2:03e5f7ab473f 73 IMU.Comp.calibrate(60);
maetugr 2:03e5f7ab473f 74 pc.printf("\r\n***C***%.3f,%.3f,%.3f***\r\n", IMU.Comp.offset[ROLL], IMU.Comp.offset[PITCH], IMU.Comp.offset[YAW]);
maetugr 2:03e5f7ab473f 75 wait(20);
maetugr 2:03e5f7ab473f 76 }
maetugr 2:03e5f7ab473f 77
maetugr 2:03e5f7ab473f 78 pc.putc(command);
maetugr 0:12950aa67f2a 79 LEDs.tilt(2);
maetugr 0:12950aa67f2a 80 }
maetugr 0:12950aa67f2a 81
maetugr 0:12950aa67f2a 82 int main() {
maetugr 0:12950aa67f2a 83 pc.attach(&executer);
maetugr 0:12950aa67f2a 84 while(1) {
maetugr 0:12950aa67f2a 85 // IMU
maetugr 0:12950aa67f2a 86 IMU.readAngles();
maetugr 2:03e5f7ab473f 87 //IMU.readAltitude(); // TODO: reading altitude takes much more time than the angles -> don't do this in your fast loop, Ticker?
maetugr 0:12950aa67f2a 88 //pc.printf("%.1f,%.1f,%.1f,%.1f'C,%.1fhPa,%.1fmaS,%.5fs,%.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.temperature, IMU.pressure, IMU.altitude, IMU.dt, IMU.dt_sensors); // Output for Python
maetugr 0:12950aa67f2a 89
maetugr 0:12950aa67f2a 90 // Arming / disarming
maetugr 2:03e5f7ab473f 91 RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe
maetugr 1:5e2b81f2d0b4 92 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
maetugr 1:5e2b81f2d0b4 93 armed = true;
maetugr 2:03e5f7ab473f 94 RC_angle[YAW] = IMU.angle[YAW];
maetugr 1:5e2b81f2d0b4 95 }
maetugr 2:03e5f7ab473f 96 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
maetugr 0:12950aa67f2a 97 armed = false;
maetugr 0:12950aa67f2a 98 }
maetugr 0:12950aa67f2a 99
maetugr 2:03e5f7ab473f 100 // Setting PID Values from auxiliary RC channels
maetugr 10:14390c90c3f5 101 //if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000)
maetugr 10:14390c90c3f5 102 // P_R = 0 + (((float)RC[CHANNEL8].read()) * 3 / 1000);
maetugr 8:e79c7939d6de 103 /*if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000)
maetugr 8:e79c7939d6de 104 I_R = 0 + (((float)RC[CHANNEL7].read()) * 12 / 1000);*/
maetugr 7:ac2895479e34 105 for(int i=0;i<3;i++)
maetugr 7:ac2895479e34 106 Controller_Angle[i].setPID(P_A,I_A,D_A);
maetugr 2:03e5f7ab473f 107 for(int i=0;i<2;i++)
maetugr 7:ac2895479e34 108 Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller
maetugr 7:ac2895479e34 109 Controller_Rate[YAW].setPID(PY,IY,DY);
maetugr 1:5e2b81f2d0b4 110
maetugr 2:03e5f7ab473f 111 // RC Angle ROLL-PITCH-Part
maetugr 2:03e5f7ab473f 112 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
maetugr 2:03e5f7ab473f 113 if (RC_present)
maetugr 2:03e5f7ab473f 114 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
maetugr 2:03e5f7ab473f 115 else
maetugr 2:03e5f7ab473f 116 RC_angle[i] = 0;
maetugr 2:03e5f7ab473f 117 }
maetugr 2:03e5f7ab473f 118
maetugr 2:03e5f7ab473f 119 // RC Angle YAW-Part
maetugr 7:ac2895479e34 120 if (RC_present && RC[THROTTLE].read() > 20)
maetugr 7:ac2895479e34 121 RC_angle[YAW] -= (RC[RUDDER].read()-500)*YAWSPEED/500;
maetugr 7:ac2895479e34 122
maetugr 2:03e5f7ab473f 123 float RC_yaw_adding; // temporary variable to take the desired yaw adjustment
maetugr 2:03e5f7ab473f 124 if (RC_present && RC[THROTTLE].read() > 20)
maetugr 2:03e5f7ab473f 125 RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500;
maetugr 1:5e2b81f2d0b4 126 else
maetugr 2:03e5f7ab473f 127 RC_yaw_adding = 0;
maetugr 7:ac2895479e34 128
maetugr 7:ac2895479e34 129 RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding < -180 ? RC_angle[YAW] + 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding;
maetugr 7:ac2895479e34 130 RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding;
maetugr 7:ac2895479e34 131
maetugr 7:ac2895479e34 132 /*float RC_yaw_adding; // temporary variable to take the desired yaw adjustment
maetugr 7:ac2895479e34 133 if (RC_present && RC[THROTTLE].read() > 20)
maetugr 7:ac2895479e34 134 RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500;
maetugr 7:ac2895479e34 135 else
maetugr 7:ac2895479e34 136 RC_yaw_adding = 0;
maetugr 7:ac2895479e34 137
maetugr 2:03e5f7ab473f 138 while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180
maetugr 2:03e5f7ab473f 139 if(RC_angle[YAW] + RC_yaw_adding < -180)
maetugr 2:03e5f7ab473f 140 RC_yaw_adding += 360;
maetugr 2:03e5f7ab473f 141 if(RC_angle[YAW] + RC_yaw_adding > 180)
maetugr 2:03e5f7ab473f 142 RC_yaw_adding -= 360;
maetugr 2:03e5f7ab473f 143 }
maetugr 7:ac2895479e34 144 RC_angle[YAW] += RC_yaw_adding; // the yaw angle is integrated from stick input*/
maetugr 1:5e2b81f2d0b4 145
maetugr 0:12950aa67f2a 146 // Controlling
maetugr 2:03e5f7ab473f 147 for(int i=0;i<2;i++) {
maetugr 7:ac2895479e34 148 Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:ac2895479e34 149 Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct
maetugr 7:ac2895479e34 150 }
maetugr 7:ac2895479e34 151 Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 8:e79c7939d6de 152 if (RC[THROTTLE].read() > 20)
maetugr 8:e79c7939d6de 153 Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct
maetugr 8:e79c7939d6de 154 else
maetugr 8:e79c7939d6de 155 Controller_Rate[2].compute(0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct
maetugr 7:ac2895479e34 156 /*for(int i=0;i<3;i++) {
maetugr 7:ac2895479e34 157 Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:ac2895479e34 158 Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct
maetugr 7:ac2895479e34 159 Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:ac2895479e34 160 Controller_Rate[i].compute(-Controller_Angle[i].Value, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct
maetugr 7:ac2895479e34 161 }*/
maetugr 7:ac2895479e34 162
maetugr 7:ac2895479e34 163 // OLD Controlling
maetugr 7:ac2895479e34 164 /*for(int i=0;i<2;i++) {
maetugr 2:03e5f7ab473f 165 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 4:b0a60b0b24a9 166 Controller[i].compute(RC_angle[i], IMU.angle[i], IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values for D and angle for P,I and get his advice to correct
maetugr 2:03e5f7ab473f 167 }
maetugr 2:03e5f7ab473f 168 Controller[YAW].setIntegrate(armed); // same for YAW
maetugr 2:03e5f7ab473f 169 if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180
maetugr 2:03e5f7ab473f 170 if (RC_angle[YAW] > IMU.angle[YAW])
maetugr 4:b0a60b0b24a9 171 Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);
maetugr 2:03e5f7ab473f 172 else
maetugr 4:b0a60b0b24a9 173 Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);
maetugr 2:03e5f7ab473f 174 else
maetugr 7:ac2895479e34 175 Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);*/
maetugr 0:12950aa67f2a 176
maetugr 0:12950aa67f2a 177 // Mixing
maetugr 8:e79c7939d6de 178 /*Motor_speed[2] = RC[THROTTLE].read() + Controller_Rate[PITCH].Value; // PITCH in direction + Configuration
maetugr 8:e79c7939d6de 179 Motor_speed[0] = RC[THROTTLE].read() - Controller_Rate[PITCH].Value; // PITCH against direction
maetugr 8:e79c7939d6de 180 Motor_speed[1] = RC[THROTTLE].read() + Controller_Rate[ROLL].Value; // ROLL in direction
maetugr 8:e79c7939d6de 181 Motor_speed[3] = RC[THROTTLE].read() - Controller_Rate[ROLL].Value; // ROLL against direction*/
maetugr 8:e79c7939d6de 182
maetugr 8:e79c7939d6de 183 Motor_speed[0] = RC[THROTTLE].read() +SQRT2*Controller_Rate[PITCH].Value +SQRT2*Controller_Rate[ROLL].Value; // PITCH in direction X Configuration
maetugr 8:e79c7939d6de 184 Motor_speed[1] = RC[THROTTLE].read() +SQRT2*Controller_Rate[PITCH].Value -SQRT2*Controller_Rate[ROLL].Value; // PITCH against direction
maetugr 8:e79c7939d6de 185 Motor_speed[2] = RC[THROTTLE].read() -SQRT2*Controller_Rate[PITCH].Value -SQRT2*Controller_Rate[ROLL].Value; // ROLL in direction
maetugr 8:e79c7939d6de 186 Motor_speed[3] = RC[THROTTLE].read() -SQRT2*Controller_Rate[PITCH].Value +SQRT2*Controller_Rate[ROLL].Value; // ROLL against direction
maetugr 5:06e978fd147a 187
maetugr 7:ac2895479e34 188 Motor_speed[0] -= Controller_Rate[YAW].Value;
maetugr 7:ac2895479e34 189 Motor_speed[2] -= Controller_Rate[YAW].Value;
maetugr 7:ac2895479e34 190 Motor_speed[3] += Controller_Rate[YAW].Value;
maetugr 7:ac2895479e34 191 Motor_speed[1] += Controller_Rate[YAW].Value;
maetugr 5:06e978fd147a 192
maetugr 0:12950aa67f2a 193 if (armed) // for SECURITY!
maetugr 0:12950aa67f2a 194 {
maetugr 7:ac2895479e34 195 debug = false;
maetugr 7:ac2895479e34 196 // PITCH
maetugr 7:ac2895479e34 197 //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
maetugr 7:ac2895479e34 198 //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
maetugr 7:ac2895479e34 199 // ROLL
maetugr 7:ac2895479e34 200 //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50;
maetugr 7:ac2895479e34 201 //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50;
maetugr 7:ac2895479e34 202 for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 7:ac2895479e34 203 ESC[i] = (int)Motor_speed[i]>50 ? (int)Motor_speed[i] : 50;
maetugr 0:12950aa67f2a 204
maetugr 0:12950aa67f2a 205 } else {
maetugr 0:12950aa67f2a 206 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 0:12950aa67f2a 207 ESC[i] = 0;
maetugr 0:12950aa67f2a 208 }
maetugr 0:12950aa67f2a 209
maetugr 5:06e978fd147a 210 if (debug) {
maetugr 4:b0a60b0b24a9 211 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", IMU.Acc.data[0], IMU.Acc.data[1], IMU.Acc.data[2], D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt);
maetugr 5:06e978fd147a 212 //MAIN OUTPUT pc.printf("%d,%.1f,%.1f,%.1f,%.3f,%.3f,%.3f,%.2f,%.2f\r\n", armed, IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW], Controller[ROLL].Value, Controller[PITCH].Value, Controller[YAW].Value, P, D); // RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read()
maetugr 3:e277653258ab 213 //pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", armed, P, PY, D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt);
maetugr 3:e277653258ab 214 //pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", armed, P, PY, D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt);
maetugr 5:06e978fd147a 215 //pc.printf("%+.3f,%+.3f,%+.3f,%+.3f,%+.3f,%+.3f,%.5f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.Sensor.data_gyro[0], IMU.Sensor.data_gyro[1], IMU.Sensor.data_gyro[2], IMU.dt);
maetugr 5:06e978fd147a 216 pc.printf("$STATE,%d,%.3f\r\n", armed, IMU.dt);
maetugr 5:06e978fd147a 217 pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read());
maetugr 5:06e978fd147a 218 pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW]);
maetugr 5:06e978fd147a 219 pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_acc[ROLL], IMU.Sensor.data_acc[PITCH], IMU.Sensor.data_acc[YAW]);
maetugr 5:06e978fd147a 220 pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
maetugr 7:ac2895479e34 221 pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]);
maetugr 7:ac2895479e34 222 pc.printf("$CONT,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R);
maetugr 5:06e978fd147a 223 pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]);
maetugr 5:06e978fd147a 224 /*for (int i=0;i<3;i++) {
maetugr 5:06e978fd147a 225 min[i] = IMU.Sensor.data_gyro[i]<min[i] ? IMU.Sensor.data_gyro[i] : min[i];
maetugr 5:06e978fd147a 226 max[i] = IMU.Sensor.data_gyro[i]>max[i] ? IMU.Sensor.data_gyro[i] : max[i];
maetugr 5:06e978fd147a 227 }*/
maetugr 5:06e978fd147a 228 //pc.printf("%.5f\r\n", IMU.dt);
maetugr 5:06e978fd147a 229 //pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", IMU.Sensor.raw_gyro[ROLL], IMU.Sensor.raw_gyro[PITCH], IMU.Sensor.raw_gyro[YAW], min[0], min[1], min[2], max[0], max[1], max[2]);
maetugr 5:06e978fd147a 230 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW], min[0], min[1], min[2], max[0], max[1], max[2]);
maetugr 7:ac2895479e34 231 //pc.printf("%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW]);
maetugr 7:ac2895479e34 232
maetugr 7:ac2895479e34 233 // SimPlot output
maetugr 7:ac2895479e34 234 /*int16_t sendvalue[4]; //Buffer to hold the packet, note it is 16bit data type
maetugr 7:ac2895479e34 235 sendvalue[0] = (int16_t) IMU.Sensor.data_gyro[ROLL]; //Channel 1 data. 16bit signed integer
maetugr 7:ac2895479e34 236 sendvalue[1] = (int16_t) IMU.Sensor.data_gyro[PITCH]; //Channel 2 data. 16bit signed integer
maetugr 7:ac2895479e34 237 sendvalue[2] = (int16_t) IMU.Sensor.data_gyro[YAW]; //Channel 3 data. 16bit signed integer
maetugr 7:ac2895479e34 238 sendvalue[3] = (int16_t) 0; //Channel 4 data. 16bit signed integer
maetugr 7:ac2895479e34 239
maetugr 7:ac2895479e34 240 pc.putc(0xAB); // header
maetugr 7:ac2895479e34 241 pc.putc(0xCD);
maetugr 7:ac2895479e34 242 pc.putc(0x08); // size LSB
maetugr 7:ac2895479e34 243 pc.putc(0x00); // size MSB
maetugr 7:ac2895479e34 244 for(int i=0; i<4; i++) {
maetugr 7:ac2895479e34 245 pc.putc((char)sendvalue[i]); // LSB
maetugr 7:ac2895479e34 246 pc.putc((char)(sendvalue[i] >> 8)); // MSB
maetugr 7:ac2895479e34 247 }*/
maetugr 7:ac2895479e34 248
maetugr 5:06e978fd147a 249 wait(0.04);
maetugr 5:06e978fd147a 250 }
maetugr 0:12950aa67f2a 251
maetugr 0:12950aa67f2a 252 LEDs.rollnext();
maetugr 0:12950aa67f2a 253 }
maetugr 0:12950aa67f2a 254 }