Diff: flightController.h

Revision:

3:82665e39f1ea

Parent:

2:b3b771c8f7d1

Child:

5:7b7db24ef6eb

--- a/flightController.h	Fri May 16 14:22:18 2014 +0000
+++ b/flightController.h	Thu Sep 18 08:45:46 2014 +0000
@@ -3,79 +3,34 @@
 #include "hardware.h"
 
 //Declarations
-float Constrain(const float in, const float min, const float max);
-float map(float x, float in_min, float in_max, float out_min, float out_max);
 void GetAttitude();
-void Task500Hz(void const *n);
-void Task10Hz();
+void FlightControllerTask(void const *n);
+void NotFlying();
 
 //Variables
-float _gyroRate[3] ={}; // Yaw, Pitch, Roll
-float _ypr[3] = {0,0,0}; // Yaw, pitch, roll
-float _yawTarget = 0;
-int _notFlying = 0; 
-float _altitude = 0;
-int _10HzIterator = 0;
-float _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
-float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
-float _motorPower [4] = {0,0,0,0};
-
+float           _yawTarget = 0;
+int             _notFlying = 0; 
+float           _altitude = 0;
+double          _basePower = 0;
 
 //Timers
-RtosTimer *_updateTimer;
+RtosTimer       *_flightControllerUpdateTimer;
 
-// A thread to monitor the serial ports
+// A thread to control flight
 void FlightControllerThread(void const *args) 
-{  
+{         
     //Update Timer
-    _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0);
-    int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
-    _updateTimer->start(updateTime);
+    _flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0);
+    int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
+    _flightControllerUpdateTimer->start(updateTime);
     
     // Wait here forever
     Thread::wait(osWaitForever);
 }
 
-//Constrains value to between min and max
-float Constrain(const float in, const float min, const float max)
-{
-    float out = in;
-    out = out > max ? max : out;
-    out = out < min ? min : out;
-    return out;
-}
-
-//Maps input to output
-float map(float x, float in_min, float in_max, float out_min, float out_max)
-{
-    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
-//Zeros values
-void GetAttitude()
+void FlightControllerTask(void const *n)
 {
-    //Take off zero values to account for any angle inbetween the PCB level and ground
-    _ypr[1] = _ypr[1] - _zeroValues[1];
-    _ypr[2] = _ypr[2] - _zeroValues[2];
-    
-    //Swap pitch and roll because IMU is mounted at a right angle to the board
-    float pitch = _ypr[2];
-    float roll = _ypr[1];
-    _ypr[1] = pitch;
-    _ypr[2] = roll;
-}
-
-void Task500Hz(void const *n)
-{
-    _10HzIterator++;
-    if(_10HzIterator % 50 == 0)
-    {
-        Task10Hz();
-    }
-
     //Get IMU data
-    _freeIMU.getYawPitchRoll(_ypr);
-    _freeIMU.getRate(_gyroRate);
     GetAttitude();
     
     //Rate mode
@@ -95,9 +50,14 @@
         _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
         _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
         _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
+        
+        //Set stability PID outputs to 0
+        _stabPIDControllerOutputs[0] = 0;
+        _stabPIDControllerOutputs[1] = 0;
+        _stabPIDControllerOutputs[2] = 0;
     }
     //Stability mode
-    else
+    else if(_rate == false && _stab == true)
     {
         //Update stab PID process value with ypr
         _yawStabPIDController->setProcessValue(_ypr[0]);
@@ -140,23 +100,44 @@
     //Testing
     _ratePIDControllerOutputs[0] = 0; // yaw
     //_ratePIDControllerOutputs[1] = 0; // pitch
-    _ratePIDControllerOutputs[2] = 0; // roll
+    //_ratePIDControllerOutputs[2] = 0; // roll
+    _stabPIDControllerOutputs[0] = 0; // yaw
+    //_stabPIDControllerOutputs[1] = 0; // pitch
+    //_stabPIDControllerOutputs[2] = 0; // roll
 
     //Calculate motor power if flying
-    if(_rcMappedCommands[3] > 0.1 && _armed == true)
+    //RC Mapped thottle is between 0 and 1
+    //Add 0.1 to try to avoid false starts
+    if(_rcMappedCommands[3] > (RC_THRUST_MIN + 0.1) && _armed == true)
     {
-        //Constrain motor power to 1, this means at max throttle there is no overhead for stabilising
-        _motorPower[0] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
-        _motorPower[1] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
-        _motorPower[2] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
-        _motorPower[3] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
-    
+        //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
+        float basePower = MOTORS_MIN + (_rcMappedCommands[3] * 800);
+        
+        //Map motor power - each PID returns -100 <-> 100
+        _motorPower[0] = basePower + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
+        _motorPower[1] = basePower + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
+        _motorPower[2] = basePower - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0];
+        _motorPower[3] = basePower - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0];
         
-        //Map 0-1 value to actual pwm pulsewidth 1060 - 1860
-        _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1500); //Reduced to 1500 to limit power for testing
-        _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1500);
-        _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1500);
-        _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1500);
+        //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
+        float motorFix = 0;
+        float motorMin = _motorPower[0];
+        float motorMax = _motorPower[0];
+        for(int i=1; i<4; i++)
+        {
+            if(_motorPower[i] < motorMin) motorMin = _motorPower[i];
+            if(_motorPower[i] > motorMax) motorMax = _motorPower[i];
+        }
+               
+        //Check if min or max is outside of the limits
+        if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
+        else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
+        
+        //Add/remove constant if neccessary
+        for(int i=0; i<4; i++)
+        {
+            _motorPower[i] = _motorPower[i] + motorFix;
+        }
     }
 
     //Not flying
@@ -173,16 +154,7 @@
         _notFlying ++;
         if(_notFlying > 500) //Not flying for 1 second
         {
-            //Reset iteratior
-            _notFlying = 0;
-            
-            //Reset I
-            _yawRatePIDController->reset();
-            _pitchRatePIDController->reset();
-            _rollRatePIDController->reset();
-            _yawStabPIDController->reset();
-            _pitchStabPIDController->reset();
-            _rollStabPIDController->reset();
+            NotFlying();
         }
     } 
     else
@@ -192,6 +164,12 @@
         _motorPower[1] = MOTORS_OFF;
         _motorPower[2] = MOTORS_OFF;
         _motorPower[3] = MOTORS_OFF;
+        
+        _notFlying ++;
+        if(_notFlying > 500) //Not flying for 1 second
+        {
+            NotFlying();
+        }
     }
     
     //Set motor power
@@ -201,10 +179,40 @@
     _motor4.pulsewidth_us(_motorPower[3]);
 }
 
-//Print data
-void Task10Hz()
+void GetAttitude()
 {
-    int batt = 0;
-    _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f>",
-    _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);    
+    //Get raw data from IMU
+    _freeIMU.getYawPitchRoll(_ypr);
+    _freeIMU.getRate(_gyroRate);
+    
+    //Take off zero values to account for any angle inbetween the PCB level and ground
+    _ypr[1] = _ypr[1] - _zeroValues[1];
+    _ypr[2] = _ypr[2] - _zeroValues[2];
+    
+    //Swap pitch and roll angle because IMU is mounted at a right angle to the board
+    //Rate does not need to be switched
+    float pitch = _ypr[2];
+    float roll = -_ypr[1]; //Needs to be negative
+    _ypr[1] = pitch;
+    _ypr[2] = roll;
+    
+    //Try swapping yaw
+    //_ypr[0] = - _ypr[0];
+}
+
+void NotFlying()
+{
+    //Reset iteratior
+    _notFlying = 0;
+    
+    //Zero gyro
+    _freeIMU.zeroGyro();
+    
+    //Reset I
+    _yawRatePIDController->reset();
+    _pitchRatePIDController->reset();
+    _rollRatePIDController->reset();
+    _yawStabPIDController->reset();
+    _pitchStabPIDController->reset();
+    _rollStabPIDController->reset();
 }
\ No newline at end of file