Arduino - Quad_V1

Extrait intégral de la première version du programme permettant le contrôle d'un multicopter à 4 moteurs disposés en "X". Le but est de faire un programme adapter à la carte utilisée (Drotek avec ITG3200 et BMA180). Les programmes libres genres ArduPilote, Fcwii sont trop polyvalent pour que je puisse modifier ces programmes facilement. La solution pour moi a donc été de partir de Zéro.

Programme V1 avec atmega 328p sur board Arduino Uno et arduino. Utilisation d'une bibliothèque supplémentaire "MegaServo" permettant la gestion de plus de 3 signaux pwm en sorties sans interruption sur celles-ci. Acquisition des entrées Pwm sur détection de l'état bas du signal "throttle" qui déclenche une interuption puis acquiert avec la fonction PulseIn les largeurs d'impulsion de "roll, elev, yaw, throttle et aux". La voie "aux" permet d'avoir un Gain réglable par la radio.

Code de Quad_V1.ino :

1. #include
2. #include
3. // define input RC
4. #define THROTTLEPIN 2
5. #define ROLLPIN 4
6. #define ELEVPIN 5
7. #define YAWPIN 6
8. #define AUXPIN 7
9. #define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; // PIN A4&A5 (SDA&SCL)
10. // define adress sensor
11. #define ITGadr 0x68
12. #define BMA180adr 64
13. // define Output RC
14. #define MFtL 3 //M1
15. #define MFtR 10 //M2
16. #define MRrR 9 //M3
17. #define MRrL 11 //M4
18. #define LedBoard 13
19. // define limit system
20. #define LIMITCORRECT 300
21. #define TIMEOUT 20000 //time for pwm scan
22. #define MINPWM 1120
23. #define RANGE_RC 160 // velocity stick
24. #define RANGE_RC_Yaw 200 // velocity stick yaw
25. #define SafePWM 1030
26. #define DEADZONE 2
27. #define FILTRE_MOY 4
28. #define CONC_ACC_GYRO 250
29. // volatile, important for interruption
30. volatile int throttle = 0;
31. volatile int roll = 0;
32. volatile int elev = 0;
33. volatile int yaw = 0;
34. volatile int throttle1 = 0;
35. volatile int roll1 = 0;
36. volatile int elev1 = 0;
37. volatile int yaw1 = 0;
38. volatile int aux= 0;
39. volatile byte timeSlot;
40. volatile int outMFtR = SafePWM, outMFtL = SafePWM, outMRrR = SafePWM, outMRrL = SafePWM;
41. int SerialValue = 0;
42. int BufferInt;
43. char Buffer;
44. int Xgyro, Ygyro, Zgyro;
45. int X0gyro=0, Y0gyro=0, Z0gyro=0;
46. int X1gyro, Y1gyro, Z1gyro;
47. int Xacc, Yacc, Zacc;
48. int X1acc, Y1acc, Z1acc;
49. int X0acc=0, Y0acc=0, Z0acc=0;
50. float Xcorrect, Ycorrect, Zcorrect;
51. float X1correct = 0, Y1correct = 0, Z1correct = 0;
52. // int Xngyro[3], Yngyro[3], Zngyro[3];
53. unsigned long t1, t2, dT, Freq20ms, SpyFreq20ms, time;
54. unsigned int td = 7;
55. float ti = 50000000; //500000
56. float Gain , Gain0;
57. int Xerror, Yerror, Zerror;
58. float Pterm, Iterm, Dterm;
59. byte i;
60. boolean ARMED, WritePwm=0;
61. float filter = 0.5, filterAcc = 0.1, RCfilter = 0.8; //Gain filter
62. //float tabGyro[1000];
63. int LimitAcc;
64. //float kxl=1, kxr=1, kyf=1, kyr=1;
65. byte i_filtre = 0;
66. // only for debug
67. volatile unsigned int time1, time2 ;
68. //define ESC
69. MegaServo escMFtL; //pin drotek n°3
70. MegaServo escMFtR; //pin drotek n°10
71. MegaServo escMRrR; //pin drotek n°9
72. MegaServo escMRrL; //pin drotek n°11
73. void setup(){
74. PORTD = (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7);
75. pinMode (THROTTLEPIN, INPUT);
76. pinMode (ROLLPIN, INPUT);
77. pinMode (ELEVPIN, INPUT);
78. pinMode (YAWPIN, INPUT);
79. pinMode (AUXPIN, INPUT);
80. pinMode (LedBoard, OUTPUT);
81.  
82. digitalWrite(LedBoard,HIGH);
83. escMFtL.attach(MFtL); escMFtR.attach(MFtR); escMRrR.attach(MRrR); escMRrL.attach(MRrL);
84. SafeCmdMotor();
85. attachInterrupt(0, interruptHandler, RISING); // throttle pin 2
86. // Serial.begin(9600);
87. Wire.begin();
88. delay(100);
89. I2C_PULLUPS_ENABLE;
90. //Set the gyroscope scale for the outputs to +/-2000 degrees per second
91. WriteDataI2C(ITGadr, 0x16, B00011001);
92. //Set the sample rate to 000 => 256Hz with 8000 hz
93. //Set the sample rate to 001 (1) => 188Hz with 1000 hz (5ms)
94. //Set the sample rate to 010 (2) => 98Hz with 1000 hz (10ms)
95. WriteDataI2C(ITGadr, 0x15, 18);
96. // see page 23 datasheet - Fsample gyro = 500µs with 8KHz
97. // for 10ms, put 79, Fsample gyro = 10ms with 8KHz
98. // for 5ms, put 39, Fsample gyro = 5ms with 8KHz
99. //WriteDataI2C(BMA180adr, 0x35, 0x03);
100. delay(2000);
101. X0gyro = ReadDataI2C(ITGadr, 0x1F, 0x20);
102. Y0gyro = ReadDataI2C(ITGadr, 0x1D, 0x1E);
103. Z0gyro = ReadDataI2C(ITGadr, 0x21, 0x22);
104. X1gyro = X0gyro; Y1gyro = Y0gyro; Z1gyro = Z0gyro;
105. X0acc = (ReadDataI2C(BMA180adr, 0x03, 0x02) & 0xFFF0);
106. Y0acc = (ReadDataI2C(BMA180adr, 0x05, 0x04) & 0xFFF0);
107. Z0acc = (ReadDataI2C(BMA180adr, 0x07, 0x06) & 0xFFF0);
108. X1acc = X0acc; Y1acc = Y0acc; Z1acc = Z0acc;
109. for (i=0 ;i<4; i++) { digitalWrite(LedBoard, HIGH); delay(100); digitalWrite(LedBoard, LOW); delay(100); }
110. t1=micros(); SpyFreq20ms=t1;
111. }
112. void loop(){
113. Xacc = (ReadDataI2C(BMA180adr, 0x03, 0x02) & 0xFFF0) ; Yacc = (ReadDataI2C(BMA180adr, 0x05, 0x04) & 0xFFF0) ;// Zacc = (ReadDataI2C(BMA180adr, 0x07, 0x06) & 0xFFF0);
114. Xgyro = ReadDataI2C(ITGadr, 0x1F, 0x20)-X0gyro; Ygyro = ReadDataI2C(ITGadr, 0x1D, 0x1E)-Y0gyro; Zgyro = ReadDataI2C(ITGadr, 0x21, 0x22)-Z0gyro;
115. // FILTER BY OUPUT=PREVIOUS_GYRO+(FILTER*(GYRO-PREVIOUS_GYRO))
116. Xacc = X1acc + (filterAcc*(Xacc-X1acc)); Yacc = Y1acc + (filterAcc*(Yacc-Y1acc)); 
117. Xgyro = X1gyro + (filter*(Xgyro-X1gyro)); Ygyro = Y1gyro + (filter*(Ygyro-Y1gyro)); Zgyro = Z1gyro + (filter*(Zgyro-Z1gyro));
118. // calc delta time for 1 total cycle
119. t1 = t2;
120. t2 = micros();
121. dT = t2-t1; // dt in millisecond time intergral and derivate Ti and Td => sec | dt ~ 56 ms
122. Gain = 0.51;
123. LimitAcc = map(aux,1000,2000,10,500);
124. //roll
125. //rotate trigo -
126. //rotate time +
127. Xacc = map(Xacc,-16383,16383,-LimitAcc,LimitAcc);
128. Yacc = map(Yacc,-16383,16383,-LimitAcc,LimitAcc);
129. //if ((Xacc > CONC_ACC_GYRO) || (Xacc <- CONC_ACC_GYRO)) {
130. //Redondance before correction
131. Pterm = Gain*Xgyro;
132. Pterm /= 16;
133. Xcorrect = Pterm+Dterm+Iterm-Xacc;
134. Xcorrect = constrain(Xcorrect, -LIMITCORRECT, LIMITCORRECT);
135. //ELEV
136. //up => +
137. //down => -
138. Pterm = Gain*Ygyro;
139. Pterm /= 16;
140. Iterm = 0;
141. Dterm = 0;
142. Ycorrect = Pterm+Dterm+Iterm+Yacc;
143. Ycorrect = constrain(Ycorrect, -LIMITCORRECT, LIMITCORRECT);
144. Pterm = Gain*Zgyro;
145. Zcorrect = Pterm;//+Iterm+Dterm;
146. Pterm /= 16;
147. Zcorrect = constrain(Zcorrect, -210, 210);
148. // Save Mesure n-1
149. X1acc = Xacc; Y1acc = Yacc; Z1acc = Zacc;
150. X1gyro = Xgyro; Y1gyro = Ygyro; Z1gyro = Zgyro;
151. Freq20ms = micros()-SpyFreq20ms;
152. if (Freq20ms > 17500) { // refresh ~50Hz pwm, 20000µs
153. if (throttle<1100) { SafeCmdMotor(); } else { WriteEscPwm(); }
154. SpyFreq20ms = micros();
155. } 
156. }

Fichier function.ino :

1. void interruptHandler() {
2. //  time1 = micros();
3. //  switch ((timeSlot += 1) % 5) {
4. //    case 0:
5.     if (((timeSlot += 1) % 5)==0) {
6.     throttle = pulseIn(THROTTLEPIN, HIGH, TIMEOUT);
7.     roll = map(pulseIn(ROLLPIN, HIGH, TIMEOUT), 1000,2000,-RANGE_RC,RANGE_RC);
8.     roll = int(roll1+(RCfilter*(roll-roll1)));
9.     aux = pulseIn(AUXPIN, HIGH, TIMEOUT);
10.     elev = map(pulseIn(ELEVPIN, HIGH, TIMEOUT), 1000,2000,-RANGE_RC,RANGE_RC);
11.     elev = int(elev1+(RCfilter*(elev-elev1)));
12.     yaw = map(pulseIn(YAWPIN, HIGH, TIMEOUT), 1000,2000,-RANGE_RC_Yaw,RANGE_RC_Yaw);
13.     yaw = int(yaw1+(RCfilter*(yaw-yaw1)));
14.     roll1 = roll;
15.     elev1 = elev;
16.     yaw1 = yaw;
17. //    break;
18. //    default:
19. //    break;
20.     }
21. //    time2 = micros()-time1;
22. //      Serial.println(time2);
23. }
24.  
25. void WriteEscPwm(){
26.     detachInterrupt(0);
27.     /*
28.     outMFtL = throttle - elev + roll - yaw - int(kxl*Xcorrect) - int(kyf*Ycorrect) - Zcorrect;
29.     outMFtR = throttle - elev - roll + yaw + int(kxr*Xcorrect) - int(kyf*Ycorrect) + Zcorrect;
30.     outMRrR = throttle + elev - roll - yaw + int(kxr*Xcorrect) + int(kyr*Ycorrect) - Zcorrect;
31.     outMRrL = throttle + elev + roll + yaw - int(kxl*Xcorrect) + int(kyr*Ycorrect) + Zcorrect;
32. */
33.     outMFtL = throttle - elev + roll - yaw - Xcorrect - Ycorrect - Zcorrect;
34.     outMFtR = throttle - elev - roll + yaw + Xcorrect - Ycorrect + Zcorrect;
35.     outMRrR = throttle + elev - roll - yaw + Xcorrect + Ycorrect - Zcorrect;
36.     outMRrL = throttle + elev + roll + yaw - Xcorrect + Ycorrect + Zcorrect;
37.  
38.     outMFtL = constrain(outMFtL, MINPWM, 2000);
39.     outMFtR = constrain(outMFtR, MINPWM, 2000);
40.     outMRrR = constrain(outMRrR, MINPWM, 2000);
41.     outMRrL = constrain(outMRrL, MINPWM, 2000);
42.     escMFtL.writeMicroseconds(outMFtL);
43.     escMFtR.writeMicroseconds(outMFtR);
44.     escMRrR.writeMicroseconds(outMRrR);
45.     escMRrL.writeMicroseconds(outMRrL);
46.     attachInterrupt(0, interruptHandler, RISING);
47. }
48.  
49. void SafeCmdMotor(){
50.   escMFtL.writeMicroseconds(SafePWM);
51.   escMFtR.writeMicroseconds(SafePWM);
52.   escMRrR.writeMicroseconds(SafePWM);
53.   escMRrL.writeMicroseconds(SafePWM);
54. }
55.  
56. void WriteDataI2C(char address, char registerAddress, char data) {
57.   Wire.beginTransmission(address);
58.   //Tell the I2C address which register we are writing to
59.   Wire.write(registerAddress);
60.   //Send the value to write to the specified register
61.   Wire.write(data);
62.   //End the communication sequence
63.   Wire.endTransmission();
64. }
65.  
66. // ***************Function : Extract I2C sensor data 8+8 bits, registrer => adress High Byte*****************
67. word ReadDataI2C(int Wadress, int Wregister1, int Wregister2) {
68. // extract High Byte data
69.   Wire.beginTransmission(Wadress);
70.   Wire.write(Wregister1);
71.   Wire.endTransmission();
72.   Wire.requestFrom(Wadress,1);
73.   byte Byte_Out_H = Wire.read();
74.  
75. // extract Low Byte data
76.   Wire.beginTransmission(Wadress);
77.   Wire.write(Wregister2);
78.   Wire.endTransmission();
79.   Wire.requestFrom(Wadress,1);
80.   byte Byte_Out_L = Wire.read(); // & B11111110;
81.   return word(Byte_Out_H, Byte_Out_L);
82. }