Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

Sensors/Sensors.cpp@2:969dfa4f2436, 2015-04-01 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 2:969dfa4f2436 1 #include "Sensors.h"
joe4465 2:969dfa4f2436 2
joe4465 2:969dfa4f2436 3 Sensors::Sensors(Status& status, ConfigFileWrapper& configFileWrapper, PinName gpsPinTx, PinName gpsPinRx, PinName i2cSda, PinName i2cScl, PinName lidarInterrupt) : _status(status)
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 _gpsValues = Gps::Value();
joe4465 2:969dfa4f2436 6 _position = Position();
joe4465 2:969dfa4f2436 7 _altitude = Altitude();
joe4465 2:969dfa4f2436 8
joe4465 2:969dfa4f2436 9 //Initialise IMU
joe4465 2:969dfa4f2436 10 _imu = new Imu(configFileWrapper);
joe4465 2:969dfa4f2436 11 _imu->zeroGyro();
joe4465 2:969dfa4f2436 12
joe4465 2:969dfa4f2436 13 //Initialise GPS
joe4465 2:969dfa4f2436 14 _gps = new Gps(gpsPinTx, gpsPinRx);
joe4465 2:969dfa4f2436 15
joe4465 2:969dfa4f2436 16 //Initialise Lidar
joe4465 2:969dfa4f2436 17 _lidar = new LidarLitePwm(lidarInterrupt);
joe4465 2:969dfa4f2436 18
joe4465 2:969dfa4f2436 19 //Initialise kalman
joe4465 2:969dfa4f2436 20 _altitudeKalmanFilter = new Kalman(0.75, 2, 1, 0);
joe4465 2:969dfa4f2436 21
joe4465 2:969dfa4f2436 22 DEBUG("Sensors initialised\r\n");
joe4465 2:969dfa4f2436 23 }
joe4465 2:969dfa4f2436 24
joe4465 2:969dfa4f2436 25 Sensors::~Sensors(){}
joe4465 2:969dfa4f2436 26
joe4465 2:969dfa4f2436 27 void Sensors::update()
joe4465 2:969dfa4f2436 28 {
joe4465 2:969dfa4f2436 29 //Update GPS
joe4465 2:969dfa4f2436 30 updateGpsValues();
joe4465 2:969dfa4f2436 31
joe4465 2:969dfa4f2436 32 //Update Altitude
joe4465 2:969dfa4f2436 33 updateAltitude();
joe4465 2:969dfa4f2436 34
joe4465 2:969dfa4f2436 35 //updatePosition();
joe4465 2:969dfa4f2436 36 }
joe4465 2:969dfa4f2436 37
joe4465 2:969dfa4f2436 38 Imu::Rate Sensors::getRate()
joe4465 2:969dfa4f2436 39 {
joe4465 2:969dfa4f2436 40 return _rate;
joe4465 2:969dfa4f2436 41 }
joe4465 2:969dfa4f2436 42
joe4465 2:969dfa4f2436 43 Imu::Angle Sensors::getAngle()
joe4465 2:969dfa4f2436 44 {
joe4465 2:969dfa4f2436 45 return _angle;
joe4465 2:969dfa4f2436 46 }
joe4465 2:969dfa4f2436 47
joe4465 2:969dfa4f2436 48 Gps::Value Sensors::getGpsValues()
joe4465 2:969dfa4f2436 49 {
joe4465 2:969dfa4f2436 50 return _gpsValues;
joe4465 2:969dfa4f2436 51 }
joe4465 2:969dfa4f2436 52
joe4465 2:969dfa4f2436 53 Sensors::Altitude Sensors::getAltitude()
joe4465 2:969dfa4f2436 54 {
joe4465 2:969dfa4f2436 55 return _altitude;
joe4465 2:969dfa4f2436 56 }
joe4465 2:969dfa4f2436 57
joe4465 2:969dfa4f2436 58 Sensors::Position Sensors::getPosition()
joe4465 2:969dfa4f2436 59 {
joe4465 2:969dfa4f2436 60 return _position;
joe4465 2:969dfa4f2436 61 }
joe4465 2:969dfa4f2436 62
joe4465 2:969dfa4f2436 63 double Sensors::getZVelocity()
joe4465 2:969dfa4f2436 64 {
joe4465 2:969dfa4f2436 65 return _imu->getVelocity();
joe4465 2:969dfa4f2436 66 }
joe4465 2:969dfa4f2436 67
joe4465 2:969dfa4f2436 68 void Sensors::updateImu()
joe4465 2:969dfa4f2436 69 {
joe4465 2:969dfa4f2436 70 _angle = _imu->getAngle();
joe4465 2:969dfa4f2436 71 _rate = _imu->getRate();
joe4465 2:969dfa4f2436 72
joe4465 2:969dfa4f2436 73 return;
joe4465 2:969dfa4f2436 74 }
joe4465 2:969dfa4f2436 75
joe4465 2:969dfa4f2436 76 void Sensors::updateGpsValues()
joe4465 2:969dfa4f2436 77 {
joe4465 2:969dfa4f2436 78 _gpsValues = _gps->getValues();
joe4465 2:969dfa4f2436 79 }
joe4465 2:969dfa4f2436 80
joe4465 2:969dfa4f2436 81 void Sensors::updateAltitude()
joe4465 2:969dfa4f2436 82 {
joe4465 2:969dfa4f2436 83 _altitude.lidar = getLidarAltitude(); //cm
joe4465 2:969dfa4f2436 84 _altitude.barometer = _imu->getAltitude(); //cm
joe4465 2:969dfa4f2436 85 _altitude.gps = _gpsValues.altitude; //m
joe4465 2:969dfa4f2436 86
joe4465 2:969dfa4f2436 87 //Update IMU velocity
joe4465 2:969dfa4f2436 88 double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds
joe4465 2:969dfa4f2436 89 double velocity = _imu->getVelocity(time);
joe4465 2:969dfa4f2436 90
joe4465 2:969dfa4f2436 91 //Compute predicted altitude
joe4465 2:969dfa4f2436 92 double predictedAltitude = _altitude.computed + velocity;
joe4465 2:969dfa4f2436 93
joe4465 2:969dfa4f2436 94 //Compute predicted change in altitude
joe4465 2:969dfa4f2436 95 double predictedChange = 1;
joe4465 2:969dfa4f2436 96 if(_altitude.computed != 0) predictedChange = predictedAltitude / _altitude.computed;
joe4465 2:969dfa4f2436 97
joe4465 2:969dfa4f2436 98 //Compute estimated altitude
joe4465 2:969dfa4f2436 99 double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, _altitude.lidar);
joe4465 2:969dfa4f2436 100
joe4465 2:969dfa4f2436 101 //Compute estimated velocity
joe4465 2:969dfa4f2436 102 double estimatedVelocity = estimatedAltitude - _altitude.computed;
joe4465 2:969dfa4f2436 103
joe4465 2:969dfa4f2436 104 //Reset IMU velocity to estimated velocity
joe4465 2:969dfa4f2436 105 _imu->setCurrentVelocity(estimatedVelocity);
joe4465 2:969dfa4f2436 106
joe4465 2:969dfa4f2436 107 //Save new altitude
joe4465 2:969dfa4f2436 108 _altitude.computed = estimatedAltitude;
joe4465 2:969dfa4f2436 109
joe4465 2:969dfa4f2436 110 //printf("%1.6f, %1.6f, %1.6f\r\n", predictedAltitude, _altitude.lidar, _altitude.computed);
joe4465 2:969dfa4f2436 111 }
joe4465 2:969dfa4f2436 112
joe4465 2:969dfa4f2436 113
joe4465 2:969dfa4f2436 114 void Sensors::updatePosition()
joe4465 2:969dfa4f2436 115 {
joe4465 2:969dfa4f2436 116
joe4465 2:969dfa4f2436 117 }
joe4465 2:969dfa4f2436 118
joe4465 2:969dfa4f2436 119 void Sensors::zeroGyro()
joe4465 2:969dfa4f2436 120 {
joe4465 2:969dfa4f2436 121 _imu->zeroGyro();
joe4465 2:969dfa4f2436 122 }
joe4465 2:969dfa4f2436 123
joe4465 2:969dfa4f2436 124 void Sensors::zeroBarometer()
joe4465 2:969dfa4f2436 125 {
joe4465 2:969dfa4f2436 126 _imu->zeroBarometer();
joe4465 2:969dfa4f2436 127 }
joe4465 2:969dfa4f2436 128
joe4465 2:969dfa4f2436 129 void Sensors::enable(bool enable)
joe4465 2:969dfa4f2436 130 {
joe4465 2:969dfa4f2436 131 _imu->enable(enable);
joe4465 2:969dfa4f2436 132 }
joe4465 2:969dfa4f2436 133
joe4465 2:969dfa4f2436 134 int Sensors::getLidarAltitude()
joe4465 2:969dfa4f2436 135 {
joe4465 2:969dfa4f2436 136 Imu::Angle angles = getAngle();
joe4465 2:969dfa4f2436 137 int rawAltitude = _lidar->read();
joe4465 2:969dfa4f2436 138
joe4465 2:969dfa4f2436 139 double oppAng = 0;
joe4465 2:969dfa4f2436 140 double multiplier = 0;
joe4465 2:969dfa4f2436 141 int pitchAltitude = 0;
joe4465 2:969dfa4f2436 142 int rollAltitude = 0;
joe4465 2:969dfa4f2436 143
joe4465 2:969dfa4f2436 144 //Calulate pitch altitude
joe4465 2:969dfa4f2436 145 oppAng = 90 - abs(angles.pitch);
joe4465 2:969dfa4f2436 146 multiplier = sin(oppAng*PI/180);
joe4465 2:969dfa4f2436 147 pitchAltitude = multiplier * rawAltitude;
joe4465 2:969dfa4f2436 148
joe4465 2:969dfa4f2436 149 //Calulate roll altitude
joe4465 2:969dfa4f2436 150 oppAng = 90 - abs(angles.roll);
joe4465 2:969dfa4f2436 151 multiplier = sin(oppAng*PI/180);
joe4465 2:969dfa4f2436 152 rollAltitude = multiplier * rawAltitude;
joe4465 2:969dfa4f2436 153
joe4465 2:969dfa4f2436 154 return (pitchAltitude + rollAltitude)/ 2;
joe4465 2:969dfa4f2436 155 }
joe4465 2:969dfa4f2436 156
joe4465 2:969dfa4f2436 157 void Sensors::zeroAccel()
joe4465 2:969dfa4f2436 158 {
joe4465 2:969dfa4f2436 159 _imu->zeroAccel();
joe4465 2:969dfa4f2436 160 }