added gy80 dcm
Dependencies: mbed DCM_AHRS_GY80 PID MMA8451Q
Fork of quadCommand by
Diff: quadCommand/quadCommand.cpp
- Revision:
- 63:f4cb482ac5d4
- Parent:
- 62:70fc58d4be9b
--- 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;