Diff: quadcopter.cpp

Revision:

35:35997980a8ba

Parent:

34:eaea0ae92dfa

Child:

36:40b134328376

--- a/quadcopter.cpp	Sun Apr 24 17:15:20 2016 +0000
+++ b/quadcopter.cpp	Tue Apr 26 18:50:55 2016 +0000
@@ -20,21 +20,21 @@
 
     // 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.3 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = 0.3 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
+    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_psi_   = 0;
     // kp_phi_ = 0;
 //    kp_theta_ = 0;
 
     // derivative attitude control gains
-    kd_phi_   = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI;
-    kd_theta_ = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI;
+    kd_phi_   = 0.4 * (maxPwm - zeroVelPwm) * 2 / M_PI; //0.25 good
+    kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI;
     kd_psi_   = 0.1;
     // kd_phi_   = 0;
     //kd_theta_ = 0;
 
 // incresae ki_phi
-    ki_phi_ = 0*0.1 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error
+    ki_phi_ = 0 * (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,14 +62,14 @@
 
     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;
@@ -79,12 +79,8 @@
 void Quadcopter::readSensorValues()
 {
     accel_.getEvent(&accel_event_);
-    if (dof_.accelGetOrientation(&accel_event_, &orientation_)) {
-    }
-    /* Calculate the heading using the magnetometer */
     mag_.getEvent(&mag_event_);
-    if (dof_.magGetOrientation(SENSOR_AXIS_Z, &mag_event_, &orientation_)) {
-    }
+    dof_.fusionGetOrientation(&accel_event_, &mag_event_, &orientation_);
 
     gyro_.getEvent(&gyro_event_);
 
@@ -95,6 +91,8 @@
     orientation_.pitch   -= initial_offsets_->pitch;
     orientation_.heading -= initial_offsets_->heading;
 
+
+
     static int current_filter = 0;
     filters_.p[current_filter]     = gyro_event_.gyro.x * M_PI / 180;
     filters_.q[current_filter]     = gyro_event_.gyro.y * M_PI / 180;
@@ -120,28 +118,50 @@
         psi_sum   += filters_.psi[i];
     }
 
-    // angular velocities in body coordinate system
+//    double radMaxAngle = M_PI / 180 * 20;
+    double phi_new = phi_sum / FILTER_SIZE;
+//    if (phi_new < radMaxAngle && phi_new > -radMaxAngle) {
+//        state_.phi = phi_new;
+//    };
+    double theta_new = theta_sum / FILTER_SIZE;
+//    if (theta_new < radMaxAngle && theta_new > -radMaxAngle) {
+//        state_.theta = theta_new;
+//    };
+
+    state_.phi = phi_new;
+    state_.theta = theta_new;
     state_.p = p_sum / FILTER_SIZE;
+
     state_.q = q_sum / FILTER_SIZE;
     state_.r = r_sum / FILTER_SIZE;
 
-    state_.phi = phi_sum / FILTER_SIZE;
-    state_.theta = theta_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;
     }
-    
+
+    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;
     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;
     
-    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;
+    //state_.phi = compAngleX;
+    //state_.theta = compAngleY;
 
-    //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);
+    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);
 }
 
 void Quadcopter::controller()
@@ -189,7 +209,6 @@
     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 %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);
 }
@@ -249,6 +268,8 @@
     float roll;
     long long id;
 
+//    static int thrust_outliers = 0;
+
     receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
     if (receive > 0) {
         sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust, &yaw, &pitch, &roll);
@@ -256,15 +277,24 @@
         pc_->printf("Receive failure\r\n");
     }
 
+//    if (thrust < -0.3) {
+//        thrust_outliers++;
+//        if (thrust_outliers < 3) {
+//            thrust = F_des_;
+//        }
+//    } else {
+//        thrust_outliers = 0;
+//    }
+
 // 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.  //((thrust - 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
 
     // print id with thrust, yaw, pitch, and roll
-    //pc_->printf("%lld: thrust: %f yaw: %f pitch: %f roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi);
+//    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);
 
 }