Diff: quadcopter.cpp

Revision:

34:eaea0ae92dfa

Parent:

33:244dea7a4e81

Child:

35:35997980a8ba

diff -r 244dea7a4e81 -r eaea0ae92dfa quadcopter.cpp
--- a/quadcopter.cpp	Sat Apr 23 20:52:10 2016 +0000
+++ b/quadcopter.cpp	Sun Apr 24 17:15:20 2016 +0000
@@ -8,7 +8,7 @@
 #endif
 
 // constructor
-Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr)
+Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer)
 {
     pc_= pcPntr;  // enable printing
     g_= 9.81;
@@ -20,8 +20,8 @@
 
     // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle.
     kp_f_     = (maxPwm - zeroVelPwm) * 4 / 0.5;
-    kp_phi_   = 0.16 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = 0.12 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    kp_phi_   = 0.3 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    kp_theta_ = 0.3 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
     kp_psi_   = 0;
     // kp_phi_ = 0;
 //    kp_theta_ = 0;
@@ -34,7 +34,7 @@
     //kd_theta_ = 0;
 
 // incresae ki_phi
-    ki_phi_ = 0.1 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error
+    ki_phi_ = 0*0.1 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error
     ki_theta_ = 0 *  (maxPwm - zeroVelPwm)/(2*M_PI/4);
 
     i_e_phi_ = 0;
@@ -62,6 +62,18 @@
 
     initial_offsets_ = (offset*) malloc(sizeof(offset));
     initSensors(*this);  // IMU
+    
+    controlTimer = timer;
+    controlTimer->start();
+    
+    prev_kalman_time = 0;
+    
+    readSensorValues();
+        
+    kalmanPitch.setAngle(state_.phi); // set initial pitch
+    kalmanRoll.setAngle(state_.theta); // set initial theta
+    compAngleX = state_.phi;
+    compAngleY = state_.theta;
 }
 
 void Quadcopter::readSensorValues()
@@ -116,19 +128,30 @@
     state_.phi = phi_sum / FILTER_SIZE;
     state_.theta = theta_sum / FILTER_SIZE;
     state_.psi = psi_sum / FILTER_SIZE;
+    
+    if (prev_kalman_time == 0) {
+        prev_kalman_time = controlTimer->read();
+        return;
+    }
+    
+    float dt = controlTimer->read() - prev_kalman_time;
+    state_.phi = kalmanRoll.getAngle(state_.phi * 180 / M_PI, state_.p * 180 / M_PI, dt) * M_PI / 180;
+    state_.theta = kalmanPitch.getAngle(state_.theta * 180 / M_PI, state_.q * 180 / M_PI, dt) * M_PI / 180;
+    
+    compAngleX = 0.93 * (compAngleX + state_.p * dt) + 0.07 * state_.phi; // Calculate the angle using a Complimentary filter
+    compAngleY = 0.93 * (compAngleY + state_.q * dt) + 0.07 * state_.theta;
 
     //pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
 }
 
-void Quadcopter::controller(float time)
+void Quadcopter::controller()
 {
-
+    float time = controlTimer->read();
     if (prev_time_ == 0) {
         prev_time_ = time;
         return;
     }
 
-
     // PD controller
     double e_phi = desiredState_.phi - state_.phi;
     double e_theta = desiredState_.theta - state_.theta;
@@ -166,7 +189,7 @@
     prev_time_ = time;
 
     //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4);
-    pc_->printf("%f %f %f %f %f %f %f %f \r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi);
+    pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY);
 
     //pc_->printf("%f %f %f %f %f %f %f %f \r\n", time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi);
 }