![](/media/cache/profiles/8158fa3e4de806e614f7ff02e7b22fde.jpg.50x50_q85.png)
New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Diff: NavigationController/NavigationController.cpp
- 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