Joseph Roberts
New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Diff: BaseStation/BaseStation.cpp
- Revision:
- 2:969dfa4f2436
- Parent:
- 0:c6a85bb2a827
- Child:
- 3:4823d6750629
--- a/BaseStation/BaseStation.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/BaseStation/BaseStation.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,342 @@
+#include "BaseStation.h"
+
+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)
+{
+ _wireless = new MODSERIAL(wirelessPinTx, wirelessPinRx);
+ _wireless->baud(57600);
+ _wirelessSerialRxPos = 0;
+
+ _thread = new Thread(&BaseStation::threadStarter, this, osPriorityNormal);
+ DEBUG("Base Station initialised\r\n");
+}
+
+BaseStation::~BaseStation(){}
+
+void BaseStation::threadStarter(void const *p)
+{
+ BaseStation *instance = (BaseStation*)p;
+ instance->threadWorker();
+}
+
+void BaseStation::threadWorker()
+{
+ while(true)
+ {
+ //Check comms mode and print correct data back to PC application
+ Status::BaseStationMode mode = _status.getBaseStationMode();
+
+ if(mode == Status::MOTOR_POWER)
+ {
+ MotorMixer::MotorPower motorPower = _flightController.getMotorPower();
+
+ _wireless->printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>",
+ motorPower.motor1, motorPower.motor2, motorPower.motor3, motorPower.motor4);
+ }
+ else if (mode == Status::PID_OUTPUTS)
+ {
+ PidWrapper::PidOutput pidOutputs = _flightController.getPidOutputs();
+
+ _wireless->printf("<YPID=%1.2f:PPID=%1.2f:RPID=%1.2f>",
+ pidOutputs.yaw, pidOutputs.pitch, pidOutputs.roll);
+ }
+ else if (mode == Status::IMU_OUTPUTS)
+ {
+ Imu::Rate rate = _sensors.getRate();
+ Imu::Angle angle = _sensors.getAngle();
+
+ _wireless->printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>",
+ angle.yaw, angle.pitch, angle.roll, rate.yaw, rate.pitch, rate.roll);
+ }
+ else if (mode == Status::STATUS)
+ {
+ _wireless->printf("<Batt=%f:Armed=%d:Init=%d:FlightMode=%d:State=%d:NavMode=%d>",
+ _status.getBatteryLevel(), _status.getArmed(), _status.getInitialised(), _status.getFlightMode(), _status.getState(), _status.getNavigationMode());
+ }
+ else if (mode == Status::RC)
+ {
+ Rc::MappedRc mappedRc = _rc.getMappedRc();
+ Rc::RawRc rawRc = _rc.getRawRc();
+
+ _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>",
+ mappedRc.yaw, mappedRc.pitch, mappedRc.roll, mappedRc.throttle, rawRc.channel0, rawRc.channel1, rawRc.channel2, rawRc.channel3, rawRc.channel4, rawRc.channel5, rawRc.channel6, rawRc.channel7);
+ }
+ else if (mode == Status::PID_TUNING)
+ {
+ PidWrapper::FlightControllerPidParameters flightControllerPidParameters = _flightController.getPidParameters();
+ PidWrapper::NavigationControllerPidParameters navigationControllerPidParameters = _navigationController.getPidParameters();
+ _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>",
+ flightControllerPidParameters.yawRate.p, flightControllerPidParameters.yawRate.i, flightControllerPidParameters.yawRate.d,
+ flightControllerPidParameters.pitchRate.p, flightControllerPidParameters.pitchRate.i, flightControllerPidParameters.pitchRate.d,
+ flightControllerPidParameters.rollRate.p, flightControllerPidParameters.rollRate.i, flightControllerPidParameters.rollRate.d,
+ flightControllerPidParameters.yawStab.p, flightControllerPidParameters.yawStab.i, flightControllerPidParameters.yawStab.d,
+ flightControllerPidParameters.pitchStab.p, flightControllerPidParameters.pitchStab.i, flightControllerPidParameters.pitchStab.d,
+ flightControllerPidParameters.rollStab.p, flightControllerPidParameters.rollStab.i, flightControllerPidParameters.rollStab.d,
+ navigationControllerPidParameters.altitudeRate.p, navigationControllerPidParameters.altitudeRate.i, navigationControllerPidParameters.altitudeRate.d,
+ navigationControllerPidParameters.altitudeStab.p, navigationControllerPidParameters.altitudeStab.i, navigationControllerPidParameters.altitudeStab.d);
+ }
+ else if (mode == Status::GPS)
+ {
+ Gps::Value gpsValues = _sensors.getGpsValues();
+
+ _wireless->printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GInit=%d>",
+ gpsValues.latitude, gpsValues.longitude, gpsValues.altitude, gpsValues.fix);
+ }/*
+ else if (mode == Status::ZERO)
+ {
+ _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>",
+ _zeroValues[0], _zeroValues[1], _zeroValues[2]);
+ break;
+
+ }*/
+ else if (mode == Status::RATE_TUNING)
+ {
+ if(_status.getFlightMode() == Status::RATE)
+ {
+ PidWrapper::RatePidState ratePidState = _flightController.getRatePidState();
+
+ //Yaw set point, Yaw actual, Yaw PID output
+ //Pitch set point, Pitch actual, Pitch PID output
+ //Roll set point, Roll actual, Roll PID output
+ _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>",
+ 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);
+ }
+ else if (_status.getFlightMode() == Status::STAB)
+ {
+ PidWrapper::StabPidState stabPidState = _flightController.getStabPidState();
+
+ //Yaw set point, Yaw actual, Yaw PID output
+ //Pitch set point, Pitch actual, Pitch PID output
+ //Roll set point, Roll actual, Roll PID output
+ _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>",
+ 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);
+ }
+ }
+ else if (mode == Status::STAB_TUNING)
+ {
+ if(_status.getFlightMode() == Status::RATE)
+ {
+ //Yaw set point, Yaw actual, Yaw PID output
+ //Pitch set point, Pitch actual, Pitch PID output
+ //Roll set point, Roll actual, Roll PID output
+ _wireless->printf("<SYPIDS=0:SYPIDP=0:SYPIDO=0:SPPIDS=0:SPPIDP=0:SPPIDO=0:SRPIDS=0:SRPIDP=0:SRPIDO=0>");
+ }
+ else if (_status.getFlightMode() == Status::STAB)
+ {
+ PidWrapper::StabPidState stabPidState = _flightController.getStabPidState();
+
+ //Yaw set point, Yaw actual, Yaw PID output
+ //Pitch set point, Pitch actual, Pitch PID output
+ //Roll set point, Roll actual, Roll PID output
+ _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>",
+ 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);
+ }
+ }
+ else if (mode == Status::ALTITUDE)
+ {
+ Sensors::Altitude altitude = _sensors.getAltitude();
+ _wireless->printf("<GAlt=%1.2f:CAlt=%1.2f:BAlt=%1.2f:LAlt=%1.2f>",
+ altitude.gps, altitude.computed, altitude.barometer, altitude.lidar);
+ }
+ else if (mode == Status::VELOCITY)
+ {
+ //Sensors::Velocity velocity = _sensors.getVelocity();
+ //_wireless->printf("<AVelX=%1.2f:AVelY=%1.2f:AVelZ=%1.2f:LVel=%1.2f:CVelX=%1.2f:CVelY=%1.2f:CVelZ=%1.2f>",
+ //velocity.accelX, velocity.accelY, velocity.accelZ, velocity.lidar, velocity.computedX, velocity.computedY, velocity.computedZ);
+ }
+ else if (mode == Status::ALTITUDE_STATUS)
+ {
+ NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
+ Sensors::Altitude altitude = _sensors.getAltitude();
+ _wireless->printf("<ACR=%1.2f:AT=%1.2f:ATHR=%1.2f:LAlt=%1.2f>",
+ setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed);
+ }
+
+ //Check for wireless serial command
+ while (_wireless->readable() > 0)
+ {
+ int c = _wireless->getc();
+
+ switch (c)
+ {
+ case 60: //
+ _wirelessSerialRxPos = 0;
+ break;
+
+ case 10: // LF
+ case 13: // CR
+ case 62: // >
+ checkCommand();
+ break;
+
+ default:
+ _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
+ if (_wirelessSerialRxPos > 200)
+ {
+ _wirelessSerialRxPos = 0;
+ }
+ break;
+ }
+ }
+
+ Thread::wait(200);
+ }
+}
+
+void BaseStation::checkCommand()
+{
+ int length = _wirelessSerialRxPos;
+ _wirelessSerialBuffer[_wirelessSerialRxPos] = 0;
+ _wirelessSerialRxPos = 0;
+
+ if (length < 1)
+ {
+ return;
+ }
+
+ char command = _wirelessSerialBuffer[0];
+ double value = 0;
+ if(length > 1)
+ {
+ value = atof((char*)&_wirelessSerialBuffer[2]);
+ }
+
+ PidWrapper::FlightControllerPidParameters flightControllerPidParameters = _flightController.getPidParameters();
+ PidWrapper::NavigationControllerPidParameters navigationControllerPidParameters = _navigationController.getPidParameters();
+
+ switch (command)
+ {
+ case 'a':
+ navigationControllerPidParameters.altitudeRate.p = value;
+ _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate);
+ break;
+
+ case 'b':
+ navigationControllerPidParameters.altitudeRate.i = value;
+ _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate);
+ break;
+
+ case 'c':
+ navigationControllerPidParameters.altitudeRate.d = value;
+ _navigationController.setAltitudeRatePidParameters(navigationControllerPidParameters.altitudeRate);
+ break;
+
+ case 'd':
+ navigationControllerPidParameters.altitudeStab.p = value;
+ _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab);
+ break;
+
+ case 'e':
+ navigationControllerPidParameters.altitudeStab.i = value;
+ _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab);
+ break;
+
+ case 'f':
+ navigationControllerPidParameters.altitudeStab.d = value;
+ _navigationController.setAltitudeStabPidParameters(navigationControllerPidParameters.altitudeStab);
+ break;
+
+ case 'g':
+ _sensors.zeroAccel();
+ break;
+
+ //Set PID values
+ case 'h':
+ flightControllerPidParameters.yawRate.p = value;
+ _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate);
+ break;
+
+ case 'i':
+ flightControllerPidParameters.yawRate.i = value;
+ _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate);
+ break;
+
+ case 'j':
+ flightControllerPidParameters.yawRate.d = value;
+ _flightController.setYawRatePidParameters(flightControllerPidParameters.yawRate);
+ break;
+
+ case 'k':
+ flightControllerPidParameters.pitchRate.p = value;
+ _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate);
+ break;
+
+ case 'l':
+ flightControllerPidParameters.pitchRate.i = value;
+ _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate);
+ break;
+
+ case 'm':
+ flightControllerPidParameters.pitchRate.d = value;
+ _flightController.setPitchRatePidParameters(flightControllerPidParameters.pitchRate);
+ break;
+
+ case 'n':
+ flightControllerPidParameters.rollRate.p = value;
+ _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate);
+ break;
+
+ case 'o':
+ flightControllerPidParameters.rollRate.i = value;
+ _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate);
+ break;
+
+ case 'p':
+ flightControllerPidParameters.rollRate.d = value;
+ _flightController.setRollRatePidParameters(flightControllerPidParameters.rollRate);
+ break;
+
+ case 'q':
+ flightControllerPidParameters.yawStab.p = value;
+ _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab);
+ break;
+
+ case 'r':
+ flightControllerPidParameters.yawStab.i = value;
+ _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab);
+ break;
+
+ case 's':
+ flightControllerPidParameters.yawStab.d = value;
+ _flightController.setYawStabPidParameters(flightControllerPidParameters.yawStab);
+ break;
+
+ case 't':
+ flightControllerPidParameters.pitchStab.p = value;
+ _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab);
+ break;
+
+ case 'u':
+ flightControllerPidParameters.pitchStab.i = value;
+ _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab);
+ break;
+
+ case 'v':
+ flightControllerPidParameters.pitchStab.d = value;
+ _flightController.setPitchStabPidParameters(flightControllerPidParameters.pitchStab);
+ break;
+
+ case 'w':
+ flightControllerPidParameters.rollStab.p = value;
+ _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab);
+ break;
+
+ case 'x':
+ flightControllerPidParameters.rollStab.i = value;
+ _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab);
+ break;
+
+ case 'y':
+ flightControllerPidParameters.rollStab.d = value;
+ _flightController.setRollStabPidParameters(flightControllerPidParameters.rollStab);
+ break;
+
+ case 'z':
+ _status.setBaseStationMode(static_cast<Status::BaseStationMode>(value));
+ break;
+
+ default:
+ break;
+ }
+
+ return;
+}
\ No newline at end of file