Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

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

FlightController/RateController/RateController.cpp

Committer:
joe4465
Date:
2015-05-08
Revision:
4:9ffbf9101992
Parent:
2:969dfa4f2436

File content as of revision 4:9ffbf9101992:

#include "RateController.h"

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(){}

PidWrapper::PidOutput RateController::compute()
{   
    _setPoints = _navigationController.getSetPoint();
    _rate = _sensors.getRate();
    
    _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;
}

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);
}

void RateController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
{
    _rollRatePidController.setPidParameters(pidParameters);
}

PidWrapper::RatePidState RateController::getRatePidState()
{
    PidWrapper::RatePidState ratePidState;
    PidWrapper::PidState yawRatePidState;
    PidWrapper::PidState pitchRatePidState;
    PidWrapper::PidState rollRatePidState;
    
    yawRatePidState.setPoint = _setPoints.yaw;
    yawRatePidState.processValue = _rate.yaw;
    yawRatePidState.output = _pidOutputs.yaw;
    
    pitchRatePidState.setPoint = _setPoints.pitch;
    pitchRatePidState.processValue = _rate.pitch;
    pitchRatePidState.output = _pidOutputs.pitch;
    
    rollRatePidState.setPoint = _setPoints.roll;
    rollRatePidState.processValue = _rate.roll;
    rollRatePidState.output = _pidOutputs.roll;
    
    ratePidState.yawRate = yawRatePidState;
    ratePidState.pitchRate = pitchRatePidState;
    ratePidState.rollRate = rollRatePidState;
    
    return ratePidState;
}