New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Diff: Rc/Rc.cpp
- Revision:
- 0:c6a85bb2a827
- Child:
- 2:969dfa4f2436
diff -r 000000000000 -r c6a85bb2a827 Rc/Rc.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Rc/Rc.cpp Wed Mar 04 18:50:37 2015 +0000 @@ -0,0 +1,111 @@ +#include "Rc.h" + +// A class to get RC commands and convert to correct values +//Channel 0 is roll. min 1000. max 1900 +//Channel 1 is pitch. min 1000. max 1900 +//Channel 2 is throttle < 900 when not connected. min 1000. max 1900 +//Channel 3 is yaw. min 1000. max 1900 +//Channel 4 is arm. armed > 1800 else unarmed +//Channel 5 is mode. rate > 1800. stab < 1100 +//Channel 6 is spare +//Channel 7 is spare + +Rc::Rc(){} + +Rc::~Rc(){} + +bool Rc::initialise(Status& status, PinName pin) +{ + _status = status; + + _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL); + _yawMedianFilter = new medianFilter(5); + _pitchMedianFilter = new medianFilter(5); + _rollMedianFilter = new medianFilter(5); + _thrustMedianFilter = new medianFilter(5); + + _thread = new Thread(&Rc::threadStarter, this, osPriorityHigh); + DEBUG("Rc initialised"); + return true; +} + +void Rc::threadStarter(void const *p) +{ + Rc *instance = (Rc*)p; + instance->threadWorker(); +} + +void Rc::threadWorker() +{ + while(true) + { + float rc[8] = {0,0,0,0,0,0,0,0}; + + //Get channel data - mapped to between 0 and 1 + _ppm->GetChannelData(rc); + + //Put channel data into raw rc struct + _rawRc.channel0 = rc[0]; + _rawRc.channel1 = rc[1]; + _rawRc.channel2 = rc[2]; + _rawRc.channel3 = rc[3]; + _rawRc.channel4 = rc[4]; + _rawRc.channel5 = rc[5]; + _rawRc.channel6 = rc[6]; + _rawRc.channel7 = rc[7]; + + //Check whether transmitter is connected + if(_rawRc.channel2 != -1) + { + _status.setRcConnected(true); + + //Map yaw channel + _mappedRc.yaw = - _yawMedianFilter->process(Map(_rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX)); + + //Map thust channel + _mappedRc.throttle = _thrustMedianFilter->process(Map(_rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX)); + + //Map arm channel. + if(_rawRc.channel4 > 0.5) _status.setArmed(true); + else _status.setArmed(false); + + //Map mode channel + if(_rawRc.channel5 < 0.5) _status.setFlightMode(Status::STAB); + else _status.setFlightMode(Status::RATE); + + //Roll and pitch mapping depends on the mode + if(_status.getFlightMode() == Status::STAB)//Stability mode + { + //Roll + _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX)); + //Pitch + _mappedRc.pitch = _pitchMedianFilter->process(-Map(_rawRc.channel1, RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX)); //Needs to be reverse + } + else if(_status.getFlightMode() == Status::RATE)//Rate mode + { + //Roll + _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX)); + //Pitch + _mappedRc.pitch = _pitchMedianFilter->process(-Map(_rawRc.channel1, RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX)); //Needs to be reverse + } + } + else _status.setRcConnected(false); + + Thread::wait(20); + } +} + +float Rc::Map(float input, float inputMin, float inputMax, float outputMin, float outputMax) +{ + return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin; +} + +Rc::MappedRc Rc::getMappedRc() +{ + return _mappedRc; +} + +Rc::RawRc Rc::getRawRc() +{ + return _rawRc; +} \ No newline at end of file