Diff: Sensors/Sensors.cpp

Revision:

3:4823d6750629

Parent:

2:969dfa4f2436

Child:

4:9ffbf9101992

--- a/Sensors/Sensors.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Sensors.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -4,6 +4,7 @@
 {
     _gpsValues = Gps::Value();
     _position = Position();
+    _startingPosition = Position();
     _altitude = Altitude();
     
     //Initialise IMU
@@ -16,8 +17,11 @@
     //Initialise Lidar
     _lidar = new LidarLitePwm(lidarInterrupt);
     
+    
     //Initialise kalman
-    _altitudeKalmanFilter = new Kalman(0.75, 2, 1, 0);
+    _altitudeKalmanFilter = new Kalman(1, 1, 1, 0);
+    //_xPosKalmanFilter = new Kalman(0.1, 500, 1, 0);
+    //_yPosKalmanFilter = new Kalman(0.1, 500, 1, 0);*/
     
     DEBUG("Sensors initialised\r\n");
 }
@@ -32,6 +36,7 @@
     //Update Altitude
     updateAltitude();
     
+    //Update Position
     //updatePosition();
 }
 
@@ -47,7 +52,7 @@
 
 Gps::Value Sensors::getGpsValues()
 {
-    return _gpsValues;
+    return _position.gpsPosition;
 }
 
 Sensors::Altitude Sensors::getAltitude()
@@ -60,7 +65,7 @@
     return _position;   
 }
 
-double Sensors::getZVelocity()
+Imu::Velocity Sensors::getImuVelocity()
 {
     return _imu->getVelocity();
 }
@@ -69,13 +74,11 @@
 {
     _angle = _imu->getAngle();
     _rate = _imu->getRate();
-    
-    return;
 }
 
 void Sensors::updateGpsValues()
 {
-    _gpsValues = _gps->getValues();
+    _position.gpsPosition = _gps->getValues();
 }
 
 void Sensors::updateAltitude()
@@ -84,25 +87,28 @@
     _altitude.barometer = _imu->getAltitude(); //cm
     _altitude.gps = _gpsValues.altitude; //m
     
-    //Update IMU velocity
+    //Get IMU velocity
     double time = (1.0 / ((double)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // In seconds
-    double velocity = _imu->getVelocity(time);
+    Imu::Velocity velocity = _imu->getVelocity(time);
     
     //Compute predicted altitude
-    double predictedAltitude = _altitude.computed + velocity;
+    double predictedAltitude = _altitude.computed + velocity.z;
     
     //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);
+    float altitude = 0;
+    if(altitude.computed < 20 * 100) altitude = _altitude.lidar;
+    else altitude = _altitude.barometer;
+    double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, altitude);
     
     //Compute estimated velocity
-    double estimatedVelocity = estimatedAltitude - _altitude.computed;
+    velocity.z = estimatedAltitude - _altitude.computed;
     
     //Reset IMU velocity to estimated velocity
-    _imu->setCurrentVelocity(estimatedVelocity);
+    _imu->setCurrentVelocity(velocity);
     
     //Save new altitude
     _altitude.computed = estimatedAltitude;
@@ -113,7 +119,43 @@
 
 void Sensors::updatePosition()
 {
+    /*
+    //Get IMU velocity
+    Imu::Velocity velocity = _imu->getVelocity();
     
+    //Compute predicted positions
+    double predictedXPos = _position.computedX + velocity.x;
+    double predictedYPos = _position.computedY + velocity.y;
+    
+    //Compute predicted change in positions
+    double predictedXChange = 1;
+    double predictedYChange = 1;
+    if(_position.computedX != 0) predictedXChange = predictedXPos / _position.computedX;
+    if(_position.computedY != 0) predictedYChange = predictedYPos / _position.computedY;
+    
+    //Calc difference between current and starting GPS location
+    //Assume pointed north for now
+    Gps::Difference difference = _gps->getDifference(_startingPosition.gpsPosition, _position.gpsPosition);
+    
+    //printf("x vel %f, x diff %f, ", velocity.x, difference.x);
+    //printf("y vel %f, y diff %f, ", velocity.y, difference.y);
+    
+    //Compute estimated position
+    double estimatedXPos = _xPosKalmanFilter->update(predictedXChange, difference.x);
+    double estimatedYPos = _yPosKalmanFilter->update(predictedYChange, difference.y);
+    
+    //Compute estimated velocity
+    velocity.x = estimatedXPos - _position.computedX;
+    velocity.y = estimatedYPos - _position.computedY;
+    
+    //Reset IMU velocity to estimated velocity
+    _imu->setCurrentVelocity(velocity);
+    
+    //Save new position
+    _position.computedX = estimatedXPos;
+    _position.computedY = estimatedYPos;
+    
+    printf("Pos X %1.6f Pos Y %1.6f\r\n", _position.computedX, _position.computedY);*/
 }
 
 void Sensors::zeroGyro()
@@ -126,6 +168,22 @@
     _imu->zeroBarometer();
 }
 
+void Sensors::zeroPos()
+{
+    _startingPosition.gpsPosition.latitude = getGpsValues().latitude;
+    _startingPosition.gpsPosition.longitude = getGpsValues().longitude;
+    
+    //Get IMU velocity
+    Imu::Velocity velocity = _imu->getVelocity();
+    
+    //Set x y to 0
+    velocity.x = 0;
+    velocity.y = 0;
+ 
+   //Reset IMU velocity to estimated velocity
+    _imu->setCurrentVelocity(velocity);   
+}
+
 void Sensors::enable(bool enable)
 {
     _imu->enable(enable);
@@ -136,6 +194,8 @@
     Imu::Angle angles = getAngle();
     int rawAltitude = _lidar->read();
     
+    float correctedAltitude = rawAltitude * cos(toRadian(angles.roll)) * cos(toRadian(-angles.pitch));  
+    /*
     double oppAng = 0;
     double multiplier = 0;  
     int pitchAltitude = 0;
@@ -151,10 +211,16 @@
     multiplier = sin(oppAng*PI/180);
     rollAltitude = multiplier * rawAltitude;
     
-    return (pitchAltitude + rollAltitude)/ 2;
+    return (pitchAltitude + rollAltitude)/ 2;*/
+    return correctedAltitude;
 }
 
 void Sensors::zeroAccel()
 {
     _imu->zeroAccel();
+}
+
+double Sensors::toRadian(double deg)
+{
+    return (deg * PI / 180);
 }
\ No newline at end of file