Diff: Rc/Rc.cpp

Revision:

2:969dfa4f2436

Parent:

0:c6a85bb2a827

Child:

3:4823d6750629

--- a/Rc/Rc.cpp	Wed Mar 04 18:53:43 2015 +0000
+++ b/Rc/Rc.cpp	Wed Apr 01 11:19:21 2015 +0000
@@ -10,102 +10,119 @@
 //Channel 6 is spare
 //Channel 7 is spare
 
-Rc::Rc(){}
+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(){}
 
-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)
+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()
 {
-    return _mappedRc;
+    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_THRUST_MAX / 2.0) + (float)RC_DEAD_ZONE;
+                float minDeadZone = ((float)RC_THRUST_MAX / 2.0)  - (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()
 {
-    return _rawRc;  
+    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;  
 }
\ No newline at end of file