Diff: Rc/Rc.cpp

Revision:

0:c6a85bb2a827

Child:

2:969dfa4f2436

--- /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