New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
BaseStation/BaseStation.cpp@4:9ffbf9101992, 2015-05-08 (annotated)
- Committer:
- joe4465
- Date:
- Fri May 08 09:07:38 2015 +0000
- Revision:
- 4:9ffbf9101992
- Parent:
- 3:4823d6750629
End of FYP
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
joe4465 | 2:969dfa4f2436 | 1 | #include "BaseStation.h" |
joe4465 | 2:969dfa4f2436 | 2 | |
joe4465 | 2:969dfa4f2436 | 3 | BaseStation::BaseStation(Status& status, Rc& rc, Sensors& sensors, NavigationController& navigationController, FlightController& flightController, ConfigFileWrapper& configFileWrapper, PinName wirelessPinTx, PinName wirelessPinRx) : _status(status), _rc(rc), _sensors(sensors), _navigationController(navigationController), _flightController(flightController), _configFileWrapper(configFileWrapper) |
joe4465 | 2:969dfa4f2436 | 4 | { |
joe4465 | 2:969dfa4f2436 | 5 | _wireless = new MODSERIAL(wirelessPinTx, wirelessPinRx); |
joe4465 | 2:969dfa4f2436 | 6 | _wireless->baud(57600); |
joe4465 | 2:969dfa4f2436 | 7 | _wirelessSerialRxPos = 0; |
joe4465 | 2:969dfa4f2436 | 8 | |
joe4465 | 2:969dfa4f2436 | 9 | _thread = new Thread(&BaseStation::threadStarter, this, osPriorityNormal); |
joe4465 | 2:969dfa4f2436 | 10 | DEBUG("Base Station initialised\r\n"); |
joe4465 | 2:969dfa4f2436 | 11 | } |
joe4465 | 2:969dfa4f2436 | 12 | |
joe4465 | 2:969dfa4f2436 | 13 | BaseStation::~BaseStation(){} |
joe4465 | 4:9ffbf9101992 | 14 | l |
joe4465 | 2:969dfa4f2436 | 15 | void BaseStation::threadStarter(void const *p) |
joe4465 | 2:969dfa4f2436 | 16 | { |
joe4465 | 2:969dfa4f2436 | 17 | BaseStation *instance = (BaseStation*)p; |
joe4465 | 2:969dfa4f2436 | 18 | instance->threadWorker(); |
joe4465 | 2:969dfa4f2436 | 19 | } |
joe4465 | 2:969dfa4f2436 | 20 | |
joe4465 | 2:969dfa4f2436 | 21 | void BaseStation::threadWorker() |
joe4465 | 2:969dfa4f2436 | 22 | { |
joe4465 | 2:969dfa4f2436 | 23 | while(true) |
joe4465 | 2:969dfa4f2436 | 24 | { |
joe4465 | 2:969dfa4f2436 | 25 | //Check comms mode and print correct data back to PC application |
joe4465 | 2:969dfa4f2436 | 26 | Status::BaseStationMode mode = _status.getBaseStationMode(); |
joe4465 | 2:969dfa4f2436 | 27 | |
joe4465 | 2:969dfa4f2436 | 28 | if(mode == Status::MOTOR_POWER) |
joe4465 | 2:969dfa4f2436 | 29 | { |
joe4465 | 2:969dfa4f2436 | 30 | MotorMixer::MotorPower motorPower = _flightController.getMotorPower(); |
joe4465 | 2:969dfa4f2436 | 31 | |
joe4465 | 2:969dfa4f2436 | 32 | _wireless->printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 33 | motorPower.motor1, motorPower.motor2, motorPower.motor3, motorPower.motor4); |
joe4465 | 2:969dfa4f2436 | 34 | } |
joe4465 | 2:969dfa4f2436 | 35 | else if (mode == Status::PID_OUTPUTS) |
joe4465 | 2:969dfa4f2436 | 36 | { |
joe4465 | 2:969dfa4f2436 | 37 | PidWrapper::PidOutput pidOutputs = _flightController.getPidOutputs(); |
joe4465 | 2:969dfa4f2436 | 38 | |
joe4465 | 2:969dfa4f2436 | 39 | _wireless->printf("<YPID=%1.2f:PPID=%1.2f:RPID=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 40 | pidOutputs.yaw, pidOutputs.pitch, pidOutputs.roll); |
joe4465 | 2:969dfa4f2436 | 41 | } |
joe4465 | 2:969dfa4f2436 | 42 | else if (mode == Status::IMU_OUTPUTS) |
joe4465 | 2:969dfa4f2436 | 43 | { |
joe4465 | 2:969dfa4f2436 | 44 | Imu::Rate rate = _sensors.getRate(); |
joe4465 | 2:969dfa4f2436 | 45 | Imu::Angle angle = _sensors.getAngle(); |
joe4465 | 2:969dfa4f2436 | 46 | |
joe4465 | 2:969dfa4f2436 | 47 | _wireless->printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 48 | angle.yaw, angle.pitch, angle.roll, rate.yaw, rate.pitch, rate.roll); |
joe4465 | 2:969dfa4f2436 | 49 | } |
joe4465 | 2:969dfa4f2436 | 50 | else if (mode == Status::STATUS) |
joe4465 | 2:969dfa4f2436 | 51 | { |
joe4465 | 2:969dfa4f2436 | 52 | _wireless->printf("<Batt=%f:Armed=%d:Init=%d:FlightMode=%d:State=%d:NavMode=%d>", |
joe4465 | 2:969dfa4f2436 | 53 | _status.getBatteryLevel(), _status.getArmed(), _status.getInitialised(), _status.getFlightMode(), _status.getState(), _status.getNavigationMode()); |
joe4465 | 2:969dfa4f2436 | 54 | } |
joe4465 | 2:969dfa4f2436 | 55 | else if (mode == Status::RC) |
joe4465 | 2:969dfa4f2436 | 56 | { |
joe4465 | 2:969dfa4f2436 | 57 | Rc::MappedRc mappedRc = _rc.getMappedRc(); |
joe4465 | 2:969dfa4f2436 | 58 | Rc::RawRc rawRc = _rc.getRawRc(); |
joe4465 | 2:969dfa4f2436 | 59 | |
joe4465 | 2:969dfa4f2436 | 60 | _wireless->printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f:RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 61 | mappedRc.yaw, mappedRc.pitch, mappedRc.roll, mappedRc.throttle, rawRc.channel0, rawRc.channel1, rawRc.channel2, rawRc.channel3, rawRc.channel4, rawRc.channel5, rawRc.channel6, rawRc.channel7); |
joe4465 | 2:969dfa4f2436 | 62 | } |
joe4465 | 2:969dfa4f2436 | 63 | else if (mode == Status::PID_TUNING) |
joe4465 | 2:969dfa4f2436 | 64 | { |
joe4465 | 2:969dfa4f2436 | 65 | PidWrapper::FlightControllerPidParameters flightControllerPidParameters = _flightController.getPidParameters(); |
joe4465 | 2:969dfa4f2436 | 66 | PidWrapper::NavigationControllerPidParameters navigationControllerPidParameters = _navigationController.getPidParameters(); |
joe4465 | 2:969dfa4f2436 | 67 | _wireless->printf("<RYPIDP=%1.8f:RYPIDI=%1.8f:RYPIDD=%1.8f:RPPIDP=%1.8f:RPPIDI=%1.8f:RPPIDD=%1.8f:RRPIDP=%1.8f:RRPIDI=%1.8f:RRPIDD=%1.8f:SYPIDP=%1.8f:SYPIDI=%1.8f:SYPIDD=%1.8f:SPPIDP=%1.8f:SPPIDI=%1.8f:SPPIDD=%1.8f:SRPIDP=%1.8f:SRPIDI=%1.8f:SRPIDD=%1.8f:ARPIDP=%1.8f:ARPIDI=%1.8f:ARPIDD=%1.8f:ASPIDP=%1.8f:ASPIDI=%1.8f:ASPIDD=%1.8f>", |
joe4465 | 2:969dfa4f2436 | 68 | flightControllerPidParameters.yawRate.p, flightControllerPidParameters.yawRate.i, flightControllerPidParameters.yawRate.d, |
joe4465 | 2:969dfa4f2436 | 69 | flightControllerPidParameters.pitchRate.p, flightControllerPidParameters.pitchRate.i, flightControllerPidParameters.pitchRate.d, |
joe4465 | 2:969dfa4f2436 | 70 | flightControllerPidParameters.rollRate.p, flightControllerPidParameters.rollRate.i, flightControllerPidParameters.rollRate.d, |
joe4465 | 2:969dfa4f2436 | 71 | flightControllerPidParameters.yawStab.p, flightControllerPidParameters.yawStab.i, flightControllerPidParameters.yawStab.d, |
joe4465 | 2:969dfa4f2436 | 72 | flightControllerPidParameters.pitchStab.p, flightControllerPidParameters.pitchStab.i, flightControllerPidParameters.pitchStab.d, |
joe4465 | 2:969dfa4f2436 | 73 | flightControllerPidParameters.rollStab.p, flightControllerPidParameters.rollStab.i, flightControllerPidParameters.rollStab.d, |
joe4465 | 2:969dfa4f2436 | 74 | navigationControllerPidParameters.altitudeRate.p, navigationControllerPidParameters.altitudeRate.i, navigationControllerPidParameters.altitudeRate.d, |
joe4465 | 2:969dfa4f2436 | 75 | navigationControllerPidParameters.altitudeStab.p, navigationControllerPidParameters.altitudeStab.i, navigationControllerPidParameters.altitudeStab.d); |
joe4465 | 2:969dfa4f2436 | 76 | } |
joe4465 | 2:969dfa4f2436 | 77 | else if (mode == Status::GPS) |
joe4465 | 2:969dfa4f2436 | 78 | { |
joe4465 | 2:969dfa4f2436 | 79 | Gps::Value gpsValues = _sensors.getGpsValues(); |
joe4465 | 2:969dfa4f2436 | 80 | |
joe4465 | 2:969dfa4f2436 | 81 | _wireless->printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GInit=%d>", |
joe4465 | 2:969dfa4f2436 | 82 | gpsValues.latitude, gpsValues.longitude, gpsValues.altitude, gpsValues.fix); |
joe4465 | 2:969dfa4f2436 | 83 | }/* |
joe4465 | 2:969dfa4f2436 | 84 | else if (mode == Status::ZERO) |
joe4465 | 2:969dfa4f2436 | 85 | { |
joe4465 | 2:969dfa4f2436 | 86 | _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>", |
joe4465 | 2:969dfa4f2436 | 87 | _zeroValues[0], _zeroValues[1], _zeroValues[2]); |
joe4465 | 2:969dfa4f2436 | 88 | break; |
joe4465 | 2:969dfa4f2436 | 89 | |
joe4465 | 2:969dfa4f2436 | 90 | }*/ |
joe4465 | 2:969dfa4f2436 | 91 | else if (mode == Status::RATE_TUNING) |
joe4465 | 2:969dfa4f2436 | 92 | { |
joe4465 | 2:969dfa4f2436 | 93 | if(_status.getFlightMode() == Status::RATE) |
joe4465 | 2:969dfa4f2436 | 94 | { |
joe4465 | 2:969dfa4f2436 | 95 | PidWrapper::RatePidState ratePidState = _flightController.getRatePidState(); |
joe4465 | 2:969dfa4f2436 | 96 | |
joe4465 | 2:969dfa4f2436 | 97 | //Yaw set point, Yaw actual, Yaw PID output |
joe4465 | 2:969dfa4f2436 | 98 | //Pitch set point, Pitch actual, Pitch PID output |
joe4465 | 2:969dfa4f2436 | 99 | //Roll set point, Roll actual, Roll PID output |
joe4465 | 2:969dfa4f2436 | 100 | _wireless->printf("<RYPIDS=%1.2f:RYPIDP=%1.2f:RYPIDO=%1.2f:RPPIDS=%1.2f:RPPIDP=%1.2f:RPPIDO=%1.2f:RRPIDS=%1.2f:RRPIDP=%1.2f:RRPIDO=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 101 | ratePidState.yawRate.setPoint, ratePidState.yawRate.processValue, ratePidState.yawRate.output, ratePidState.pitchRate.setPoint, ratePidState.pitchRate.processValue, ratePidState.pitchRate.output, ratePidState.rollRate.setPoint, ratePidState.rollRate.processValue, ratePidState.rollRate.output); |
joe4465 | 2:969dfa4f2436 | 102 | } |
joe4465 | 2:969dfa4f2436 | 103 | else if (_status.getFlightMode() == Status::STAB) |
joe4465 | 2:969dfa4f2436 | 104 | { |
joe4465 | 2:969dfa4f2436 | 105 | PidWrapper::StabPidState stabPidState = _flightController.getStabPidState(); |
joe4465 | 2:969dfa4f2436 | 106 | |
joe4465 | 2:969dfa4f2436 | 107 | //Yaw set point, Yaw actual, Yaw PID output |
joe4465 | 2:969dfa4f2436 | 108 | //Pitch set point, Pitch actual, Pitch PID output |
joe4465 | 2:969dfa4f2436 | 109 | //Roll set point, Roll actual, Roll PID output |
joe4465 | 2:969dfa4f2436 | 110 | _wireless->printf("<RYPIDS=%1.2f:RYPIDP=%1.2f:RYPIDO=%1.2f:RPPIDS=%1.2f:RPPIDP=%1.2f:RPPIDO=%1.2f:RRPIDS=%1.2f:RRPIDP=%1.2f:RRPIDO=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 111 | stabPidState.yawRate.setPoint, stabPidState.yawRate.processValue, stabPidState.yawRate.output, stabPidState.pitchRate.setPoint, stabPidState.pitchRate.processValue, stabPidState.pitchRate.output, stabPidState.rollRate.setPoint, stabPidState.rollRate.processValue, stabPidState.rollRate.output); |
joe4465 | 2:969dfa4f2436 | 112 | } |
joe4465 | 2:969dfa4f2436 | 113 | } |
joe4465 | 2:969dfa4f2436 | 114 | else if (mode == Status::STAB_TUNING) |
joe4465 | 2:969dfa4f2436 | 115 | { |
joe4465 | 2:969dfa4f2436 | 116 | if(_status.getFlightMode() == Status::RATE) |
joe4465 | 2:969dfa4f2436 | 117 | { |
joe4465 | 2:969dfa4f2436 | 118 | //Yaw set point, Yaw actual, Yaw PID output |
joe4465 | 2:969dfa4f2436 | 119 | //Pitch set point, Pitch actual, Pitch PID output |
joe4465 | 2:969dfa4f2436 | 120 | //Roll set point, Roll actual, Roll PID output |
joe4465 | 2:969dfa4f2436 | 121 | _wireless->printf("<SYPIDS=0:SYPIDP=0:SYPIDO=0:SPPIDS=0:SPPIDP=0:SPPIDO=0:SRPIDS=0:SRPIDP=0:SRPIDO=0>"); |
joe4465 | 2:969dfa4f2436 | 122 | } |
joe4465 | 2:969dfa4f2436 | 123 | else if (_status.getFlightMode() == Status::STAB) |
joe4465 | 2:969dfa4f2436 | 124 | { |
joe4465 | 2:969dfa4f2436 | 125 | PidWrapper::StabPidState stabPidState = _flightController.getStabPidState(); |
joe4465 | 2:969dfa4f2436 | 126 | |
joe4465 | 2:969dfa4f2436 | 127 | //Yaw set point, Yaw actual, Yaw PID output |
joe4465 | 2:969dfa4f2436 | 128 | //Pitch set point, Pitch actual, Pitch PID output |
joe4465 | 2:969dfa4f2436 | 129 | //Roll set point, Roll actual, Roll PID output |
joe4465 | 2:969dfa4f2436 | 130 | _wireless->printf("<SYPIDS=%1.2f:SYPIDP=%1.2f:SYPIDO=%1.2f:SPPIDS=%1.2f:SPPIDP=%1.2f:SPPIDO=%1.2f:SRPIDS=%1.2f:SRPIDP=%1.2f:SRPIDO=%1.2f>", |
joe4465 | 2:969dfa4f2436 | 131 | stabPidState.yawStab.setPoint, stabPidState.yawStab.processValue, stabPidState.yawStab.output, stabPidState.pitchStab.setPoint, stabPidState.pitchStab.processValue, stabPidState.pitchStab.output, stabPidState.rollStab.setPoint, stabPidState.rollStab.processValue, stabPidState.rollStab.output); |
joe4465 | 2:969dfa4f2436 | 132 | } |
joe4465 | 2:969dfa4f2436 | 133 | } |
joe4465 | 2:969dfa4f2436 | 134 | else if (mode == Status::ALTITUDE) |
joe4465 | 2:969dfa4f2436 | 135 | { |
joe4465 | 4:9ffbf9101992 | 136 | Sensors::Altitude altitude = _sensors.getAltitude(); |
joe4465 | 4:9ffbf9101992 | 137 | Imu::Acceleration acceleration = _sensors.getImuAcceleration(); |
joe4465 | 4:9ffbf9101992 | 138 | |
joe4465 | 4:9ffbf9101992 | 139 | _wireless->printf("<ZVEL=%1.2f:CAlt=%1.2f:BAlt=%1.2f:LAlt=%1.2f>", |
joe4465 | 4:9ffbf9101992 | 140 | acceleration.z, altitude.computed, altitude.barometer, altitude.lidar); |
joe4465 | 2:969dfa4f2436 | 141 | } |
joe4465 | 2:969dfa4f2436 | 142 | else if (mode == Status::VELOCITY) |
joe4465 | 2:969dfa4f2436 | 143 | { |
joe4465 | 4:9ffbf9101992 | 144 | //Imu::Velocity velocity = _sensors.getImuVelocity(); |
joe4465 | 4:9ffbf9101992 | 145 | //_wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>", |
joe4465 | 4:9ffbf9101992 | 146 | //velocity.x, velocity.y, velocity.z); |
joe4465 | 4:9ffbf9101992 | 147 | Imu::Acceleration acceleration = _sensors.getImuAcceleration(); |
joe4465 | 3:4823d6750629 | 148 | _wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>", |
joe4465 | 4:9ffbf9101992 | 149 | acceleration.x, acceleration.y, acceleration.z); |
joe4465 | 2:969dfa4f2436 | 150 | } |
joe4465 | 2:969dfa4f2436 | 151 | else if (mode == Status::ALTITUDE_STATUS) |
joe4465 | 2:969dfa4f2436 | 152 | { |
joe4465 | 2:969dfa4f2436 | 153 | NavigationController::SetPoint setPoints = _navigationController.getSetPoint(); |
joe4465 | 2:969dfa4f2436 | 154 | Sensors::Altitude altitude = _sensors.getAltitude(); |
joe4465 | 3:4823d6750629 | 155 | _wireless->printf("<ACR=%1.2f:ATA=%1.2f:ATHR=%1.2f:CAlt=%1.2f:ZVEL=%1.2f>", |
joe4465 | 3:4823d6750629 | 156 | setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed, _sensors.getImuVelocity().z); |
joe4465 | 3:4823d6750629 | 157 | } |
joe4465 | 3:4823d6750629 | 158 | else if (mode == Status::LIDAR) |
joe4465 | 3:4823d6750629 | 159 | { |
joe4465 | 3:4823d6750629 | 160 | Sensors::Altitude altitude = _sensors.getAltitude(); |
joe4465 | 3:4823d6750629 | 161 | Imu::Angle angle = _sensors.getAngle(); |
joe4465 | 3:4823d6750629 | 162 | _wireless->printf("<SP=%1.2f:SR=%1.2f:LAlt=%1.2f>", angle.pitch, angle.roll, altitude.lidar); |
joe4465 | 2:969dfa4f2436 | 163 | } |
joe4465 | 2:969dfa4f2436 | 164 | |
joe4465 | 2:969dfa4f2436 | 165 | //Check for wireless serial command |
joe4465 | 2:969dfa4f2436 | 166 | while (_wireless->readable() > 0) |
joe4465 | 2:969dfa4f2436 | 167 | { |
joe4465 | 2:969dfa4f2436 | 168 | int c = _wireless->getc(); |
joe4465 | 2:969dfa4f2436 | 169 | |
joe4465 | 2:969dfa4f2436 | 170 | switch (c) |
joe4465 | 2:969dfa4f2436 | 171 | { |
joe4465 | 2:969dfa4f2436 | 172 | case 60: // |
joe4465 | 2:969dfa4f2436 | 173 | _wirelessSerialRxPos = 0; |
joe4465 | 2:969dfa4f2436 | 174 | break; |
joe4465 | 2:969dfa4f2436 | 175 | |
joe4465 | 2:969dfa4f2436 | 176 | case 10: // LF |
joe4465 | 2:969dfa4f2436 | 177 | case 13: // CR |
joe4465 | 2:969dfa4f2436 | 178 | case 62: // > |
joe4465 | 2:969dfa4f2436 | 179 | checkCommand(); |
joe4465 | 2:969dfa4f2436 | 180 | break; |
joe4465 | 2:969dfa4f2436 | 181 | |
joe4465 | 2:969dfa4f2436 | 182 | default: |
joe4465 | 2:969dfa4f2436 | 183 | _wirelessSerialBuffer[_wirelessSerialRxPos++] = c; |
joe4465 | 2:969dfa4f2436 | 184 | if (_wirelessSerialRxPos > 200) |
joe4465 | 2:969dfa4f2436 | 185 | { |
joe4465 | 2:969dfa4f2436 | 186 | _wirelessSerialRxPos = 0; |
joe4465 | 2:969dfa4f2436 | 187 | } |
joe4465 | 2:969dfa4f2436 | 188 | break; |
joe4465 | 2:969dfa4f2436 | 189 | } |
joe4465 | 2:969dfa4f2436 | 190 | } |
joe4465 | 2:969dfa4f2436 | 191 | |
joe4465 | 2:969dfa4f2436 | 192 | Thread::wait(200); |
joe4465 | 2:969dfa4f2436 | 193 | } |
joe4465 | 2:969dfa4f2436 | 194 | } |
joe4465 | 2:969dfa4f2436 | 195 | |
joe4465 | 2:969dfa4f2436 | 196 | void BaseStation::checkCommand() |
joe4465 | 2:969dfa4f2436 | 197 | { |
joe4465 | 2:969dfa4f2436 | 198 | int length = _wirelessSerialRxPos; |
joe4465 | 2:969dfa4f2436 | 199 | _wirelessSerialBuffer[_wirelessSerialRxPos] = 0; |
joe4465 | 2:969dfa4f2436 | 200 | _wirelessSerialRxPos = 0; |
joe4465 | 2:969dfa4f2436 | 201 | |
joe4465 | 2:969dfa4f2436 | 202 | if (length < 1) |
joe4465 | 2:969dfa4f2436 | 203 | { |
joe4465 | 2:969dfa4f2436 | 204 | return; |
joe4465 | 2:969dfa4f2436 | 205 | } |
joe4465 | 2:969dfa4f2436 | 206 | |
joe4465 | 2:969dfa4f2436 | 207 | char command = _wirelessSerialBuffer[0]; |
joe4465 | 2:969dfa4f2436 | 208 | double value = 0; |
joe4465 | 2:969dfa4f2436 | 209 | if(length > 1) |
joe4465 | 2:969dfa4f2436 | 210 | { |
joe4465 | 2:969dfa4f2436 | 211 | value = atof((char*)&_wirelessSerialBuffer[2]); |
joe4465 | 2:969dfa4f2436 | 212 | } |
joe4465 | 2:969dfa4f2436 | 213 | |
joe4465 | 2:969dfa4f2436 | 214 | PidWrapper::FlightControllerPidParameters flightControllerPidParameters = _flightController.getPidParameters(); |
joe4465 | 2:969dfa4f2436 | 215 | PidWrapper::NavigationControllerPidParameters navigationControllerPidParameters = _navigationController.getPidParameters(); |
joe4465 | 2:969dfa4f2436 | 216 | |
joe4465 | 2:969dfa4f2436 | 217 | switch (command) |
joe4465 | 2:969dfa4f2436 | 218 | { |
joe4465 | 2:969dfa4f2436 | 219 | case 'a': |
joe4465 | 2:969dfa4f2436 | 220 | navigationControllerPidParameters.altitudeRate.p = value; |
joe4465 | 2:969dfa4f2436 | 221 | _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate); |
joe4465 | 2:969dfa4f2436 | 222 | break; |
joe4465 | 2:969dfa4f2436 | 223 | |
joe4465 | 2:969dfa4f2436 | 224 | case 'b': |
joe4465 | 2:969dfa4f2436 | 225 | navigationControllerPidParameters.altitudeRate.i = value; |
joe4465 | 2:969dfa4f2436 | 226 | _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate); |
joe4465 | 2:969dfa4f2436 | 227 | break; |
joe4465 | 2:969dfa4f2436 | 228 | |
joe4465 | 2:969dfa4f2436 | 229 | case 'c': |
joe4465 | 2:969dfa4f2436 | 230 | navigationControllerPidParameters.altitudeRate.d = value; |
joe4465 | 2:969dfa4f2436 | 231 | _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate); |
joe4465 | 2:969dfa4f2436 | 232 | break; |
joe4465 | 2:969dfa4f2436 | 233 | |
joe4465 | 2:969dfa4f2436 | 234 | case 'd': |
joe4465 | 2:969dfa4f2436 | 235 | navigationControllerPidParameters.altitudeStab.p = value; |
joe4465 | 2:969dfa4f2436 | 236 | _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab); |
joe4465 | 2:969dfa4f2436 | 237 | break; |
joe4465 | 2:969dfa4f2436 | 238 | |
joe4465 | 2:969dfa4f2436 | 239 | case 'e': |
joe4465 | 2:969dfa4f2436 | 240 | navigationControllerPidParameters.altitudeStab.i = value; |
joe4465 | 2:969dfa4f2436 | 241 | _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab); |
joe4465 | 2:969dfa4f2436 | 242 | break; |
joe4465 | 2:969dfa4f2436 | 243 | |
joe4465 | 2:969dfa4f2436 | 244 | case 'f': |
joe4465 | 2:969dfa4f2436 | 245 | navigationControllerPidParameters.altitudeStab.d = value; |
joe4465 | 2:969dfa4f2436 | 246 | _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab); |
joe4465 | 2:969dfa4f2436 | 247 | break; |
joe4465 | 2:969dfa4f2436 | 248 | |
joe4465 | 2:969dfa4f2436 | 249 | case 'g': |
joe4465 | 2:969dfa4f2436 | 250 | _sensors.zeroAccel(); |
joe4465 | 2:969dfa4f2436 | 251 | break; |
joe4465 | 2:969dfa4f2436 | 252 | |
joe4465 | 2:969dfa4f2436 | 253 | //Set PID values |
joe4465 | 2:969dfa4f2436 | 254 | case 'h': |
joe4465 | 2:969dfa4f2436 | 255 | flightControllerPidParameters.yawRate.p = value; |
joe4465 | 2:969dfa4f2436 | 256 | _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate); |
joe4465 | 2:969dfa4f2436 | 257 | break; |
joe4465 | 2:969dfa4f2436 | 258 | |
joe4465 | 2:969dfa4f2436 | 259 | case 'i': |
joe4465 | 2:969dfa4f2436 | 260 | flightControllerPidParameters.yawRate.i = value; |
joe4465 | 2:969dfa4f2436 | 261 | _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate); |
joe4465 | 2:969dfa4f2436 | 262 | break; |
joe4465 | 2:969dfa4f2436 | 263 | |
joe4465 | 2:969dfa4f2436 | 264 | case 'j': |
joe4465 | 2:969dfa4f2436 | 265 | flightControllerPidParameters.yawRate.d = value; |
joe4465 | 2:969dfa4f2436 | 266 | _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate); |
joe4465 | 2:969dfa4f2436 | 267 | break; |
joe4465 | 2:969dfa4f2436 | 268 | |
joe4465 | 2:969dfa4f2436 | 269 | case 'k': |
joe4465 | 2:969dfa4f2436 | 270 | flightControllerPidParameters.pitchRate.p = value; |
joe4465 | 2:969dfa4f2436 | 271 | _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate); |
joe4465 | 2:969dfa4f2436 | 272 | break; |
joe4465 | 2:969dfa4f2436 | 273 | |
joe4465 | 2:969dfa4f2436 | 274 | case 'l': |
joe4465 | 2:969dfa4f2436 | 275 | flightControllerPidParameters.pitchRate.i = value; |
joe4465 | 2:969dfa4f2436 | 276 | _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate); |
joe4465 | 2:969dfa4f2436 | 277 | break; |
joe4465 | 2:969dfa4f2436 | 278 | |
joe4465 | 2:969dfa4f2436 | 279 | case 'm': |
joe4465 | 2:969dfa4f2436 | 280 | flightControllerPidParameters.pitchRate.d = value; |
joe4465 | 2:969dfa4f2436 | 281 | _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate); |
joe4465 | 2:969dfa4f2436 | 282 | break; |
joe4465 | 2:969dfa4f2436 | 283 | |
joe4465 | 2:969dfa4f2436 | 284 | case 'n': |
joe4465 | 2:969dfa4f2436 | 285 | flightControllerPidParameters.rollRate.p = value; |
joe4465 | 2:969dfa4f2436 | 286 | _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate); |
joe4465 | 2:969dfa4f2436 | 287 | break; |
joe4465 | 2:969dfa4f2436 | 288 | |
joe4465 | 2:969dfa4f2436 | 289 | case 'o': |
joe4465 | 2:969dfa4f2436 | 290 | flightControllerPidParameters.rollRate.i = value; |
joe4465 | 2:969dfa4f2436 | 291 | _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate); |
joe4465 | 2:969dfa4f2436 | 292 | break; |
joe4465 | 2:969dfa4f2436 | 293 | |
joe4465 | 2:969dfa4f2436 | 294 | case 'p': |
joe4465 | 2:969dfa4f2436 | 295 | flightControllerPidParameters.rollRate.d = value; |
joe4465 | 2:969dfa4f2436 | 296 | _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate); |
joe4465 | 2:969dfa4f2436 | 297 | break; |
joe4465 | 2:969dfa4f2436 | 298 | |
joe4465 | 2:969dfa4f2436 | 299 | case 'q': |
joe4465 | 2:969dfa4f2436 | 300 | flightControllerPidParameters.yawStab.p = value; |
joe4465 | 2:969dfa4f2436 | 301 | _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab); |
joe4465 | 2:969dfa4f2436 | 302 | break; |
joe4465 | 2:969dfa4f2436 | 303 | |
joe4465 | 2:969dfa4f2436 | 304 | case 'r': |
joe4465 | 2:969dfa4f2436 | 305 | flightControllerPidParameters.yawStab.i = value; |
joe4465 | 2:969dfa4f2436 | 306 | _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab); |
joe4465 | 2:969dfa4f2436 | 307 | break; |
joe4465 | 2:969dfa4f2436 | 308 | |
joe4465 | 2:969dfa4f2436 | 309 | case 's': |
joe4465 | 2:969dfa4f2436 | 310 | flightControllerPidParameters.yawStab.d = value; |
joe4465 | 2:969dfa4f2436 | 311 | _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab); |
joe4465 | 2:969dfa4f2436 | 312 | break; |
joe4465 | 2:969dfa4f2436 | 313 | |
joe4465 | 2:969dfa4f2436 | 314 | case 't': |
joe4465 | 2:969dfa4f2436 | 315 | flightControllerPidParameters.pitchStab.p = value; |
joe4465 | 2:969dfa4f2436 | 316 | _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab); |
joe4465 | 2:969dfa4f2436 | 317 | break; |
joe4465 | 2:969dfa4f2436 | 318 | |
joe4465 | 2:969dfa4f2436 | 319 | case 'u': |
joe4465 | 2:969dfa4f2436 | 320 | flightControllerPidParameters.pitchStab.i = value; |
joe4465 | 2:969dfa4f2436 | 321 | _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab); |
joe4465 | 2:969dfa4f2436 | 322 | break; |
joe4465 | 2:969dfa4f2436 | 323 | |
joe4465 | 2:969dfa4f2436 | 324 | case 'v': |
joe4465 | 2:969dfa4f2436 | 325 | flightControllerPidParameters.pitchStab.d = value; |
joe4465 | 2:969dfa4f2436 | 326 | _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab); |
joe4465 | 2:969dfa4f2436 | 327 | break; |
joe4465 | 2:969dfa4f2436 | 328 | |
joe4465 | 2:969dfa4f2436 | 329 | case 'w': |
joe4465 | 2:969dfa4f2436 | 330 | flightControllerPidParameters.rollStab.p = value; |
joe4465 | 2:969dfa4f2436 | 331 | _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab); |
joe4465 | 2:969dfa4f2436 | 332 | break; |
joe4465 | 2:969dfa4f2436 | 333 | |
joe4465 | 2:969dfa4f2436 | 334 | case 'x': |
joe4465 | 2:969dfa4f2436 | 335 | flightControllerPidParameters.rollStab.i = value; |
joe4465 | 2:969dfa4f2436 | 336 | _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab); |
joe4465 | 2:969dfa4f2436 | 337 | break; |
joe4465 | 2:969dfa4f2436 | 338 | |
joe4465 | 2:969dfa4f2436 | 339 | case 'y': |
joe4465 | 2:969dfa4f2436 | 340 | flightControllerPidParameters.rollStab.d = value; |
joe4465 | 2:969dfa4f2436 | 341 | _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab); |
joe4465 | 2:969dfa4f2436 | 342 | break; |
joe4465 | 2:969dfa4f2436 | 343 | |
joe4465 | 2:969dfa4f2436 | 344 | case 'z': |
joe4465 | 2:969dfa4f2436 | 345 | _status.setBaseStationMode(static_cast<Status::BaseStationMode>(value)); |
joe4465 | 2:969dfa4f2436 | 346 | break; |
joe4465 | 2:969dfa4f2436 | 347 | |
joe4465 | 2:969dfa4f2436 | 348 | default: |
joe4465 | 2:969dfa4f2436 | 349 | break; |
joe4465 | 2:969dfa4f2436 | 350 | } |
joe4465 | 2:969dfa4f2436 | 351 | |
joe4465 | 2:969dfa4f2436 | 352 | return; |
joe4465 | 2:969dfa4f2436 | 353 | } |