Diff: quadcopter.cpp

Revision:

36:40b134328376

Parent:

35:35997980a8ba

Child:

37:a983eb9fd9c5

--- a/quadcopter.cpp	Tue Apr 26 18:50:55 2016 +0000
+++ b/quadcopter.cpp	Fri Apr 29 18:24:15 2016 +0000
@@ -8,7 +8,7 @@
 #endif
 
 // constructor
-Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer)
+Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr, Timer *timer, Mutex *desired)
 {
     pc_= pcPntr;  // enable printing
     g_= 9.81;
@@ -17,17 +17,18 @@
 
     zeroVelPwm = 0.11;
     maxPwm = 0.15;
+    desired_mutex = desired;
 
     // 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 * 0.25 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    kp_theta_ = 10 * 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
     kp_psi_   = 0;
     // kp_phi_ = 0;
 //    kp_theta_ = 0;
 
     // derivative attitude control gains
-    kd_phi_   = 0.4 * (maxPwm - zeroVelPwm) * 2 / M_PI; //0.25 good
+    kd_phi_   = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; //0.25 good
     kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI;
     kd_psi_   = 0.1;
     // kd_phi_   = 0;
@@ -78,6 +79,11 @@
 
 void Quadcopter::readSensorValues()
 {
+    if (prev_kalman_time == 0) {
+        prev_kalman_time = controlTimer->read();
+        return;
+    }
+    
     accel_.getEvent(&accel_event_);
     mag_.getEvent(&mag_event_);
     dof_.fusionGetOrientation(&accel_event_, &mag_event_, &orientation_);
@@ -98,7 +104,7 @@
     filters_.q[current_filter]     = gyro_event_.gyro.y * M_PI / 180;
     filters_.r[current_filter]     = gyro_event_.gyro.z * M_PI / 180;
     filters_.phi[current_filter]   = orientation_.roll * M_PI / 180;
-    filters_.theta[current_filter] = -orientation_.pitch * M_PI / 180;
+    filters_.theta[current_filter] = orientation_.pitch * M_PI / 180;
     filters_.psi[current_filter]   = orientation_.heading * M_PI / 180;
 
     current_filter = (current_filter + 1) % FILTER_SIZE;
@@ -138,30 +144,32 @@
     //state_.theta = theta_sum / FILTER_SIZE;
     state_.psi = psi_sum / FILTER_SIZE;
 
-    if (prev_kalman_time == 0) {
-        prev_kalman_time = controlTimer->read();
-        return;
-    }
-
     double raw_phi = state_.phi;
     double raw_theta = state_.theta;
     double raw_p = state_.p;
     double raw_q = state_.q;
-    float dt = controlTimer->read() - prev_kalman_time;
+    float time = controlTimer->read();
+    float dt = time - 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;
     state_.p = kalmanRoll.getRate() * M_PI / 180;
     state_.q = kalmanPitch.getRate() * M_PI / 180;
-
-    compAngleX = 0.93 * (compAngleX + raw_p * dt) + 0.07 * raw_phi; // Calculate the angle using a Complimentary filter
-    compAngleY = 0.93 * (compAngleY + raw_q * dt) + 0.07 * raw_theta;
+    
+    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;
     
     //state_.phi = compAngleX;
     //state_.theta = compAngleY;
 
-    prev_kalman_time = controlTimer->read();
-
-    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);
+    prev_kalman_time = time;
+    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);
 }
 
 void Quadcopter::controller()
@@ -250,7 +258,11 @@
 
 double Quadcopter::getForce()
 {
-    return F_des_;
+    double force;
+    desired_mutex->lock();
+    force = F_des_;
+    desired_mutex->unlock();
+    return force;
 }
 
 void Quadcopter::readRc()
@@ -268,7 +280,7 @@
     float roll;
     long long id;
 
-//    static int thrust_outliers = 0;
+    static int thrust_outliers = 0;
 
     receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
     if (receive > 0) {
@@ -277,6 +289,9 @@
         pc_->printf("Receive failure\r\n");
     }
 
+    desired_mutex->lock();
+//    thrust = thrust - 0.5;
+//    
 //    if (thrust < -0.3) {
 //        thrust_outliers++;
 //        if (thrust_outliers < 3) {
@@ -288,11 +303,14 @@
 
 // TODO eliminate the zeros again after testing
     // convert to radians, range is = +-40° or +-0.698132 radians
+    
+    
     desiredState_.phi = 1 * (-((roll - 0.5) * 80) * M_PI / 180); // minus, because joystick to right should result in positive moment
     desiredState_.theta = 1 * ((pitch - 0.5) * 80) * M_PI / 180;
     desiredState_.r = 1 * (yaw - 0.5); // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180;
     F_des_ = thrust - 0.5; // number between 0 and 1 -> number between -0.5 and 0.5
 
+    desired_mutex->unlock();
     // print id with thrust, yaw, pitch, and roll
 //    pc_->printf("%lld: thrust: %f yaw: %f pitch: %f roll: %f outliers: %d\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, thrust_outliers);