69浏览
查看: 69|回复: 8

[项目] 【Arduino 动手做】3D打印的变形金刚飞天入地机器人

[复制链接]
本帖最后由 驴友花雕 于 2025-6-4 10:21 编辑

Arduino是一个开放源码的电子原型平台,它可以让你用简单的硬件和软件来创建各种互动的项目。Arduino的核心是一个微控制器板,它可以通过一系列的引脚来连接各种传感器、执行器、显示器等外部设备。Arduino的编程是基于C/C++语言的,你可以使用Arduino IDE(集成开发环境)来编写、编译和上传代码到Arduino板上。Arduino还有一个丰富的库和社区,你可以利用它们来扩展Arduino的功能和学习Arduino的知识。

Arduino的特点是:
1、开放源码:Arduino的硬件和软件都是开放源码的,你可以自由地修改、复制和分享它们。
2、易用:Arduino的硬件和软件都是为初学者和非专业人士设计的,你可以轻松地上手和使用它们。
3、便宜:Arduino的硬件和软件都是非常经济的,你可以用很低的成本来实现你的想法。
4、多样:Arduino有多种型号和版本,你可以根据你的需要和喜好来选择合适的Arduino板。
5、创新:Arduino可以让你用电子的方式来表达你的创意和想象,你可以用Arduino来制作各种有趣和有用的项目,如机器人、智能家居、艺术装置等。

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图2

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1

驴友花雕  中级技神
 楼主|

发表于 前天 10:02

【Arduino 动手做】3D打印变形金刚飞行机器人

发明家迈克尔·雷希廷打造了一款真实的变形金刚机器人,它不仅可以像无人机一样飞行,还能在道路上行驶。这款机器人的3D打印螺旋桨护罩兼作轮子,轻巧的履带使其能够穿越各种地形。两台低速无刷电机安装在每对车轮之间,通过与驱动履带的相同齿直接啮合的链轮驱动系统。它是如何从行驶状态过渡到飞行状态的?一个平行四边形安装的四连杆机构,其底部有一个线性执行器作为缩回的杆,将车轮/螺旋桨护罩旋转至垂直位置。

“ 我喜欢这个概念,真希望有更多类似的商业方案。能够让无人机着陆并保持其飞行能力,这将是一项巨大的成就。你可以像PeterScripol的遥控坦克一样,把它绑在迷你飞行器上,然后用伺服臂回收其他坠毁的迷你飞行器。你可以用它来着陆并检查排水管、隧道或其他狭窄的入口,这在勘测、研究和侦察方面非常有用,”一位评论者说道。

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1

回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:03

【Arduino 动手做】3D打印变形金刚飞天机器人


能够根据运行环境改变形状和形态的车辆一直以来都吸引着科技爱好者的目光,而制造这样一架车辆也一直是许多创客的梦想。现在,[Michael Rechtin] 的 3D 打印四轴飞行器让这个梦想更容易实现,它可以无缝转换为履带式地面车辆。

该设计解决了一个关键的工程难题:大多数多模式飞行器都难以应对飞行和驾驶所需的巨大转速差异。[Michael] 的解决方案是使用打印的螺旋桨护罩作为车轮,并搭配轻质履带。每对车轮之间额外安装一对低速无刷电机,通过与驱动履带的齿直接啮合的链轮驱动系统。


【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1
回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:05

【Arduino 动手做】3D打印变形金刚机器人

这种神奇的过渡是通过一个平行四边形结构的四连杆机构实现的,线性执行器充当底部连杆。要从飞行状态切换到驾驶状态,线性执行器会缩回,将轮子/螺旋桨护罩旋转到垂直位置。然后,伺服器会旋转顶部连杆,将机身抬离地面。虽然这种方法会增加一些重量——对于多功能机器来说,这是不可避免的妥协——但它不失为一个实用的解决方案。

为该变压器供电的是运行dRehmFlight 的Teensy 4.0 飞行控制器,dRehmFlight 是一个可破解的飞行稳定套件,我们已看到它成功适用于从垂直起降飞机到主动稳定水翼船的所有物体。

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1

回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:06

【Arduino 动手做】3D打印变形金刚机器人

构建变形无人机的分步指南:

步骤 1:定义项目
1. 概念化混合设计:• 决定一种可以在坦克和四轴飞行器模式之间无缝切换的双模式车辆。• 优先考虑强大的驾驶能力和平稳的飞行转换过程。
2. 设定目标:• 设计一种重量轻但耐用的车辆,在两种模式下都具有出色的机动性。

步骤 2:初步设计和原型制作
1. 使用 CAD 软件:• 利用 Onshape 等软件创建详细的 3D 模型。• 专注于基本设计特征,例如变换连杆系统、伺服器位置和电机对准。
2. 对连杆系统进行原型制作:• 设计和测试将车辆从坦克模式转换为四轴飞行器模式的机制。• 从简化的 2D 模型开始改进运动和对齐。
3. 迭代和改进:• 根据功能和测试结果根据需要修改设计。

步骤 3:3D 打印组件
1. 选择打印机和材料:• 使用高质量打印机,例如 Bambu Lab X1 Carbon 或 P1S。• 对于结构部件:• 碳纤维尼龙,提高强度和耐用性。• PLA 用于不太重要的部件,以减轻重量。
2. 优化强度和重量:• 通过增加填充和增加额外周长来加固 PLA 部件。• 如果需要,使用更轻的材料重新打印较重的部件。

步骤 4:制造非 3D 打印部件
1. 切割结构部件:• 使用 3 毫米碳纤维板作为关键侧板。• 使用 CNC 路由器切出减轻重量的图案和安装孔。

步骤 5:组装组件
1. 组装连杆系统:• 安装伺服外壳以使身体能够运动。• 加入轴承以确保手臂关节​​平稳• 使用主控制板(例如,带有 Dreamflight 固件的 TT 4.0)进行协调。 • 使用自定义 PCB 组织布线以获得干净的设置。
2.  安装坦克履带: • 将 3D 打印的履带固定在车轮周围,以确保可靠的牵引力。

步骤 6:测试和故障排除
1. 验证转换机制: • 测试伺服运动以确保平稳过渡。 • 进行调整以防止零件碰撞或卡住。
2.评估驾驶和飞行: • 测试地面性能的稳定性和速度。 • 评估四轴飞行器模式的升力和控制。

步骤 7:减轻重量
1. 减轻组件重量: • 用碳纤维尼龙版本替换较重的部件。 • 考虑在低速部分使用 3D 打印的滚柱轴承。

步骤 8:执行最终测试
1. 进行过渡测试: • 确保坦克和四轴飞行器模式之间的无缝切换。
2. 模拟真实场景

步骤 9:微调设计
1.增强飞行和驾驶性能:• 优化控制算法,提升操控性。• 减轻整体重量,提升效率。

第十步:分享构建过程
1. 记录并发布:• 分享构建过程,包括 CAD 文件和设计说明,以激发他人灵感。
2. 收集反馈:• 根据社区反馈,探索进一步的改进,例如增加续航里程或提升有效载荷能力。

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1

回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:08

【Arduino 动手做】3D打印变形金刚机器人

硬件(附属链接):
线性执行器: https ://amzn.to/3ADCbcM
伺服: https ://amzn.to/4fw7RjC
EMAX 2812 电机: https://emax-usa.com/collections/28xx...
5010 电机: https ://amzn.to/4fnkPjt
RadioMaster Pocket: https: //amzn.to/3CrXxuc
Teensy 4.0: https: //amzn.to/3AS2CeJ
MPU6050: https://amzn.to/4fqPmNp

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1

回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:11

【Arduino 动手做】3D打印变形金刚机器人

附录
项目视频:https://www.youtube.com/watch?v=f1GSzysrYtw
项目作者:迈克尔·雷希廷[Michael Rechtin]
(我是一名全职航空航天工程师,闲暇时总是忙着做项目,涉及 3D 打印、电子产品、遥控飞机、汽车等等!)
项目介绍:https://hackaday.com/2024/11/22/ ... e-drives-and-flies/
https://www.techeblog.com/real-transformers-robot-drone-drive/
项目代码:https://github.com/MichaelRechtin/Transforming-Quadcopter
3D 文件:https://makerworld.com/en/models ... er#profileId-723196
Onshape 帐户: https: //Onshape.pro/MichaelRechtin

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图4

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图2

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图1

【Arduino 动手做】3D打印的变形金刚飞天入地机器人图3

Transforming-Quadcopter-main.zip

141.05 KB, 下载次数: 15

回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:17

【Arduino 动手做】3D打印变形金刚机器人

通信代码

  1.   unsigned long returnPWM = 0;
  2.   
  3.   if (ch_num == 1) {
  4.     returnPWM = channel_1_raw;
  5.   }
  6.   else if (ch_num == 2) {
  7.     returnPWM = channel_2_raw;
  8.   }
  9.   else if (ch_num == 3) {
  10.     returnPWM = channel_3_raw;
  11.   }
  12.   else if (ch_num == 4) {
  13.     returnPWM = channel_4_raw;
  14.   }
  15.   else if (ch_num == 5) {
  16.     returnPWM = channel_5_raw;
  17.   }
  18.   else if (ch_num == 6) {
  19.     returnPWM = channel_6_raw;
  20.   }
  21.   
  22.   return returnPWM;
  23. }
  24. //For DSM type receivers
  25. void serialEvent3(void)
  26. {
  27.   #if defined USE_DSM_RX
  28.     while (Serial3.available()) {
  29.         DSM.handleSerialEvent(Serial3.read(), micros());
  30.     }
  31.   #endif
  32. }
  33. //========================================================================================================================//
  34. //INTERRUPT SERVICE ROUTINES (for reading PWM and PPM)
  35. void getPPM() {
  36.   unsigned long dt_ppm;
  37.   int trig = digitalRead(PPM_Pin);
  38.   if (trig==1) { //Only care about rising edge
  39.     dt_ppm = micros() - time_ms;
  40.     time_ms = micros();
  41.    
  42.     if (dt_ppm > 5000) { //Waiting for long pulse to indicate a new pulse train has arrived
  43.       ppm_counter = 0;
  44.     }
  45.   
  46.     if (ppm_counter == 1) { //First pulse
  47.       channel_1_raw = dt_ppm;
  48.     }
  49.   
  50.     if (ppm_counter == 2) { //Second pulse
  51.       channel_2_raw = dt_ppm;
  52.     }
  53.   
  54.     if (ppm_counter == 3) { //Third pulse
  55.       channel_3_raw = dt_ppm;
  56.     }
  57.   
  58.     if (ppm_counter == 4) { //Fourth pulse
  59.       channel_4_raw = dt_ppm;
  60.     }
  61.   
  62.     if (ppm_counter == 5) { //Fifth pulse
  63.       channel_5_raw = dt_ppm;
  64.     }
  65.   
  66.     if (ppm_counter == 6) { //Sixth pulse
  67.       channel_6_raw = dt_ppm;
  68.     }
  69.    
  70.     ppm_counter = ppm_counter + 1;
  71.   }
  72. }
  73. void getCh1() {
  74.   int trigger = digitalRead(ch1Pin);
  75.   if(trigger == 1) {
  76.     rising_edge_start_1 = micros();
  77.   }
  78.   else if(trigger == 0) {
  79.     channel_1_raw = micros() - rising_edge_start_1;
  80.   }
  81. }
  82. void getCh2() {
  83.   int trigger = digitalRead(ch2Pin);
  84.   if(trigger == 1) {
  85.     rising_edge_start_2 = micros();
  86.   }
  87.   else if(trigger == 0) {
  88.     channel_2_raw = micros() - rising_edge_start_2;
  89.   }
  90. }
  91. void getCh3() {
  92.   int trigger = digitalRead(ch3Pin);
  93.   if(trigger == 1) {
  94.     rising_edge_start_3 = micros();
  95.   }
  96.   else if(trigger == 0) {
  97.     channel_3_raw = micros() - rising_edge_start_3;
  98.   }
  99. }
  100. void getCh4() {
  101.   int trigger = digitalRead(ch4Pin);
  102.   if(trigger == 1) {
  103.     rising_edge_start_4 = micros();
  104.   }
  105.   else if(trigger == 0) {
  106.     channel_4_raw = micros() - rising_edge_start_4;
  107.   }
  108. }
  109. void getCh5() {
  110.   int trigger = digitalRead(ch5Pin);
  111.   if(trigger == 1) {
  112.     rising_edge_start_5 = micros();
  113.   }
  114.   else if(trigger == 0) {
  115.     channel_5_raw = micros() - rising_edge_start_5;
  116.   }
  117. }
  118. void getCh6() {
  119.   int trigger = digitalRead(ch6Pin);
  120.   if(trigger == 1) {
  121.     rising_edge_start_6 = micros();
  122.   }
  123.   else if(trigger == 0) {
  124.     channel_6_raw = micros() - rising_edge_start_6;
  125.   }
  126. }
复制代码


回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 前天 10:21

【Arduino 动手做】3D打印的变形金刚飞天入地机器人

主程序

  1. //Project Start: 1/6/2020
  2. //Last Updated: 7/29/2022
  3. //Version: Beta 1.3
  4. //========================================================================================================================//
  5. //CREDITS + SPECIAL THANKS
  6. /*
  7. Some elements inspired by:
  8. http://www.brokking.net/ymfc-32_main.html
  9. Madgwick filter function adapted from:
  10. https://github.com/arduino-libraries/MadgwickAHRS
  11. MPU9250 implementation based on MPU9250 library by:
  12. brian.taylor@bolderflight.com
  13. http://www.bolderflight.com
  14. Thank you to:
  15. RcGroups 'jihlein' - IMU implementation overhaul + SBUS implementation.
  16. Everyone that sends me pictures and videos of your flying creations! -Nick
  17. */
  18. //========================================================================================================================//
  19. //                                                 USER-SPECIFIED DEFINES                                                 //                                                                 
  20. //========================================================================================================================//
  21. //Uncomment only one receiver type
  22. //#define USE_PWM_RX
  23. //#define USE_PPM_RX
  24. #define USE_SBUS_RX
  25. //#define USE_DSM_RX
  26. static const uint8_t num_DSM_channels = 16; //If using DSM RX, change this to match the number of transmitter channels you have
  27. //Uncomment only one IMU
  28. #define USE_MPU6050_I2C //Default
  29. //#define USE_MPU9250_SPI
  30. //Uncomment only one full scale gyro range (deg/sec)
  31. #define GYRO_250DPS //Default
  32. //#define GYRO_500DPS
  33. //#define GYRO_1000DPS
  34. //#define GYRO_2000DPS
  35. //Uncomment only one full scale accelerometer range (G's)
  36. #define ACCEL_2G //Default
  37. //#define ACCEL_4G
  38. //#define ACCEL_8G
  39. //#define ACCEL_16G
  40. //========================================================================================================================//
  41. //REQUIRED LIBRARIES (included with download in main sketch folder)
  42. #include <Wire.h>     //I2c communication
  43. #include <SPI.h>      //SPI communication
  44. #include <PWMServo.h> //Commanding any extra actuators, installed with teensyduino installer
  45. #include <FastLED.h>
  46. #if defined USE_SBUS_RX
  47.   #include "src/SBUS/SBUS.h"   //sBus interface
  48. #endif
  49. #if defined USE_DSM_RX
  50.   #include "src/DSMRX/DSMRX.h"  
  51. #endif
  52. #if defined USE_MPU6050_I2C
  53.   #include "src/MPU6050/MPU6050.h"
  54.   MPU6050 mpu6050;
  55. #elif defined USE_MPU9250_SPI
  56.   #include "src/MPU9250/MPU9250.h"
  57.   MPU9250 mpu9250(SPI2,36);
  58. #else
  59.   #error No MPU defined...
  60. #endif
  61. //========================================================================================================================//
  62. //Setup gyro and accel full scale value selection and scale factor
  63. #if defined USE_MPU6050_I2C
  64.   #define GYRO_FS_SEL_250    MPU6050_GYRO_FS_250
  65.   #define GYRO_FS_SEL_500    MPU6050_GYRO_FS_500
  66.   #define GYRO_FS_SEL_1000   MPU6050_GYRO_FS_1000
  67.   #define GYRO_FS_SEL_2000   MPU6050_GYRO_FS_2000
  68.   #define ACCEL_FS_SEL_2     MPU6050_ACCEL_FS_2
  69.   #define ACCEL_FS_SEL_4     MPU6050_ACCEL_FS_4
  70.   #define ACCEL_FS_SEL_8     MPU6050_ACCEL_FS_8
  71.   #define ACCEL_FS_SEL_16    MPU6050_ACCEL_FS_16
  72. #elif defined USE_MPU9250_SPI
  73.   #define GYRO_FS_SEL_250    mpu9250.GYRO_RANGE_250DPS
  74.   #define GYRO_FS_SEL_500    mpu9250.GYRO_RANGE_500DPS
  75.   #define GYRO_FS_SEL_1000   mpu9250.GYRO_RANGE_1000DPS                                                        
  76.   #define GYRO_FS_SEL_2000   mpu9250.GYRO_RANGE_2000DPS
  77.   #define ACCEL_FS_SEL_2     mpu9250.ACCEL_RANGE_2G
  78.   #define ACCEL_FS_SEL_4     mpu9250.ACCEL_RANGE_4G
  79.   #define ACCEL_FS_SEL_8     mpu9250.ACCEL_RANGE_8G
  80.   #define ACCEL_FS_SEL_16    mpu9250.ACCEL_RANGE_16G
  81. #endif
  82.   
  83. #if defined GYRO_250DPS
  84.   #define GYRO_SCALE GYRO_FS_SEL_250
  85.   #define GYRO_SCALE_FACTOR 131.0
  86. #elif defined GYRO_500DPS
  87.   #define GYRO_SCALE GYRO_FS_SEL_500
  88.   #define GYRO_SCALE_FACTOR 65.5
  89. #elif defined GYRO_1000DPS
  90.   #define GYRO_SCALE GYRO_FS_SEL_1000
  91.   #define GYRO_SCALE_FACTOR 32.8
  92. #elif defined GYRO_2000DPS
  93.   #define GYRO_SCALE GYRO_FS_SEL_2000
  94.   #define GYRO_SCALE_FACTOR 16.4
  95. #endif
  96. #if defined ACCEL_2G
  97.   #define ACCEL_SCALE ACCEL_FS_SEL_2
  98.   #define ACCEL_SCALE_FACTOR 16384.0
  99. #elif defined ACCEL_4G
  100.   #define ACCEL_SCALE ACCEL_FS_SEL_4
  101.   #define ACCEL_SCALE_FACTOR 8192.0
  102. #elif defined ACCEL_8G
  103.   #define ACCEL_SCALE ACCEL_FS_SEL_8
  104.   #define ACCEL_SCALE_FACTOR 4096.0
  105. #elif defined ACCEL_16G
  106.   #define ACCEL_SCALE ACCEL_FS_SEL_16
  107.   #define ACCEL_SCALE_FACTOR 2048.0
  108. #endif
  109. #define LED_PIN     12      // Pin connected to the LED strip
  110. #define NUM_LEDS    1     // Number of LEDs in the strip
  111. CRGB leds[NUM_LEDS];
  112. //========================================================================================================================//
  113. //                                               USER-SPECIFIED VARIABLES                                                 //                           
  114. //========================================================================================================================//
  115. //Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults:
  116. unsigned long channel_1_fs = 1000; //thro
  117. unsigned long channel_2_fs = 1000; //ail
  118. unsigned long channel_3_fs = 1000; //elev
  119. unsigned long channel_4_fs = 1000; //rudd
  120. unsigned long channel_5_fs = 1500; //gear, greater than 1500 = throttle cut
  121. unsigned long channel_6_fs = 1500; //aux1
  122. unsigned long mode_1_servo_1 = 177; //Servo 1 Mode 1 Value
  123. unsigned long mode_1_servo_2 = 180; //Servo 2 Mode 1 Value
  124. unsigned long mode_2_servo_1 = 2; //Servo 1 Mode 2 Value
  125. unsigned long mode_2_servo_2 = 2; //Servo 2 Mode 2 Value
  126. unsigned long mode_3_servo_1 = 2; //Servo 1 Mode 3 Value
  127. unsigned long mode_3_servo_2 = 2; //Servo 2 Mode 3 Value
  128. int mode=0;  //Mode 1 = Driving    Mode 2 = Landed     Mode 3 = Flying
  129. //Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing:
  130. float B_madgwick = 0.04;  //Madgwick filter parameter
  131. float B_accel = 0.14;     //Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2)
  132. float B_gyro = 0.1;       //Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17)
  133. float B_mag = 1.0;        //Magnetometer LP filter parameter
  134. //Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these
  135. float MagErrorX = 0.0;
  136. float MagErrorY = 0.0;
  137. float MagErrorZ = 0.0;
  138. float MagScaleX = 1.0;
  139. float MagScaleY = 1.0;
  140. float MagScaleZ = 1.0;
  141. //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error()
  142. float AccErrorX = 0.04;
  143. float AccErrorY = -0.05;
  144. float AccErrorZ = -0.04;
  145. float GyroErrorX = -3.76;
  146. float GyroErrorY = 0.66;
  147. float GyroErrorZ = -0.02;
  148. //Controller parameters (take note of defaults before modifying!):
  149. float i_limit = 25.0;     //Integrator saturation level, mostly for safety (default 25.0)
  150. float maxRoll = 30.0;     //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode
  151. float maxPitch = 30.0;    //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode
  152. float maxYaw = 160.0;     //Max yaw rate in deg/sec
  153. float Kp_roll_angle = 0.3;    //Roll P-gain - angle mode
  154. float Ki_roll_angle = 0.0;    //Roll I-gain - angle mode
  155. float Kd_roll_angle = 0.05;   //Roll D-gain - angle mode (has no effect on controlANGLE2)
  156. float B_loop_roll = 0.9;      //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1)
  157. float Kp_pitch_angle = 0.3;   //Pitch P-gain - angle mode
  158. float Ki_pitch_angle = 0.0;   //Pitch I-gain - angle mode
  159. float Kd_pitch_angle = 0.05;  //Pitch D-gain - angle mode (has no effect on controlANGLE2)
  160. float B_loop_pitch = 0.9;     //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1)
  161. float Kp_roll_rate = 0.15;    //Roll P-gain - rate mode
  162. float Ki_roll_rate = 0.2;     //Roll I-gain - rate mode
  163. float Kd_roll_rate = 0.0002;  //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!)
  164. float Kp_pitch_rate = 0.15;   //Pitch P-gain - rate mode
  165. float Ki_pitch_rate = 0.2;    //Pitch I-gain - rate mode
  166. float Kd_pitch_rate = 0.0002; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!)
  167. float Kp_yaw = 0.5;           //Yaw P-gain
  168. float Ki_yaw = 0.05;          //Yaw I-gain
  169. float Kd_yaw = 0.00015;       //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!)
  170. //========================================================================================================================//
  171. //                                                     DECLARE PINS                                                       //                           
  172. //========================================================================================================================//                                          
  173. //NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup
  174. //Radio:
  175. //Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3)
  176. const int ch1Pin = 15; //throttle
  177. const int ch2Pin = 16; //ail
  178. const int ch3Pin = 17; //ele
  179. const int ch4Pin = 20; //rudd
  180. const int ch5Pin = 21; //gear (throttle cut)
  181. const int ch6Pin = 22; //aux1 (free aux channel)
  182. const int PPM_Pin = 23;
  183. //OneShot125 ESC pin outputs:
  184. const int m1Pin = 0;
  185. const int m2Pin = 1;
  186. const int m3Pin = 2;
  187. const int m4Pin = 3;
  188. const int m5Pin = 4;
  189. const int m6Pin = 5;
  190. //PWM servo or ESC outputs:
  191. const int servo1Pin = 6;  // Front Servo
  192. const int servo2Pin = 7;  // Aft Servo
  193. const int servo3Pin = 8;  // Linear Actuator 1 Signal 1
  194. const int servo4Pin = 9;  // Linear Actuatot 1 Signal 2
  195. const int servo5Pin = 10; // Linear Actuator 2 Signal 1
  196. const int servo6Pin = 11; // Linear Actuatot 2 Signal 2
  197. const int servo7Pin = 12;
  198. PWMServo servo1;  //Create servo objects to control a servo or ESC with PWM
  199. PWMServo servo2;
  200. PWMServo servo3;
  201. PWMServo servo4;
  202. PWMServo servo5;
  203. PWMServo servo6;
  204. //PWMServo servo7;
  205. //========================================================================================================================//
  206. //DECLARE GLOBAL VARIABLES
  207. //General stuff
  208. float dt;
  209. unsigned long current_time, prev_time;
  210. unsigned long print_counter, serial_counter;
  211. unsigned long blink_counter, blink_delay;
  212. bool blinkAlternate;
  213. //Radio communication:
  214. unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm;
  215. unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev;
  216. #if defined USE_SBUS_RX
  217.   SBUS sbus(Serial5);
  218.   uint16_t sbusChannels[16];
  219.   bool sbusFailSafe;
  220.   bool sbusLostFrame;
  221. #endif
  222. #if defined USE_DSM_RX
  223.   DSM1024 DSM;
  224. #endif
  225. //IMU:
  226. float AccX, AccY, AccZ;
  227. float AccX_prev, AccY_prev, AccZ_prev;
  228. float GyroX, GyroY, GyroZ;
  229. float GyroX_prev, GyroY_prev, GyroZ_prev;
  230. float MagX, MagY, MagZ;
  231. float MagX_prev, MagY_prev, MagZ_prev;
  232. float roll_IMU, pitch_IMU, yaw_IMU;
  233. float roll_IMU_prev, pitch_IMU_prev;
  234. float q0 = 1.0f; //Initialize quaternion for madgwick filter
  235. float q1 = 0.0f;
  236. float q2 = 0.0f;
  237. float q3 = 0.0f;
  238. //Normalized desired state:
  239. float thro_des, roll_des, pitch_des, yaw_des;
  240. float roll_passthru, pitch_passthru, yaw_passthru;
  241. //Controller:
  242. float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, roll_PID = 0;
  243. float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch, pitch_PID = 0;
  244. float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw, yaw_PID = 0;
  245. //Mixer
  246. float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled;
  247. int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM;
  248. float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled;
  249. int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM;
  250. //Flight status
  251. bool armedFly = false;
  252. //========================================================================================================================//
  253. //                                                      VOID SETUP                                                        //                           
  254. //========================================================================================================================//
  255. void setup() {
  256.   Serial.begin(500000); //USB serial
  257.   delay(500);
  258.   
  259.   //Initialize all pins
  260.   pinMode(13, OUTPUT); //Pin 13 LED blinker on board, do not modify
  261.   pinMode(m1Pin, OUTPUT);
  262.   pinMode(m2Pin, OUTPUT);
  263.   pinMode(m3Pin, OUTPUT);
  264.   pinMode(m4Pin, OUTPUT);
  265.   pinMode(m5Pin, OUTPUT);
  266.   pinMode(m6Pin, OUTPUT);
  267.   servo1.attach(servo1Pin, 760, 2400); //Pin, min PWM value, max PWM value
  268.   servo2.attach(servo2Pin, 900, 2030);
  269.   servo3.attach(servo3Pin, 900, 2100);
  270.   servo4.attach(servo4Pin, 900, 2100);
  271.   servo5.attach(servo5Pin, 900, 2100);
  272.   servo6.attach(servo6Pin, 900, 2100);
  273.   //servo7.attach(servo7Pin, 900, 2100);
  274.   FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS);
  275.   FastLED.clear();  // Ensure strip starts off
  276.   FastLED.show();
  277.   //Set built in LED to turn on to signal startup
  278.   digitalWrite(13, HIGH);
  279.   delay(5);
  280.   //Initialize radio communication
  281.   radioSetup();
  282.   
  283.   //Set radio channels to default (safe) values before entering main loop
  284.   channel_1_pwm = channel_1_fs;
  285.   channel_2_pwm = channel_2_fs;
  286.   channel_3_pwm = channel_3_fs;
  287.   channel_4_pwm = channel_4_fs;
  288.   channel_5_pwm = channel_5_fs;
  289.   channel_6_pwm = channel_6_fs;
  290.   //Initialize IMU communication
  291.   IMUinit();
  292.   delay(5);
  293.   //Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up
  294.   //calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever.
  295.   //Arm servo channels
  296.   servo1.write(0); //Command servo angle from 0-180 degrees (1000 to 2000 PWM)
  297.   servo2.write(0); //Set these to 90 for servos if you do not want them to briefly max out on startup
  298. //  servo3.write(0); //Keep these at 0 if you are using servo outputs for motors
  299. //  servo4.write(0);
  300. //  servo5.write(0);
  301. //  servo6.write(0);
  302. //  servo7.write(0);
  303. //  
  304.   delay(5);
  305.   //calibrateESCs(); //PROPS OFF. Uncomment this to calibrate your ESCs by setting throttle stick to max, powering on, and lowering throttle to zero after the beeps
  306.   //Code will not proceed past here if this function is uncommented!
  307.   //Arm OneShot125 motors
  308.   m1_command_PWM = 125; //Command OneShot125 ESC from 125 to 250us pulse length
  309.   m2_command_PWM = 125;
  310.   m3_command_PWM = 125;
  311.   m4_command_PWM = 125;
  312.   m5_command_PWM = 125;
  313.   m6_command_PWM = 125;
  314.   armMotors(); //Loop over commandMotors() until ESCs happily arm
  315.   
  316.   //Indicate entering main loop with 3 quick blinks
  317.   setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms)
  318.   //If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations)
  319.   //calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section
  320. }
  321. //========================================================================================================================//
  322. //                                                       MAIN LOOP                                                        //                           
  323. //========================================================================================================================//
  324.                                                   
  325. void loop() {
  326.   //Keep track of what time it is and how much time has elapsed since the last loop
  327.   prev_time = current_time;      
  328.   current_time = micros();      
  329.   dt = (current_time - prev_time)/1000000.0;
  330.   loopBlink(); //Indicate we are in main loop with short blink every 1.5 seconds
  331.   //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE:
  332.   //printRadioData();     //Prints radio pwm values (expected: 1000 to 2000)
  333.   //printDesiredState();  //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
  334.   //printGyroData();      //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
  335.   //printAccelData();     //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
  336.   //printMagData();       //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300)
  337.   //printRollPitchYaw();  //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level)
  338.   //printPIDoutput();     //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
  339.   //printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250)
  340.   //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180)
  341.   //printLoopRate();      //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
  342.   // Get arming status
  343.   armedStatus(); //Check if the throttle cut is off and throttle is low.
  344.   //Get vehicle state
  345.   getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise
  346.   Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees)
  347.   //Compute desired state
  348.   getDesState(); //Convert raw commands to normalized values based on saturated control limits
  349.   
  350.   //PID Controller - SELECT ONE:
  351.   controlANGLE(); //Stabilize on angle setpoint
  352.   //controlANGLE2(); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first!
  353.   //controlRATE(); //Stabilize on rate setpoint
  354.   //Actuator mixing and scaling to PWM values
  355.   controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here
  356.   modeControl();
  357.   scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library)
  358.   //Throttle cut check
  359.   throttleCut(); //Directly sets motor commands to low based on state of ch5
  360.   //Command actuators
  361.   commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol
  362.   servo1.write(s1_command_PWM); // Front Lifting Servo//Writes PWM value to servo object
  363.   servo2.write(s2_command_PWM); // Aft Lifting Servo
  364.   //servo3.write(s3_command_PWM);
  365.   //servo4.write(s4_command_PWM);
  366.   //servo5.write(s5_command_PWM);
  367.   //servo6.write(s6_command_PWM);
  368.   //servo7.write(s7_command_PWM);
  369.    
  370.   //Get vehicle commands for next loop iteration
  371.   getCommands(); //Pulls current available radio commands
  372.   failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup
  373.   //Regulate loop rate
  374.   loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default
  375. }
  376. //========================================================================================================================//
  377. //                                                      FUNCTIONS                                                         //                           
  378. //========================================================================================================================//
  379. void modeControl(){
  380.   if (channel_6_pwm<1100){
  381.     mode=1;
  382.   }
  383.   else if ((channel_6_pwm>1400) & (channel_6_pwm<1600)){
  384.     mode=2;
  385.   }
  386.   else if (channel_6_pwm>1900){
  387.     mode=3;
  388.   }
  389. if (mode==0){
  390.   mode=2;
  391. }
  392. else if(mode==1){
  393.   digitalWrite(servo3Pin,LOW); //Actuator 1 Signal 1
  394.   digitalWrite(servo4Pin,HIGH); //Actuator 1 Signal 1
  395.   digitalWrite(servo5Pin,LOW); //Actuator 2 Signal 1
  396.   digitalWrite(servo6Pin,HIGH); //Actuator 2 Signal 1
  397.   s1_command_PWM=mode_1_servo_1;
  398.   s2_command_PWM=mode_1_servo_2;
  399.   m1_command_scaled=0;
  400.   m2_command_scaled=0;
  401.   m3_command_scaled=0;
  402.   m4_command_scaled=0;
  403.   leds[0] = CRGB(255, 255, 255);
  404. }
  405. else if(mode==2){
  406.   digitalWrite(servo3Pin,LOW); //Actuator 1 Signal 1
  407.   digitalWrite(servo4Pin,HIGH); //Actuator 1 Signal 1
  408.   digitalWrite(servo5Pin,LOW); //Actuator 2 Signal 1
  409.   digitalWrite(servo6Pin,HIGH); //Actuator 2 Signal 1
  410.   s1_command_PWM=mode_2_servo_1;
  411.   s2_command_PWM=mode_2_servo_2;
  412.   m5_command_scaled=0.5;
  413.   m6_command_scaled=0.5;
  414.   m1_command_scaled=0;
  415.   m2_command_scaled=0;
  416.   m3_command_scaled=0;
  417.   m4_command_scaled=0;
  418.   leds[0] = CRGB(1, 1, 255);
  419. }
  420. else if(mode==3){
  421.   digitalWrite(servo3Pin,HIGH); //Actuator 1 Signal 1
  422.   digitalWrite(servo4Pin,LOW); //Actuator 1 Signal 1
  423.   digitalWrite(servo5Pin,HIGH); //Actuator 2 Signal 1
  424.   digitalWrite(servo6Pin,LOW); //Actuator 2 Signal 1
  425.   s1_command_PWM=mode_3_servo_1;
  426.   s2_command_PWM=mode_3_servo_2;
  427.   m5_command_scaled=0.5;
  428.   m6_command_scaled=0.5;
  429.   
  430.   leds[0] = CRGB(255, 255, 255);
  431.   
  432. }
  433. FastLED.show();
  434.   
  435. }
  436. void controlMixer() {
  437.   //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration
  438.   /*
  439.    * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired
  440.    * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors
  441.    * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has
  442.    * normalized (0 to 1) thro_des command for throttle control. Can also apply direct unstabilized commands from the transmitter with
  443.    * roll_passthru, pitch_passthru, and yaw_passthu. mX_command_scaled and sX_command scaled variables are used in scaleCommands()
  444.    * in preparation to be sent to the motor ESCs and servos.
  445.    *
  446.    *Relevant variables:
  447.    *thro_des - direct thottle control
  448.    *roll_PID, pitch_PID, yaw_PID - stabilized axis variables
  449.    *roll_passthru, pitch_passthru, yaw_passthru - direct unstabilized command passthrough
  450.    *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement
  451.    */
  452.    
  453.   //Quad mixing - EXAMPLE
  454.   m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; //Front Left
  455.   m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; //Front Right
  456.   m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; //Back Right
  457.   m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; //Back Left
  458.   m5_command_scaled = ((float(channel_2_pwm-1000)/1000))-(((float(channel_1_pwm-1000)/1000))-0.5)*0.2;
  459.   m6_command_scaled = ((float(channel_2_pwm-1000)/1000))+(((float(channel_1_pwm-1000)/1000))-0.5)*0.2;;
  460.   if(abs(0.5-m5_command_scaled)<0.04){
  461.     m5_command_scaled=0.5;
  462.   }
  463.   if(abs(0.5-m6_command_scaled)<0.04){
  464.     m6_command_scaled=0.5;
  465.   }
  466.   //0.5 is centered servo, 0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle
  467.   //s1_command_scaled = 0;
  468.   //s2_command_scaled = 0;
  469.   //s3_command_scaled = 0;
  470.   //s4_command_scaled = 0;
  471.   //s5_command_scaled = 0;
  472.   //s6_command_scaled = 0;
  473.   //s7_command_scaled = 0;
  474. }
  475. void armedStatus() {
  476.   //DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight.
  477.   if ((channel_5_pwm > 1500) && (channel_3_pwm < 1050)) {
  478.     armedFly = true;
  479.   }
  480. }
  481. void IMUinit() {
  482.   //DESCRIPTION: Initialize IMU
  483.   /*
  484.    * Don't worry about how this works.
  485.    */
  486.   #if defined USE_MPU6050_I2C
  487.     Wire.begin();
  488.     Wire.setClock(1000000); //Note this is 2.5 times the spec sheet 400 kHz max...
  489.    
  490.     mpu6050.initialize();
  491.    
  492.     if (mpu6050.testConnection() == false) {
  493.       Serial.println("MPU6050 initialization unsuccessful");
  494.       Serial.println("Check MPU6050 wiring or try cycling power");
  495.       while(1) {}
  496.     }
  497.     //From the reset state all registers should be 0x00, so we should be at
  498.     //max sample rate with digital low pass filter(s) off.  All we need to
  499.     //do is set the desired fullscale ranges
  500.     mpu6050.setFullScaleGyroRange(GYRO_SCALE);
  501.     mpu6050.setFullScaleAccelRange(ACCEL_SCALE);
  502.    
  503.   #elif defined USE_MPU9250_SPI
  504.     int status = mpu9250.begin();   
  505.     if (status < 0) {
  506.       Serial.println("MPU9250 initialization unsuccessful");
  507.       Serial.println("Check MPU9250 wiring or try cycling power");
  508.       Serial.print("Status: ");
  509.       Serial.println(status);
  510.       while(1) {}
  511.     }
  512.     //From the reset state all registers should be 0x00, so we should be at
  513.     //max sample rate with digital low pass filter(s) off.  All we need to
  514.     //do is set the desired fullscale ranges
  515.     mpu9250.setGyroRange(GYRO_SCALE);
  516.     mpu9250.setAccelRange(ACCEL_SCALE);
  517.     mpu9250.setMagCalX(MagErrorX, MagScaleX);
  518.     mpu9250.setMagCalY(MagErrorY, MagScaleY);
  519.     mpu9250.setMagCalZ(MagErrorZ, MagScaleZ);
  520.     mpu9250.setSrd(0); //sets gyro and accel read to 1khz, magnetometer read to 100hz
  521.   #endif
  522. }
  523. void getIMUdata() {
  524.   //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data
  525.   /*
  526.    * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ.
  527.    * These values are scaled according to the IMU datasheet to put them into correct units of g's, deg/sec, and uT. A simple first-order
  528.    * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut
  529.    * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in
  530.    * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally,
  531.    * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings.
  532.    */
  533.   int16_t AcX,AcY,AcZ,GyX,GyY,GyZ,MgX,MgY,MgZ;
  534.   #if defined USE_MPU6050_I2C
  535.     mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
  536.   #elif defined USE_MPU9250_SPI
  537.     mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
  538.   #endif
  539. //Accelerometer
  540.   AccX = AcX / ACCEL_SCALE_FACTOR; //G's
  541.   AccY = AcY / ACCEL_SCALE_FACTOR;
  542.   AccZ = AcZ / ACCEL_SCALE_FACTOR;
  543.   //Correct the outputs with the calculated error values
  544.   AccX = AccX - AccErrorX;
  545.   AccY = AccY - AccErrorY;
  546.   AccZ = AccZ - AccErrorZ;
  547.   //LP filter accelerometer data
  548.   AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX;
  549.   AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY;
  550.   AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ;
  551.   AccX_prev = AccX;
  552.   AccY_prev = AccY;
  553.   AccZ_prev = AccZ;
  554.   //Gyro
  555.   GyroX = GyX / GYRO_SCALE_FACTOR; //deg/sec
  556.   GyroY = GyY / GYRO_SCALE_FACTOR;
  557.   GyroZ = GyZ / GYRO_SCALE_FACTOR;
  558.   //Correct the outputs with the calculated error values
  559.   GyroX = GyroX - GyroErrorX;
  560.   GyroY = GyroY - GyroErrorY;
  561.   GyroZ = GyroZ - GyroErrorZ;
  562.   //LP filter gyro data
  563.   GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX;
  564.   GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY;
  565.   GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ;
  566.   GyroX_prev = GyroX;
  567.   GyroY_prev = GyroY;
  568.   GyroZ_prev = GyroZ;
  569.   //Magnetometer
  570.   MagX = MgX/6.0; //uT
  571.   MagY = MgY/6.0;
  572.   MagZ = MgZ/6.0;
  573.   //Correct the outputs with the calculated error values
  574.   MagX = (MagX - MagErrorX)*MagScaleX;
  575.   MagY = (MagY - MagErrorY)*MagScaleY;
  576.   MagZ = (MagZ - MagErrorZ)*MagScaleZ;
  577.   //LP filter magnetometer data
  578.   MagX = (1.0 - B_mag)*MagX_prev + B_mag*MagX;
  579.   MagY = (1.0 - B_mag)*MagY_prev + B_mag*MagY;
  580.   MagZ = (1.0 - B_mag)*MagZ_prev + B_mag*MagZ;
  581.   MagX_prev = MagX;
  582.   MagY_prev = MagY;
  583.   MagZ_prev = MagZ;
  584. }
  585. void calculate_IMU_error() {
  586.   //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface
  587.   /*
  588.    * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and
  589.    * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the
  590.    * measurement.
  591.    */
  592.   int16_t AcX,AcY,AcZ,GyX,GyY,GyZ,MgX,MgY,MgZ;
  593.   AccErrorX = 0.0;
  594.   AccErrorY = 0.0;
  595.   AccErrorZ = 0.0;
  596.   GyroErrorX = 0.0;
  597.   GyroErrorY= 0.0;
  598.   GyroErrorZ = 0.0;
  599.   
  600.   //Read IMU values 12000 times
  601.   int c = 0;
  602.   while (c < 12000) {
  603.     #if defined USE_MPU6050_I2C
  604.       mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
  605.     #elif defined USE_MPU9250_SPI
  606.       mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
  607.     #endif
  608.    
  609.     AccX  = AcX / ACCEL_SCALE_FACTOR;
  610.     AccY  = AcY / ACCEL_SCALE_FACTOR;
  611.     AccZ  = AcZ / ACCEL_SCALE_FACTOR;
  612.     GyroX = GyX / GYRO_SCALE_FACTOR;
  613.     GyroY = GyY / GYRO_SCALE_FACTOR;
  614.     GyroZ = GyZ / GYRO_SCALE_FACTOR;
  615.    
  616.     //Sum all readings
  617.     AccErrorX  = AccErrorX + AccX;
  618.     AccErrorY  = AccErrorY + AccY;
  619.     AccErrorZ  = AccErrorZ + AccZ;
  620.     GyroErrorX = GyroErrorX + GyroX;
  621.     GyroErrorY = GyroErrorY + GyroY;
  622.     GyroErrorZ = GyroErrorZ + GyroZ;
  623.     c++;
  624.   }
  625.   //Divide the sum by 12000 to get the error value
  626.   AccErrorX  = AccErrorX / c;
  627.   AccErrorY  = AccErrorY / c;
  628.   AccErrorZ  = AccErrorZ / c - 1.0;
  629.   GyroErrorX = GyroErrorX / c;
  630.   GyroErrorY = GyroErrorY / c;
  631.   GyroErrorZ = GyroErrorZ / c;
  632.   Serial.print("float AccErrorX = ");
  633.   Serial.print(AccErrorX);
  634.   Serial.println(";");
  635.   Serial.print("float AccErrorY = ");
  636.   Serial.print(AccErrorY);
  637.   Serial.println(";");
  638.   Serial.print("float AccErrorZ = ");
  639.   Serial.print(AccErrorZ);
  640.   Serial.println(";");
  641.   
  642.   Serial.print("float GyroErrorX = ");
  643.   Serial.print(GyroErrorX);
  644.   Serial.println(";");
  645.   Serial.print("float GyroErrorY = ");
  646.   Serial.print(GyroErrorY);
  647.   Serial.println(";");
  648.   Serial.print("float GyroErrorZ = ");
  649.   Serial.print(GyroErrorZ);
  650.   Serial.println(";");
  651.   Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup.");
  652. }
  653. void calibrateAttitude() {
  654.   //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators
  655.   //Assuming vehicle is powered up on level surface!
  656.   /*
  657.    * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds
  658.    * to boot.
  659.    */
  660.   //Warm up IMU and madgwick filter in simulated main loop
  661.   for (int i = 0; i <= 10000; i++) {
  662.     prev_time = current_time;      
  663.     current_time = micros();      
  664.     dt = (current_time - prev_time)/1000000.0;
  665.     getIMUdata();
  666.     Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt);
  667.     loopRate(2000); //do not exceed 2000Hz
  668.   }
  669. }
  670. void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq) {
  671.   //DESCRIPTION: Attitude estimation through sensor fusion - 9DOF
  672.   /*
  673.    * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation.
  674.    * Don't worry about the math. There is a tunable parameter B_madgwick in the user specified variable section which basically
  675.    * adjusts the weight of gyro data in the state estimate. Higher beta leads to noisier estimate, lower
  676.    * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU,
  677.    * pitch_IMU, and yaw_IMU variables which are in degrees. If magnetometer data is not available, this function calls Madgwick6DOF() instead.
  678.    */
  679.   float recipNorm;
  680.   float s0, s1, s2, s3;
  681.   float qDot1, qDot2, qDot3, qDot4;
  682.   float hx, hy;
  683.   float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  684.   //use 6DOF algorithm if MPU6050 is being used
  685.   #if defined USE_MPU6050_I2C
  686.     Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq);
  687.     return;
  688.   #endif
  689.   
  690.   //Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
  691.   if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
  692.     Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq);
  693.     return;
  694.   }
  695.   //Convert gyroscope degrees/sec to radians/sec
  696.   gx *= 0.0174533f;
  697.   gy *= 0.0174533f;
  698.   gz *= 0.0174533f;
  699.   //Rate of change of quaternion from gyroscope
  700.   qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
  701.   qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
  702.   qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
  703.   qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
  704.   //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  705.   if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
  706.     //Normalise accelerometer measurement
  707.     recipNorm = invSqrt(ax * ax + ay * ay + az * az);
  708.     ax *= recipNorm;
  709.     ay *= recipNorm;
  710.     az *= recipNorm;
  711.     //Normalise magnetometer measurement
  712.     recipNorm = invSqrt(mx * mx + my * my + mz * mz);
  713.     mx *= recipNorm;
  714.     my *= recipNorm;
  715.     mz *= recipNorm;
  716.     //Auxiliary variables to avoid repeated arithmetic
  717.     _2q0mx = 2.0f * q0 * mx;
  718.     _2q0my = 2.0f * q0 * my;
  719.     _2q0mz = 2.0f * q0 * mz;
  720.     _2q1mx = 2.0f * q1 * mx;
  721.     _2q0 = 2.0f * q0;
  722.     _2q1 = 2.0f * q1;
  723.     _2q2 = 2.0f * q2;
  724.     _2q3 = 2.0f * q3;
  725.     _2q0q2 = 2.0f * q0 * q2;
  726.     _2q2q3 = 2.0f * q2 * q3;
  727.     q0q0 = q0 * q0;
  728.     q0q1 = q0 * q1;
  729.     q0q2 = q0 * q2;
  730.     q0q3 = q0 * q3;
  731.     q1q1 = q1 * q1;
  732.     q1q2 = q1 * q2;
  733.     q1q3 = q1 * q3;
  734.     q2q2 = q2 * q2;
  735.     q2q3 = q2 * q3;
  736.     q3q3 = q3 * q3;
  737.     //Reference direction of Earth's magnetic field
  738.     hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
  739.     hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
  740.     _2bx = sqrtf(hx * hx + hy * hy);
  741.     _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
  742.     _4bx = 2.0f * _2bx;
  743.     _4bz = 2.0f * _2bz;
  744.     //Gradient decent algorithm corrective step
  745.     s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
  746.     s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
  747.     s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
  748.     s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
  749.     recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
  750.     s0 *= recipNorm;
  751.     s1 *= recipNorm;
  752.     s2 *= recipNorm;
  753.     s3 *= recipNorm;
  754.     //Apply feedback step
  755.     qDot1 -= B_madgwick * s0;
  756.     qDot2 -= B_madgwick * s1;
  757.     qDot3 -= B_madgwick * s2;
  758.     qDot4 -= B_madgwick * s3;
  759.   }
  760.   //Integrate rate of change of quaternion to yield quaternion
  761.   q0 += qDot1 * invSampleFreq;
  762.   q1 += qDot2 * invSampleFreq;
  763.   q2 += qDot3 * invSampleFreq;
  764.   q3 += qDot4 * invSampleFreq;
  765.   //Normalize quaternion
  766.   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  767.   q0 *= recipNorm;
  768.   q1 *= recipNorm;
  769.   q2 *= recipNorm;
  770.   q3 *= recipNorm;
  771.   
  772.   //compute angles - NWU
  773.   roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; //degrees
  774.   pitch_IMU = -asin(constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees
  775.   yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees
  776. }
  777. void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) {
  778.   //DESCRIPTION: Attitude estimation through sensor fusion - 6DOF
  779.   /*
  780.    * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not
  781.    * available (for example when using the recommended MPU6050 IMU for the default setup).
  782.    */
  783.   float recipNorm;
  784.   float s0, s1, s2, s3;
  785.   float qDot1, qDot2, qDot3, qDot4;
  786.   float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
  787.   //Convert gyroscope degrees/sec to radians/sec
  788.   gx *= 0.0174533f;
  789.   gy *= 0.0174533f;
  790.   gz *= 0.0174533f;
  791.   //Rate of change of quaternion from gyroscope
  792.   qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
  793.   qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
  794.   qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
  795.   qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
  796.   //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  797.   if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
  798.     //Normalise accelerometer measurement
  799.     recipNorm = invSqrt(ax * ax + ay * ay + az * az);
  800.     ax *= recipNorm;
  801.     ay *= recipNorm;
  802.     az *= recipNorm;
  803.     //Auxiliary variables to avoid repeated arithmetic
  804.     _2q0 = 2.0f * q0;
  805.     _2q1 = 2.0f * q1;
  806.     _2q2 = 2.0f * q2;
  807.     _2q3 = 2.0f * q3;
  808.     _4q0 = 4.0f * q0;
  809.     _4q1 = 4.0f * q1;
  810.     _4q2 = 4.0f * q2;
  811.     _8q1 = 8.0f * q1;
  812.     _8q2 = 8.0f * q2;
  813.     q0q0 = q0 * q0;
  814.     q1q1 = q1 * q1;
  815.     q2q2 = q2 * q2;
  816.     q3q3 = q3 * q3;
  817.     //Gradient decent algorithm corrective step
  818.     s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
  819.     s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
  820.     s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
  821.     s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
  822.     recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude
  823.     s0 *= recipNorm;
  824.     s1 *= recipNorm;
  825.     s2 *= recipNorm;
  826.     s3 *= recipNorm;
  827.     //Apply feedback step
  828.     qDot1 -= B_madgwick * s0;
  829.     qDot2 -= B_madgwick * s1;
  830.     qDot3 -= B_madgwick * s2;
  831.     qDot4 -= B_madgwick * s3;
  832.   }
  833.   //Integrate rate of change of quaternion to yield quaternion
  834.   q0 += qDot1 * invSampleFreq;
  835.   q1 += qDot2 * invSampleFreq;
  836.   q2 += qDot3 * invSampleFreq;
  837.   q3 += qDot4 * invSampleFreq;
  838.   //Normalise quaternion
  839.   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  840.   q0 *= recipNorm;
  841.   q1 *= recipNorm;
  842.   q2 *= recipNorm;
  843.   q3 *= recipNorm;
  844.   //Compute angles
  845.   roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; //degrees
  846.   pitch_IMU = -asin(constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees
  847.   yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees
  848. }
  849. void getDesState() {
  850.   //DESCRIPTION: Normalizes desired control values to appropriate values
  851.   /*
  852.    * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw
  853.    * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range.
  854.    * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec
  855.    * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and
  856.    * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer().
  857.    */
  858.   thro_des = (channel_3_pwm - 1000.0)/1000.0; //Between 0 and 1
  859.   roll_des = (channel_1_pwm - 1500.0)/500.0; //Between -1 and 1
  860.   pitch_des = (channel_2_pwm - 1500.0)/500.0; //Between -1 and 1
  861.   yaw_des = (channel_4_pwm - 1500.0)/500.0; //Between -1 and 1
  862.   roll_passthru = roll_des/2.0; //Between -0.5 and 0.5
  863.   pitch_passthru = pitch_des/2.0; //Between -0.5 and 0.5
  864.   yaw_passthru = yaw_des/2.0; //Between -0.5 and 0.5
  865.   
  866.   //Constrain within normalized bounds
  867.   thro_des = constrain(thro_des, 0.0, 1.0); //Between 0 and 1
  868.   roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //Between -maxRoll and +maxRoll
  869.   pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //Between -maxPitch and +maxPitch
  870.   yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //Between -maxYaw and +maxYaw
  871.   roll_passthru = constrain(roll_passthru, -0.5, 0.5);
  872.   pitch_passthru = constrain(pitch_passthru, -0.5, 0.5);
  873.   yaw_passthru = constrain(yaw_passthru, -0.5, 0.5);
  874. }
  875. void controlANGLE() {
  876.   //DESCRIPTION: Computes control commands based on state error (angle)
  877.   /*
  878.    * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in
  879.    * getDesState(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features
  880.    * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent
  881.    * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until
  882.    * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0
  883.    * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I
  884.    * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which
  885.    * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in controlMixer().
  886.    */
  887.   
  888.   //Roll
  889.   error_roll = roll_des - roll_IMU;
  890.   integral_roll = integral_roll_prev + error_roll*dt;
  891.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  892.     integral_roll = 0;
  893.   }
  894.   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  895.   derivative_roll = GyroX;
  896.   roll_PID = 0.01*(Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range
  897.   //Pitch
  898.   error_pitch = pitch_des - pitch_IMU;
  899.   integral_pitch = integral_pitch_prev + error_pitch*dt;
  900.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  901.     integral_pitch = 0;
  902.   }
  903.   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  904.   derivative_pitch = GyroY;
  905.   pitch_PID = .01*(Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range
  906.   //Yaw, stablize on rate from GyroZ
  907.   error_yaw = yaw_des - GyroZ;
  908.   integral_yaw = integral_yaw_prev + error_yaw*dt;
  909.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  910.     integral_yaw = 0;
  911.   }
  912.   integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  913.   derivative_yaw = (error_yaw - error_yaw_prev)/dt;
  914.   yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range
  915.   //Update roll variables
  916.   integral_roll_prev = integral_roll;
  917.   //Update pitch variables
  918.   integral_pitch_prev = integral_pitch;
  919.   //Update yaw variables
  920.   error_yaw_prev = error_yaw;
  921.   integral_yaw_prev = integral_yaw;
  922. }
  923. void controlANGLE2() {
  924.   //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme
  925.   /*
  926.    * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup.
  927.    * See the documentation for tuning this controller.
  928.    */
  929.   //Outer loop - PID on angle
  930.   float roll_des_ol, pitch_des_ol;
  931.   //Roll
  932.   error_roll = roll_des - roll_IMU;
  933.   integral_roll_ol = integral_roll_prev_ol + error_roll*dt;
  934.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  935.     integral_roll_ol = 0;
  936.   }
  937.   integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  938.   derivative_roll = (roll_IMU - roll_IMU_prev)/dt;
  939.   roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol;// - Kd_roll_angle*derivative_roll;
  940.   //Pitch
  941.   error_pitch = pitch_des - pitch_IMU;
  942.   integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt;
  943.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  944.     integral_pitch_ol = 0;
  945.   }
  946.   integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup
  947.   derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt;
  948.   pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol;// - Kd_pitch_angle*derivative_pitch;
  949.   //Apply loop gain, constrain, and LP filter for artificial damping
  950.   float Kl = 30.0;
  951.   roll_des_ol = Kl*roll_des_ol;
  952.   pitch_des_ol = Kl*pitch_des_ol;
  953.   roll_des_ol = constrain(roll_des_ol, -240.0, 240.0);
  954.   pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0);
  955.   roll_des_ol = (1.0 - B_loop_roll)*roll_des_prev + B_loop_roll*roll_des_ol;
  956.   pitch_des_ol = (1.0 - B_loop_pitch)*pitch_des_prev + B_loop_pitch*pitch_des_ol;
  957.   //Inner loop - PID on rate
  958.   //Roll
  959.   error_roll = roll_des_ol - GyroX;
  960.   integral_roll_il = integral_roll_prev_il + error_roll*dt;
  961.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  962.     integral_roll_il = 0;
  963.   }
  964.   integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  965.   derivative_roll = (error_roll - error_roll_prev)/dt;
  966.   roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range
  967.   //Pitch
  968.   error_pitch = pitch_des_ol - GyroY;
  969.   integral_pitch_il = integral_pitch_prev_il + error_pitch*dt;
  970.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  971.     integral_pitch_il = 0;
  972.   }
  973.   integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  974.   derivative_pitch = (error_pitch - error_pitch_prev)/dt;
  975.   pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range
  976.   
  977.   //Yaw
  978.   error_yaw = yaw_des - GyroZ;
  979.   integral_yaw = integral_yaw_prev + error_yaw*dt;
  980.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  981.     integral_yaw = 0;
  982.   }
  983.   integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  984.   derivative_yaw = (error_yaw - error_yaw_prev)/dt;
  985.   yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range
  986.   
  987.   //Update roll variables
  988.   integral_roll_prev_ol = integral_roll_ol;
  989.   integral_roll_prev_il = integral_roll_il;
  990.   error_roll_prev = error_roll;
  991.   roll_IMU_prev = roll_IMU;
  992.   roll_des_prev = roll_des_ol;
  993.   //Update pitch variables
  994.   integral_pitch_prev_ol = integral_pitch_ol;
  995.   integral_pitch_prev_il = integral_pitch_il;
  996.   error_pitch_prev = error_pitch;
  997.   pitch_IMU_prev = pitch_IMU;
  998.   pitch_des_prev = pitch_des_ol;
  999.   //Update yaw variables
  1000.   error_yaw_prev = error_yaw;
  1001.   integral_yaw_prev = integral_yaw;
  1002. }
  1003. void controlRATE() {
  1004.   //DESCRIPTION: Computes control commands based on state error (rate)
  1005.   /*
  1006.    * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading.
  1007.    */
  1008.   //Roll
  1009.   error_roll = roll_des - GyroX;
  1010.   integral_roll = integral_roll_prev + error_roll*dt;
  1011.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  1012.     integral_roll = 0;
  1013.   }
  1014.   integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  1015.   derivative_roll = (error_roll - error_roll_prev)/dt;
  1016.   roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range
  1017.   //Pitch
  1018.   error_pitch = pitch_des - GyroY;
  1019.   integral_pitch = integral_pitch_prev + error_pitch*dt;
  1020.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  1021.     integral_pitch = 0;
  1022.   }
  1023.   integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  1024.   derivative_pitch = (error_pitch - error_pitch_prev)/dt;
  1025.   pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range
  1026.   //Yaw, stablize on rate from GyroZ
  1027.   error_yaw = yaw_des - GyroZ;
  1028.   integral_yaw = integral_yaw_prev + error_yaw*dt;
  1029.   if (channel_1_pwm < 1060) {   //Don't let integrator build if throttle is too low
  1030.     integral_yaw = 0;
  1031.   }
  1032.   integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup
  1033.   derivative_yaw = (error_yaw - error_yaw_prev)/dt;
  1034.   yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range
  1035.   //Update roll variables
  1036.   error_roll_prev = error_roll;
  1037.   integral_roll_prev = integral_roll;
  1038.   GyroX_prev = GyroX;
  1039.   //Update pitch variables
  1040.   error_pitch_prev = error_pitch;
  1041.   integral_pitch_prev = integral_pitch;
  1042.   GyroY_prev = GyroY;
  1043.   //Update yaw variables
  1044.   error_yaw_prev = error_yaw;
  1045.   integral_yaw_prev = integral_yaw;
  1046. }
  1047. void scaleCommands() {
  1048.   //DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol
  1049.   /*
  1050.    * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from
  1051.    * the mixer function are scaled to 0-180 for the servo library using standard PWM.
  1052.    * mX_command_PWM are updated here which are used to command the motors in commandMotors(). sX_command_PWM are updated
  1053.    * which are used to command the servos.
  1054.    */
  1055.   //Scaled to 125us - 250us for oneshot125 protocol
  1056.   m1_command_PWM = m1_command_scaled*125 + 125;
  1057.   m2_command_PWM = m2_command_scaled*125 + 125;
  1058.   m3_command_PWM = m3_command_scaled*125 + 125;
  1059.   m4_command_PWM = m4_command_scaled*125 + 125;
  1060.   m5_command_PWM = -m5_command_scaled*125 + 250;
  1061.   m6_command_PWM = -m6_command_scaled*125 + 250;
  1062.   //Constrain commands to motors within oneshot125 bounds
  1063.   m1_command_PWM = constrain(m1_command_PWM, 125, 250);
  1064.   m2_command_PWM = constrain(m2_command_PWM, 125, 250);
  1065.   m3_command_PWM = constrain(m3_command_PWM, 125, 250);
  1066.   m4_command_PWM = constrain(m4_command_PWM, 125, 250);
  1067.   m5_command_PWM = constrain(m5_command_PWM, 125, 250);
  1068.   m6_command_PWM = constrain(m6_command_PWM, 125, 250);
  1069.   //Scaled to 0-180 for servo library
  1070.   //s1_command_PWM = s1_command_scaled*180;
  1071.   //s2_command_PWM = s2_command_scaled*180;
  1072.   s3_command_PWM = s3_command_scaled*180;
  1073.   s4_command_PWM = s4_command_scaled*180;
  1074.   s5_command_PWM = s5_command_scaled*180;
  1075.   s6_command_PWM = s6_command_scaled*180;
  1076.   s7_command_PWM = s7_command_scaled*180;
  1077.   //Constrain commands to servos within servo library bounds
  1078.   //s1_command_PWM = constrain(s1_command_PWM, 0, 180);
  1079.   //s2_command_PWM = constrain(s2_command_PWM, 0, 180);
  1080.   s3_command_PWM = constrain(s3_command_PWM, 0, 180);
  1081.   s4_command_PWM = constrain(s4_command_PWM, 0, 180);
  1082.   s5_command_PWM = constrain(s5_command_PWM, 0, 180);
  1083.   s6_command_PWM = constrain(s6_command_PWM, 0, 180);
  1084.   s7_command_PWM = constrain(s7_command_PWM, 0, 180);
  1085. }
  1086. void getCommands() {
  1087.   //DESCRIPTION: Get raw PWM values for every channel from the radio
  1088.   /*
  1089.    * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of
  1090.    * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which
  1091.    * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly.
  1092.    * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise.
  1093.    */
  1094.   #if defined USE_PPM_RX || defined USE_PWM_RX
  1095.     channel_1_pwm = getRadioPWM(1);
  1096.     channel_2_pwm = getRadioPWM(2);
  1097.     channel_3_pwm = getRadioPWM(3);
  1098.     channel_4_pwm = getRadioPWM(4);
  1099.     channel_5_pwm = getRadioPWM(5);
  1100.     channel_6_pwm = getRadioPWM(6);
  1101.    
  1102.   #elif defined USE_SBUS_RX
  1103.     if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame))
  1104.     {
  1105.       //sBus scaling below is for Taranis-Plus and X4R-SB
  1106.       float scale = 0.615;  
  1107.       float bias  = 895.0;
  1108.       channel_1_pwm = sbusChannels[0] * scale + bias;
  1109.       channel_2_pwm = sbusChannels[1] * scale + bias;
  1110.       channel_3_pwm = sbusChannels[2] * scale + bias;
  1111.       channel_4_pwm = sbusChannels[3] * scale + bias;
  1112.       channel_5_pwm = sbusChannels[4] * scale + bias;
  1113.       channel_6_pwm = sbusChannels[5] * scale + bias;
  1114.     }
  1115.   #elif defined USE_DSM_RX
  1116.     if (DSM.timedOut(micros())) {
  1117.         //Serial.println("*** DSM RX TIMED OUT ***");
  1118.     }
  1119.     else if (DSM.gotNewFrame()) {
  1120.         uint16_t values[num_DSM_channels];
  1121.         DSM.getChannelValues(values, num_DSM_channels);
  1122.         channel_1_pwm = values[0];
  1123.         channel_2_pwm = values[1];
  1124.         channel_3_pwm = values[2];
  1125.         channel_4_pwm = values[3];
  1126.         channel_5_pwm = values[4];
  1127.         channel_6_pwm = values[5];
  1128.     }
  1129.   #endif
  1130.   
  1131.   //Low-pass the critical commands and update previous values
  1132.   float b = 0.7; //Lower=slower, higher=noiser
  1133.   channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm;
  1134.   channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm;
  1135.   channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm;
  1136.   channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm;
  1137.   channel_1_pwm_prev = channel_1_pwm;
  1138.   channel_2_pwm_prev = channel_2_pwm;
  1139.   channel_3_pwm_prev = channel_3_pwm;
  1140.   channel_4_pwm_prev = channel_4_pwm;
  1141. }
  1142. void failSafe() {
  1143.   //DESCRIPTION: If radio gives garbage values, set all commands to default values
  1144.   /*
  1145.    * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of
  1146.    * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio
  1147.    * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands
  1148.    * channel_x_pwm are set to default failsafe values specified in the setup. Comment out this function when troubleshooting
  1149.    * your radio connection in case any extreme values are triggering this function to overwrite the printed variables.
  1150.    */
  1151.   unsigned minVal = 800;
  1152.   unsigned maxVal = 2200;
  1153.   int check1 = 0;
  1154.   int check2 = 0;
  1155.   int check3 = 0;
  1156.   int check4 = 0;
  1157.   int check5 = 0;
  1158.   int check6 = 0;
  1159.   //Triggers for failure criteria
  1160.   if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1;
  1161.   if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1;
  1162.   if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1;
  1163.   if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1;
  1164.   if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1;
  1165.   if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1;
  1166.   //If any failures, set to default failsafe values
  1167.   if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) {
  1168.     channel_1_pwm = channel_1_fs;
  1169.     channel_2_pwm = channel_2_fs;
  1170.     channel_3_pwm = channel_3_fs;
  1171.     channel_4_pwm = channel_4_fs;
  1172.     channel_5_pwm = channel_5_fs;
  1173.     channel_6_pwm = channel_6_fs;
  1174.   }
  1175. }
  1176. void commandMotors() {
  1177.   //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol
  1178.   /*
  1179.    * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being
  1180.    * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future.
  1181.    */
  1182.   int wentLow = 0;
  1183.   int pulseStart, timer;
  1184.   int flagM1 = 0;
  1185.   int flagM2 = 0;
  1186.   int flagM3 = 0;
  1187.   int flagM4 = 0;
  1188.   int flagM5 = 0;
  1189.   int flagM6 = 0;
  1190.   
  1191.   //Write all motor pins high
  1192.   digitalWrite(m1Pin, HIGH);
  1193.   digitalWrite(m2Pin, HIGH);
  1194.   digitalWrite(m3Pin, HIGH);
  1195.   digitalWrite(m4Pin, HIGH);
  1196.   digitalWrite(m5Pin, HIGH);
  1197.   digitalWrite(m6Pin, HIGH);
  1198.   pulseStart = micros();
  1199.   //Write each motor pin low as correct pulse length is reached
  1200.   while (wentLow < 6 ) { //Keep going until final (6th) pulse is finished, then done
  1201.     timer = micros();
  1202.     if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) {
  1203.       digitalWrite(m1Pin, LOW);
  1204.       wentLow = wentLow + 1;
  1205.       flagM1 = 1;
  1206.     }
  1207.     if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) {
  1208.       digitalWrite(m2Pin, LOW);
  1209.       wentLow = wentLow + 1;
  1210.       flagM2 = 1;
  1211.     }
  1212.     if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) {
  1213.       digitalWrite(m3Pin, LOW);
  1214.       wentLow = wentLow + 1;
  1215.       flagM3 = 1;
  1216.     }
  1217.     if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) {
  1218.       digitalWrite(m4Pin, LOW);
  1219.       wentLow = wentLow + 1;
  1220.       flagM4 = 1;
  1221.     }
  1222.     if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) {
  1223.       digitalWrite(m5Pin, LOW);
  1224.       wentLow = wentLow + 1;
  1225.       flagM5 = 1;
  1226.     }
  1227.     if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) {
  1228.       digitalWrite(m6Pin, LOW);
  1229.       wentLow = wentLow + 1;
  1230.       flagM6 = 1;
  1231.     }
  1232.   }
  1233. }
  1234. void armMotors() {
  1235.   //DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup()
  1236.   /*  
  1237.    *  Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors()
  1238.    *  function is used in the main loop. Ensures motors arm within the void setup() where there are some delays
  1239.    *  for other processes that sometimes prevent motors from arming.
  1240.    */
  1241.   for (int i = 0; i <= 50; i++) {
  1242.     commandMotors();
  1243.     delay(2);
  1244.   }
  1245. }
  1246. void calibrateESCs() {
  1247.   //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place.
  1248.   /*  
  1249.    *  Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can
  1250.    *  power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be
  1251.    *  uncommented when performing an ESC calibration.
  1252.    */
  1253.    while (true) {
  1254.       prev_time = current_time;      
  1255.       current_time = micros();      
  1256.       dt = (current_time - prev_time)/1000000.0;
  1257.    
  1258.       digitalWrite(13, HIGH); //LED on to indicate we are not in main loop
  1259.       getCommands(); //Pulls current available radio commands
  1260.       failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup
  1261.       getDesState(); //Convert raw commands to normalized values based on saturated control limits
  1262.       getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise
  1263.       Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees)
  1264.       getDesState(); //Convert raw commands to normalized values based on saturated control limits
  1265.       
  1266.       m1_command_scaled = thro_des;
  1267.       m2_command_scaled = thro_des;
  1268.       m3_command_scaled = thro_des;
  1269.       m4_command_scaled = thro_des;
  1270.       m5_command_scaled = thro_des;
  1271.       m6_command_scaled = thro_des;
  1272.       s1_command_scaled = thro_des;
  1273.       s2_command_scaled = thro_des;
  1274.       s3_command_scaled = thro_des;
  1275.       s4_command_scaled = thro_des;
  1276.       s5_command_scaled = thro_des;
  1277.       s6_command_scaled = thro_des;
  1278.       s7_command_scaled = thro_des;
  1279.       scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library)
  1280.    
  1281.       //throttleCut(); //Directly sets motor commands to low based on state of ch5
  1282.       
  1283.       servo1.write(s1_command_PWM);
  1284.       servo2.write(s2_command_PWM);
  1285.       servo3.write(s3_command_PWM);
  1286.       servo4.write(s4_command_PWM);
  1287.       servo5.write(s5_command_PWM);
  1288.       servo6.write(s6_command_PWM);
  1289.       //servo7.write(s7_command_PWM);
  1290.       commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol
  1291.       
  1292.       //printRadioData(); //Radio pwm values (expected: 1000 to 2000)
  1293.       
  1294.       loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default
  1295.    }
  1296. }
  1297. float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){
  1298.   //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time
  1299.   /*  
  1300.    *  Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency
  1301.    *  and linearly interpolates that param variable between the maximum and minimum bounds. This function can be called in controlMixer()
  1302.    *  and high/low states can be determined by monitoring the state of an auxillarly radio channel. For example, if channel_6_pwm is being
  1303.    *  monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical
  1304.    *  statements in order to fade controller gains, for example between the two dynamic configurations. The 'state' (1 or 0) can be used
  1305.    *  to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel.
  1306.    *  
  1307.    */
  1308.   float diffParam = (param_max - param_min)/(fadeTime*loopFreq); //Difference to add or subtract from param for each loop iteration for desired fadeTime
  1309.   if (state == 1) { //Maximum param bound desired, increase param by diffParam for each loop iteration
  1310.     param = param + diffParam;
  1311.   }
  1312.   else if (state == 0) { //Minimum param bound desired, decrease param by diffParam for each loop iteration
  1313.     param = param - diffParam;
  1314.   }
  1315.   param = constrain(param, param_min, param_max); //Constrain param within max bounds
  1316.   
  1317.   return param;
  1318. }
  1319. float floatFaderLinear2(float param, float param_des, float param_lower, float param_upper, float fadeTime_up, float fadeTime_down, int loopFreq){
  1320.   //DESCRIPTION: Linearly fades a float type variable from its current value to the desired value, up or down
  1321.   /*  
  1322.    *  Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency
  1323.    *  and linearly fades that param variable up or down to the desired value. This function can be called in controlMixer()
  1324.    *  to fade up or down between flight modes monitored by an auxillary radio channel. For example, if channel_6_pwm is being
  1325.    *  monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical
  1326.    *  statements in order to fade controller gains, for example between the two dynamic configurations.
  1327.    *  
  1328.    */
  1329.   if (param > param_des) { //Need to fade down to get to desired
  1330.     float diffParam = (param_upper - param_des)/(fadeTime_down*loopFreq);
  1331.     param = param - diffParam;
  1332.   }
  1333.   else if (param < param_des) { //Need to fade up to get to desired
  1334.     float diffParam = (param_des - param_lower)/(fadeTime_up*loopFreq);
  1335.     param = param + diffParam;
  1336.   }
  1337.   param = constrain(param, param_lower, param_upper); //Constrain param within max bounds
  1338.   
  1339.   return param;
  1340. }
  1341. void switchRollYaw(int reverseRoll, int reverseYaw) {
  1342.   //DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations
  1343.   /*
  1344.    * Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively.
  1345.    * Reversing of the roll or yaw axis may be needed when switching between the two for some dynamic configurations. Inputs of 1, 1 does not
  1346.    * reverse either of them, while -1, 1 will reverse the output corresponding to the new roll axis.
  1347.    * This function may be replaced in the future by a function that switches the IMU data instead (so that angle can also be estimated with the
  1348.    * IMU tilted 90 degrees from default level).
  1349.    */
  1350.   float switch_holder;
  1351.   switch_holder = yaw_des;
  1352.   yaw_des = reverseYaw*roll_des;
  1353.   roll_des = reverseRoll*switch_holder;
  1354. }
  1355. void throttleCut() {
  1356.   //DESCRIPTION: Directly set actuator outputs to minimum value if triggered
  1357.   /*
  1358.       Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is
  1359.       minimum for oneshot125 protocol, 0 is minimum for standard PWM servo library used) if channel 5 is high. This is the last function
  1360.       called before commandMotors() is called so that the last thing checked is if the user is giving permission to command
  1361.       the motors to anything other than minimum value. Safety first.
  1362.       channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED)
  1363.       channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED)
  1364.   */
  1365.   if ((channel_5_pwm < 1500) || (armedFly == false)) {
  1366.     armedFly = false;
  1367.     m1_command_PWM = 120;
  1368.     m2_command_PWM = 120;
  1369.     m3_command_PWM = 120;
  1370.     m4_command_PWM = 120;
  1371.     m5_command_PWM = 187.5;
  1372.     m6_command_PWM = 187.5;
  1373.     //Uncomment if using servo PWM variables to control motor ESCs
  1374.     //s1_command_PWM = 0;
  1375.     //s2_command_PWM = 0;
  1376.     //s3_command_PWM = 0;
  1377.     //s4_command_PWM = 0;
  1378.     //s5_command_PWM = 0;
  1379.     //s6_command_PWM = 0;
  1380.     //s7_command_PWM = 0;
  1381.   }
  1382. }
  1383. void calibrateMagnetometer() {
  1384.   #if defined USE_MPU9250_SPI
  1385.     float success;
  1386.     Serial.println("Beginning magnetometer calibration in");
  1387.     Serial.println("3...");
  1388.     delay(1000);
  1389.     Serial.println("2...");
  1390.     delay(1000);
  1391.     Serial.println("1...");
  1392.     delay(1000);
  1393.     Serial.println("Rotate the IMU about all axes until complete.");
  1394.     Serial.println(" ");
  1395.     success = mpu9250.calibrateMag();
  1396.     if(success) {
  1397.       Serial.println("Calibration Successful!");
  1398.       Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:");
  1399.       Serial.print("float MagErrorX = ");
  1400.       Serial.print(mpu9250.getMagBiasX_uT());
  1401.       Serial.println(";");
  1402.       Serial.print("float MagErrorY = ");
  1403.       Serial.print(mpu9250.getMagBiasY_uT());
  1404.       Serial.println(";");
  1405.       Serial.print("float MagErrorZ = ");
  1406.       Serial.print(mpu9250.getMagBiasZ_uT());
  1407.       Serial.println(";");
  1408.       Serial.print("float MagScaleX = ");
  1409.       Serial.print(mpu9250.getMagScaleFactorX());
  1410.       Serial.println(";");
  1411.       Serial.print("float MagScaleY = ");
  1412.       Serial.print(mpu9250.getMagScaleFactorY());
  1413.       Serial.println(";");
  1414.       Serial.print("float MagScaleZ = ");
  1415.       Serial.print(mpu9250.getMagScaleFactorZ());
  1416.       Serial.println(";");
  1417.       Serial.println(" ");
  1418.       Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed.");
  1419.     }
  1420.     else {
  1421.       Serial.println("Calibration Unsuccessful. Please reset the board and try again.");
  1422.     }
  1423.   
  1424.     while(1); //Halt code so it won't enter main loop until this function commented out
  1425.   #endif
  1426.   Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer.");
  1427.   while(1); //Halt code so it won't enter main loop until this function commented out
  1428. }
  1429. void loopRate(int freq) {
  1430.   //DESCRIPTION: Regulate main loop rate to specified frequency in Hz
  1431.   /*
  1432.    * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the
  1433.    * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until
  1434.    * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to
  1435.    * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations
  1436.    * and remain above 2kHz, without needing to retune all of our filtering parameters.
  1437.    */
  1438.   float invFreq = 1.0/freq*1000000.0;
  1439.   unsigned long checker = micros();
  1440.   
  1441.   //Sit in loop until appropriate time has passed
  1442.   while (invFreq > (checker - current_time)) {
  1443.     checker = micros();
  1444.   }
  1445. }
  1446. void loopBlink() {
  1447.   //DESCRIPTION: Blink LED on board to indicate main loop is running
  1448.   /*
  1449.    * It looks cool.
  1450.    */
  1451.   if (current_time - blink_counter > blink_delay) {
  1452.     blink_counter = micros();
  1453.     digitalWrite(13, blinkAlternate); //Pin 13 is built in LED
  1454.    
  1455.     if (blinkAlternate == 1) {
  1456.       blinkAlternate = 0;
  1457.       blink_delay = 100000;
  1458.       }
  1459.     else if (blinkAlternate == 0) {
  1460.       blinkAlternate = 1;
  1461.       blink_delay = 2000000;
  1462.       }
  1463.   }
  1464. }
  1465. void setupBlink(int numBlinks,int upTime, int downTime) {
  1466.   //DESCRIPTION: Simple function to make LED on board blink as desired
  1467.   for (int j = 1; j<= numBlinks; j++) {
  1468.     digitalWrite(13, LOW);
  1469.     delay(downTime);
  1470.     digitalWrite(13, HIGH);
  1471.     delay(upTime);
  1472.   }
  1473. }
  1474. void printRadioData() {
  1475.   if (current_time - print_counter > 10000) {
  1476.     print_counter = micros();
  1477.     Serial.print(F(" CH1:"));
  1478.     Serial.print(channel_1_pwm);
  1479.     Serial.print(F(" CH2:"));
  1480.     Serial.print(channel_2_pwm);
  1481.     Serial.print(F(" CH3:"));
  1482.     Serial.print(channel_3_pwm);
  1483.     Serial.print(F(" CH4:"));
  1484.     Serial.print(channel_4_pwm);
  1485.     Serial.print(F(" CH5:"));
  1486.     Serial.print(channel_5_pwm);
  1487.     Serial.print(F(" CH6:"));
  1488.     Serial.println(channel_6_pwm);
  1489.   }
  1490. }
  1491. void printDesiredState() {
  1492.   if (current_time - print_counter > 10000) {
  1493.     print_counter = micros();
  1494.     Serial.print(F("thro_des:"));
  1495.     Serial.print(thro_des);
  1496.     Serial.print(F(" roll_des:"));
  1497.     Serial.print(roll_des);
  1498.     Serial.print(F(" pitch_des:"));
  1499.     Serial.print(pitch_des);
  1500.     Serial.print(F(" yaw_des:"));
  1501.     Serial.println(yaw_des);
  1502.   }
  1503. }
  1504. void printGyroData() {
  1505.   if (current_time - print_counter > 10000) {
  1506.     print_counter = micros();
  1507.     Serial.print(F("GyroX:"));
  1508.     Serial.print(GyroX);
  1509.     Serial.print(F(" GyroY:"));
  1510.     Serial.print(GyroY);
  1511.     Serial.print(F(" GyroZ:"));
  1512.     Serial.println(GyroZ);
  1513.   }
  1514. }
  1515. void printAccelData() {
  1516.   if (current_time - print_counter > 10000) {
  1517.     print_counter = micros();
  1518.     Serial.print(F("AccX:"));
  1519.     Serial.print(AccX);
  1520.     Serial.print(F(" AccY:"));
  1521.     Serial.print(AccY);
  1522.     Serial.print(F(" AccZ:"));
  1523.     Serial.println(AccZ);
  1524.   }
  1525. }
  1526. void printMagData() {
  1527.   if (current_time - print_counter > 10000) {
  1528.     print_counter = micros();
  1529.     Serial.print(F("MagX:"));
  1530.     Serial.print(MagX);
  1531.     Serial.print(F(" MagY:"));
  1532.     Serial.print(MagY);
  1533.     Serial.print(F(" MagZ:"));
  1534.     Serial.println(MagZ);
  1535.   }
  1536. }
  1537. void printRollPitchYaw() {
  1538.   if (current_time - print_counter > 10000) {
  1539.     print_counter = micros();
  1540.     Serial.print(F("roll:"));
  1541.     Serial.print(roll_IMU);
  1542.     Serial.print(F(" pitch:"));
  1543.     Serial.print(pitch_IMU);
  1544.     Serial.print(F(" yaw:"));
  1545.     Serial.println(yaw_IMU);
  1546.   }
  1547. }
  1548. void printPIDoutput() {
  1549.   if (current_time - print_counter > 10000) {
  1550.     print_counter = micros();
  1551.     Serial.print(F("roll_PID:"));
  1552.     Serial.print(roll_PID);
  1553.     Serial.print(F(" pitch_PID:"));
  1554.     Serial.print(pitch_PID);
  1555.     Serial.print(F(" yaw_PID:"));
  1556.     Serial.println(yaw_PID);
  1557.   }
  1558. }
  1559. void printMotorCommands() {
  1560.   if (current_time - print_counter > 10000) {
  1561.     print_counter = micros();
  1562.     Serial.print(F("m1_command:"));
  1563.     Serial.print(m1_command_PWM);
  1564.     Serial.print(F(" m2_command:"));
  1565.     Serial.print(m2_command_PWM);
  1566.     Serial.print(F(" m3_command:"));
  1567.     Serial.print(m3_command_PWM);
  1568.     Serial.print(F(" m4_command:"));
  1569.     Serial.print(m4_command_PWM);
  1570.     Serial.print(F(" m5_command:"));
  1571.     Serial.print(m5_command_PWM);
  1572.     Serial.print(F(" m6_command:"));
  1573.     Serial.println(m6_command_PWM);
  1574.   }
  1575. }
  1576. void printServoCommands() {
  1577.   if (current_time - print_counter > 10000) {
  1578.     print_counter = micros();
  1579.     Serial.print(F("s1_command:"));
  1580.     Serial.print(s1_command_PWM);
  1581.     Serial.print(F(" s2_command:"));
  1582.     Serial.print(s2_command_PWM);
  1583.     Serial.print(F(" s3_command:"));
  1584.     Serial.print(s3_command_PWM);
  1585.     Serial.print(F(" s4_command:"));
  1586.     Serial.print(s4_command_PWM);
  1587.     Serial.print(F(" s5_command:"));
  1588.     Serial.print(s5_command_PWM);
  1589.     Serial.print(F(" s6_command:"));
  1590.     Serial.print(s6_command_PWM);
  1591.     Serial.print(F(" s7_command:"));
  1592.     Serial.println(s7_command_PWM);
  1593.   }
  1594. }
  1595. void printLoopRate() {
  1596.   if (current_time - print_counter > 10000) {
  1597.     print_counter = micros();
  1598.     Serial.print(F("dt:"));
  1599.     Serial.println(dt*1000000.0);
  1600.   }
  1601. }
  1602. //=========================================================================================//
  1603. //HELPER FUNCTIONS
  1604. float invSqrt(float x) {
  1605.   //Fast inverse sqrt for madgwick filter
  1606.   /*
  1607.   float halfx = 0.5f * x;
  1608.   float y = x;
  1609.   long i = *(long*)&y;
  1610.   i = 0x5f3759df - (i>>1);
  1611.   y = *(float*)&i;
  1612.   y = y * (1.5f - (halfx * y * y));
  1613.   y = y * (1.5f - (halfx * y * y));
  1614.   return y;
  1615.   */
  1616.   /*
  1617.   //alternate form:
  1618.   unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
  1619.   float tmp = *(float*)&i;
  1620.   float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
  1621.   return y;
  1622.   */
  1623.   return 1.0/sqrtf(x); //Teensy is fast enough to just take the compute penalty lol suck it arduino nano
  1624. }
复制代码


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4 备案 沪公网安备31011502402448

© 2013-2025 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail