Diff: quadcopter.cpp

Revision:

37:a983eb9fd9c5

Parent:

36:40b134328376

Child:

38:14bf11115f9f

--- a/quadcopter.cpp	Fri Apr 29 18:24:15 2016 +0000
+++ b/quadcopter.cpp	Sun May 01 18:45:02 2016 +0000
@@ -21,8 +21,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_   = 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_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;
@@ -85,10 +85,10 @@
     }
     
     accel_.getEvent(&accel_event_);
-    mag_.getEvent(&mag_event_);
-    dof_.fusionGetOrientation(&accel_event_, &mag_event_, &orientation_);
-
-    gyro_.getEvent(&gyro_event_);
+//    mag_.getEvent(&mag_event_);
+    dof_.accelGetOrientation(&accel_event_, &orientation_);
+//
+//    gyro_.getEvent(&gyro_event_);
 
     gyro_event_.gyro.x   -= initial_offsets_->gyro_x;
     gyro_event_.gyro.y   -= initial_offsets_->gyro_y;
@@ -104,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;
@@ -164,11 +164,11 @@
     //state_.theta = compAngleY;
 
     prev_kalman_time = time;
-    static int count = 0;
-    if (count % 100 == 0) {
-        pc_->printf("%d\r\n", count);
-    }
-    count++;
+//    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);
 }
 
@@ -258,11 +258,7 @@
 
 double Quadcopter::getForce()
 {
-    double force;
-    desired_mutex->lock();
-    force = F_des_;
-    desired_mutex->unlock();
-    return force;
+    return F_des_;
 }
 
 void Quadcopter::readRc()
@@ -275,24 +271,29 @@
     char rxBuffer[rcLength_];
 
     float thrust;
-    float yaw;
-    float pitch;
-    float roll;
+    float yaw = 0;
+    float pitch = 0;
+    float roll = 0;
     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) {
-        sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust, &yaw, &pitch, &roll);
+    //pc_->printf("%s\r\n", rxBuffer);
+    if (receive > 10) {
+        int written = sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust, &yaw, &pitch, &roll);
+        pc_->printf("&d", written);
+        if (written != 5) {
+            return;
+        }
     } else {
         pc_->printf("Receive failure\r\n");
+        return;
     }
-
-    desired_mutex->lock();
-//    thrust = thrust - 0.5;
+    //pc_->printf("thrust: %f\r\n", thrust);
+//    float temp_thrust = thrust - 0.5;
 //    
-//    if (thrust < -0.3) {
+//    if (temp_thrust < -0.3) {
 //        thrust_outliers++;
 //        if (thrust_outliers < 3) {
 //            thrust = F_des_;
@@ -303,16 +304,12 @@
 
 // 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);
+    //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);
 
 }