New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Rc/Rc.cpp
- Committer:
- joe4465
- Date:
- 2015-03-04
- Revision:
- 0:c6a85bb2a827
- Child:
- 2:969dfa4f2436
File content as of revision 0:c6a85bb2a827:
#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; }