Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

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

Rc/Rc.cpp@0:c6a85bb2a827, 2015-03-04 (annotated)

Committer:
joe4465
Date:
Wed Mar 04 18:50:37 2015 +0000
Revision:
0:c6a85bb2a827
Child:
2:969dfa4f2436
New version of quadcopter software, written following OO principles

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:c6a85bb2a827 1 #include "Rc.h"
joe4465 0:c6a85bb2a827 2
joe4465 0:c6a85bb2a827 3 // A class to get RC commands and convert to correct values
joe4465 0:c6a85bb2a827 4 //Channel 0 is roll. min 1000. max 1900
joe4465 0:c6a85bb2a827 5 //Channel 1 is pitch. min 1000. max 1900
joe4465 0:c6a85bb2a827 6 //Channel 2 is throttle < 900 when not connected. min 1000. max 1900
joe4465 0:c6a85bb2a827 7 //Channel 3 is yaw. min 1000. max 1900
joe4465 0:c6a85bb2a827 8 //Channel 4 is arm. armed > 1800 else unarmed
joe4465 0:c6a85bb2a827 9 //Channel 5 is mode. rate > 1800. stab < 1100
joe4465 0:c6a85bb2a827 10 //Channel 6 is spare
joe4465 0:c6a85bb2a827 11 //Channel 7 is spare
joe4465 0:c6a85bb2a827 12
joe4465 0:c6a85bb2a827 13 Rc::Rc(){}
joe4465 0:c6a85bb2a827 14
joe4465 0:c6a85bb2a827 15 Rc::~Rc(){}
joe4465 0:c6a85bb2a827 16
joe4465 0:c6a85bb2a827 17 bool Rc::initialise(Status& status, PinName pin)
joe4465 0:c6a85bb2a827 18 {
joe4465 0:c6a85bb2a827 19 _status = status;
joe4465 0:c6a85bb2a827 20
joe4465 0:c6a85bb2a827 21 _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
joe4465 0:c6a85bb2a827 22 _yawMedianFilter = new medianFilter(5);
joe4465 0:c6a85bb2a827 23 _pitchMedianFilter = new medianFilter(5);
joe4465 0:c6a85bb2a827 24 _rollMedianFilter = new medianFilter(5);
joe4465 0:c6a85bb2a827 25 _thrustMedianFilter = new medianFilter(5);
joe4465 0:c6a85bb2a827 26
joe4465 0:c6a85bb2a827 27 _thread = new Thread(&Rc::threadStarter, this, osPriorityHigh);
joe4465 0:c6a85bb2a827 28 DEBUG("Rc initialised");
joe4465 0:c6a85bb2a827 29 return true;
joe4465 0:c6a85bb2a827 30 }
joe4465 0:c6a85bb2a827 31
joe4465 0:c6a85bb2a827 32 void Rc::threadStarter(void const *p)
joe4465 0:c6a85bb2a827 33 {
joe4465 0:c6a85bb2a827 34 Rc *instance = (Rc*)p;
joe4465 0:c6a85bb2a827 35 instance->threadWorker();
joe4465 0:c6a85bb2a827 36 }
joe4465 0:c6a85bb2a827 37
joe4465 0:c6a85bb2a827 38 void Rc::threadWorker()
joe4465 0:c6a85bb2a827 39 {
joe4465 0:c6a85bb2a827 40 while(true)
joe4465 0:c6a85bb2a827 41 {
joe4465 0:c6a85bb2a827 42 float rc[8] = {0,0,0,0,0,0,0,0};
joe4465 0:c6a85bb2a827 43
joe4465 0:c6a85bb2a827 44 //Get channel data - mapped to between 0 and 1
joe4465 0:c6a85bb2a827 45 _ppm->GetChannelData(rc);
joe4465 0:c6a85bb2a827 46
joe4465 0:c6a85bb2a827 47 //Put channel data into raw rc struct
joe4465 0:c6a85bb2a827 48 _rawRc.channel0 = rc[0];
joe4465 0:c6a85bb2a827 49 _rawRc.channel1 = rc[1];
joe4465 0:c6a85bb2a827 50 _rawRc.channel2 = rc[2];
joe4465 0:c6a85bb2a827 51 _rawRc.channel3 = rc[3];
joe4465 0:c6a85bb2a827 52 _rawRc.channel4 = rc[4];
joe4465 0:c6a85bb2a827 53 _rawRc.channel5 = rc[5];
joe4465 0:c6a85bb2a827 54 _rawRc.channel6 = rc[6];
joe4465 0:c6a85bb2a827 55 _rawRc.channel7 = rc[7];
joe4465 0:c6a85bb2a827 56
joe4465 0:c6a85bb2a827 57 //Check whether transmitter is connected
joe4465 0:c6a85bb2a827 58 if(_rawRc.channel2 != -1)
joe4465 0:c6a85bb2a827 59 {
joe4465 0:c6a85bb2a827 60 _status.setRcConnected(true);
joe4465 0:c6a85bb2a827 61
joe4465 0:c6a85bb2a827 62 //Map yaw channel
joe4465 0:c6a85bb2a827 63 _mappedRc.yaw = - _yawMedianFilter->process(Map(_rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
joe4465 0:c6a85bb2a827 64
joe4465 0:c6a85bb2a827 65 //Map thust channel
joe4465 0:c6a85bb2a827 66 _mappedRc.throttle = _thrustMedianFilter->process(Map(_rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
joe4465 0:c6a85bb2a827 67
joe4465 0:c6a85bb2a827 68 //Map arm channel.
joe4465 0:c6a85bb2a827 69 if(_rawRc.channel4 > 0.5) _status.setArmed(true);
joe4465 0:c6a85bb2a827 70 else _status.setArmed(false);
joe4465 0:c6a85bb2a827 71
joe4465 0:c6a85bb2a827 72 //Map mode channel
joe4465 0:c6a85bb2a827 73 if(_rawRc.channel5 < 0.5) _status.setFlightMode(Status::STAB);
joe4465 0:c6a85bb2a827 74 else _status.setFlightMode(Status::RATE);
joe4465 0:c6a85bb2a827 75
joe4465 0:c6a85bb2a827 76 //Roll and pitch mapping depends on the mode
joe4465 0:c6a85bb2a827 77 if(_status.getFlightMode() == Status::STAB)//Stability mode
joe4465 0:c6a85bb2a827 78 {
joe4465 0:c6a85bb2a827 79 //Roll
joe4465 0:c6a85bb2a827 80 _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
joe4465 0:c6a85bb2a827 81 //Pitch
joe4465 0:c6a85bb2a827 82 _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
joe4465 0:c6a85bb2a827 83 }
joe4465 0:c6a85bb2a827 84 else if(_status.getFlightMode() == Status::RATE)//Rate mode
joe4465 0:c6a85bb2a827 85 {
joe4465 0:c6a85bb2a827 86 //Roll
joe4465 0:c6a85bb2a827 87 _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
joe4465 0:c6a85bb2a827 88 //Pitch
joe4465 0:c6a85bb2a827 89 _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
joe4465 0:c6a85bb2a827 90 }
joe4465 0:c6a85bb2a827 91 }
joe4465 0:c6a85bb2a827 92 else _status.setRcConnected(false);
joe4465 0:c6a85bb2a827 93
joe4465 0:c6a85bb2a827 94 Thread::wait(20);
joe4465 0:c6a85bb2a827 95 }
joe4465 0:c6a85bb2a827 96 }
joe4465 0:c6a85bb2a827 97
joe4465 0:c6a85bb2a827 98 float Rc::Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
joe4465 0:c6a85bb2a827 99 {
joe4465 0:c6a85bb2a827 100 return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
joe4465 0:c6a85bb2a827 101 }
joe4465 0:c6a85bb2a827 102
joe4465 0:c6a85bb2a827 103 Rc::MappedRc Rc::getMappedRc()
joe4465 0:c6a85bb2a827 104 {
joe4465 0:c6a85bb2a827 105 return _mappedRc;
joe4465 0:c6a85bb2a827 106 }
joe4465 0:c6a85bb2a827 107
joe4465 0:c6a85bb2a827 108 Rc::RawRc Rc::getRawRc()
joe4465 0:c6a85bb2a827 109 {
joe4465 0:c6a85bb2a827 110 return _rawRc;
joe4465 0:c6a85bb2a827 111 }