flightController.h

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "hardware.h"
00004 
00005 //Declarations
00006 void GetAttitude();
00007 void FlightControllerTask(void const *n);
00008 void NotFlying();
00009 
00010 //Variables
00011 int             _notFlying = 0; 
00012 
00013 //Timers
00014 RtosTimer       *_flightControllerUpdateTimer;
00015 
00016 // A thread to control flight
00017 void FlightControllerThread(void const *args) 
00018 {    
00019     printf("Flight controller thread started\r\n");
00020      
00021     //Update Timer
00022     _flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0);
00023     int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
00024     _flightControllerUpdateTimer->start(updateTime);
00025     
00026     // Wait here forever
00027     Thread::wait(osWaitForever);
00028 }
00029 
00030 void FlightControllerTask(void const *n)
00031 {
00032     //Get IMU data
00033     GetAttitude();
00034     
00035     //Rate mode
00036     if(_rate == true && _stab == false)
00037     {
00038         //Update rate PID process value with gyro rate
00039         _yawRatePIDController->setProcessValue(_gyroRate[0]);
00040         _pitchRatePIDController->setProcessValue(_gyroRate[1]);
00041         _rollRatePIDController->setProcessValue(_gyroRate[2]);
00042         
00043         //Update rate PID set point with desired rate from RC
00044         _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
00045         _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
00046         _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
00047         
00048         //Compute rate PID outputs
00049         _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
00050         _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
00051         _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
00052         
00053         //Set stability PID outputs to 0
00054         _stabPIDControllerOutputs[0] = 0;
00055         _stabPIDControllerOutputs[1] = 0;
00056         _stabPIDControllerOutputs[2] = 0;
00057     }
00058     //Stability mode
00059     else if(_rate == false && _stab == true)
00060     {
00061         //Update stab PID process value with ypr
00062         _yawStabPIDController->setProcessValue(_ypr[0]);
00063         _pitchStabPIDController->setProcessValue(_ypr[1]);
00064         _rollStabPIDController->setProcessValue(_ypr[2]);
00065         
00066         //Update stab PID set point with desired angle from RC
00067         _yawStabPIDController->setSetPoint(_yawTarget);
00068         _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
00069         _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);
00070         
00071         //Compute stab PID outputs
00072         _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
00073         _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
00074         _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
00075         
00076         //If pilot commanding yaw
00077         if(abs(_rcMappedCommands[0]) > 0)
00078         {  
00079             _stabPIDControllerOutputs[0] = _rcMappedCommands[0];  //Feed to rate PID (overwriting stab PID output)
00080             _yawTarget = _ypr[0];
00081         }
00082         
00083         //Update rate PID process value with gyro rate
00084         _yawRatePIDController->setProcessValue(_gyroRate[0]);
00085         _pitchRatePIDController->setProcessValue(_gyroRate[1]);
00086         _rollRatePIDController->setProcessValue(_gyroRate[2]);
00087         
00088         //Update rate PID set point with desired rate from stab PID
00089         _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
00090         _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
00091         _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
00092         
00093         //Compute rate PID outputs
00094         _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
00095         _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
00096         _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
00097     }
00098     
00099     //Testing
00100     //_ratePIDControllerOutputs[0] = 0; // yaw
00101     //_ratePIDControllerOutputs[1] = 0; // pitch
00102     //_ratePIDControllerOutputs[2] = 0; // roll
00103     //_stabPIDControllerOutputs[0] = 0; // yaw
00104     //_stabPIDControllerOutputs[1] = 0; // pitch
00105     //_stabPIDControllerOutputs[2] = 0; // roll
00106 
00107     //Calculate motor power if flying
00108     //RC Mapped thottle is between 0 and 1
00109     //Add 0.2 to try to avoid false starts
00110     if(_rcMappedCommands[3] > (RC_THRUST_MIN + 0.2) && _armed == true)
00111     {
00112         //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
00113         float basePower = MOTORS_MIN + (_rcMappedCommands[3] * 800);
00114         
00115         //Map motor power - each PID returns -100 <-> 100
00116         _motorPower[0] = basePower + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
00117         _motorPower[1] = basePower + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
00118         _motorPower[2] = basePower - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
00119         _motorPower[3] = basePower - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
00120         
00121         //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
00122         float motorFix = 0;
00123         float motorMin = _motorPower[0];
00124         float motorMax = _motorPower[0];
00125         
00126         for(int i=1; i<4; i++)
00127         {
00128             if(_motorPower[i] < motorMin) motorMin = _motorPower[i];
00129             if(_motorPower[i] > motorMax) motorMax = _motorPower[i];
00130         }
00131                
00132         //Check if min or max is outside of the limits
00133         if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
00134         else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
00135         
00136         //Add/remove constant if neccessary
00137         for(int i=0; i<4; i++)
00138         {
00139             _motorPower[i] = _motorPower[i] + motorFix;
00140         }
00141     }
00142 
00143     //Not flying
00144     else if(_armed == true)
00145     {
00146         _yawTarget = _ypr[0];
00147         
00148         //Set motors to armed state
00149         _motorPower[0] = MOTORS_ARMED;
00150         _motorPower[1] = MOTORS_ARMED;
00151         _motorPower[2] = MOTORS_ARMED;
00152         _motorPower[3] = MOTORS_ARMED;
00153         
00154         _notFlying ++;
00155         if(_notFlying > 500) //Not flying for 1 second
00156         {
00157             NotFlying();
00158         }
00159     } 
00160     else
00161     {
00162         //Disable Motors
00163         _motorPower[0] = MOTORS_OFF;
00164         _motorPower[1] = MOTORS_OFF;
00165         _motorPower[2] = MOTORS_OFF;
00166         _motorPower[3] = MOTORS_OFF;
00167         
00168         _notFlying ++;
00169         if(_notFlying > 500) //Not flying for 1 second
00170         {
00171             NotFlying();
00172         }
00173     }
00174     
00175     //Set motor power
00176     _motor1.pulsewidth_us(_motorPower[0]);
00177     _motor2.pulsewidth_us(_motorPower[1]);
00178     _motor3.pulsewidth_us(_motorPower[2]);
00179     _motor4.pulsewidth_us(_motorPower[3]);
00180 }
00181 
00182 void GetAttitude()
00183 {
00184     
00185     //Get raw data from IMU
00186     _freeIMU.getYawPitchRoll(_ypr);
00187     _freeIMU.getRate(_gyroRate);
00188     
00189     //Take off zero values to account for any angle inbetween level and ground
00190     //_ypr[1] = _ypr[1] - _zeroValues[1];
00191     //_ypr[2] = _ypr[2] - _zeroValues[2];
00192     
00193     //Swap pitch and roll angle because IMU is mounted at a right angle to the board
00194     float pitch = _ypr[2];
00195     float roll = -_ypr[1];
00196     _ypr[1] = pitch;
00197     _ypr[2] = roll;
00198     
00199     _ypr[0] = _ypr[0];
00200     
00201     //Swap pitch, roll and yaw rate because IMU is mounted at a right angle to the board
00202     float yaw = _gyroRate[2];
00203     pitch = _gyroRate[0];
00204     roll = _gyroRate[1];
00205     _gyroRate[0] = yaw;
00206     _gyroRate[1] = pitch;
00207     _gyroRate[2] = roll;
00208 }
00209 
00210 void NotFlying()
00211 {
00212     //Reset iteratior
00213     _notFlying = 0;
00214     
00215     //Zero gyro
00216     _freeIMU.zeroGyro();
00217     
00218     //Reset I
00219     _yawRatePIDController->reset();
00220     _pitchRatePIDController->reset();
00221     _rollRatePIDController->reset();
00222     _yawStabPIDController->reset();
00223     _pitchStabPIDController->reset();
00224     _rollStabPIDController->reset();
00225 }