Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
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?
User | Revision | Line number | New 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 | } |