File content as of revision 55:bca9c9e92da6:

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

#include "quadCommand.h"

quadCommand::quadCommand() :
    // Proportional, Intagral, Derivitive, Windup
    pidThrottle(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
    pidPitch(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
    pidRoll(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
    pidYaw(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD)
{
        motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
        //sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSOR_DELAY/1000 );
        //sendProcess.attach( this, &quadCommand::sendTelemetry, SEND_DELAY/1000 );
     
        myCom = new com( TXPIN, RXPIN );            // Setup com object.       
        world = new sensors();                      // Setup the sensors.
        //pc = new Serial(USBTX, USBRX);
        
        myMotors[0] = new motor( MOTOR1 );          // Connect motor 0 to PTD4 pin. // motor 2
        myMotors[1] = new motor( MOTOR2 );          // Connect motor 1 to PTA12 pin. // motor 1
        myMotors[2] = new motor( MOTOR3 );          // Connect motor 2 to PTA4 pin. // motor 3
        myMotors[3] = new motor( MOTOR4 );          // Connect motor 3 to PTA5 pin. // motor 4
        
        myMotors[1]->setPulseMin(0.0011850);
        myMotors[2]->setPulseMin(0.00116875);
        myMotors[3]->setPulseMin(0.0011625);
        
        targetThrottle  = currentThrottle   = 0;
        targetPitch     = currentPitch      = 0;
        targetRoll      = currentRoll       = 0;
        targetYaw       = currentYaw        = 0;
        pitchTrim       = 0;
        rollTrim        = 0;
}

/******************************* 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 -> Motor 1 trimUp                                      */
/*              6 -> Motor 1 trimDown                                    */
/*              7 -> Motor 1 setTrim                                     */
/*              8 -> Motor 2 trimUp                                      */
/*              9 -> Motor 2 trimDown                                    */
/*              10-> Motor 2 setTrim                                     */
/*              11-> Motor 3 trimUp                                      */
/*              12-> Motor 3 trimDown                                    */
/*              13-> Motor 3 setTrim                                     */
/*              14-> Motor 4 trimUp                                      */
/*              15-> Motor 4 trimDown                                    */
/*              16-> Motor 4 setTrim                                     */
/*              17-> valueUp                                             */
/*              18-> valueDown                                           */
/*************************************************************************/

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;
        
     delete[] command;
}


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

void quadCommand::updateMotors() 
{
    updateCurrent();
    float throttle, pitch, roll, yaw;
    
    throttle = targetThrottle; //pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
    pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
    roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
    yaw = 0.0f; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
    
    //pc->printf("\r\n----------------------------------------\r\n");
    //pc->printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);
    //pc->printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);
    //pc->printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);
    //pc->printf("\r\n----------------------------------------\r\n");

    int go = 0;
    if( targetThrottle > 0 )
        go = 1;

    float speed0 = go * (throttle - roll - pitch + yaw);
    float speed1 = go * (throttle + roll - pitch - yaw);
    float speed2 = go * (throttle + roll + pitch - yaw);
    float speed3 = go * (throttle - roll + pitch + yaw);
    
    //DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]\r\n", speed0, speed1, speed2, speed3);)
    
    myMotors[ 0 ]->setSpeed(speed0);    // Motor 1
    myMotors[ 1 ]->setSpeed(speed1);    // Motor 2
    myMotors[ 2 ]->setSpeed(speed2);    // Motor 3
    myMotors[ 3 ]->setSpeed(speed3);    // Motor 4
}

/**************************** sendData() *********************************/
/*                                                                       */
/*************************************************************************/

void quadCommand::sendTelemetry()
{
    myCom->write( 2, (short)(currentPitch * 90) );
    myCom->write( 3, (short)(currentRoll * 90) );
}

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

void quadCommand::updateCurrent()
{
    currentPitch    = world->getAbsoluteX() - pitchTrim;
    currentRoll     = world->getAbsoluteY() - rollTrim;
}