File content as of revision 62:70fc58d4be9b:

/************************ quadCommand.cpp ********************************/
/*                                                                       */
/*************************************************************************/

#include "quadCommand.h"
#include "DCM.h"

quadCommand::quadCommand()
{ 
        // Initialize variables to zero
        targetThrottle  = currentThrottle   = 0;    
        targetPitch     = currentPitch      = 0;
        targetRoll      = currentRoll       = 0;
        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();    
        DCM *world = new DCM();
        
        // Create PID instances.
        pidPitch = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
        pidRoll = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
        pidYaw = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
        
        myMotors[0] = new motor( MOTOR1 );  // Connect motor 0 to PTD4 pin.
        myMotors[1] = new motor( MOTOR2 );  // Connect motor 1 to PTA12 pin. 
        myMotors[2] = new motor( MOTOR3 );  // Connect motor 2 to PTA4 pin. 
        myMotors[3] = new motor( MOTOR4 );  // Connect motor 3 to PTA5 pin.  
        
        motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);    
}

/******************************* run() ***********************************/
/*                                                                       */
/*************************************************************************/

void quadCommand::run() 
{   
    while(1)
    {     
        if( myCom->isData() ) 
            rxInput();    
    }
}

/***************************** rxInput() *********************************/
/* Commands:    0 -> Ack, does not get placed in rxQueue.                */
/*              1 -> Throttle                                            */
/*              2 -> Pitch                                               */
/*              3 -> Roll                                                */
/*              4 -> Yaw                                                 */
/*              5 -> Roll Trim Left                                      */
/*              6 -> Roll Trim Right                                     */
/*              7 -> Pitch Trim Forward                                  */
/*              8 -> Pitch Trim Back                                     */
/*************************************************************************/

void quadCommand::rxInput()
{
    short * command = myCom->read();
            
    if( command[0] == 1 )       // Throttle command.
        targetThrottle = command[1];
                
    if( command[0] == 2 )       // Pitch command.
        targetPitch = command[1];
                
    if( command[0] == 3 )       // Roll command.
        targetRoll = command[1];            
            
    if( command[0] == 4 )      // Yaw command.
        targetYaw = command[1];
            
    if( command[0] == 5 )      // Roll Trim Left.
        rollTrim += TRIM_VALUE;
        
    if( command[0] == 6 )      // Roll Trim Right.
        rollTrim -= TRIM_VALUE;
        
    if( command[0] == 7 )      // Pitch Trim Forward
        pitchTrim += TRIM_VALUE;          
        
    if( command[0] == 8 )      // Pitch Trim Back
        pitchTrim -= TRIM_VALUE;
        
    if( command[0] == 9 )      // Pitch Trim Forward
        yawTrim += TRIM_VALUE;          
        
    if( command[0] == 10 )      // Pitch Trim Back
        yawTrim -= TRIM_VALUE;
        
    delete[] command;
}


/*************************** updateMotors() ******************************/
/*Called in main by motorprocess                                         */
/*************************************************************************/

void quadCommand::updateMotors() 
{

    updateCurrent();
    float throttle, pitch, roll, yaw;   
        
    throttle = targetThrottle;
    pitch = pidPitch->correct(currentPitch, targetPitch, MOTOR_UPDATE);
    roll = pidRoll->correct(currentRoll, targetRoll, MOTOR_UPDATE);
    yaw = pidYaw->correct(currentYaw, targetYaw, MOTOR_UPDATE);

    int go = 0;
    if( targetThrottle > 0 )
        go = 1;
   
    // Set motor speeds
    myMotors[ 0 ]->setSpeed( go * ( throttle - roll - pitch + yaw));   
    myMotors[ 1 ]->setSpeed( go * ( throttle + roll - pitch - yaw));    
    myMotors[ 2 ]->setSpeed( go * ( throttle + roll + pitch + yaw));    
    myMotors[ 3 ]->setSpeed( go * ( throttle - roll + pitch - yaw));
}

/************************** updateCurrent() ******************************/
/*                                                                       */
/*************************************************************************/

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)
    else
    G_Dt = 0;
    world->Update_step(G_Dt);
    currentPitch    = world->pitch - pitchTrim;
    currentRoll     = world->roll - rollTrim;
    currentYaw      = 0 - yawTrim;
}