Diff: quadcopter.cpp

Revision:

40:09a59d5b7944

Parent:

39:fff0a72633ee

Child:

41:d103f9aa44f0

--- a/quadcopter.cpp	Sun May 01 22:13:01 2016 +0000
+++ b/quadcopter.cpp	Wed May 04 17:28:48 2016 +0000
@@ -21,13 +21,13 @@
 
     // 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.25 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    kp_phi_   = 10 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    kp_theta_ = 10 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
     kp_psi_   = 0;
 
     // derivative attitude control gains
-    kd_phi_   = 0.05 * (maxPwm - zeroVelPwm) * 2 / M_PI; // 0.25 maybe a good
-    kd_theta_ = 0.05 * (maxPwm - zeroVelPwm) * 2 / M_PI;
+    kd_phi_   = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; // 0.25 maybe a good
+    kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI;
     kd_psi_   = 0.1;
 
     // incresae ki_phi
@@ -83,7 +83,7 @@
     accel_.getEvent(&accel_event_);
     // mag_.getEvent(&mag_event_);
     dof_.accelGetOrientation(&accel_event_, &orientation_);
-    // gyro_.getEvent(&gyro_event_);
+    gyro_.getEvent(&gyro_event_);
 
     gyro_event_.gyro.x   -= initial_offsets_->gyro_x;
     gyro_event_.gyro.y   -= initial_offsets_->gyro_y;
@@ -148,21 +148,33 @@
     state_.p = kalmanRoll.getRate() * M_PI / 180;
     state_.q = kalmanPitch.getRate() * M_PI / 180;
 
-    double alphaX = 0.7;
-    double alphaY = 0.7;
-    compAngleX = (1 - alphaX) * (compAngleX + raw_p * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter
-    compAngleY = (1 - alphaY) * (compAngleY + raw_q * dt) + alphaY * raw_theta;
+    double alphaX = 0.001;
+    double alphaY = 0.001;
+    //compAngleX = (1 - alphaX) * (compAngleX + raw_p * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter
+    //compAngleY = (1 - alphaY) * (compAngleY + raw_q * dt) + alphaY * raw_theta;
+    compAngleX = (1 - alphaX) * compAngleX + raw_p * dt + alphaX * raw_phi; // Calculate the angle using a Complimentary filter
+    compAngleY = (1 - alphaY) * compAngleY + raw_q * dt + alphaY * raw_theta;
 
-    //state_.phi = compAngleX;
-    //state_.theta = compAngleY;
+
+
+    state_.phi = compAngleX;
+    state_.theta = compAngleY;
 
     prev_kalman_time = time;
+
+    /* RAW DATA
+    pc_->printf("%f %f %f %f %f %f %f \r\n",
+        prev_kalman_time,
+        accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z,
+        gyro_event_.gyro.x, gyro_event_.gyro.y, gyro_event_.gyro.z);
+    */
     // static int count = 0;
     // if (count % 100 == 0) {
     //  pc_->printf("%d\r\n", count);
     // }
     // count++;
-    //pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_kalman_time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY, raw_phi, raw_theta);
+    // TODO: print HERE
+    // pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_kalman_time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY, raw_phi, raw_theta);
 }
 
 void Quadcopter::controller()