Diff: ZTC.cpp

Revision:

3:197b748a397a

Child:

8:79ca11e0129d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ZTC.cpp	Wed Jun 22 04:35:18 2016 +0000
@@ -0,0 +1,56 @@
+#include "ZTC.h"
+#include "SensorFusion.h"
+
+///ZTC
+float K_h[3] = {1636.8, 80.0, 0.0};///{1261.8, 70, 0};///{2088, 28.65, 0};///
+float K_m[3] = {3458.8, 117.0, 0.0};///{1643.7, 80.2, 0};///{2536.8, 100, 0};///
+float K_l[3] = {2143.8, 0.0, 0.0};
+float K_s[3] = {0,0,0};
+float K_hphi[3] = {0.100359*3.0, 0.034599*3.0, -0.05653*3.0};///{10.0359, 3.4599, -5.653};///
+float K_mphi[3]= {3.7165*0.03, 2.6747*0.03, -2.0436*0.03};///{0.037165*6.0, 0.026747*6.0, -0.020436*6.0};///
+float u = 0.0;
+float phi_hat = 0.0;
+float phi_hat_old = 0.0;
+float phi_hat_aw = 0.0;
+float dphi_hat= 0.0;
+float roll_ref = 0.0;
+float gamma_ref = 0.0;
+float gamma_rad = 0.0;
+float gamma_rad_old = 0.0;
+int8_t gamma_degree = 0;
+
+void calc_PD(float* K_in, float phi_hat_in)
+{
+    u = -K_in[0]*(roll_angle - phi_hat_in - roll_ref) - K_in[1]*droll_angle;
+}
+
+void calc_Phi(float* K_phi_in)
+{
+    dphi_hat = -K_phi_in[0]*(roll_angle - phi_hat - roll_ref) - K_phi_in[1]*droll_angle - K_phi_in[2]*(gamma_rad-gamma_ref);
+    phi_hat = phi_hat_old + (dphi_hat + phi_hat_aw)*sample_time;
+}
+
+void calc_Gamma(float u_in, float alpha, float multr)
+{
+    gamma_rad = lpf(u_in,gamma_rad_old,alpha)/multr - GAMMA_OFFSET; ///38
+    gamma_rad_old = gamma_rad;
+}
+
+void anti_wdup(void)
+{
+    if(phi_hat > PHI_HAT_UB)
+    {
+        phi_hat_aw = K_aw * (PHI_HAT_UB - phi_hat);
+        phi_hat = PHI_HAT_UB;
+    }
+    else if(phi_hat < PHI_HAT_LB)
+    {
+        phi_hat_aw = K_aw * (PHI_HAT_LB - phi_hat);
+        phi_hat = PHI_HAT_LB;
+    }
+    else
+    {
+        phi_hat_aw = 0.0;
+    }
+    phi_hat_old = phi_hat;
+}
\ No newline at end of file