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@3:82665e39f1ea, 2014-09-18 (annotated)

Committer:
joe4465
Date:
Thu Sep 18 08:45:46 2014 +0000
Revision:
3:82665e39f1ea
Child:
6:4c207e7b1203
First revision of quadcopter software

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 3:82665e39f1ea 5 //Declarations
joe4465 3:82665e39f1ea 6 void RcCommandMonitorTask(void const *n);
joe4465 3:82665e39f1ea 7
joe4465 3:82665e39f1ea 8 //Timers
joe4465 3:82665e39f1ea 9 RtosTimer *_rcCommandMonitorUpdateTimer;
joe4465 3:82665e39f1ea 10
joe4465 3:82665e39f1ea 11 // A thread to control flight
joe4465 3:82665e39f1ea 12 void RcCommandMonitorThread(void const *args)
joe4465 3:82665e39f1ea 13 {
joe4465 3:82665e39f1ea 14 //Update Timer
joe4465 3:82665e39f1ea 15 _rcCommandMonitorUpdateTimer = new RtosTimer(RcCommandMonitorTask, osTimerPeriodic, (void *)0);
joe4465 3:82665e39f1ea 16 int updateTime = (1.0 / RC_COMMANDS_FREQUENCY) * 1000;
joe4465 3:82665e39f1ea 17 _rcCommandMonitorUpdateTimer->start(updateTime);
joe4465 3:82665e39f1ea 18
joe4465 3:82665e39f1ea 19 // Wait here forever
joe4465 3:82665e39f1ea 20 Thread::wait(osWaitForever);
joe4465 3:82665e39f1ea 21 }
joe4465 3:82665e39f1ea 22
joe4465 3:82665e39f1ea 23 //Get RC commands
joe4465 3:82665e39f1ea 24 //Channel 1 is roll. min 1000. max 1900
joe4465 3:82665e39f1ea 25 //Channel 2 is pitch. min 1000. max 1900
joe4465 3:82665e39f1ea 26 //Channel 3 is throttle < 900 when not connected. min 1000. max 1900
joe4465 3:82665e39f1ea 27 //Channel 4 is yaw. min 1000. max 1900
joe4465 3:82665e39f1ea 28 //Channel 5 is arm. armed > 1800 else unarmed
joe4465 3:82665e39f1ea 29 //Channel 6 is mode. rate > 1800. stab < 1100
joe4465 3:82665e39f1ea 30 //Channel 7 is spare
joe4465 3:82665e39f1ea 31 //Channel 8 is spare
joe4465 3:82665e39f1ea 32 void RcCommandMonitorTask(void const *n)
joe4465 3:82665e39f1ea 33 {
joe4465 3:82665e39f1ea 34 //Initialise array to hold channel data
joe4465 3:82665e39f1ea 35 float rcCommands[8] = {0,0,0,0,0,0,0,0};
joe4465 3:82665e39f1ea 36
joe4465 3:82665e39f1ea 37 //Get channel data - mapped to between 0 and 1
joe4465 3:82665e39f1ea 38 _ppm->GetChannelData(rcCommands);
joe4465 3:82665e39f1ea 39
joe4465 3:82665e39f1ea 40 //Map yaw channel
joe4465 3:82665e39f1ea 41 _rcMappedCommands[0] = Map(rcCommands[3], 0, 1, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
joe4465 3:82665e39f1ea 42
joe4465 3:82665e39f1ea 43 //Map thust channel
joe4465 3:82665e39f1ea 44 _rcMappedCommands[3] = Map(rcCommands[2], 0, 1, RC_THRUST_MIN, RC_THRUST_MAX);
joe4465 3:82665e39f1ea 45
joe4465 3:82665e39f1ea 46 //Map arm channel. 0.85 = 1765
joe4465 3:82665e39f1ea 47 if(rcCommands[4] > 0.85) Arm();
joe4465 3:82665e39f1ea 48 else Disarm();
joe4465 3:82665e39f1ea 49
joe4465 3:82665e39f1ea 50 //Map mode channel
joe4465 3:82665e39f1ea 51 if(rcCommands[5] < 0.5)
joe4465 3:82665e39f1ea 52 {
joe4465 3:82665e39f1ea 53 _stab = true;
joe4465 3:82665e39f1ea 54 _rate = false;
joe4465 3:82665e39f1ea 55 }
joe4465 3:82665e39f1ea 56 else
joe4465 3:82665e39f1ea 57 {
joe4465 3:82665e39f1ea 58 _stab = false;
joe4465 3:82665e39f1ea 59 _rate = true;
joe4465 3:82665e39f1ea 60 }
joe4465 3:82665e39f1ea 61
joe4465 3:82665e39f1ea 62 //Roll and pitch mapping depends on the mode
joe4465 3:82665e39f1ea 63 if(_rate == false && _stab == true)//Stability mode
joe4465 3:82665e39f1ea 64 {
joe4465 3:82665e39f1ea 65 //Roll
joe4465 3:82665e39f1ea 66 _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
joe4465 3:82665e39f1ea 67 //Pitch
joe4465 3:82665e39f1ea 68 _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
joe4465 3:82665e39f1ea 69 }
joe4465 3:82665e39f1ea 70 else if(_rate == true && _stab == false)//Rate mode
joe4465 3:82665e39f1ea 71 {
joe4465 3:82665e39f1ea 72 //Roll
joe4465 3:82665e39f1ea 73 _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
joe4465 3:82665e39f1ea 74 //Pitch
joe4465 3:82665e39f1ea 75 _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
joe4465 3:82665e39f1ea 76 }
joe4465 3:82665e39f1ea 77 else
joe4465 3:82665e39f1ea 78 {
joe4465 3:82665e39f1ea 79 _rcMappedCommands[1] = 0;
joe4465 3:82665e39f1ea 80 _rcMappedCommands[2] = 0;
joe4465 3:82665e39f1ea 81 }
joe4465 3:82665e39f1ea 82 }