Revision 7:fb6174d2ea78, committed 2018-11-05

Comitter:

yvesyuzo

Date:

Mon Nov 05 11:12:11 2018 +0000

Parent:

6:09cc3b307252

Commit message:

5

Changed in this revision

--- a/AttitudeController/AttitudeController.cpp	Wed Oct 31 01:04:12 2018 +0000
+++ b/AttitudeController/AttitudeController.cpp	Mon Nov 05 11:12:11 2018 +0000
@@ -10,15 +10,15 @@
 void AttitudeController::control ( float phi_r, float theta_r, float psi_r, float phi, float theta, float psi)
 {
     tau_phi = single_axis_control ( phi_r , phi , Kp_phi , Td_phi , I_xx, last_erro_phi);
-    
+
     last_erro_phi = last_erro;
-    
+
     tau_theta = single_axis_control (  psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi);
-    
+
     last_erro_theta = last_erro;
-    
+
     tau_psi = single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta);
-    
+
     last_erro_psi = last_erro;
 }
 // Control torque of a single axis given reference angles and current angles and angular velocities ( with given gains and /or time constants and moments of inertia )

--- a/Library/Library.h	Wed Oct 31 01:04:12 2018 +0000
+++ b/Library/Library.h	Mon Nov 05 11:12:11 2018 +0000
@@ -14,23 +14,14 @@
 float const I_yy = 16.0e-6f;
 float const I_zz = 29.0e-6f;
 // Attitude Estimator constants
- float const pi = 3.14159265f;
- float const dt = 0.005f;
- float const rho = 0.05f;
- // Mixer
+float const pi = 3.14159265f;
+float const dt = 0.005f;
+float const rho = 0.05f;
+// Mixer
 const float alpha = 1.081E-7;
 const float beta = 2.678E-11;
 const float kl = 2.69E-8;
 const float kd = 1.59E-10;
 const float l = 33E-3;
-//Vertical
-//float z = 0.0f;//leitura da distancia vertical para função predict
-//float w = 0.0f;//definido a velocidade vertical para função predict
-//float zm = 0.0f;//definido uma leitura para distancia vertical
-//float zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
-//float pon = 0.3f;//constante de ponderação
-//float z_est = 0.0f;
-//float w_est = 0.0f; 
-//float wm = 0.0f;
 
 # endif

--- a/Mixer/Mixer.cpp	Wed Oct 31 01:04:12 2018 +0000
+++ b/Mixer/Mixer.cpp	Mon Nov 05 11:12:11 2018 +0000
@@ -4,22 +4,22 @@
 #include "Library.h"
 
 //Declare motors as PWM outputs
-PwmOut motor[4] = {(PA_1),(PB_11),(PA_15),(PB_9_ALT1)};
+//PwmOut motor[4] = {(PA_1),(PB_11),(PA_15),(PB_9_ALT1)};
 
 // Class constructor
-Mixer :: Mixer () : motor_1 ( PA_1 ) , motor_2 ( PB_11 ) , motor_3 ( PA_15 ) , motor_4 (PB_9_ALT1 )
+/*Mixer :: Mixer () : motor_1(PA_1) ,motor_2(PB_11) , motor_3(PA_15) , motor_4(PB_9_ALT1) //para rodar comando serial é necessario comentar essa linha
 {
 }
-
+*/
 void Mixer :: actuate ( float f_t , float tau_phi , float tau_theta , float tau_psi )
 {
-    
+
     // Define parameters
     float const alpha = 1.081e-7;
     float const beta = 2.678e-11;
-    
+
     //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular
-    w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl))* tau_theta) - ((1/(4*kd))* tau_psi)  ;
+    w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl))* tau_theta) - ((1/(4*kd))* tau_psi) ;
     w2 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
     w3 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) - ((1/(4*kd))* tau_psi);
     w4 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) - ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
@@ -44,11 +44,10 @@
     w4 = sqrt(w4);
 
     // Turn on all motors for 5s
-    motor [0] = alpha *pow(w1 ,2) + beta * w1;
-    motor [1] = alpha *pow(w2 ,2) + beta * w2;
-    motor [2] = alpha *pow(w3 ,2) + beta * w3;
-    motor [3] = alpha *pow(w4 ,2) + beta * w4;
-    
+    motor_1 = alpha *pow(w1 ,2) + beta * w1;
+    motor_2 = alpha *pow(w2 ,2) + beta * w2;
+    motor_3 = alpha *pow(w3 ,2) + beta * w3;
+    motor_4 = alpha *pow(w4 ,2) + beta * w4;
 
 }
 

--- a/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 31 01:04:12 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Mon Nov 05 11:12:11 2018 +0000
@@ -3,41 +3,40 @@
 #include <math.h>
 #include "Library.h"
 
-/*
-    float z = 0.0f;//leitura da distancia vertical para função predict
-    float w = 0.0f;//definido a velocidade vertical para função predict
-    
-    float zm = 0.0f;//definido uma leitura para distancia vertical
-    float zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
-    float pon = 0.3f;//constante de ponderação
-    float z_est = 0.0f;
-    float w_est = 0.0f; 
-    float wm = 0.0f;
-*/
-
 // Class constructor
 VerticalEstimator::VerticalEstimator():range (PB_7 , PB_6 )
 {
+}
 
-}
 // Initialize class
 void VerticalEstimator::init ()
 {
     VL53L0X range (PB_7 , PB_6 );
     range.init() ;
+    
+    float z = 0.0f;//leitura da distancia vertical para função predict
+    float zm = 0.0f;//definido uma leitura para distancia vertical
+    float zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
+    float z_est = 0.0f;
+    
+    float w = 0.0f;//definido a velocidade vertical para função predict
+    float wm = 0.0f;
+    float w_est = 0.0f;  
+    
+    float pon = 0.3f;//constante de ponderação
+    
 }
 // Predict vertical position and velocity from model
 void VerticalEstimator::predict ()
 {
     range.read();
-    z = range.d;   
+    z = range.d;
     z = z + w*0.002f;//velocidade prevista, como a leitura é feita com 500Hz 0.002f sao os 2ms equivalentes
-    
-    
 }
+
 // Correct vertical position and velocity with measurement
 void VerticalEstimator::correct ( float phi , float theta )//phi e theta são os angulos de euler estimados
-{ 
+{
 
     zm = range.d;
     zm = zm*cos(phi)*cos(theta);//zm é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
@@ -45,12 +44,9 @@
     zml = range.d;
     zm = zml*cos(phi)*cos(theta);//d é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
     wm = (zm-zml)/0.05f;
-       
+
     z_est = (1-0.3f)*z + 0.3f*zm;
     w_est = (1-0.3f)*w + 0.3f*wm;
 
-
-    //zml = zm;
-    
 }
 

--- a/VerticalEstimator/VerticalEstimator.h	Wed Oct 31 01:04:12 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.h	Mon Nov 05 11:12:11 2018 +0000
@@ -11,28 +11,28 @@
 {
 public :
 //Class constructor
-VerticalEstimator () ;
+    VerticalEstimator () ;
 // Initialize class
-void init () ;
+    void init () ;
 // Predict vertical position and velocity from model
-void predict () ;
+    void predict () ;
 // Correct vertical position and velocity with measurement
-void correct ( float phi , float theta );
+    void correct (float phi, float theta);
 // Vertical position (m) and velocity (m/s) estimation
-float z, w, w_est, z_est, wm, zm;
-/*
-float w_est; 
-float z_est;
-float wm;
-float zm ;//definido uma leitura para distancia vertical
-float zml ;//definindo uma segunda leitura para calcular a velocidade vertical
-*/
+    float w_est, z_est;
+    /*
+    float w_est;
+    float z_est;
+    float wm;
+    float zm ;//definido uma leitura para distancia vertical
+    float zml ;//definindo uma segunda leitura para calcular a velocidade vertical
+    */
 
 private :
 // Range sensor object
-VL53L0X range ;
+    VL53L0X range ;
 // Last vertical position (m) measurement
-float zml ;
+    float z, w, zml, wm, zm;
 };
 
 # endif

--- a/VerticalEstimator/teste.cpp	Wed Oct 31 01:04:12 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-#include "mbed.h"
-#include "USBSerial.h"
-#include "CrazyflieController.h"
-
-// USB serial object
-USBSerial pc;
-// Crazyflie controller objects
-AttitudeEstimator att_est ;
-VerticalEstimator ver_est ;
-// Tickers
-Ticker tic ;
-Ticker tic_range ;
-
-// Interrupt flags
-bool flag = false ;
-bool flag_range = false ;
-
-// Callback function
-void callback ()
-{
-flag = true ;
-}
-
-// Callback function ( range correct )
-void callback_range ()
-{
-flag_range = true ;
-}
-
-int main ()
-{
-// Wait 5s for safety
-wait (5) ;
-// Initialize estimators objects
-att_est.init () ;
-ver_est.init () ;
-// Initialize interrupts
-tic.attach (& callback , dt);
-tic_range.attach (& callback_range , dt_range ) ;
-while (1)
-{
-// Estimate
-if ( flag )
-{
-flag = false ;
-att_est.estimate () ;
-ver_est.predict () ;
-}
-// Correct vertical estimation and print values
-if ( flag_range )
-{
-flag_range = false ;
-ver_est.correct ( att_est.phi , att_est.theta );
-pc.printf ("z [m ]:%6.2 f | w [m/s ]:%6.2 f \n", ver_est.z, ver_est.w);
-}
-}
-}