rcCommandMonitor.h

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "hardware.h"
00004 
00005 //Variables
00006 int i;
00007 
00008 // A thread to get RC commands and convert to correct values
00009 //Channel 1 is roll. min 1000. max 1900
00010 //Channel 2 is pitch. min 1000. max 1900
00011 //Channel 3 is throttle < 900 when not connected. min 1000. max 1900
00012 //Channel 4 is yaw. min 1000. max 1900
00013 //Channel 5 is arm. armed > 1800 else unarmed
00014 //Channel 6 is mode. rate > 1800. stab < 1100
00015 //Channel 7 is spare
00016 //Channel 8 is spare
00017 void RcCommandMonitorThread(void const *args) 
00018 {    
00019     printf("RC command monitor thread started\r\n");
00020     
00021     //Set lost connection iterator to 0
00022     i = 0;
00023     
00024     //Main loop
00025     while(true)
00026     {
00027         //Get channel data - mapped to between 0 and 1
00028         _ppm->GetChannelData(_rcCommands);
00029         
00030         //Check whether transmitter is connected
00031         if(_rcCommands[2] != -1)
00032         {
00033             //Transmitter is connected so reset not connected iterator
00034             i = 0;
00035             
00036             //Map yaw channel
00037             _rcMappedCommands[0] = - _yawMedianFilter->process(Map(_rcCommands[3], RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
00038         
00039             //Map thust channel
00040             _rcMappedCommands[3] = _thrustMedianFilter->process(Map(_rcCommands[2], RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
00041         
00042             //Map arm channel.
00043             if(_rcCommands[4] > 0.5 && _armed == false) Arm();
00044             else if(_rcCommands[4] <= 0.5 && _armed == true)
00045             {
00046                 Disarm();
00047             }
00048             
00049             //Map mode channel
00050             if(_rcCommands[5] < 0.5)
00051             {
00052                 _stab = true;
00053                 _rate = false;
00054             }
00055             else
00056             {
00057                 _stab = false;
00058                 _rate = true;
00059             }  
00060         
00061             //Roll and pitch mapping depends on the mode
00062             if(_rate == false && _stab == true)//Stability mode
00063             {
00064                 //Roll
00065                 _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
00066                 //Pitch
00067                 _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
00068             }
00069             else if(_rate == true && _stab == false)//Rate mode
00070             {
00071                 //Roll
00072                 _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
00073                 //Pitch
00074                 _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
00075             }
00076             else
00077             {
00078                 _rcMappedCommands[1] = 0;
00079                 _rcMappedCommands[2] = 0;
00080             }
00081         }
00082         else
00083         {
00084             //Transmitter not connected so increase iterator
00085             i++;
00086             
00087             //If connection has been down for 10 loops then assume the connection really is lost
00088             if(i > 10 && _armed == true) Disarm();
00089         }
00090         Thread::wait(20);
00091     }
00092 }