New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

Revision:
2:969dfa4f2436
Parent:
0:c6a85bb2a827
Child:
3:4823d6750629
--- a/NavigationController/NavigationController.cpp	Wed Mar 04 18:53:43 2015 +0000
+++ b/NavigationController/NavigationController.cpp	Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,189 @@
+#include "NavigationController.h"
+
+NavigationController::NavigationController(Status& status, Sensors& sensors, Rc& rc, ConfigFileWrapper& configFileWrapper) : 
+_status(status), _sensors(sensors), _rc(rc), _configFileWrapper(configFileWrapper)
+{
+    _altitudeController = new AltitudeController(_sensors, _configFileWrapper, _status);
+    _altitudeHoldPidOutput = 0;
+    
+    _thread = new Thread(&NavigationController::threadStarter, this, osPriorityHigh);
+
+    DEBUG("Navigation controller initialised\r\n");    
+}
+
+NavigationController::~NavigationController(){}
+
+void NavigationController::threadStarter(void const *p)
+{
+    NavigationController *instance = (NavigationController*)p;
+    instance->threadWorker();
+}
+
+void NavigationController::threadWorker()
+{
+    while(true)
+    {
+        //Get latest sensor readings
+        _sensors.update();
+        
+        //Get state
+        _state = _status.getState();
+        
+        //Get navigation mode
+        _navigationMode = _status.getNavigationMode();
+        
+        //Get Rc commands
+        _mappedRc = _rc.getMappedRc();
+        
+        //Get angle
+        _angle = _sensors.getAngle();
+        
+        //Reset accel data if not flying
+        if(_state == Status::PREFLIGHT || _state == Status::STANDBY)
+        {
+            //Reset accel
+            _sensors.zeroAccel();
+        }
+        
+        //Update yaw target
+        if(abs(_mappedRc.yaw) >= 5 || _state == Status::PREFLIGHT || _state == Status::STANDBY) updateYawTarget();
+             
+        //Make sure we are initialised         
+        if(_state != Status::PREFLIGHT)
+        {            
+            //Update yaw difference
+            _setPoints.yawDifference = MOD(_setPoints.yawTarget - _angle.yaw); 
+            
+            if(_navigationMode == Status::NONE)
+            {
+                //Motors are directly controlled by rc remote
+                if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
+                else _status.setMotorsSpinning(false);
+                
+                //Update target altitude 
+                updateAltitudeTarget();
+            }
+            else if(_navigationMode == Status::ALTITUDE_HOLD)
+            {
+                //Check if throttle is in dead zone
+                if(_status.getDeadZone() == true) _setPoints.climbRate = 0;
+                else
+                {
+                    //Throttle not in dead zone so map to climb rate
+                    _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE);
+                    
+                    //Update target altitude
+                    updateAltitudeTarget();
+                }
+                
+                //Get PID output
+                _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
+            }
+            
+            if(_state == Status::STANDBY)  
+            {
+                _setPoints.yaw = 0;
+                _setPoints.pitch = 0;
+                _setPoints.roll = 0;
+                _setPoints.throttle = 0; 
+                
+                _altitudeController->reset();
+            }
+            else if(_state == Status::GROUND_READY)  
+            {
+                _setPoints.yaw = _mappedRc.yaw;
+                _setPoints.pitch = _mappedRc.pitch;
+                _setPoints.roll = _mappedRc.roll;
+                _setPoints.throttle = _mappedRc.throttle; 
+                
+                _altitudeController->reset();
+            }
+            else if(_state == Status::MANUAL)  
+            {
+                _setPoints.yaw = _mappedRc.yaw;
+                _setPoints.pitch = _mappedRc.pitch;
+                _setPoints.roll = _mappedRc.roll;
+                _setPoints.throttle = _mappedRc.throttle; 
+                
+                _altitudeController->reset();
+            }
+            else if(_state == Status::STABILISED)
+            {
+                _setPoints.yaw = _mappedRc.yaw;
+                _setPoints.pitch = _mappedRc.pitch;
+                _setPoints.roll = _mappedRc.roll;
+                _setPoints.throttle = _altitudeHoldPidOutput;
+            }
+            else if(_state == Status::AUTO)
+            {
+                //Waypoint navigation
+            }
+        }
+        //Not initialised
+        else
+        {
+            _setPoints.yaw = 0;
+            _setPoints.pitch = 0;
+            _setPoints.roll = 0;
+            _setPoints.throttle = 0;
+            
+            _altitudeController->reset();
+        }
+        
+        Thread::wait(20);
+    }
+}
+
+NavigationController::SetPoint NavigationController::getSetPoint()
+{
+    return _setPoints;
+}
+
+void NavigationController::updateYawTarget()
+{
+   _setPoints.yawTarget = _angle.yaw;
+}
+
+void NavigationController::updateAltitudeTarget()
+{
+    _altitude = _sensors.getAltitude();
+    _setPoints.targetAltitude = _altitude.computed;
+    
+    if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN;
+    else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX;
+}
+  
+double NavigationController::map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
+{
+    return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
+}  
+
+PidWrapper::NavigationControllerPidParameters NavigationController::getPidParameters()
+{
+    return _altitudeController->getPidParameters();
+}
+
+void NavigationController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _altitudeController->setAltitudeRatePidParameters(pidParameters);
+    _configFileWrapper.setAltitudeRateParameters(pidParameters);
+    saveSettings();
+}
+
+void NavigationController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _altitudeController->setAltitudeStabPidParameters(pidParameters);
+    _configFileWrapper.setAltitudeStabParameters(pidParameters);
+    saveSettings();
+}
+
+void NavigationController::saveSettings()
+{
+    Status::State state = _status.getState(); 
+    if(state == Status::STANDBY || state == Status::PREFLIGHT)
+    {
+        _sensors.enable(false);
+        _configFileWrapper.saveSettings();
+        _sensors.enable(true);
+    }   
+}
\ No newline at end of file