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

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?

UserRevisionLine numberNew 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 }