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@2:969dfa4f2436, 2015-04-01 (annotated)

Committer:
joe4465
Date:
Wed Apr 01 11:19:21 2015 +0000
Revision:
2:969dfa4f2436
Parent:
0:c6a85bb2a827
Child:
3:4823d6750629
Altitude hold with 2 PID's working. Exported to offline compiler

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 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 2:969dfa4f2436 59 float maxDeadZone = ((float)RC_THRUST_MAX / 2.0) + (float)RC_DEAD_ZONE;
joe4465 2:969dfa4f2436 60 float minDeadZone = ((float)RC_THRUST_MAX / 2.0) - (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 }