Arduino Drone With GPS
We have included instructions on how to replicate our product if you are happy with the more limited quadcopter.
Components and supplies
4
2830, 900kV brushless motors
1
quadcopter frame (the exact frame likely doesn't matter)
1
Female/Female Jumper Wires
1
Inertial Measurement Unit (IMU) (6 deg of freedom)
1
HC-12 wireless transceivers
1
power distribution board (with XT-60 connection)
1
Turnigy 9x 2.4GHz, 9 channel transmitter/receiver pair
1
One 3s, 3000-5000mAh LiPo battery with XT-60 connection (3000mAh corresponds with approx. 20 min of flight time)
1
Solo Propellers
4
20A UBEC ESCs
1
USB-A to Mini-USB Cable
1
LiPo Battery Balance charger (and 12V DC adapter, not, included)
2
Arduino UNO
Tools and machines
1
Soldering iron (generic)
Apps and platforms
1
Arduino IDE
Project description
Code
FQR6TWEJGGTGFQ7.ino
c_cpp
Code for drone
1/////////////////////////////////////////////////////////////////////////////////////// 2//Terms of use 3/////////////////////////////////////////////////////////////////////////////////////// 4//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 5//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 6//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 7//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 8//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 9//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 10//THE SOFTWARE. 11/////////////////////////////////////////////////////////////////////////////////////// 12//Safety note 13/////////////////////////////////////////////////////////////////////////////////////// 14//Always remove the propellers and stay away from the motors unless you 15//are 100% certain of what you are doing. 16/////////////////////////////////////////////////////////////////////////////////////// 17 18#include <Wire.h> //Include the Wire.h library so we can communicate with the gyro 19#include <EEPROM.h> //Include the EEPROM.h library so we can store information onto the EEPROM 20 21//Declaring Global Variables 22byte last_channel_1, last_channel_2, last_channel_3, last_channel_4; 23byte lowByte, highByte, type, gyro_address, error, clockspeed_ok; 24byte channel_1_assign, channel_2_assign, channel_3_assign, channel_4_assign; 25byte roll_axis, pitch_axis, yaw_axis; 26byte receiver_check_byte, gyro_check_byte; 27volatile int receiver_input_channel_1, receiver_input_channel_2, receiver_input_channel_3, receiver_input_channel_4; 28int center_channel_1, center_channel_2, center_channel_3, center_channel_4; 29int high_channel_1, high_channel_2, high_channel_3, high_channel_4; 30int low_channel_1, low_channel_2, low_channel_3, low_channel_4; 31int address, cal_int; 32unsigned long timer, timer_1, timer_2, timer_3, timer_4, current_time; 33float gyro_pitch, gyro_roll, gyro_yaw; 34float gyro_roll_cal, gyro_pitch_cal, gyro_yaw_cal; 35 36 37//Setup routine 38void setup(){ 39 pinMode(12, OUTPUT); 40 //Arduino (Atmega) pins default to inputs, so they don't need to be explicitly declared as inputs 41 PCICR |= (1 << PCIE0); // set PCIE0 to enable PCMSK0 scan 42 PCMSK0 |= (1 << PCINT0); // set PCINT0 (digital input 8) to trigger an interrupt on state change 43 PCMSK0 |= (1 << PCINT1); // set PCINT1 (digital input 9)to trigger an interrupt on state change 44 PCMSK0 |= (1 << PCINT2); // set PCINT2 (digital input 10)to trigger an interrupt on state change 45 PCMSK0 |= (1 << PCINT3); // set PCINT3 (digital input 11)to trigger an interrupt on state change 46 Wire.begin(); //Start the I2C as master 47 Serial.begin(57600); //Start the serial connetion @ 57600bps 48 delay(250); //Give the gyro time to start 49} 50//Main program 51void loop(){ 52 //Show the YMFC-3D V2 intro 53 intro(); 54 55 Serial.println(F("")); 56 Serial.println(F("===================================================")); 57 Serial.println(F("System check")); 58 Serial.println(F("===================================================")); 59 delay(1000); 60 Serial.println(F("Checking I2C clock speed.")); 61 delay(1000); 62 63 TWBR = 12; //Set the I2C clock speed to 400kHz. 64 65 #if F_CPU == 16000000L //If the clock speed is 16MHz include the next code line when compiling 66 clockspeed_ok = 1; //Set clockspeed_ok to 1 67 #endif //End of if statement 68 69 if(TWBR == 12 && clockspeed_ok){ 70 Serial.println(F("I2C clock speed is correctly set to 400kHz.")); 71 } 72 else{ 73 Serial.println(F("I2C clock speed is not set to 400kHz. (ERROR 8)")); 74 error = 1; 75 } 76 77 if(error == 0){ 78 Serial.println(F("")); 79 Serial.println(F("===================================================")); 80 Serial.println(F("Transmitter setup")); 81 Serial.println(F("===================================================")); 82 delay(1000); 83 Serial.print(F("Checking for valid receiver signals.")); 84 //Wait 10 seconds until all receiver inputs are valid 85 wait_for_receiver(); 86 Serial.println(F("")); 87 } 88 //Quit the program in case of an error 89 if(error == 0){ 90 delay(2000); 91 Serial.println(F("Place all sticks and subtrims in the center position within 10 seconds.")); 92 for(int i = 9;i > 0;i--){ 93 delay(1000); 94 Serial.print(i); 95 Serial.print(" "); 96 } 97 Serial.println(" "); 98 //Store the central stick positions 99 center_channel_1 = receiver_input_channel_1; 100 center_channel_2 = receiver_input_channel_2; 101 center_channel_3 = receiver_input_channel_3; 102 center_channel_4 = receiver_input_channel_4; 103 Serial.println(F("")); 104 Serial.println(F("Center positions stored.")); 105 Serial.print(F("Digital input 08 = ")); 106 Serial.println(receiver_input_channel_1); 107 Serial.print(F("Digital input 09 = ")); 108 Serial.println(receiver_input_channel_2); 109 Serial.print(F("Digital input 10 = ")); 110 Serial.println(receiver_input_channel_3); 111 Serial.print(F("Digital input 11 = ")); 112 Serial.println(receiver_input_channel_4); 113 Serial.println(F("")); 114 Serial.println(F("")); 115 } 116 if(error == 0){ 117 Serial.println(F("Move the throttle stick to full throttle and back to center")); 118 //Check for throttle movement 119 check_receiver_inputs(1); 120 Serial.print(F("Throttle is connected to digital input ")); 121 Serial.println((channel_3_assign & 0b00000111) + 7); 122 if(channel_3_assign & 0b10000000)Serial.println(F("Channel inverted = yes")); 123 else Serial.println(F("Channel inverted = no")); 124 wait_sticks_zero(); 125 126 Serial.println(F("")); 127 Serial.println(F("")); 128 Serial.println(F("Move the roll stick to simulate left wing up and back to center")); 129 //Check for throttle movement 130 check_receiver_inputs(2); 131 Serial.print(F("Roll is connected to digital input ")); 132 Serial.println((channel_1_assign & 0b00000111) + 7); 133 if(channel_1_assign & 0b10000000)Serial.println(F("Channel inverted = yes")); 134 else Serial.println(F("Channel inverted = no")); 135 wait_sticks_zero(); 136 } 137 if(error == 0){ 138 Serial.println(F("")); 139 Serial.println(F("")); 140 Serial.println(F("Move the pitch stick to simulate nose up and back to center")); 141 //Check for throttle movement 142 check_receiver_inputs(3); 143 Serial.print(F("Pitch is connected to digital input ")); 144 Serial.println((channel_2_assign & 0b00000111) + 7); 145 if(channel_2_assign & 0b10000000)Serial.println(F("Channel inverted = yes")); 146 else Serial.println(F("Channel inverted = no")); 147 wait_sticks_zero(); 148 } 149 if(error == 0){ 150 Serial.println(F("")); 151 Serial.println(F("")); 152 Serial.println(F("Move the yaw stick to simulate nose right and back to center")); 153 //Check for throttle movement 154 check_receiver_inputs(4); 155 Serial.print(F("Yaw is connected to digital input ")); 156 Serial.println((channel_4_assign & 0b00000111) + 7); 157 if(channel_4_assign & 0b10000000)Serial.println(F("Channel inverted = yes")); 158 else Serial.println(F("Channel inverted = no")); 159 wait_sticks_zero(); 160 } 161 if(error == 0){ 162 Serial.println(F("")); 163 Serial.println(F("")); 164 Serial.println(F("Gently move all the sticks simultaneously to their extends")); 165 Serial.println(F("When ready put the sticks back in their center positions")); 166 //Register the min and max values of the receiver channels 167 register_min_max(); 168 Serial.println(F("")); 169 Serial.println(F("")); 170 Serial.println(F("High, low and center values found during setup")); 171 Serial.print(F("Digital input 08 values:")); 172 Serial.print(low_channel_1); 173 Serial.print(F(" - ")); 174 Serial.print(center_channel_1); 175 Serial.print(F(" - ")); 176 Serial.println(high_channel_1); 177 Serial.print(F("Digital input 09 values:")); 178 Serial.print(low_channel_2); 179 Serial.print(F(" - ")); 180 Serial.print(center_channel_2); 181 Serial.print(F(" - ")); 182 Serial.println(high_channel_2); 183 Serial.print(F("Digital input 10 values:")); 184 Serial.print(low_channel_3); 185 Serial.print(F(" - ")); 186 Serial.print(center_channel_3); 187 Serial.print(F(" - ")); 188 Serial.println(high_channel_3); 189 Serial.print(F("Digital input 11 values:")); 190 Serial.print(low_channel_4); 191 Serial.print(F(" - ")); 192 Serial.print(center_channel_4); 193 Serial.print(F(" - ")); 194 Serial.println(high_channel_4); 195 Serial.println(F("Move stick 'nose up' and back to center to continue")); 196 check_to_continue(); 197 } 198 199 if(error == 0){ 200 //What gyro is connected 201 Serial.println(F("")); 202 Serial.println(F("===================================================")); 203 Serial.println(F("Gyro search")); 204 Serial.println(F("===================================================")); 205 delay(2000); 206 207 Serial.println(F("Searching for MPU-6050 on address 0x68/104")); 208 delay(1000); 209 if(search_gyro(0x68, 0x75) == 0x68){ 210 Serial.println(F("MPU-6050 found on address 0x68")); 211 type = 1; 212 gyro_address = 0x68; 213 } 214 215 if(type == 0){ 216 Serial.println(F("Searching for MPU-6050 on address 0x69/105")); 217 delay(1000); 218 if(search_gyro(0x69, 0x75) == 0x68){ 219 Serial.println(F("MPU-6050 found on address 0x69")); 220 type = 1; 221 gyro_address = 0x69; 222 } 223 } 224 225 if(type == 0){ 226 Serial.println(F("Searching for L3G4200D on address 0x68/104")); 227 delay(1000); 228 if(search_gyro(0x68, 0x0F) == 0xD3){ 229 Serial.println(F("L3G4200D found on address 0x68")); 230 type = 2; 231 gyro_address = 0x68; 232 } 233 } 234 235 if(type == 0){ 236 Serial.println(F("Searching for L3G4200D on address 0x69/105")); 237 delay(1000); 238 if(search_gyro(0x69, 0x0F) == 0xD3){ 239 Serial.println(F("L3G4200D found on address 0x69")); 240 type = 2; 241 gyro_address = 0x69; 242 } 243 } 244 245 if(type == 0){ 246 Serial.println(F("Searching for L3GD20H on address 0x6A/106")); 247 delay(1000); 248 if(search_gyro(0x6A, 0x0F) == 0xD7){ 249 Serial.println(F("L3GD20H found on address 0x6A")); 250 type = 3; 251 gyro_address = 0x6A; 252 } 253 } 254 255 if(type == 0){ 256 Serial.println(F("Searching for L3GD20H on address 0x6B/107")); 257 delay(1000); 258 if(search_gyro(0x6B, 0x0F) == 0xD7){ 259 Serial.println(F("L3GD20H found on address 0x6B")); 260 type = 3; 261 gyro_address = 0x6B; 262 } 263 } 264 265 if(type == 0){ 266 Serial.println(F("No gyro device found!!! (ERROR 3)")); 267 error = 1; 268 } 269 270 else{ 271 delay(3000); 272 Serial.println(F("")); 273 Serial.println(F("===================================================")); 274 Serial.println(F("Gyro register settings")); 275 Serial.println(F("===================================================")); 276 start_gyro(); //Setup the gyro for further use 277 } 278 } 279 280 //If the gyro is found we can setup the correct gyro axes. 281 if(error == 0){ 282 delay(3000); 283 Serial.println(F("")); 284 Serial.println(F("===================================================")); 285 Serial.println(F("Gyro calibration")); 286 Serial.println(F("===================================================")); 287 Serial.println(F("Don't move the quadcopter!! Calibration starts in 3 seconds")); 288 delay(3000); 289 Serial.println(F("Calibrating the gyro, this will take +/- 8 seconds")); 290 Serial.print(F("Please wait")); 291 //Let's take multiple gyro data samples so we can determine the average gyro offset (calibration). 292 for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Take 2000 readings for calibration. 293 if(cal_int % 100 == 0)Serial.print(F(".")); //Print dot to indicate calibration. 294 gyro_signalen(); //Read the gyro output. 295 gyro_roll_cal += gyro_roll; //Ad roll value to gyro_roll_cal. 296 gyro_pitch_cal += gyro_pitch; //Ad pitch value to gyro_pitch_cal. 297 gyro_yaw_cal += gyro_yaw; //Ad yaw value to gyro_yaw_cal. 298 delay(4); //Wait 3 milliseconds before the next loop. 299 } 300 //Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset. 301 gyro_roll_cal /= 2000; //Divide the roll total by 2000. 302 gyro_pitch_cal /= 2000; //Divide the pitch total by 2000. 303 gyro_yaw_cal /= 2000; //Divide the yaw total by 2000. 304 305 //Show the calibration results 306 Serial.println(F("")); 307 Serial.print(F("Axis 1 offset=")); 308 Serial.println(gyro_roll_cal); 309 Serial.print(F("Axis 2 offset=")); 310 Serial.println(gyro_pitch_cal); 311 Serial.print(F("Axis 3 offset=")); 312 Serial.println(gyro_yaw_cal); 313 Serial.println(F("")); 314 315 Serial.println(F("===================================================")); 316 Serial.println(F("Gyro axes configuration")); 317 Serial.println(F("===================================================")); 318 319 //Detect the left wing up movement 320 Serial.println(F("Lift the left side of the quadcopter to a 45 degree angle within 10 seconds")); 321 //Check axis movement 322 check_gyro_axes(1); 323 if(error == 0){ 324 Serial.println(F("OK!")); 325 Serial.print(F("Angle detection = ")); 326 Serial.println(roll_axis & 0b00000011); 327 if(roll_axis & 0b10000000)Serial.println(F("Axis inverted = yes")); 328 else Serial.println(F("Axis inverted = no")); 329 Serial.println(F("Put the quadcopter back in its original position")); 330 Serial.println(F("Move stick 'nose up' and back to center to continue")); 331 check_to_continue(); 332 333 //Detect the nose up movement 334 Serial.println(F("")); 335 Serial.println(F("")); 336 Serial.println(F("Lift the nose of the quadcopter to a 45 degree angle within 10 seconds")); 337 //Check axis movement 338 check_gyro_axes(2); 339 } 340 if(error == 0){ 341 Serial.println(F("OK!")); 342 Serial.print(F("Angle detection = ")); 343 Serial.println(pitch_axis & 0b00000011); 344 if(pitch_axis & 0b10000000)Serial.println(F("Axis inverted = yes")); 345 else Serial.println(F("Axis inverted = no")); 346 Serial.println(F("Put the quadcopter back in its original position")); 347 Serial.println(F("Move stick 'nose up' and back to center to continue")); 348 check_to_continue(); 349 350 //Detect the nose right movement 351 Serial.println(F("")); 352 Serial.println(F("")); 353 Serial.println(F("Rotate the nose of the quadcopter 45 degree to the right within 10 seconds")); 354 //Check axis movement 355 check_gyro_axes(3); 356 } 357 if(error == 0){ 358 Serial.println(F("OK!")); 359 Serial.print(F("Angle detection = ")); 360 Serial.println(yaw_axis & 0b00000011); 361 if(yaw_axis & 0b10000000)Serial.println(F("Axis inverted = yes")); 362 else Serial.println(F("Axis inverted = no")); 363 Serial.println(F("Put the quadcopter back in its original position")); 364 Serial.println(F("Move stick 'nose up' and back to center to continue")); 365 check_to_continue(); 366 } 367 } 368 if(error == 0){ 369 Serial.println(F("")); 370 Serial.println(F("===================================================")); 371 Serial.println(F("LED test")); 372 Serial.println(F("===================================================")); 373 digitalWrite(12, HIGH); 374 Serial.println(F("The LED should now be lit")); 375 Serial.println(F("Move stick 'nose up' and back to center to continue")); 376 check_to_continue(); 377 digitalWrite(12, LOW); 378 } 379 380 Serial.println(F("")); 381 382 if(error == 0){ 383 Serial.println(F("===================================================")); 384 Serial.println(F("Final setup check")); 385 Serial.println(F("===================================================")); 386 delay(1000); 387 if(receiver_check_byte == 0b00001111){ 388 Serial.println(F("Receiver channels ok")); 389 } 390 else{ 391 Serial.println(F("Receiver channel verification failed!!! (ERROR 6)")); 392 error = 1; 393 } 394 delay(1000); 395 if(gyro_check_byte == 0b00000111){ 396 Serial.println(F("Gyro axes ok")); 397 } 398 else{ 399 Serial.println(F("Gyro exes verification failed!!! (ERROR 7)")); 400 error = 1; 401 } 402 } 403 404 if(error == 0){ 405 //If all is good, store the information in the EEPROM 406 Serial.println(F("")); 407 Serial.println(F("===================================================")); 408 Serial.println(F("Storing EEPROM information")); 409 Serial.println(F("===================================================")); 410 Serial.println(F("Writing EEPROM")); 411 delay(1000); 412 Serial.println(F("Done!")); 413 EEPROM.write(0, center_channel_1 & 0b11111111); 414 EEPROM.write(1, center_channel_1 >> 8); 415 EEPROM.write(2, center_channel_2 & 0b11111111); 416 EEPROM.write(3, center_channel_2 >> 8); 417 EEPROM.write(4, center_channel_3 & 0b11111111); 418 EEPROM.write(5, center_channel_3 >> 8); 419 EEPROM.write(6, center_channel_4 & 0b11111111); 420 EEPROM.write(7, center_channel_4 >> 8); 421 EEPROM.write(8, high_channel_1 & 0b11111111); 422 EEPROM.write(9, high_channel_1 >> 8); 423 EEPROM.write(10, high_channel_2 & 0b11111111); 424 EEPROM.write(11, high_channel_2 >> 8); 425 EEPROM.write(12, high_channel_3 & 0b11111111); 426 EEPROM.write(13, high_channel_3 >> 8); 427 EEPROM.write(14, high_channel_4 & 0b11111111); 428 EEPROM.write(15, high_channel_4 >> 8); 429 EEPROM.write(16, low_channel_1 & 0b11111111); 430 EEPROM.write(17, low_channel_1 >> 8); 431 EEPROM.write(18, low_channel_2 & 0b11111111); 432 EEPROM.write(19, low_channel_2 >> 8); 433 EEPROM.write(20, low_channel_3 & 0b11111111); 434 EEPROM.write(21, low_channel_3 >> 8); 435 EEPROM.write(22, low_channel_4 & 0b11111111); 436 EEPROM.write(23, low_channel_4 >> 8); 437 EEPROM.write(24, channel_1_assign); 438 EEPROM.write(25, channel_2_assign); 439 EEPROM.write(26, channel_3_assign); 440 EEPROM.write(27, channel_4_assign); 441 EEPROM.write(28, roll_axis); 442 EEPROM.write(29, pitch_axis); 443 EEPROM.write(30, yaw_axis); 444 EEPROM.write(31, type); 445 EEPROM.write(32, gyro_address); 446 //Write the EEPROM signature 447 EEPROM.write(33, 'J'); 448 EEPROM.write(34, 'M'); 449 EEPROM.write(35, 'B'); 450 451 452 //To make sure evrything is ok, verify the EEPROM data. 453 Serial.println(F("Verify EEPROM data")); 454 delay(1000); 455 if(center_channel_1 != ((EEPROM.read(1) << 8) | EEPROM.read(0)))error = 1; 456 if(center_channel_2 != ((EEPROM.read(3) << 8) | EEPROM.read(2)))error = 1; 457 if(center_channel_3 != ((EEPROM.read(5) << 8) | EEPROM.read(4)))error = 1; 458 if(center_channel_4 != ((EEPROM.read(7) << 8) | EEPROM.read(6)))error = 1; 459 460 if(high_channel_1 != ((EEPROM.read(9) << 8) | EEPROM.read(8)))error = 1; 461 if(high_channel_2 != ((EEPROM.read(11) << 8) | EEPROM.read(10)))error = 1; 462 if(high_channel_3 != ((EEPROM.read(13) << 8) | EEPROM.read(12)))error = 1; 463 if(high_channel_4 != ((EEPROM.read(15) << 8) | EEPROM.read(14)))error = 1; 464 465 if(low_channel_1 != ((EEPROM.read(17) << 8) | EEPROM.read(16)))error = 1; 466 if(low_channel_2 != ((EEPROM.read(19) << 8) | EEPROM.read(18)))error = 1; 467 if(low_channel_3 != ((EEPROM.read(21) << 8) | EEPROM.read(20)))error = 1; 468 if(low_channel_4 != ((EEPROM.read(23) << 8) | EEPROM.read(22)))error = 1; 469 470 if(channel_1_assign != EEPROM.read(24))error = 1; 471 if(channel_2_assign != EEPROM.read(25))error = 1; 472 if(channel_3_assign != EEPROM.read(26))error = 1; 473 if(channel_4_assign != EEPROM.read(27))error = 1; 474 475 if(roll_axis != EEPROM.read(28))error = 1; 476 if(pitch_axis != EEPROM.read(29))error = 1; 477 if(yaw_axis != EEPROM.read(30))error = 1; 478 if(type != EEPROM.read(31))error = 1; 479 if(gyro_address != EEPROM.read(32))error = 1; 480 481 if('J' != EEPROM.read(33))error = 1; 482 if('M' != EEPROM.read(34))error = 1; 483 if('B' != EEPROM.read(35))error = 1; 484 485 if(error == 1)Serial.println(F("EEPROM verification failed!!! (ERROR 5)")); 486 else Serial.println(F("Verification done")); 487 } 488 489 490 if(error == 0){ 491 Serial.println(F("Setup is finished.")); 492 Serial.println(F("You can now calibrate the esc's and upload the YMFC-AL code.")); 493 } 494 else{ 495 Serial.println(F("The setup is aborted due to an error.")); 496 Serial.println(F("Check the Q and A page of the YMFC-AL project on:")); 497 Serial.println(F("www.brokking.net for more information about this error.")); 498 } 499 while(1); 500} 501 502//Search for the gyro and check the Who_am_I register 503byte search_gyro(int gyro_address, int who_am_i){ 504 Wire.beginTransmission(gyro_address); 505 Wire.write(who_am_i); 506 Wire.endTransmission(); 507 Wire.requestFrom(gyro_address, 1); 508 timer = millis() + 100; 509 while(Wire.available() < 1 && timer > millis()); 510 lowByte = Wire.read(); 511 address = gyro_address; 512 return lowByte; 513} 514 515void start_gyro(){ 516 //Setup the L3G4200D or L3GD20H 517 if(type == 2 || type == 3){ 518 Wire.beginTransmission(address); //Start communication with the gyro with the address found during search 519 Wire.write(0x20); //We want to write to register 1 (20 hex) 520 Wire.write(0x0F); //Set the register bits as 00001111 (Turn on the gyro and enable all axis) 521 Wire.endTransmission(); //End the transmission with the gyro 522 523 Wire.beginTransmission(address); //Start communication with the gyro (adress 1101001) 524 Wire.write(0x20); //Start reading @ register 28h and auto increment with every read 525 Wire.endTransmission(); //End the transmission 526 Wire.requestFrom(address, 1); //Request 6 bytes from the gyro 527 while(Wire.available() < 1); //Wait until the 1 byte is received 528 Serial.print(F("Register 0x20 is set to:")); 529 Serial.println(Wire.read(),BIN); 530 531 Wire.beginTransmission(address); //Start communication with the gyro with the address found during search 532 Wire.write(0x23); //We want to write to register 4 (23 hex) 533 Wire.write(0x90); //Set the register bits as 10010000 (Block Data Update active & 500dps full scale) 534 Wire.endTransmission(); //End the transmission with the gyro 535 536 Wire.beginTransmission(address); //Start communication with the gyro (adress 1101001) 537 Wire.write(0x23); //Start reading @ register 28h and auto increment with every read 538 Wire.endTransmission(); //End the transmission 539 Wire.requestFrom(address, 1); //Request 6 bytes from the gyro 540 while(Wire.available() < 1); //Wait until the 1 byte is received 541 Serial.print(F("Register 0x23 is set to:")); 542 Serial.println(Wire.read(),BIN); 543 544 } 545 //Setup the MPU-6050 546 if(type == 1){ 547 548 Wire.beginTransmission(address); //Start communication with the gyro 549 Wire.write(0x6B); //PWR_MGMT_1 register 550 Wire.write(0x00); //Set to zero to turn on the gyro 551 Wire.endTransmission(); //End the transmission 552 553 Wire.beginTransmission(address); //Start communication with the gyro 554 Wire.write(0x6B); //Start reading @ register 28h and auto increment with every read 555 Wire.endTransmission(); //End the transmission 556 Wire.requestFrom(address, 1); //Request 1 bytes from the gyro 557 while(Wire.available() < 1); //Wait until the 1 byte is received 558 Serial.print(F("Register 0x6B is set to:")); 559 Serial.println(Wire.read(),BIN); 560 561 Wire.beginTransmission(address); //Start communication with the gyro 562 Wire.write(0x1B); //GYRO_CONFIG register 563 Wire.write(0x08); //Set the register bits as 00001000 (500dps full scale) 564 Wire.endTransmission(); //End the transmission 565 566 Wire.beginTransmission(address); //Start communication with the gyro (adress 1101001) 567 Wire.write(0x1B); //Start reading @ register 28h and auto increment with every read 568 Wire.endTransmission(); //End the transmission 569 Wire.requestFrom(address, 1); //Request 1 bytes from the gyro 570 while(Wire.available() < 1); //Wait until the 1 byte is received 571 Serial.print(F("Register 0x1B is set to:")); 572 Serial.println(Wire.read(),BIN); 573 574 } 575} 576 577void gyro_signalen(){ 578 if(type == 2 || type == 3){ 579 Wire.beginTransmission(address); //Start communication with the gyro 580 Wire.write(168); //Start reading @ register 28h and auto increment with every read 581 Wire.endTransmission(); //End the transmission 582 Wire.requestFrom(address, 6); //Request 6 bytes from the gyro 583 while(Wire.available() < 6); //Wait until the 6 bytes are received 584 lowByte = Wire.read(); //First received byte is the low part of the angular data 585 highByte = Wire.read(); //Second received byte is the high part of the angular data 586 gyro_roll = ((highByte<<8)|lowByte); //Multiply highByte by 256 (shift left by 8) and ad lowByte 587 if(cal_int == 2000)gyro_roll -= gyro_roll_cal; //Only compensate after the calibration 588 lowByte = Wire.read(); //First received byte is the low part of the angular data 589 highByte = Wire.read(); //Second received byte is the high part of the angular data 590 gyro_pitch = ((highByte<<8)|lowByte); //Multiply highByte by 256 (shift left by 8) and ad lowByte 591 if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal; //Only compensate after the calibration 592 lowByte = Wire.read(); //First received byte is the low part of the angular data 593 highByte = Wire.read(); //Second received byte is the high part of the angular data 594 gyro_yaw = ((highByte<<8)|lowByte); //Multiply highByte by 256 (shift left by 8) and ad lowByte 595 if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal; //Only compensate after the calibration 596 } 597 if(type == 1){ 598 Wire.beginTransmission(address); //Start communication with the gyro 599 Wire.write(0x43); //Start reading @ register 43h and auto increment with every read 600 Wire.endTransmission(); //End the transmission 601 Wire.requestFrom(address,6); //Request 6 bytes from the gyro 602 while(Wire.available() < 6); //Wait until the 6 bytes are received 603 gyro_roll=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data 604 if(cal_int == 2000)gyro_roll -= gyro_roll_cal; //Only compensate after the calibration 605 gyro_pitch=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data 606 if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal; //Only compensate after the calibration 607 gyro_yaw=Wire.read()<<8|Wire.read(); //Read high and low part of the angular data 608 if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal; //Only compensate after the calibration 609 } 610} 611 612//Check if a receiver input value is changing within 30 seconds 613void check_receiver_inputs(byte movement){ 614 byte trigger = 0; 615 int pulse_length; 616 timer = millis() + 30000; 617 while(timer > millis() && trigger == 0){ 618 delay(250); 619 if(receiver_input_channel_1 > 1750 || receiver_input_channel_1 < 1250){ 620 trigger = 1; 621 receiver_check_byte |= 0b00000001; 622 pulse_length = receiver_input_channel_1; 623 } 624 if(receiver_input_channel_2 > 1750 || receiver_input_channel_2 < 1250){ 625 trigger = 2; 626 receiver_check_byte |= 0b00000010; 627 pulse_length = receiver_input_channel_2; 628 } 629 if(receiver_input_channel_3 > 1750 || receiver_input_channel_3 < 1250){ 630 trigger = 3; 631 receiver_check_byte |= 0b00000100; 632 pulse_length = receiver_input_channel_3; 633 } 634 if(receiver_input_channel_4 > 1750 || receiver_input_channel_4 < 1250){ 635 trigger = 4; 636 receiver_check_byte |= 0b00001000; 637 pulse_length = receiver_input_channel_4; 638 } 639 } 640 if(trigger == 0){ 641 error = 1; 642 Serial.println(F("No stick movement detected in the last 30 seconds!!! (ERROR 2)")); 643 } 644 //Assign the stick to the function. 645 else{ 646 if(movement == 1){ 647 channel_3_assign = trigger; 648 if(pulse_length < 1250)channel_3_assign += 0b10000000; 649 } 650 if(movement == 2){ 651 channel_1_assign = trigger; 652 if(pulse_length < 1250)channel_1_assign += 0b10000000; 653 } 654 if(movement == 3){ 655 channel_2_assign = trigger; 656 if(pulse_length < 1250)channel_2_assign += 0b10000000; 657 } 658 if(movement == 4){ 659 channel_4_assign = trigger; 660 if(pulse_length < 1250)channel_4_assign += 0b10000000; 661 } 662 } 663} 664 665void check_to_continue(){ 666 byte continue_byte = 0; 667 while(continue_byte == 0){ 668 if(channel_2_assign == 0b00000001 && receiver_input_channel_1 > center_channel_1 + 150)continue_byte = 1; 669 if(channel_2_assign == 0b10000001 && receiver_input_channel_1 < center_channel_1 - 150)continue_byte = 1; 670 if(channel_2_assign == 0b00000010 && receiver_input_channel_2 > center_channel_2 + 150)continue_byte = 1; 671 if(channel_2_assign == 0b10000010 && receiver_input_channel_2 < center_channel_2 - 150)continue_byte = 1; 672 if(channel_2_assign == 0b00000011 && receiver_input_channel_3 > center_channel_3 + 150)continue_byte = 1; 673 if(channel_2_assign == 0b10000011 && receiver_input_channel_3 < center_channel_3 - 150)continue_byte = 1; 674 if(channel_2_assign == 0b00000100 && receiver_input_channel_4 > center_channel_4 + 150)continue_byte = 1; 675 if(channel_2_assign == 0b10000100 && receiver_input_channel_4 < center_channel_4 - 150)continue_byte = 1; 676 delay(100); 677 } 678 wait_sticks_zero(); 679} 680 681//Check if the transmitter sticks are in the neutral position 682void wait_sticks_zero(){ 683 byte zero = 0; 684 while(zero < 15){ 685 if(receiver_input_channel_1 < center_channel_1 + 20 && receiver_input_channel_1 > center_channel_1 - 20)zero |= 0b00000001; 686 if(receiver_input_channel_2 < center_channel_2 + 20 && receiver_input_channel_2 > center_channel_2 - 20)zero |= 0b00000010; 687 if(receiver_input_channel_3 < center_channel_3 + 20 && receiver_input_channel_3 > center_channel_3 - 20)zero |= 0b00000100; 688 if(receiver_input_channel_4 < center_channel_4 + 20 && receiver_input_channel_4 > center_channel_4 - 20)zero |= 0b00001000; 689 delay(100); 690 } 691} 692 693//Checck if the receiver values are valid within 10 seconds 694void wait_for_receiver(){ 695 byte zero = 0; 696 timer = millis() + 10000; 697 while(timer > millis() && zero < 15){ 698 if(receiver_input_channel_1 < 2100 && receiver_input_channel_1 > 900)zero |= 0b00000001; 699 if(receiver_input_channel_2 < 2100 && receiver_input_channel_2 > 900)zero |= 0b00000010; 700 if(receiver_input_channel_3 < 2100 && receiver_input_channel_3 > 900)zero |= 0b00000100; 701 if(receiver_input_channel_4 < 2100 && receiver_input_channel_4 > 900)zero |= 0b00001000; 702 delay(500); 703 Serial.print(F(".")); 704 } 705 if(zero == 0){ 706 error = 1; 707 Serial.println(F(".")); 708 Serial.println(F("No valid receiver signals found!!! (ERROR 1)")); 709 } 710 else Serial.println(F(" OK")); 711} 712 713//Register the min and max receiver values and exit when the sticks are back in the neutral position 714void register_min_max(){ 715 byte zero = 0; 716 low_channel_1 = receiver_input_channel_1; 717 low_channel_2 = receiver_input_channel_2; 718 low_channel_3 = receiver_input_channel_3; 719 low_channel_4 = receiver_input_channel_4; 720 while(receiver_input_channel_1 < center_channel_1 + 20 && receiver_input_channel_1 > center_channel_1 - 20)delay(250); 721 Serial.println(F("Measuring endpoints....")); 722 while(zero < 15){ 723 if(receiver_input_channel_1 < center_channel_1 + 20 && receiver_input_channel_1 > center_channel_1 - 20)zero |= 0b00000001; 724 if(receiver_input_channel_2 < center_channel_2 + 20 && receiver_input_channel_2 > center_channel_2 - 20)zero |= 0b00000010; 725 if(receiver_input_channel_3 < center_channel_3 + 20 && receiver_input_channel_3 > center_channel_3 - 20)zero |= 0b00000100; 726 if(receiver_input_channel_4 < center_channel_4 + 20 && receiver_input_channel_4 > center_channel_4 - 20)zero |= 0b00001000; 727 if(receiver_input_channel_1 < low_channel_1)low_channel_1 = receiver_input_channel_1; 728 if(receiver_input_channel_2 < low_channel_2)low_channel_2 = receiver_input_channel_2; 729 if(receiver_input_channel_3 < low_channel_3)low_channel_3 = receiver_input_channel_3; 730 if(receiver_input_channel_4 < low_channel_4)low_channel_4 = receiver_input_channel_4; 731 if(receiver_input_channel_1 > high_channel_1)high_channel_1 = receiver_input_channel_1; 732 if(receiver_input_channel_2 > high_channel_2)high_channel_2 = receiver_input_channel_2; 733 if(receiver_input_channel_3 > high_channel_3)high_channel_3 = receiver_input_channel_3; 734 if(receiver_input_channel_4 > high_channel_4)high_channel_4 = receiver_input_channel_4; 735 delay(100); 736 } 737} 738 739//Check if the angular position of a gyro axis is changing within 10 seconds 740void check_gyro_axes(byte movement){ 741 byte trigger_axis = 0; 742 float gyro_angle_roll, gyro_angle_pitch, gyro_angle_yaw; 743 //Reset all axes 744 gyro_angle_roll = 0; 745 gyro_angle_pitch = 0; 746 gyro_angle_yaw = 0; 747 gyro_signalen(); 748 timer = millis() + 10000; 749 while(timer > millis() && gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_pitch > -30 && gyro_angle_pitch < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){ 750 gyro_signalen(); 751 if(type == 2 || type == 3){ 752 gyro_angle_roll += gyro_roll * 0.00007; //0.00007 = 17.5 (md/s) / 250(Hz) 753 gyro_angle_pitch += gyro_pitch * 0.00007; 754 gyro_angle_yaw += gyro_yaw * 0.00007; 755 } 756 if(type == 1){ 757 gyro_angle_roll += gyro_roll * 0.0000611; // 0.0000611 = 1 / 65.5 (LSB degr/s) / 250(Hz) 758 gyro_angle_pitch += gyro_pitch * 0.0000611; 759 gyro_angle_yaw += gyro_yaw * 0.0000611; 760 } 761 762 delayMicroseconds(3700); //Loop is running @ 250Hz. +/-300us is used for communication with the gyro 763 } 764 //Assign the moved axis to the orresponding function (pitch, roll, yaw) 765 if((gyro_angle_roll < -30 || gyro_angle_roll > 30) && gyro_angle_pitch > -30 && gyro_angle_pitch < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){ 766 gyro_check_byte |= 0b00000001; 767 if(gyro_angle_roll < 0)trigger_axis = 0b10000001; 768 else trigger_axis = 0b00000001; 769 } 770 if((gyro_angle_pitch < -30 || gyro_angle_pitch > 30) && gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){ 771 gyro_check_byte |= 0b00000010; 772 if(gyro_angle_pitch < 0)trigger_axis = 0b10000010; 773 else trigger_axis = 0b00000010; 774 } 775 if((gyro_angle_yaw < -30 || gyro_angle_yaw > 30) && gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_pitch > -30 && gyro_angle_pitch < 30){ 776 gyro_check_byte |= 0b00000100; 777 if(gyro_angle_yaw < 0)trigger_axis = 0b10000011; 778 else trigger_axis = 0b00000011; 779 } 780 781 if(trigger_axis == 0){ 782 error = 1; 783 Serial.println(F("No angular motion is detected in the last 10 seconds!!! (ERROR 4)")); 784 } 785 else 786 if(movement == 1)roll_axis = trigger_axis; 787 if(movement == 2)pitch_axis = trigger_axis; 788 if(movement == 3)yaw_axis = trigger_axis; 789 790} 791 792//This routine is called every time input 8, 9, 10 or 11 changed state 793ISR(PCINT0_vect){ 794 current_time = micros(); 795 //Channel 1========================================= 796 if(PINB & B00000001){ //Is input 8 high? 797 if(last_channel_1 == 0){ //Input 8 changed from 0 to 1 798 last_channel_1 = 1; //Remember current input state 799 timer_1 = current_time; //Set timer_1 to current_time 800 } 801 } 802 else if(last_channel_1 == 1){ //Input 8 is not high and changed from 1 to 0 803 last_channel_1 = 0; //Remember current input state 804 receiver_input_channel_1 = current_time - timer_1; //Channel 1 is current_time - timer_1 805 } 806 //Channel 2========================================= 807 if(PINB & B00000010 ){ //Is input 9 high? 808 if(last_channel_2 == 0){ //Input 9 changed from 0 to 1 809 last_channel_2 = 1; //Remember current input state 810 timer_2 = current_time; //Set timer_2 to current_time 811 } 812 } 813 else if(last_channel_2 == 1){ //Input 9 is not high and changed from 1 to 0 814 last_channel_2 = 0; //Remember current input state 815 receiver_input_channel_2 = current_time - timer_2; //Channel 2 is current_time - timer_2 816 } 817 //Channel 3========================================= 818 if(PINB & B00000100 ){ //Is input 10 high? 819 if(last_channel_3 == 0){ //Input 10 changed from 0 to 1 820 last_channel_3 = 1; //Remember current input state 821 timer_3 = current_time; //Set timer_3 to current_time 822 } 823 } 824 else if(last_channel_3 == 1){ //Input 10 is not high and changed from 1 to 0 825 last_channel_3 = 0; //Remember current input state 826 receiver_input_channel_3 = current_time - timer_3; //Channel 3 is current_time - timer_3 827 828 } 829 //Channel 4========================================= 830 if(PINB & B00001000 ){ //Is input 11 high? 831 if(last_channel_4 == 0){ //Input 11 changed from 0 to 1 832 last_channel_4 = 1; //Remember current input state 833 timer_4 = current_time; //Set timer_4 to current_time 834 } 835 } 836 else if(last_channel_4 == 1){ //Input 11 is not high and changed from 1 to 0 837 last_channel_4 = 0; //Remember current input state 838 receiver_input_channel_4 = current_time - timer_4; //Channel 4 is current_time - timer_4 839 } 840} 841 842//Intro subroutine 843void intro(){ 844 Serial.println(F("===================================================")); 845 delay(1500); 846 Serial.println(F("")); 847 Serial.println(F("Your")); 848 delay(500); 849 Serial.println(F(" Multicopter")); 850 delay(500); 851 Serial.println(F(" Flight")); 852 delay(500); 853 Serial.println(F(" Controller")); 854 delay(1000); 855 Serial.println(F("")); 856 Serial.println(F("YMFC-AL Setup Program")); 857 Serial.println(F("")); 858 Serial.println(F("===================================================")); 859 delay(1500); 860 Serial.println(F("For support and questions: www.brokking.net")); 861 Serial.println(F("")); 862 Serial.println(F("Have fun!")); 863} 864
FC0G96FJGGTGFHF.ino
c_cpp
Code for HC12
1/* HC12 Send/Receive Example Program 1 2 By Mark J. Hughes 3 for AllAboutCircuits.com 4 5 Connect HC12 "RXD" pin to Arduino Digital Pin 4 6 Connect HC12 "TXD" pin to Arduino Digital Pin 5 7 Connect HC12 "Set" pin to Arduino Digital Pin 6 8 9 Do not power over USB. Per datasheet, 10 power HC12 with a supply of at least 100 mA with 11 a 22 uF - 1000 uF reservoir capacitor. 12 Upload code to two Arduinos connected to two computers. 13 14 Transceivers must be at least several meters apart to work. 15 16 */ 17 18#include <SoftwareSerial.h> 19 20const byte HC12RxdPin = 2; // Recieve Pin on HC12 21const byte HC12TxdPin = 3; // Transmit Pin on HC12 22 23SoftwareSerial HC12(HC12TxdPin,HC12RxdPin); // Create Software Serial Port 24 25void setup() { 26 Serial.begin(9600); // Open serial port to computer 27 HC12.begin(9600); // Open serial port to HC12 28} 29 30void loop() { 31 if(HC12.available()){ // If Arduino's HC12 rx buffer has data 32 Serial.write(HC12.read()); // Send the data to the computer 33 } 34 if(Serial.available()){ // If Arduino's computer rx buffer has data 35 HC12.write(Serial.read()); // Send that data to serial 36 } 37} 38 39 40
FQR6TWEJGGTGFQ7.ino
c_cpp
Code for drone
1/////////////////////////////////////////////////////////////////////////////////////// 2//Terms 3 of use 4/////////////////////////////////////////////////////////////////////////////////////// 5//THE 6 SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 7//IMPLIED, 8 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9//FITNESS FOR 10 A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 11//AUTHORS OR 12 COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 13//LIABILITY, WHETHER 14 IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 15//OUT OF OR IN CONNECTION 16 WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17//THE SOFTWARE. 18/////////////////////////////////////////////////////////////////////////////////////// 19//Safety 20 note 21/////////////////////////////////////////////////////////////////////////////////////// 22//Always 23 remove the propellers and stay away from the motors unless you 24//are 100% certain 25 of what you are doing. 26/////////////////////////////////////////////////////////////////////////////////////// 27 28#include 29 <Wire.h> //Include the Wire.h library so we can communicate with the 30 gyro 31#include <EEPROM.h> //Include the EEPROM.h library so we can 32 store information onto the EEPROM 33 34//Declaring Global Variables 35byte last_channel_1, 36 last_channel_2, last_channel_3, last_channel_4; 37byte lowByte, highByte, type, 38 gyro_address, error, clockspeed_ok; 39byte channel_1_assign, channel_2_assign, 40 channel_3_assign, channel_4_assign; 41byte roll_axis, pitch_axis, yaw_axis; 42byte 43 receiver_check_byte, gyro_check_byte; 44volatile int receiver_input_channel_1, 45 receiver_input_channel_2, receiver_input_channel_3, receiver_input_channel_4; 46int 47 center_channel_1, center_channel_2, center_channel_3, center_channel_4; 48int high_channel_1, 49 high_channel_2, high_channel_3, high_channel_4; 50int low_channel_1, low_channel_2, 51 low_channel_3, low_channel_4; 52int address, cal_int; 53unsigned long timer, timer_1, 54 timer_2, timer_3, timer_4, current_time; 55float gyro_pitch, gyro_roll, gyro_yaw; 56float 57 gyro_roll_cal, gyro_pitch_cal, gyro_yaw_cal; 58 59 60//Setup routine 61void 62 setup(){ 63 pinMode(12, OUTPUT); 64 //Arduino (Atmega) pins default to inputs, 65 so they don't need to be explicitly declared as inputs 66 PCICR |= (1 << PCIE0); 67 // set PCIE0 to enable PCMSK0 scan 68 PCMSK0 |= (1 << PCINT0); // set PCINT0 69 (digital input 8) to trigger an interrupt on state change 70 PCMSK0 |= (1 << PCINT1); 71 // set PCINT1 (digital input 9)to trigger an interrupt on state change 72 PCMSK0 73 |= (1 << PCINT2); // set PCINT2 (digital input 10)to trigger an interrupt on state 74 change 75 PCMSK0 |= (1 << PCINT3); // set PCINT3 (digital input 11)to trigger 76 an interrupt on state change 77 Wire.begin(); //Start the I2C as master 78 79 Serial.begin(57600); //Start the serial connetion @ 57600bps 80 delay(250); 81 //Give the gyro time to start 82} 83//Main program 84void loop(){ 85 86 //Show the YMFC-3D V2 intro 87 intro(); 88 89 Serial.println(F("")); 90 91 Serial.println(F("===================================================")); 92 93 Serial.println(F("System check")); 94 Serial.println(F("===================================================")); 95 96 delay(1000); 97 Serial.println(F("Checking I2C clock speed.")); 98 delay(1000); 99 100 101 TWBR = 12; //Set the I2C clock speed to 400kHz. 102 103 104 #if F_CPU == 16000000L //If the clock speed is 16MHz include the 105 next code line when compiling 106 clockspeed_ok = 1; //Set clockspeed_ok 107 to 1 108 #endif //End of if statement 109 110 if(TWBR 111 == 12 && clockspeed_ok){ 112 Serial.println(F("I2C clock speed is correctly 113 set to 400kHz.")); 114 } 115 else{ 116 Serial.println(F("I2C clock speed 117 is not set to 400kHz. (ERROR 8)")); 118 error = 1; 119 } 120 121 if(error 122 == 0){ 123 Serial.println(F("")); 124 Serial.println(F("===================================================")); 125 126 Serial.println(F("Transmitter setup")); 127 Serial.println(F("===================================================")); 128 129 delay(1000); 130 Serial.print(F("Checking for valid receiver signals.")); 131 132 //Wait 10 seconds until all receiver inputs are valid 133 wait_for_receiver(); 134 135 Serial.println(F("")); 136 } 137 //Quit the program in case of an error 138 139 if(error == 0){ 140 delay(2000); 141 Serial.println(F("Place all sticks 142 and subtrims in the center position within 10 seconds.")); 143 for(int i = 9;i 144 > 0;i--){ 145 delay(1000); 146 Serial.print(i); 147 Serial.print(" 148 "); 149 } 150 Serial.println(" "); 151 //Store the central stick positions 152 153 center_channel_1 = receiver_input_channel_1; 154 center_channel_2 = receiver_input_channel_2; 155 156 center_channel_3 = receiver_input_channel_3; 157 center_channel_4 = receiver_input_channel_4; 158 159 Serial.println(F("")); 160 Serial.println(F("Center positions stored.")); 161 162 Serial.print(F("Digital input 08 = ")); 163 Serial.println(receiver_input_channel_1); 164 165 Serial.print(F("Digital input 09 = ")); 166 Serial.println(receiver_input_channel_2); 167 168 Serial.print(F("Digital input 10 = ")); 169 Serial.println(receiver_input_channel_3); 170 171 Serial.print(F("Digital input 11 = ")); 172 Serial.println(receiver_input_channel_4); 173 174 Serial.println(F("")); 175 Serial.println(F("")); 176 } 177 if(error 178 == 0){ 179 Serial.println(F("Move the throttle stick to full throttle and 180 back to center")); 181 //Check for throttle movement 182 check_receiver_inputs(1); 183 184 Serial.print(F("Throttle is connected to digital input ")); 185 Serial.println((channel_3_assign 186 & 0b00000111) + 7); 187 if(channel_3_assign & 0b10000000)Serial.println(F("Channel 188 inverted = yes")); 189 else Serial.println(F("Channel inverted = no")); 190 191 wait_sticks_zero(); 192 193 Serial.println(F("")); 194 Serial.println(F("")); 195 196 Serial.println(F("Move the roll stick to simulate left wing up and back to 197 center")); 198 //Check for throttle movement 199 check_receiver_inputs(2); 200 201 Serial.print(F("Roll is connected to digital input ")); 202 Serial.println((channel_1_assign 203 & 0b00000111) + 7); 204 if(channel_1_assign & 0b10000000)Serial.println(F("Channel 205 inverted = yes")); 206 else Serial.println(F("Channel inverted = no")); 207 208 wait_sticks_zero(); 209 } 210 if(error == 0){ 211 Serial.println(F("")); 212 213 Serial.println(F("")); 214 Serial.println(F("Move the pitch stick to simulate 215 nose up and back to center")); 216 //Check for throttle movement 217 check_receiver_inputs(3); 218 219 Serial.print(F("Pitch is connected to digital input ")); 220 Serial.println((channel_2_assign 221 & 0b00000111) + 7); 222 if(channel_2_assign & 0b10000000)Serial.println(F("Channel 223 inverted = yes")); 224 else Serial.println(F("Channel inverted = no")); 225 226 wait_sticks_zero(); 227 } 228 if(error == 0){ 229 Serial.println(F("")); 230 231 Serial.println(F("")); 232 Serial.println(F("Move the yaw stick to simulate 233 nose right and back to center")); 234 //Check for throttle movement 235 check_receiver_inputs(4); 236 237 Serial.print(F("Yaw is connected to digital input ")); 238 Serial.println((channel_4_assign 239 & 0b00000111) + 7); 240 if(channel_4_assign & 0b10000000)Serial.println(F("Channel 241 inverted = yes")); 242 else Serial.println(F("Channel inverted = no")); 243 244 wait_sticks_zero(); 245 } 246 if(error == 0){ 247 Serial.println(F("")); 248 249 Serial.println(F("")); 250 Serial.println(F("Gently move all the sticks 251 simultaneously to their extends")); 252 Serial.println(F("When ready put the 253 sticks back in their center positions")); 254 //Register the min and max values 255 of the receiver channels 256 register_min_max(); 257 Serial.println(F("")); 258 259 Serial.println(F("")); 260 Serial.println(F("High, low and center values 261 found during setup")); 262 Serial.print(F("Digital input 08 values:")); 263 264 Serial.print(low_channel_1); 265 Serial.print(F(" - ")); 266 Serial.print(center_channel_1); 267 268 Serial.print(F(" - ")); 269 Serial.println(high_channel_1); 270 Serial.print(F("Digital 271 input 09 values:")); 272 Serial.print(low_channel_2); 273 Serial.print(F(" 274 - ")); 275 Serial.print(center_channel_2); 276 Serial.print(F(" - ")); 277 278 Serial.println(high_channel_2); 279 Serial.print(F("Digital input 10 values:")); 280 281 Serial.print(low_channel_3); 282 Serial.print(F(" - ")); 283 Serial.print(center_channel_3); 284 285 Serial.print(F(" - ")); 286 Serial.println(high_channel_3); 287 Serial.print(F("Digital 288 input 11 values:")); 289 Serial.print(low_channel_4); 290 Serial.print(F(" 291 - ")); 292 Serial.print(center_channel_4); 293 Serial.print(F(" - ")); 294 295 Serial.println(high_channel_4); 296 Serial.println(F("Move stick 'nose up' 297 and back to center to continue")); 298 check_to_continue(); 299 } 300 301 302 if(error == 0){ 303 //What gyro is connected 304 Serial.println(F("")); 305 306 Serial.println(F("===================================================")); 307 308 Serial.println(F("Gyro search")); 309 Serial.println(F("===================================================")); 310 311 delay(2000); 312 313 Serial.println(F("Searching for MPU-6050 on address 314 0x68/104")); 315 delay(1000); 316 if(search_gyro(0x68, 0x75) == 0x68){ 317 318 Serial.println(F("MPU-6050 found on address 0x68")); 319 type = 1; 320 321 gyro_address = 0x68; 322 } 323 324 if(type == 0){ 325 Serial.println(F("Searching 326 for MPU-6050 on address 0x69/105")); 327 delay(1000); 328 if(search_gyro(0x69, 329 0x75) == 0x68){ 330 Serial.println(F("MPU-6050 found on address 0x69")); 331 332 type = 1; 333 gyro_address = 0x69; 334 } 335 } 336 337 338 if(type == 0){ 339 Serial.println(F("Searching for L3G4200D on address 340 0x68/104")); 341 delay(1000); 342 if(search_gyro(0x68, 0x0F) == 0xD3){ 343 344 Serial.println(F("L3G4200D found on address 0x68")); 345 type = 346 2; 347 gyro_address = 0x68; 348 } 349 } 350 351 if(type == 352 0){ 353 Serial.println(F("Searching for L3G4200D on address 0x69/105")); 354 355 delay(1000); 356 if(search_gyro(0x69, 0x0F) == 0xD3){ 357 Serial.println(F("L3G4200D 358 found on address 0x69")); 359 type = 2; 360 gyro_address = 0x69; 361 362 } 363 } 364 365 if(type == 0){ 366 Serial.println(F("Searching 367 for L3GD20H on address 0x6A/106")); 368 delay(1000); 369 if(search_gyro(0x6A, 370 0x0F) == 0xD7){ 371 Serial.println(F("L3GD20H found on address 0x6A")); 372 373 type = 3; 374 gyro_address = 0x6A; 375 } 376 } 377 378 379 if(type == 0){ 380 Serial.println(F("Searching for L3GD20H on address 0x6B/107")); 381 382 delay(1000); 383 if(search_gyro(0x6B, 0x0F) == 0xD7){ 384 Serial.println(F("L3GD20H 385 found on address 0x6B")); 386 type = 3; 387 gyro_address = 0x6B; 388 389 } 390 } 391 392 if(type == 0){ 393 Serial.println(F("No gyro 394 device found!!! (ERROR 3)")); 395 error = 1; 396 } 397 398 else{ 399 400 delay(3000); 401 Serial.println(F("")); 402 Serial.println(F("===================================================")); 403 404 Serial.println(F("Gyro register settings")); 405 Serial.println(F("===================================================")); 406 407 start_gyro(); //Setup the gyro for further use 408 } 409 } 410 411 //If 412 the gyro is found we can setup the correct gyro axes. 413 if(error == 0){ 414 delay(3000); 415 416 Serial.println(F("")); 417 Serial.println(F("===================================================")); 418 419 Serial.println(F("Gyro calibration")); 420 Serial.println(F("===================================================")); 421 422 Serial.println(F("Don't move the quadcopter!! Calibration starts in 3 seconds")); 423 424 delay(3000); 425 Serial.println(F("Calibrating the gyro, this will take 426 +/- 8 seconds")); 427 Serial.print(F("Please wait")); 428 //Let's take 429 multiple gyro data samples so we can determine the average gyro offset (calibration). 430 431 for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Take 2000 readings 432 for calibration. 433 if(cal_int % 100 == 0)Serial.print(F(".")); //Print 434 dot to indicate calibration. 435 gyro_signalen(); //Read 436 the gyro output. 437 gyro_roll_cal += gyro_roll; //Ad 438 roll value to gyro_roll_cal. 439 gyro_pitch_cal += gyro_pitch; //Ad 440 pitch value to gyro_pitch_cal. 441 gyro_yaw_cal += gyro_yaw; //Ad 442 yaw value to gyro_yaw_cal. 443 delay(4); //Wait 444 3 milliseconds before the next loop. 445 } 446 //Now that we have 2000 measures, 447 we need to devide by 2000 to get the average gyro offset. 448 gyro_roll_cal /= 449 2000; //Divide the roll total by 2000. 450 451 gyro_pitch_cal /= 2000; //Divide the pitch 452 total by 2000. 453 gyro_yaw_cal /= 2000; //Divide 454 the yaw total by 2000. 455 456 //Show the calibration results 457 Serial.println(F("")); 458 459 Serial.print(F("Axis 1 offset=")); 460 Serial.println(gyro_roll_cal); 461 462 Serial.print(F("Axis 2 offset=")); 463 Serial.println(gyro_pitch_cal); 464 465 Serial.print(F("Axis 3 offset=")); 466 Serial.println(gyro_yaw_cal); 467 468 Serial.println(F("")); 469 470 Serial.println(F("===================================================")); 471 472 Serial.println(F("Gyro axes configuration")); 473 Serial.println(F("===================================================")); 474 475 476 //Detect the left wing up movement 477 Serial.println(F("Lift the 478 left side of the quadcopter to a 45 degree angle within 10 seconds")); 479 //Check 480 axis movement 481 check_gyro_axes(1); 482 if(error == 0){ 483 Serial.println(F("OK!")); 484 485 Serial.print(F("Angle detection = ")); 486 Serial.println(roll_axis 487 & 0b00000011); 488 if(roll_axis & 0b10000000)Serial.println(F("Axis inverted 489 = yes")); 490 else Serial.println(F("Axis inverted = no")); 491 Serial.println(F("Put 492 the quadcopter back in its original position")); 493 Serial.println(F("Move 494 stick 'nose up' and back to center to continue")); 495 check_to_continue(); 496 497 498 //Detect the nose up movement 499 Serial.println(F("")); 500 Serial.println(F("")); 501 502 Serial.println(F("Lift the nose of the quadcopter to a 45 degree angle within 503 10 seconds")); 504 //Check axis movement 505 check_gyro_axes(2); 506 507 } 508 if(error == 0){ 509 Serial.println(F("OK!")); 510 Serial.print(F("Angle 511 detection = ")); 512 Serial.println(pitch_axis & 0b00000011); 513 if(pitch_axis 514 & 0b10000000)Serial.println(F("Axis inverted = yes")); 515 else Serial.println(F("Axis 516 inverted = no")); 517 Serial.println(F("Put the quadcopter back in its original 518 position")); 519 Serial.println(F("Move stick 'nose up' and back to center 520 to continue")); 521 check_to_continue(); 522 523 //Detect the nose 524 right movement 525 Serial.println(F("")); 526 Serial.println(F("")); 527 528 Serial.println(F("Rotate the nose of the quadcopter 45 degree to the right 529 within 10 seconds")); 530 //Check axis movement 531 check_gyro_axes(3); 532 533 } 534 if(error == 0){ 535 Serial.println(F("OK!")); 536 Serial.print(F("Angle 537 detection = ")); 538 Serial.println(yaw_axis & 0b00000011); 539 if(yaw_axis 540 & 0b10000000)Serial.println(F("Axis inverted = yes")); 541 else Serial.println(F("Axis 542 inverted = no")); 543 Serial.println(F("Put the quadcopter back in its original 544 position")); 545 Serial.println(F("Move stick 'nose up' and back to center 546 to continue")); 547 check_to_continue(); 548 } 549 } 550 if(error == 551 0){ 552 Serial.println(F("")); 553 Serial.println(F("===================================================")); 554 555 Serial.println(F("LED test")); 556 Serial.println(F("===================================================")); 557 558 digitalWrite(12, HIGH); 559 Serial.println(F("The LED should now be lit")); 560 561 Serial.println(F("Move stick 'nose up' and back to center to continue")); 562 563 check_to_continue(); 564 digitalWrite(12, LOW); 565 } 566 567 Serial.println(F("")); 568 569 570 if(error == 0){ 571 Serial.println(F("===================================================")); 572 573 Serial.println(F("Final setup check")); 574 Serial.println(F("===================================================")); 575 576 delay(1000); 577 if(receiver_check_byte == 0b00001111){ 578 Serial.println(F("Receiver 579 channels ok")); 580 } 581 else{ 582 Serial.println(F("Receiver channel 583 verification failed!!! (ERROR 6)")); 584 error = 1; 585 } 586 delay(1000); 587 588 if(gyro_check_byte == 0b00000111){ 589 Serial.println(F("Gyro axes ok")); 590 591 } 592 else{ 593 Serial.println(F("Gyro exes verification failed!!! 594 (ERROR 7)")); 595 error = 1; 596 } 597 } 598 599 if(error == 0){ 600 601 //If all is good, store the information in the EEPROM 602 Serial.println(F("")); 603 604 Serial.println(F("===================================================")); 605 606 Serial.println(F("Storing EEPROM information")); 607 Serial.println(F("===================================================")); 608 609 Serial.println(F("Writing EEPROM")); 610 delay(1000); 611 Serial.println(F("Done!")); 612 613 EEPROM.write(0, center_channel_1 & 0b11111111); 614 EEPROM.write(1, center_channel_1 615 >> 8); 616 EEPROM.write(2, center_channel_2 & 0b11111111); 617 EEPROM.write(3, 618 center_channel_2 >> 8); 619 EEPROM.write(4, center_channel_3 & 0b11111111); 620 621 EEPROM.write(5, center_channel_3 >> 8); 622 EEPROM.write(6, center_channel_4 623 & 0b11111111); 624 EEPROM.write(7, center_channel_4 >> 8); 625 EEPROM.write(8, 626 high_channel_1 & 0b11111111); 627 EEPROM.write(9, high_channel_1 >> 8); 628 EEPROM.write(10, 629 high_channel_2 & 0b11111111); 630 EEPROM.write(11, high_channel_2 >> 8); 631 632 EEPROM.write(12, high_channel_3 & 0b11111111); 633 EEPROM.write(13, high_channel_3 634 >> 8); 635 EEPROM.write(14, high_channel_4 & 0b11111111); 636 EEPROM.write(15, 637 high_channel_4 >> 8); 638 EEPROM.write(16, low_channel_1 & 0b11111111); 639 EEPROM.write(17, 640 low_channel_1 >> 8); 641 EEPROM.write(18, low_channel_2 & 0b11111111); 642 EEPROM.write(19, 643 low_channel_2 >> 8); 644 EEPROM.write(20, low_channel_3 & 0b11111111); 645 EEPROM.write(21, 646 low_channel_3 >> 8); 647 EEPROM.write(22, low_channel_4 & 0b11111111); 648 EEPROM.write(23, 649 low_channel_4 >> 8); 650 EEPROM.write(24, channel_1_assign); 651 EEPROM.write(25, 652 channel_2_assign); 653 EEPROM.write(26, channel_3_assign); 654 EEPROM.write(27, 655 channel_4_assign); 656 EEPROM.write(28, roll_axis); 657 EEPROM.write(29, pitch_axis); 658 659 EEPROM.write(30, yaw_axis); 660 EEPROM.write(31, type); 661 EEPROM.write(32, 662 gyro_address); 663 //Write the EEPROM signature 664 EEPROM.write(33, 'J'); 665 666 EEPROM.write(34, 'M'); 667 EEPROM.write(35, 'B'); 668 669 670 671 //To make sure evrything is ok, verify the EEPROM data. 672 Serial.println(F("Verify 673 EEPROM data")); 674 delay(1000); 675 if(center_channel_1 != ((EEPROM.read(1) 676 << 8) | EEPROM.read(0)))error = 1; 677 if(center_channel_2 != ((EEPROM.read(3) 678 << 8) | EEPROM.read(2)))error = 1; 679 if(center_channel_3 != ((EEPROM.read(5) 680 << 8) | EEPROM.read(4)))error = 1; 681 if(center_channel_4 != ((EEPROM.read(7) 682 << 8) | EEPROM.read(6)))error = 1; 683 684 if(high_channel_1 != ((EEPROM.read(9) 685 << 8) | EEPROM.read(8)))error = 1; 686 if(high_channel_2 != ((EEPROM.read(11) 687 << 8) | EEPROM.read(10)))error = 1; 688 if(high_channel_3 != ((EEPROM.read(13) 689 << 8) | EEPROM.read(12)))error = 1; 690 if(high_channel_4 != ((EEPROM.read(15) 691 << 8) | EEPROM.read(14)))error = 1; 692 693 if(low_channel_1 != ((EEPROM.read(17) 694 << 8) | EEPROM.read(16)))error = 1; 695 if(low_channel_2 != ((EEPROM.read(19) 696 << 8) | EEPROM.read(18)))error = 1; 697 if(low_channel_3 != ((EEPROM.read(21) 698 << 8) | EEPROM.read(20)))error = 1; 699 if(low_channel_4 != ((EEPROM.read(23) 700 << 8) | EEPROM.read(22)))error = 1; 701 702 if(channel_1_assign != EEPROM.read(24))error 703 = 1; 704 if(channel_2_assign != EEPROM.read(25))error = 1; 705 if(channel_3_assign 706 != EEPROM.read(26))error = 1; 707 if(channel_4_assign != EEPROM.read(27))error 708 = 1; 709 710 if(roll_axis != EEPROM.read(28))error = 1; 711 if(pitch_axis 712 != EEPROM.read(29))error = 1; 713 if(yaw_axis != EEPROM.read(30))error = 1; 714 715 if(type != EEPROM.read(31))error = 1; 716 if(gyro_address != EEPROM.read(32))error 717 = 1; 718 719 if('J' != EEPROM.read(33))error = 1; 720 if('M' != EEPROM.read(34))error 721 = 1; 722 if('B' != EEPROM.read(35))error = 1; 723 724 if(error == 1)Serial.println(F("EEPROM 725 verification failed!!! (ERROR 5)")); 726 else Serial.println(F("Verification 727 done")); 728 } 729 730 731 if(error == 0){ 732 Serial.println(F("Setup 733 is finished.")); 734 Serial.println(F("You can now calibrate the esc's and 735 upload the YMFC-AL code.")); 736 } 737 else{ 738 Serial.println(F("The setup 739 is aborted due to an error.")); 740 Serial.println(F("Check the Q and A page 741 of the YMFC-AL project on:")); 742 Serial.println(F("www.brokking.net for more 743 information about this error.")); 744 } 745 while(1); 746} 747 748//Search for 749 the gyro and check the Who_am_I register 750byte search_gyro(int gyro_address, int 751 who_am_i){ 752 Wire.beginTransmission(gyro_address); 753 Wire.write(who_am_i); 754 755 Wire.endTransmission(); 756 Wire.requestFrom(gyro_address, 1); 757 timer = millis() 758 + 100; 759 while(Wire.available() < 1 && timer > millis()); 760 lowByte = Wire.read(); 761 762 address = gyro_address; 763 return lowByte; 764} 765 766void start_gyro(){ 767 768 //Setup the L3G4200D or L3GD20H 769 if(type == 2 || type == 3){ 770 Wire.beginTransmission(address); 771 //Start communication with the gyro with the address 772 found during search 773 Wire.write(0x20); //We 774 want to write to register 1 (20 hex) 775 Wire.write(0x0F); //Set 776 the register bits as 00001111 (Turn on the gyro and enable all axis) 777 Wire.endTransmission(); 778 //End the transmission with the gyro 779 780 781 Wire.beginTransmission(address); //Start communication 782 with the gyro (adress 1101001) 783 Wire.write(0x20); //Start 784 reading @ register 28h and auto increment with every read 785 Wire.endTransmission(); 786 //End the transmission 787 Wire.requestFrom(address, 788 1); //Request 6 bytes from the gyro 789 while(Wire.available() 790 < 1); //Wait until the 1 byte is received 791 Serial.print(F("Register 792 0x20 is set to:")); 793 Serial.println(Wire.read(),BIN); 794 795 Wire.beginTransmission(address); 796 //Start communication with the gyro with the address 797 found during search 798 Wire.write(0x23); //We 799 want to write to register 4 (23 hex) 800 Wire.write(0x90); //Set 801 the register bits as 10010000 (Block Data Update active & 500dps full scale) 802 803 Wire.endTransmission(); //End the transmission 804 with the gyro 805 806 Wire.beginTransmission(address); //Start 807 communication with the gyro (adress 1101001) 808 Wire.write(0x23); //Start 809 reading @ register 28h and auto increment with every read 810 Wire.endTransmission(); 811 //End the transmission 812 Wire.requestFrom(address, 813 1); //Request 6 bytes from the gyro 814 while(Wire.available() 815 < 1); //Wait until the 1 byte is received 816 Serial.print(F("Register 817 0x23 is set to:")); 818 Serial.println(Wire.read(),BIN); 819 820 } 821 //Setup 822 the MPU-6050 823 if(type == 1){ 824 825 Wire.beginTransmission(address); 826 //Start communication with the gyro 827 Wire.write(0x6B); 828 //PWR_MGMT_1 register 829 Wire.write(0x00); 830 //Set to zero to turn on the gyro 831 832 Wire.endTransmission(); //End the transmission 833 834 835 Wire.beginTransmission(address); //Start 836 communication with the gyro 837 Wire.write(0x6B); //Start 838 reading @ register 28h and auto increment with every read 839 Wire.endTransmission(); 840 //End the transmission 841 Wire.requestFrom(address, 842 1); //Request 1 bytes from the gyro 843 while(Wire.available() 844 < 1); //Wait until the 1 byte is received 845 Serial.print(F("Register 846 0x6B is set to:")); 847 Serial.println(Wire.read(),BIN); 848 849 Wire.beginTransmission(address); 850 //Start communication with the gyro 851 Wire.write(0x1B); 852 //GYRO_CONFIG register 853 Wire.write(0x08); 854 //Set the register bits as 00001000 855 (500dps full scale) 856 Wire.endTransmission(); //End 857 the transmission 858 859 Wire.beginTransmission(address); //Start 860 communication with the gyro (adress 1101001) 861 Wire.write(0x1B); //Start 862 reading @ register 28h and auto increment with every read 863 Wire.endTransmission(); 864 //End the transmission 865 Wire.requestFrom(address, 866 1); //Request 1 bytes from the gyro 867 while(Wire.available() 868 < 1); //Wait until the 1 byte is received 869 Serial.print(F("Register 870 0x1B is set to:")); 871 Serial.println(Wire.read(),BIN); 872 873 } 874} 875 876void 877 gyro_signalen(){ 878 if(type == 2 || type == 3){ 879 Wire.beginTransmission(address); 880 //Start communication with the gyro 881 Wire.write(168); 882 //Start reading @ register 28h and 883 auto increment with every read 884 Wire.endTransmission(); //End 885 the transmission 886 Wire.requestFrom(address, 6); //Request 887 6 bytes from the gyro 888 while(Wire.available() < 6); //Wait 889 until the 6 bytes are received 890 lowByte = Wire.read(); //First 891 received byte is the low part of the angular data 892 highByte = Wire.read(); 893 //Second received byte is the high part of 894 the angular data 895 gyro_roll = ((highByte<<8)|lowByte); //Multiply 896 highByte by 256 (shift left by 8) and ad lowByte 897 if(cal_int == 2000)gyro_roll 898 -= gyro_roll_cal; //Only compensate after the calibration 899 lowByte 900 = Wire.read(); //First received byte is the 901 low part of the angular data 902 highByte = Wire.read(); //Second 903 received byte is the high part of the angular data 904 gyro_pitch = ((highByte<<8)|lowByte); 905 //Multiply highByte by 256 (shift left by 8) and ad lowByte 906 907 if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal; //Only compensate 908 after the calibration 909 lowByte = Wire.read(); //First 910 received byte is the low part of the angular data 911 highByte = Wire.read(); 912 //Second received byte is the high part of 913 the angular data 914 gyro_yaw = ((highByte<<8)|lowByte); //Multiply 915 highByte by 256 (shift left by 8) and ad lowByte 916 if(cal_int == 2000)gyro_yaw 917 -= gyro_yaw_cal; //Only compensate after the calibration 918 } 919 920 if(type == 1){ 921 Wire.beginTransmission(address); //Start 922 communication with the gyro 923 Wire.write(0x43); //Start 924 reading @ register 43h and auto increment with every read 925 Wire.endTransmission(); 926 //End the transmission 927 Wire.requestFrom(address,6); 928 //Request 6 bytes from the gyro 929 while(Wire.available() 930 < 6); //Wait until the 6 bytes are received 931 932 gyro_roll=Wire.read()<<8|Wire.read(); //Read high and 933 low part of the angular data 934 if(cal_int == 2000)gyro_roll -= gyro_roll_cal; 935 //Only compensate after the calibration 936 gyro_pitch=Wire.read()<<8|Wire.read(); 937 //Read high and low part of the angular data 938 if(cal_int 939 == 2000)gyro_pitch -= gyro_pitch_cal; //Only compensate after the calibration 940 941 gyro_yaw=Wire.read()<<8|Wire.read(); //Read high and 942 low part of the angular data 943 if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal; 944 //Only compensate after the calibration 945 } 946} 947 948//Check 949 if a receiver input value is changing within 30 seconds 950void check_receiver_inputs(byte 951 movement){ 952 byte trigger = 0; 953 int pulse_length; 954 timer = millis() + 955 30000; 956 while(timer > millis() && trigger == 0){ 957 delay(250); 958 if(receiver_input_channel_1 959 > 1750 || receiver_input_channel_1 < 1250){ 960 trigger = 1; 961 receiver_check_byte 962 |= 0b00000001; 963 pulse_length = receiver_input_channel_1; 964 } 965 if(receiver_input_channel_2 966 > 1750 || receiver_input_channel_2 < 1250){ 967 trigger = 2; 968 receiver_check_byte 969 |= 0b00000010; 970 pulse_length = receiver_input_channel_2; 971 } 972 if(receiver_input_channel_3 973 > 1750 || receiver_input_channel_3 < 1250){ 974 trigger = 3; 975 receiver_check_byte 976 |= 0b00000100; 977 pulse_length = receiver_input_channel_3; 978 } 979 if(receiver_input_channel_4 980 > 1750 || receiver_input_channel_4 < 1250){ 981 trigger = 4; 982 receiver_check_byte 983 |= 0b00001000; 984 pulse_length = receiver_input_channel_4; 985 } 986 } 987 988 if(trigger == 0){ 989 error = 1; 990 Serial.println(F("No stick movement 991 detected in the last 30 seconds!!! (ERROR 2)")); 992 } 993 //Assign the stick 994 to the function. 995 else{ 996 if(movement == 1){ 997 channel_3_assign 998 = trigger; 999 if(pulse_length < 1250)channel_3_assign += 0b10000000; 1000 } 1001 1002 if(movement == 2){ 1003 channel_1_assign = trigger; 1004 if(pulse_length 1005 < 1250)channel_1_assign += 0b10000000; 1006 } 1007 if(movement == 3){ 1008 channel_2_assign 1009 = trigger; 1010 if(pulse_length < 1250)channel_2_assign += 0b10000000; 1011 } 1012 1013 if(movement == 4){ 1014 channel_4_assign = trigger; 1015 if(pulse_length 1016 < 1250)channel_4_assign += 0b10000000; 1017 } 1018 } 1019} 1020 1021void check_to_continue(){ 1022 1023 byte continue_byte = 0; 1024 while(continue_byte == 0){ 1025 if(channel_2_assign 1026 == 0b00000001 && receiver_input_channel_1 > center_channel_1 + 150)continue_byte 1027 = 1; 1028 if(channel_2_assign == 0b10000001 && receiver_input_channel_1 < center_channel_1 1029 - 150)continue_byte = 1; 1030 if(channel_2_assign == 0b00000010 && receiver_input_channel_2 1031 > center_channel_2 + 150)continue_byte = 1; 1032 if(channel_2_assign == 0b10000010 1033 && receiver_input_channel_2 < center_channel_2 - 150)continue_byte = 1; 1034 if(channel_2_assign 1035 == 0b00000011 && receiver_input_channel_3 > center_channel_3 + 150)continue_byte 1036 = 1; 1037 if(channel_2_assign == 0b10000011 && receiver_input_channel_3 < center_channel_3 1038 - 150)continue_byte = 1; 1039 if(channel_2_assign == 0b00000100 && receiver_input_channel_4 1040 > center_channel_4 + 150)continue_byte = 1; 1041 if(channel_2_assign == 0b10000100 1042 && receiver_input_channel_4 < center_channel_4 - 150)continue_byte = 1; 1043 delay(100); 1044 1045 } 1046 wait_sticks_zero(); 1047} 1048 1049//Check if the transmitter sticks are in 1050 the neutral position 1051void wait_sticks_zero(){ 1052 byte zero = 0; 1053 while(zero 1054 < 15){ 1055 if(receiver_input_channel_1 < center_channel_1 + 20 && receiver_input_channel_1 1056 > center_channel_1 - 20)zero |= 0b00000001; 1057 if(receiver_input_channel_2 < 1058 center_channel_2 + 20 && receiver_input_channel_2 > center_channel_2 - 20)zero |= 1059 0b00000010; 1060 if(receiver_input_channel_3 < center_channel_3 + 20 && receiver_input_channel_3 1061 > center_channel_3 - 20)zero |= 0b00000100; 1062 if(receiver_input_channel_4 < 1063 center_channel_4 + 20 && receiver_input_channel_4 > center_channel_4 - 20)zero |= 1064 0b00001000; 1065 delay(100); 1066 } 1067} 1068 1069//Checck if the receiver values 1070 are valid within 10 seconds 1071void wait_for_receiver(){ 1072 byte zero = 0; 1073 1074 timer = millis() + 10000; 1075 while(timer > millis() && zero < 15){ 1076 if(receiver_input_channel_1 1077 < 2100 && receiver_input_channel_1 > 900)zero |= 0b00000001; 1078 if(receiver_input_channel_2 1079 < 2100 && receiver_input_channel_2 > 900)zero |= 0b00000010; 1080 if(receiver_input_channel_3 1081 < 2100 && receiver_input_channel_3 > 900)zero |= 0b00000100; 1082 if(receiver_input_channel_4 1083 < 2100 && receiver_input_channel_4 > 900)zero |= 0b00001000; 1084 delay(500); 1085 1086 Serial.print(F(".")); 1087 } 1088 if(zero == 0){ 1089 error = 1; 1090 Serial.println(F(".")); 1091 1092 Serial.println(F("No valid receiver signals found!!! (ERROR 1)")); 1093 } 1094 1095 else Serial.println(F(" OK")); 1096} 1097 1098//Register the min and max receiver 1099 values and exit when the sticks are back in the neutral position 1100void register_min_max(){ 1101 1102 byte zero = 0; 1103 low_channel_1 = receiver_input_channel_1; 1104 low_channel_2 1105 = receiver_input_channel_2; 1106 low_channel_3 = receiver_input_channel_3; 1107 low_channel_4 1108 = receiver_input_channel_4; 1109 while(receiver_input_channel_1 < center_channel_1 1110 + 20 && receiver_input_channel_1 > center_channel_1 - 20)delay(250); 1111 Serial.println(F("Measuring 1112 endpoints....")); 1113 while(zero < 15){ 1114 if(receiver_input_channel_1 < center_channel_1 1115 + 20 && receiver_input_channel_1 > center_channel_1 - 20)zero |= 0b00000001; 1116 1117 if(receiver_input_channel_2 < center_channel_2 + 20 && receiver_input_channel_2 1118 > center_channel_2 - 20)zero |= 0b00000010; 1119 if(receiver_input_channel_3 < 1120 center_channel_3 + 20 && receiver_input_channel_3 > center_channel_3 - 20)zero |= 1121 0b00000100; 1122 if(receiver_input_channel_4 < center_channel_4 + 20 && receiver_input_channel_4 1123 > center_channel_4 - 20)zero |= 0b00001000; 1124 if(receiver_input_channel_1 < 1125 low_channel_1)low_channel_1 = receiver_input_channel_1; 1126 if(receiver_input_channel_2 1127 < low_channel_2)low_channel_2 = receiver_input_channel_2; 1128 if(receiver_input_channel_3 1129 < low_channel_3)low_channel_3 = receiver_input_channel_3; 1130 if(receiver_input_channel_4 1131 < low_channel_4)low_channel_4 = receiver_input_channel_4; 1132 if(receiver_input_channel_1 1133 > high_channel_1)high_channel_1 = receiver_input_channel_1; 1134 if(receiver_input_channel_2 1135 > high_channel_2)high_channel_2 = receiver_input_channel_2; 1136 if(receiver_input_channel_3 1137 > high_channel_3)high_channel_3 = receiver_input_channel_3; 1138 if(receiver_input_channel_4 1139 > high_channel_4)high_channel_4 = receiver_input_channel_4; 1140 delay(100); 1141 1142 } 1143} 1144 1145//Check if the angular position of a gyro axis is changing within 1146 10 seconds 1147void check_gyro_axes(byte movement){ 1148 byte trigger_axis = 0; 1149 1150 float gyro_angle_roll, gyro_angle_pitch, gyro_angle_yaw; 1151 //Reset all axes 1152 1153 gyro_angle_roll = 0; 1154 gyro_angle_pitch = 0; 1155 gyro_angle_yaw = 0; 1156 gyro_signalen(); 1157 1158 timer = millis() + 10000; 1159 while(timer > millis() && gyro_angle_roll > 1160 -30 && gyro_angle_roll < 30 && gyro_angle_pitch > -30 && gyro_angle_pitch < 30 && 1161 gyro_angle_yaw > -30 && gyro_angle_yaw < 30){ 1162 gyro_signalen(); 1163 if(type 1164 == 2 || type == 3){ 1165 gyro_angle_roll += gyro_roll * 0.00007; //0.00007 1166 = 17.5 (md/s) / 250(Hz) 1167 gyro_angle_pitch += gyro_pitch * 0.00007; 1168 gyro_angle_yaw 1169 += gyro_yaw * 0.00007; 1170 } 1171 if(type == 1){ 1172 gyro_angle_roll += 1173 gyro_roll * 0.0000611; // 0.0000611 = 1 / 65.5 (LSB degr/s) / 250(Hz) 1174 1175 gyro_angle_pitch += gyro_pitch * 0.0000611; 1176 gyro_angle_yaw += gyro_yaw 1177 * 0.0000611; 1178 } 1179 1180 delayMicroseconds(3700); //Loop is running 1181 @ 250Hz. +/-300us is used for communication with the gyro 1182 } 1183 //Assign the 1184 moved axis to the orresponding function (pitch, roll, yaw) 1185 if((gyro_angle_roll 1186 < -30 || gyro_angle_roll > 30) && gyro_angle_pitch > -30 && gyro_angle_pitch < 30 1187 && gyro_angle_yaw > -30 && gyro_angle_yaw < 30){ 1188 gyro_check_byte |= 0b00000001; 1189 1190 if(gyro_angle_roll < 0)trigger_axis = 0b10000001; 1191 else trigger_axis = 1192 0b00000001; 1193 } 1194 if((gyro_angle_pitch < -30 || gyro_angle_pitch > 30) && 1195 gyro_angle_roll > -30 && gyro_angle_roll < 30 && gyro_angle_yaw > -30 && gyro_angle_yaw 1196 < 30){ 1197 gyro_check_byte |= 0b00000010; 1198 if(gyro_angle_pitch < 0)trigger_axis 1199 = 0b10000010; 1200 else trigger_axis = 0b00000010; 1201 } 1202 if((gyro_angle_yaw 1203 < -30 || gyro_angle_yaw > 30) && gyro_angle_roll > -30 && gyro_angle_roll < 30 && 1204 gyro_angle_pitch > -30 && gyro_angle_pitch < 30){ 1205 gyro_check_byte |= 0b00000100; 1206 1207 if(gyro_angle_yaw < 0)trigger_axis = 0b10000011; 1208 else trigger_axis = 1209 0b00000011; 1210 } 1211 1212 if(trigger_axis == 0){ 1213 error = 1; 1214 Serial.println(F("No 1215 angular motion is detected in the last 10 seconds!!! (ERROR 4)")); 1216 } 1217 else 1218 1219 if(movement == 1)roll_axis = trigger_axis; 1220 if(movement == 2)pitch_axis = 1221 trigger_axis; 1222 if(movement == 3)yaw_axis = trigger_axis; 1223 1224} 1225 1226//This 1227 routine is called every time input 8, 9, 10 or 11 changed state 1228ISR(PCINT0_vect){ 1229 1230 current_time = micros(); 1231 //Channel 1========================================= 1232 1233 if(PINB & B00000001){ //Is input 8 high? 1234 1235 if(last_channel_1 == 0){ //Input 8 changed 1236 from 0 to 1 1237 last_channel_1 = 1; //Remember 1238 current input state 1239 timer_1 = current_time; //Set 1240 timer_1 to current_time 1241 } 1242 } 1243 else if(last_channel_1 == 1){ //Input 1244 8 is not high and changed from 1 to 0 1245 last_channel_1 = 0; //Remember 1246 current input state 1247 receiver_input_channel_1 = current_time - timer_1; //Channel 1248 1 is current_time - timer_1 1249 } 1250 //Channel 2========================================= 1251 1252 if(PINB & B00000010 ){ //Is input 9 high? 1253 1254 if(last_channel_2 == 0){ //Input 9 changed 1255 from 0 to 1 1256 last_channel_2 = 1; //Remember 1257 current input state 1258 timer_2 = current_time; //Set 1259 timer_2 to current_time 1260 } 1261 } 1262 else if(last_channel_2 == 1){ //Input 1263 9 is not high and changed from 1 to 0 1264 last_channel_2 = 0; //Remember 1265 current input state 1266 receiver_input_channel_2 = current_time - timer_2; //Channel 1267 2 is current_time - timer_2 1268 } 1269 //Channel 3========================================= 1270 1271 if(PINB & B00000100 ){ //Is input 10 high? 1272 1273 if(last_channel_3 == 0){ //Input 10 changed 1274 from 0 to 1 1275 last_channel_3 = 1; //Remember 1276 current input state 1277 timer_3 = current_time; //Set 1278 timer_3 to current_time 1279 } 1280 } 1281 else if(last_channel_3 == 1){ //Input 1282 10 is not high and changed from 1 to 0 1283 last_channel_3 = 0; //Remember 1284 current input state 1285 receiver_input_channel_3 = current_time - timer_3; //Channel 1286 3 is current_time - timer_3 1287 1288 } 1289 //Channel 4========================================= 1290 1291 if(PINB & B00001000 ){ //Is input 11 high? 1292 1293 if(last_channel_4 == 0){ //Input 11 changed 1294 from 0 to 1 1295 last_channel_4 = 1; //Remember 1296 current input state 1297 timer_4 = current_time; //Set 1298 timer_4 to current_time 1299 } 1300 } 1301 else if(last_channel_4 == 1){ //Input 1302 11 is not high and changed from 1 to 0 1303 last_channel_4 = 0; //Remember 1304 current input state 1305 receiver_input_channel_4 = current_time - timer_4; //Channel 1306 4 is current_time - timer_4 1307 } 1308} 1309 1310//Intro subroutine 1311void intro(){ 1312 1313 Serial.println(F("===================================================")); 1314 1315 delay(1500); 1316 Serial.println(F("")); 1317 Serial.println(F("Your")); 1318 1319 delay(500); 1320 Serial.println(F(" Multicopter")); 1321 delay(500); 1322 Serial.println(F(" 1323 Flight")); 1324 delay(500); 1325 Serial.println(F(" Controller")); 1326 1327 delay(1000); 1328 Serial.println(F("")); 1329 Serial.println(F("YMFC-AL Setup 1330 Program")); 1331 Serial.println(F("")); 1332 Serial.println(F("===================================================")); 1333 1334 delay(1500); 1335 Serial.println(F("For support and questions: www.brokking.net")); 1336 1337 Serial.println(F("")); 1338 Serial.println(F("Have fun!")); 1339} 1340
FC0G96FJGGTGFHF.ino
c_cpp
Code for HC12
1/* HC12 Send/Receive Example Program 1 2 By Mark J. Hughes 3 4 for AllAboutCircuits.com 5 6 Connect HC12 "RXD" pin to Arduino 7 Digital Pin 4 8 Connect HC12 "TXD" pin to Arduino Digital Pin 5 9 Connect 10 HC12 "Set" pin to Arduino Digital Pin 6 11 12 Do not power over USB. Per 13 datasheet, 14 power HC12 with a supply of at least 100 mA with 15 a 22 16 uF - 1000 uF reservoir capacitor. 17 Upload code to two Arduinos connected to 18 two computers. 19 20 Transceivers must be at least several meters apart 21 to work. 22 23 */ 24 25#include <SoftwareSerial.h> 26 27const byte HC12RxdPin 28 = 2; // Recieve Pin on HC12 29const byte HC12TxdPin = 3; // 30 Transmit Pin on HC12 31 32SoftwareSerial HC12(HC12TxdPin,HC12RxdPin); // Create 33 Software Serial Port 34 35void setup() { 36 Serial.begin(9600); // 37 Open serial port to computer 38 HC12.begin(9600); // Open 39 serial port to HC12 40} 41 42void loop() { 43 if(HC12.available()){ // 44 If Arduino's HC12 rx buffer has data 45 Serial.write(HC12.read()); // 46 Send the data to the computer 47 } 48 if(Serial.available()){ // 49 If Arduino's computer rx buffer has data 50 HC12.write(Serial.read()); // 51 Send that data to serial 52 } 53} 54 55 56
Downloadable files
fy56sbojggtg87z_gfBGEaIfsa.jpg
fy56sbojggtg87z_gfBGEaIfsa.jpg
GPS Schematics
GPS Schematics
fy56sbojggtg87z_gfBGEaIfsa.jpg
fy56sbojggtg87z_gfBGEaIfsa.jpg
Documentation
fecltorjgcj0zvp_RwD0Qb63mh.jpg
fecltorjgcj0zvp_RwD0Qb63mh.jpg
f5we3skjg74ycfs_7DcosCYYdP.jpg
f5we3skjg74ycfs_7DcosCYYdP.jpg
fe48eurjg74yfxx_sZTGgG2wgs.jpg
fe48eurjg74yfxx_sZTGgG2wgs.jpg
untitled
untitled
f3un3gnjg74ydga_kRHHj80dRI.jpg
f3un3gnjg74ydga_kRHHj80dRI.jpg
fe48eurjg74yfxx_sZTGgG2wgs.jpg
fe48eurjg74yfxx_sZTGgG2wgs.jpg
untitled
untitled
fecltorjgcj0zvp_RwD0Qb63mh.jpg
fecltorjgcj0zvp_RwD0Qb63mh.jpg
f3un3gnjg74ydga_kRHHj80dRI.jpg
f3un3gnjg74ydga_kRHHj80dRI.jpg
f5we3skjg74ycfs_7DcosCYYdP.jpg
f5we3skjg74ycfs_7DcosCYYdP.jpg
Comments
Only logged in users can leave comments