Diff: FlightController/FlightController.cpp

Revision:

2:969dfa4f2436

Parent:

0:c6a85bb2a827

--- a/FlightController/FlightController.cpp	Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/FlightController.cpp	Wed Apr 01 11:19:21 2015 +0000
@@ -1,24 +1,21 @@
 #include "FlightController.h"
 
-FlightController::FlightController(){}
+FlightController::FlightController(Status& status, Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper, PinName motor1, PinName motor2, PinName motor3, PinName motor4)
+    : _status(status), _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
+{
+    _motorMixer = new MotorMixer(motor1, motor2, motor3, motor4);
+    _rateController = new RateController(_sensors, _navigationController, _configFileWrapper);
+    _stabController = new StabController(_sensors, _navigationController, _configFileWrapper);   
+
+    _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void*)this);
+    int updateTime = (1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY) * 1000;
+    _rtosTimer->start(updateTime);
+    
+    DEBUG("Flight controller initialised\r\n");
+}
 
 FlightController::~FlightController(){}
 
-bool FlightController::initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4)
-{
-    _status = status;
-    
-    _motorMixer.initialise(motor1, motor2, motor3, motor4);
-    _rateController.initialise(sensors, navigationController);
-    _stabController.initialise(sensors, navigationController);
-   
-    _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void *)0);
-    int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
-    _rtosTimer->start(updateTime);
-    DEBUG("Flight controller initialised");
-    return true; 
-}
-
 void FlightController::threadStarter(void const *p)
 {
     FlightController *instance = (FlightController*)p;
@@ -26,37 +23,123 @@
 }
 
 void FlightController::threadWorker()
-{
-    //Struct to hold PID outputs
-    PidWrapper::PidOutputs pidOutputs;
+{   
+    Status::FlightMode flightMode = _status.getFlightMode();
+    
+    _sensors.updateImu();
     
-    //Check state is valid
+    if(flightMode == Status::RATE)
+    {
+        //Rate mode  
+        _pidOutputs = _rateController->compute(); 
+    }
+    else if (flightMode == Status::STAB)
+    {
+        //Stab mode
+        _pidOutputs = _stabController->compute();  
+    }
+      
     Status::State state = _status.getState();
+    NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
+    
     if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO)  
     {
-        //State valid
-        Status::FlightMode flightMode = _status.getFlightMode();
-        if(flightMode == Status::RATE)
-        {
-            //Rate mode  
-            pidOutputs = _rateController.compute(); 
-        }
-        else if (flightMode == Status::STAB)
-        {
-            //Stab mode
-            pidOutputs = _stabController.compute();    
-        }
-        else
-        {
-            //Invalid state
-            _status.setState(Status::ABORT);
-            return;
-        }  
-        
-        _motorMixer.computePower(pidOutputs, throttle);       
+        _motorMixer->computePower(_pidOutputs, setPoints.throttle);
+    }      
+    else if(state == Status::GROUND_READY) 
+    {
+        _motorMixer->setPower(MOTORS_ARMED);
+        _rateController->reset();
+        _stabController->reset(); 
+    }
+    else if(state == Status::STANDBY)
+    {
+        _motorMixer->setPower(MOTORS_OFF);
+        _rateController->reset();
+        _stabController->reset();
     }
-    //Set motors to armed if state is ground ready
-    else if (state == Status::GROUND_READY) _motorMixer.setPower(MOTORS_ARMED);
     //Disable motors if state is not valid
-    else _motorMixer.setPower(MOTORS_OFF);
+    else _motorMixer->setPower(MOTORS_OFF);
+}
+
+MotorMixer::MotorPower FlightController::getMotorPower()
+{
+    return _motorMixer->getMotorPower();
+}
+
+PidWrapper::PidOutput FlightController::getPidOutputs()
+{
+    return _pidOutputs;  
+}
+
+PidWrapper::FlightControllerPidParameters FlightController::getPidParameters()
+{
+    return _stabController->getPidParameters();
+}
+
+void FlightController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _stabController->setYawStabPidParameters(pidParameters);
+    _configFileWrapper.setYawStabParameters(pidParameters); 
+    Status::State state = _status.getState(); 
+    saveSettings();
+}
+
+void FlightController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _stabController->setPitchStabPidParameters(pidParameters);
+    _configFileWrapper.setPitchStabParameters(pidParameters);
+    saveSettings();
+}
+
+void FlightController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _stabController->setRollStabPidParameters(pidParameters);
+    _configFileWrapper.setRollStabParameters(pidParameters);
+    saveSettings();
 }
+
+void FlightController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _stabController->setYawRatePidParameters(pidParameters);
+    _rateController->setYawRatePidParameters(pidParameters);
+    _configFileWrapper.setYawRateParameters(pidParameters);
+    saveSettings();
+}
+
+void FlightController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _stabController->setPitchRatePidParameters(pidParameters);
+    _rateController->setPitchRatePidParameters(pidParameters);
+    _configFileWrapper.setPitchRateParameters(pidParameters);
+    saveSettings();
+}
+
+void FlightController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _stabController->setRollRatePidParameters(pidParameters);
+    _rateController->setRollRatePidParameters(pidParameters);
+    _configFileWrapper.setRollRateParameters(pidParameters);
+    saveSettings();
+}
+
+void FlightController::saveSettings()
+{
+    Status::State state = _status.getState(); 
+    if(state == Status::STANDBY || state == Status::PREFLIGHT)
+    {
+        _sensors.enable(false);
+        _configFileWrapper.saveSettings();
+        _sensors.enable(true);
+    }   
+}
+
+PidWrapper::RatePidState FlightController::getRatePidState()
+{
+    return _rateController->getRatePidState();
+}
+
+PidWrapper::StabPidState FlightController::getStabPidState()
+{
+    return _stabController->getStabPidState();
+}
\ No newline at end of file