Rc.cpp

00001 #include "Rc.h"
00002 
00003 // A class to get RC commands and convert to correct values
00004 //Channel 0 is roll. min 1000. max 1900
00005 //Channel 1 is pitch. min 1000. max 1900
00006 //Channel 2 is throttle < 900 when not connected. min 1000. max 1900
00007 //Channel 3 is yaw. min 1000. max 1900
00008 //Channel 4 is arm. armed > 1800 else unarmed
00009 //Channel 5 is mode. rate > 1800. stab < 1100
00010 //Channel 6 is spare
00011 //Channel 7 is spare
00012 
00013 Rc::Rc(Status& status, PinName pin) : _status(status)
00014 {
00015     _notConnectedIterator = 0;
00016     _connectedIterator = 0;
00017     _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
00018     _yawMedianFilter = new filter(5);
00019     _pitchMedianFilter = new filter(5);
00020     _rollMedianFilter = new filter(5);
00021     _thrustMedianFilter = new filter(5);
00022     _armMedianFilter = new filter(5);
00023     _modeMedianFilter = new filter(5);
00024 
00025     DEBUG("Rc initialised\r\n"); 
00026 }
00027 
00028 Rc::~Rc(){}
00029 
00030 double Rc::Map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
00031 {
00032     return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
00033 }
00034 
00035 Rc::MappedRc Rc::getMappedRc()
00036 {
00037     Rc::RawRc rawRc = getRawRc();
00038     Rc::MappedRc mappedRc = MappedRc();
00039     
00040     //Check if transmitter is connected
00041     if(rawRc.channel2 != -1)
00042     {
00043         if(_connectedIterator < 10) _connectedIterator++;
00044         else
00045         {
00046             _notConnectedIterator = 0;
00047             _status.setRcConnected(true);
00048             
00049             //Map yaw channel
00050             mappedRc.yaw = _yawMedianFilter->process(-Map(rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
00051         
00052             //Map thust channel
00053             mappedRc.throttle = _thrustMedianFilter->process(Map(rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
00054             
00055             //Manual mode
00056             //Altitude mode
00057             if(_status.getNavigationMode() == Status::ALTITUDE_HOLD)
00058             {
00059                 float maxDeadZone = (float)RC_HOVER + (float)RC_DEAD_ZONE;
00060                 float minDeadZone = (float)RC_HOVER  - (float)RC_DEAD_ZONE;
00061                 if((minDeadZone < mappedRc.throttle) && (mappedRc.throttle <= maxDeadZone)) _status.setDeadZone(true);
00062                 else _status.setDeadZone(false);
00063             }
00064         
00065             //Map arm channel.
00066             rawRc.channel4 = _armMedianFilter->process(rawRc.channel4);
00067             if(rawRc.channel4 > 0.5) _status.setArmed(true);
00068             else _status.setArmed(false);
00069             
00070             //Map mode channel
00071             rawRc.channel5 = _modeMedianFilter->process(rawRc.channel5);
00072             //if(rawRc.channel5 > 0.5) _status.setFlightMode(Status::RATE);
00073             //else _status.setFlightMode(Status::STAB);
00074             if(rawRc.channel5 > 0.5) _status.setNavigationMode(Status::ALTITUDE_HOLD);
00075             else _status.setNavigationMode(Status::NONE);
00076         
00077             //Roll and pitch mapping depends on the mode
00078             if(_status.getFlightMode() == Status::STAB)//Stability mode
00079             {
00080                 //Roll
00081                 mappedRc.roll = _rollMedianFilter->process(Map(rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
00082                 //Pitch
00083                 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
00084             }
00085             else if(_status.getFlightMode() == Status::RATE)//Rate mode
00086             {
00087                 //Roll
00088                 mappedRc.roll = _rollMedianFilter->process(Map(rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
00089                 //Pitch
00090                 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
00091             }
00092         }
00093     }
00094     else if(_notConnectedIterator < 10) _notConnectedIterator++;
00095     else
00096     {
00097         _status.setRcConnected(false);
00098         _notConnectedIterator = 0;
00099         _connectedIterator = 0;
00100         mappedRc.yaw = 0;
00101         mappedRc.pitch = 0;
00102         mappedRc.roll = 0;
00103         mappedRc.throttle = 0;
00104     }
00105     
00106     return mappedRc;
00107 }
00108 
00109 Rc::RawRc Rc::getRawRc()
00110 {
00111     double rc[8] = {0,0,0,0,0,0,0,0};
00112     Rc::RawRc rawRc = RawRc();
00113         
00114     //Get channel data - mapped to between 0 and 1
00115     _ppm->GetChannelData(rc);
00116     
00117     //Put channel data into raw rc struct
00118     rawRc.channel0 = rc[0];
00119     rawRc.channel1 = rc[1];
00120     rawRc.channel2 = rc[2];
00121     rawRc.channel3 = rc[3];
00122     rawRc.channel4 = rc[4];
00123     rawRc.channel5 = rc[5];
00124     rawRc.channel6 = rc[6];
00125     rawRc.channel7 = rc[7];
00126     
00127     return rawRc;  
00128 }