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@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
3:4823d6750629 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 2:969dfa4f2436 1 #include "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 3:4823d6750629 7 _startingPosition = Position();
joe4465 2:969dfa4f2436 8 _altitude = Altitude();
joe4465 2:969dfa4f2436 9
joe4465 2:969dfa4f2436 10 //Initialise IMU
joe4465 2:969dfa4f2436 11 _imu = new Imu(configFileWrapper);
joe4465 2:969dfa4f2436 12 _imu->zeroGyro();
joe4465 2:969dfa4f2436 13
joe4465 2:969dfa4f2436 14 //Initialise GPS
joe4465 2:969dfa4f2436 15 _gps = new Gps(gpsPinTx, gpsPinRx);
joe4465 2:969dfa4f2436 16
joe4465 2:969dfa4f2436 17 //Initialise Lidar
joe4465 2:969dfa4f2436 18 _lidar = new LidarLitePwm(lidarInterrupt);
joe4465 2:969dfa4f2436 19
joe4465 3:4823d6750629 20
joe4465 2:969dfa4f2436 21 //Initialise kalman
joe4465 3:4823d6750629 22 _altitudeKalmanFilter = new Kalman(1, 1, 1, 0);
joe4465 3:4823d6750629 23 //_xPosKalmanFilter = new Kalman(0.1, 500, 1, 0);
joe4465 3:4823d6750629 24 //_yPosKalmanFilter = new Kalman(0.1, 500, 1, 0);*/
joe4465 2:969dfa4f2436 25
joe4465 2:969dfa4f2436 26 DEBUG("Sensors initialised\r\n");
joe4465 2:969dfa4f2436 27 }
joe4465 2:969dfa4f2436 28
joe4465 2:969dfa4f2436 29 Sensors::~Sensors(){}
joe4465 2:969dfa4f2436 30
joe4465 2:969dfa4f2436 31 void Sensors::update()
joe4465 2:969dfa4f2436 32 {
joe4465 2:969dfa4f2436 33 //Update GPS
joe4465 2:969dfa4f2436 34 updateGpsValues();
joe4465 2:969dfa4f2436 35
joe4465 2:969dfa4f2436 36 //Update Altitude
joe4465 2:969dfa4f2436 37 updateAltitude();
joe4465 2:969dfa4f2436 38
joe4465 3:4823d6750629 39 //Update Position
joe4465 2:969dfa4f2436 40 //updatePosition();
joe4465 2:969dfa4f2436 41 }
joe4465 2:969dfa4f2436 42
joe4465 2:969dfa4f2436 43 Imu::Rate Sensors::getRate()
joe4465 2:969dfa4f2436 44 {
joe4465 2:969dfa4f2436 45 return _rate;
joe4465 2:969dfa4f2436 46 }
joe4465 2:969dfa4f2436 47
joe4465 2:969dfa4f2436 48 Imu::Angle Sensors::getAngle()
joe4465 2:969dfa4f2436 49 {
joe4465 2:969dfa4f2436 50 return _angle;
joe4465 2:969dfa4f2436 51 }
joe4465 2:969dfa4f2436 52
joe4465 2:969dfa4f2436 53 Gps::Value Sensors::getGpsValues()
joe4465 2:969dfa4f2436 54 {
joe4465 3:4823d6750629 55 return _position.gpsPosition;
joe4465 2:969dfa4f2436 56 }
joe4465 2:969dfa4f2436 57
joe4465 2:969dfa4f2436 58 Sensors::Altitude Sensors::getAltitude()
joe4465 2:969dfa4f2436 59 {
joe4465 2:969dfa4f2436 60 return _altitude;
joe4465 2:969dfa4f2436 61 }
joe4465 2:969dfa4f2436 62
joe4465 2:969dfa4f2436 63 Sensors::Position Sensors::getPosition()
joe4465 2:969dfa4f2436 64 {
joe4465 2:969dfa4f2436 65 return _position;
joe4465 2:969dfa4f2436 66 }
joe4465 2:969dfa4f2436 67
joe4465 3:4823d6750629 68 Imu::Velocity Sensors::getImuVelocity()
joe4465 2:969dfa4f2436 69 {
joe4465 2:969dfa4f2436 70 return _imu->getVelocity();
joe4465 2:969dfa4f2436 71 }
joe4465 2:969dfa4f2436 72
joe4465 4:9ffbf9101992 73 Imu::Acceleration Sensors::getImuAcceleration()
joe4465 4:9ffbf9101992 74 {
joe4465 4:9ffbf9101992 75 return _imu->getAcceleration();
joe4465 4:9ffbf9101992 76 }
joe4465 4:9ffbf9101992 77
joe4465 2:969dfa4f2436 78 void Sensors::updateImu()
joe4465 2:969dfa4f2436 79 {
joe4465 2:969dfa4f2436 80 _angle = _imu->getAngle();
joe4465 2:969dfa4f2436 81 _rate = _imu->getRate();
joe4465 2:969dfa4f2436 82 }
joe4465 2:969dfa4f2436 83
joe4465 2:969dfa4f2436 84 void Sensors::updateGpsValues()
joe4465 2:969dfa4f2436 85 {
joe4465 3:4823d6750629 86 _position.gpsPosition = _gps->getValues();
joe4465 2:969dfa4f2436 87 }
joe4465 2:969dfa4f2436 88
joe4465 2:969dfa4f2436 89 void Sensors::updateAltitude()
joe4465 2:969dfa4f2436 90 {
joe4465 2:969dfa4f2436 91 _altitude.lidar = getLidarAltitude(); //cm
joe4465 2:969dfa4f2436 92 _altitude.barometer = _imu->getAltitude(); //cm
joe4465 2:969dfa4f2436 93 _altitude.gps = _gpsValues.altitude; //m
joe4465 2:969dfa4f2436 94
joe4465 3:4823d6750629 95 //Get IMU velocity
joe4465 2:969dfa4f2436 96 double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds
joe4465 3:4823d6750629 97 Imu::Velocity velocity = _imu->getVelocity(time);
joe4465 2:969dfa4f2436 98
joe4465 2:969dfa4f2436 99 //Compute predicted altitude
joe4465 3:4823d6750629 100 double predictedAltitude = _altitude.computed + velocity.z;
joe4465 2:969dfa4f2436 101
joe4465 2:969dfa4f2436 102 //Compute predicted change in altitude
joe4465 2:969dfa4f2436 103 double predictedChange = 1;
joe4465 2:969dfa4f2436 104 if(_altitude.computed != 0) predictedChange = predictedAltitude / _altitude.computed;
joe4465 2:969dfa4f2436 105
joe4465 2:969dfa4f2436 106 //Compute estimated altitude
joe4465 3:4823d6750629 107 float altitude = 0;
joe4465 4:9ffbf9101992 108 if(_altitude.computed < 20 * 100) altitude = _altitude.lidar;
joe4465 3:4823d6750629 109 else altitude = _altitude.barometer;
joe4465 3:4823d6750629 110 double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, altitude);
joe4465 2:969dfa4f2436 111
joe4465 2:969dfa4f2436 112 //Compute estimated velocity
joe4465 3:4823d6750629 113 velocity.z = estimatedAltitude - _altitude.computed;
joe4465 2:969dfa4f2436 114
joe4465 2:969dfa4f2436 115 //Reset IMU velocity to estimated velocity
joe4465 3:4823d6750629 116 _imu->setCurrentVelocity(velocity);
joe4465 2:969dfa4f2436 117
joe4465 2:969dfa4f2436 118 //Save new altitude
joe4465 2:969dfa4f2436 119 _altitude.computed = estimatedAltitude;
joe4465 2:969dfa4f2436 120
joe4465 2:969dfa4f2436 121 //printf("%1.6f, %1.6f, %1.6f\r\n", predictedAltitude, _altitude.lidar, _altitude.computed);
joe4465 2:969dfa4f2436 122 }
joe4465 2:969dfa4f2436 123
joe4465 2:969dfa4f2436 124
joe4465 2:969dfa4f2436 125 void Sensors::updatePosition()
joe4465 2:969dfa4f2436 126 {
joe4465 3:4823d6750629 127 /*
joe4465 3:4823d6750629 128 //Get IMU velocity
joe4465 3:4823d6750629 129 Imu::Velocity velocity = _imu->getVelocity();
joe4465 2:969dfa4f2436 130
joe4465 3:4823d6750629 131 //Compute predicted positions
joe4465 3:4823d6750629 132 double predictedXPos = _position.computedX + velocity.x;
joe4465 3:4823d6750629 133 double predictedYPos = _position.computedY + velocity.y;
joe4465 3:4823d6750629 134
joe4465 3:4823d6750629 135 //Compute predicted change in positions
joe4465 3:4823d6750629 136 double predictedXChange = 1;
joe4465 3:4823d6750629 137 double predictedYChange = 1;
joe4465 3:4823d6750629 138 if(_position.computedX != 0) predictedXChange = predictedXPos / _position.computedX;
joe4465 3:4823d6750629 139 if(_position.computedY != 0) predictedYChange = predictedYPos / _position.computedY;
joe4465 3:4823d6750629 140
joe4465 3:4823d6750629 141 //Calc difference between current and starting GPS location
joe4465 3:4823d6750629 142 //Assume pointed north for now
joe4465 3:4823d6750629 143 Gps::Difference difference = _gps->getDifference(_startingPosition.gpsPosition, _position.gpsPosition);
joe4465 3:4823d6750629 144
joe4465 3:4823d6750629 145 //printf("x vel %f, x diff %f, ", velocity.x, difference.x);
joe4465 3:4823d6750629 146 //printf("y vel %f, y diff %f, ", velocity.y, difference.y);
joe4465 3:4823d6750629 147
joe4465 3:4823d6750629 148 //Compute estimated position
joe4465 3:4823d6750629 149 double estimatedXPos = _xPosKalmanFilter->update(predictedXChange, difference.x);
joe4465 3:4823d6750629 150 double estimatedYPos = _yPosKalmanFilter->update(predictedYChange, difference.y);
joe4465 3:4823d6750629 151
joe4465 3:4823d6750629 152 //Compute estimated velocity
joe4465 3:4823d6750629 153 velocity.x = estimatedXPos - _position.computedX;
joe4465 3:4823d6750629 154 velocity.y = estimatedYPos - _position.computedY;
joe4465 3:4823d6750629 155
joe4465 3:4823d6750629 156 //Reset IMU velocity to estimated velocity
joe4465 3:4823d6750629 157 _imu->setCurrentVelocity(velocity);
joe4465 3:4823d6750629 158
joe4465 3:4823d6750629 159 //Save new position
joe4465 3:4823d6750629 160 _position.computedX = estimatedXPos;
joe4465 3:4823d6750629 161 _position.computedY = estimatedYPos;
joe4465 3:4823d6750629 162
joe4465 3:4823d6750629 163 printf("Pos X %1.6f Pos Y %1.6f\r\n", _position.computedX, _position.computedY);*/
joe4465 2:969dfa4f2436 164 }
joe4465 2:969dfa4f2436 165
joe4465 2:969dfa4f2436 166 void Sensors::zeroGyro()
joe4465 2:969dfa4f2436 167 {
joe4465 2:969dfa4f2436 168 _imu->zeroGyro();
joe4465 2:969dfa4f2436 169 }
joe4465 2:969dfa4f2436 170
joe4465 2:969dfa4f2436 171 void Sensors::zeroBarometer()
joe4465 2:969dfa4f2436 172 {
joe4465 2:969dfa4f2436 173 _imu->zeroBarometer();
joe4465 2:969dfa4f2436 174 }
joe4465 2:969dfa4f2436 175
joe4465 3:4823d6750629 176 void Sensors::zeroPos()
joe4465 3:4823d6750629 177 {
joe4465 3:4823d6750629 178 _startingPosition.gpsPosition.latitude = getGpsValues().latitude;
joe4465 3:4823d6750629 179 _startingPosition.gpsPosition.longitude = getGpsValues().longitude;
joe4465 3:4823d6750629 180
joe4465 3:4823d6750629 181 //Get IMU velocity
joe4465 3:4823d6750629 182 Imu::Velocity velocity = _imu->getVelocity();
joe4465 3:4823d6750629 183
joe4465 3:4823d6750629 184 //Set x y to 0
joe4465 3:4823d6750629 185 velocity.x = 0;
joe4465 3:4823d6750629 186 velocity.y = 0;
joe4465 3:4823d6750629 187
joe4465 3:4823d6750629 188 //Reset IMU velocity to estimated velocity
joe4465 3:4823d6750629 189 _imu->setCurrentVelocity(velocity);
joe4465 3:4823d6750629 190 }
joe4465 3:4823d6750629 191
joe4465 2:969dfa4f2436 192 void Sensors::enable(bool enable)
joe4465 2:969dfa4f2436 193 {
joe4465 2:969dfa4f2436 194 _imu->enable(enable);
joe4465 2:969dfa4f2436 195 }
joe4465 2:969dfa4f2436 196
joe4465 2:969dfa4f2436 197 int Sensors::getLidarAltitude()
joe4465 2:969dfa4f2436 198 {
joe4465 2:969dfa4f2436 199 Imu::Angle angles = getAngle();
joe4465 2:969dfa4f2436 200 int rawAltitude = _lidar->read();
joe4465 2:969dfa4f2436 201
joe4465 3:4823d6750629 202 float correctedAltitude = rawAltitude * cos(toRadian(angles.roll)) * cos(toRadian(-angles.pitch));
joe4465 3:4823d6750629 203 /*
joe4465 2:969dfa4f2436 204 double oppAng = 0;
joe4465 2:969dfa4f2436 205 double multiplier = 0;
joe4465 2:969dfa4f2436 206 int pitchAltitude = 0;
joe4465 2:969dfa4f2436 207 int rollAltitude = 0;
joe4465 2:969dfa4f2436 208
joe4465 2:969dfa4f2436 209 //Calulate pitch altitude
joe4465 2:969dfa4f2436 210 oppAng = 90 - abs(angles.pitch);
joe4465 2:969dfa4f2436 211 multiplier = sin(oppAng*PI/180);
joe4465 2:969dfa4f2436 212 pitchAltitude = multiplier * rawAltitude;
joe4465 2:969dfa4f2436 213
joe4465 2:969dfa4f2436 214 //Calulate roll altitude
joe4465 2:969dfa4f2436 215 oppAng = 90 - abs(angles.roll);
joe4465 2:969dfa4f2436 216 multiplier = sin(oppAng*PI/180);
joe4465 2:969dfa4f2436 217 rollAltitude = multiplier * rawAltitude;
joe4465 2:969dfa4f2436 218
joe4465 3:4823d6750629 219 return (pitchAltitude + rollAltitude)/ 2;*/
joe4465 3:4823d6750629 220 return correctedAltitude;
joe4465 2:969dfa4f2436 221 }
joe4465 2:969dfa4f2436 222
joe4465 2:969dfa4f2436 223 void Sensors::zeroAccel()
joe4465 2:969dfa4f2436 224 {
joe4465 2:969dfa4f2436 225 _imu->zeroAccel();
joe4465 3:4823d6750629 226 }
joe4465 3:4823d6750629 227
joe4465 3:4823d6750629 228 double Sensors::toRadian(double deg)
joe4465 3:4823d6750629 229 {
joe4465 3:4823d6750629 230 return (deg * PI / 180);
joe4465 2:969dfa4f2436 231 }