Diff: rcCommandMonitor.h

Revision:

6:4c207e7b1203

Parent:

3:82665e39f1ea

Child:

7:bc5822aa8878

--- a/rcCommandMonitor.h	Mon Sep 22 10:16:31 2014 +0000
+++ b/rcCommandMonitor.h	Thu Jan 22 18:03:22 2015 +0000
@@ -2,25 +2,10 @@
 #include "rtos.h"
 #include "hardware.h"
 
-//Declarations
-void RcCommandMonitorTask(void const *n);
-
-//Timers
-RtosTimer       *_rcCommandMonitorUpdateTimer;
+//Variables
+int i;
 
-// A thread to control flight
-void RcCommandMonitorThread(void const *args) 
-{         
-    //Update Timer
-    _rcCommandMonitorUpdateTimer = new RtosTimer(RcCommandMonitorTask, osTimerPeriodic, (void *)0);
-    int updateTime = (1.0 / RC_COMMANDS_FREQUENCY) * 1000;
-    _rcCommandMonitorUpdateTimer->start(updateTime);
-    
-    // Wait here forever
-    Thread::wait(osWaitForever);
-}
-
-//Get RC commands
+// A thread to get RC commands and convert to correct values
 //Channel 1 is roll. min 1000. max 1900
 //Channel 2 is pitch. min 1000. max 1900
 //Channel 3 is throttle < 900 when not connected. min 1000. max 1900
@@ -29,54 +14,85 @@
 //Channel 6 is mode. rate > 1800. stab < 1100
 //Channel 7 is spare
 //Channel 8 is spare
-void RcCommandMonitorTask(void const *n)
-{
-    //Initialise array to hold channel data
-    float rcCommands[8] = {0,0,0,0,0,0,0,0};
-    
-    //Get channel data - mapped to between 0 and 1
-    _ppm->GetChannelData(rcCommands);
+void RcCommandMonitorThread(void const *args) 
+{    
+    printf("RC command monitor thread started\r\n");
     
-    //Map yaw channel
-    _rcMappedCommands[0] = Map(rcCommands[3], 0, 1, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
-    
-    //Map thust channel
-    _rcMappedCommands[3] = Map(rcCommands[2], 0, 1, RC_THRUST_MIN, RC_THRUST_MAX);
+    //Set lost connection iterator to 0
+    i = 0;
     
-    //Map arm channel. 0.85 = 1765
-    if(rcCommands[4] > 0.85) Arm();
-    else Disarm();
-    
-    //Map mode channel
-    if(rcCommands[5] < 0.5)
+    //Main loop
+    while(true)
     {
-        _stab = true;
-        _rate = false;
-    }
-    else
-    {
-        _stab = false;
-        _rate = true;
-    }  
-    
-    //Roll and pitch mapping depends on the mode
-    if(_rate == false && _stab == true)//Stability mode
-    {
-        //Roll
-        _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
-        //Pitch
-        _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
-    }
-    else if(_rate == true && _stab == false)//Rate mode
-    {
-        //Roll
-        _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
-        //Pitch
-        _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
-    }
-    else
-    {
-        _rcMappedCommands[1] = 0;
-        _rcMappedCommands[2] = 0;
+        //Get channel data - mapped to between 0 and 1
+        _ppm->GetChannelData(_rcCommands);
+        
+        //Check whether transmitter is connected
+        if(_rcCommands[2] != -1)
+        {
+            //Transmitter is connected so reset not connected iterator
+            i = 0;
+            
+            //Map yaw channel
+            _rcMappedCommands[0] = - _yawMedianFilter->process(Map(_rcCommands[3], RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
+        
+            //Map thust channel
+            _rcMappedCommands[3] = _thrustMedianFilter->process(Map(_rcCommands[2], RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
+        
+            //Map arm channel.
+            if(_rcCommands[4] > 0.5 && _initialised == true && _armed == false) Arm();
+            else if(_rcCommands[4] <= 0.5 && _armed == true)
+            {
+                Disarm();
+            }
+            
+            //Map mode channel
+            if(_rcCommands[5] < 0.5)
+            {
+                _stab = true;
+                _rate = false;
+            }
+            else
+            {
+                _stab = false;
+                _rate = true;
+            }  
+        
+            //Roll and pitch mapping depends on the mode
+            if(_rate == false && _stab == true)//Stability mode
+            {
+                //Roll
+                _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
+                //Pitch
+                _rcMappedCommands[1] = _pitchMedianFilter->process(-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX)); //Needs to be reverse
+            }
+            else if(_rate == true && _stab == false)//Rate mode
+            {
+                //Roll
+                _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
+                //Pitch
+                _rcMappedCommands[1] = _pitchMedianFilter->process(-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX)); //Needs to be reverse
+            }
+            else
+            {
+                _rcMappedCommands[1] = 0;
+                _rcMappedCommands[2] = 0;
+            }
+        }
+        else
+        {
+            //Transmitter not connected so increase iterator
+            i++;
+            
+            //Set commands to 0
+            _rcMappedCommands[0] = 0;
+            _rcMappedCommands[1] = 0;
+            _rcMappedCommands[2] = 0;
+            _rcMappedCommands[3] = -1;
+            
+            //If connection has been down for 10 loops then assume the connection really is lost
+            if(i > 10) Disarm();
+        }
+        Thread::wait(20);
     }
 }
\ No newline at end of file