การเขียนโปรแกรมQuadAuto32bitV1 26 4 59

โครงการอบรมเสริมสร้างทักษะวิชาชีพด้านวิศวกรรมเมคคาทรอนิกส์ คณะวิศวกรรมศาสตร์ ------------------------------เรื่องการสร้างเครื่องบินสี่ใบพัดและควบคุมตาแหน่งเบื้องต้นด้วยระบบจีพีเอส สาขาวิชาวิศวกรรมเมคคาทรอนิกส์ มหาวิทยาลัยเทคโนโลยีราชมงคลอีสานวิทยาเขตขอนแก่น .................................. วันที่ 7-8 พฤษภาคม 2559 สาขาวิชาวิศวกรรมเมคคาทรอนิกส์ คณะวิศวกรรมศาสตร์ มหาวิทยาลัยเทคโนโลยีราชมงคลอีสานวิทยาเขตขอนแก่น โดย วิทยากร นายทินกร เขียวรี อาจารย์วิศวกรรมเครื่องกล คณะวิศวกรรมศาสตร์และสถาปัตยกรรมศาสตร์ มหาวิทยาลัยเทคโนโลยีราชมงคลสุวรรณภูมิ ศูนย์นนทบุรี http://quad3d-tin.lnwshop.com tinnakon_za@hotmail.com https://www.facebook.com/tinnakonza โทร 086-054-0582 ……………อัพเดทวันที่ 26 เม.ย 59…………… 2 สารบัญ หน้า รายละเอียดอุปกรณ์ที่ใช้ ขั้นตอนการประกอบ หัวข้อที่ต้องรู้ในการเขียนโปรแกรม ตัวอย่างโปรแกรมที่เขียน (Quadrotor Autopilot / Flight Controller) 1. ไฟล์ Due32bit_GY521MPU_5_V1.ino 2. ไฟล์ Aconfigsam3x8e.h 3. ไฟล์ AnalogSource_Arduino.h 4. ไฟล์ Control_PPIDsam3x8e.h 5. ไฟล์ Kalman_ObserverTin.h 6. ไฟล์ Ultrasonic04.h 7. ไฟล์ ahrs_tinsam3x8e.h 8. ไฟล์ motorX4sam3x8e.h 9. ไฟล์ mpu6050sam3x8e.h 10. ไฟล์ multi_rx_sam3x8e.h 11. ไฟล์ GPSNEO6N_multi.h 12. ไฟล์ AP_Baro_MS5611.cpp 13. ไฟล์ AP_Baro_MS5611.h 14. ไฟล์ SPI_sam.cpp 15. ไฟล์ SPI_sam.h 16. ไฟล์ Wire_due32.cpp 17. ไฟล์ Wire_due32.h 3 8 19 21 21 28 31 32 37 38 40 43 47 53 55 58 - เอกสารอ้างอิง ประวัติผู้จัดทา 59 60 โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 3 รายการชิ้นส่วนที่ใช้ ที่ รายการ ๑ GPS ublox NEO 7n with compass ๒ Arduino Due 32bit ๓ GY-521 IMU MPU6050 ๔ MS5611 SPI Communication ๕ Logic Level Converter 3.3 v ๖ Afro ESC 30Amp ๗ มอเตอร์ UFO 4010-850KV ๘ ใบพัด 10x4.5 นิ้ว ๙ เฟรม quad Frame_Auto450_V2 ๑๐ Hobbywing UBEC 5V/6V 3A ๑๑ สายไฟ ขั้วแบท แผ่นปริ้น รัดแบต ๑๒ รีโมท STORM i6S 2.4GHz ๑๓ แบต 2200 mah 3s 30c ๑๔ เครื่องชาจ์ท i6AC- 80W ๑๕ Programmable and Documents ๑๖ เสาตั้ง GPS สีดา ๑๗ Module RGB LED ๑๘ อัลตราโซนิก HC-SR04 จานวน 1 1 1 1 1 4 4 4 1 1 1 1 1 1 1 1 1 1 หน่วย ชิ้น ชิ้น ชิ้น ชิ้น ชิ้น ชิ้น ตัว ใบ ชุด ชิ้น อัน ชิ้น ชิ้น ชุด ชุด ชิ้น ชิ้น ชิ้น รายละเอียดอุปกรณ์ที่ใช้ 1. GPS ublox NEO 7n with compass Receiver type 56 Channels Cold Start 29 s 30 s Warm Start 28 s 28 s Hot Start 1 s 1 s Horizontal position accuracy Autonomous 2.5 m SBAS 2.0 m Max navigation update rate 10 Hz Velocity accuracy 0.1 m/s Heading accuracy 0.5 degrees Dynamics ≤ 4 g Altitude 50,000 m Velocity 500 m/s 2. Arduino Due 32bit Microcontroller AT91SAM3X8E Operating Voltage 3.3V Flash Memory 512 KB SRAM 96 KB (two banks: 64KB and 32KB) Clock Speed 84 MHz โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 4 3. GY-521 IMU MPU6050 Use the chip: MPU-6050 Power supply: 3V-5v. Communication modes: standard IIC communication protocol. Chip built-in 16 bit AD converter, 16 bits of data output Gyroscope range: + 250 500 1000 2000 ° / s Acceleration range: ± 2 ± 4 ± 8 ± 16 g 4. MS5611 GY-63 SPI Communication Name: MS5611 module (atmospheric pressure module) Built-in 24bit AD converter chip High quality Immersion Gold PCB Using the chip: MS5611 Power supply: 3V-5V (internal low dropout regulator) Communication: Standard IIC / SPI communications protocol 5. Logic Level Converter 3.3 v Can be used with normal serial, I2C, SPI, and any other digital signal 6. Afro ESC 30Amp high efficiency, all N-channel MOSFET design multi-rotor specific SimonK software Current Draw: 30A Continuous Voltage Range: 2-4s Lipoly BEC: 0.5A Linear Input Freq: 1KHz Firmware: afro_nfet.hex 7.มอเตอร์ UFO 4010-850KV UFO 4010 850KV Motor Lipo 3-4 S prop 10-13 in FlightPower 3S 40C 5200mA batt 1255 T-Type Carbon fober prop Frame 1020 g + batt 440 g = 1460 g Flight time : 20 mins โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 5 8. ใบพัด 10x4.5 นิ้ว Propeller 10x4.5 Black (CW/CCW) (4pcs) Prop Hole Size: 4.75mm Prop-Adaptor Hole Size 3.0mm, 3.17mm, 4.0mm Weight of 1 prop: 8g approximately 1 x ABS Plastic Prop CW (clockwise) 1 x ABS Plastic Prop CCW (counter - clockwise) 2 x Prop Adaptor Rings 9 เฟรม quad Frame_Auto450_V2 10 Hobbywing UBEC 5V/6V 3A Item Name: 3A-UBEC Output Voltage: 5V@3A or 6V@3A (Selectable by using a jumper connector) Continuous output current: 3 Amps Input: 5.5V-26V (2-6S Lipo or 5-18 cells NiMH /NiCd) Size: 43x 17 x 7mm Weight: 11g 11 สายไฟ ขั้วแบท แผ่นปริ้น รัดแบต Silicone Wire 16AWG 14 AWG Battery Strap สายรัดแบตเตอร์รี่ สายรัดแยต กว้าง 20 ยาว 200 มิลลิเมตร Nylon XT60 Connectors Male/Female XT = heXTronik. 60 = 60A constant. โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 6 12 รีโมท STORM i6S 2.4GHz Frequency: 2.4GHz Modulation: DSMX No. of Channels: 6 Model Memory: 10 Stick Mode: Mode 2 (left stick throttle) 13 แบต 2200 mah 3s 30c Fullymax 2200 mah/3s/30c XT60 plug 14 เครื่องชาจ์ท i6AC- 80W Dual input: DC 11-18V, AC 100-240V AC to DC adapter(DC 11-18V/6A) Charge current/power: 0.1-8A/80W Discharge current/power: 0.1-2A/10W Current drain for balancing Lipo: 300mAh/cell Cell count: 1-6s Lixx; 1-15s Nixx; 2-10s Pb Dimensions: 135*145*43mm Approx Weight: 590 g 15 Programmable and Documents The open-source Arduino Software (IDE) Arduino ide 1.5.8 Due32bit_GY521MPU_1-6_V1 16 เสาตั้ง GPS สีดา GPS Holder Mount Bracket Stand For 7N 6M M8N Neo-7N GPS Module Black Color โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 7 17 Module RGB LED Wrobot Full Color RGB LED Module adopt one pcs 5050 anode luminotron. Common termination port connect with +5V, it is effective when control prot is low level. This module can achieve effect of different color change through PWM program code. Wrobot Full Color RGB LED Module pin definitions : (1) Vcc (2) R (3) B (4) G. 18 อัลตราโซนิก HC-SR04 PIN ที่ 1 -- 5V Supply PIN ที่ 2 -- Trigger Pulse Input PIN ที่ 3 -- Echo Pulse Output PIN ที่ 4 -- GND Working voltage: DC 5 V Static current: 3 mA Induction Angle: Less than 15 Detection range:2 cm to 4 m Detecting precision: 0.3 cm + 1% โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 8 ขั้นตอนการประกอบเริ่มต้น 1.ทาการประกอบเฟรมเครื่องบิน 4 ใบพัด เตรียมอุปกรณ์ ประกอบแผ่นเฟรมหลัก แขนยึดมอเตอร์ โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 9 ประกอบขาตั้ง ไขควงขันน็อตให้แน่นทุกตัว โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 10 2. ทาการประกอบมอเตอร์ 3. ทาการประกอบสปีดคอนโทรลเลอร์ (ESC) แสดงทิศทางการหมุน กับการเสียบสายไฟมอเตอร์ บรัชเรด Motor Brushless โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 11 4. ทาการประกอบขาตั้ง GPS จะต้องมีสว่านมือ เจาะรู 3 mm ประกอบมอเตอร์ สปีด เฟรม ขาตั้ง GPS ดังรูปที่แสดง โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 12 5. ตรวจสอบทิศทางหมุนของมอเตอร์ โดยใช้ Servo Tester แสดงทิศทางหมุนมอเตอร์แบบต่าง ๆ ที่มา http://wiki.dji.com/en/index.php/A2_Mixer_Type_Supported โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 13 6. ติดตั้งเซนเซอร์และไมโครคอนโทรลเลอร์ โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 14 ไดอะแกรมการต่อสายไฟ ตัว Logic Level Converter 3.3 v ใช้เพื่อแก้ปัญหา ไมโครค้างเนื่องจาก i2c เข็มทิศ hmc5883l โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 15 7. ติดตั้งรีซีปรีโมท ต่อสายไฟเข้าขา A8 ของ Arduino แสดงกราฟสัญญาณ PPM และ PWM โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 16 8. ทาการอับโหลดโปรแกรม Due32bit_GY521MPU_1_V1 โดยการเสียบสาย usb เข้า Programming Port กด Upload รอสักพักหนึ่ง โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 17 9. เปิด Serial Monitor เพื่อดูค่าสถานะเซนเซอร์ต่างๆ ขึ้นอยู่กับที่เราเปิด code ว่าจะปริ้นค่าอะไรมาดูบ้าง โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 18 แสดงการเปิดคาสั่งปริ้นค่า Serial Monitor 10.ตรวจสอบทิศทางเซนเซอร์ ค่าความเร่งและความเร็วเชิงมุม accelRaw[XAXIS]; accelRaw[YAXIS]; accelRaw[ZAXIS]; GyroXf*RAD_TO_DEG; GyroYf*RAD_TO_DEG; GyroZf*RAD_TO_DEG; แสดงทิศทางการหมุนแกนเครื่องบิน ที่มา https://www.grc.nasa.gov โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 19 หัวข้อที่ต้องรู้ในการเขียนโปรแกรม บอร์ด Arduino Due 32 bit ,กับ เซนเซอร์ MPU6050 - i2c , MS5611 - spi อธิบาย Code Due32bit_GY521MPU_1-6_V1.ino 1. การกาหนดความเร็วลูป 1000 Hz, 200 Hz, 100 Hz, 50 Hz, 20 Hz, 10 Hz, 5 Hz, 2 Hz, 1 Hz 1000 Hz ทา อ่านเซนเซอร์ gyro,acc, 200 Hz ทา ลูปคานวณ ค่ามุม , ควบคุม PID , Filter ,สั่งมอเตอร์ 100 Hz ทา อ่านเซนเซอร์ baro ความดัน 50 Hz ทา อ่านค่ารีโมท PPM หรือ PWM 20 Hz อ่านค่า Ultrasonic , อ่านค่าเข็มทิศ, ควบคุม Position Hold 10 Hz ทา Automatic Takeoff และ Landing และ Chack_Command, By Remote idle-up settings 0,1,2,3 5 Hz คานวณค่า GPS , คานวณความสูงจาก baro 2 Hz ทาไฟกระพริบจานวนดาวเทียมจาก GPS 1 Hz แสดงสถานะหลอดไฟ LED ทา Accelerometers trim โดยใช้ รีโมท 2. การสร้างตัวแปรเพื่อเก็บค่า int 16 bit, long 32 bit ,uint8_t, unsigned long , float 32bit, 3. การอ่านค่าเซนเซอร์ gyroscope , accelerometer, magnetometer 4. การอ่านค่าบาโรเพื่อมาคานวณค่าความสูงจากเซนเซอร์ MS5611 5. การทาตัวสังเกตเพื่อประมาณค่าความสูง ,State estimation with Kalman Filter 6. การอ่านค่าเซนเซอร์ Ultrasonic , Trigger, Echo 7. การอ่านค่าเซนเซอร์ GPS ใช้ NMEA_BINARY settings ของ MultiWii หรือ APM 8. การอ่านค่ารีโมท PWM 1-1.9 ms แบบ PPM (Pulse Position Modulation) สายเส้นเดียว ได้ 6 CH 9. การกาหนดค่าเริ่มต้นใน setup() - ค่าเริ่มต้นการอ่านรีโมท - ค่าเริ่มต้นสั่งมอเตอร์ - ค่าเริ่มต้น GPS - ค่าเริ่มต้น I2C เปลี่ยน I2C clock rate to 400kHz - ค่าเริ่มต้นเซนเซอร์ mpu6050, HMC5883, MS5611 - ค่าเริ่มต้นอัลตร้าโซนิค - การอ่านเซนเซอร์เริ่มต้น - การทาคาริเบทเซนเซอร์ Gyro ให้เป็นศูนย์ - ตั้งค่าเริ่มต้นการหามุม AHRS (An attitude and heading reference system , AHRS) โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 20 - กาหนดเวลาเริ่มต้น 10. การทา sensor Calibration และการแปลงหน่วย rad/s , deg/s , m/s^2 11. การลดสัญญานรบกวน sensor , Moving average filter , Low pass filter 12. วิธีการหาค่ามุม AHRS, Quaternion , direction cosine matrix , Euler angles, Rotation matrix, Body Fixed Frame, Earth-Fixed frame 13. การอ่านค่า GPS latitude and longitude , ความเร็วการเคลื่อนที่ Earth-Fixed frame 14. การควบคุมพีไอดี , P-PID , Roll, Pitch, Yaw - คานวณหาค่ามุมรีโมท - การดิฟหาความเร็วจากมุมรีโมท - การคานวณค่า error - การคานวณกฏการควบคุมมุมและ Rate Gyro - การป้องกันค่าเกิน Saturation 15. การควบคุมความสูง Altitude ใช้ State feedback control หรือ PIDA 16. เทคนิคการจูนค่า Gain ที่ใช้ควบคุม 17. เงื่อนไขการทา Automatic Takeoff และ Landing 18. การควบคุมต่าแหน่ง GPS ใช้ PID control 19. การทา Motor mixing theory, Quad +,x 20. การสั่ง PWM 1-1.9 ms, 400 Hz ให้มอเตอร์หมุน 21. การทา Electronic Speed Controllers Calibration 1000 - 1850 us 22. การทา Accelerometers trim โดยใช้ รีโมท 23. การทา Calibration sensor Accelerometer หาค่า Max , Min 24. การทา Calibration sensor Magnetometer หาค่า Max , Min 25. การกาหนดค่าเข้าโหมดบิน ต่าง ๆ Stabilizer, Altitude Hold, Position Hold, RTH, Automatic 26. การทา ARM and DISARM มอเตอร์หมุนและไม่หมุน 27. อื่น ๆ ..... - การตั้งค่ารีโมทวิทยุ 2.4 GHz - การชาร์จแบตเตอรี่ลิโพ 4.2 โวลต์ Lithium Polymer ----------------------------------------------------- โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 21 ตัวอย่างโปรแกรมที่เขียน (Quadrotor Autopilot / Flight Controllers) ดาวโหลด Code ได้ที่ https://github.com/QuadTinnakon/Due32bit_GY521MPU_5_V1 1. ไฟล์ Due32bit_GY521MPU_5_V1.ino /* Test By tinnakon kheowree project tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza 27/03/2559 write Due32bit_GY521MPU_5_V1 support : Arduino 1.5.8 Arduino Due 32 bit , MPU6050 MS5611 • Atmel SAM3X8E ARM Cortex-M3 CPU 32-bit a 84 MHz clock, ARM core microcontroller • MPU6050 Gyro Accelerometer //I2C 400kHz nois gyro +-0.05 deg/s , acc +-0.04 m/s^2 • MS5611 Barometer//SPI • HMC5883L Magnetometer //I2C_BYPASS ,I2C 400kHz • GPS NEO-7N // Quad-X pin 9 FRONTL M1CW M2CCW \ FRONTR pin 8 / \ --- / | | / --- \ / pin 6 motor_BackL \ M4 CCW M3 CW motor_BackR pin 7 ----------rx----------A8 = PPM 8 CH */ #include <Arduino.h> #include "Wire_due32.h" #include "SPI_sam.h" #include "AP_Baro_MS5611.h" #include "Aconfigsam3x8e.h" #include "multi_rx_sam3x8e.h" #include "mpu6050sam3x8e.h" #include "ahrs_tinsam3x8e.h" #include "GPSNEO6N_multi.h" #include "Control_PPIDsam3x8e.h" #include "motorX4sam3x8e.h" #include "Ultrasonic04.h" #include "Kalman_ObserverTin.h" #include "AnalogSource_Arduino.h" //////////////////////////////////////////////////////////////////////////////////////////////// void setup() { Serial.begin(57600);//115200 //Serial1.begin(57600); Serial.print("Due32bit_GY521MPU_5_V1");Serial.println("\n"); GPS_multiInt(); pinMode(13, OUTPUT);//pinMode (30, OUTPUT);pinMode (31, OUTPUT);//pinMode (30, OUTPUT);pinMode (31, OUTPUT);//(13=A=M),(31=B=STABLEPIN),(30=C,GPS FIG LEDPIN) pinMode(12, OUTPUT); digitalWrite(12, HIGH); pinMode(Pin_Laser, OUTPUT); pinMode(Pin_LED_B, OUTPUT); pinMode(Pin_LED_G, OUTPUT); pinMode(Pin_LED_R, OUTPUT); digitalWrite(13, HIGH); digitalWrite(Pin_Laser, HIGH); digitalWrite(Pin_LED_B, LOW); digitalWrite(Pin_LED_G, LOW); digitalWrite(Pin_LED_R, LOW); configureReceiver(); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 22 motor_initialize(); ESC_calibration(); delay(10); Wire.begin(); Wire.setClock(400000); delay(10); Wire1.begin(); Wire1.setClock(400000);//400000 delay(10); mpu6050_initialize(); delay(30); //GYROSCOPE START-UP TIME 30 ms MagHMC5883Int(); Serial.print("HMC5883_initialize");Serial.print("\n"); delay(10); SPI.begin(); //SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later delay(10); //baro.init(MS561101BA_ADDR_CSB_LOW); baro.init(); //baro.calibrate(); //Serial.print("MS5611_initialize");Serial.print("\n"); delay(10); UltrasonicInt(); delay(10); Serial.print("Read Sensor");Serial.println("\t"); for(int i=0; i<100; i++) { mpu6050_readGyroSum(); mpu6050_readAccelSum(); Mag5883Read(); //temperaturetr = baro.getTemperature(MS561101BA_OSR_4096); //presser = baro.getPressure(MS561101BA_OSR_4096); baro._update(micros()); baro.read(); presser = baro.get_pressure(); temperaturetr = baro.get_temperature(); pushAvg(presser, temperaturetr); delay(10); } presserf = getAvg(movavg_buff, MOVAVG_SIZE); temperaturetrf = getAvg(movavg_buffT, MOVAVG_SIZE); sea_press = presserf - 0.12;// + 0.09 presser 1007.25 1003.52 Serial.print("presser ");Serial.print(sea_press); Serial.print(" Temperature ");Serial.println(temperaturetrf); digitalWrite(Pin_LED_B, HIGH); digitalWrite(Pin_LED_G, HIGH); digitalWrite(Pin_LED_R, HIGH); mpu6050_Get_accel(); mpu6050_Get_gyro(); delay(10); sensor_Calibrate();//sensor.h ahrs_initialize();//ahrs.h RC_Calibrate();//"multi_rxPPM2560.h" setupFourthOrder();//ahrs delay(100); digitalWrite(13, LOW); for(uint8_t i=0;i<=10;i++){ //GPS_NewData(); while(Serial2.available()) { char byteGPS1=Serial2.read(); GPS_UBLOX_newFrame(byteGPS1); } GPS_LAT1 = GPS_coord[LAT]/10000000.0;// 1e-7 degrees / position as degrees (*10E7) GPS_LON1 = GPS_coord[LON]/10000000.0; GPS_LAT_HOME = GPS_LAT1; GPS_LON_HOME = GPS_LON1; digitalWrite(Pin_LED_B, LOW); delay(20); digitalWrite(Pin_LED_B, HIGH); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 23 delay(80); } digitalWrite(Pin_Laser, LOW); digitalWrite(Pin_LED_B, LOW); digitalWrite(Pin_LED_G, LOW); digitalWrite(Pin_LED_R, LOW); sensorPreviousTime = micros(); previousTime = micros(); } void loop() { while(1){ while(Serial2.available()){ /////GPS/////////////////////////////// char byteGPS1=Serial2.read(); GPS_UBLOX_newFrame(byteGPS1); }//end gps /////////////////////////////////////// Dt_sensor = micros() - sensorPreviousTime;///////////Roop sensor 1 kHz///////// if(Dt_sensor <= 0){Dt_sensor = 1001;} if(Dt_sensor >= 1000 && gyroSamples < 4)////Collect 2 samples = 1000 us { sensorPreviousTime = micros(); mpu6050_readGyroSum(); mpu6050_readAccelSum(); } Dt_roop = micros() - previousTime;// 200 Hz task loop (5 ms) , 2500 us = 400 Hz if(Dt_roop <= 0){Dt_roop = 5001;} if (Dt_roop >= 5000) { previousTime = micros(); G_Dt = Dt_roop*0.000001; frameCounter++; /////get sensor//////////////////////////////////////////////////////////// mpu6050_Get_accel(); mpu6050_Get_gyro(); baro._update(previousTime); //digitalWrite(12, HIGH); //digitalWrite(12, LOW); ////////////////////////////////////////////////////////// if (frameCounter % TASK_100HZ == 0)// 100 Hz tak { /* ///////////////////Filter FourthOrder /////////////////////////////////////// Accel[XAXIS] = AccX; Accel[YAXIS] = AccY; Accel[ZAXIS] = AccZ; for (int axis = XAXIS; axis <= ZAXIS; axis++) { filteredAccel[axis] = computeFourthOrder(Accel[axis], &fourthOrder[axis]);//"ahrs_tin.h" } AccXf = filteredAccel[XAXIS]; AccYf = filteredAccel[YAXIS]; AccZf = filteredAccel[ZAXIS]; */ //presser = baro.getPressure(MS561101BA_OSR_4096); baro.read(); presser = baro.get_pressure(); temperaturetr = baro.get_temperature(); pushAvg(presser, temperaturetr); } ////////////////Moving Average Filters/////////////////////////// GyroXf = (GyroX + GyroX2)/2.0; GyroYf = (GyroY + GyroY2)/2.0; GyroZf = (GyroZ + GyroZ2)/2.0; //AccXf = (AccX + AccX2)/2.0; //AccYf = (AccY + AccY2)/2.0; //AccZf = (AccZ + AccZ2)/2.0; //AccX2 = AccX;AccY2 = AccY;AccZ2 = AccZ;//acc Old1 GyroX2 = GyroX;GyroY2 = GyroY;GyroZ2 = GyroZ;//gyro Old1 ////////////////Low pass filter///////////////////////////////// AccXf = AccXf + (AccX - AccXf)*0.12101;//0.240121 ,0.121 //Low pass filter ,smoothing factor α := dt / (RC + dt) AccYf = AccYf + (AccY - AccYf)*0.12101;//0.240121 ,0.121 AccZf = AccZf + (AccZ - AccZf)*0.12101;//0.240121 ,0.121 โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 24 ////////////////////////////////////////////////////////// ahrs_updateMARG(GyroXf, GyroYf, GyroZf, AccXf, AccYf, AccZf, c_magnetom_x, c_magnetom_y, c_magnetom_z, G_Dt);//quaternion ,direction cosine matrix ,Euler angles //Observer kalman filter////////////////////////////////// Observer_kalman_filter(); //Sliding modeControl///////////////////////////////////// Control_PPIDRate();//"Control_Slid.h" //////Out motor/////////// //armed = 1; motor_Mix();//"motor.h" ///////////////////////// motor_command(); ////////end Out motor////// if (frameCounter % TASK_50HZ == 0)// 50 Hz tak (20 ms) { //mpu6050_Get_accel(); computeRC();//multi_rx.h if (CH_THR < MINCHECK) //////ARM and DISARM your Quadrotor/////////////// { if (CH_RUD > MAXCHECK && armed == 0 && abs(ahrs_p) < 10 && abs(ahrs_r) < 10)//+- 10 deg, ARM { armed = 1; digitalWrite(Pin_LED_R, HIGH); Altitude_Ground = Altitude_baro; setHeading = ahrs_y;// 0 degree ,ahrs_tin.h } if (CH_RUD < MINCHECK && armed == 1) //DISARM { armed = 0; digitalWrite(Pin_LED_R, LOW); Altitude_Ground = Altitude_baro; setHeading = ahrs_y;// 0 degree ,ahrs_tin.h } if (CH_RUD < MINCHECK && armed == 0 && CH_ELE > MAXCHECK) //Mag_Calibrate { Mag_Calibrate();//#include "mpu6050.h" } }//end ARM and DISARM your helicopter/////////////// } if (frameCounter % TASK_20HZ == 0)// 20 Hz task (50 ms) { Mag5883Read(); UltrasonicRead();//"Ultrasonic04.h" //updateOF();//AP_OPTICALFLOW_ADNS3080 //Get_pixy(); //update_positionA3080(GyroXfMO ,GyroYfMO ,1.0 ,0.0 ,z1_hat);//(float Gyroroll, float Gyropitch, float cos_yaw_x, float sin_yaw_y, float altitude) Control_PositionHold();//#include "Control_SlidingPIDsam3x8e.h" } if (frameCounter % TASK_10HZ == 0)//roop TASK_10HZ { Chack_Command(); Automatictakeland(); } if (frameCounter % TASK_5HZ == 0)//GPS_calc TASK_5HZ { presserf = getAvg(movavg_buff, MOVAVG_SIZE); temperaturetrf = getAvg(movavg_buffT, MOVAVG_SIZE); presserfF = presserfF + (presserf - presserfF)*0.3852101;//0.9852101 0.785 0.685 0.545 0.345 Low Pass Filter Altitude_baro = getAltitude(presserfF,temperaturetrf);//Altitude_Ground Altitude_barof = Altitude_baro - Altitude_Ground + Altitude_II; baro_vz = (z1_hat - baro_vz_old)/0.2;//G_Dt Diff Baro 5HZ=0.2 s ,,20 Hz=0.05 baro_vz_old = z1_hat; if(GPS_FIX == 1){ //baro_vz = _vel_down/100.0; } else{ //baro_vz = baro_vz; } โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 25 GPS_LAT1 = GPS_coord[LAT]/10000000.0;// 1e-7 degrees / position as degrees (*10E7) GPS_LON1 = GPS_coord[LON]/10000000.0; Cal_GPS();//#include "Control_PPIDsam3x8e.h" //Control_PositionHold(); //GPS_distance_m_bearing(GPS_LAT1, GPS_LON1, GPS_LAT_HOME, GPS_LON_HOME, Altitude_hat); } if (frameCounter % TASK_2HZ == 0)//LED GPS { if(Status_LED_GPS == LOW && GPS_FIX == 1) { Status_LED_GPS = HIGH; Counter_LED_GPS++; if(Counter_LED_GPS >= GPS_numSat){ Counter_LED_GPS = 0; digitalWrite(Pin_LED_G, HIGH); } else { digitalWrite(Pin_LED_G, LOW); } } else { Status_LED_GPS = LOW; } digitalWrite(Pin_LED_B, Status_LED_GPS); }//end if LED GPS if (frameCounter % TASK_NoHZ == 0)//roop print ,TASK_NoHZ TASK_5HZ TASK_10HZ ,TASK_NoHZ TASK_5HZ TASK_10HZ { Voltage = analogRead(A5); Ampere = analogRead(A6); } if (frameCounter % TASK_NoHZ == 0)//roop print { Serial.print(CH_THRf);Serial.print("\t"); Serial.print(CH_AILf);Serial.print("\t"); Serial.print(CH_ELEf);Serial.print("\t"); Serial.print(CH_RUDf);Serial.print("\t"); Serial.print(AUX_1);Serial.print("\t"); //Serial.print(AUX_2);Serial.print("\t"); //Serial.print(AUX_3);Serial.print("\t"); //Serial.print(AUX_4);Serial.print("\t"); //Serial.print(failsafeCnt);Serial.print("\t"); //Serial.print(setpoint_rate_roll);Serial.print("\t"); //Serial.print(setpoint_rate_pitch);Serial.print("\t"); //Serial.print(MagX1);Serial.print("\t"); Serial.print(MagXf);Serial.print("\t"); //Serial.print(MagY1);Serial.print("\t"); Serial.print(MagYf);Serial.print("\t"); //Serial.print(MagZ1);Serial.print("\t"); Serial.print(MagZf);Serial.print("\t"); //Serial.print(c_magnetom_x);Serial.print("\t"); //Serial.print(c_magnetom_y);Serial.print("\t"); //Serial.print(c_magnetom_z);Serial.print("\t"); //Serial.print(GPS_FIX1);Serial.print("\t"); //Serial.print(GPS_LAT1,9);Serial.print("\t"); //Serial.print(GPS_LAT1f,9);Serial.print("\t"); //Serial.print(GPS_LON1,9);Serial.print("\t"); //Serial.print(GPS_LON1f,9);Serial.print("\t"); //Serial.print(GPS_LON1f2,9);Serial.print("\t"); //Serial.print(error_LAT);Serial.print("\t"); //Serial.print(error_LON);Serial.print("\t"); //Serial.print(GPS_speed);Serial.print("\t");//cm/s //Serial.print(GPS_ground_course);Serial.print("\t");//deg //Serial.print(_velocity_north);Serial.print("\t"); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 26 //Serial.print(actual_speedX);Serial.print("\t"); //Serial.print(actual_speedXf);Serial.print("\t"); //Serial.print(vx_hat);Serial.print("\t"); //Serial.print(_velocity_east);Serial.print("\t"); //Serial.print(_vel_down);Serial.print("\t"); //Serial.print(actual_speedY);Serial.print("\t"); //Serial.print(actual_speedYf);Serial.print("\t"); //Serial.print(vy_hat);Serial.print("\t"); //Serial3.print(GPS_Distance);Serial3.print("\t"); //Serial3.print(GPS_ground_course);Serial3.print("\t"); //Serial3.print(Control_XEf);Serial3.print("\t"); //Serial3.print(Control_YEf);Serial3.print("\t"); //Serial3.print(Control_XBf);Serial3.print("\t"); //Serial3.print(Control_YBf);Serial3.print("\t"); //Serial.print(Control_XEf);Serial.print("\t"); //Serial.print(Control_YEf);Serial.print("\t"); //Serial.print(Control_XBf);Serial.print("\t"); //Serial.print(Control_YBf);Serial.print("\t"); //Serial.print(panError);Serial.print("\t"); //Serial.print(tiltError);Serial.print("\t"); //Serial.print(LError);Serial.print("\t"); //Serial.print(posistion_X);Serial.print("\t"); //Serial.print(posistion_Y);Serial.print("\t"); //Serial.print(surface_quality);Serial.print("\t"); //Serial.print(Distance_L);Serial.print("\t"); //Serial.print(Distance_R);Serial.print("\t"); //Serial.print(Distance_F);Serial.print("\t"); //Serial.print(Distance_B);Serial.print("\t"); //Serial.print(Distance_X);Serial.print("\t"); //Serial.print(Distance_Y);Serial.print("\t"); //Serial.print(Voltage);Serial.print("\t"); //Serial.print(Ampere);Serial.print("\t"); //Serial.print(TempMPU);Serial.print("\t"); //Serial.print(temperaturetr);Serial.print("\t"); //Serial.print(presser,3);Serial.print("\t"); //Serial.print(presserf,3);Serial.print("\t"); //Serial.print(presserfF,3);Serial.print("\t"); //Serial.print(Altitude_baro);Serial.print("\t"); //Serial.print(Altitude_Baro_ult);Serial.print("\t"); Serial.print(Altitude_barof);Serial.print("\t"); Serial.print(Altitude_sonaf);Serial.print("\t"); Serial.print(z1_hat);Serial.print("\t"); //Serial.print(Vz_Baro_ult);Serial.print("\t"); //Serial.print(baro_vz);Serial.print("\t"); //Serial.print(vz_sonaf);Serial.print("\t"); //Serial.print(z2_hat);Serial.print("\t"); //Serial.print(Altitude_sona);Serial.print("\t"); //Serial.print(Altitude_hat);Serial.print("\t"); //Serial.print(vz_sona*10);Serial.print("\t"); //Serial.print(vz_hat*10);Serial.print("\t"); //Serial.print(h_counter);Serial.print("\t"); //Serial.print(GPS_hz);Serial.print("\t"); //Serial.print(vz_hat);Serial.print("\t"); //Serial.print(DCM10);Serial.print("\t"); //Serial.print(DCM11);Serial.print("\t"); //Serial.print(DCM12);Serial.print("\t"); //Serial.print(AccX);Serial.print("\t"); //Serial.print(AccXf);Serial.print("\t"); //Serial.print(AccXf2);Serial.print("\t"); //Serial.print(AccY);Serial.print("\t"); //Serial.print(AccYf);Serial.print("\t"); //Serial.print(AccZ);Serial.print("\t"); //Serial.print(AccZf2,3);Serial.print("\t"); //Serial.print(AccZf3,3);Serial.print("\t"); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 27 //Serial.print(AccZf);Serial.print("\t"); //Serial.print(accrX_Earth);Serial.print("\t"); //Serial.print(accrY_Earth);Serial.print("\t"); //Serial.print(accrZ_Earthf);Serial.print("\t"); //Serial.print(accrZ_Earth);Serial.print("\t"); Serial.print(accelRaw[XAXIS]);Serial.print("\t"); Serial.print(accelRaw[YAXIS]);Serial.print("\t"); Serial.print(accelRaw[ZAXIS]);Serial.print("\t"); //Serial.print(-GyroX*RAD_TO_DEG);Serial.print("\t"); //Serial.print(GyroXf*RAD_TO_DEG);Serial.print("\t"); //Serial.print(roll_D_rate);Serial.print("\t"); //Serial.print(_accel9250X);Serial.print("\t"); //Serial.print(_accel9250Y);Serial.print("\t"); //Serial.print(_accel9250Z);Serial.print("\t"); Serial.print(ahrs_r);Serial.print("\t"); Serial.print(ahrs_p);Serial.print("\t"); Serial.print(ahrs_y);Serial.print("\t"); //Serial3.print(ahrs_y*RAD_TO_DEG);Serial3.print("\t"); //Serial.print(cos_rollcos_pitch);Serial.print("\t"); //Serial.print(x_angle);Serial.print("\t"); //Serial.print(u_roll);Serial.print("\t"); //Serial.print(u_pitch);Serial.print("\t"); //Serial.print(u_yaw);Serial.print("\t"); //Serial.print(motor_FrontL);Serial.print("\t"); //Serial.print(motor_FrontLf);Serial.print("\t"); //Serial.print(motor_FrontR);Serial.print("\t"); //Serial.print(motor_FrontRf);Serial.print("\t"); //Serial.print(motor_BackL);Serial.print("\t"); //Serial.print(motor_BackLf);Serial.print("\t"); //Serial.print(motor_BackR);Serial.print("\t"); //Serial.print(motor_BackRf);Serial.print("\t"); //Serial.print(motor_Left);Serial.print("\t"); //Serial.print(motor_Right);Serial.print("\t"); //Serial.print(GPS_numSat);Serial.print("\t"); Serial.print(Mode);Serial.print("\t"); //Serial.print(gyroSamples2);Serial.print("\t"); //Serial.print(1/G_Dt);Serial.print("\t"); //Serial.print("Hz"); //Serial.print(millis()/1000.0);//millis() micros() Serial.print("\n"); }//end roop 5 Hz if (frameCounter >= TASK_1HZ) { // Reset frameCounter back to 0 after reaching 100 (1s) frameCounter = 0; time_auto++; //temperaturetr = baro.getTemperature(MS561101BA_OSR_4096); //mpu6050_Temp_Values(); Remote_TrimACC();//motor.h if(Status_LED == LOW) { Status_LED = HIGH; //digitalWrite(Pin_LED_G, LOW); } else { Status_LED = LOW; //digitalWrite(Pin_LED_G, HIGH); } digitalWrite(13, Status_LED);//A }//end roop 1 Hz }//end roop 400 Hz }//end while roop } โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 28 2. ไฟล์ Aconfigsam3x8e.h /* project_Quad 32 bit Arduino Due 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza */ //The type of multicopter //#define HEX_X #define Quad_X //#define Quad_P #define CPPM ///////////////Mode/////////////////////////// #define AltHold 1259 #define RTH 1890 #define PositionHold 1443 //Loiter mode #define Auto 1659 #define FailSafe 980 //////////////////////////////////////// #define MINTHROTTLE 1200 //1064 1090 #define MAXTHROTTLE 1950 //1850 #define MINCOMMAND 1000 #define MAXCOMMAND 1950 #define MIDRC 1500 #define MINCHECK 1100 #define MAXCHECK 1900 int Mode = 0; //Mode 0 = Stabilize //Mode 1 = Altitude Hold //Mode 2 = Position Hold ,Loiter //Mode 3 = Automatic Takeoff ,Landing ///////////////////////////////////////////// // Automatic take-off and landing float h_control = 3.5; //2.7 0.6 0.9 meter //Parameter system Quadrotor #define m_quad 1.22 #define L_quad 0.16 //kg //m quad+=0.25 I = (1/3)mL2 = 0.02291 ///////////////////////////////////////////////////////////////////// //P-PID-------------Rate float Kp_rateRoll = 2.12;//2.42 2.78 1.18 5.28 float Ki_rateRoll = 1.12;//1.12 2.75 float Kd_rateRoll = 0.075;//0.095 0.15 0.085 0.025 - 0.045 float Kp_ratePitch = 2.12;//2.42 1.18 5.28 float Ki_ratePitch = 1.12;//1.12 2.75 0.5 - 2.8 float Kd_ratePitch = 0.075;//0.095 0.15 0.078 0.025 - 0.045 float Kp_rateYaw = 2.75;//2.75 3.75 5.75 1.75 - 3.450 float Ki_rateYaw = 1.85;//1.85 3.65 350.0 2.95 float Kd_rateYaw = 0.042;//0.095 0.035 0.065 //PID--------------Stable float Kp_levelRoll= 4.75;//4.95 4.2 6.2 7.8 9.2 //float Ki_levelRoll= 0.00;//0.0 //float Kd_levelRoll= 0.00;//0.0 float Kp_levelPitch= 4.75;//4.95 4.2 6.2 9.2 //float Ki_levelPitch= 0.00; //float Kd_levelPitch= 0.00; float Kp_levelyaw= 5.25;//5.15 4.2 4.5 //stat feedback--------------Altitude float Kp_altitude = 145.2;//155.2 225.2 265.2 175.0 165.0 float Ki_altitude = 1.15;//2.1 12.25 52.13 2.13 0.018 2.5,0.0 float Kd_altitude = 175.2;//185.2 195.2 185.2 250 280.5 315.5 120 float Kd2_altitude = 35.25;//35.25 18.25 22.25 42.5 12.2 1.25 float Ka_altitude = 11.5;//12.5 8.5 18.5 26 32.5 38.5 41.5 35 25 - 45 //////////////////RC////////////////////////////////// //#define tarremote 0.025 // fast //#define tarremote 0.062 //0.092 slow 0.12 0.02 0.08 remote โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 29 #define tar 0.011 //0.012 0.015 float tarremote = 0.095;//0.065 ////////////////////////////////////////////////// //PID GPS//////////////////////////////////////////// float Kp_gps = 0.245;//0.15 0.101 2.101 5.101 float Ki_gps = 0.122;//0.68 2.68 0.25 0.085 0.15 float Kd_gps = 1.85;//2.85 3.35 4.35 1.05 1.9 4.3 0.35 1.35 3.35 float Kp_speed = 0.43;//0.37 0.27 0.15 0.35 0.095 min 0.15 #define Pin_Laser 2 #define Pin_LED_B 4 #define Pin_LED_G 5 #define Pin_LED_R 3 //GPS // สตาร์ท////////////////////////////////////// float GPS_LAT_HOME = 13.868180; float GPS_LON_HOME = 100.496297; // ลงจอด float waypoint1_LAT = 13.868544; float waypoint1_LON = 100.496433; // //float waypoint2_LAT = 13.8670005791; //float waypoint2_LON = 100.483291625;//13.866970, 100.483240 // //float waypoint3_LAT = 13.8670005791; //float waypoint3_LON = 100.483291625; // //float waypoint4_LAT = 13.8670005791; //float waypoint4_LON = 100.483291625; //////////////////////////////////////////////////////////////////// //Accelerometer calibration constants; use the Calibrate example from print(accelRaw[XAXIS]); int A_X_MIN = -4100; // int A_X_MAX = 4110; // int A_Y_MIN = -3980; // int A_Y_MAX = 4230; // int A_Z_MIN = -4270; // int A_Z_MAX = 3970; //4007 //////////////////////////////////////////////////////////////////// //magnetometer calibration constants; use the Calibrate example from print(MagXf); // the Pololu library to find the right values for your board int M_X_MIN = -325; //-490 -654 -693 -688 int M_X_MAX = 426; //310 185 209 170 int M_Y_MIN = -380; //-369 -319 -311 -310 int M_Y_MAX = 380; //397 513 563 546 int M_Z_MIN = -370; //-392 -363 -374 -377 int M_Z_MAX = 350; //346 386 429 502 //////////////////////////////////////////////////////////////////// //Observer hz float Altitude_Hold = 0.0; //float Altitude_hat=0.0;//Observer hx float vx_hat=0.0; float vx_hat2=0.0; float vy_hat=0.0; float vy_hat2=0.0; //float vz_hat=0.0; //float vz_hat2=0.0; //float h=0.0; float seth=0.0;//set control float uthrottle=0.0; float uAltitude = 1000.0; float accrX_Earth = 0.0; float accrY_Earth = 0.0; float accrZ_Earth = 0.0; float accrX_Earthf = 0.0; float accrY_Earthf = 0.0; float accrZ_Earthf = 0.0; //float vz = 0.0; //kalman float z1_hat = 0.0; float z2_hat = 0.0; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 30 float z1_hat2 = 0.0; float z2_hat2 = 0.0; float u_z = 0.0; float baro_vz = 0.0; float baro_vz_old = 0.0; float baro_vz_old2 = 0.0; //GPS float GPS_LAT1 = 0.0; float GPS_LON1 = 0.0; float GPS_LAT1f = 0.0; float GPS_LON1f = 0.0; float GPS_LAT1Lf = 0.0; float GPS_LON1Lf = 0.0; float GPS_LAT1lead = 0.0; float GPS_LON1lead = 0.0; float GPS_speed = 0.0; float actual_speedX = 0.0; float actual_speedY = 0.0; float actual_speedXf = 0.0; float actual_speedYf = 0.0; float actual_speedXold = 0.0; float actual_speedYold = 0.0; float _last_velocityX = 0.0; float _last_velocityY = 0.0; float GPS_LAT1_old = GPS_LAT_HOME; float GPS_LON1_old = GPS_LON_HOME; float Control_XEf = 0.0; float Control_YEf = 0.0; float Control_XBf = 0.0; float Control_YBf = 0.0; float target_LAT = 0.0; float target_LON = 0.0; byte currentCommand[23]; byte Read_command = 0; float GPS_hz = 0.0; float GPS_vz = 0.0; float GPS_ground_course2 = 0.0; float GPS_Distance = 0.0; float error_LAT = 0.0; float error_LON = 0.0; float GPSI_LAT = 0.0; float GPSI_LON = 0.0; uint8_t GPS_filter_index = 0; float GPS_SUM_LAT[5]; float GPS_SUM_LON[5]; bool Status_LED_GPS = LOW; uint8_t Status_waypoint = 0; uint8_t Counter_LED_GPS = 0; int Voltage = 0; int Ampere = 0; ////////time roop//////////////////////////////////////////// #define TASK_100HZ 2 #define TASK_50HZ 4 #define TASK_20HZ 10 #define TASK_10HZ 20 #define TASK_5HZ 40 #define TASK_2HZ 100 #define TASK_1HZ 200 #define TASK_NoHZ 410 //#define RAD_TO_DEG 57.295779513082320876798154814105 //#define DEG_TO_RAD 0.017453292519943295769236907684886 //direction cosine matrix (DCM) Rotated Frame to Stationary Frame ZYX float DCM00 = 1.0; float DCM01 = 0.0; float DCM02 = 0.0; float DCM10 = 0.0; float DCM11 = 1.0; float DCM12 = 0.0; float DCM20 = 0.0; float DCM21 = 0.0; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 31 float DCM22 = 1.0; //float DCM23 = 1.0; float cos_rollcos_pitch = 1.0; // Main loop variables unsigned long currentTime = 0; unsigned long previousTime = 0; unsigned long sensorPreviousTime = 0; uint16_t frameCounter = 0; uint8_t timeLanding = 0; uint8_t timeOff = 0; byte armed = 0; float G_Dt = 0.01; long Dt_sensor = 1000; long Dt_roop = 10000; bool Status_LED = LOW; int ESC_calibra = 0; int Counter_LED_Auto = 0; //Baro //MS561101BA32bit baro = MS561101BA32bit(); AP_Baro_MS5611 baro; #define MOVAVG_SIZE 10 //100 80 20 float movavg_buff[MOVAVG_SIZE]; float movavg_buffT[MOVAVG_SIZE]; int movavg_i=0; float sea_press=1013.25; float temperaturetr = 32.5; float temperaturetrf = 32.5; float presser=1013.25; float presserf=1013.25; float presserfF=1013.25; float Altitude_baro = 0.0; float Altitude_barof=0.0; float Altitude_Ground = 0.0; 3. ไฟล์ AnalogSource_Arduino.h //project_Quad 32 bit Arduino Due //1. stabilized quadrotor //by: tinnakon kheowree //0860540582 //tinnakon_za@hotmail.com //tinnakonza@gmail.com //https://www.facebook.com/tinnakonza int AD0_V = 0; int AD1_L = 0; int AD2_R = 0; int AD3_F = 0; int AD4_B = 0; int AD0_SUM = 0; int AD1_SUM = 0; int AD2_SUM = 0; int AD3_SUM = 0; int AD4_SUM = 0; int Distance_L = 0; int Distance_R = 0; int Distance_F = 0; int Distance_B = 0; float V_Bat = 0; int Distance_X = 0; int Distance_Y = 0; uint8_t ADCSamples; // increase this if we need more than 5 analog sources //#define MAX_PIN_SOURCES 5 void Distance_Measuring_Sensor(){ AD0_V = analogRead(A0); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 32 AD1_L = analogRead(A1); AD2_R = analogRead(A2); AD3_F = analogRead(A3); AD4_B = analogRead(A4); } void analogReadSUM(){ Distance_Measuring_Sensor(); AD0_SUM += AD0_V; AD1_SUM += AD1_L; AD2_SUM += AD2_R; AD3_SUM += AD3_F; AD4_SUM += AD4_B; ADCSamples++; } void Distance_Measuring_Get() { // Calculate average V_Bat = (AD0_SUM / ADCSamples)*0.01409;//867 = 0.01409 ,889 = 0.01374; Distance_L = (AD1_SUM / ADCSamples); Distance_R = (AD2_SUM / ADCSamples); Distance_F = (AD3_SUM / ADCSamples); Distance_B = (AD4_SUM / ADCSamples); applyDeadband(Distance_L, 160);//190 = 1 m applyDeadband(Distance_R, 160); applyDeadband(Distance_F, 160); applyDeadband(Distance_B, 160); Distance_X = Distance_L - Distance_R; Distance_Y = Distance_B - Distance_F; // Reset SUM variables AD0_SUM = 0; AD1_SUM = 0; AD2_SUM = 0; AD3_SUM = 0; AD4_SUM = 0; ADCSamples = 0; } 4. ไฟล์ Control_PPIDsam3x8e.h /* project_Quad 32 bit Arduino Due 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza */ float u_roll = 0; float u_pitch = 0; float u_yaw = 0; float roll_I_rate = 0.0; float roll_D_rate = 0.0; float setpoint_rollold = 0.0; float setpoint_rate_roll = 0.0; float error_rollold = 0.0; float error_rate_rollold = 0.0; float pitch_I_rate = 0.0; float pitch_D_rate = 0.0; float setpoint_pitchold = 0.0; float setpoint_rate_pitch = 0.0; float error_pitchold = 0.0; float error_rate_pitchold = 0.0; float yaw_I_rate = 0.0; float yaw_D_rate = 0.0; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 33 float error_rate_yawold = 0.0; //Automatic take-off and landing float err_hz = 0.0; int time_auto = 0; float h_counter = 0.1;//0.08 float h_counter_old = 0.1; float Vz_Hold = 0.0; float hz_I = 0.0; float hz_D_rate = 0.0; float error_Vz_old = 0.0; uint8_t takeoff = 0; uint8_t endAuto = 0; void Control_PPIDRate(){ // ROLL CONTROL P-PID control By tinnakon/////////// float setpoint_roll = ((CH_AILf-CH_AIL_Cal)*0.085) + Control_YBf;//0.12 max +-45 deg ////+-18 + Control_YBf applyDeadband(setpoint_roll, 2.5);//1.2 setpoint_rate_roll = (0.065*setpoint_rate_roll/(0.065+G_Dt)) + ((setpoint_rollsetpoint_rollold)/(0.065+G_Dt));//Diff remote setpoint_rollold = setpoint_roll; setpoint_rate_roll = constrain(setpoint_rate_roll, -80, 80);//+-80 deg/s float error_roll = setpoint_roll - ahrs_r;//ahrs_r*ToDeg float error_rate_roll = setpoint_rate_roll + error_roll*Kp_levelRoll - GyroXf*RAD_TO_DEG; roll_I_rate += error_rate_roll*Ki_rateRoll*G_Dt; roll_I_rate = constrain(roll_I_rate, -5, 5);//+-150 roll_D_rate = (tar*roll_D_rate/(tar+G_Dt)) + ((error_rate_roll-error_rate_rollold)/(tar+G_Dt)); error_rate_rollold = error_rate_roll; u_roll = Kp_rateRoll*error_rate_roll + roll_I_rate + Kd_rateRoll*roll_D_rate; u_roll = constrain(u_roll, -280, 280);//+-250 +-300 120 // PITCH CONTROL P-PID control By tinnakon/////////// float setpoint_pitch = ((CH_ELEf-CH_ELE_Cal)*-0.085) + Control_XBf;//max +-45 deg ////+-18 - Control_XBf applyDeadband(setpoint_pitch, 2.5);//1.2 setpoint_rate_pitch = (0.065*setpoint_rate_pitch/(0.065+G_Dt)) + ((setpoint_pitchsetpoint_pitchold)/(0.065+G_Dt));//Diff remote setpoint_pitchold = setpoint_pitch; setpoint_rate_pitch = constrain(setpoint_rate_pitch, -80, 80);//+-80 float error_pitch = setpoint_pitch - ahrs_p;//ahrs_p*RAD_TO_DEG float error_rate_pitch = setpoint_rate_pitch + error_pitch*Kp_levelPitch - GyroYf*RAD_TO_DEG; pitch_I_rate += error_rate_pitch*Ki_ratePitch*G_Dt; pitch_I_rate = constrain(pitch_I_rate, -5, 5);//+-150 pitch_D_rate = (tar*pitch_D_rate/(tar+G_Dt)) + ((error_rate_pitch-error_rate_pitchold)/(tar+G_Dt)); error_rate_pitchold = error_rate_pitch; u_pitch = Kp_ratePitch*error_rate_pitch + pitch_I_rate + Kd_ratePitch*pitch_D_rate; u_pitch = constrain(u_pitch, -280, 280);//+-250 +-300 120 // YAW CONTROL P-PID control By tinnakon/////////// float setpoint_rate_yaw = (CH_RUDf-CH_RUD_Cal)*0.55;//0.4 0.35 applyDeadband(setpoint_rate_yaw, 8.5);//6.5 if(abs(setpoint_rate_yaw) > 0.1){ setHeading = ahrs_y;// 0 degree ,ahrs_tin.h } float error_yaw = 0.0 - Heading; float error_rate_yaw = setpoint_rate_yaw + error_yaw*Kp_levelyaw - GyroZf*RAD_TO_DEG; yaw_I_rate += error_rate_yaw*Ki_rateYaw*G_Dt; yaw_I_rate = constrain(yaw_I_rate, -50, 50);//+-100 yaw_D_rate = (tar*yaw_D_rate/(tar+G_Dt)) + ((error_rate_yaw-error_rate_yawold)/(tar+G_Dt)); error_rate_yawold = error_rate_yaw; u_yaw = Kp_rateYaw*error_rate_yaw + yaw_I_rate + Kd_rateYaw*yaw_D_rate; u_yaw = constrain(u_yaw, -170, 170);//+-170 +-150 ////Altitude////////////////////////////////////////////////////////////////////////////////////////////////////// if(Mode == 1 || Mode == 2)//Altitude Hold, { err_hz = Altitude_Hold - z1_hat; err_hz = constrain(err_hz, -1.0, 1.0);//+-2 m //applyDeadband(err_hz, 0.11);//nois 0.2 m float error_Vz = 0.0 - z2_hat; hz_D_rate = (tar*hz_D_rate/(tar+G_Dt)) + ((error_Vz - error_Vz_old)/(tar+G_Dt));// D Controller error_Vz_old = error_Vz; float error_hII = err_hz*1.25 + error_Vz;//0.55 hz_I = hz_I + (Ki_altitude*error_hII*G_Dt);//1.9 - 2.9 18.5 22.5 hz_I = constrain(hz_I, -20, 20);//+-200 float uthrottle2 = (err_hz*Kp_altitude) + hz_I + (error_Vz*Kd_altitude) + (hz_D_rate*Kd2_altitude) - โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 34 (accrZ_Earth*Ka_altitude);//state feedback //uthrottle = uthrottle2*m_quad/(cos_roll*cos_pitch);// = u*m*d/b*(cosr*cosp) ,m*d/b = 122.33 (cos_roll*cos_pitch) uthrottle = uthrottle2*m_quad;// /cos_rollcos_pitch } else if(Mode == 3)//Automatic Takeoff, Landing { err_hz = h_counter - z1_hat; err_hz = constrain(err_hz, -1.0, 1.0);//+-2 m float error_Vz = Vz_Hold - z2_hat;//Vz_Hold hz_D_rate = (tar*hz_D_rate/(tar+G_Dt)) + ((error_Vz - error_Vz_old)/(tar+G_Dt));// D Controller error_Vz_old = error_Vz; float error_hII = err_hz*1.25 + error_Vz;//0.55 hz_I = hz_I + (Ki_altitude*error_hII*G_Dt);//1.9 - 2.9 18.5 22.5 hz_I = constrain(hz_I, -100, 100);//+-200 float uthrottle2 = (err_hz*Kp_altitude) + hz_I + (error_Vz*Kd_altitude) + (hz_D_rate*Kd2_altitude) (accrZ_Earth*Ka_altitude);//state feedback //uthrottle = uthrottle2*m_quad/(cos_roll*cos_pitch);// = u*m*d/b*(cosr*cosp) (cos_roll*cos_pitch) uthrottle = uthrottle2*m_quad;// ,m*d/b = 122.33 /cos_rollcos_pitch } else { hz_I = 0.0; uthrottle = 0.0; Altitude_Hold = z1_hat; }//end Altitude Hold uthrottle = constrain(uthrottle, -200, 200);//+-120 +-150 uAltitude = CH_THRf + uthrottle;//m*g = 10.8 N = uAltitude = constrain(uAltitude, MINCOMMAND, MAXTHROTTLE); } ///////////////////////////// void LED_Auto(){ Counter_LED_Auto++; if(Counter_LED_Auto > 5){ digitalWrite(Pin_LED_R, HIGH); } if(Counter_LED_Auto > 7){ digitalWrite(Pin_LED_R, LOW); Counter_LED_Auto = 0; } } /////////////////////// void Chack_Command(){ if(AUX_1 <= (AltHold-10))//Stabilize { Mode = 0; } if(AUX_1 > (AltHold-10) && AUX_1 <= (AltHold+10))//Altitude Hold, { Mode = 1; } if(AUX_1 > (PositionHold-10) && AUX_1 <= (PositionHold+10))//Position Hold { Mode = 2; } if(AUX_1 > (Auto-10))//Automatic Takeoff { Mode = 3; LED_Auto(); } if(AUX_1 > (RTH-10) && AUX_1 <= (RTH+10))//RTH { Mode = 2; target_LAT = GPS_LAT_HOME;//GPS_LAT_Hold target_LON = GPS_LON_HOME;//GPS_LON_Hold } ////////////////// if(AUX_2 <= 1300)//Set Home { โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 35 GPS_LAT_HOME = GPS_LAT1f; GPS_LON_HOME = GPS_LON1f; digitalWrite(Pin_LED_G, HIGH); digitalWrite(Pin_LED_R, LOW); } if(AUX_2 >= 1700)//Set waypoint1 { waypoint1_LAT = GPS_LAT1f; waypoint1_LON = GPS_LON1f; digitalWrite(Pin_LED_R, HIGH); digitalWrite(Pin_LED_G, LOW); } }//end Chack_Command() /////////////////////////////////////////////////////////////////////////////////// void Automatictakeland(){ //Altitude control and 1 waypoint navigation if(Mode == 3 && CH_THRf > MINCHECK && armed == 1) { if(time_auto < 2){//Check time < 5 takeoff = 1; } if(z1_hat >= h_control && endAuto == 1)//waypoint1 { target_LAT = waypoint1_LAT; target_LON = waypoint1_LON; Status_waypoint = 1; } if(time_auto > 8 && abs(error_LAT) <= 70 && abs(error_LON) <= 70 && endAuto == 1 && Status_waypoint == 1)//50 10 Landing and position hold mode { timeLanding++; if(timeLanding >= 30)//relay 3 s Landing { takeoff = 0; } } } else//(Mode == 3 && CH_THR > MINCHECK && armed == 1) { takeoff = 0; timeLanding = 0; timeOff = 0; time_auto = 0; h_counter = 0.1;//0.0 h_counter_old = 0.1; Vz_Hold = 0.0; Status_waypoint = 0; } ////////////////////////////////////////////////////////////////// if(h_counter < h_control && takeoff == 1)//take-off { endAuto = 1; h_counter = h_counter + 0.042;//0.023 0.01- 0.03 0.1 = 10 s //ramp input hz take-off Vz_Hold = (h_counter - h_counter_old)/0.1;//(m/s) roop 10 Hz h_counter_old = h_counter; } if(takeoff == 0 && endAuto == 1)//landing { h_counter = h_counter - 0.029;//0.023 ramp input hz landing Vz_Hold = (h_counter - h_counter_old)/0.1;//(m/s) roop 10 Hz h_counter_old = h_counter; if(z1_hat <= 0.18) { endAuto = 0; } } //////////////////////////////////// if(time_auto > 25 && endAuto == 0) //15 s End waypoint quadrotor { timeOff++; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 36 if(timeOff > 60)//relay 6 s time-Off { armed = 0; } } /////////////////////////////////////////////////////////// } /////////////////////////////////////////////////////////////////////////////// void Control_PositionHold(){ if(Mode == 2 || Mode == 3 && GPS_FIX == 1){ error_LAT = (target_LAT - GPS_LAT1f)*6371000.0; // X Error cm error_LON = (target_LON - GPS_LON1f)*6371000.0;// Y Error cm error_LAT = constrain(error_LAT,-500,500);//200 = +-2 m error_LON = constrain(error_LON,-500,500); float target_speedLAT = error_LAT*Kp_speed;//P Control Velocity GPS float target_speedLON = error_LON*Kp_speed;//P Control Velocity GPS target_speedLAT = constrain(target_speedLAT,-200,200);//+-100 cm/s = 1m/s target_speedLON = constrain(target_speedLON,-200,200); float error_rate_LAT = target_speedLAT - vx_hat; float error_rate_LON = target_speedLON - vy_hat; error_rate_LAT = constrain(error_rate_LAT,-300,300);//+-200 cm/s error_rate_LON = constrain(error_rate_LON,-300,300); GPSI_LAT = GPSI_LAT + (error_rate_LAT*Ki_gps*0.05);//5Hz=0.2 ,, 20 Hz = 0.05 GPSI_LON = GPSI_LON + (error_rate_LON*Ki_gps*0.05); GPSI_LAT = constrain(GPSI_LAT,-200,200);//win speed +-200 cm/s GPSI_LON = constrain(GPSI_LON,-200,200);//250 //Control_XEf = error_rate_LAT*Kd_gps;//P Control speed //Control_YEf = error_rate_LON*Kd_gps; Control_XEf = error_LAT*Kp_gps + error_rate_LAT*Kd_gps + GPSI_LAT;//PID Control speed Control_YEf = error_LON*Kp_gps + error_rate_LON*Kd_gps + GPSI_LON; Control_XEf = constrain(Control_XEf,-800,800);//PWM 1000 - 1900 Control_YEf = constrain(Control_YEf,-800,800); //The desired roll and pitch angles by tinnakon float urolldesir = (Control_YEf*m_quad)/uAltitude;//uAltitude/2 = 1000 - 1900 float upitchdesir = (Control_XEf*m_quad*-1.0)/uAltitude;//*-1 urolldesir = constrain(urolldesir,-0.7,0.7);//+-0.7 = +-44.427 upitchdesir = constrain(upitchdesir,-0.7,0.7); float temp_YBf = asin(urolldesir)*RAD_TO_DEG;//Control Earth Frame float temp_XBf = asin(upitchdesir)*RAD_TO_DEG;//Control Earth Frame Control_XBf = (DCM00*temp_XBf) + (DCM01*temp_YBf);//Control Body Frame use Rotation matrix Control_YBf = (DCM10*temp_XBf) + (DCM11*temp_YBf);//Control Body Frame use Rotation matrix Control_XBf = constrain(Control_XBf, -20, 20);//+-20 deg Control_YBf = constrain(Control_YBf, -20, 20);//+-20 deg //The desired roll and pitch angles by paper Modeling and Backstepping-based Nonlinear Control //Ashfaq Ahman Mian 2008 (eq.25 and eq.26) //float urolldesir = ((Control_XEf*m_quad*sin(ahrs_y))/uAltitude) ((Control_YEf*m_quad*cos_yaw)/uAltitude); //float upitchdesir = ((Control_XEf*m_quad)/(uAltitude*cos_roll*cos_yaw)) ((sin(ahrs_r)*sin(ahrs_y))/(cos_roll*cos_yaw)); //urolldesir = constrain(urolldesir,-0.7,0.7);//+-0.7 = +-44.427 //upitchdesir = constrain(upitchdesir,-0.7,0.7); //Control_YBf = asin(urolldesir)*RAD_TO_DEG;//Control roll eq.25 //Control_XBf = asin(upitchdesir)*RAD_TO_DEG*-1.0;//Control roll eq.26 //Control_XBf = constrain(Control_XBf, -20, 20);//+-20 +- 44 //Control_YBf = constrain(Control_YBf, -20, 20); } else{ Control_XBf = 0.0; Control_YBf = 0.0; GPSI_LAT = 0.0; GPSI_LON = 0.0; target_LAT = GPS_LAT1f;//GPS_LAT_Hold target_LON = GPS_LON1f;//GPS_LON_Hold } }//end Control_PositionHold() ///////////////////////////////////////////////////////////////////////// void Cal_GPS(){ if(GPS_FIX == 1){ //Apply moving average filter to GPS data โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 37 GPS_filter_index = (GPS_filter_index+1) % 4;// 4 moving average GPS_SUM_LAT[GPS_filter_index] = GPS_LAT1; GPS_SUM_LON[GPS_filter_index] = GPS_LON1; float sum1=0.0; float sum2=0.0; for(int i=0;i<4;i++) { sum1 += GPS_SUM_LAT[i]; sum2 += GPS_SUM_LON[i]; } GPS_LAT1f = sum1/4.0; GPS_LON1f = sum2/4.0; } /* /////////////////////////////////////// //Diff speed actual_speedX = (GPS_LAT1f - GPS_LAT1_old)*6371000.0/0.3;//5 Hz = 0.2 ,cm/s 10000000.0 ,R = 6371000.0 actual_speedY = (GPS_LON1f - GPS_LON1_old)*637100.0/0.3;//cm/s //actual_speedX = constrain(actual_speedX, -400, 400);//+-400 cm/s //actual_speedY = constrain(actual_speedY, -400, 400);//+-400 cm/s GPS_LAT1_old = GPS_LAT1f; GPS_LON1_old = GPS_LON1f; actual_speedXf = (actual_speedX + actual_speedXold)/2.0;//Moving Average Filters/ actual_speedYf = (actual_speedY + actual_speedYold)/2.0;//Moving Average Filters/ actual_speedXold = actual_speedX; actual_speedYold = actual_speedY; /////////////LeadFilter GPS///////////////////////////////// float lag_in_seconds = 0.85;//1.0 0.5 float accel_contribution = (actual_speedXf - _last_velocityX) * lag_in_seconds * lag_in_seconds; float vel_contribution = actual_speedXf * lag_in_seconds; _last_velocityX = actual_speedXf; GPS_LAT1lead = GPS_LAT1f // store velocity for next iteration + vel_contribution + accel_contribution; float accel_contributio = (actual_speedYf - _last_velocityY) * lag_in_seconds * lag_in_seconds; float vel_contributio = actual_speedYf * lag_in_seconds; _last_velocityY = actual_speedYf; GPS_LON1lead = GPS_LON1f // store velocity for next iteration + vel_contributio + accel_contributio; */ } 5. ไฟล์ Kalman_ObserverTin.h /* project_Quad 32 bit Arduino Due //GPS 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza //A=[1 0.01;0 1]; //B=[0;0.01]; //C = [1 0]; //Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y'); //Q = 0.1; % A number greater than zero //R = 0.135; % A number greater than zero //[kalmf,L,P,M,Z] = kalman(Plant,Q,R); //%kalmf = kalmf(1,:); //M, % innovation gain */ void Observer_kalman_filter() { ///Complimentary Filter, Observer velocity vx vy vz kalman//accelerometer and GPS///////////////////////////////////////////////////// //GPS Speed Low Pass Filter //GPS_LAT1Lf = GPS_LAT1Lf + (GPS_LAT1lead - GPS_LAT1Lf)*8.2521*G_Dt;//12.5412 //GPS_LON1Lf = GPS_LON1Lf + (GPS_LON1lead - GPS_LON1Lf)*8.2521*G_Dt; //actual_speedXf = actual_speedXf + (actual_speedX - actual_speedXf)*10.2*G_Dt;//5.2 10.4 //cm/s +-400 cm/s //actual_speedYf = actual_speedYf + (actual_speedY - actual_speedYf)*10.2*G_Dt;//5.2 10.17 float temp_vx = accrX_Earth*100.1 + (_velocity_north - vx_hat2)*9.2;//2.15 1.5 6.5 vx_hat = vx_hat + โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 38 temp_vx*G_Dt; vx_hat2 = vx_hat2 + temp_vx*G_Dt; vx_hat = constrain(vx_hat2, -400, 400);//+-400 cm/s applyDeadband(vx_hat, 4.5);//4.5 10.5 float temp_vy = accrY_Earth*100.1 + (_velocity_east - vy_hat2)*9.2;//2.15 1.5 6.5 vy_hat2 = vy_hat2 + temp_vy*G_Dt; vy_hat = constrain(vy_hat2, -400, 400); applyDeadband(vy_hat, 4.5);//4.5 10.5 //Predicted (a priori) state estimate Altitude //u_z = (motor_FrontLf + motor_FrontRf + motor_BackLf + motor_BackRf);//uz - g ,,unit N *0.001691) - 9.81 u_z = accrZ_Earth;//applyDeadband = 0.1 m/s^2 = ,,(u - c*z2_hat)/m = Accz z2_hat2 = z2_hat + u_z*G_Dt;//z2_hat = velocity ,, m/s z1_hat2 = z1_hat + z2_hat2*G_Dt;//z1_hat = Altitude ,, m //z1_hat = constrain(z1_hat, 0, 100);//0 - 100 m /////////////////////////////////////////////////////////////////// //Updated (a posteriori) state estimate //Update estimate with measurement zk z2_hat = z2_hat2 + 0.012502*(Altitude_Baro_ult - z1_hat2) + 0.01641023*(Vz_Baro_ult - z2_hat2);//K2 = 0.0002141023, 0.0001641, 0.00145, 0.00045,,,k2 0.097502, 0.065502, 0.009 z1_hat = z1_hat2 + 0.015102*(Altitude_Baro_ult - z1_hat2) + 0.01741023*(Vz_Baro_ult - z2_hat2);//K1 =0.035102, 0.015102 0.065 0.2887 0.09187 0.01187 0.0140 } 6. ไฟล์ Ultrasonic04.h /* project_Quad 32 bit Arduino Due //GPS 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza #include "Ultrasonic.h" Ultrasonic LV-EZ4 AnalogRead every 50mS, (20-Hz rate) from 6-inches out to 254-inches from 0.15-m out to 6.45 m 5V yields ~9.8mV/in. , 1 in = 0.0254 m =~9.8mV/0.0254 m = 0.38582 v/m Ultrasonic_HC-SR04 from 0.02 -m out to 1.4 m //Just connect VCC to (+) on D9, trig to D9, echo to D10, Gnd to (-) */ #define HCSR04_TriggerPin 27 // should be modified to 9 #define HCSR04_EchoPin 26 12 in next version // should be modified to 10 11 in next version float hz_Ultra = 0.0; float Altitude_sona = 0.0; float Altitude_sona2 = 0.0; float Altitude_sonaf = 0.0; float Altitude_sonaold = 0.0; float vz_sona = 0.0; float vz_sona2 = 0.0; float vz_sonaf = 0.0; float Altitude_II = 0.0; float Altitude_Baro_ult = 0.0; float Vz_Baro_ult = 0.0; unsigned long HCSR04_startTime = 0; unsigned long HCSR04_echoTime = 0; int tempSonarAlt=0; // EchoPin will change to high signalize beginning // and back to low after 58*cm us // First interrupt is needed to start measurement, second interrupt to calculate the distance void UltaHandler() { โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 39 // Here is a routine missing, to check, if the interrupt was raised for echo pin - not needed at the moment, because we don't have any interrupts // for this interrupt group, but maybe later uint8_t sensorVal = digitalRead(HCSR04_EchoPin); if (sensorVal == 1) { //indicates if the EchoPin is at a high state HCSR04_startTime = micros(); } else { HCSR04_echoTime = micros() - HCSR04_startTime; if (HCSR04_echoTime <= 25000) // maximum = 4,31 meter - 30000 us means out of range tempSonarAlt = HCSR04_echoTime / 5.8;//to mm else tempSonarAlt = 9999; } } void UltrasonicInt() { Serial.print("Altitude_sona");Serial.print("\n"); pinMode(HCSR04_EchoPin,INPUT); pinMode(HCSR04_TriggerPin,OUTPUT); attachInterrupt(HCSR04_EchoPin, UltaHandler, CHANGE); //RISING FALLING CHANGE //PCICR |= (1<<PCIE0);// enable PCINT0 // PCINT 0-7 belong to PCIE0 //HCSR04_EchoPin_PCICR //PCMSK0 = (1<<PCINT4);//PB4 ( OC2A/PCINT4 )= Digital pin 10 (PWM) // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts! } void UltrasonicRead() { //Ultrasonic LV-EZ4 //int sensorValue = analogRead(A0); //hz_Ultra = (sensorValue*5/1024.0)/0.38528;//5V yields ~9.8mV/in. , 1 in = 0.0254 m =~9.8mV/0.0254 m = 0.38582 v/m //hz_Ultra = constrain(hz_Ultra, 0, 6.45);//m //Ultrasonic_HC-SR04 // create a trigger pulse for 10 us //digitalWrite(HCSR04_TriggerPin, LOW); //delayMicroseconds(2); digitalWrite(HCSR04_TriggerPin, HIGH); delayMicroseconds(10); digitalWrite(HCSR04_TriggerPin, LOW); Altitude_sona = tempSonarAlt/1000.0;//m //Altitude_sonaf = (Altitude_sona + Altitude_sona2)/2.0;//filter Altitude_sonaf = Altitude_sonaf + (((Altitude_sona + Altitude_sona2)/2.0) - Altitude_sonaf)*0.622012;//*0.687 filter 42 Hz Altitude_sona2 = Altitude_sona; vz_sona = (Altitude_sonaf - Altitude_sonaold)/0.05;//diff 20 Hz = 0.05 s Altitude_sonaold = Altitude_sonaf; //vz_sonaf = (vz_sona + vz_sona2)/2.0;//filter //vz_sonaf = vz_sonaf + (((vz_sona + vz_sona2)/2.0) - vz_sonaf)*0.62;//filter 42 Hz //vz_sona2 = vz_sona; vz_sonaf = constrain(vz_sona, -3, 3); //Altitude_sonano = constrain(Altitude_sonano, 0, 3.0);//m //Ultrasonic max 0.7 m change to Baro////////////////////////////// if(z1_hat > 0.8){//Altitude_sonaf Altitude_Baro_ult = Altitude_barof; Vz_Baro_ult = baro_vz; } else{ float error_Altitude = Altitude_sonaf - Altitude_barof; Altitude_II = Altitude_II + (error_Altitude*0.019501);//0.0185 0.005 ,,20 Hz = 0.05 Altitude_II = constrain(Altitude_II, -100, 100); Altitude_Baro_ult = Altitude_sonaf; Vz_Baro_ult = vz_sonaf; } } /////////////////////////////////////////////////////////////////////////////////////////////// float getAltitude(float pressure2, float temperature2) { //return (1.0f - pow(pressure2/sea_press, 0.190295f)) * 44330.0f; //return log(sea_press/pressure2) * (temperature2+273.15f) * 29.271267f; // in meter 1007.23 1008.83 return ((pow((sea_press/pressure2),1/5.257)-1.0)*(temperature2+273.15))/0.0065; } โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 40 void pushAvg(float val, float val2) { movavg_buff[movavg_i] = val; movavg_buffT[movavg_i] = val2; movavg_i = (movavg_i + 1) % MOVAVG_SIZE; } float getAvg(float * buff, int size) { float sum=0.0; for(int i=0;i<size;i++) { sum += buff[i]; } return sum/size; } 7. ไฟล์ ahrs_tinsam3x8e.h /* //An attitude and heading reference system (AHRS) By tinnakon 26_05_2557(2014) project_Quad 32 bit Arduino Due 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza */ #define Kp 0.35 //0.22 0.15 //0.2 1.62 // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.0005011 //0.122 0.072 0.096 0.05 // integral gain governs rate of convergence of gyroscope biases #define Kp_mag 0.25 //0.22 #define Ki_mag 0.0005101 //0.142 float exInt , eyInt , ezInt ; // scaled integral error float q0, q1 , q2 , q3 ; // quaternion elements representing the estimated orientation float ahrs_p,ahrs_r,ahrs_y; float Heading = 0.0; float setHeading = 0.0; float previousEx = 0.0; float previousEy = 0.0; float previousEz = 0.0; void ahrs_initialize() { exInt=0.0; eyInt=0.0; ezInt=0.0; q0=1.0; q1=0.0; q2=0.0; q3=0.0; } boolean isSwitched(float previousError, float currentError) { if ((previousError > 0.0 && currentError < 0.0) || (previousError < 0.0 && currentError > 0.0)) { return true; } return false; } void ahrs_updateMARG(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float G_Dt){ float norm1,halfT; float hx, hy, hz, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez, ezMag; halfT=G_Dt/2.0; norm1 = sqrt(ax*ax + ay*ay + az*az); // normalise the measurements ax = ax / norm1; ay = ay / norm1; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 41 az = az / norm1; norm1 = sqrt(mx*mx + my*my + mz*mz); mx = mx / norm1; my = my / norm1; mz = mz / norm1; hx = mx*DCM00 + my*DCM01 + mz*DCM02; // compute reference direction of flux, Earth Frame hy = mx*DCM10 + my*DCM11 + mz*DCM12; hz = mx*DCM20 + my*DCM21 + mz*DCM22; bx = sqrt((hx*hx) + (hy*hy)); bz = hz; vx = DCM20;//2*(q1q3 - q0q2) // estimated direction of gravity and flux (v and w) vy = DCM21;//2*(q0q1 + q2q3) vz = DCM22;//q0q0 - q1q1 - q2q2 + q3q3 wx = bx*DCM00 + bz*DCM20; wy = bx*DCM01 + bz*DCM21; wz = bx*DCM02 + bz*DCM22; ex = (ay*vz - az*vy); // error is sum of cross product between reference direction of fields and direction measured by sensors ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); //ez = (ax*vy - ay*vx);// + (mx*wy - my*wx) ezMag = (mx*wy - my*wx); // integral error scaled integral gain //exInt += ex*Ki*G_Dt; //eyInt += ey*Ki*G_Dt; //ezInt += (ez*Ki + ezMag*Ki_mag)*G_Dt; //* exInt += ex*Ki; if (isSwitched(previousEx,ex)) { exInt = 0.0; } previousEx = ex; eyInt += ey*Ki; if (isSwitched(previousEy,ey)) { eyInt = 0.0; } previousEy = ey; float ezi = (ez*Ki + ezMag*Ki_mag); ezInt += ezi; if (isSwitched(previousEz,ezi)) { ezInt = 0.0; } previousEz = ezi; gx += (Kp*ex + exInt); // adjusted gyroscope measurements gy += (Kp*ey + eyInt); gz += (Kp*ez + ezMag*Kp_mag + ezInt); q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; // integrate quaternion rate and normalise q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; norm1 = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); // normalise quaternion q0 = q0 / norm1; q1 = q1 / norm1; q2 = q2 / norm1; q3 = q3 / norm1; //float q0q0 = q0*q0; float q0q1 = q0*q1;// auxiliary variables to reduce number of repeated operations float q0q2 = q0*q2; float q0q3 = q0*q3; float q1q1 = q1*q1; float q1q2 = q1*q2; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; //direction cosine matrix (DCM), Rotation matrix , Rotated Frame to Stationary Frame XYZ ,, Quaternions_and_spatial_rotation DCM00 = 2*(0.5 - q2q2 - q3q3);//2*(0.5 - q2q2 - q3q3);//=q0q0 + q1q1 - q2q2 - q3q3 DCM01 = 2*(q1q2 - q0q3);//2*(q0q1 + q2q3) DCM02 = 2*(q1q3 + q0q2);//2*(q1q3 - q0q2); 2*(q0q2 - q1q3) โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 42 DCM10 = 2*(q1q2 + q0q3); DCM11 = 2*(0.5 - q1q1 - q3q3);//2*(0.5 - q1q1 - q3q3);//q0q0 - q1q1 + q2q2 - q3q3 DCM12 = 2*(q2q3 - q0q1); DCM20 = 2*(q1q3 - q0q2);//-sin pitch DCM21 = 2*(q2q3 + q0q1); DCM22 = 2*(0.5 - q1q1 - q2q2);//2*(0.5 - q1q1 - q2q2);//=q0q0 - q1q1 - q2q2 + q3q3 //DCM23 = q0*q0 - q1*q1 + q2*q2 - q3*q3; //ahrs_toEuler(); ahrs_y=degrees(atan2(DCM10, DCM00));//2*q0*q0+2*q1*q1-1) ahrs_p=degrees(-asin(DCM20)); // theta //ahrs_p=acos(22);//http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions ahrs_r=degrees(atan2(DCM21, DCM22)); // phi cos_rollcos_pitch = DCM22; if(cos_rollcos_pitch <= 0.866025403)//30 deg = 0.866025403 { cos_rollcos_pitch = 0.866025403; } ////transition between 0 and 360 or -180 and 180 ,By aeroquad////////// Heading = ahrs_y - setHeading; if (ahrs_y <= (setHeading - 180)) { Heading += 360; } if (ahrs_y >= (setHeading + 180)) { Heading -= 360; } //////Earth Frame/////////////// //accrX_Earth = hx; //accrY_Earth = hy; //accrZ_Earth = hz; accrX_Earthf = (AccXf*DCM00 + AccYf*DCM01 + AccZf*DCM02)*-1.0; accrY_Earthf = (AccXf*DCM10 + AccYf*DCM11 + AccZf*DCM12)*-1.0; accrZ_Earthf = (AccXf*DCM20 + AccYf*DCM21 + AccZf*DCM22) - acc_offsetZ2;//acc_offsetZ2 accrX_Earth = accrX_Earth + (accrX_Earthf - accrX_Earth)*G_Dt*32.51245; accrY_Earth = accrY_Earth + (accrY_Earthf - accrY_Earth)*G_Dt*32.51245; accrZ_Earth = accrZ_Earth + (accrZ_Earthf - accrZ_Earth)*G_Dt*32.51245;//22.51245 42.5 18.5 Low pass filter ,smoothing factor α := dt / (RC + dt) applyDeadband(accrX_Earthf, 0.02);//+-0.03 m/s^2 applyDeadband(accrY_Earthf, 0.02);//+-0.03 m/s^2 applyDeadband(accrZ_Earthf, 0.02);//+-0.03 m/s^2 } ///////////////////Filter FourthOrder /////////////////////////////////////// struct fourthOrderData { float inputTm1, inputTm2, inputTm3, inputTm4; float outputTm1, outputTm2, outputTm3, outputTm4; } fourthOrder[4]; float computeFourthOrder(float currentInput, struct fourthOrderData *filterParameters) { // cheby2(4,60,12.5/50) #define _b0 0.001893594048567 #define _b1 -0.002220262954039 #define _b2 0.003389066536478 #define _b3 -0.002220262954039 #define _b4 0.001893594048567 #define _a1 -3.362256889209355 #define _a2 4.282608240117919 #define _a3 -2.444765517272841 #define _a4 0.527149895089809 float output; output = _b0 * currentInput + _b1 * filterParameters->inputTm1 + _b2 * filterParameters->inputTm2 + _b3 * filterParameters->inputTm3 + _b4 * filterParameters->inputTm4 - _a1 * filterParameters->outputTm1 _a2 * filterParameters->outputTm2 _a3 * filterParameters->outputTm3 _a4 * filterParameters->outputTm4; filterParameters->inputTm4 = filterParameters->inputTm3; filterParameters->inputTm3 = filterParameters->inputTm2; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 43 filterParameters->inputTm2 = filterParameters->inputTm1; filterParameters->inputTm1 = currentInput; filterParameters->outputTm4 = filterParameters->outputTm3; filterParameters->outputTm3 = filterParameters->outputTm2; filterParameters->outputTm2 = filterParameters->outputTm1; filterParameters->outputTm1 = output; return output; } void setupFourthOrder() { fourthOrder[XAXIS].inputTm1 = 0.0; fourthOrder[XAXIS].inputTm2 = 0.0; fourthOrder[XAXIS].inputTm3 = 0.0; fourthOrder[XAXIS].inputTm4 = 0.0; fourthOrder[XAXIS].outputTm1 = 0.0; fourthOrder[XAXIS].outputTm2 = 0.0; fourthOrder[XAXIS].outputTm3 = 0.0; fourthOrder[XAXIS].outputTm4 = 0.0; fourthOrder[YAXIS].inputTm1 = 0.0; fourthOrder[YAXIS].inputTm2 = 0.0; fourthOrder[YAXIS].inputTm3 = 0.0; fourthOrder[YAXIS].inputTm4 = 0.0; fourthOrder[YAXIS].outputTm1 = 0.0; fourthOrder[YAXIS].outputTm2 = 0.0; fourthOrder[YAXIS].outputTm3 = 0.0; fourthOrder[YAXIS].outputTm4 = 0.0; fourthOrder[ZAXIS].inputTm1 = acc_offsetZ; fourthOrder[ZAXIS].inputTm2 = acc_offsetZ; fourthOrder[ZAXIS].inputTm3 = acc_offsetZ; fourthOrder[ZAXIS].inputTm4 = acc_offsetZ; fourthOrder[ZAXIS].outputTm1 = acc_offsetZ; fourthOrder[ZAXIS].outputTm2 = acc_offsetZ; fourthOrder[ZAXIS].outputTm3 = acc_offsetZ; fourthOrder[ZAXIS].outputTm4 = acc_offsetZ; } 8. ไฟล์ motorX4sam3x8e.h /* project_Quad 32 bit Arduino Due 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza */ int MOTOR_FrontL_PIN = 4;//PWML7 = pin 6 int MOTOR_FrontR_PIN = 5;//PWML6 = pin 7 int MOTOR_BackL_PIN = 7;// PWML5 = pin 8 int MOTOR_BackR_PIN = 6;// PWML4 = pin 9 #define PWM_DUE_FREQUENCY 400 #define PWM_DUE_PERIOD 2500 //400 in Hz // in us //#define PWM_PRESCALER 8 //#define PWM_COUNTER_PERIOD (F_CPU/PWM_PRESCALER/PWM_FREQUENCY) float motor_FrontL = 1000; float motor_FrontR = 1000; float motor_BackL = 1000; float motor_BackR = 1000; //lag filter motor/ float motor_FrontLf = 1000; float motor_FrontLold = 1000; float motor_FrontLold2 = 1000; float motor_FrontRf = 1000; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 44 float motor_FrontRold = 1000; float motor_FrontRold2 = 1000; float motor_BackLf = 1000; float motor_BackLold = 1000; float motor_BackLold2 = 1000; float motor_BackRf = 1000; float motor_BackRold = 1000; float motor_BackRold2 = 1000; /////////////////////////////////////////////////////////////////////////////// static void setPWMpin(uint32_t pin) { PIO_Configure(g_APinDescription[pin].pPort, PIO_PERIPH_B, //hack Arduino does not allow high PWM by default g_APinDescription[pin].ulPin, g_APinDescription[pin].ulPinConfiguration); } static void configOneMotor(uint8_t ch, uint32_t period) { PWMC_ConfigureChannel(PWM, ch, PWM_CMR_CPRE_CLKA, 0, 0); PWMC_SetPeriod(PWM, ch, period); PWMC_SetDutyCycle(PWM, ch, MINCOMMAND); PWMC_EnableChannel(PWM, ch); } void motor_command_all() { for (int j = 0 ; j <= 50 ; j++) { motor_FrontL = 1000; motor_FrontR = 1000; motor_BackL = 1000; motor_BackR = 1000; PWMC_SetDutyCycle(PWM, 4, 1000); PWMC_SetDutyCycle(PWM, 5, 1000); PWMC_SetDutyCycle(PWM, 6, 1000); PWMC_SetDutyCycle(PWM, 7, 1000); delay(20); } } void motor_initialize() { Serial.print("motor_initialize");Serial.print("\n"); pinMode(MOTOR_FrontL_PIN,OUTPUT); pinMode(MOTOR_FrontR_PIN,OUTPUT); pinMode(MOTOR_BackL_PIN,OUTPUT); pinMode(MOTOR_BackR_PIN,OUTPUT); digitalWrite(MOTOR_FrontL_PIN, LOW); digitalWrite(MOTOR_FrontR_PIN, LOW); digitalWrite(MOTOR_BackL_PIN, LOW); digitalWrite(MOTOR_BackR_PIN, LOW); delay(1000); setPWMpin(6); PWMC_DisableChannel(PWM, 4); setPWMpin(7); PWMC_DisableChannel(PWM, 5); setPWMpin(8); PWMC_DisableChannel(PWM, 6); setPWMpin(9); PWMC_DisableChannel(PWM, 7); pmc_enable_periph_clk(ID_PWM); PWMC_ConfigureClocks(1000000,0,VARIANT_MCK); // set PWM clock A to 1MHz configOneMotor(4, PWM_DUE_PERIOD); configOneMotor(5, PWM_DUE_PERIOD); configOneMotor(6, PWM_DUE_PERIOD); configOneMotor(7, PWM_DUE_PERIOD); motor_command_all(); } void motor_command() { PWMC_SetDutyCycle(PWM, MOTOR_FrontL_PIN, motor_FrontLf);//6 = PWML7 PWMC_SetDutyCycle(PWM, MOTOR_BackR_PIN, motor_BackRf);//7 = PWML6 PWMC_SetDutyCycle(PWM, MOTOR_FrontR_PIN, motor_FrontRf);//8 = PWML5 PWMC_SetDutyCycle(PWM, MOTOR_BackL_PIN, motor_BackLf);//9 = PWML4 โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 45 } /********************************************************************/ /**** ESCs calibration By tinnakon ****/ /********************************************************************/ void ESC_calibration() { Serial.print("ESC_calibration");Serial.print("\n"); for (int i = 0; i < 5; i++) { computeRC(); if(CH_THR > MAXCHECK) { ESC_calibra = 1; } else { ESC_calibra = 0; } delay(20); } int jprint = 0; while(ESC_calibra == 1){ computeRC(); motor_FrontL = (CH_THR - 500)*1.5; motor_FrontR = (CH_THR - 500)*1.5; motor_BackL = (CH_THR - 500)*1.5; motor_BackR = (CH_THR - 500)*1.5; motor_FrontL = constrain(motor_FrontL, MINCOMMAND, MAXCOMMAND); motor_FrontR = constrain(motor_FrontR, MINCOMMAND, MAXCOMMAND); motor_BackL = constrain(motor_BackL, MINCOMMAND, MAXCOMMAND); motor_BackR = constrain(motor_BackR, MINCOMMAND, MAXCOMMAND); PWMC_SetDutyCycle(PWM, 7, motor_FrontL); PWMC_SetDutyCycle(PWM, 6, motor_BackR); PWMC_SetDutyCycle(PWM, 5, motor_FrontR); PWMC_SetDutyCycle(PWM, 4, motor_BackL); jprint++; if(jprint > 10) { jprint = 0; Serial.print(motor_FrontL);Serial.print("\t"); Serial.print(motor_FrontR);Serial.print("\t"); Serial.print(motor_BackL);Serial.print("\t"); Serial.print(motor_BackR);Serial.println("\t"); //Serial1.println(motor_Back); if(Status_LED == LOW) Status_LED = HIGH; else Status_LED = LOW; digitalWrite(13, Status_LED); digitalWrite(Pin_LED_R, Status_LED); digitalWrite(Pin_LED_G, Status_LED); digitalWrite(Pin_LED_B, Status_LED); } delay(20); } } /* void motor_Lag(){ ////////lag lead compensator filter motor//////////////////////////////////// float k_Lag = 2.45;//2.65 3.95 5.85 1.85 0.45 1.078 1.0 1.25 float k_Lead = 25.5;//35.25 float diff_motor_FrontL = (motor_FrontL - motor_FrontLold2)/0.02; float temp_motor_FrontL = diff_motor_FrontL + (motor_FrontL - motor_FrontLf)*k_Lead;//35.5 motor_FrontLf = motor_FrontLf + (temp_motor_FrontL*G_Dt*k_Lag); float diff_motor_FrontR = (motor_FrontR - motor_FrontRold2)/0.02; float temp_motor_FrontR = diff_motor_FrontR + (motor_FrontR - motor_FrontRf)*k_Lead;//35.5 motor_FrontRf = motor_FrontRf + (temp_motor_FrontR*G_Dt*k_Lag); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 46 float diff_motor_BackL = (motor_BackL - motor_BackLold2)/0.02; float temp_motor_BackL = diff_motor_BackL + (motor_BackL - motor_BackLf)*k_Lead;//35.5 motor_BackLf = motor_BackLf + (temp_motor_BackL*G_Dt*k_Lag); float diff_motor_BackR = (motor_BackR - motor_BackRold2)/0.02; float temp_motor_BackR = diff_motor_BackR + (motor_BackR - motor_BackRf)*k_Lead;//35.5 motor_BackRf = motor_BackRf + (temp_motor_BackR*G_Dt*k_Lag); motor_FrontLold2 = motor_FrontLold; motor_FrontLold = motor_FrontL;//store PWM for next diff motor_FrontRold2 = motor_FrontRold; motor_FrontRold = motor_FrontR; motor_BackLold2 = motor_BackLold; motor_BackLold = motor_BackL; motor_BackRold2 = motor_BackRold; motor_BackRold = motor_BackR; } */ void motor_Mix(){ motor_FrontL = uAltitude + u_pitch*0.7071 + u_roll*0.7071 - u_yaw;//Front L , cos45 = 0.7071 motor_FrontR = uAltitude + u_pitch*0.7071 - u_roll*0.7071 + u_yaw;//Front R motor_BackL = uAltitude - u_pitch*0.7071 + u_roll*0.7071 + u_yaw;//Back L motor_BackR = uAltitude - u_pitch*0.7071 - u_roll*0.7071 - u_yaw;////Back R /* #ifdef Quad_P motor_Front = uAltitude + u_pitch - u_yaw; motor_Right = uAltitude - u_roll + u_yaw; motor_Left = uAltitude + u_roll + u_yaw; motor_Back = uAltitude - u_pitch - u_yaw; #endif motor_FrontL = constrain(motor_FrontL, MINCOMMAND, MAXCOMMAND); motor_FrontR = constrain(motor_FrontR, MINCOMMAND, MAXCOMMAND); motor_BackL = constrain(motor_BackL, MINCOMMAND, MAXCOMMAND); motor_BackR = constrain(motor_BackR, MINCOMMAND, MAXCOMMAND); */ if (CH_THR < MINTHROTTLE) { roll_I_rate = 0; pitch_I_rate = 0; yaw_I_rate = 0; motor_FrontL = 1000; motor_FrontR = 1000; motor_BackL = 1000; motor_BackR = 1000; } if(armed == 1) { //motor_Lag(); motor_FrontLf = constrain(motor_FrontL, MINTHROTTLE, MAXCOMMAND);//set PWM data (1000 - 2000)*2 to (2000 4000) motor_FrontRf = constrain(motor_FrontR, MINTHROTTLE, MAXCOMMAND); motor_BackLf = constrain(motor_BackL, MINTHROTTLE, MAXCOMMAND); motor_BackRf = constrain(motor_BackR, MINTHROTTLE, MAXCOMMAND); } else { motor_FrontLf = 1000;//set PWM data (1000 - 2000)*2 to (2000 - 4000) motor_FrontRf = 1000; motor_BackLf = 1000; motor_BackRf = 1000; } } โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 47 9. ไฟล์ mpu6050sam3x8e.h /* project_Quad 32 bit Arduino Due 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza //#include "mpu6050.h" MPU 6050 I2C Gyroscope and Accelerometer MPU6050 HMC5883L Magnetometer */ #define XAXIS 0 #define YAXIS 1 #define ZAXIS 2 float gyro[3]; float accel[3]; float gyroScaleFactor = radians(2000.0/32768.0);//32768.0 32730 , +-32756 32768.0 float accelScaleFactor = 9.80665;//9.80665/4100.62 9.81/8192.09 9.81 / 8205 uint8_t gyroSamples = 0; uint8_t gyroSamples2 = 0; uint8_t accSamples = 0; int16_t gyroRaw[3]; float gyroSum[3]; int16_t accelRaw[3]; float accelSum[3]; float AccXm,AccYm,AccZm; float AccX,AccY,AccZ; float AccXf,AccYf,AccZf; float AccX2,AccY2,AccZ2; float GyroXf,GyroYf,GyroZf; float gyro_offsetX,gyro_offsetY,gyro_offsetZ,acc_offsetX,acc_offsetY,acc_offsetZ; float acc_offsetZ2 = 9.81; float GyroX,GyroY,GyroZ,GyroTemp; float GyroX2,GyroY2,GyroZ2; float Accel[3] = {0.0,0.0,0.0}; float filteredAccel[3] = {0.0,0.0,0.0}; #define HMC5883_Address 0x1E // I2C adress: 0x3C (8bit) 0x1E (7bit) int16_t MagX1,MagY1,MagZ1; float MagXf,MagYf,MagZf; float c_magnetom_x; float c_magnetom_y; float c_magnetom_z; //sensor MPU6050 ------------------------------------// MPU 6050 Registers #define MPU6050_ADDRESS //#define MPUREG_WHOAMI 0x68 0x75 #define MPUREG_SMPLRT_DIV 0x19 #define MPUREG_CONFIG 0x1A #define MPUREG_GYRO_CONFIG 0x1B #define MPUREG_ACCEL_CONFIG 0x1C //#define MPUREG_FIFO_EN #define MPUREG_INT_PIN_CFG //#define MPUREG_INT_ENABLE //#define MPUREG_INT_STATUS #define MPUREG_ACCEL_XOUT_H 0x23 0x37 0x38 0x3A 0x3B //#define MPUREG_ACCEL_XOUT_L 0x3C //#define MPUREG_ACCEL_YOUT_H 0x3D //#define MPUREG_ACCEL_YOUT_L 0x3E //#define MPUREG_ACCEL_ZOUT_H 0x3F //#define MPUREG_ACCEL_ZOUT_L 0x40 //#define MPUREG_TEMP_OUT_H 0x41 //#define MPUREG_TEMP_OUT_L #define MPUREG_GYRO_XOUT_H 0x42 0x43 //#define MPUREG_GYRO_XOUT_L 0x44 //#define MPUREG_GYRO_YOUT_H 0x45 //#define MPUREG_GYRO_YOUT_L 0x46 //#define MPUREG_GYRO_ZOUT_H 0x47 //#define MPUREG_GYRO_ZOUT_L 0x48 โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 48 //#define MPUREG_USER_CTRL 0x6A #define MPUREG_PWR_MGMT_1 0x6B //#define MPUREG_PWR_MGMT_2 0x6C //#define MPUREG_FIFO_COUNTH 0x72 //#define MPUREG_FIFO_COUNTL 0x73 //#define MPUREG_FIFO_R_W 0x74 // Configuration bits //#define BIT_SLEEP 0x40 #define BIT_H_RESET 0x80 //#define BITS_CLKSEL 0x07 //#define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 //#define MPU_EXT_SYNC_GYROX 0x02 //#define BITS_FS_250DPS 0x00 //#define BITS_FS_500DPS 0x08 //#define BITS_FS_1000DPS 0x10 #define BITS_FS_2000DPS 0x18 //#define BITS_FS_MASK //#define BITS_DLPF_CFG_256HZ 0x18 0x00// //Default settings LPF 256Hz/8000Hz sample //#define BITS_DLPF_CFG_188HZ 0x01 //#define BITS_DLPF_CFG_98HZ 0x02 #define BITS_DLPF_CFG_42HZ 0x03 //#define BITS_DLPF_CFG_20HZ 0x04 //#define BITS_DLPF_CFG_10HZ 0x05 //#define BITS_DLPF_CFG_5HZ 0x06 //#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 //#define BITS_DLPF_CFG_MASK 0x07 //#define BIT_INT_ANYRD_2CLEAR 0x10 //#define BIT_RAW_RDY_EN 0x01 //#define BIT_I2C_IF_DIS 0x10 //#define BIT_INT_STATUS_DATA 0x01 //sensor --------------------#define applyDeadband(value, deadband) if(fabs(value) < deadband) { value = 0.0; \ \ \ } else if(value > 0.0){ \ value -= deadband; \ } else if(value < 0.0){ \ value += deadband; \ } void mpu6050_initialize() { Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_PWR_MGMT_1); // Chip reset DEVICE_RESET 1 Wire.write(BIT_H_RESET);//DEVICE_RESET Wire.endTransmission(); delay(30);// Startup delay Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_PWR_MGMT_1); Wire.write(MPU_CLK_SEL_PLLGYROZ);//CLKSEL 3 (PLL with Z Gyro reference) Wire.endTransmission(); delay(5); Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_SMPLRT_DIV);// SAMPLE RATE Wire.write(0x00);//// Sample rate = 1kHz Wire.endTransmission(); delay(5); Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_CONFIG); Wire.write(BITS_DLPF_CFG_42HZ);//98 BITS_DLPF_CFG_188HZ, BITS_DLPF_CFG_42HZ, default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) Wire.endTransmission(); delay(5); Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_GYRO_CONFIG); Wire.write(BITS_FS_2000DPS);//BITS_FS_1000DPS FS_SEL = 3: Full scale set to 2000 deg/sec, BITS_FS_2000DPS Wire.endTransmission(); delay(5); Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_ACCEL_CONFIG); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 49 Wire.write(0x10);//0x10 = 1G=4096, AFS_SEL=2 (0x00 = +/-2G) 1G = 16,384 //0x10 = 1G = 4096 ,//0x08 = +-4g Wire.endTransmission(); delay(5); Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_INT_PIN_CFG);// enable I2C bypass for AUX I2C Wire.write(0x00);//0x00=off ,, 0x02 , I2C_BYPASS_EN=1 Wire.endTransmission(); } void mpu6050_Gyro_Values() { Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_GYRO_XOUT_H); Wire.endTransmission(); Wire.requestFrom(MPU6050_ADDRESS, 6); int i = 0; byte result[6]; while(Wire.available()) { result[i] = Wire.read(); i++; } Wire.endTransmission(); gyroRaw[XAXIS] = ((result[0] << 8) | result[1]);//-12 -3 gyroRaw[YAXIS] = ((result[2] << 8) | result[3])*-1;//37 gyroRaw[ZAXIS] = ((result[4] << 8) | result[5])*-1;//11 15 5 } void mpu6050_Accel_Values() { Wire.beginTransmission(MPU6050_ADDRESS); Wire.write(MPUREG_ACCEL_XOUT_H); Wire.endTransmission(); Wire.requestFrom(MPU6050_ADDRESS, 6); int i = 0; byte result[6]; while(Wire.available()) { result[i] = Wire.read(); i++; } Wire.endTransmission(); accelRaw[XAXIS] = ((result[0] << 8) | result[1])*-1 + 61;//+ 105 + 100 max=4054.46, min=-4149.48 accelRaw[YAXIS] = ((result[2] << 8) | result[3]) + 167;// - 45 - 35 max=4129.57, min=-4070.64 accelRaw[ZAXIS] = ((result[4] << 8) | result[5]);// + 150 + 170 // adjust for max=4014.73 , min=-4165.13 acc axis offsets/sensitivity differences by scaling to +/-1 g range AccXm = ((float)(accelRaw[XAXIS] - A_X_MIN) / (A_X_MAX - A_X_MIN))*2.0 - 1.0; AccYm = ((float)(accelRaw[YAXIS] - A_Y_MIN) / (A_Y_MAX - A_Y_MIN))*2.0 - 1.0; AccZm = ((float)(accelRaw[ZAXIS] - A_Z_MIN) / (A_Z_MAX - A_Z_MIN))*2.0 - 1.0; } void MagHMC5883Int() { Wire1.beginTransmission(HMC5883_Address); //open communication with HMC5883 Wire1.write(0x00); //Configuration Register A Wire1.write(0x70); //num samples: 8 ; output rate: 15Hz ; normal measurement mode Wire1.endTransmission(); delay(10); Wire1.beginTransmission(HMC5883_Address); //open communication with HMC5883 Wire1.write(0x01); //Configuration Register B Wire1.write(0x20); //configuration gain 1.3Ga Wire1.endTransmission(); delay(10); //Put the HMC5883 IC into the correct operating mode Wire1.beginTransmission(HMC5883_Address); //open communication with HMC5883 Wire1.write(0x02); //select mode register Wire1.write(0x00); //continuous measurement mode Wire1.endTransmission(); delay(10); } void Mag5883Read() { Wire1.beginTransmission(HMC5883_Address); //Tell the HMC5883 where to begin reading data โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 50 Wire1.write(0x03); //select register 3, X MSB register Wire1.endTransmission();//Read data from each axis, 2 registers per axis Wire1.requestFrom(HMC5883_Address, 6); int i = 0; byte result[6]; while(Wire1.available()) { result[i] = Wire1.read(); i++; } Wire1.endTransmission(); MagY1 = ((result[0] << 8) | result[1]);//offset + 1.05 MagZ1 = ((result[2] << 8) | result[3]);// + 0.05 MagX1 = ((result[4] << 8) | result[5])*-1;// - 0.55 MagXf = MagXf + (MagX1 - MagXf)*0.45;//0.45 MagYf = MagYf + (MagY1 - MagYf)*0.45; MagZf = MagZf + (MagZ1 - MagZf)*0.45; // adjust for compass axis offsets/sensitivity differences by scaling to +/-5 range c_magnetom_x = ((float)(MagXf - M_X_MIN) / (M_X_MAX - M_X_MIN))*10.0 - 5.0; c_magnetom_y = ((float)(MagYf - M_Y_MIN) / (M_Y_MAX - M_Y_MIN))*10.0 - 5.0; c_magnetom_z = ((float)(MagZf - M_Z_MIN) / (M_Z_MAX - M_Z_MIN))*10.0 - 5.0; } void Mag_Calibrate()//Calibration_sensor Magnetometer { // Output MIN/MAX values M_X_MIN = 0; M_Y_MIN = 0; M_Z_MIN = 0; M_X_MAX = 0; M_Y_MAX = 0; M_Z_MAX = 0; Serial.print("magn x,y,z (min/max) = "); for (int i = 0; i < 600; i++) {//Calibration 30 s digitalWrite(13, HIGH);//30 Mag5883Read(); if (MagX1 < M_X_MIN) M_X_MIN = MagX1; if (MagX1 > M_X_MAX) M_X_MAX = MagX1; if (MagY1 < M_Y_MIN) M_Y_MIN = MagY1; if (MagY1 > M_Y_MAX) M_Y_MAX = MagY1; if (MagZ1 < M_Z_MIN) M_Z_MIN = MagZ1; if (MagZ1 > M_Z_MAX) M_Z_MAX = MagZ1; delay(25); digitalWrite(13, LOW);//30 delay(25); } Serial.print(M_X_MIN);Serial.print("/"); Serial.print(M_X_MAX);Serial.print("\t"); Serial.print(M_Y_MIN);Serial.print("/"); Serial.print(M_Y_MAX);Serial.print("\t"); Serial.print(M_Z_MIN);Serial.print("/"); Serial.print(M_Z_MAX); Serial.print("\n"); } void mpu6050_readGyroSum() { mpu6050_Gyro_Values(); gyroSum[XAXIS] += gyroRaw[XAXIS]; gyroSum[YAXIS] += gyroRaw[YAXIS]; gyroSum[ZAXIS] += gyroRaw[ZAXIS]; gyroSamples++; } void mpu6050_Get_gyro() { if(gyroSamples == 0){ gyroSamples = 1; } GyroX = (gyroSum[XAXIS] / gyroSamples)*gyroScaleFactor - gyro_offsetX;// Calculate average GyroY = (gyroSum[YAXIS] / gyroSamples)*gyroScaleFactor - gyro_offsetY; GyroZ = (gyroSum[ZAXIS] / gyroSamples)*gyroScaleFactor - gyro_offsetZ; gyroSum[XAXIS] = 0.0;// Reset SUM variables gyroSum[YAXIS] = 0.0; โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 51 gyroSum[ZAXIS] = 0.0; gyroSamples2 = gyroSamples; gyroSamples = 0; } void mpu6050_readAccelSum() { mpu6050_Accel_Values(); accelSum[XAXIS] += AccXm; accelSum[YAXIS] += AccYm; accelSum[ZAXIS] += AccZm; accSamples++; } void mpu6050_Get_accel() { if(accSamples == 0){ accSamples = 1; } AccX = (accelSum[XAXIS] / accSamples)*accelScaleFactor - acc_offsetX;// Calculate average AccY = (accelSum[YAXIS] / accSamples)*accelScaleFactor - acc_offsetY; AccZ = (accelSum[ZAXIS] / accSamples)*accelScaleFactor;// Apply correct scaling (at this point accel reprensents +- 1g = 9.81 m/s^2) accelSum[XAXIS] = 0.0; // Reset SUM variables accelSum[YAXIS] = 0.0; accelSum[ZAXIS] = 0.0; accSamples = 0; } void sensor_Calibrate() { Serial.print("Sensor_Calibrate");Serial.println("\t"); gyroSum[XAXIS] = 0.0;// Reset SUM variables gyroSum[YAXIS] = 0.0; gyroSum[ZAXIS] = 0.0; gyroSamples = 0; accelSum[XAXIS] = 0.0; // Reset SUM variables accelSum[YAXIS] = 0.0; accelSum[ZAXIS] = 0.0; for (uint8_t i=0; i<45; i++) //Collect 60, 100 samples { Serial.print("- "); mpu6050_readGyroSum(); mpu6050_readAccelSum(); Mag5883Read(); digitalWrite(13, HIGH); digitalWrite(Pin_LED_R, HIGH); delay(20); digitalWrite(13, LOW); digitalWrite(Pin_LED_R, LOW); delay(20); } Serial.println("- "); gyro_offsetX = (gyroSum[XAXIS]/gyroSamples)*gyroScaleFactor; gyro_offsetY = (gyroSum[YAXIS]/gyroSamples)*gyroScaleFactor; gyro_offsetZ = (gyroSum[ZAXIS]/gyroSamples)*gyroScaleFactor; acc_offsetX = (accelSum[XAXIS]/gyroSamples)*accelScaleFactor; acc_offsetY = (accelSum[YAXIS]/gyroSamples)*accelScaleFactor; acc_offsetZ = (accelSum[ZAXIS]/gyroSamples)*accelScaleFactor; acc_offsetZ2 = sqrt(acc_offsetX*acc_offsetX + acc_offsetY*acc_offsetY + acc_offsetZ*acc_offsetZ); AccZf = acc_offsetZ;//15.4 MagXf = MagX1; MagYf = MagY1; MagZf = MagZ1; gyroSamples = 0; accSamples = 0; Serial.print("GYRO_Calibrate");Serial.print("\t"); Serial.print(gyro_offsetX);Serial.print("\t");//-0.13 Serial.print(gyro_offsetY);Serial.print("\t");//-0.10 Serial.print(gyro_offsetZ);Serial.println("\t");//0.03 Serial.print("ACC_Calibrate");Serial.print("\t"); Serial.print(acc_offsetX);Serial.print("\t"); Serial.print(acc_offsetY);Serial.print("\t"); Serial.print(acc_offsetZ);Serial.print("\t"); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 52 Serial.print(acc_offsetZ2);Serial.println("\t"); acc_offsetX = 0.18;//-0.18 0.11 -0.36 Trim PITCH CONTROL acc_offsetY = 0.12;//0.16 -0.14 0.18 Trim ROLL CONTROL -10.07 -10.55 10.39 9.74 11 -9.82 //acc_offsetZ = 0.0;//0.245 0.235 10.2 } /* ****************************************************************** *** Accelerometers trim Remote Trim By tinnakon *** //With the help of your roll and pitch stick you could now trim the ACC mode. //You must first put the throttle stick in maximal position. (obviously with motors disarmed) //full PITCH forward/backward and full ROLL left/right (2 axis possibilities) will trim the level //mode according to the neutral angle you want to change. //The status LED will blink to confirm each ticks. */ void Remote_TrimACC() { if(CH_THR > MAXCHECK && armed == 0) { ////Trim ROLL CONTROL///////////// if(CH_AIL > MAXCHECK) { acc_offsetY = acc_offsetY + 0.04; for (int i = 0; i < 5; i++) { digitalWrite(13, HIGH); delay(20); digitalWrite(13, LOW); delay(20); } Serial.println(acc_offsetY); } if(CH_AIL < MINCHECK) { acc_offsetY = acc_offsetY - 0.04; for (int i = 0; i < 5; i++) { digitalWrite(13, HIGH); delay(20); digitalWrite(13, LOW); delay(20); } Serial.println(acc_offsetY); } ///////Trim PITCH CONTROL////////////////////// if(CH_ELE > MAXCHECK) { acc_offsetX = acc_offsetX + 0.04; for(int i = 0; i < 5; i++) { digitalWrite(13, HIGH); delay(20); digitalWrite(13, LOW); delay(20); } Serial.println(acc_offsetX); } if(CH_ELE < MINCHECK) { acc_offsetX = acc_offsetX - 0.04; for(int i = 0; i < 5; i++) { digitalWrite(13, HIGH); delay(20); digitalWrite(13, LOW); delay(20); } Serial.println(acc_offsetX); } }//end CH_THR > MAXCHECK && armed == 0 }/////////////////////////////////////////////////////// โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 53 10. ไฟล์ multi_rx_sam3x8e.h /* project_Quad 32 bit Arduino Due 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza //clock timer7 (1000 - 2000 us) = (2625 - 5250) = 2625 */ float CH_THR = 1000.0; float CH_AIL = 1500.0; float CH_ELE = 1500.0; float CH_RUD = 1500.0; float AUX_1 = 1000.0; float AUX_2 = 1000.0; float AUX_3 = 1000.0; float AUX_4 = 1000.0; float CH_THRf = 1000.0; float CH_AILf = 1500.0; float CH_ELEf = 1500.0; float CH_RUDf = 1500.0; int CH_AIL_Cal = 1500; int CH_ELE_Cal = 1500; int CH_RUD_Cal = 1500; //RX PIN assignment inside the port //SET YOUR PINS! TO MATCH RECIEVER CHANNELS #define CHAN1PIN 62 // RECIEVER 1 PPM ,pin A8 //#define CHAN2PIN 63 // RECIEVER 2 //#define CHAN3PIN 64 // RECIEVER 3 //#define CHAN4PIN 65 // RECIEVER 4 //#define CHAN5PIN 61 // RECIEVER 5 //#define CHAN6PIN 67 //not used at the moment //#define CHAN7PIN 50 //not used at the moment //#define CHAN8PIN 51 //not used at the moment #define ROLL 0 #define PITCH 1 #define YAW 3 #define THROTTLE 2 #define AUX1 4 #define AUX2 5 #define CAMPITCH 6 #define CAMROLL 7 //volatile unsigned long pwmLast[8]; volatile unsigned long last = 0; uint8_t chan1 = 0; volatile uint32_t rcValue[8] = {3937, 3937, 3937, 3937, 3937, 3937, 3937, 3937}; // interval [1000;2000] void pwmHandler(int ch, int pin) { ///PPM/////////////////// Arduino Due RX PPM uint32_t now, diff; //now = micros(); diff = TC_ReadCV(TC2, 1);// Read TC2 = timer 7 TC_Stop(TC2, 1); TC_Start(TC2, 1); //last = now; if (diff > 7875) chan1 = 0;//3000 us else { if (2363 < diff && diff < 5775 && chan1 < 8) { //900 - 2200 us rcValue[chan1] = diff; } chan1++; } } void ch1Handler() { pwmHandler(0, CHAN1PIN); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 54 } //void ch2Handler() { pwmHandler(1, CHAN2PIN); } //void ch3Handler() { pwmHandler(2, CHAN3PIN); } //void ch4Handler() { pwmHandler(3, CHAN4PIN); } //void ch5Handler() { pwmHandler(4, CHAN5PIN); } void configureReceiver() { Serial.print("Configure Receiver CPPM");Serial.print("\n"); for (uint8_t chan = 0; chan < 8; chan++) { for (uint8_t a = 0; a < 4; a++) { rcValue[a] = 3937;//1500 = 3937.5 } } rcValue[THROTTLE] = 2625;//2625 = 1000 us rcValue[AUX1] = 2625; rcValue[AUX2] = 2625; rcValue[CAMPITCH] = 2625; rcValue[CAMROLL] = 2625; attachInterrupt(CHAN1PIN, ch1Handler, RISING); //RISING FALLING CHANGE //attachInterrupt(CHAN2PIN,ch2Handler,CHANGE); //attachInterrupt(CHAN3PIN,ch3Handler,CHANGE); //attachInterrupt(CHAN4PIN,ch4Handler,CHANGE); //attachInterrupt(CHAN5PIN,ch5Handler,CHANGE); /* turn on the timer clock in the power management controller */ pmc_set_writeprotect(false); // disable write protection for pmc registers pmc_enable_periph_clk(ID_TC7); // enable peripheral clock TC7 //TC_CMR_TCCLKS_TIMER_CLOCK1 84Mhz/2 = 42.000 MHz //TC_CMR_TCCLKS_TIMER_CLOCK2 84Mhz/8 = 10.500 MHz //TC_CMR_TCCLKS_TIMER_CLOCK3 84Mhz/32 = 2.625 MHz //TC_CMR_TCCLKS_TIMER_CLOCK4 84Mhz/128 = 656.250 KHz /* we want wavesel 01 with RC (timer 7 = TC2,1) (timer 8 = TC2,2)*/ TC_Configure(/* clock */TC2,/* channel */1, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK3);//TC_CMR_TCCLKS_TIMER_CLOCK3 //TC_SetRC(TC2, 1, 131200); TC_Start(TC2, 1); } void computeRC() { CH_THR = rcValue[THROTTLE]*0.3809523;// 84MHz/32 * 1500 us = 3937.5 CH_AIL = rcValue[ROLL]*0.3809523; CH_ELE = rcValue[PITCH]*0.3809523; CH_RUD = rcValue[YAW]*0.3809523; AUX_1 = rcValue[AUX1]*0.3809523; AUX_2 = rcValue[AUX2]*0.3809523; AUX_3 = rcValue[CAMPITCH]*0.3809523; AUX_4 = rcValue[CAMROLL]*0.3809523; CH_THRf = CH_THRf + (CH_THR - CH_THRf) * 0.02 / tarremote; //Low pass filter CH_AILf = CH_AILf + (CH_AIL - CH_AILf) * 0.02 / tarremote; //Low pass filter CH_ELEf = CH_ELEf + (CH_ELE - CH_ELEf) * 0.02 / tarremote; CH_RUDf = CH_RUDf + (CH_RUD - CH_RUDf) * 0.02 / tarremote; } void RC_Calibrate() { Serial.print("RC_Calibrate"); Serial.println("\t"); //By tinnakon for (int i = 0; i < 10; i++) { computeRC(); delay(20); } CH_AIL_Cal = CH_AIL; CH_ELE_Cal = CH_ELE; CH_RUD_Cal = CH_RUD; Serial.print(CH_AIL_Cal); Serial.print("\t"); //1505 Serial.print(CH_ELE_Cal); Serial.print("\t"); //1498 Serial.print(CH_RUD_Cal); Serial.println("\t"); //1502 } โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 55 11. ไฟล์ GPSNEO6N_multi.h /* project_Quad 32 bit Arduino Due //GPS I2C_GPS_NAV_v2_2 1. stabilized quadrotor by: tinnakon kheowree 0860540582 tinnakon_za@hotmail.com tinnakonza@gmail.com http://quad3d-tin.lnwshop.com/ https://www.facebook.com/tinnakonza UBLOX tree - U-Blox binary protocol, use the ublox config file (u-blox-config.ublox.txt) from the source */ int32_t GPS_coord[2];// 1e-7 degrees / position as degrees (*10E7) float GPS_ground_course = 0.0; // velocities in cm/s if available from the GPS int32_t _velocity_north; int32_t _velocity_east; int32_t _vel_down; int LAT=0; int LON=1; uint8_t GPS_FIX=0; int GPS_numSat=0; int GPS_altitude=0; int GPS_Present=0; float Alt_Home = 2;//m float yaw = 0.0; float pitch = 0.0; float Distance = 0.0; #define ToRad 0.01745329252 // *pi/180 #define ToDeg 57.2957795131 // *180/pi //prog_char prog_char UBLOX_INIT[] PROGMEM = { // PROGMEM array must be outside any function !!! 0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x05,0x00,0xFF,0x19, //disable all default NMEA messages 0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x03,0x00,0xFD,0x15, //0x06 Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set Baud Rate, etc. 0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x01,0x00,0xFB,0x11, 0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x00,0x00,0xFA,0x0F, 0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x02,0x00,0xFC,0x13, 0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x04,0x00,0xFE,0x17, 0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x02,0x01,0x0E,0x47, //set POSLLH MSG rate 0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x03,0x01,0x0F,0x49, //set STATUS MSG rate 0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x06,0x01,0x12,0x4F, //set SOL MSG rate 0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x12,0x01,0x1E,0x67, //set VELNED MSG rate 0xB5,0x62,0x06,0x16,0x08,0x00,0x03,0x07,0x03,0x00,0x51,0x08,0x00,0x00,0x8A,0x41, 0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A //set WAAS to EGNOS // set rate to 5Hz }; //http://www.multiwii.com/forum/viewtopic.php?f=6&t=4964 //0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A //set rate to 5Hz //#define UBLOX_1HZ 0xB5,0x62,0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00,0x01,0x39 // set rate to 0xB5,0x62,0x06,0x08,0x06,0x00,0xF4,0x01,0x01,0x00,0x01,0x00,0x0B,0x77 // set rate 0xB5,0x62,0x06,0x08,0x06,0x00,0x4D,0x01,0x01,0x00,0x01,0x00,0x64,0x8D // set rate to 0xB5,0x62,0x06,0x08,0x06,0x00,0xFA,0x00,0x01,0x00,0x01,0x00,0x10,0x96 // set rate 0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A // set rate to 1Hz // #define UBLOX_2HZ to 2Hz //#define UBLOX_3HZ 3Hz // #define UBLOX_4HZ to 4Hz //#define UBLOX_5HZ 5Hz #define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26\n\265\142\006\001\003\000\001\006\001\022\117" void GPS_multiInt() { Serial.print("GPS_multiInt 9600 to 38400");Serial.print("\n"); Serial2.begin(9600);//GPS2 38400 //GPS UBLOX switch UART speed for sending SET BAUDRATE command (NMEA mode) // (which depends on the NMEA or MTK_BINARYxx settings) delay(200); โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 56 Serial2.print("$PUBX,41,1,0003,0001,38400,0*26\r\n"); //Serial2.print("$PUBX,41,1,0003,0001,115200,0*1E\r\n"); //delay(10); //Serial1.print("$PUBX,41,1,0003,0001,38400,0*26\r\n"); //Serial2.print("$PUBX,41,1,0003,0001,115200,0*1E\r\n"); delay(200); Serial2.begin(38400); delay(20); for(uint8_t i=0; i<sizeof(UBLOX_INIT); i++) { // send configuration data in UBX protocol Serial2.write(pgm_read_byte(UBLOX_INIT+i)); delay(5); //simulating a 38400baud pace (or less), otherwise commands are not accepted by the device. } Serial.print("End");Serial.print("\n"); } struct ubx_header { uint8_t preamble1; uint8_t preamble2; uint8_t msg_class; uint8_t msg_id; uint16_t length; }; struct ubx_nav_posllh { uint32_t time; // GPS msToW int32_t longitude; int32_t latitude; int32_t altitude_ellipsoid; int32_t altitude_msl; uint32_t horizontal_accuracy; uint32_t vertical_accuracy; }; struct ubx_nav_solution { uint32_t time; int32_t time_nsec; int16_t week; uint8_t fix_type; uint8_t fix_status; int32_t ecef_x; int32_t ecef_y; int32_t ecef_z; uint32_t position_accuracy_3d; int32_t ecef_x_velocity; int32_t ecef_y_velocity; int32_t ecef_z_velocity; uint32_t speed_accuracy; uint16_t position_DOP; uint8_t res; uint8_t satellites; uint32_t res2; }; struct ubx_nav_velned { uint32_t time; // GPS msToW int32_t ned_north; int32_t ned_east; int32_t ned_down; uint32_t speed_3d; uint32_t speed_2d; int32_t heading_2d; uint32_t speed_accuracy; uint32_t heading_accuracy; }; enum ubs_protocol_bytes { PREAMBLE1 = 0xb5, PREAMBLE2 = 0x62, CLASS_NAV = 0x01, CLASS_ACK = 0x05, CLASS_CFG = 0x06, MSG_ACK_NACK = 0x00, MSG_ACK_ACK = 0x01, MSG_POSLLH = 0x2, MSG_STATUS = 0x3, โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 57 MSG_SOL = 0x6, MSG_VELNED = 0x12, MSG_CFG_PRT = 0x00, MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, MSG_CFG_NAV_SETTINGS = 0x24 }; enum ubs_nav_fix_type { FIX_NONE = 0, FIX_DEAD_RECKONING = 1, FIX_2D = 2, FIX_3D = 3, FIX_GPS_DEAD_RECKONING = 4, FIX_TIME = 5 }; enum ubx_nav_status_bits { NAV_STATUS_FIX_VALID = 1 }; // Packet checksum accumulators static uint8_t _ck_a; static uint8_t _ck_b; // State machine state static uint8_t _step; static uint8_t _msg_id; static uint16_t _payload_length; static uint16_t _payload_counter; // static bool next_fix; static uint8_t _class; static uint8_t _fix_ok; // Receive buffer static union { ubx_nav_posllh posllh; // ubx_nav_status status; ubx_nav_solution solution; ubx_nav_velned velned; uint8_t bytes[]; } _buffer; bool UBLOX_parse_gps(void) { GPS_Present = 1; switch (_msg_id) { case MSG_POSLLH: //i2c_dataset.time = _buffer.posllh.time; if(_fix_ok) { GPS_coord[LON] = _buffer.posllh.longitude; GPS_coord[LAT] = _buffer.posllh.latitude; GPS_altitude = _buffer.posllh.altitude_msl / 1000; //alt in m } GPS_FIX = _fix_ok; return true; // POSLLH message received, allow blink GUI icon and LED break; case MSG_SOL: _fix_ok = 0; if((_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D || _buffer.solution.fix_type == FIX_2D)) _fix_ok = 1; GPS_numSat = _buffer.solution.satellites; break; case MSG_VELNED: GPS_speed = _buffer.velned.speed_2d; // cm/s GPS_ground_course = (_buffer.velned.heading_2d / 100000.0); // (uint16_t) Heading 2D deg * 100000 rescaled to deg * 10 _velocity_north = _buffer.velned.ned_north; _velocity_east = _buffer.velned.ned_east; _vel_down = _buffer.velned.ned_down * -1; break; default: break; } return false; } โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 58 bool GPS_UBLOX_newFrame(uint8_t data){ bool parsed = false; GPS_Present = 0; switch(_step) { case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; case 0: if(PREAMBLE1 == data) _step++; break; case 2: _step++; _class = data; _ck_b = _ck_a = data; // reset the checksum accumulators break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _msg_id = data; break; case 4: _step++; _ck_b += (_ck_a += data); _payload_length = data; // checksum byte // payload length low byte break; case 5: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)(data<<8); if (_payload_length > 512) { _payload_length = 0; _step = 0; } _payload_counter = 0; // prepare to receive payload break; case 6: _ck_b += (_ck_a += data); // checksum byte if (_payload_counter < sizeof(_buffer)) { _buffer.bytes[_payload_counter] = data; } if (++_payload_counter == _payload_length) _step++; break; case 7: _step++; if (_ck_a != data) _step = 0; // bad checksum break; case 8: _step = 0; if (_ck_b != data) break; // bad checksum GPS_Present = 1; if (UBLOX_parse_gps()) { parsed = true;} } //end switch return parsed; } 12. ไฟล์ AP_Baro_MS5611.cpp 13. ไฟล์ AP_Baro_MS5611.h 14. ไฟล์ SPI_sam.cpp 15. ไฟล์ SPI_sam.h 16. ไฟล์ Wire_due32.cpp 17. ไฟล์ Wire_due32.h โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 59 เอกสารอ้างอิง 1. น.ต.วัชรพงษ์ เข็มเพ็ชร Optical flow-based http://www.dae.mi.th/aeroblog/?p=2796 2. น.ต.ดร. กฤษฎา แสงเพ็ชร์ส่อง Kalman Filter แนวคิดพื้นฐานและหลักการทางานของ Kalman Filter Algorithm http://library.rtna.ac.th/web/RTNA_Journal/y.4c.4/05.pdf 3. DIY mini Quad Copter Chiang Mai Maker Club http://www.cmmakerclub.com/2014/11/667 4. Quadcopter Parts List | What You Need to Build a DIY Quadcopter http://quadcoptergarage.com/quadcopter-parts-list-what-you-need-to-build-adiy-quadcopter/ 5. MultiWii http://www.multiwii.com/ http://witespyquad.gostorego.com/ 6. AeroQuad http://aeroquad.com/content.php 7. MegaPirateNG https://github.com/MegaPirateNG/ardupilot-mpng 8. APM Open source autopilot http://planner.ardupilot.com/ http://ardupilot.com/ 9. OpenPilot CC3D http://www.openpilot.org/ 10. UAVX-ARM32 Full Sensors http://www.quadroufo.com/ 11. AutoQuad is an Open Source firmware project http://autoquad.org/ 12. ศึกษาการเขียนโปรแกรมควบคุม quadrotor http://www.rcthai.net/ 13. ทินกร เขียวรี การเขียนโปรแกรม Quadrotor พื้นฐาน http://quad3d-tin.lnwshop.com https://github.com/QuadTinnakon/QuadX_2560GY86_PIDAuto_V5 โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582 60 ประวัติผู้จัดทา ชื่อ สาขาวิชา : นายทินกร เขียวรี : วิศวกรรมเครื่องกล สถานที่อยู่ที่ติดต่อได้สะดวก สาขาวิชาวิศวกรรมเครื่องกล คณะวิศวกรรมศาสตร์และสถาปัตยกรรมศาสตร์มหาวิทยาลัยเทคโนโลยีราชมงคล สุวรรณภูมิ ศูนย์นนทบุรี เลขที่ 7/1 ถ.นนทบุรี ต.สวนใหญ่ อ.เมือง จ.นนทบุรี 11000 โทรศัพท์ 086-0540582 e-mail tinnakon_za@hotmail.com หรือ tinnakonza@gmail.com , เว็บไซต์ http://quad3d-tin.lnwshop.com/ , https://www.facebook.com/tinnakonza อัพเดทล่าสุด 8 มกราคม 2558 ประวัติ ประวัติส่วนตัว เกิดเมื่อวันที่ 3 มกราคม 2530 บ้านเลขที่ 28/5 หมู่ 6 ตาบล พักทัน อาเภอ บางระจัน จังหวัดสิงห์บุรี 16130 ประวัติการศึกษา สาเร็จการศึกษามัธยมศึกษาตอนต้นและตอนปลายจาก โรงเรียนศรีศักดิ์สุวรรณ วิทยา จังหวัดสิงห์บุรี ปีการศึกษา 2544 - 2547 สาเร็จการศึกษาระดับประกาศนียบัตรวิชาชีพชั้นสูง สาขาช่างยนต์ จากมหาวิทยาลัยเทคโนโลยีราชมงคลสุวรรณภูมิ ศูนย์หันตรา จังหวัด พระนครศรีอยุธยา ปีการศึกษา 25 49 และสาเร็จการศึกษาระดับวิศวกรรมศาสตรบัณฑิต สาขาวิชา วิศวกรรมเครื่องกล คณะวิศวกรรมศาสตร์และสถาปัตยกรรมศาสตร์ ณ มหาวิทยาลัยเทคโนโลยีราช มงคลสุวรรณภูมิ ศูนย์นนทบุรี ปีการศึกษา 25 53 และสาเร็จการศึกษาระดับวิศวกรรมศาสตร มหาบัณฑิต สาขา วิชาวิศวกรรมเครื่องกล คณะวิศวกรรมศาสตร์ มหาวิทยาลัยเทคโนโลยีพระจอม เกล้าพระนครเหนือ ปีการศึกษา 2555 ประวัติการทางาน ปี พ.ศ. 2552 - 2553 เป็นวิศวกรด้านการจัดการพลังงาน และวิทยากรงาน สัมมนาโครงการอนุรักษ์พลังงานแบบมีส่วนร่วม ปี พ.ศ. 2554 – 2555 ทางานเป็นผู้ช่วยวิจัย มหาวิทยาลัยเทคโนโลยีพระจอมเกล้าพระนครเหนือ ปี พ.ศ. 2556 – ปัจจุบันทางานตาแหน่งอาจารย์ สอนวิชาการควบคุมอัตโนมัติ สาขาวิศวกรรมเครื่องกล คณะวิศวกรรมศาสตร์ และสถาปัตยกรรม ศาสตร์ มหาวิทยาลัยเทคโนโลยีราชมงคลสุวรรณภูมิ ศูนย์นนทบุรี ประวัติงานวิจัย การควบคุมแบบชดเชยแรงเสียดทานสาหรับฐานเคลื่อนไหวที่ขับเคลื่อนด้วย ระบบไฮดรอลิกส์. การประชุมวิชาการเครือข่ายวิศวกรรมเครื่องกลแห่งประเทศไทย ครั้งที่ 25 จังหวัด กระบี่ วันที่ 19 - 21 ตุลาคม 2554 การค้นคว้าและศึกษาตัวสังเกตแบบไม่เชิงเส้นของระบบพลวัต เชิงกล โดยประมาณค่าแรงเสียดทานที่เกิดขึ้นในกระบอกสูบไฮดรอลิกส์ นาไปใช้ชดเชยการควบคุมไม่ เชิงเส้นโดยใช้วิธีการควบคุมแบบขั้นถอยหลัง (Backstepping Control) และการควบคุมแบบปรับตัว เองได้ (Adaptive Control) และ ปี 2557 ได้รับทุนงบประมาณเงินกองทุนส่งเสริมงานวิจัยเรื่องการ ควบคุมความสูงระดับเพดานบินโดยใช้ตัวควบคุมแบบปรับตัวเองได้ร่วมกับตัวสังเกตคาลมานสาหรับ เครื่องบิน 4 ใบพัดขนาดเล็ก โดยนายทินกร เขียวรี http://quad3d-tin.lnwshop.com/ tinnakonza@gmail.com T.086-054-0582

การเขียนโปรแกรมQuadAuto32bitV1 26 4 59

Add this document to collection(s)

Add this document to saved

Suggest us how to improve StudyLib