Diff: FlightController/RateController/RateController.cpp

Revision:

2:969dfa4f2436

Parent:

0:c6a85bb2a827

diff -r ec3521d90369 -r 969dfa4f2436 FlightController/RateController/RateController.cpp
--- a/FlightController/RateController/RateController.cpp	Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/RateController/RateController.cpp	Wed Apr 01 11:19:21 2015 +0000
@@ -1,39 +1,74 @@
 #include "RateController.h"
 
-RateController::RateController(){}
+RateController::RateController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper) : _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
+{
+    float updateTime = 1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY;
+    _yawRatePidController.initialise(_configFileWrapper.getYawRateParameters(), IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+    _pitchRatePidController.initialise(_configFileWrapper.getPitchRateParameters(), IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+    _rollRatePidController.initialise(_configFileWrapper.getRollRateParameters(), IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+    
+    DEBUG("Rate controller initialised\r\n");
+}
 
 RateController::~RateController(){}
 
-bool RateController::initialise(Sensors& sensors, NavigationController& navigationController)
-{
-    _sensors = sensors;
-    _navigationController = navigationController;
+PidWrapper::PidOutput RateController::compute()
+{   
+    _setPoints = _navigationController.getSetPoint();
+    _rate = _sensors.getRate();
     
-    ConfigFileWrapper configFileWrapper = new ConfigFileWrapper();
-    configFileWrapper.initialise();
-    
-    _yawRatePidController.initialise(configFileWrapper.getYawRateParameters, IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
-    _pitchRatePidController.initialise(configFileWrapper.getPitchRateParameters, IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
-    _rollRatePidController.initialise(configFileWrapper.getRollRateParameters, IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
+    _pidOutputs.yaw = _yawRatePidController.compute(_setPoints.yaw, _rate.yaw);
+    _pidOutputs.pitch = _pitchRatePidController.compute(_setPoints.pitch, _rate.pitch);
+    _pidOutputs.roll = _rollRatePidController.compute(_setPoints.roll, _rate.roll);
+
+    return _pidOutputs;
+}
 
-    DEBUG("Rate controller initialised");
-    return true; 
+void RateController::reset()
+{
+    _yawRatePidController.reset();
+    _pitchRatePidController.reset();
+    _rollRatePidController.reset();
+}
+
+
+void RateController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _yawRatePidController.setPidParameters(pidParameters);
+}
+
+void RateController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+    _pitchRatePidController.setPidParameters(pidParameters);
 }
 
-PidWrapper::PidOutputs RateController::compute()
+void RateController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
 {
-    //Update rate PID process value with gyro rate
-    _yawRatePIDController->setProcessValue(_gyroRate[0]);
-    _pitchRatePIDController->setProcessValue(_gyroRate[1]);
-    _rollRatePIDController->setProcessValue(_gyroRate[2]);
+    _rollRatePidController.setPidParameters(pidParameters);
+}
+
+PidWrapper::RatePidState RateController::getRatePidState()
+{
+    PidWrapper::RatePidState ratePidState;
+    PidWrapper::PidState yawRatePidState;
+    PidWrapper::PidState pitchRatePidState;
+    PidWrapper::PidState rollRatePidState;
     
-    //Update rate PID set point with desired rate from RC
-    _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
-    _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
-    _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
+    yawRatePidState.setPoint = _setPoints.yaw;
+    yawRatePidState.processValue = _rate.yaw;
+    yawRatePidState.output = _pidOutputs.yaw;
+    
+    pitchRatePidState.setPoint = _setPoints.pitch;
+    pitchRatePidState.processValue = _rate.pitch;
+    pitchRatePidState.output = _pidOutputs.pitch;
     
-    //Compute rate PID outputs
-    _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
-    _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
-    _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
+    rollRatePidState.setPoint = _setPoints.roll;
+    rollRatePidState.processValue = _rate.roll;
+    rollRatePidState.output = _pidOutputs.roll;
+    
+    ratePidState.yawRate = yawRatePidState;
+    ratePidState.pitchRate = pitchRatePidState;
+    ratePidState.rollRate = rollRatePidState;
+    
+    return ratePidState;
 }
\ No newline at end of file