Diff: quadcopter.cpp

Revision:

23:04338a5ef404

Parent:

22:92401a4fec13

Child:

24:e220fbb70ded

--- a/quadcopter.cpp	Mon Apr 11 16:41:56 2016 +0000
+++ b/quadcopter.cpp	Tue Apr 12 22:54:16 2016 +0000
@@ -2,6 +2,11 @@
 #include "sensor.h"
 #include "receiver.h"
 #include <string>
+#include <math.h>
+
+#ifndef M_PI
+#define M_PI           3.14159265358979323846
+#endif
 
 //#include "mbed.h"
 
@@ -9,15 +14,17 @@
 Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr)
 {   
     pc_= pcPntr;  // enable printing
-   // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
+    initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
 
-    m_=1;
-    g_=9.81;
-      l_=0.25;
-    gamma_=1;
+    m_= 1;
+    g_= 9.81;
+    l_= 0.25;
+    gamma_= 1;
     
     
     // proportional attitude control gains
+    // TODO change gains so that joystick deflection never produces pwm duty cycle >10%. 
+    
     kp_f_ =2.5;
     kp_phi_ = 0.2;
     kp_theta_ = 0.2;
@@ -78,6 +85,15 @@
     state_.p = gyro_event_.gyro.x;
     state_.q = gyro_event_.gyro.y;
     state_.r = gyro_event_.gyro.z;
+    
+    // TODO print values to check what they are
+    // TODO convert to Radians (*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);
+    state_.phi = orientation_.roll * M_PI / 180;
+    state_.theta =orientation_.pitch * M_PI / 180;
+    state_.psi =orientation_.heading * M_PI / 180;
+    //pc_->printf("Converted 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
@@ -148,9 +164,14 @@
     } else {
         pc_->printf("Receive failure\r\n");
     }  
+    
+    // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian). 
+    
     desiredState_.phi=roll-0.5;
     desiredState_.theta=pitch-0.5;
     desiredState_.psi=yaw-0.5;
     F_des_=thrust-0.5;
+    
+    
 }