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@3:4823d6750629, 2015-04-24 (annotated)
- Committer:
- joe4465
- Date:
- Fri Apr 24 16:50:20 2015 +0000
- Revision:
- 3:4823d6750629
- Parent:
- 2:969dfa4f2436
Altitude mode working.
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:969dfa4f2436 | 13 | Rc::Rc(Status& status, PinName pin) : _status(status) |
joe4465 | 2:969dfa4f2436 | 14 | { |
joe4465 | 2:969dfa4f2436 | 15 | _notConnectedIterator = 0; |
joe4465 | 2:969dfa4f2436 | 16 | _connectedIterator = 0; |
joe4465 | 2:969dfa4f2436 | 17 | _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL); |
joe4465 | 2:969dfa4f2436 | 18 | _yawMedianFilter = new filter(5); |
joe4465 | 2:969dfa4f2436 | 19 | _pitchMedianFilter = new filter(5); |
joe4465 | 2:969dfa4f2436 | 20 | _rollMedianFilter = new filter(5); |
joe4465 | 2:969dfa4f2436 | 21 | _thrustMedianFilter = new filter(5); |
joe4465 | 2:969dfa4f2436 | 22 | _armMedianFilter = new filter(5); |
joe4465 | 2:969dfa4f2436 | 23 | _modeMedianFilter = new filter(5); |
joe4465 | 2:969dfa4f2436 | 24 | |
joe4465 | 2:969dfa4f2436 | 25 | DEBUG("Rc initialised\r\n"); |
joe4465 | 2:969dfa4f2436 | 26 | } |
joe4465 | 0:c6a85bb2a827 | 27 | |
joe4465 | 0:c6a85bb2a827 | 28 | Rc::~Rc(){} |
joe4465 | 0:c6a85bb2a827 | 29 | |
joe4465 | 2:969dfa4f2436 | 30 | double Rc::Map(double input, double inputMin, double inputMax, double outputMin, double outputMax) |
joe4465 | 0:c6a85bb2a827 | 31 | { |
joe4465 | 0:c6a85bb2a827 | 32 | return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin; |
joe4465 | 0:c6a85bb2a827 | 33 | } |
joe4465 | 0:c6a85bb2a827 | 34 | |
joe4465 | 0:c6a85bb2a827 | 35 | Rc::MappedRc Rc::getMappedRc() |
joe4465 | 0:c6a85bb2a827 | 36 | { |
joe4465 | 2:969dfa4f2436 | 37 | Rc::RawRc rawRc = getRawRc(); |
joe4465 | 2:969dfa4f2436 | 38 | Rc::MappedRc mappedRc = MappedRc(); |
joe4465 | 2:969dfa4f2436 | 39 | |
joe4465 | 2:969dfa4f2436 | 40 | //Check if transmitter is connected |
joe4465 | 2:969dfa4f2436 | 41 | if(rawRc.channel2 != -1) |
joe4465 | 2:969dfa4f2436 | 42 | { |
joe4465 | 2:969dfa4f2436 | 43 | if(_connectedIterator < 10) _connectedIterator++; |
joe4465 | 2:969dfa4f2436 | 44 | else |
joe4465 | 2:969dfa4f2436 | 45 | { |
joe4465 | 2:969dfa4f2436 | 46 | _notConnectedIterator = 0; |
joe4465 | 2:969dfa4f2436 | 47 | _status.setRcConnected(true); |
joe4465 | 2:969dfa4f2436 | 48 | |
joe4465 | 2:969dfa4f2436 | 49 | //Map yaw channel |
joe4465 | 2:969dfa4f2436 | 50 | mappedRc.yaw = _yawMedianFilter->process(-Map(rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX)); |
joe4465 | 2:969dfa4f2436 | 51 | |
joe4465 | 2:969dfa4f2436 | 52 | //Map thust channel |
joe4465 | 2:969dfa4f2436 | 53 | mappedRc.throttle = _thrustMedianFilter->process(Map(rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX)); |
joe4465 | 2:969dfa4f2436 | 54 | |
joe4465 | 2:969dfa4f2436 | 55 | //Manual mode |
joe4465 | 2:969dfa4f2436 | 56 | //Altitude mode |
joe4465 | 2:969dfa4f2436 | 57 | if(_status.getNavigationMode() == Status::ALTITUDE_HOLD) |
joe4465 | 2:969dfa4f2436 | 58 | { |
joe4465 | 3:4823d6750629 | 59 | float maxDeadZone = (float)RC_HOVER + (float)RC_DEAD_ZONE; |
joe4465 | 3:4823d6750629 | 60 | float minDeadZone = (float)RC_HOVER - (float)RC_DEAD_ZONE; |
joe4465 | 2:969dfa4f2436 | 61 | if((minDeadZone < mappedRc.throttle) && (mappedRc.throttle <= maxDeadZone)) _status.setDeadZone(true); |
joe4465 | 2:969dfa4f2436 | 62 | else _status.setDeadZone(false); |
joe4465 | 2:969dfa4f2436 | 63 | } |
joe4465 | 2:969dfa4f2436 | 64 | |
joe4465 | 2:969dfa4f2436 | 65 | //Map arm channel. |
joe4465 | 2:969dfa4f2436 | 66 | rawRc.channel4 = _armMedianFilter->process(rawRc.channel4); |
joe4465 | 2:969dfa4f2436 | 67 | if(rawRc.channel4 > 0.5) _status.setArmed(true); |
joe4465 | 2:969dfa4f2436 | 68 | else _status.setArmed(false); |
joe4465 | 2:969dfa4f2436 | 69 | |
joe4465 | 2:969dfa4f2436 | 70 | //Map mode channel |
joe4465 | 2:969dfa4f2436 | 71 | rawRc.channel5 = _modeMedianFilter->process(rawRc.channel5); |
joe4465 | 2:969dfa4f2436 | 72 | //if(rawRc.channel5 > 0.5) _status.setFlightMode(Status::RATE); |
joe4465 | 2:969dfa4f2436 | 73 | //else _status.setFlightMode(Status::STAB); |
joe4465 | 2:969dfa4f2436 | 74 | if(rawRc.channel5 > 0.5) _status.setNavigationMode(Status::ALTITUDE_HOLD); |
joe4465 | 2:969dfa4f2436 | 75 | else _status.setNavigationMode(Status::NONE); |
joe4465 | 2:969dfa4f2436 | 76 | |
joe4465 | 2:969dfa4f2436 | 77 | //Roll and pitch mapping depends on the mode |
joe4465 | 2:969dfa4f2436 | 78 | if(_status.getFlightMode() == Status::STAB)//Stability mode |
joe4465 | 2:969dfa4f2436 | 79 | { |
joe4465 | 2:969dfa4f2436 | 80 | //Roll |
joe4465 | 2:969dfa4f2436 | 81 | mappedRc.roll = _rollMedianFilter->process(Map(rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX)); |
joe4465 | 2:969dfa4f2436 | 82 | //Pitch |
joe4465 | 2:969dfa4f2436 | 83 | 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 | 2:969dfa4f2436 | 84 | } |
joe4465 | 2:969dfa4f2436 | 85 | else if(_status.getFlightMode() == Status::RATE)//Rate mode |
joe4465 | 2:969dfa4f2436 | 86 | { |
joe4465 | 2:969dfa4f2436 | 87 | //Roll |
joe4465 | 2:969dfa4f2436 | 88 | mappedRc.roll = _rollMedianFilter->process(Map(rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX)); |
joe4465 | 2:969dfa4f2436 | 89 | //Pitch |
joe4465 | 2:969dfa4f2436 | 90 | 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 | 2:969dfa4f2436 | 91 | } |
joe4465 | 2:969dfa4f2436 | 92 | } |
joe4465 | 2:969dfa4f2436 | 93 | } |
joe4465 | 2:969dfa4f2436 | 94 | else if(_notConnectedIterator < 10) _notConnectedIterator++; |
joe4465 | 2:969dfa4f2436 | 95 | else |
joe4465 | 2:969dfa4f2436 | 96 | { |
joe4465 | 2:969dfa4f2436 | 97 | _status.setRcConnected(false); |
joe4465 | 2:969dfa4f2436 | 98 | _notConnectedIterator = 0; |
joe4465 | 2:969dfa4f2436 | 99 | _connectedIterator = 0; |
joe4465 | 2:969dfa4f2436 | 100 | mappedRc.yaw = 0; |
joe4465 | 2:969dfa4f2436 | 101 | mappedRc.pitch = 0; |
joe4465 | 2:969dfa4f2436 | 102 | mappedRc.roll = 0; |
joe4465 | 2:969dfa4f2436 | 103 | mappedRc.throttle = 0; |
joe4465 | 2:969dfa4f2436 | 104 | } |
joe4465 | 2:969dfa4f2436 | 105 | |
joe4465 | 2:969dfa4f2436 | 106 | return mappedRc; |
joe4465 | 0:c6a85bb2a827 | 107 | } |
joe4465 | 0:c6a85bb2a827 | 108 | |
joe4465 | 0:c6a85bb2a827 | 109 | Rc::RawRc Rc::getRawRc() |
joe4465 | 0:c6a85bb2a827 | 110 | { |
joe4465 | 2:969dfa4f2436 | 111 | double rc[8] = {0,0,0,0,0,0,0,0}; |
joe4465 | 2:969dfa4f2436 | 112 | Rc::RawRc rawRc = RawRc(); |
joe4465 | 2:969dfa4f2436 | 113 | |
joe4465 | 2:969dfa4f2436 | 114 | //Get channel data - mapped to between 0 and 1 |
joe4465 | 2:969dfa4f2436 | 115 | _ppm->GetChannelData(rc); |
joe4465 | 2:969dfa4f2436 | 116 | |
joe4465 | 2:969dfa4f2436 | 117 | //Put channel data into raw rc struct |
joe4465 | 2:969dfa4f2436 | 118 | rawRc.channel0 = rc[0]; |
joe4465 | 2:969dfa4f2436 | 119 | rawRc.channel1 = rc[1]; |
joe4465 | 2:969dfa4f2436 | 120 | rawRc.channel2 = rc[2]; |
joe4465 | 2:969dfa4f2436 | 121 | rawRc.channel3 = rc[3]; |
joe4465 | 2:969dfa4f2436 | 122 | rawRc.channel4 = rc[4]; |
joe4465 | 2:969dfa4f2436 | 123 | rawRc.channel5 = rc[5]; |
joe4465 | 2:969dfa4f2436 | 124 | rawRc.channel6 = rc[6]; |
joe4465 | 2:969dfa4f2436 | 125 | rawRc.channel7 = rc[7]; |
joe4465 | 2:969dfa4f2436 | 126 | |
joe4465 | 2:969dfa4f2436 | 127 | return rawRc; |
joe4465 | 0:c6a85bb2a827 | 128 | } |