Revision 2:9e07bed8e8ed, committed 2018-11-21

Comitter:

IgneousGuikas

Date:

Wed Nov 21 10:46:29 2018 +0000

Parent:

1:2abfa02e99d5

Commit message:

Teste;

Changed in this revision

--- a/AttitudeEstimator/AttitudeEstimator.cpp	Tue Oct 09 18:00:51 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -23,11 +23,11 @@
         p_bias += imu.gx;
         q_bias += imu.gy;
         r_bias += imu.gz;
-        wait_ms(5);
+        wait_ms(dt);
     }
-    p_bias /= 200;
-    q_bias /= 200;
-    r_bias /= 200;
+    p_bias /= 200.0f;
+    q_bias /= 200.0f;
+    r_bias /= 200.0f;
 }
 
 // Estimate Euler angles (rad) and angular velocity (rad/s)
@@ -38,8 +38,8 @@
     estimate_accel();
     estimate_gyro();
     
-    phi = rho*phi_accel + (1.0f-rho)*phi_gyro;
-    theta = rho*theta_accel + (1.0f-rho)*theta_gyro;
+    phi = rho_att*phi_accel + (1.0f - rho_att)*phi_gyro;
+    theta = rho_att*theta_accel + (1.0f - rho_att)*theta_gyro;
     psi = psi_gyro;
 }
 
@@ -48,7 +48,7 @@
 {
     phi_accel = atan2(-imu.ay,-imu.az);
     
-    if(imu.az >= 0){
+    if(imu.az >= 0.0f){
         theta_accel = atan2(imu.ax,-sqrt(pow(imu.ay,2) + pow(imu.az,2)));
     }else{
         theta_accel = atan2(imu.ax,sqrt(pow(imu.ay,2) + pow(imu.az,2)));
@@ -67,12 +67,12 @@
     psi_gyro = psi + ((sin(phi)/cos(theta))*q + (cos(phi)/cos(theta))*r)*dt;
     
     if(phi_gyro > pi){
-        phi_gyro -= 2*pi;
+        phi_gyro -= 2.0f*pi;
     }
     if(theta_gyro > pi){
-        theta_gyro -= 2*pi;
+        theta_gyro -= 2.0f*pi;
     }
     if(psi_gyro > pi){
-        psi_gyro -= 2*pi;
+        psi_gyro -= 2.0f*pi;
     }
 }
\ No newline at end of file

--- a/CrazyflieController.h	Tue Oct 09 18:00:51 2018 +0000
+++ b/CrazyflieController.h	Wed Nov 21 10:46:29 2018 +0000
@@ -4,7 +4,10 @@
 #include "Mixer.h"
 #include "AttitudeEstimator.h"
 #include "AttitudeController.h"
+#include "VerticalEstimator.h"
+#include "VerticalController.h"
+#include "HorizontalEstimator.h"
+#include "HorizontalController.h"
 #include "Parameters.h"
-#include "MPU9250.h"
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalController/HorizontalController.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,19 @@
+#include "mbed.h"
+#include "HorizontalController.h"
+
+HorizontalController::HorizontalController()
+{
+    phi_r = 0.0f;
+    theta_r = 0.0f;
+}
+
+void HorizontalController::control(float x_r, float y_r, float x, float y, float u, float v)
+{
+    phi_r = control_single(y_r, y, v, y_kp, y_kd)/(-g);
+    theta_r = control_single(x_r, x, u, x_kp, x_kd)/g;
+}
+
+float HorizontalController::control_single(float pos_r, float pos, float vel, float kp, float kd)
+{
+    return kp*(pos_r - pos) - kd*vel;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalController/HorizontalController.h	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,22 @@
+#ifndef HorizontalController_h
+#define HorizontalController_h
+
+#include "mbed.h"
+#include "Parameters.h"
+
+class HorizontalController
+{
+    public:
+        
+        HorizontalController();
+        
+        void control(float x_r, float y_r, float x, float y, float u, float v);
+        
+        float phi_r, theta_r;
+    
+    private:
+        
+        float control_single(float pos_r, float pos, float vel, float kp, float kd);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalEstimator/HorizontalEstimator.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "HorizontalEstimator.h"
+
+// Class constructor
+HorizontalEstimator::HorizontalEstimator():flow(PA_7,PA_6,PA_5,PB_4)
+{
+    x = 0.0f;
+    y = 0.0f;
+    u = 0.0f;
+    v = 0.0f;
+    x_m_last = 0.0f;
+    y_m_last = 0.0f;
+}
+// Initialize class
+void HorizontalEstimator::init()
+{
+    flow.init();
+}
+// Predict horizontal position and velocity from model
+void HorizontalEstimator::predict()
+{
+    x = x + u*dt;
+    y = y + v*dt;
+}
+// Correct horizontal position and velocity with measurements
+void HorizontalEstimator::correct(float phi,float theta,float p,float q,float z)
+{
+    float Cphi = cos(phi);
+    float Ctheta = cos(theta);
+    
+    if( Cphi >= 0.7f && Ctheta >= 0.7f )
+    {
+        float d = z/(Cphi*Ctheta);
+        
+        flow.read();
+        float u_m = (sigma*flow.x + q)*d;
+        float v_m = (sigma*flow.y - p)*d;
+        
+        float x_m = x_m_last + u_m*dt_flow;
+        float y_m = y_m_last + v_m*dt_flow;
+        
+        x = (1.0f - rho_horiz_corr)*x + rho_horiz_corr*x_m;
+        y = (1.0f - rho_horiz_corr)*y + rho_horiz_corr*y_m;
+        u = (1.0f - rho_horiz_corr)*u + rho_horiz_corr*u_m;
+        v = (1.0f - rho_horiz_corr)*v + rho_horiz_corr*v_m;
+        
+        x_m_last = x_m;
+        y_m_last = y_m;
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalEstimator/HorizontalEstimator.h	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,29 @@
+#ifndef HorizontalEstimator_h
+#define HorizontalEstimator_h
+
+#include "mbed.h"
+#include "Parameters.h"
+#include "PMW3901.h"
+
+// Horizontal estimator class
+class HorizontalEstimator
+{
+    public:
+        // Class constructor
+        HorizontalEstimator();
+        // Initialize class
+        void init();
+        // Predict horizontal position and velocity from model
+        void predict();
+        // Correct horizontal position and velocity with measurements
+        void correct(float phi,float theta,float p,float q,float z);
+        // Horizontal positions (m) and velocities (m/s) estimations
+        float x, y, u, v;
+    private:
+        // Flow sensor object
+        PMW3901 flow;
+        // Last horizontal positions (m) measurements
+        float x_m_last, y_m_last;
+};
+
+#endif
\ No newline at end of file

--- a/Mixer/Mixer.cpp	Tue Oct 09 18:00:51 2018 +0000
+++ b/Mixer/Mixer.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -20,22 +20,27 @@
 // Converts desired force (N) and torques (N.m) to angular velocities ( rad /s)
 void Mixer::ft_to_omega( float ft , float tau_phi , float tau_theta , float tau_psi )
 {
-    float omega02 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi;
-    float omega12 = (1/(4*kl))*ft - (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi;
-    float omega22 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi + (1/(4*kl*l))*tau_theta - (1/(4*kd))*tau_psi;
-    float omega32 = (1/(4*kl))*ft + (1/(4*kl*l))*tau_phi - (1/(4*kl*l))*tau_theta + (1/(4*kd))*tau_psi;
+    float a1 = (1.0f/(4.0f*kl))*ft;
+    float a2 = (1.0f/(4.0f*kl*l))*tau_phi;
+    float a3 = (1.0f/(4.0f*kl*l))*tau_theta;
+    float a4 = (1.0f/(4.0f*kd))*tau_psi;
     
-    if (omega02 < 0){
-        omega02 = 0;
+    float omega02 = a1 - a2 - a3 - a4;
+    float omega12 = a1 - a2 + a3 + a4;
+    float omega22 = a1 + a2 + a3 - a4;
+    float omega32 = a1 + a2 - a3 + a4;
+    
+    if (omega02 < 0.0f){
+        omega02 = 0.0f;
     }
-    if (omega12 < 0){
-        omega12 = 0;
+    if (omega12 < 0.0f){
+        omega12 = 0.0f;
     }
-    if (omega22 < 0){
-        omega22 = 0;
+    if (omega22 < 0.0f){
+        omega22 = 0.0f;
     }
-    if (omega32 < 0){
-        omega32 = 0;
+    if (omega32 < 0.0f){
+        omega32 = 0.0f;
     }
     
     omega0 = sqrt(omega02);

--- a/Parameters/Parameters.h	Tue Oct 09 18:00:51 2018 +0000
+++ b/Parameters/Parameters.h	Wed Nov 21 10:46:29 2018 +0000
@@ -1,20 +1,36 @@
 #ifndef Parameters_h
 #define Parameters_h
 
+const float m = 0.03f;
 const float I_xx = 1.6f*pow(10.0f,-5);
 const float I_yy = 1.6f*pow(10.0f,-5);
 const float I_zz = 2.9f*pow(10.0f,-5);
 
-const float phi_kp = 100.0f;
-const float phi_kd = 20.0f;
-const float theta_kp = 100.0f;
-const float theta_kd = 20.0f;
-const float psi_kp = 100.0f;
-const float psi_kd = 20.0f;
+const float g = 9.81f;
+
+const float phi_kp = 240.3f;//100.0f
+const float phi_kd = 26.7f;//24.0f
+const float theta_kp = 240.3f;//100.0f
+const float theta_kd = 26.7f;//24.0f
+const float psi_kp = 60.1f;//50.0f
+const float psi_kd = 13.3f;//12.0f
+const float x_kp = 1.28f;//1.28f;1.35
+const float x_kd = 2.5f;//1.6f;
+const float y_kp = 1.28f;//1.28f;1.35
+const float y_kd = 2.5f;//1.6f;
+const float z_kp = 5.41f;//32.0f;
+const float z_kd = 4.0f;//10.0f;
+
+const float dt = 0.002f;
+const float dt_range = 0.05f;
+const float dt_flow = 0.002f;
 
 const float pi = 3.14159265f;
-const float dt = 0.005f;
-const float rho = 0.05f;
+const float sigma = 0.001827923976f/dt_flow;
+
+const float rho_att = 0.01f; //0.05f
+const float rho_vert_corr = 0.3f; //0.5f
+const float rho_horiz_corr = 0.3f; //0.3f
 
 const float alpha = 1.145f*pow(10.0f,-7);
 const float beta = 1.001f*pow(10.0f,-9);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController/VerticalController.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,12 @@
+#include "mbed.h"
+#include "VerticalController.h"
+
+VerticalController::VerticalController()
+{
+    f_t = 0.0f;
+}
+
+void VerticalController::control(float z_r, float z, float w)
+{
+    f_t = m*( z_kp*( z_r - z ) - z_kd*w + g);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController/VerticalController.h	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,18 @@
+#ifndef VerticalController_h
+#define VerticalController_h
+
+#include "mbed.h"
+#include "Parameters.h"
+
+class VerticalController 
+{
+    public:
+        
+        VerticalController();
+        
+        void control(float z_r, float z, float w);
+        
+        float f_t;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,32 @@
+#include "mbed.h"
+#include "VerticalEstimator.h"
+
+// Class constructor
+VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6)
+{
+    z = 0.0f;
+    w = 0.0f;
+    z_m_last = 0.0f;
+}
+// Initialize class
+void VerticalEstimator::init()
+{
+    range.init();
+}
+// Predict vertical position and velocity from model
+void VerticalEstimator::predict()
+{
+    z = z + w*dt;
+}
+// Correct vertical position and velocity with measurement
+void VerticalEstimator::correct(float phi,float theta)
+{
+    range.read();
+    float z_m = range.z*cos(phi)*cos(theta);
+    float w_m = (z_m - z_m_last)/dt_range;
+    
+    z = (1.0f - rho_vert_corr)*z + rho_vert_corr*z_m;
+    w = (1.0f - rho_vert_corr)*w + rho_vert_corr*w_m;
+    
+    z_m_last = z_m;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalEstimator/VerticalEstimator.h	Wed Nov 21 10:46:29 2018 +0000
@@ -0,0 +1,29 @@
+#ifndef VerticalEstimator_h
+#define VerticalEstimator_h
+
+#include "mbed.h"
+#include "Parameters.h"
+#include "VL53L0X.h"
+
+// Vertical estimator class
+class VerticalEstimator
+{
+    public:
+        // Class constructor
+        VerticalEstimator();
+        // Initialize class
+        void init();
+        // Predict vertical position and velocity from model
+        void predict();
+        // Correct vertical position and velocity with measurement
+        void correct(float phi,float theta);
+        // Vertical position (m) and velocity (m/s) estimation
+        float z,w;
+    private:
+        // Range sensor object
+        VL53L0X range;
+        // Last vertical position (m) measurement
+        float z_m_last;
+};
+
+#endif
\ No newline at end of file