From 71b856f024bc574a48d83e6deee0c60a5be53e77 Mon Sep 17 00:00:00 2001
From: but0n <but0n@icloud.com>
Date: Wed, 5 Oct 2016 21:33:55 +0800
Subject: [PATCH] [MOD]: PID test plathform

---
 .../STM32F10x_StdPeriph_Driver/inc/pid.h      | 15 +++++++++
 .../STM32F10x_StdPeriph_Driver/src/pid.c      | 26 ++++++++++++++++
 src/main.c                                    | 31 ++-----------------
 3 files changed, 43 insertions(+), 29 deletions(-)
 create mode 100644 libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/pid.h
 create mode 100644 libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c

diff --git a/libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/pid.h b/libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/pid.h
new file mode 100644
index 0000000..400cd2e
--- /dev/null
+++ b/libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/inc/pid.h
@@ -0,0 +1,15 @@
+#pragma once
+#ifndef PID_H_
+#define PID_H_
+
+extern float g_Yaw, g_Pitch, g_Roll;     //eular
+
+#define K_P 2.57f //0.257 * 0.83 0.255
+#define K_I 0
+#define K_D 0
+#define SUM_ERRO_MAX 900
+#define SUM_ERRO_MIN -900
+
+void pid(float setPoint, float d);
+
+#endif
diff --git a/libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c b/libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c
new file mode 100644
index 0000000..8154b3d
--- /dev/null
+++ b/libs/STM32_USB-FS-Device_Lib_V4.0.0/Libraries/STM32F10x_StdPeriph_Driver/src/pid.c
@@ -0,0 +1,26 @@
+#include "pid.h"
+#include "stm32f10x.h"
+#include "motor.h"
+
+float g_iErro, g_sumErro = 0;
+void pid(float setPoint, float d) {
+    g_iErro = g_Pitch - setPoint;    //计算当前误差
+    g_sumErro += g_iErro;       //对误差进行积分
+
+    if(g_sumErro > SUM_ERRO_MAX) g_sumErro = SUM_ERRO_MAX;  //积分限幅
+    else if(g_sumErro < SUM_ERRO_MIN) g_sumErro = SUM_ERRO_MIN;
+
+    short resu = (short)(g_iErro * K_P + g_sumErro * K_I + d * K_D);  //PID输出
+
+    if((MOTOR2 + resu) > MOTOR_MAX)
+        MOTOR2 = MOTOR_MAX;
+    else if((MOTOR2 + resu) < MOTOR_LOW)
+        MOTOR2 = MOTOR_LOW;
+    else MOTOR2 += resu;
+
+    if((MOTOR4 + resu) > MOTOR_MAX)
+        MOTOR4 = MOTOR_MAX;
+    else if((MOTOR4 + resu) < MOTOR_LOW)
+        MOTOR4 = MOTOR_LOW;
+    else MOTOR4 += resu;
+}
diff --git a/src/main.c b/src/main.c
index 36a94b7..4dce3b0 100644
--- a/src/main.c
+++ b/src/main.c
@@ -5,6 +5,7 @@
 #include "motor.h"
 #include "uart.h"
 #include "wifi.h"
+#include "pid.h"
 
 #define Kp      100.0f      //比例增益支配率(常量)
 #define Ki      0.002f      //积分增益支配率
@@ -12,36 +13,8 @@
 
 float g_q0 = 1, g_q1 = 0, g_q2 = 0, g_q3 = 0;   //Quaternion
 float g_exInt = 0, g_eyInt = 0, g_ezInt = 0;
-float g_Yaw, g_Pitch, g_Roll;     //eular
+float g_Yaw, g_Pitch, g_Roll;
 
-#define K_P 2.57f //0.257 * 0.83 0.255
-#define K_I 0
-#define K_D 0
-#define SUM_ERRO_MAX 900
-#define SUM_ERRO_MIN -900
-
-float g_iErro, g_sumErro = 0;
-void pid(float setPoint, float d) {
-    g_iErro = g_Pitch - setPoint;    //计算当前误差
-    g_sumErro += g_iErro;       //对误差进行积分
-
-    if(g_sumErro > SUM_ERRO_MAX) g_sumErro = SUM_ERRO_MAX;  //积分限幅
-    else if(g_sumErro < SUM_ERRO_MIN) g_sumErro = SUM_ERRO_MIN;
-
-    short resu = (short)(g_iErro * K_P + g_sumErro * K_I + d * K_D);  //PID输出
-
-    if((MOTOR2 + resu) > MOTOR_MAX)
-        MOTOR2 = MOTOR_MAX;
-    else if((MOTOR2 + resu) < MOTOR_LOW)
-        MOTOR2 = MOTOR_LOW;
-    else MOTOR2 += resu;
-
-    if((MOTOR4 + resu) > MOTOR_MAX)
-        MOTOR4 = MOTOR_MAX;
-    else if((MOTOR4 + resu) < MOTOR_LOW)
-        MOTOR4 = MOTOR_LOW;
-    else MOTOR4 += resu;
-}
 
 //ms
 void delay(volatile unsigned int count) {