Joseph Roberts / Mbed 2 deprecated QuadCopter

Quadcopter software. Flying - Contact me for more details or a copy of the PC ground station code

Dependencies:   ConfigFile PID PPM MODSERIAL mbed-rtos mbed MaxbotixDriver TinyGPS filter

rcCommandMonitor.h@9:7b194f83e567, 2015-02-22 (annotated)

Committer:
joe4465
Date:
Sun Feb 22 20:10:12 2015 +0000
Revision:
9:7b194f83e567
Parent:
7:bc5822aa8878 
Added external magnetometer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 3:82665e39f1ea 1 #include "mbed.h"
joe4465 3:82665e39f1ea 2 #include "rtos.h"
joe4465 3:82665e39f1ea 3 #include "hardware.h"
joe4465 3:82665e39f1ea 4
joe4465 6:4c207e7b1203 5 //Variables
joe4465 6:4c207e7b1203 6 int i;
joe4465 3:82665e39f1ea 7
joe4465 6:4c207e7b1203 8 // A thread to get RC commands and convert to correct values
joe4465 3:82665e39f1ea 9 //Channel 1 is roll. min 1000. max 1900
joe4465 3:82665e39f1ea 10 //Channel 2 is pitch. min 1000. max 1900
joe4465 3:82665e39f1ea 11 //Channel 3 is throttle < 900 when not connected. min 1000. max 1900
joe4465 3:82665e39f1ea 12 //Channel 4 is yaw. min 1000. max 1900
joe4465 3:82665e39f1ea 13 //Channel 5 is arm. armed > 1800 else unarmed
joe4465 3:82665e39f1ea 14 //Channel 6 is mode. rate > 1800. stab < 1100
joe4465 3:82665e39f1ea 15 //Channel 7 is spare
joe4465 3:82665e39f1ea 16 //Channel 8 is spare
joe4465 6:4c207e7b1203 17 void RcCommandMonitorThread(void const *args)
joe4465 6:4c207e7b1203 18 {
joe4465 6:4c207e7b1203 19 printf("RC command monitor thread started\r\n");
joe4465 3:82665e39f1ea 20
joe4465 6:4c207e7b1203 21 //Set lost connection iterator to 0
joe4465 6:4c207e7b1203 22 i = 0;
joe4465 3:82665e39f1ea 23
joe4465 6:4c207e7b1203 24 //Main loop
joe4465 6:4c207e7b1203 25 while(true)
joe4465 3:82665e39f1ea 26 {
joe4465 6:4c207e7b1203 27 //Get channel data - mapped to between 0 and 1
joe4465 6:4c207e7b1203 28 _ppm->GetChannelData(_rcCommands);
joe4465 6:4c207e7b1203 29
joe4465 6:4c207e7b1203 30 //Check whether transmitter is connected
joe4465 6:4c207e7b1203 31 if(_rcCommands[2] != -1)
joe4465 6:4c207e7b1203 32 {
joe4465 6:4c207e7b1203 33 //Transmitter is connected so reset not connected iterator
joe4465 6:4c207e7b1203 34 i = 0;
joe4465 6:4c207e7b1203 35
joe4465 6:4c207e7b1203 36 //Map yaw channel
joe4465 9:7b194f83e567 37 _rcMappedCommands[0] = - _yawMedianFilter->process(Map(_rcCommands[3], RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
joe4465 6:4c207e7b1203 38
joe4465 6:4c207e7b1203 39 //Map thust channel
joe4465 9:7b194f83e567 40 _rcMappedCommands[3] = _thrustMedianFilter->process(Map(_rcCommands[2], RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
joe4465 6:4c207e7b1203 41
joe4465 6:4c207e7b1203 42 //Map arm channel.
joe4465 7:bc5822aa8878 43 if(_rcCommands[4] > 0.5 && _armed == false) Arm();
joe4465 6:4c207e7b1203 44 else if(_rcCommands[4] <= 0.5 && _armed == true)
joe4465 6:4c207e7b1203 45 {
joe4465 6:4c207e7b1203 46 Disarm();
joe4465 6:4c207e7b1203 47 }
joe4465 6:4c207e7b1203 48
joe4465 6:4c207e7b1203 49 //Map mode channel
joe4465 6:4c207e7b1203 50 if(_rcCommands[5] < 0.5)
joe4465 6:4c207e7b1203 51 {
joe4465 6:4c207e7b1203 52 _stab = true;
joe4465 6:4c207e7b1203 53 _rate = false;
joe4465 6:4c207e7b1203 54 }
joe4465 6:4c207e7b1203 55 else
joe4465 6:4c207e7b1203 56 {
joe4465 6:4c207e7b1203 57 _stab = false;
joe4465 6:4c207e7b1203 58 _rate = true;
joe4465 6:4c207e7b1203 59 }
joe4465 6:4c207e7b1203 60
joe4465 6:4c207e7b1203 61 //Roll and pitch mapping depends on the mode
joe4465 6:4c207e7b1203 62 if(_rate == false && _stab == true)//Stability mode
joe4465 6:4c207e7b1203 63 {
joe4465 6:4c207e7b1203 64 //Roll
joe4465 9:7b194f83e567 65 _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
joe4465 6:4c207e7b1203 66 //Pitch
joe4465 9:7b194f83e567 67 _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
joe4465 6:4c207e7b1203 68 }
joe4465 6:4c207e7b1203 69 else if(_rate == true && _stab == false)//Rate mode
joe4465 6:4c207e7b1203 70 {
joe4465 6:4c207e7b1203 71 //Roll
joe4465 9:7b194f83e567 72 _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
joe4465 6:4c207e7b1203 73 //Pitch
joe4465 9:7b194f83e567 74 _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
joe4465 6:4c207e7b1203 75 }
joe4465 6:4c207e7b1203 76 else
joe4465 6:4c207e7b1203 77 {
joe4465 6:4c207e7b1203 78 _rcMappedCommands[1] = 0;
joe4465 6:4c207e7b1203 79 _rcMappedCommands[2] = 0;
joe4465 6:4c207e7b1203 80 }
joe4465 6:4c207e7b1203 81 }
joe4465 6:4c207e7b1203 82 else
joe4465 6:4c207e7b1203 83 {
joe4465 6:4c207e7b1203 84 //Transmitter not connected so increase iterator
joe4465 6:4c207e7b1203 85 i++;
joe4465 6:4c207e7b1203 86
joe4465 6:4c207e7b1203 87 //If connection has been down for 10 loops then assume the connection really is lost
joe4465 7:bc5822aa8878 88 if(i > 10 && _armed == true) Disarm();
joe4465 6:4c207e7b1203 89 }
joe4465 6:4c207e7b1203 90 Thread::wait(20);
joe4465 3:82665e39f1ea 91 }
joe4465 3:82665e39f1ea 92 }