Diff: flightController.h

Revision:

0:0010a5abcc31

Child:

1:045edcf091f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/flightController.h	Fri May 09 10:04:36 2014 +0000
@@ -0,0 +1,234 @@
+#include "mbed.h"
+#include "rtos.h"
+#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 Task50Hz();
+void Task10Hz();
+
+//Variables
+float _gyroRate[3] ={}; // Yaw, Pitch, Roll
+float _rcConstrainedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust
+float _yawTarget = 0;
+int _notFlying = 0; 
+float _altitude = 0;
+int _50HzIterator = 0;
+int _10HzIterator = 0;
+
+// A thread to monitor the serial ports
+void FlightControllerThread(void const *args) 
+{  
+    //Rtos Timers
+    _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0);
+    int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
+    _updateTimer->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()
+{
+    //Take off zero values to account for any angle inbetween the PCB and quadcopter chassis
+    _ypr[0] = _ypr[0] - _zeroValues[0];
+    _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)
+{
+    _50HzIterator++;
+    if(_50HzIterator % 10 == 0)
+    {
+        Task50Hz();
+    }
+
+    //Get IMU data
+    _freeIMU.getYawPitchRoll(_ypr);
+    _freeIMU.getRate(_gyroRate);
+    GetAttitude();
+    
+    //Rate mode
+    if(_rate == true && _stab == false)
+    {
+        //Update rate PID process value with gyro rate
+        _yawRatePIDController->setProcessValue(_gyroRate[0]);
+        _pitchRatePIDController->setProcessValue(_gyroRate[1]);
+        _rollRatePIDController->setProcessValue(_gyroRate[2]);
+        
+        //Update rate PID set point with desired rate from RC
+        _yawRatePIDController->setSetPoint(_rcConstrainedCommands[0]);
+        _pitchRatePIDController->setSetPoint(_rcConstrainedCommands[1]);
+        _rollRatePIDController->setSetPoint(_rcConstrainedCommands[2]);
+        
+        //Compute rate PID outputs
+        _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
+        _ratePIDControllerOutputs[1] = -_pitchRatePIDController->compute();
+        _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
+    }
+    //Stability mode
+    else
+    {
+        //Update stab PID process value with ypr
+        _yawStabPIDController->setProcessValue(_ypr[0]);
+        _pitchStabPIDController->setProcessValue(_ypr[1]);
+        _rollStabPIDController->setProcessValue(_ypr[2]);
+        
+        //Update stab PID set point with desired angle from RC
+        _yawStabPIDController->setSetPoint(_yawTarget);
+        _pitchStabPIDController->setSetPoint(_rcConstrainedCommands[1]);
+        _rollStabPIDController->setSetPoint(_rcConstrainedCommands[2]);
+        
+        //Compute stab PID outputs
+        _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
+        _stabPIDControllerOutputs[1] = -_pitchStabPIDController->compute();
+        _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();
+        
+        //if pilot commanding yaw
+        if(abs(_rcConstrainedCommands[0]) > 5)
+        {  
+            _stabPIDControllerOutputs[0] = _rcConstrainedCommands[0];  //Feed to rate PID (overwriting stab PID output)
+            _yawTarget = _ypr[0];
+        }
+        
+        //Update rate PID process value with gyro rate
+        _yawRatePIDController->setProcessValue(_gyroRate[0]);
+        _pitchRatePIDController->setProcessValue(_gyroRate[1]);
+        _rollRatePIDController->setProcessValue(_gyroRate[2]);
+        
+        //Update rate PID set point with desired rate from stab PID
+        _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
+        _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
+        _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);
+        
+        //Compute rate PID outputs
+        _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
+        _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
+        _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
+    }
+    
+    //Testing
+    //_ratePIDControllerOutputs[0] = 0; // yaw
+    //_ratePIDControllerOutputs[1] = 0; // pitch
+    //_ratePIDControllerOutputs[2] = 0; // roll
+
+    //Calculate motor power if flying
+    if(_rcCommands[3] > 0 && _armed == true)
+    {
+        //Constrain motor power to 1, this means at max throttle there is no overhead for stabilising
+        _motorPower[0] = Constrain((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[1] = Constrain((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[2] = Constrain((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
+        _motorPower[3] = Constrain((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
+    
+        
+        //Map 0-1 value to actual pwm pulsewidth 1060 - 1860
+        _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1300); //Reduced to 1300 to limit power for testing
+        _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1300);
+        _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1300);
+        _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1300);
+    }
+
+    //Not flying
+    else if(_armed == true)
+    {
+        //Set motors to armed state
+        _motorPower[0] = MOTORS_ARMED;
+        _motorPower[1] = MOTORS_ARMED;
+        _motorPower[2] = MOTORS_ARMED;
+        _motorPower[3] = MOTORS_ARMED;
+        
+        _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();
+        }
+    } 
+    else
+    {
+        //Disable Motors
+        _motorPower[0] = MOTORS_OFF;
+        _motorPower[1] = MOTORS_OFF;
+        _motorPower[2] = MOTORS_OFF;
+        _motorPower[3] = MOTORS_OFF;
+    }
+    
+    //Set motor power
+    _motor1.pulsewidth_us(_motorPower[0]);
+    _motor2.pulsewidth_us(_motorPower[1]);
+    _motor3.pulsewidth_us(_motorPower[2]);
+    _motor4.pulsewidth_us(_motorPower[3]);
+}
+
+void Task50Hz()
+{
+    //Get RC control values
+    
+    //Constrain
+    //Rate mode
+    if(_rate == true && _stab == false)
+    {
+        _rcConstrainedCommands[0] = Constrain(_rcCommands[0], RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
+        _rcConstrainedCommands[1] = Constrain(_rcCommands[1], RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
+        _rcConstrainedCommands[2] = Constrain(_rcCommands[2], RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
+        _rcConstrainedCommands[3] = Constrain(_rcCommands[3], RC_THRUST_MIN, RC_THRUST_MAX);
+    }
+    //Stability  
+    else
+    {
+        _rcConstrainedCommands[0] = Constrain(_rcCommands[0], RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
+        _rcConstrainedCommands[1] = Constrain(_rcCommands[1], RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
+        _rcConstrainedCommands[2] = Constrain(_rcCommands[2], RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
+        _rcConstrainedCommands[3] = Constrain(_rcCommands[3], RC_THRUST_MIN, RC_THRUST_MAX);
+    }
+    
+    //10Hz Task
+    _10HzIterator++;
+    if(_10HzIterator % 5 == 0)
+    {
+        Task10Hz();
+    }
+}
+
+//Print data
+void Task10Hz()
+{
+    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], _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);    
+}
\ No newline at end of file