![](/media/cache/profiles/8158fa3e4de806e614f7ff02e7b22fde.jpg.50x50_q85.png)
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?
User | Revision | Line number | New 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 | } |