Diff: Sensors/Sensors.cpp

Revision:

2:969dfa4f2436

Parent:

0:c6a85bb2a827

Child:

3:4823d6750629

--- 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