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-04-24
- Revision:
- 3:4823d6750629
- Parent:
- 2:969dfa4f2436
File content as of revision 3:4823d6750629:
#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(Status& status, PinName pin) : _status(status) { _notConnectedIterator = 0; _connectedIterator = 0; _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL); _yawMedianFilter = new filter(5); _pitchMedianFilter = new filter(5); _rollMedianFilter = new filter(5); _thrustMedianFilter = new filter(5); _armMedianFilter = new filter(5); _modeMedianFilter = new filter(5); DEBUG("Rc initialised\r\n"); } Rc::~Rc(){} double Rc::Map(double input, double inputMin, double inputMax, double outputMin, double outputMax) { return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin; } Rc::MappedRc Rc::getMappedRc() { Rc::RawRc rawRc = getRawRc(); Rc::MappedRc mappedRc = MappedRc(); //Check if transmitter is connected if(rawRc.channel2 != -1) { if(_connectedIterator < 10) _connectedIterator++; else { _notConnectedIterator = 0; _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)); //Manual mode //Altitude mode if(_status.getNavigationMode() == Status::ALTITUDE_HOLD) { float maxDeadZone = (float)RC_HOVER + (float)RC_DEAD_ZONE; float minDeadZone = (float)RC_HOVER - (float)RC_DEAD_ZONE; if((minDeadZone < mappedRc.throttle) && (mappedRc.throttle <= maxDeadZone)) _status.setDeadZone(true); else _status.setDeadZone(false); } //Map arm channel. rawRc.channel4 = _armMedianFilter->process(rawRc.channel4); if(rawRc.channel4 > 0.5) _status.setArmed(true); else _status.setArmed(false); //Map mode channel rawRc.channel5 = _modeMedianFilter->process(rawRc.channel5); //if(rawRc.channel5 > 0.5) _status.setFlightMode(Status::RATE); //else _status.setFlightMode(Status::STAB); if(rawRc.channel5 > 0.5) _status.setNavigationMode(Status::ALTITUDE_HOLD); else _status.setNavigationMode(Status::NONE); //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 if(_notConnectedIterator < 10) _notConnectedIterator++; else { _status.setRcConnected(false); _notConnectedIterator = 0; _connectedIterator = 0; mappedRc.yaw = 0; mappedRc.pitch = 0; mappedRc.roll = 0; mappedRc.throttle = 0; } return mappedRc; } Rc::RawRc Rc::getRawRc() { double rc[8] = {0,0,0,0,0,0,0,0}; Rc::RawRc rawRc = RawRc(); //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]; return rawRc; }