Diff: quadcopter.cpp

Revision:

28:61f7356325c3

Parent:

27:11116aa69f32

Child:

29:ae765492fa8b

diff -r 11116aa69f32 -r 61f7356325c3 quadcopter.cpp
--- a/quadcopter.cpp	Thu Apr 14 22:32:30 2016 +0000
+++ b/quadcopter.cpp	Fri Apr 15 19:08:56 2016 +0000
@@ -83,10 +83,6 @@
     orientation_.heading -= initial_offsets_->heading;
 
     // measured values (will come from IMU/parameter class/Input to function later)
-    // angles
-    state_.phi = orientation_.roll;
-    state_.theta =orientation_.pitch;
-    state_.psi =orientation_.heading;
     // angular velocities in body coordinate system
     state_.p = gyro_event_.gyro.x;
     state_.q = gyro_event_.gyro.y;
@@ -97,9 +93,9 @@
 
     // 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);
     state_.phi = orientation_.roll * M_PI / 180;
-    state_.theta = orientation_.pitch * M_PI / 180;
+    state_.theta = -orientation_.pitch * M_PI / 180;
     state_.psi = orientation_.heading * M_PI / 180;
-    //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);
+    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);
 }
 
 // Date member function
@@ -131,9 +127,9 @@
     // set pwm values
     // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma...
     motorPwm_.m1=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m2=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
+    motorPwm_.m2=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
     motorPwm_.m3=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m4=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
+    motorPwm_.m4=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
 
     motorPwm_.m1 = min(maxPwm, motorPwm_.m1);
     motorPwm_.m2 = min(maxPwm, motorPwm_.m2);
@@ -206,13 +202,15 @@
         pc_->printf("Receive failure\r\n");
     }
 
+    //pc_->printf("buffer: %s\r\n", rxBuffer );
+
     // convert to radians, range is = +-40° or +-0.698132 radians
-    desiredState_.phi = ((roll - 0.5) * 80) * M_PI / 180;
+    desiredState_.phi = -((roll - 0.5) * 80) * M_PI / 180; // minus, because joystick to right should result in positive moment
     desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180;
     desiredState_.r = 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  //((thrust - 0.5) * 80) * M_PI / 180;
 
     // 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\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi);
 }