Diff: quadCommand/quadCommand.cpp

Revision:

63:f4cb482ac5d4

Parent:

62:70fc58d4be9b

diff -r 70fc58d4be9b -r f4cb482ac5d4 quadCommand/quadCommand.cpp
--- a/quadCommand/quadCommand.cpp	Fri Jul 26 02:42:31 2013 +0000
+++ b/quadCommand/quadCommand.cpp	Sun Jul 28 21:44:00 2013 +0000
@@ -3,7 +3,6 @@
 /*************************************************************************/
 
 #include "quadCommand.h"
-#include "DCM.h"
 
 quadCommand::quadCommand()
 { 
@@ -14,12 +13,15 @@
         targetYaw       = currentYaw        = 0;
         pitchTrim       = 0;
         rollTrim        = 0;
-        timestamp = 0;
-        timestamp_old = 0;
+        G_Dt            = 0;
+
+     
+        myCom = new com( TXPIN, RXPIN );            // Setup com object.       
+        world = new DCM();
         timer.start();
-        myCom = new com( TXPIN, RXPIN );            // Setup com object.       
-//        world = new sensors();    
-        DCM *world = new DCM();
+        
+        timestamp_old = 0;
+        timestamp = timer.read_ms();                      // Setup the sensors.
         
         // Create PID instances.
         pidPitch = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
@@ -103,11 +105,10 @@
 
 void quadCommand::updateMotors() 
 {
-
     updateCurrent();
     float throttle, pitch, roll, yaw;   
         
-    throttle = targetThrottle;
+    throttle = targetThrottle * 5;  // Increased throttle Range to match higher resolution
     pitch = pidPitch->correct(currentPitch, targetPitch, MOTOR_UPDATE);
     roll = pidRoll->correct(currentRoll, targetRoll, MOTOR_UPDATE);
     yaw = pidYaw->correct(currentYaw, targetYaw, MOTOR_UPDATE);
@@ -129,15 +130,15 @@
 
 void quadCommand::updateCurrent()
 {
-//    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)
+    if (timestamp > timestamp_old)// Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+        G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; 
     else
-    G_Dt = 0;
+        G_Dt = 0;
+        
     world->Update_step(G_Dt);
+
     currentPitch    = world->pitch - pitchTrim;
     currentRoll     = world->roll - rollTrim;
     currentYaw      = 0 - yawTrim;