Diff: quadCommand/quadCommand.cpp

Revision:

62:70fc58d4be9b

Parent:

60:6906a96344a0

Child:

63:f4cb482ac5d4

--- a/quadCommand/quadCommand.cpp	Wed Jul 24 02:06:21 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Fri Jul 26 02:42:31 2013 +0000
@@ -3,6 +3,7 @@
 /*************************************************************************/
 
 #include "quadCommand.h"
+#include "DCM.h"
 
 quadCommand::quadCommand()
 { 
@@ -13,9 +14,12 @@
         targetYaw       = currentYaw        = 0;
         pitchTrim       = 0;
         rollTrim        = 0;
-     
+        timestamp = 0;
+        timestamp_old = 0;
+        timer.start();
         myCom = new com( TXPIN, RXPIN );            // Setup com object.       
-        world = new sensors();                      // Setup the sensors.
+//        world = new sensors();    
+        DCM *world = new DCM();
         
         // Create PID instances.
         pidPitch = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
@@ -99,10 +103,11 @@
 
 void quadCommand::updateMotors() 
 {
+
     updateCurrent();
     float throttle, pitch, roll, yaw;   
         
-    throttle = targetThrottle * 5;  // Increased throttle Range to match higher resolution
+    throttle = targetThrottle;
     pitch = pidPitch->correct(currentPitch, targetPitch, MOTOR_UPDATE);
     roll = pidRoll->correct(currentRoll, targetRoll, MOTOR_UPDATE);
     yaw = pidYaw->correct(currentYaw, targetYaw, MOTOR_UPDATE);
@@ -124,7 +129,16 @@
 
 void quadCommand::updateCurrent()
 {
-    currentPitch    = world->getAbsoluteX() - pitchTrim;
-    currentRoll     = world->getAbsoluteY() - rollTrim;
+//    currentPitch    = world->getAbsoluteX() - pitchTrim;
+//    currentRoll     = world->getAbsoluteY() - rollTrim;
+    timestamp_old = timestamp;
+    timestamp = timer.read_ms();
+    if (timestamp > timestamp_old)
+    G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+    else
+    G_Dt = 0;
+    world->Update_step(G_Dt);
+    currentPitch    = world->pitch - pitchTrim;
+    currentRoll     = world->roll - rollTrim;
     currentYaw      = 0 - yawTrim;
 }
\ No newline at end of file