Diff: quadcopter.cpp

Revision:

47:ae478c380136

Parent:

46:4bcf2e679f96

Child:

48:0346cdff12a7

--- a/quadcopter.cpp	Thu May 05 15:58:49 2016 +0000
+++ b/quadcopter.cpp	Thu May 05 20:47:55 2016 +0000
@@ -98,8 +98,8 @@
     // compute orientation with correctet acc data
     dof_.accelGetOrientation(&accel_event_, &orientation_);
     // compute roll and pitch values
-    double raw_phi = orientation_.roll * M_PI / 180;;
-    double raw_theta = orientation_.pitch * M_PI / 180;;
+    double raw_phi = orientation_.roll * M_PI / 180;
+    double raw_theta = orientation_.pitch * M_PI / 180;
 
     // get gyro values
     gyro_.getEvent(&gyro_event_);
@@ -123,10 +123,10 @@
     
     // complementary filter parameters
     double alphaX = 0.0002;//0.002;
-    double alphaY = 0.0001; //0.002;
+    double alphaY = 0.0001; //maybe 0.0002; 
     // different version of complementary filter
     //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;
+    //compAngleY = (1 - alphaY) * (com pAngleY + raw_q * dt) + alphaY * raw_theta;
     compAngleX = (1 - alphaX) * (compAngleX + (raw_p + 8.446703927263016e-04) * dt) + alphaX * raw_phi; // hardcoded value compensates drift with alpha == 0. 
     compAngleY = (1 - alphaY) * (compAngleY + (raw_q - 0.008936763695439) * dt) + alphaY * raw_theta;
 
@@ -151,7 +151,7 @@
     //  pc_->printf("%d\r\n", count);
     // }
     // count++;
-    //pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_sensor_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,accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z);
+    pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_sensor_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,accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z);
 }
 
 void Quadcopter::controller()
@@ -180,7 +180,7 @@
     controlInput_.f  = kp_f_ * F_des_;
     controlInput_.mx = kp_phi_ * e_phi + kd_phi_ * (desiredState_.p - state_.p) + ki_phi_ * i_e_phi_;
     controlInput_.my = kp_theta_ * e_theta + kd_theta_ * (desiredState_.q - state_.q) + ki_theta_ * i_e_theta_;
-    controlInput_.mz = kd_psi_ * desiredState_.r; // feedforward desired yaw rate.  // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
+    controlInput_.mz = kd_psi_ * (desiredState_.r - state_.q); // feedforward desired yaw rate.  // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
 
     // set pwm values
     double forcePerMotor = 0.25 * controlInput_.f;
@@ -294,8 +294,8 @@
 //    }
 
     // convert to radians, range is = +-40° or +-0.698132 radians
-    desiredState_.phi = 1 * (-(roll * 80) * M_PI / 180); // minus, because joystick to right should result in positive moment
-    desiredState_.theta = 1 * (pitch * 80) * M_PI / 180;
+    desiredState_.phi = 1 * (-(roll * 30) * M_PI / 180); // minus, because joystick to right should result in positive moment
+    desiredState_.theta = 1 * (pitch * 30) * M_PI / 180;
     desiredState_.r = 1 * yaw; // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180;
     F_des_ = thrust; // number between 0 and 1 -> number between -0.5 and 0.5