Joseph Roberts
New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Revision 2:969dfa4f2436, committed 2015-04-01
- Comitter:
- joe4465
- Date:
- Wed Apr 01 11:19:21 2015 +0000
- Parent:
- 1:ec3521d90369
- Child:
- 3:4823d6750629
- Commit message:
- Altitude hold with 2 PID's working. Exported to offline compiler
Changed in this revision
--- 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
--- a/BaseStation/BaseStation.h Wed Mar 04 18:53:43 2015 +0000
+++ b/BaseStation/BaseStation.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,37 @@
+#include "mbed.h"
+#include "Global.h"
+#include "rtos.h"
+#include "MODSERIAL.h"
+#include "Rc.h"
+#include "Sensors.h"
+#include "Status.h"
+#include "NavigationController.h"
+#include "FlightController.h"
+
+#ifndef BaseStation_H
+#define BaseStation_H
+
+class BaseStation
+{
+ public:
+ BaseStation(Status& status, Rc& rc, Sensors& sensors, NavigationController& navigationController, FlightController& flightController, ConfigFileWrapper& configFileWrapper, PinName wirelessPinTx, PinName wirelessPinRx);
+ ~BaseStation();
+
+ private:
+ static void threadStarter(void const *p);
+ void threadWorker();
+ void checkCommand();
+
+ Thread* _thread;
+ MODSERIAL* _wireless;
+ Status& _status;
+ Rc& _rc;
+ Sensors& _sensors;
+ NavigationController& _navigationController;
+ FlightController& _flightController;
+ ConfigFileWrapper& _configFileWrapper;
+ char _wirelessSerialBuffer[255];
+ int _wirelessSerialRxPos;
+};
+
+#endif
\ No newline at end of file
--- a/ConfigFileWrapper/ConfigFile.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/shintamainjp/code/ConfigFile/#f6ceafabe9f8
--- a/ConfigFileWrapper/ConfigFileWrapper.cpp Wed Mar 04 18:53:43 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,161 +0,0 @@
-#include "ConfigFileWrapper.h"
-
-ConfigFileWrapper::ConfigFileWrapper(){}
-
-ConfigFileWrapper::~ConfigFileWrapper(){}
-
-bool PidWrapper::initialise()
-{
- _str = new char[1024];
- loadSettings();
-
- DEBUG("Config file wrapper initialised");
- return true;
-}
-
-pidWrapper::PidParameters ConfigFileWrapper::getYawRateParameters()
-{
- return _yawRateParameters;
-}
-
-pidWrapper::PidParameters ConfigFileWrapper::getPitchRateParameters()
-{
- return _pitchRateParameters;
-}
-
-pidWrapper::PidParameters ConfigFileWrapper::getRollRateParameters()
-{
- return _rollRateParameters;
-}
-
-pidWrapper::PidParameters ConfigFileWrapper::getYawStabParameters()
-{
- return _yawStabParameters;
-}
-
-pidWrapper::PidParameters ConfigFileWrapper::getPitchStabParameters()
-{
- return _pitchStabParameters;
-}
-
-pidWrapper::PidParameters ConfigFileWrapper::getRollStabParameters()
-{
- return _rollStabParameters;
-}
-
-bool ConfigFileWrapper::setYawRateParameters(pidWrapper::PidParameters)
-{
- return true;
-}
-
-bool ConfigFileWrapper::setPitchRateParameters(pidWrapper::PidParameters)
-{
- return true;
-}
-
-bool ConfigFileWrapper::setRollRateParameters(pidWrapper::PidParameters)
-{
- return true;
-}
-
-bool ConfigFileWrapper::setYawStabParameters(pidWrapper::PidParameters)
-{
- return true;
-}
-
-bool ConfigFileWrapper::setPitchStabParameters(pidWrapper::PidParameters)
-{
- return true;
-}
-
-bool ConfigFileWrapper::setRollStabParameters(pidWrapper::PidParameters)
-{
- return true;
-}
-
-void ConfigFileWrapper::convertToCharArray(float number)
-{
- sprintf(_str, "%1.8f", number );
-}
-
-void ConfigFileWrapper::convertToCharArray(int number)
-{
- sprintf(_str, "%d", number );
-}
-
-void ConfigFileWrapper::loadSettings()
-{
- char value[BUFSIZ];
-
- //Read a configuration file from a mbed.
- if (!_configFile.read("/local/config.cfg"))
- {
- DEBUG("Config file does not exist\n\r");
- }
- else
- {
- //Get values
- if (_configFile.getValue("yawRatePIDControllerP", &value[0], sizeof(value))) _yawRateParameters.p = atof(value);
- else DEBUG("Failed to get value for yawRatePIDControllerP\n\r");
-
- if (_configFile.getValue("yawRatePIDControllerI", &value[0], sizeof(value))) _yawRateParameters.i = atof(value);
- else DEBUG("Failed to get value for yawRatePIDControllerI\n\r");
-
- if (_configFile.getValue("yawRatePIDControllerD", &value[0], sizeof(value))) _yawRateParameters.d = atof(value);
- else DEBUG("Failed to get value for yawRatePIDControllerD\n\r");
-
- if (_configFile.getValue("pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRateParameters.p = atof(value);
- else DEBUG("Failed to get value for pitchRatePIDControllerP\n\r");
-
- if (_configFile.getValue("pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRateParameters.i = atof(value);
- else DEBUG("Failed to get value for pitchRatePIDControllerI\n\r");
-
- if (_configFile.getValue("pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRateParameters.d = atof(value);
- else DEBUG("Failed to get value for pitchRatePIDControllerD\n\r");
-
- if (_configFile.getValue("rollRatePIDControllerP", &value[0], sizeof(value))) _rollRateParameters.p = atof(value);
- else DEBUG("Failed to get value for rollRatePIDControllerP\n\r");
-
- if (_configFile.getValue("rollRatePIDControllerI", &value[0], sizeof(value))) _rollRateParameters.i = atof(value);
- else DEBUG("Failed to get value for rollRatePIDControllerI\n\r");
-
- if (_configFile.getValue("rollRatePIDControllerD", &value[0], sizeof(value))) _rollRateParameters.d = atof(value);
- else DEBUG("Failed to get value for rollRatePIDControllerD\n\r");
-
-
- if (_configFile.getValue("yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabParameters.p = atof(value);
- else DEBUG("Failed to get value for yawStabPIDControllerP\n\r");
-
- if (_configFile.getValue("yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabParameters.i = atof(value);
- else DEBUG("Failed to get value for yawStabPIDControllerI\n\r");
-
- if (_configFile.getValue("yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabParameters.d = atof(value);
- else DEBUG("Failed to get value for yawStabPIDControllerD\n\r");
-
- if (_configFile.getValue("pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabParameters.p = atof(value);
- else DEBUG("Failed to get value for pitchStabPIDControllerP\n\r");
-
- if (_configFile.getValue("pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabParameters.i = atof(value);
- else DEBUG("Failed to get value for pitchStabPIDControllerI\n\r");
-
- if (_configFile.getValue("pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabParameters.d = atof(value);
- else DEBUG("Failed to get value for pitchStabPIDControllerD\n\r");
-
- if (_configFile.getValue("rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabParameters.p = atof(value);
- else DEBUG("Failed to get value for rollStabPIDControllerP\n\r");
-
- if (_configFile.getValue("rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabParameters.i = atof(value);
- else DEBUG("Failed to get value for rollStabPIDControllerI\n\r");
-
- if (_configFile.getValue("rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabParameters.d = atof(value);
- else DEBUG("Failed to get value for rollStabPIDControllerD\n\r");
-
- /*if (_configFile.getValue("zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
- else DEBUG("Failed to get value for zero pitch\n\r");
-
- if (_configFile.getValue("zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
- else printf("Failed to get value for zero roll\n\r");*/
- }
-
- DEBUG("Finished loading settings from config file\n\r");
-}
--- a/ConfigFileWrapper/ConfigFileWrapper.h Wed Mar 04 18:53:43 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,44 +0,0 @@
-#include "mbed.h"
-#include "Global.h"
-#include "ConfigFile.h"
-#include "PidWrapper.h"
-
-#ifndef ConfigFileWrapper_H
-#define ConfigFileWrapper_H
-
-class ConfigFileWrapper // begin declaration of the class
-{
- public: // begin public section
- ConfigFileWrapper(); // constructor
- ~ConfigFileWrapper();
-
- bool initialise();
- pidWrapper::PidParameters getYawRateParameters();
- pidWrapper::PidParameters getPitchRateParameters();
- pidWrapper::PidParameters getRollRateParameters();
- pidWrapper::PidParameters getYawStabParameters();
- pidWrapper::PidParameters getPitchStabParameters();
- pidWrapper::PidParameters getRollStabParameters();
-
- bool setYawRateParameters(pidWrapper::PidParameters);
- bool setPitchRateParameters(pidWrapper::PidParameters);
- bool setRollRateParameters(pidWrapper::PidParameters);
- bool setYawStabParameters(pidWrapper::PidParameters);
- bool setPitchStabParameters(pidWrapper::PidParameters);
- bool setRollStabParameters(pidWrapper::PidParameters);
-
- private:
- ConfigFile _configFile;
- char* _str
- pidWrapper::PidParameters _yawRateParameters;
- pidWrapper::PidParameters _pitchRateParameters;
- pidWrapper::PidParameters _rollRateParameters;
- pidWrapper::PidParameters _yawStabParameters;
- pidWrapper::PidParameters _pitchStabParameters;
- pidWrapper::PidParameters _rollStabParameters;
- void convertToCharArray(float number);
- void convertToCharArray(int number);
- void loadSettings();
-};
-
-#endif
\ No newline at end of file
--- a/FlightController/FlightController.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/FlightController.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -1,24 +1,21 @@
#include "FlightController.h"
-FlightController::FlightController(){}
+FlightController::FlightController(Status& status, Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper, PinName motor1, PinName motor2, PinName motor3, PinName motor4)
+ : _status(status), _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
+{
+ _motorMixer = new MotorMixer(motor1, motor2, motor3, motor4);
+ _rateController = new RateController(_sensors, _navigationController, _configFileWrapper);
+ _stabController = new StabController(_sensors, _navigationController, _configFileWrapper);
+
+ _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void*)this);
+ int updateTime = (1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY) * 1000;
+ _rtosTimer->start(updateTime);
+
+ DEBUG("Flight controller initialised\r\n");
+}
FlightController::~FlightController(){}
-bool FlightController::initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4)
-{
- _status = status;
-
- _motorMixer.initialise(motor1, motor2, motor3, motor4);
- _rateController.initialise(sensors, navigationController);
- _stabController.initialise(sensors, navigationController);
-
- _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void *)0);
- int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
- _rtosTimer->start(updateTime);
- DEBUG("Flight controller initialised");
- return true;
-}
-
void FlightController::threadStarter(void const *p)
{
FlightController *instance = (FlightController*)p;
@@ -26,37 +23,123 @@
}
void FlightController::threadWorker()
-{
- //Struct to hold PID outputs
- PidWrapper::PidOutputs pidOutputs;
+{
+ Status::FlightMode flightMode = _status.getFlightMode();
+
+ _sensors.updateImu();
- //Check state is valid
+ if(flightMode == Status::RATE)
+ {
+ //Rate mode
+ _pidOutputs = _rateController->compute();
+ }
+ else if (flightMode == Status::STAB)
+ {
+ //Stab mode
+ _pidOutputs = _stabController->compute();
+ }
+
Status::State state = _status.getState();
+ NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
+
if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO)
{
- //State valid
- Status::FlightMode flightMode = _status.getFlightMode();
- if(flightMode == Status::RATE)
- {
- //Rate mode
- pidOutputs = _rateController.compute();
- }
- else if (flightMode == Status::STAB)
- {
- //Stab mode
- pidOutputs = _stabController.compute();
- }
- else
- {
- //Invalid state
- _status.setState(Status::ABORT);
- return;
- }
-
- _motorMixer.computePower(pidOutputs, throttle);
+ _motorMixer->computePower(_pidOutputs, setPoints.throttle);
+ }
+ else if(state == Status::GROUND_READY)
+ {
+ _motorMixer->setPower(MOTORS_ARMED);
+ _rateController->reset();
+ _stabController->reset();
+ }
+ else if(state == Status::STANDBY)
+ {
+ _motorMixer->setPower(MOTORS_OFF);
+ _rateController->reset();
+ _stabController->reset();
}
- //Set motors to armed if state is ground ready
- else if (state == Status::GROUND_READY) _motorMixer.setPower(MOTORS_ARMED);
//Disable motors if state is not valid
- else _motorMixer.setPower(MOTORS_OFF);
+ else _motorMixer->setPower(MOTORS_OFF);
+}
+
+MotorMixer::MotorPower FlightController::getMotorPower()
+{
+ return _motorMixer->getMotorPower();
+}
+
+PidWrapper::PidOutput FlightController::getPidOutputs()
+{
+ return _pidOutputs;
+}
+
+PidWrapper::FlightControllerPidParameters FlightController::getPidParameters()
+{
+ return _stabController->getPidParameters();
+}
+
+void FlightController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _stabController->setYawStabPidParameters(pidParameters);
+ _configFileWrapper.setYawStabParameters(pidParameters);
+ Status::State state = _status.getState();
+ saveSettings();
+}
+
+void FlightController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _stabController->setPitchStabPidParameters(pidParameters);
+ _configFileWrapper.setPitchStabParameters(pidParameters);
+ saveSettings();
+}
+
+void FlightController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _stabController->setRollStabPidParameters(pidParameters);
+ _configFileWrapper.setRollStabParameters(pidParameters);
+ saveSettings();
}
+
+void FlightController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _stabController->setYawRatePidParameters(pidParameters);
+ _rateController->setYawRatePidParameters(pidParameters);
+ _configFileWrapper.setYawRateParameters(pidParameters);
+ saveSettings();
+}
+
+void FlightController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _stabController->setPitchRatePidParameters(pidParameters);
+ _rateController->setPitchRatePidParameters(pidParameters);
+ _configFileWrapper.setPitchRateParameters(pidParameters);
+ saveSettings();
+}
+
+void FlightController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _stabController->setRollRatePidParameters(pidParameters);
+ _rateController->setRollRatePidParameters(pidParameters);
+ _configFileWrapper.setRollRateParameters(pidParameters);
+ saveSettings();
+}
+
+void FlightController::saveSettings()
+{
+ Status::State state = _status.getState();
+ if(state == Status::STANDBY || state == Status::PREFLIGHT)
+ {
+ _sensors.enable(false);
+ _configFileWrapper.saveSettings();
+ _sensors.enable(true);
+ }
+}
+
+PidWrapper::RatePidState FlightController::getRatePidState()
+{
+ return _rateController->getRatePidState();
+}
+
+PidWrapper::StabPidState FlightController::getStabPidState()
+{
+ return _stabController->getStabPidState();
+}
\ No newline at end of file
--- a/FlightController/FlightController.h Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/FlightController.h Wed Apr 01 11:19:21 2015 +0000
@@ -6,6 +6,7 @@
#include "PidWrapper.h"
#include "RateController.h"
#include "StabController.h"
+#include "ConfigFileWrapper.h"
#ifndef FlightController_H
#define FlightController_H
@@ -13,26 +14,35 @@
class FlightController
{
public:
- FlightController();
+ FlightController(Status& status, Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper, PinName motor1, PinName motor2, PinName motor3, PinName motor4);
~FlightController();
- bool initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4);
- PidWrapper::PidValues getyawRatePID();
- PidWrapper::PidValues getPitchRatePID();
- PidWrapper::PidValues getRollRatePID();
- PidWrapper::PidValues getyawStabPID();
- PidWrapper::PidValues getPitchStabPID();
- PidWrapper::PidValues getRollStabPID();
MotorMixer::MotorPower getMotorPower();
+ PidWrapper::PidOutput getPidOutputs();
+ PidWrapper::FlightControllerPidParameters getPidParameters();
+ void setYawStabPidParameters(PidWrapper::PidParameter pidParameters);
+ void setPitchStabPidParameters(PidWrapper::PidParameter pidParameters);
+ void setRollStabPidParameters(PidWrapper::PidParameter pidParameters);
+ void setYawRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setPitchRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setRollRatePidParameters(PidWrapper::PidParameter pidParameters);
+ PidWrapper::RatePidState getRatePidState();
+ PidWrapper::StabPidState getStabPidState();
private:
static void threadStarter(void const *p);
void threadWorker();
RtosTimer* _rtosTimer;
- Status _status;
- MotorMixer _motorMixer;
- RateController _rateController;
- StabController _stabController;
+ RateController* _rateController;
+ StabController* _stabController;
+ void saveSettings();
+ Status& _status;
+ Sensors& _sensors;
+ NavigationController& _navigationController;
+ ConfigFileWrapper& _configFileWrapper;
+ MotorMixer* _motorMixer;
+
+ PidWrapper::PidOutput _pidOutputs;
};
#endif
\ No newline at end of file
--- a/FlightController/MotorMixer/MotorMixer.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/MotorMixer/MotorMixer.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -1,83 +1,111 @@
#include "MotorMixer.h"
-MotorMixer::MotorMixer(){}
-
-MotorMixer::~MotorMixer(){}
-
-bool MotorMixer::initialise(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
-{
+MotorMixer::MotorMixer(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
+{
+ _motorPower = MotorPower();
+
_motor1 = new PwmOut(motor1);
_motor2 = new PwmOut(motor2);
_motor3 = new PwmOut(motor3);
_motor4 = new PwmOut(motor4);
- //Set frequency
- float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
+ //Set period
+ double period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
_motor1->period(period);
_motor2->period(period);
_motor3->period(period);
_motor4->period(period);
//Disable
- _motor1 = MOTORS_OFF;
- _motor2 = MOTORS_OFF;
- _motor2 = MOTORS_OFF;
- _motor2 = MOTORS_OFF;
+ setPower(MOTORS_OFF);
- DEBUG("Motor power initialised");
- return true;
+ DEBUG("Motor power initialised\r\n");
}
-void MotorMixer::computePower(PidWrapper::PidOutputs pidOutputs, float throttle)
-{
+MotorMixer::~MotorMixer(){}
+
+void MotorMixer::computePower(PidWrapper::PidOutput pidOutput, double throttle)
+{
//Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
- float basePower = MOTORS_MIN + (throttle * 800);
+ double basePower = MOTORS_MIN + (throttle * 800);
+
+ MotorPower motorPower = MotorPower();
//Map motor power - each PID returns -100 <-> 100
- _motorPower.motor1 = basePower + pidOutputs.pitch + pidOutputs.roll + pidOutputs.yaw;
- _motorPower.motor2 = basePower + pidOutputs.pitch - pidOutputs.roll - pidOutputs.yaw;
- _motorPower.motor3 = basePower - pidOutputs.pitch - pidOutputs.roll + pidOutputs.yaw;
- _motorPower.motor4 = basePower - pidOutputs.pitch + pidOutputs.roll - pidOutputs.yaw;
+ motorPower.motor1 = basePower + pidOutput.pitch + pidOutput.roll + pidOutput.yaw;
+ motorPower.motor2 = basePower + pidOutput.pitch - pidOutput.roll - pidOutput.yaw;
+ motorPower.motor3 = basePower - pidOutput.pitch - pidOutput.roll + pidOutput.yaw;
+ motorPower.motor4 = basePower - pidOutput.pitch + pidOutput.roll - pidOutput.yaw;
//Specify intial motor power limits
- float motorFix = 0;
- float motorMin = _motorPower.motor1;
- float motorMax = _motorPower.motor1;
+ double motorFix = 0;
+ double motorMin = motorPower.motor1;
+ double motorMax = motorPower.motor1;
//Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
- if(_motorPower.motor1 < motorMin) motorMin = _motorPower.motor1;
- if(_motorPower.motor1 > motorMax) motorMax = _motorPower.motor1;
- if(_motorPower.motor2 < motorMin) motorMin = _motorPower.motor2;
- if(_motorPower.motor2 > motorMax) motorMax = _motorPower.motor2;
- if(_motorPower.motor3 < motorMin) motorMin = _motorPower.motor3;
- if(_motorPower.motor3 > motorMax) motorMax = _motorPower.motor3;
- if(_motorPower.motor4 < motorMin) motorMin = _motorPower.motor4;
- if(_motorPower.motor4 > motorMax) motorMax = _motorPower.motor4;
+ if(motorPower.motor1 < motorMin) motorMin = motorPower.motor1;
+ if(motorPower.motor1 > motorMax) motorMax = motorPower.motor1;
+ if(motorPower.motor2 < motorMin) motorMin = motorPower.motor2;
+ if(motorPower.motor2 > motorMax) motorMax = motorPower.motor2;
+ if(motorPower.motor3 < motorMin) motorMin = motorPower.motor3;
+ if(motorPower.motor3 > motorMax) motorMax = motorPower.motor3;
+ if(motorPower.motor4 < motorMin) motorMin = motorPower.motor4;
+ if(motorPower.motor4 > motorMax) motorMax = motorPower.motor4;
//Check if min or max is outside of the limits
if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
//Add/remove constant
- _motorPower.motor1 += motorFix;
- _motorPower.motor2 += motorFix;
- _motorPower.motor3 += motorFix;
- _motorPower.motor4 += motorFix;
+ motorPower.motor1 += motorFix;
+ motorPower.motor2 += motorFix;
+ motorPower.motor3 += motorFix;
+ motorPower.motor4 += motorFix;
+
+ //Set motor power
+ setPower(motorPower);
+}
+
+void MotorMixer::computePower(double throttle)
+{
+ //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
+ double basePower = MOTORS_MIN + (throttle * 800);
+
+ MotorPower motorPower = MotorPower();
+ motorPower.motor1 = basePower;
+ motorPower.motor2 = basePower;
+ motorPower.motor3 = basePower;
+ motorPower.motor4 = basePower;
+
+ //Set motor power
+ setPower(motorPower);
}
-void MotorMixer::setPower(float motor1Power, float motor2Power, float motor3Power, float motor4Power)
+void MotorMixer::setPower(double motor1Power, double motor2Power, double motor3Power, double motor4Power)
{
- _motor1->pulsewidth_us(motor1Power);
- _motor2->pulsewidth_us(motor2Power);
- _motor3->pulsewidth_us(motor3Power);
- _motor4->pulsewidth_us(motor4Power);
+ _motorPower.motor1 = motor1Power;
+ _motorPower.motor2 = motor2Power;
+ _motorPower.motor3 = motor3Power;
+ _motorPower.motor4 = motor4Power;
+
+ #ifdef MOTORS_ENABLED
+ _motor1->pulsewidth_us(motor1Power);
+ _motor2->pulsewidth_us(motor2Power);
+ _motor3->pulsewidth_us(motor3Power);
+ _motor4->pulsewidth_us(motor4Power);
+ #endif
}
-void MotorMixer::setPower(float motorPower)
+void MotorMixer::setPower(double motorPower)
{
setPower(motorPower, motorPower, motorPower, motorPower);
}
+void MotorMixer::setPower(MotorMixer::MotorPower motorPower)
+{
+ setPower(motorPower.motor1, motorPower.motor2, motorPower.motor3, motorPower.motor4);
+}
+
MotorMixer::MotorPower MotorMixer::getMotorPower()
{
return _motorPower;
--- a/FlightController/MotorMixer/MotorMixer.h Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/MotorMixer/MotorMixer.h Wed Apr 01 11:19:21 2015 +0000
@@ -1,6 +1,7 @@
#include "mbed.h"
#include "Global.h"
#include "PidWrapper.h"
+#include "NavigationController.h"
#ifndef MotorMixer_H
#define MotorMixer_H
@@ -8,21 +9,22 @@
class MotorMixer
{
public:
- MotorMixer();
+ MotorMixer(PinName motor1, PinName motor2, PinName motor3, PinName motor4);
~MotorMixer();
struct MotorPower
{
- float motor1;
- float motor2;
- float motor3;
- float motor4;
+ double motor1;
+ double motor2;
+ double motor3;
+ double motor4;
};
- bool initialise(PinName motor1, PinName motor2, PinName motor3, PinName motor4);
- void computePower(PidWrapper::PidOutputs pidOutputs, float throttle);
- void setPower(float motorPower);
- void setPower(float motor1Power, float motor2Power, float motor3Power, float motor4Power);
+ void computePower(PidWrapper::PidOutput pidOutput, double throttle);
+ void computePower(double throttle);
+ void setPower(double motorPower);
+ void setPower(double motor1Power, double motor2Power, double motor3Power, double motor4Power);
+ void setPower(MotorMixer::MotorPower motorPower);
MotorPower getMotorPower();
private:
--- a/FlightController/RateController/RateController.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/RateController/RateController.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -1,39 +1,74 @@
#include "RateController.h"
-RateController::RateController(){}
+RateController::RateController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper) : _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
+{
+ float updateTime = 1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY;
+ _yawRatePidController.initialise(_configFileWrapper.getYawRateParameters(), IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+ _pitchRatePidController.initialise(_configFileWrapper.getPitchRateParameters(), IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+ _rollRatePidController.initialise(_configFileWrapper.getRollRateParameters(), IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+
+ DEBUG("Rate controller initialised\r\n");
+}
RateController::~RateController(){}
-bool RateController::initialise(Sensors& sensors, NavigationController& navigationController)
-{
- _sensors = sensors;
- _navigationController = navigationController;
+PidWrapper::PidOutput RateController::compute()
+{
+ _setPoints = _navigationController.getSetPoint();
+ _rate = _sensors.getRate();
- ConfigFileWrapper configFileWrapper = new ConfigFileWrapper();
- configFileWrapper.initialise();
-
- _yawRatePidController.initialise(configFileWrapper.getYawRateParameters, IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
- _pitchRatePidController.initialise(configFileWrapper.getPitchRateParameters, IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
- _rollRatePidController.initialise(configFileWrapper.getRollRateParameters, IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
+ _pidOutputs.yaw = _yawRatePidController.compute(_setPoints.yaw, _rate.yaw);
+ _pidOutputs.pitch = _pitchRatePidController.compute(_setPoints.pitch, _rate.pitch);
+ _pidOutputs.roll = _rollRatePidController.compute(_setPoints.roll, _rate.roll);
+
+ return _pidOutputs;
+}
- DEBUG("Rate controller initialised");
- return true;
+void RateController::reset()
+{
+ _yawRatePidController.reset();
+ _pitchRatePidController.reset();
+ _rollRatePidController.reset();
+}
+
+
+void RateController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _yawRatePidController.setPidParameters(pidParameters);
+}
+
+void RateController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _pitchRatePidController.setPidParameters(pidParameters);
}
-PidWrapper::PidOutputs RateController::compute()
+void RateController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
{
- //Update rate PID process value with gyro rate
- _yawRatePIDController->setProcessValue(_gyroRate[0]);
- _pitchRatePIDController->setProcessValue(_gyroRate[1]);
- _rollRatePIDController->setProcessValue(_gyroRate[2]);
+ _rollRatePidController.setPidParameters(pidParameters);
+}
+
+PidWrapper::RatePidState RateController::getRatePidState()
+{
+ PidWrapper::RatePidState ratePidState;
+ PidWrapper::PidState yawRatePidState;
+ PidWrapper::PidState pitchRatePidState;
+ PidWrapper::PidState rollRatePidState;
- //Update rate PID set point with desired rate from RC
- _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
- _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
- _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);
+ yawRatePidState.setPoint = _setPoints.yaw;
+ yawRatePidState.processValue = _rate.yaw;
+ yawRatePidState.output = _pidOutputs.yaw;
+
+ pitchRatePidState.setPoint = _setPoints.pitch;
+ pitchRatePidState.processValue = _rate.pitch;
+ pitchRatePidState.output = _pidOutputs.pitch;
- //Compute rate PID outputs
- _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
- _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
- _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
+ rollRatePidState.setPoint = _setPoints.roll;
+ rollRatePidState.processValue = _rate.roll;
+ rollRatePidState.output = _pidOutputs.roll;
+
+ ratePidState.yawRate = yawRatePidState;
+ ratePidState.pitchRate = pitchRatePidState;
+ ratePidState.rollRate = rollRatePidState;
+
+ return ratePidState;
}
\ No newline at end of file
--- a/FlightController/RateController/RateController.h Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/RateController/RateController.h Wed Apr 01 11:19:21 2015 +0000
@@ -2,6 +2,7 @@
#include "Global.h"
#include "PidWrapper.h"
#include "ConfigFileWrapper.h"
+#include "NavigationController.h"
#ifndef RateController_H
#define RateController_H
@@ -9,18 +10,27 @@
class RateController
{
public:
- RateController();
+ RateController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper);
~RateController();
- bool initialise(Sensors& sensors, NavigationController& navigationController);
- PidWrapper::PidOutputs compute();
+ PidWrapper::PidOutput compute();
+ void reset();
+ void setYawRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setPitchRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setRollRatePidParameters(PidWrapper::PidParameter pidParameters);
+ PidWrapper::RatePidState getRatePidState();
private:
- Sensors _sensors;
- NavigationController _navigationController;
- PidWrapper _yawRatePIDController;
- PidWrapper _pitchRatePIDController;
- PidWrapper _rollRatePIDController;
+ Sensors& _sensors;
+ NavigationController& _navigationController;
+ ConfigFileWrapper& _configFileWrapper;
+ PidWrapper _yawRatePidController;
+ PidWrapper _pitchRatePidController;
+ PidWrapper _rollRatePidController;
+ NavigationController::SetPoint _setPoints;
+ Imu::Rate _rate;
+ Imu::Angle _angle;
+ PidWrapper::PidOutput _pidOutputs;
};
#endif
\ No newline at end of file
--- a/FlightController/StabController/StabController.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/StabController/StabController.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,133 @@
+#include "StabController.h"
+
+StabController::StabController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper) : _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
+{
+ float updateTime = 1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY;
+ _yawRatePidController.initialise(_configFileWrapper.getYawRateParameters(), IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+ _pitchRatePidController.initialise(_configFileWrapper.getPitchRateParameters(), IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+ _rollRatePidController.initialise(_configFileWrapper.getRollRateParameters(), IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
+ _yawStabPidController.initialise(_configFileWrapper.getYawStabParameters(), IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX, IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, updateTime);
+ _pitchStabPidController.initialise(_configFileWrapper.getPitchStabParameters(), IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX, IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, updateTime);
+ _rollStabPidController.initialise(_configFileWrapper.getRollStabParameters(), IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX, IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, updateTime);
+
+ DEBUG("Stab controller initialised\r\n");
+}
+
+StabController::~StabController(){}
+
+PidWrapper::PidOutput StabController::compute()
+{
+ _setPoints = _navigationController.getSetPoint();
+ _angle = _sensors.getAngle();
+ _rate = _sensors.getRate();
+
+ //_stabPidOutputs.yaw = _yawStabPidController.compute(_setPoints.yawTarget, _angle.yaw);
+ _stabPidOutputs.yaw = _yawStabPidController.compute(_setPoints.yawDifference, 0);
+ _stabPidOutputs.pitch = _pitchStabPidController.compute(_setPoints.pitch, _angle.pitch);
+ _stabPidOutputs.roll = _rollStabPidController.compute(_setPoints.roll, _angle.roll);
+
+ //If pilot commanding yaw
+ if(abs(_setPoints.yaw) >= 5) _stabPidOutputs.yaw = _setPoints.yaw; //Feed to rate PID (overwriting stab PID output)
+
+ _ratePidOutputs.yaw = _yawRatePidController.compute(_stabPidOutputs.yaw, _rate.yaw);
+ _ratePidOutputs.pitch = _pitchRatePidController.compute(_stabPidOutputs.pitch, _rate.pitch);
+ _ratePidOutputs.roll = _rollRatePidController.compute(_stabPidOutputs.roll, _rate.roll);
+
+ return _ratePidOutputs;
+}
+
+PidWrapper::FlightControllerPidParameters StabController::getPidParameters()
+{
+ PidWrapper::FlightControllerPidParameters allPidParameters;
+ allPidParameters.yawStab = _yawStabPidController.getPidParameters();
+ allPidParameters.pitchStab = _pitchStabPidController.getPidParameters();
+ allPidParameters.rollStab = _rollStabPidController.getPidParameters();
+ allPidParameters.yawRate = _yawRatePidController.getPidParameters();
+ allPidParameters.pitchRate = _pitchRatePidController.getPidParameters();
+ allPidParameters.rollRate = _rollRatePidController.getPidParameters();
+ return allPidParameters;
+}
+
+void StabController::reset()
+{
+ _yawRatePidController.reset();
+ _pitchRatePidController.reset();
+ _rollRatePidController.reset();
+ _yawStabPidController.reset();
+ _pitchStabPidController.reset();
+ _rollStabPidController.reset();
+}
+
+void StabController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _yawStabPidController.setPidParameters(pidParameters);
+}
+
+void StabController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _pitchStabPidController.setPidParameters(pidParameters);
+}
+
+void StabController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _rollStabPidController.setPidParameters(pidParameters);
+}
+
+void StabController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _yawRatePidController.setPidParameters(pidParameters);
+}
+
+void StabController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _pitchRatePidController.setPidParameters(pidParameters);
+}
+
+void StabController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _rollRatePidController.setPidParameters(pidParameters);
+}
+
+PidWrapper::StabPidState StabController::getStabPidState()
+{
+ PidWrapper::StabPidState stabPidState;
+ PidWrapper::PidState yawRatePidState;
+ PidWrapper::PidState pitchRatePidState;
+ PidWrapper::PidState rollRatePidState;
+ PidWrapper::PidState yawStabPidState;
+ PidWrapper::PidState pitchStabPidState;
+ PidWrapper::PidState rollStabPidState;
+
+ yawRatePidState.setPoint = _stabPidOutputs.yaw;
+ yawRatePidState.processValue = _rate.yaw;
+ yawRatePidState.output = _ratePidOutputs.yaw;
+
+ pitchRatePidState.setPoint = _stabPidOutputs.pitch;
+ pitchRatePidState.processValue = _rate.pitch;
+ pitchRatePidState.output = _ratePidOutputs.pitch;
+
+ rollRatePidState.setPoint = _stabPidOutputs.roll;
+ rollRatePidState.processValue = _rate.roll;
+ rollRatePidState.output = _ratePidOutputs.roll;
+
+ yawStabPidState.setPoint = _setPoints.yawDifference;
+ yawStabPidState.processValue = 0;
+ yawStabPidState.output = _stabPidOutputs.yaw;
+
+ pitchStabPidState.setPoint = _setPoints.pitch;
+ pitchStabPidState.processValue = _angle.pitch;
+ pitchStabPidState.output = _stabPidOutputs.pitch;
+
+ rollStabPidState.setPoint = _setPoints.roll;
+ rollStabPidState.processValue = _angle.roll;
+ rollStabPidState.output = _stabPidOutputs.roll;
+
+ stabPidState.yawRate = yawRatePidState;
+ stabPidState.pitchRate = pitchRatePidState;
+ stabPidState.rollRate = rollRatePidState;
+ stabPidState.yawStab = yawStabPidState;
+ stabPidState.pitchStab = pitchStabPidState;
+ stabPidState.rollStab = rollStabPidState;
+
+ return stabPidState;
+}
\ No newline at end of file
--- a/FlightController/StabController/StabController.h Wed Mar 04 18:53:43 2015 +0000
+++ b/FlightController/StabController/StabController.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,44 @@
+#include "mbed.h"
+#include "Global.h"
+#include "PidWrapper.h"
+#include "ConfigFileWrapper.h"
+#include "NavigationController.h"
+
+#ifndef StabController_H
+#define StabController_H
+
+class StabController
+{
+ public:
+ StabController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper);
+ ~StabController();
+
+ PidWrapper::PidOutput compute();
+ PidWrapper::FlightControllerPidParameters getPidParameters();
+ void reset();
+ void setYawStabPidParameters(PidWrapper::PidParameter pidParameters);
+ void setPitchStabPidParameters(PidWrapper::PidParameter pidParameters);
+ void setRollStabPidParameters(PidWrapper::PidParameter pidParameters);
+ void setYawRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setPitchRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setRollRatePidParameters(PidWrapper::PidParameter pidParameters);
+ PidWrapper::StabPidState getStabPidState();
+
+ private:
+ Sensors& _sensors;
+ NavigationController& _navigationController;
+ ConfigFileWrapper& _configFileWrapper;
+ PidWrapper _yawRatePidController;
+ PidWrapper _pitchRatePidController;
+ PidWrapper _rollRatePidController;
+ PidWrapper _yawStabPidController;
+ PidWrapper _pitchStabPidController;
+ PidWrapper _rollStabPidController;
+ NavigationController::SetPoint _setPoints;
+ Imu::Rate _rate;
+ Imu::Angle _angle;
+ PidWrapper::PidOutput _stabPidOutputs;
+ PidWrapper::PidOutput _ratePidOutputs;
+};
+
+#endif
\ No newline at end of file
--- a/Global.h Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,56 +0,0 @@ -#include "mbed.h" - -#ifndef GLOBAL_H -#define GLOBAL_H - -#if 1 - #define DEBUG(a) printf(a) -#else - #define DEBUG(a) (void)0 -#endif - -#define IMU_YAW_ANGLE_MAX 180 -#define IMU_YAW_ANGLE_MIN -180 -#define IMU_ROLL_ANGLE_MAX 90 -#define IMU_ROLL_ANGLE_MIN -90 -#define IMU_PITCH_ANGLE_MAX 90 -#define IMU_PITCH_ANGLE_MIN -90 -#define IMU_YAW_RATE_MAX 360 -#define IMU_YAW_RATE_MIN -360 -#define IMU_ROLL_RATE_MAX 360 -#define IMU_ROLL_RATE_MIN -360 -#define IMU_PITCH_RATE_MAX 360 -#define IMU_PITCH_RATE_MIN -360 - -#define RC_CHANNELS 8 -#define RC_THROTTLE_CHANNEL 3 -#define RC_IN_MAX 1900 -#define RC_IN_MIN 1000 -#define RC_OUT_MAX 1 -#define RC_OUT_MIN 0 -#define RC_YAW_RATE_MAX 180 -#define RC_YAW_RATE_MIN -180 -#define RC_ROLL_RATE_MAX 90 -#define RC_ROLL_RATE_MIN -90 -#define RC_PITCH_RATE_MAX 90 -#define RC_PITCH_RATE_MIN -90 -#define RC_ROLL_ANGLE_MAX 20 -#define RC_ROLL_ANGLE_MIN -20 -#define RC_PITCH_ANGLE_MAX 20 -#define RC_PITCH_ANGLE_MIN -20 -#define RC_THRUST_MAX 1 -#define RC_THRUST_MIN 0 -#define RC_VELOCITY_MAX 2.5 -#define RC_VELOCITY_MIN 2.5 - -#define MOTORS_OFF 0 -#define MOTORS_ARMED 1000 -#define MOTORS_MIN 1060 -#define MOTORS_MAX 1860 - -#define RATE_PID_CONTROLLER_OUTPUT_MAX 100 -#define RATE_PID_CONTROLLER_OUTPUT_MIN -100 - -#define FLIGHT_CONTROLLER_FREQUENCY 500 - -#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Global/ConfigFileWrapper/ConfigFile.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shintamainjp/code/ConfigFile/#f6ceafabe9f8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Global/ConfigFileWrapper/ConfigFileWrapper.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,346 @@
+#include "ConfigFileWrapper.h"
+
+ConfigFileWrapper::ConfigFileWrapper()
+{
+ _str = new char[1024];
+ loadSettings();
+
+ DEBUG("Config file wrapper initialised\r\n");
+}
+
+ConfigFileWrapper::~ConfigFileWrapper(){}
+
+PidWrapper::PidParameter ConfigFileWrapper::getYawRateParameters()
+{
+ return _yawRateParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getPitchRateParameters()
+{
+ return _pitchRateParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getRollRateParameters()
+{
+ return _rollRateParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getYawStabParameters()
+{
+ return _yawStabParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getPitchStabParameters()
+{
+ return _pitchStabParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getRollStabParameters()
+{
+ return _rollStabParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getAltitudeRateParameters()
+{
+ return _altitudeRateParameters;
+}
+
+PidWrapper::PidParameter ConfigFileWrapper::getAltitudeStabParameters()
+{
+ return _altitudeStabParameters;
+}
+
+bool ConfigFileWrapper::setYawRateParameters(PidWrapper::PidParameter pidParameters)
+{
+ _yawRateParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setPitchRateParameters(PidWrapper::PidParameter pidParameters)
+{
+ _pitchRateParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setRollRateParameters(PidWrapper::PidParameter pidParameters)
+{
+ _rollRateParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setYawStabParameters(PidWrapper::PidParameter pidParameters)
+{
+ _yawStabParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setPitchStabParameters(PidWrapper::PidParameter pidParameters)
+{
+ _pitchStabParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setRollStabParameters(PidWrapper::PidParameter pidParameters)
+{
+ _rollStabParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setAltitudeRateParameters(PidWrapper::PidParameter pidParameters)
+{
+ _altitudeRateParameters = pidParameters;
+ return true;
+}
+
+bool ConfigFileWrapper::setAltitudeStabParameters(PidWrapper::PidParameter pidParameters)
+{
+ _altitudeStabParameters = pidParameters;
+ return true;
+}
+
+float ConfigFileWrapper::getAccelZeroPitch()
+{
+ return _accelZeroPitch;
+}
+
+float ConfigFileWrapper::getAccelZeroRoll()
+{
+ return _accelZeroRoll;
+}
+
+bool ConfigFileWrapper::setAccelZeroPitch(float value)
+{
+ _accelZeroPitch = value;
+ return true;
+}
+
+bool ConfigFileWrapper::setAccelZeroRoll(float value)
+{
+ _accelZeroRoll = value;
+ return true;
+}
+
+void ConfigFileWrapper::convertToCharArray(double number)
+{
+ sprintf(_str, "%1.8f", number );
+}
+
+void ConfigFileWrapper::convertToCharArray(int number)
+{
+ sprintf(_str, "%d", number );
+}
+
+void ConfigFileWrapper::loadSettings()
+{
+ char value[BUFSIZ];
+
+ DEBUG("Loading settings from config file\n\r");
+
+ //Read a configuration file from a mbed.
+ LocalFileSystem local("local");
+ if (!_configFile.read("/local/CONFIG.CFG"))
+ {
+ DEBUG("Config file does not exist\n\r");
+ return;
+ }
+ else
+ {
+ //Get values
+ if (_configFile.getValue("yawRatePIDControllerP", &value[0], sizeof(value))) _yawRateParameters.p = atof(value);
+ else DEBUG("Failed to get value for yawRatePIDControllerP\n\r");
+
+ if (_configFile.getValue("yawRatePIDControllerI", &value[0], sizeof(value))) _yawRateParameters.i = atof(value);
+ else DEBUG("Failed to get value for yawRatePIDControllerI\n\r");
+
+ if (_configFile.getValue("yawRatePIDControllerD", &value[0], sizeof(value))) _yawRateParameters.d = atof(value);
+ else DEBUG("Failed to get value for yawRatePIDControllerD\n\r");
+
+ if (_configFile.getValue("pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRateParameters.p = atof(value);
+ else DEBUG("Failed to get value for pitchRatePIDControllerP\n\r");
+
+ if (_configFile.getValue("pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRateParameters.i = atof(value);
+ else DEBUG("Failed to get value for pitchRatePIDControllerI\n\r");
+
+ if (_configFile.getValue("pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRateParameters.d = atof(value);
+ else DEBUG("Failed to get value for pitchRatePIDControllerD\n\r");
+
+ if (_configFile.getValue("rollRatePIDControllerP", &value[0], sizeof(value))) _rollRateParameters.p = atof(value);
+ else DEBUG("Failed to get value for rollRatePIDControllerP\n\r");
+
+ if (_configFile.getValue("rollRatePIDControllerI", &value[0], sizeof(value))) _rollRateParameters.i = atof(value);
+ else DEBUG("Failed to get value for rollRatePIDControllerI\n\r");
+
+ if (_configFile.getValue("rollRatePIDControllerD", &value[0], sizeof(value))) _rollRateParameters.d = atof(value);
+ else DEBUG("Failed to get value for rollRatePIDControllerD\n\r");
+
+ if (_configFile.getValue("yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabParameters.p = atof(value);
+ else DEBUG("Failed to get value for yawStabPIDControllerP\n\r");
+
+ if (_configFile.getValue("yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabParameters.i = atof(value);
+ else DEBUG("Failed to get value for yawStabPIDControllerI\n\r");
+
+ if (_configFile.getValue("yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabParameters.d = atof(value);
+ else DEBUG("Failed to get value for yawStabPIDControllerD\n\r");
+
+ if (_configFile.getValue("pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabParameters.p = atof(value);
+ else DEBUG("Failed to get value for pitchStabPIDControllerP\n\r");
+
+ if (_configFile.getValue("pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabParameters.i = atof(value);
+ else DEBUG("Failed to get value for pitchStabPIDControllerI\n\r");
+
+ if (_configFile.getValue("pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabParameters.d = atof(value);
+ else DEBUG("Failed to get value for pitchStabPIDControllerD\n\r");
+
+ if (_configFile.getValue("rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabParameters.p = atof(value);
+ else DEBUG("Failed to get value for rollStabPIDControllerP\n\r");
+
+ if (_configFile.getValue("rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabParameters.i = atof(value);
+ else DEBUG("Failed to get value for rollStabPIDControllerI\n\r");
+
+ if (_configFile.getValue("rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabParameters.d = atof(value);
+ else DEBUG("Failed to get value for rollStabPIDControllerD\n\r");
+
+ if (_configFile.getValue("altitudeRatePIDControllerP", &value[0], sizeof(value))) _altitudeRateParameters.p = atof(value);
+ else DEBUG("Failed to get value for altitudeRatePIDControllerP\n\r");
+
+ if (_configFile.getValue("altitudeRatePIDControllerI", &value[0], sizeof(value))) _altitudeRateParameters.i = atof(value);
+ else DEBUG("Failed to get value for altitudeRatePIDControllerI\n\r");
+
+ if (_configFile.getValue("altitudeRatePIDControllerD", &value[0], sizeof(value))) _altitudeRateParameters.d = atof(value);
+ else DEBUG("Failed to get value for altitudeRatePIDControllerD\n\r");
+
+ if (_configFile.getValue("altitudeStabPIDControllerP", &value[0], sizeof(value))) _altitudeStabParameters.p = atof(value);
+ else DEBUG("Failed to get value for altitudeStabPIDControllerP\n\r");
+
+ if (_configFile.getValue("altitudeStabPIDControllerI", &value[0], sizeof(value))) _altitudeStabParameters.i = atof(value);
+ else DEBUG("Failed to get value for altitudeStabPIDControllerI\n\r");
+
+ if (_configFile.getValue("altitudeStabPIDControllerD", &value[0], sizeof(value))) _altitudeStabParameters.d = atof(value);
+ else DEBUG("Failed to get value for altitudeStabPIDControllerD\n\r");
+
+ if (_configFile.getValue("accelZeroPitch", &value[0], sizeof(value))) _accelZeroPitch = atof(value);
+ else DEBUG("Failed to get value for accelZeroPitch\n\r");
+
+ if (_configFile.getValue("accelZeroRoll", &value[0], sizeof(value))) _accelZeroRoll = atof(value);
+ else DEBUG("Failed to get value for accelZeroRoll\n\r");
+
+ /*if (_configFile.getValue("zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
+ else DEBUG("Failed to get value for zero pitch\n\r");
+
+ if (_configFile.getValue("zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
+ else printf("Failed to get value for zero roll\n\r");*/
+ }
+
+ DEBUG("Finished loading settings from config file\n\r");
+}
+
+bool ConfigFileWrapper::saveSettings()
+{
+ DEBUG("Writing settings to config file\n\r");
+
+ LocalFileSystem local("local");
+ if (!_configFile.read("/local/CONFIG.CFG"))
+ {
+ DEBUG("Config file does not exist\n\r");
+ return false;
+ }
+ else
+ {
+ //Write values
+ convertToCharArray(_yawRateParameters.p);
+ if (!_configFile.setValue("yawRatePIDControllerP", _str)) DEBUG("Failed to write value for yawRatePIDControllerP\n\r");
+
+ convertToCharArray(_yawRateParameters.i);
+ if (!_configFile.setValue("yawRatePIDControllerI", _str)) DEBUG("Failed to write value for yawRatePIDControllerI\n\r");
+
+ convertToCharArray(_yawRateParameters.d);
+ if (!_configFile.setValue("yawRatePIDControllerD", _str)) DEBUG("Failed to write value for yawRatePIDControllerD\n\r");
+
+ convertToCharArray(_pitchRateParameters.p);
+ if (!_configFile.setValue("pitchRatePIDControllerP", _str)) DEBUG("Failed to write value for pitchRatePIDControllerP\n\r");
+
+ convertToCharArray(_pitchRateParameters.i);
+ if (!_configFile.setValue("pitchRatePIDControllerI", _str)) DEBUG("Failed to write value for pitchRatePIDControllerI\n\r");
+
+ convertToCharArray(_pitchRateParameters.d);
+ if (!_configFile.setValue("pitchRatePIDControllerD", _str)) DEBUG("Failed to write value for pitchRatePIDControllerD\n\r");
+
+ convertToCharArray(_rollRateParameters.p);
+ if (!_configFile.setValue("rollRatePIDControllerP", _str)) DEBUG("Failed to write value for rollRatePIDControllerP\n\r");
+
+ convertToCharArray(_rollRateParameters.i);
+ if (!_configFile.setValue("rollRatePIDControllerI", _str)) DEBUG("Failed to write value for rollRatePIDControllerI\n\r");
+
+ convertToCharArray(_rollRateParameters.d);
+ if (!_configFile.setValue("rollRatePIDControllerD", _str)) DEBUG("Failed to write value for rollRatePIDControllerD\n\r");
+
+ convertToCharArray(_yawStabParameters.p);
+ if (!_configFile.setValue("yawStabPIDControllerP", _str)) DEBUG("Failed to write value for yawStabPIDControllerP\n\r");
+
+ convertToCharArray(_yawStabParameters.i);
+ if (!_configFile.setValue("yawStabPIDControllerI", _str)) DEBUG("Failed to write value for yawStabPIDControllerI\n\r");
+
+ convertToCharArray(_yawStabParameters.d);
+ if (!_configFile.setValue("yawStabPIDControllerD", _str)) DEBUG("Failed to write value for yawStabPIDControllerD\n\r");
+
+ convertToCharArray(_pitchStabParameters.p);
+ if (!_configFile.setValue("pitchStabPIDControllerP", _str)) DEBUG("Failed to write value for pitchStabPIDControllerP\n\r");
+
+ convertToCharArray(_pitchStabParameters.i);
+ if (!_configFile.setValue("pitchStabPIDControllerI", _str)) DEBUG("Failed to write value for pitchStabPIDControllerI\n\r");
+
+ convertToCharArray(_pitchStabParameters.d);
+ if (!_configFile.setValue("pitchStabPIDControllerD", _str)) DEBUG("Failed to write value for pitchStabPIDControllerD\n\r");
+
+ convertToCharArray(_rollStabParameters.p);
+ if (!_configFile.setValue("rollStabPIDControllerP", _str)) DEBUG("Failed to write value for rollStabPIDControllerP\n\r");
+
+ convertToCharArray(_rollStabParameters.i);
+ if (!_configFile.setValue("rollStabPIDControllerI", _str)) DEBUG("Failed to write value for _rollStabPIDControllerI\n\r");
+
+ convertToCharArray(_rollStabParameters.d);
+ if (!_configFile.setValue("rollStabPIDControllerD", _str)) DEBUG("Failed to write value for rollStabPIDControllerD\n\r");
+
+ convertToCharArray(_altitudeRateParameters.p);
+ if (!_configFile.setValue("altitudeRatePIDControllerP", _str)) DEBUG("Failed to write value for altitudeRatePIDControllerP\n\r");
+
+ convertToCharArray(_altitudeRateParameters.i);
+ if (!_configFile.setValue("altitudeRatePIDControllerI", _str)) DEBUG("Failed to write value for altitudeRatePIDControllerI\n\r");
+
+ convertToCharArray(_altitudeRateParameters.d);
+ if (!_configFile.setValue("altitudeRatePIDControllerD", _str)) DEBUG("Failed to write value for altitudeRatePIDControllerD\n\r");
+
+ convertToCharArray(_altitudeStabParameters.p);
+ if (!_configFile.setValue("altitudeStabPIDControllerP", _str)) DEBUG("Failed to write value for altitudeStabPIDControllerP\n\r");
+
+ convertToCharArray(_altitudeStabParameters.i);
+ if (!_configFile.setValue("altitudeStabPIDControllerI", _str)) DEBUG("Failed to write value for altitudeStabPIDControllerI\n\r");
+
+ convertToCharArray(_altitudeStabParameters.d);
+ if (!_configFile.setValue("altitudeStabPIDControllerD", _str)) DEBUG("Failed to write value for altitudeStabPIDControllerD\n\r");
+
+ convertToCharArray(_accelZeroPitch);
+ if (!_configFile.setValue("accelZeroPitch", _str)) DEBUG("Failed to write value for accelZeroPitch\n\r");
+
+ convertToCharArray(_accelZeroRoll);
+ if (!_configFile.setValue("accelZeroRoll", _str)) DEBUG("Failed to write value for accelZeroRoll\n\r");
+
+
+ //convertToCharArray(_zeroValues[1]);
+ //if (!_configFile.setValue("_zeroPitch", _str)) DEBUG("Failed to write value for zero pitch\n\r");
+
+ //convertToCharArray(_zeroValues[2]);
+ //if (!_configFile.setValue("_zeroRoll", _str)) DEBUG("Failed to write value for zero roll\n\r");
+
+ if (!_configFile.write("/local/CONFIG.CFG"))
+ {
+ DEBUG("Failure to write settings to configuration file.\n\r");
+ return false;
+ }
+ else
+ {
+ DEBUG("Successfully wrote settings to configuration file.\n\r");
+ return true;
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Global/ConfigFileWrapper/ConfigFileWrapper.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,58 @@
+#include "mbed.h"
+#include "Global.h"
+#include "ConfigFile.h"
+#include "PidWrapper.h"
+
+#ifndef ConfigFileWrapper_H
+#define ConfigFileWrapper_H
+
+class ConfigFileWrapper // begin declaration of the class
+{
+ public: // begin public section
+ ConfigFileWrapper(); // constructor
+ ~ConfigFileWrapper();
+
+ PidWrapper::PidParameter getYawRateParameters();
+ PidWrapper::PidParameter getPitchRateParameters();
+ PidWrapper::PidParameter getRollRateParameters();
+ PidWrapper::PidParameter getYawStabParameters();
+ PidWrapper::PidParameter getPitchStabParameters();
+ PidWrapper::PidParameter getRollStabParameters();
+ PidWrapper::PidParameter getAltitudeRateParameters();
+ PidWrapper::PidParameter getAltitudeStabParameters();
+
+ bool setYawRateParameters(PidWrapper::PidParameter pidParameters);
+ bool setPitchRateParameters(PidWrapper::PidParameter pidParameters);
+ bool setRollRateParameters(PidWrapper::PidParameter pidParameters);
+ bool setYawStabParameters(PidWrapper::PidParameter pidParameters);
+ bool setPitchStabParameters(PidWrapper::PidParameter pidParameters);
+ bool setRollStabParameters(PidWrapper::PidParameter pidParameters);
+ bool setAltitudeRateParameters(PidWrapper::PidParameter pidParameters);
+ bool setAltitudeStabParameters(PidWrapper::PidParameter pidParameters);
+
+ float getAccelZeroPitch();
+ float getAccelZeroRoll();
+ bool setAccelZeroPitch(float value);
+ bool setAccelZeroRoll(float value);
+
+ bool saveSettings();
+
+ private:
+ ConfigFile _configFile;
+ char* _str;
+ PidWrapper::PidParameter _yawRateParameters;
+ PidWrapper::PidParameter _pitchRateParameters;
+ PidWrapper::PidParameter _rollRateParameters;
+ PidWrapper::PidParameter _yawStabParameters;
+ PidWrapper::PidParameter _pitchStabParameters;
+ PidWrapper::PidParameter _rollStabParameters;
+ PidWrapper::PidParameter _altitudeRateParameters;
+ PidWrapper::PidParameter _altitudeStabParameters;
+ float _accelZeroPitch;
+ float _accelZeroRoll;
+ void convertToCharArray(double number);
+ void convertToCharArray(int number);
+ void loadSettings();
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Global/Global.h Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,64 @@ +#include "mbed.h" + +#ifndef GLOBAL_H +#define GLOBAL_H + +#if 1 + #define DEBUG(...) printf(__VA_ARGS__) +#else + #define DEBUG(a) (void)0 +#endif + +#define MOTORS_ENABLED + +#define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a)) + +#define IMU_YAW_ANGLE_MAX 180 +#define IMU_YAW_ANGLE_MIN -180 +#define IMU_ROLL_ANGLE_MAX 90 +#define IMU_ROLL_ANGLE_MIN -90 +#define IMU_PITCH_ANGLE_MAX 90 +#define IMU_PITCH_ANGLE_MIN -90 +#define IMU_YAW_RATE_MAX 360 +#define IMU_YAW_RATE_MIN -360 +#define IMU_ROLL_RATE_MAX 360 +#define IMU_ROLL_RATE_MIN -360 +#define IMU_PITCH_RATE_MAX 360 +#define IMU_PITCH_RATE_MIN -360 + +#define RC_CHANNELS 8 +#define RC_THROTTLE_CHANNEL 3 +#define RC_IN_MAX 1900 +#define RC_IN_MIN 1000 +#define RC_OUT_MAX 1 +#define RC_OUT_MIN 0 +#define RC_YAW_RATE_MAX 180 +#define RC_YAW_RATE_MIN -180 +#define RC_ROLL_RATE_MAX 90 +#define RC_ROLL_RATE_MIN -90 +#define RC_PITCH_RATE_MAX 90 +#define RC_PITCH_RATE_MIN -90 +#define RC_ROLL_ANGLE_MAX 45 +#define RC_ROLL_ANGLE_MIN -45 +#define RC_PITCH_ANGLE_MAX 45 +#define RC_PITCH_ANGLE_MIN -45 +#define RC_THRUST_MAX 1 +#define RC_THRUST_MIN 0 +#define RC_DEAD_ZONE 0.1 + +#define MOTORS_OFF 0 +#define MOTORS_ARMED 1060 +#define MOTORS_MIN 1060 +#define MOTORS_MAX 1860 + +#define RATE_PID_CONTROLLER_OUTPUT_MAX 100 +#define RATE_PID_CONTROLLER_OUTPUT_MIN -100 + +#define ALTITUDE_MIN 0 +#define ALTITUDE_MAX 1000 +#define MAX_CLIMB_RATE 25 +#define MIN_CLIMB_RATE -25 + +#define FLIGHT_CONTROLLER_FREQUENCY 500 + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Global/Kalman/Kalman.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,31 @@
+#include "Kalman.h"
+
+Kalman::Kalman(double q, double r, double p, double intialValue)
+{
+ _kalmanState = KalmanState();
+ _kalmanState.q = q;
+ _kalmanState.r = r;
+ _kalmanState.p = p;
+ _kalmanState.x = intialValue;
+}
+
+Kalman::~Kalman(){}
+
+double Kalman::update(double predicted, double measurement)
+{
+ //prediction
+ _kalmanState.x = predicted * _kalmanState.x;
+ _kalmanState.p = _kalmanState.p + _kalmanState.q;
+
+ //measurement
+ _kalmanState.k = _kalmanState.p / (_kalmanState.p + _kalmanState.r);
+ _kalmanState.x = _kalmanState.x + _kalmanState.k * (measurement - _kalmanState.x);
+ _kalmanState.p = (1 - _kalmanState.k) * _kalmanState.p;
+
+ return _kalmanState.x;
+}
+
+double Kalman::getEstimated()
+{
+ return _kalmanState.x;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Global/Kalman/Kalman.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,29 @@
+#include "mbed.h"
+#include "Global.h"
+
+#ifndef Kalman_H
+#define Kalman_H
+
+class Kalman
+{
+ public:
+ Kalman(double q, double r, double p, double intialValue);
+ ~Kalman();
+
+ struct KalmanState
+ {
+ double q; //process noise covariance
+ double r; //measurement noise covariance
+ double x; //value
+ double p; //estimation error covariance
+ double k; //kalman gain
+ };
+
+ double update(double predicted, double measurement);
+ double getEstimated();
+
+ private:
+ KalmanState _kalmanState;
+
+};
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Global/MODSERIAL.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Global/PidWrapper/PID.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Global/PidWrapper/PidWrapper.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,51 @@
+#include "PidWrapper.h"
+
+PidWrapper::PidWrapper(){}
+
+PidWrapper::~PidWrapper(){}
+
+bool PidWrapper::initialise(PidParameter pidParameter, double inputMin, double inputMax, double outputMin, double outputMax, float updateTime)
+{
+ _pid = new PID(pidParameter.p, pidParameter.i, pidParameter.d, updateTime);
+ _pid->setInputLimits(inputMin, inputMax);
+ _pid->setOutputLimits(outputMin, outputMax);
+ _pid->setMode(AUTO_MODE);
+ _pid->setSetPoint(0.0);
+ _pid->setBias(0);
+
+ DEBUG("PID wrapper initialised\r\n");
+ return true;
+}
+
+double PidWrapper::compute(double setPoint, double processValue)
+{
+ _pid->setSetPoint(setPoint);
+ _pid->setProcessValue(processValue);
+ return _pid->compute();
+}
+
+PidWrapper::PidParameter PidWrapper::getPidParameters()
+{
+ PidWrapper::PidParameter pidParameters;
+ pidParameters.p = _pid->getPParam();
+ pidParameters.i = _pid->getIParam();
+ pidParameters.d = _pid->getDParam();
+
+ return pidParameters;
+}
+
+void PidWrapper::reset()
+{
+ _pid->reset();
+}
+
+void PidWrapper::setPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _pid->setTunings(pidParameters.p, pidParameters.i, pidParameters.d);
+ DEBUG("P %1.8f, I %1.8f, D %1.8f\r\n", pidParameters.p, pidParameters.i, pidParameters.d);
+}
+
+void PidWrapper::setBias(float bias)
+{
+ _pid->setBias(bias);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Global/PidWrapper/PidWrapper.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "Global.h"
+#include "PID.h"
+
+#ifndef PidWrapper_H
+#define PidWrapper_H
+
+class PidWrapper // begin declaration of the class
+{
+ public: // begin public section
+ PidWrapper(); // constructor
+ ~PidWrapper();
+
+ struct PidOutput
+ {
+ double yaw;
+ double pitch;
+ double roll;
+ };
+
+ struct PidParameter
+ {
+ double p;
+ double i;
+ double d;
+ };
+
+ struct PidState
+ {
+ double setPoint;
+ double processValue;
+ double output;
+ };
+
+ struct FlightControllerPidParameters
+ {
+ PidParameter yawRate;
+ PidParameter pitchRate;
+ PidParameter rollRate;
+ PidParameter yawStab;
+ PidParameter pitchStab;
+ PidParameter rollStab;
+ };
+
+ struct NavigationControllerPidParameters
+ {
+ PidParameter altitudeRate;
+ PidParameter altitudeStab;
+ };
+
+ struct RatePidState
+ {
+ PidState yawRate;
+ PidState pitchRate;
+ PidState rollRate;
+ };
+
+ struct StabPidState
+ {
+ PidState yawRate;
+ PidState pitchRate;
+ PidState rollRate;
+ PidState yawStab;
+ PidState pitchStab;
+ PidState rollStab;
+ };
+
+ bool initialise(PidParameter pidParameter, double inputMin, double inputMax, double outputMin, double outputMax, float updateTime);
+ double compute(double setPoint, double processValue);
+ PidWrapper::PidParameter getPidParameters();
+ void reset();
+ void setPidParameters(PidWrapper::PidParameter pidParameters);
+ void setBias(float bias);
+
+ private:
+ PID* _pid;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Global/filter.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/networker/code/filter/#9ce370b360ba
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Global/mbed-rtos.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#721c3a93a7b8
--- a/MODSERIAL.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/NavigationController/AltitudeController/AltitudeController.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,55 @@
+#include "AltitudeController.h"
+
+AltitudeController::AltitudeController(Sensors& sensors, ConfigFileWrapper& configFileWrapper, Status& status) :
+_sensors(sensors), _configFileWrapper(configFileWrapper), _status(status)
+{
+ double updateTime = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.0));
+ _altitudeRatePidController.initialise(_configFileWrapper.getAltitudeRateParameters(), MIN_CLIMB_RATE, MAX_CLIMB_RATE, (RC_THRUST_MIN + 0.2), (RC_THRUST_MAX - 0.2), updateTime);
+ _altitudeRatePidController.setBias(0.6); // Hover throttle
+ _altitudeStabPidController.initialise(_configFileWrapper.getAltitudeStabParameters(), ALTITUDE_MIN, ALTITUDE_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE, updateTime );
+
+ //_setPoints = new NavigationController::SetPoint();
+
+ DEBUG("Altitude controller initialised\r\n");
+}
+
+AltitudeController::~AltitudeController(){}
+
+double AltitudeController::compute(double targetAltitude, double climbRate)
+{
+ _altitude = _sensors.getAltitude();
+ float velocity = _sensors.getZVelocity();
+
+ float altitudeStabPidOutput = _altitudeStabPidController.compute(targetAltitude, _altitude.computed);
+
+ //If pilot commanding climb rate
+ if(_status.getDeadZone() == false) altitudeStabPidOutput = climbRate; //Feed to rate PID (overwriting stab PID output)
+
+ float altitudeRatePidOutput = _altitudeRatePidController.compute(altitudeStabPidOutput, velocity);
+
+ return altitudeRatePidOutput;
+}
+
+void AltitudeController::reset()
+{
+ _altitudeRatePidController.reset();
+ _altitudeStabPidController.reset();
+}
+
+PidWrapper::NavigationControllerPidParameters AltitudeController::getPidParameters()
+{
+ PidWrapper::NavigationControllerPidParameters allPidParameters;
+ allPidParameters.altitudeRate = _altitudeRatePidController.getPidParameters();
+ allPidParameters.altitudeStab = _altitudeStabPidController.getPidParameters();
+ return allPidParameters;
+}
+
+void AltitudeController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _altitudeRatePidController.setPidParameters(pidParameters);
+}
+
+void AltitudeController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _altitudeStabPidController.setPidParameters(pidParameters);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/NavigationController/AltitudeController/AltitudeController.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,32 @@
+#ifndef AltitudeController_H
+#define AltitudeController_H
+
+#include "mbed.h"
+#include "Global.h"
+#include "PidWrapper.h"
+#include "Sensors.h"
+#include "ConfigFileWrapper.h"
+#include "Status.h"
+
+class AltitudeController
+{
+ public:
+ AltitudeController(Sensors& sensors, ConfigFileWrapper& configFileWrapper, Status& status);
+ ~AltitudeController();
+
+ double compute(double targetAltitude, double climbRate);
+ void reset();
+ void setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters);
+ PidWrapper::NavigationControllerPidParameters getPidParameters();
+
+ private:
+ Sensors& _sensors;
+ ConfigFileWrapper& _configFileWrapper;
+ Status& _status;
+ Sensors::Altitude _altitude;
+ PidWrapper _altitudeRatePidController;
+ PidWrapper _altitudeStabPidController;
+};
+
+#endif
\ No newline at end of file
--- a/NavigationController/NavigationController.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/NavigationController/NavigationController.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,189 @@
+#include "NavigationController.h"
+
+NavigationController::NavigationController(Status& status, Sensors& sensors, Rc& rc, ConfigFileWrapper& configFileWrapper) :
+_status(status), _sensors(sensors), _rc(rc), _configFileWrapper(configFileWrapper)
+{
+ _altitudeController = new AltitudeController(_sensors, _configFileWrapper, _status);
+ _altitudeHoldPidOutput = 0;
+
+ _thread = new Thread(&NavigationController::threadStarter, this, osPriorityHigh);
+
+ DEBUG("Navigation controller initialised\r\n");
+}
+
+NavigationController::~NavigationController(){}
+
+void NavigationController::threadStarter(void const *p)
+{
+ NavigationController *instance = (NavigationController*)p;
+ instance->threadWorker();
+}
+
+void NavigationController::threadWorker()
+{
+ while(true)
+ {
+ //Get latest sensor readings
+ _sensors.update();
+
+ //Get state
+ _state = _status.getState();
+
+ //Get navigation mode
+ _navigationMode = _status.getNavigationMode();
+
+ //Get Rc commands
+ _mappedRc = _rc.getMappedRc();
+
+ //Get angle
+ _angle = _sensors.getAngle();
+
+ //Reset accel data if not flying
+ if(_state == Status::PREFLIGHT || _state == Status::STANDBY)
+ {
+ //Reset accel
+ _sensors.zeroAccel();
+ }
+
+ //Update yaw target
+ if(abs(_mappedRc.yaw) >= 5 || _state == Status::PREFLIGHT || _state == Status::STANDBY) updateYawTarget();
+
+ //Make sure we are initialised
+ if(_state != Status::PREFLIGHT)
+ {
+ //Update yaw difference
+ _setPoints.yawDifference = MOD(_setPoints.yawTarget - _angle.yaw);
+
+ if(_navigationMode == Status::NONE)
+ {
+ //Motors are directly controlled by rc remote
+ if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
+ else _status.setMotorsSpinning(false);
+
+ //Update target altitude
+ updateAltitudeTarget();
+ }
+ else if(_navigationMode == Status::ALTITUDE_HOLD)
+ {
+ //Check if throttle is in dead zone
+ if(_status.getDeadZone() == true) _setPoints.climbRate = 0;
+ else
+ {
+ //Throttle not in dead zone so map to climb rate
+ _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE);
+
+ //Update target altitude
+ updateAltitudeTarget();
+ }
+
+ //Get PID output
+ _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
+ }
+
+ if(_state == Status::STANDBY)
+ {
+ _setPoints.yaw = 0;
+ _setPoints.pitch = 0;
+ _setPoints.roll = 0;
+ _setPoints.throttle = 0;
+
+ _altitudeController->reset();
+ }
+ else if(_state == Status::GROUND_READY)
+ {
+ _setPoints.yaw = _mappedRc.yaw;
+ _setPoints.pitch = _mappedRc.pitch;
+ _setPoints.roll = _mappedRc.roll;
+ _setPoints.throttle = _mappedRc.throttle;
+
+ _altitudeController->reset();
+ }
+ else if(_state == Status::MANUAL)
+ {
+ _setPoints.yaw = _mappedRc.yaw;
+ _setPoints.pitch = _mappedRc.pitch;
+ _setPoints.roll = _mappedRc.roll;
+ _setPoints.throttle = _mappedRc.throttle;
+
+ _altitudeController->reset();
+ }
+ else if(_state == Status::STABILISED)
+ {
+ _setPoints.yaw = _mappedRc.yaw;
+ _setPoints.pitch = _mappedRc.pitch;
+ _setPoints.roll = _mappedRc.roll;
+ _setPoints.throttle = _altitudeHoldPidOutput;
+ }
+ else if(_state == Status::AUTO)
+ {
+ //Waypoint navigation
+ }
+ }
+ //Not initialised
+ else
+ {
+ _setPoints.yaw = 0;
+ _setPoints.pitch = 0;
+ _setPoints.roll = 0;
+ _setPoints.throttle = 0;
+
+ _altitudeController->reset();
+ }
+
+ Thread::wait(20);
+ }
+}
+
+NavigationController::SetPoint NavigationController::getSetPoint()
+{
+ return _setPoints;
+}
+
+void NavigationController::updateYawTarget()
+{
+ _setPoints.yawTarget = _angle.yaw;
+}
+
+void NavigationController::updateAltitudeTarget()
+{
+ _altitude = _sensors.getAltitude();
+ _setPoints.targetAltitude = _altitude.computed;
+
+ if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN;
+ else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX;
+}
+
+double NavigationController::map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
+{
+ return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
+}
+
+PidWrapper::NavigationControllerPidParameters NavigationController::getPidParameters()
+{
+ return _altitudeController->getPidParameters();
+}
+
+void NavigationController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _altitudeController->setAltitudeRatePidParameters(pidParameters);
+ _configFileWrapper.setAltitudeRateParameters(pidParameters);
+ saveSettings();
+}
+
+void NavigationController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
+{
+ _altitudeController->setAltitudeStabPidParameters(pidParameters);
+ _configFileWrapper.setAltitudeStabParameters(pidParameters);
+ saveSettings();
+}
+
+void NavigationController::saveSettings()
+{
+ Status::State state = _status.getState();
+ if(state == Status::STANDBY || state == Status::PREFLIGHT)
+ {
+ _sensors.enable(false);
+ _configFileWrapper.saveSettings();
+ _sensors.enable(true);
+ }
+}
\ No newline at end of file
--- a/NavigationController/NavigationController.h Wed Mar 04 18:53:43 2015 +0000
+++ b/NavigationController/NavigationController.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,59 @@
+#ifndef NavigationController_H
+#define NavigationController_H
+
+#include "mbed.h"
+#include "Global.h"
+#include "Status.h"
+#include "rtos.h"
+#include "Rc.h"
+#include "Sensors.h"
+#include "AltitudeController.h"
+#include "ConfigFileWrapper.h"
+
+class NavigationController
+{
+ public:
+ NavigationController(Status& status, Sensors& sensors, Rc& rc, ConfigFileWrapper& configFileWrapper);
+ ~NavigationController();
+
+ struct SetPoint
+ {
+ double yaw;
+ double pitch;
+ double roll;
+ double throttle;
+ double yawTarget;
+ double yawDifference;
+ double climbRate;
+ double targetAltitude;
+ };
+
+ SetPoint getSetPoint();
+ void updateYawTarget();
+ void updateAltitudeTarget();
+ PidWrapper::NavigationControllerPidParameters getPidParameters();
+ void setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters);
+ void setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters);
+
+ private:
+ static void threadStarter(void const *p);
+ void threadWorker();
+ double map(double input, double inputMin, double inputMax, double outputMin, double outputMax);
+
+ Thread* _thread;
+ Status& _status;
+ Sensors& _sensors;
+ Rc& _rc;
+ ConfigFileWrapper& _configFileWrapper;
+ void saveSettings();
+ SetPoint _setPoints;
+ AltitudeController* _altitudeController;
+ Status::State _state;
+ Status::NavigationMode _navigationMode;
+ Rc::MappedRc _mappedRc;
+ Imu::Angle _angle;
+ Sensors::Altitude _altitude;
+ double _altitudeHoldPidOutput;
+};
+
+#endif
\ No newline at end of file
--- a/PidWrapper/PID.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- a/PidWrapper/PidWrapper.cpp Wed Mar 04 18:53:43 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,24 +0,0 @@
-#include "PidWrapper.h"
-
-PidWrapper::PidWrapper(){}
-
-PidWrapper::~PidWrapper(){}
-
-bool PidWrapper::initialise(PidParameters pidParameters, float inputMin, float inputMax, float outputMin, float outputMax)
-{
- float updateTime = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
- _pid = new PID(pidParameters.p, pidParameters.i, pidParameters.d, updateTime);
- _pid->setInputLimits(inputMin, inputMax);
- _pid->setOutoutLimits(outputMin, outputMax);
- _pid->setMode(AUTO_MODE);
- _pid->setSetPoint(0.0);
- _pid->setBias(0);
-
- DEBUG("PID wrapper initialised");
- return true;
-}
-
-PidWrapper::PidValues pidWrapper::compute(PidValues pidValues)
-{
-
-}
\ No newline at end of file
--- a/PidWrapper/PidWrapper.h Wed Mar 04 18:53:43 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,41 +0,0 @@
-#include "mbed.h"
-#include "Global.h"
-
-#ifndef PidWrapper_H
-#define PidWrapper_H
-
-class PidWrapper // begin declaration of the class
-{
- public: // begin public section
- PidWrapper(); // constructor
- ~PidWrapper();
-
- struct PidOutputs
- {
- float yaw;
- float pitch;
- float roll;
- };
-
- struct PidParameters
- {
- float p;
- float i;
- float d;
- };
-
- struct PidValues
- {
- float setPoint;
- float processValue;
- float output;
- };
-
- bool initialise(PidParameters pidParameters, float inputMin, float inputMax, float outputMin, float outputMax);
- PidValues compute(PidValues pidValues);
-
- private:
- PID* _pid;
-};
-
-#endif
\ No newline at end of file
--- a/Rc/Ppm.lib Wed Mar 04 18:53:43 2015 +0000 +++ b/Rc/Ppm.lib Wed Apr 01 11:19:21 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/joe4465/code/PPM/#b67f18c84c05 +http://mbed.org/users/joe4465/code/PPM/#d13b9e50312f
--- a/Rc/Rc.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/Rc/Rc.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -10,102 +10,119 @@
//Channel 6 is spare
//Channel 7 is spare
-Rc::Rc(){}
+Rc::Rc(Status& status, PinName pin) : _status(status)
+{
+ _notConnectedIterator = 0;
+ _connectedIterator = 0;
+ _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
+ _yawMedianFilter = new filter(5);
+ _pitchMedianFilter = new filter(5);
+ _rollMedianFilter = new filter(5);
+ _thrustMedianFilter = new filter(5);
+ _armMedianFilter = new filter(5);
+ _modeMedianFilter = new filter(5);
+
+ DEBUG("Rc initialised\r\n");
+}
Rc::~Rc(){}
-bool Rc::initialise(Status& status, PinName pin)
-{
- _status = status;
-
- _ppm = new Ppm(pin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
- _yawMedianFilter = new medianFilter(5);
- _pitchMedianFilter = new medianFilter(5);
- _rollMedianFilter = new medianFilter(5);
- _thrustMedianFilter = new medianFilter(5);
-
- _thread = new Thread(&Rc::threadStarter, this, osPriorityHigh);
- DEBUG("Rc initialised");
- return true;
-}
-
-void Rc::threadStarter(void const *p)
-{
- Rc *instance = (Rc*)p;
- instance->threadWorker();
-}
-
-void Rc::threadWorker()
-{
- while(true)
- {
- float rc[8] = {0,0,0,0,0,0,0,0};
-
- //Get channel data - mapped to between 0 and 1
- _ppm->GetChannelData(rc);
-
- //Put channel data into raw rc struct
- _rawRc.channel0 = rc[0];
- _rawRc.channel1 = rc[1];
- _rawRc.channel2 = rc[2];
- _rawRc.channel3 = rc[3];
- _rawRc.channel4 = rc[4];
- _rawRc.channel5 = rc[5];
- _rawRc.channel6 = rc[6];
- _rawRc.channel7 = rc[7];
-
- //Check whether transmitter is connected
- if(_rawRc.channel2 != -1)
- {
- _status.setRcConnected(true);
-
- //Map yaw channel
- _mappedRc.yaw = - _yawMedianFilter->process(Map(_rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
-
- //Map thust channel
- _mappedRc.throttle = _thrustMedianFilter->process(Map(_rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
-
- //Map arm channel.
- if(_rawRc.channel4 > 0.5) _status.setArmed(true);
- else _status.setArmed(false);
-
- //Map mode channel
- if(_rawRc.channel5 < 0.5) _status.setFlightMode(Status::STAB);
- else _status.setFlightMode(Status::RATE);
-
- //Roll and pitch mapping depends on the mode
- if(_status.getFlightMode() == Status::STAB)//Stability mode
- {
- //Roll
- _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
- //Pitch
- _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
- }
- else if(_status.getFlightMode() == Status::RATE)//Rate mode
- {
- //Roll
- _mappedRc.roll = _rollMedianFilter->process(Map(_rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
- //Pitch
- _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
- }
- }
- else _status.setRcConnected(false);
-
- Thread::wait(20);
- }
-}
-
-float Rc::Map(float input, float inputMin, float inputMax, float outputMin, float outputMax)
+double Rc::Map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
{
return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
}
Rc::MappedRc Rc::getMappedRc()
{
- return _mappedRc;
+ Rc::RawRc rawRc = getRawRc();
+ Rc::MappedRc mappedRc = MappedRc();
+
+ //Check if transmitter is connected
+ if(rawRc.channel2 != -1)
+ {
+ if(_connectedIterator < 10) _connectedIterator++;
+ else
+ {
+ _notConnectedIterator = 0;
+ _status.setRcConnected(true);
+
+ //Map yaw channel
+ mappedRc.yaw = _yawMedianFilter->process(-Map(rawRc.channel3, RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
+
+ //Map thust channel
+ mappedRc.throttle = _thrustMedianFilter->process(Map(rawRc.channel2, RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
+
+ //Manual mode
+ //Altitude mode
+ if(_status.getNavigationMode() == Status::ALTITUDE_HOLD)
+ {
+ float maxDeadZone = ((float)RC_THRUST_MAX / 2.0) + (float)RC_DEAD_ZONE;
+ float minDeadZone = ((float)RC_THRUST_MAX / 2.0) - (float)RC_DEAD_ZONE;
+ if((minDeadZone < mappedRc.throttle) && (mappedRc.throttle <= maxDeadZone)) _status.setDeadZone(true);
+ else _status.setDeadZone(false);
+ }
+
+ //Map arm channel.
+ rawRc.channel4 = _armMedianFilter->process(rawRc.channel4);
+ if(rawRc.channel4 > 0.5) _status.setArmed(true);
+ else _status.setArmed(false);
+
+ //Map mode channel
+ rawRc.channel5 = _modeMedianFilter->process(rawRc.channel5);
+ //if(rawRc.channel5 > 0.5) _status.setFlightMode(Status::RATE);
+ //else _status.setFlightMode(Status::STAB);
+ if(rawRc.channel5 > 0.5) _status.setNavigationMode(Status::ALTITUDE_HOLD);
+ else _status.setNavigationMode(Status::NONE);
+
+ //Roll and pitch mapping depends on the mode
+ if(_status.getFlightMode() == Status::STAB)//Stability mode
+ {
+ //Roll
+ mappedRc.roll = _rollMedianFilter->process(Map(rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
+ //Pitch
+ 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
+ }
+ else if(_status.getFlightMode() == Status::RATE)//Rate mode
+ {
+ //Roll
+ mappedRc.roll = _rollMedianFilter->process(Map(rawRc.channel0, RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
+ //Pitch
+ 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
+ }
+ }
+ }
+ else if(_notConnectedIterator < 10) _notConnectedIterator++;
+ else
+ {
+ _status.setRcConnected(false);
+ _notConnectedIterator = 0;
+ _connectedIterator = 0;
+ mappedRc.yaw = 0;
+ mappedRc.pitch = 0;
+ mappedRc.roll = 0;
+ mappedRc.throttle = 0;
+ }
+
+ return mappedRc;
}
Rc::RawRc Rc::getRawRc()
{
- return _rawRc;
+ double rc[8] = {0,0,0,0,0,0,0,0};
+ Rc::RawRc rawRc = RawRc();
+
+ //Get channel data - mapped to between 0 and 1
+ _ppm->GetChannelData(rc);
+
+ //Put channel data into raw rc struct
+ rawRc.channel0 = rc[0];
+ rawRc.channel1 = rc[1];
+ rawRc.channel2 = rc[2];
+ rawRc.channel3 = rc[3];
+ rawRc.channel4 = rc[4];
+ rawRc.channel5 = rc[5];
+ rawRc.channel6 = rc[6];
+ rawRc.channel7 = rc[7];
+
+ return rawRc;
}
\ No newline at end of file
--- a/Rc/Rc.h Wed Mar 04 18:53:43 2015 +0000
+++ b/Rc/Rc.h Wed Apr 01 11:19:21 2015 +0000
@@ -3,7 +3,6 @@
#include "Ppm.h"
#include "filter.h"
#include "Status.h"
-#include "rtos.h"
#ifndef Rc_H
#define Rc_H
@@ -11,47 +10,45 @@
class Rc
{
public:
- Rc();
+ Rc(Status& status, PinName pin);
~Rc();
struct MappedRc
{
- float roll;
- float pitch;
- float throttle;
- float yaw;
+ double yaw;
+ double pitch;
+ double roll;
+ double throttle;
};
struct RawRc
{
- int channel0;
- int channel1;
- int channel2;
- int channel3;
- int channel4;
- int channel5;
- int channel6;
- int channel7;
+ double channel0;
+ double channel1;
+ double channel2;
+ double channel3;
+ double channel4;
+ double channel5;
+ double channel6;
+ double channel7;
};
- bool initialise(Status& status, PinName pin);
MappedRc getMappedRc();
RawRc getRawRc();
private:
- static void threadStarter(void const *p);
- void threadWorker();
- float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax);
+ double Map(double input, double inputMin, double inputMax, double outputMin, double outputMax);
Ppm* _ppm;
- Thread* _thread;
- Status _status;
- MappedRc _mappedRc;
- RawRc _rawRc;
- medianFilter* _yawMedianFilter;
- medianFilter* _pitchMedianFilter;
- medianFilter* _rollMedianFilter;
- medianFilter* _thrustMedianFilter;
+ Status& _status;
+ filter* _yawMedianFilter;
+ filter* _pitchMedianFilter;
+ filter* _rollMedianFilter;
+ filter* _thrustMedianFilter;
+ filter* _armMedianFilter;
+ filter* _modeMedianFilter;
+ int _notConnectedIterator;
+ int _connectedIterator;
};
#endif
\ No newline at end of file
--- a/Rc/filter.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/networker/code/filter/#46a72e790df8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gps/Gps.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,32 @@
+#include "Gps.h"
+
+Gps::Gps(PinName gpsPinTx, PinName gpsPinRx)
+{
+ _values = Value();
+ _gps = new MODSERIAL(gpsPinTx, gpsPinRx);
+ _gps->baud(115200);
+
+ DEBUG("GPS initialised\r\n");
+}
+
+Gps::~Gps(){}
+
+Gps::Value Gps::getValues()
+{
+ while(_gps->readable() > 0)
+ {
+ int c = _gps->getc();
+ if(_tinyGPS.encode(c))
+ {
+ unsigned long fix_age;
+ _tinyGPS.f_get_position(&_values.latitude, &_values.longitude, &fix_age);
+
+ _values.altitude = _tinyGPS.f_altitude();
+
+ if ((fix_age == TinyGPS::GPS_INVALID_AGE) || (fix_age > 5000)) _values.fix = false;
+ else _values.fix = true;
+ }
+ }
+
+ return _values;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gps/Gps.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,31 @@
+#include "mbed.h"
+#include "Global.h"
+#include <TinyGPS.h>
+#include "MODSERIAL.h"
+
+#ifndef Gps_H
+#define Gps_H
+
+class Gps
+{
+ public:
+ Gps(PinName gpsPinTx, PinName gpsPinRx);
+ ~Gps();
+
+ struct Value
+ {
+ double latitude;
+ double longitude;
+ double altitude;
+ bool fix;
+ };
+
+ Value getValues();
+
+ private:
+ MODSERIAL *_gps;
+ TinyGPS _tinyGPS;
+ Value _values;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/Gps/TinyGPS.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/shimniok/code/TinyGPS/#f522b8bdf987
--- a/Sensors/Imu/FreeIMU_external_magnetometer.lib Wed Mar 04 18:53:43 2015 +0000 +++ b/Sensors/Imu/FreeIMU_external_magnetometer.lib Wed Apr 01 11:19:21 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/joe4465/code/FreeIMU_external_magnetometer/#48a0eae27bf1 +http://developer.mbed.org/users/joe4465/code/FreeIMU_external_magnetometer/#0065ffbcd39b
--- a/Sensors/Imu/Imu.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/Sensors/Imu/Imu.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,152 @@
+#include "Imu.h"
+
+Imu::Imu(ConfigFileWrapper& configFileWrapper) :_configFileWrapper(configFileWrapper)
+{
+ _rate = Rate();
+ _angle = Angle();
+
+ _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
+ _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
+
+ Thread::wait(500);
+ _freeImu.init(true);
+ Thread::wait(500);
+
+ _barometerZeroFilter = new filter(100);
+ _barometerFilter = new filter(5);
+
+ _barometerZero = 0;
+ zeroBarometer();
+
+ _velocity = 0;
+ _kalmanFilter = new Kalman(0.1, 32, 1, 0);
+
+ DEBUG("IMU initialised\r\n");
+}
+
+Imu::~Imu(){}
+
+Imu::Rate Imu::getRate()
+{
+ float rate[3];
+ _freeImu.getRate(rate);
+
+ float yaw = rate[2];
+ float pitch = rate[0];
+ float roll = rate[1];
+ rate[0] = yaw;
+ rate[1] = pitch;
+ rate[2] = roll;
+
+ _rate.yaw = rate[0];
+ _rate.pitch = rate[1];
+ _rate.roll = rate[2];
+
+ return _rate;
+}
+
+Imu::Angle Imu::getAngle(bool bias)
+{
+ //Filter Z accel data
+ float values[9];
+ _freeImu.getValues(values);
+ _kalmanFilter->update(1, values[2]);
+
+ //printf("%1.6f, %1.6f, %1.6f\n", values[0], values[1], values[2]);
+
+ //Get angle
+ float angle[3];
+ _freeImu.getYawPitchRoll(angle);
+
+ float yaw = -angle[0];
+ float pitch = angle[2];
+ float roll = -angle[1];
+ angle[0] = yaw;
+ angle[1] = pitch;
+ angle[2] = roll;
+
+ if(bias == true)
+ {
+ _angle.yaw = angle[0];
+ _angle.pitch = angle[1];//; - _accelZeroPitch;
+ _angle.roll = angle[2];//; - _accelZeroRoll;
+ }
+ else
+ {
+ _angle.yaw = angle[0];
+ _angle.pitch = angle[1];
+ _angle.roll = angle[2];
+ }
+
+ return _angle;
+}
+
+double Imu::getAltitude()
+{
+ float altitude = _barometerFilter->process(_freeImu.getBaroAlt());
+ float normalAltitude = (altitude - _barometerZero);
+ return (normalAltitude * 100);
+}
+
+float Imu::getVelocity(float time)
+{
+ //Get quat
+ float q[4];
+ _freeImu.getQ(q);
+
+ //Extract accelerometer data
+ float acc[3];
+ acc[0]= 0; //x
+ acc[1]= 0; //y
+ acc[2]= _kalmanFilter->getEstimated();
+
+ //Gravity compensate
+ _freeImu.gravityCompensateAcc(acc, q);
+
+ float zAcceleration = 0;
+ if(acc[2] < -0.01 || acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
+
+ _velocity += zAcceleration * time;
+
+ return _velocity; //cm/s/s
+}
+
+float Imu::getVelocity()
+{
+ return _velocity; //cm/s/s
+}
+
+void Imu::zeroGyro()
+{
+ _freeImu.zeroGyro();
+}
+
+void Imu::setCurrentVelocity(float velocity)
+{
+ _velocity = velocity;
+}
+
+void Imu::zeroBarometer()
+{
+ for(int i = 0; i < 100; i++)
+ {
+ _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
+ }
+ DEBUG("Barometer zero %f\r\n", _barometerZero);
+}
+
+void Imu::enable(bool enable)
+{
+ _freeImu.sample(enable);
+}
+
+void Imu::zeroAccel()
+{
+ Imu::Angle angle = getAngle(false);
+ _accelZeroPitch = angle.pitch;
+ _accelZeroRoll = angle.roll;
+ //DEBUG("Zero accel, pitch %f, roll %f\r\n", _accelZeroPitch, _accelZeroRoll);
+ //_configFileWrapper.setAccelZeroPitch(_accelZeroPitch);
+ //_configFileWrapper.setAccelZeroRoll(_accelZeroRoll);
+ //_configFileWrapper.saveSettings();
+}
\ No newline at end of file
--- a/Sensors/Imu/Imu.h Wed Mar 04 18:53:43 2015 +0000
+++ b/Sensors/Imu/Imu.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,65 @@
+#include "mbed.h"
+#include "Global.h"
+#include "FreeIMU.h"
+#include "filter.h"
+#include "ConfigFileWrapper.h"
+#include "Kalman.h"
+
+#ifndef Imu_H
+#define Imu_H
+
+class Imu
+{
+ public:
+ Imu(ConfigFileWrapper& configFileWrapper);
+ ~Imu();
+
+ struct Rate
+ {
+ double yaw;
+ double pitch;
+ double roll;
+ };
+
+ struct Angle
+ {
+ double yaw;
+ double pitch;
+ double roll;
+ };
+
+ struct Velocity
+ {
+ double x;
+ double y;
+ double z;
+ };
+
+ void enable(bool enable);
+
+ Rate getRate();
+ Angle getAngle(bool bias = true);
+ float getVelocity(float time);
+ float getVelocity();
+ double getAltitude();
+
+ void zeroGyro();
+ void zeroBarometer();
+ void zeroAccel();
+ void setCurrentVelocity(float velocity);
+
+ private:
+ FreeIMU _freeImu;
+ filter* _barometerZeroFilter;
+ filter* _barometerFilter;
+ Rate _rate;
+ Angle _angle;
+ float _velocity;
+ float _barometerZero;
+ ConfigFileWrapper& _configFileWrapper;
+ float _accelZeroPitch;
+ float _accelZeroRoll;
+ Kalman* _kalmanFilter;
+};
+
+#endif
\ No newline at end of file
--- a/Sensors/LidarLite.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/akashvibhute/code/LidarLite/#8e6304ab38d2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors/LidarLitePwm.lib Wed Apr 01 11:19:21 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/joe4465/code/LidarLitePwm/#0b1f4404cb21
--- a/Sensors/MaxbotixDriver.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/joe4465/code/MaxbotixDriver/#24d9d6d213aa
--- a/Sensors/Sensors.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/Sensors/Sensors.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,160 @@
+#include "Sensors.h"
+
+Sensors::Sensors(Status& status, ConfigFileWrapper& configFileWrapper, PinName gpsPinTx, PinName gpsPinRx, PinName i2cSda, PinName i2cScl, PinName lidarInterrupt) : _status(status)
+{
+ _gpsValues = Gps::Value();
+ _position = Position();
+ _altitude = Altitude();
+
+ //Initialise IMU
+ _imu = new Imu(configFileWrapper);
+ _imu->zeroGyro();
+
+ //Initialise GPS
+ _gps = new Gps(gpsPinTx, gpsPinRx);
+
+ //Initialise Lidar
+ _lidar = new LidarLitePwm(lidarInterrupt);
+
+ //Initialise kalman
+ _altitudeKalmanFilter = new Kalman(0.75, 2, 1, 0);
+
+ DEBUG("Sensors initialised\r\n");
+}
+
+Sensors::~Sensors(){}
+
+void Sensors::update()
+{
+ //Update GPS
+ updateGpsValues();
+
+ //Update Altitude
+ updateAltitude();
+
+ //updatePosition();
+}
+
+Imu::Rate Sensors::getRate()
+{
+ return _rate;
+}
+
+Imu::Angle Sensors::getAngle()
+{
+ return _angle;
+}
+
+Gps::Value Sensors::getGpsValues()
+{
+ return _gpsValues;
+}
+
+Sensors::Altitude Sensors::getAltitude()
+{
+ return _altitude;
+}
+
+Sensors::Position Sensors::getPosition()
+{
+ return _position;
+}
+
+double Sensors::getZVelocity()
+{
+ return _imu->getVelocity();
+}
+
+void Sensors::updateImu()
+{
+ _angle = _imu->getAngle();
+ _rate = _imu->getRate();
+
+ return;
+}
+
+void Sensors::updateGpsValues()
+{
+ _gpsValues = _gps->getValues();
+}
+
+void Sensors::updateAltitude()
+{
+ _altitude.lidar = getLidarAltitude(); //cm
+ _altitude.barometer = _imu->getAltitude(); //cm
+ _altitude.gps = _gpsValues.altitude; //m
+
+ //Update IMU velocity
+ double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds
+ double velocity = _imu->getVelocity(time);
+
+ //Compute predicted altitude
+ double predictedAltitude = _altitude.computed + velocity;
+
+ //Compute predicted change in altitude
+ double predictedChange = 1;
+ if(_altitude.computed != 0) predictedChange = predictedAltitude / _altitude.computed;
+
+ //Compute estimated altitude
+ double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, _altitude.lidar);
+
+ //Compute estimated velocity
+ double estimatedVelocity = estimatedAltitude - _altitude.computed;
+
+ //Reset IMU velocity to estimated velocity
+ _imu->setCurrentVelocity(estimatedVelocity);
+
+ //Save new altitude
+ _altitude.computed = estimatedAltitude;
+
+ //printf("%1.6f, %1.6f, %1.6f\r\n", predictedAltitude, _altitude.lidar, _altitude.computed);
+}
+
+
+void Sensors::updatePosition()
+{
+
+}
+
+void Sensors::zeroGyro()
+{
+ _imu->zeroGyro();
+}
+
+void Sensors::zeroBarometer()
+{
+ _imu->zeroBarometer();
+}
+
+void Sensors::enable(bool enable)
+{
+ _imu->enable(enable);
+}
+
+int Sensors::getLidarAltitude()
+{
+ Imu::Angle angles = getAngle();
+ int rawAltitude = _lidar->read();
+
+ double oppAng = 0;
+ double multiplier = 0;
+ int pitchAltitude = 0;
+ int rollAltitude = 0;
+
+ //Calulate pitch altitude
+ oppAng = 90 - abs(angles.pitch);
+ multiplier = sin(oppAng*PI/180);
+ pitchAltitude = multiplier * rawAltitude;
+
+ //Calulate roll altitude
+ oppAng = 90 - abs(angles.roll);
+ multiplier = sin(oppAng*PI/180);
+ rollAltitude = multiplier * rawAltitude;
+
+ return (pitchAltitude + rollAltitude)/ 2;
+}
+
+void Sensors::zeroAccel()
+{
+ _imu->zeroAccel();
+}
\ No newline at end of file
--- a/Sensors/Sensors.h Wed Mar 04 18:53:43 2015 +0000
+++ b/Sensors/Sensors.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+#include "Global.h"
+#include "rtos.h"
+#include "Gps.h"
+#include "Imu.h"
+#include "Status.h"
+#include "LidarLitePwm.h"
+#include "Kalman.h"
+
+#ifndef Sensors_H
+#define Sensors_H
+
+#define PI 3.14159265
+
+class Sensors
+{
+ public:
+ Sensors(Status& status, ConfigFileWrapper& configFileWrapper, PinName gpsPinTx, PinName gpsPinRx, PinName i2cSda, PinName i2cScl, PinName lidarInterrupt);
+ ~Sensors();
+
+ struct Position
+ {
+ double accelX;
+ double accelY;
+ double accelZ;
+ double latitude;
+ double longitude;
+ double computedX;
+ double computedY;
+ };
+
+ struct Altitude
+ {
+ double lidar;
+ double barometer;
+ double gps;
+ double computed;
+ };
+
+ void update();
+ Imu::Rate getRate();
+ Imu::Angle getAngle();
+ Gps::Value getGpsValues();
+ double getZVelocity();
+ Sensors::Altitude getAltitude();
+ Sensors::Position getPosition();
+ void enable(bool enable);
+ void zeroBarometer();
+ void updateImu();
+ void zeroAccel();
+
+ private:
+ void zeroGyro();
+ void updateGpsValues();
+ void updateAltitude();
+ void updateVelocity();
+ void updatePosition();
+ int getLidarAltitude();
+
+ Status& _status;
+ Imu::Rate _rate;
+ Imu::Angle _angle;
+ Position _position;
+ Altitude _altitude;
+ Gps::Value _gpsValues;
+ Imu* _imu;
+ Gps* _gps;
+ LidarLitePwm* _lidar;
+ Kalman* _altitudeKalmanFilter;
+};
+
+#endif
\ No newline at end of file
--- a/Status/Status.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/Status/Status.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -1,12 +1,34 @@
#include "Status.h"
-Status::Status(){}
+Status::Status()
+{
+ setFlightMode(STAB);
+ setBaseStationMode(STATUS);
+ setNavigationMode(NONE);
+ setBatteryLevel(0);
+ setRcConnected(false);
+ setArmed(false);
+ setMotorsSpinning(false);
+ setDeadZone(false);
+ setInitialised(false);
+ DEBUG("Status initialised\r\n");
+}
Status::~Status(){}
-bool Status::initialise()
+bool Status::update()
{
- setState(PREFLIGHT);
+ if(getInitialised() == false && getRcConnected() == false) setState(Status::PREFLIGHT);
+ else if(getInitialised() == true && getRcConnected() == false) setState(Status::PREFLIGHT);
+ else if(getInitialised() == true && getRcConnected() == true && getArmed() == false) setState(Status::STANDBY);
+ else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == false) setState(Status::GROUND_READY);
+ else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::NONE) setState(Status::MANUAL);
+ else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::POSITION_HOLD) setState(Status::AUTO);
+ else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::ALTITUDE_HOLD) setState(Status::STABILISED);
+
+ //else setState(Status::ERROR);
+
+ return true;
}
bool Status::setState(State state)
@@ -14,58 +36,77 @@
switch(state)
{
case PREFLIGHT:
- setFlightMode(NOT_SET);
- setBaseStationMode(STATUS);
- setBatteryLevel(0);
- setArmed(false);
- setInitialised(false);
+ if(_state != Status::PREFLIGHT)
+ {
+ _state = PREFLIGHT;
+ _statusLights.clear();
+ DEBUG("State set to PREFLIGHT\r\n");
+ return true;
+ }
+ _statusLights.preFlight();
return true;
case STANDBY:
-
+ if(_state != Status::STANDBY)
+ {
+ _state = STANDBY;
+ _statusLights.clear();
+ DEBUG("State set to STANDBY\r\n");
+ return true;
+ }
+ _statusLights.standby();
return true;
-
case GROUND_READY:
-
+ if(_state != Status::GROUND_READY)
+ {
+ _state = GROUND_READY;
+ _statusLights.clear();
+ DEBUG("State set to GROUND_READY\r\n");
+ return true;
+ }
+ _statusLights.groundReady();
return true;
-
-
+
case MANUAL:
-
- return true;
-
+ if(_state != Status::MANUAL)
+ {
+ _state = MANUAL;
+ _statusLights.clear();
+ DEBUG("State set to MANUAL\r\n");
+ return true;
+ }
+ _statusLights.flying();
+ return true;
case STABILISED:
-
- return true;
+ if(_state != Status::STABILISED)
+ {
+ _state = STABILISED;
+ _statusLights.clear();
+ DEBUG("State set to STABILISED\r\n");
+ return true;
+ }
+ _statusLights.flying();
+ return true;
case AUTO:
return true;
-
- case ABORT:
- return true;
-
-
- case EMG_LAND:
-
+ case ERROR:
+ if(_state != Status::ERROR)
+ {
+ _state = Status::ERROR;
+ _statusLights.clear();
+ DEBUG("State set to ERROR\r\n");
+ return true;
+ }
+ _statusLights.error();
return true;
-
- case EMG_OFF:
-
- return true;
-
-
- case GROUND_ERROR:
-
- return true;
-
-
default:
return false;
@@ -80,8 +121,13 @@
bool Status::setFlightMode(FlightMode flightMode)
{
- _flightMode = flightMode;
- return true;
+ if(flightMode != _flightMode)
+ {
+ _flightMode = flightMode;
+ DEBUG("Flight mode set to %d\r\n", _flightMode);
+ return true;
+ }
+ else return false;
}
Status::FlightMode Status::getFlightMode()
@@ -89,10 +135,31 @@
return _flightMode;
}
+bool Status::setNavigationMode(NavigationMode navigationMode)
+{
+ if(navigationMode != _navigationMode)
+ {
+ _navigationMode = navigationMode;
+ DEBUG("Navigation mode set to %d\r\n", _navigationMode);
+ return true;
+ }
+ else return false;
+}
+
+Status::NavigationMode Status::getNavigationMode()
+{
+ return _navigationMode;
+}
+
bool Status::setBaseStationMode(BaseStationMode baseStationMode)
{
- _baseStationMode = baseStationMode;
- return true;
+ if(baseStationMode != _baseStationMode)
+ {
+ _baseStationMode = baseStationMode;
+ DEBUG("Base station mode set to %d\r\n", _baseStationMode);
+ return true;
+ }
+ return false;
}
Status::BaseStationMode Status::getBaseStationMode()
@@ -100,21 +167,41 @@
return _baseStationMode;
}
-bool Status::setBatteryLevel(float batteryLevel)
+bool Status::setBatteryLevel(double batteryLevel)
{
_batteryLevel = batteryLevel;
return true;
}
-float Status::getBatteryLevel()
+double Status::getBatteryLevel()
{
return _batteryLevel;
}
bool Status::setArmed(bool armed)
{
- _armed = armed;
- return true;
+ if(armed != _armed)
+ {
+ if(armed == false)
+ {
+ _armed = armed;
+ DEBUG("Armed set to %d\r\n", _armed);
+ return true;
+ }
+ else if (armed == true && _navigationMode == Status::NONE && getMotorsSpinning() == false)
+ {
+ _armed = armed;
+ DEBUG("Armed set to %d\r\n", _armed);
+ return true;
+ }
+ else if (armed == true && _navigationMode == Status::ALTITUDE_HOLD && getMotorsSpinning() == false && getDeadZone() == true)
+ {
+ _armed = armed;
+ DEBUG("Armed set to %d\r\n", _armed);
+ return true;
+ }
+ }
+ return false;
}
bool Status::getArmed()
@@ -124,8 +211,13 @@
bool Status::setInitialised(bool initialised)
{
- _initialised = initialised;
- return true;
+ if(initialised != _initialised)
+ {
+ _initialised = initialised;
+ DEBUG("Initialised set to %d\r\n", _initialised);
+ return true;
+ }
+ else return false;
}
bool Status::getInitialised()
@@ -135,11 +227,54 @@
bool Status::setRcConnected(bool rcConnected)
{
- _rcConnected = rcConnected;
- return true;
+ if(rcConnected != _rcConnected)
+ {
+ _rcConnected = rcConnected;
+ if(_rcConnected == false)
+ {
+ setArmed(false);
+ setMotorsSpinning(false);
+ setDeadZone(false);
+ }
+ DEBUG("Rc connected set to %d\r\n", _rcConnected);
+ return true;
+ }
+ else return false;
}
bool Status::getRcConnected()
{
return _rcConnected;
+}
+
+bool Status::setMotorsSpinning(bool flying)
+{
+ if(flying != _flying)
+ {
+ _flying = flying;
+ DEBUG("Flying set to %d\r\n", _flying);
+ return true;
+ }
+ else return false;
+}
+
+bool Status::getMotorsSpinning()
+{
+ return _flying;
+}
+
+bool Status::setDeadZone(bool deadZone)
+{
+ if(deadZone != _deadZone)
+ {
+ _deadZone = deadZone;
+ DEBUG("Dead zone set to %d\r\n", _deadZone);
+ return true;
+ }
+ else return false;
+}
+
+bool Status::getDeadZone()
+{
+ return _deadZone;
}
\ No newline at end of file
--- a/Status/Status.h Wed Mar 04 18:53:43 2015 +0000
+++ b/Status/Status.h Wed Apr 01 11:19:21 2015 +0000
@@ -1,13 +1,15 @@
#include "mbed.h"
+#include "Global.h"
+#include "StatusLights.h"
#ifndef Status_H
#define Status_H
-class Status // begin declaration of the class
+class Status
{
- public: // begin public section
- Status(); // constructor
- ~Status(); // destructor
+ public:
+ Status();
+ ~Status();
enum State
{
@@ -17,17 +19,20 @@
MANUAL,
STABILISED,
AUTO,
- ABORT,
- EMG_LAND,
- EMG_OFF,
- GROUND_ERROR
+ ERROR
};
enum FlightMode
{
RATE,
- STAB,
- NOT_SET
+ STAB
+ };
+
+ enum NavigationMode
+ {
+ NONE,
+ ALTITUDE_HOLD,
+ POSITION_HOLD
};
enum BaseStationMode
@@ -36,40 +41,52 @@
PID_OUTPUTS,
IMU_OUTPUTS,
STATUS,
- MAPPED_RC,
+ RC,
PID_TUNING,
GPS,
ZERO,
RATE_TUNING,
STAB_TUNING,
ALTITUDE,
- VELOCITY
+ VELOCITY,
+ ALTITUDE_STATUS
};
- bool initialise();
- bool setState(State state);
+ bool update();
State getState();
bool setFlightMode(FlightMode flightMode);
FlightMode getFlightMode();
+ bool setNavigationMode(NavigationMode navigationMode);
+ NavigationMode getNavigationMode();
bool setBaseStationMode(BaseStationMode baseStationMode);
BaseStationMode getBaseStationMode();
- bool setBatteryLevel(float batteryLevel);
- float getBatteryLevel();
+ bool setBatteryLevel(double batteryLevel);
+ double getBatteryLevel();
bool setArmed(bool armed);
bool getArmed();
bool setInitialised(bool initialised);
bool getInitialised();
bool setRcConnected(bool rcConnected);
bool getRcConnected();
+ bool setMotorsSpinning(bool flying);
+ bool getMotorsSpinning();
+ bool setDeadZone(bool flying);
+ bool getDeadZone();
private:
State _state;
- FlightMode _flightMode;
+ FlightMode _flightMode;
+ NavigationMode _navigationMode;
BaseStationMode _baseStationMode;
- float _batteryLevel;
+ StatusLights _statusLights;
+ bool setState(State state);
+ void flash();
+ double _batteryLevel;
bool _armed;
bool _initialised;
bool _rcConnected;
+ bool _flying;
+ bool _deadZone;
};
#endif
\ No newline at end of file
--- a/Status/StatusLights/StatusLights.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/Status/StatusLights/StatusLights.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,57 @@
+#include "StatusLights.h"
+
+StatusLights::StatusLights()
+{
+ _ledState = 0;
+ _led1 = new DigitalOut(LED1);
+ _led2 = new DigitalOut(LED2);
+ _led3 = new DigitalOut(LED3);
+ _led4 = new DigitalOut(LED4);
+
+ DEBUG("Status lights initialised\r\n");
+}
+
+StatusLights::~StatusLights(){}
+
+void StatusLights::preFlight()
+{
+ _led1 -> write(!_led1 -> read());
+}
+
+void StatusLights::standby()
+{
+ _led2 -> write(!_led2 -> read());
+}
+
+void StatusLights::groundReady()
+{
+ _led3 -> write(!_led3 -> read());
+}
+
+void StatusLights::flying()
+{
+ _ledState++;
+ if (_ledState > 5) { _ledState = 0; }
+
+ _led1 -> write(_ledState == 0);
+ _led2 -> write(_ledState == 1 || _ledState == 5);
+ _led3 -> write(_ledState == 2 || _ledState == 4);
+ _led4 -> write(_ledState == 3);
+}
+
+void StatusLights::error()
+{
+ _led1 -> write(!_led1 -> read());
+ _led2 -> write(!_led2 -> read());
+ _led3 -> write(!_led3 -> read());
+ _led4 -> write(!_led4 -> read());
+}
+
+void StatusLights::clear()
+{
+ _led1->write(0);
+ _led2->write(0);
+ _led3->write(0);
+ _led4->write(0);
+ _ledState = 0;
+}
--- a/Status/StatusLights/StatusLights.h Wed Mar 04 18:53:43 2015 +0000
+++ b/Status/StatusLights/StatusLights.h Wed Apr 01 11:19:21 2015 +0000
@@ -0,0 +1,31 @@
+#include "mbed.h"
+#include "Global.h"
+#include "rtos.h"
+#include "Gps.h"
+#include "Imu.h"
+
+#ifndef StatusLights_H
+#define StatusLights_H
+
+class StatusLights
+{
+ public:
+ StatusLights();
+ ~StatusLights();
+
+ void clear();
+ void preFlight();
+ void standby();
+ void groundReady();
+ void flying();
+ void error();
+
+ private:
+ int _ledState;
+ DigitalOut* _led1;
+ DigitalOut* _led2;
+ DigitalOut* _led3;
+ DigitalOut* _led4;
+};
+
+#endif
\ No newline at end of file
--- a/main.cpp Wed Mar 04 18:53:43 2015 +0000
+++ b/main.cpp Wed Apr 01 11:19:21 2015 +0000
@@ -7,69 +7,53 @@
#include "Rc.h"
#include "FlightController.h"
#include "NavigationController.h"
+#include "ConfigFileWrapper.h"
+//Debug serial
MODSERIAL _debug(USBTX, USBRX);
-//Wireless Serial
-MODSERIAL _base(p9, p10);
-
-//GPS Serial
-//MODSERIAL _gps(p13, p14);
-
-//Onboard LED's
-//DigitalOut _led1(LED1);
-//DigitalOut _led2(LED2);
-//DigitalOut _led3(LED3);
-//DigitalOut _led4(LED4);
-
-//MaxBotix Ping sensor
-//Timer _maxBotixTimer;
-//Sonar _maxBotixSensor(p15, _maxBotixTimer);
-
-//Lidar
-//LidarLite _lidar(p28, p27);
-
-//Unused analog pins
+//Unused analog pins, set to DigitalOut to remove noise.
DigitalOut _spare1(p16);
DigitalOut _spare2(p17);
DigitalOut _spare3(p18);
DigitalOut _spare4(p19);
-
-//Classes
-Status _status;
-//Sensors *_sensors(_status, p13, p14, p15, p28, p27);
-//BaseStation *_baseStation(p9, p10);
-Rc _rc;
-FlightController _flightController;
-//NavigationController *_navigationController;
-
+
int main()
{
- _base.baud(115200);
+ _debug.baud(115200);
DEBUG("\r\n");
DEBUG("********************************************************************************\r\n");
DEBUG("Starting Setup\r\n");
DEBUG("********************************************************************************\r\n");
- //Set Status
- _status.initialise();
-
- //Initalise Base Station
-
- //Initialise RC
- _rc.initialise(_status, p8);
+ ConfigFileWrapper _configFileWrapper = ConfigFileWrapper(); // No update
+ Thread::wait(100);
+ Status _status = Status(); // 10 Hz called from main
+ Thread::wait(100);
+ Sensors _sensors = Sensors(_status, _configFileWrapper, p13, p14, p28, p27, p15); // 50Hz called from navigation controller
+ Thread::wait(100);
+ Rc _rc = Rc(_status, p8); // 50Hz called from navigation controller
+ Thread::wait(100);
+ NavigationController _navigationController = NavigationController(_status, _sensors, _rc, _configFileWrapper); // 50Hz internal thread
+ Thread::wait(100);
+ FlightController _flightController = FlightController(_status, _sensors, _navigationController, _configFileWrapper, p21, p22, p23, p24); // 500Hz internal thread
+ Thread::wait(100);
+ BaseStation _baseStation = BaseStation(_status, _rc, _sensors, _navigationController, _flightController, _configFileWrapper, p9, p10); // 5Hz internal thread
+ Thread::wait(100);
- //Initialise Sensors
-
- //Initialise Navigation
-
- //Initialise Flight Controller
- _flightController.initialise(_status, _sensors, _navigationController, p21, p22, p23, p24);
-
-
+ //Thread::wait(10000);
DEBUG("********************************************************************************\r\n");
DEBUG("Finished Setup\r\n");
DEBUG("********************************************************************************\r\n");
+
+ _status.setInitialised(true);
+
+ osThreadSetPriority(osThreadGetId(), osPriorityNormal);
+ while(true)
+ {
+ _status.update();
+ Thread::wait(100);
+ }
}
--- a/mbed-rtos.lib Wed Mar 04 18:53:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed-rtos/#721c3a93a7b8