Revision 3:4823d6750629, committed 2015-04-24

Comitter:

joe4465

Date:

Fri Apr 24 16:50:20 2015 +0000

Parent:

2:969dfa4f2436

Child:

4:9ffbf9101992

Commit message:

Altitude mode working.

Changed in this revision

--- a/BaseStation/BaseStation.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/BaseStation/BaseStation.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -139,16 +139,22 @@
         }
         else if (mode == Status::VELOCITY)
         {
-            //Sensors::Velocity velocity = _sensors.getVelocity();
-            //_wireless->printf("<AVelX=%1.2f:AVelY=%1.2f:AVelZ=%1.2f:LVel=%1.2f:CVelX=%1.2f:CVelY=%1.2f:CVelZ=%1.2f>",
-            //velocity.accelX, velocity.accelY, velocity.accelZ, velocity.lidar, velocity.computedX, velocity.computedY, velocity.computedZ);      
+            Imu::Velocity velocity = _sensors.getImuVelocity();
+            _wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>",
+            velocity.x, velocity.y, velocity.z);      
         }
         else if (mode == Status::ALTITUDE_STATUS)
         {
             NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
             Sensors::Altitude altitude = _sensors.getAltitude();
-            _wireless->printf("<ACR=%1.2f:AT=%1.2f:ATHR=%1.2f:LAlt=%1.2f>",
-            setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed);      
+            _wireless->printf("<ACR=%1.2f:ATA=%1.2f:ATHR=%1.2f:CAlt=%1.2f:ZVEL=%1.2f>",
+            setPoints.climbRate, setPoints.targetAltitude, (setPoints.throttle * 100), altitude.computed, _sensors.getImuVelocity().z);      
+        }
+        else if (mode == Status::LIDAR)
+        {
+            Sensors::Altitude altitude = _sensors.getAltitude();
+            Imu::Angle angle = _sensors.getAngle();         
+            _wireless->printf("<SP=%1.2f:SR=%1.2f:LAlt=%1.2f>", angle.pitch, angle.roll, altitude.lidar);   
         }
         
         //Check for wireless serial command

--- a/Global/Global.h	Wed Apr 01 11:19:21 2015 +0000
+++ b/Global/Global.h	Fri Apr 24 16:50:20 2015 +0000
@@ -3,15 +3,14 @@
 #ifndef GLOBAL_H
 #define GLOBAL_H
 
-#if 1
-  #define DEBUG(...) printf(__VA_ARGS__)
-#else
-  #define DEBUG(a) (void)0
-#endif
+#define DEBUG(...) printf(__VA_ARGS__)
+//#define DEBUG(a) (void)0
+
 
 #define MOTORS_ENABLED
 
 #define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a))
+#define PI 3.14159265
 
 #define             IMU_YAW_ANGLE_MAX 180
 #define             IMU_YAW_ANGLE_MIN -180
@@ -45,6 +44,7 @@
 #define             RC_THRUST_MAX 1
 #define             RC_THRUST_MIN 0
 #define             RC_DEAD_ZONE 0.1
+#define             RC_HOVER 0.6
 
 #define             MOTORS_OFF 0
 #define             MOTORS_ARMED 1060
@@ -56,8 +56,8 @@
 
 #define             ALTITUDE_MIN 0
 #define             ALTITUDE_MAX 1000
-#define             MAX_CLIMB_RATE 25
-#define             MIN_CLIMB_RATE -25
+#define             MAX_CLIMB_RATE 2.5
+#define             MIN_CLIMB_RATE -2.5
 
 #define             FLIGHT_CONTROLLER_FREQUENCY 500
 

--- a/Global/filter.lib	Wed Apr 01 11:19:21 2015 +0000
+++ b/Global/filter.lib	Fri Apr 24 16:50:20 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/networker/code/filter/#9ce370b360ba
+http://developer.mbed.org/users/joe4465/code/filter/#9ce370b360ba

--- a/NavigationController/AltitudeController/AltitudeController.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/NavigationController/AltitudeController/AltitudeController.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -5,9 +5,9 @@
 {
     double updateTime = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.0));
     _altitudeRatePidController.initialise(_configFileWrapper.getAltitudeRateParameters(), MIN_CLIMB_RATE, MAX_CLIMB_RATE, (RC_THRUST_MIN + 0.2), (RC_THRUST_MAX - 0.2), updateTime);
-    _altitudeRatePidController.setBias(0.6); // Hover throttle
+    _altitudeRatePidController.setBias(RC_HOVER);
     _altitudeStabPidController.initialise(_configFileWrapper.getAltitudeStabParameters(), ALTITUDE_MIN, ALTITUDE_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE, updateTime );
-    
+    _altitudeStabPidController.setBias(RC_HOVER);
     //_setPoints = new NavigationController::SetPoint();
     
     DEBUG("Altitude controller initialised\r\n");
@@ -18,14 +18,14 @@
 double AltitudeController::compute(double targetAltitude, double climbRate)
 {   
     _altitude = _sensors.getAltitude();
-    float velocity = _sensors.getZVelocity();
+    Imu::Velocity velocity = _sensors.getImuVelocity();
     
     float altitudeStabPidOutput = _altitudeStabPidController.compute(targetAltitude, _altitude.computed);
     
     //If pilot commanding climb rate
     if(_status.getDeadZone() == false) altitudeStabPidOutput = climbRate; //Feed to rate PID (overwriting stab PID output)
     
-    float altitudeRatePidOutput = _altitudeRatePidController.compute(altitudeStabPidOutput, velocity);
+    float altitudeRatePidOutput = _altitudeRatePidController.compute(altitudeStabPidOutput, velocity.z);
     
     return altitudeRatePidOutput;
 }

--- a/NavigationController/NavigationController.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/NavigationController/NavigationController.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -35,7 +35,7 @@
         //Get Rc commands
         _mappedRc = _rc.getMappedRc();
         
-        //Get angle
+        //Get angle to calculate yaw
         _angle = _sensors.getAngle();
         
         //Reset accel data if not flying
@@ -43,6 +43,9 @@
         {
             //Reset accel
             _sensors.zeroAccel();
+            
+            //Reset Gps
+            _sensors.zeroPos();
         }
         
         //Update yaw target
@@ -61,10 +64,15 @@
                 else _status.setMotorsSpinning(false);
                 
                 //Update target altitude 
+                _setPoints.climbRate = 0;
                 updateAltitudeTarget();
             }
             else if(_navigationMode == Status::ALTITUDE_HOLD)
             {
+                //Motors are directly controlled by rc remote
+                //if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
+                //else _status.setMotorsSpinning(false);
+                
                 //Check if throttle is in dead zone
                 if(_status.getDeadZone() == true) _setPoints.climbRate = 0;
                 else
@@ -72,13 +80,35 @@
                     //Throttle not in dead zone so map to climb rate
                     _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE);
                     
+                    //float target = _setPoints.targetAltitude + (_setPoints.climbRate * 0.02);
+                    
                     //Update target altitude
                     updateAltitudeTarget();
                 }
                 
-                //Get PID output
-                _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
+                //If altitude is less than 10cm the directly map the rc throttle stick to the throttle
+                //else use the throttle from the altitude PID controller
+                if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
+                else _status.setMotorsSpinning(false);
             }
+           /* else if(_navigationMode == Status::AUTO_LAND)
+            {
+                //Motors are directly controlled by rc remote
+                if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
+                else _status.setMotorsSpinning(false);
+                
+                //Get altitude
+                _altitude = _sensors.getAltitude();
+                
+                if(_altitude.computed > 1000) _setPoints.targetAltitude = 300;
+                else if(_altitude.computed < 600) _setPoints.targetAltitude = 100;
+                else if(_altitude.computed < 300) _setPoints.targetAltitude = 0;
+                
+                //If altitude is less than 10 the directly map the rc throttle stick to the throttle
+                //else use the throttle from the altitude PID controller
+                if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
+                else _altitudeHoldPidOutput = _mappedRc.throttle;
+            }*/
             
             if(_state == Status::STANDBY)  
             {
@@ -148,7 +178,6 @@
 {
     _altitude = _sensors.getAltitude();
     _setPoints.targetAltitude = _altitude.computed;
-    
     if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN;
     else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX;
 }

--- a/Rc/Rc.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/Rc/Rc.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -56,8 +56,8 @@
             //Altitude mode
             if(_status.getNavigationMode() == Status::ALTITUDE_HOLD)
             {
-                float maxDeadZone = ((float)RC_THRUST_MAX / 2.0) + (float)RC_DEAD_ZONE;
-                float minDeadZone = ((float)RC_THRUST_MAX / 2.0)  - (float)RC_DEAD_ZONE;
+                float maxDeadZone = (float)RC_HOVER + (float)RC_DEAD_ZONE;
+                float minDeadZone = (float)RC_HOVER  - (float)RC_DEAD_ZONE;
                 if((minDeadZone < mappedRc.throttle) && (mappedRc.throttle <= maxDeadZone)) _status.setDeadZone(true);
                 else _status.setDeadZone(false);
             }

--- a/Sensors/Gps/Gps.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Gps/Gps.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -29,4 +29,49 @@
     }
 
     return _values;
+}
+
+Gps::Difference Gps::getDifference(Value latLong1, Value latLong2)
+{
+    int R = 637100000; //cm
+    float dLat = deg2rad(latLong2.latitude - latLong1.latitude);
+    float dLon = deg2rad(latLong2.longitude - latLong1.longitude);
+    latLong1.latitude = deg2rad(latLong1.latitude);
+    latLong2.latitude = deg2rad(latLong2.latitude);
+    
+    float a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(latLong1.latitude) * cos(latLong2.latitude); 
+    float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
+    float d = R * c;
+    
+    float y = sin(dLon) * cos(latLong2.latitude);
+    float x = cos(latLong1.latitude) * sin(latLong2.latitude) - sin(latLong1.latitude) * cos(latLong2.latitude) * cos(dLat);
+    float brng = rad2deg(atan2(y, x));
+    
+    float oppAng = 90 - brng;
+    float xDiff = d * sin(deg2rad(brng));
+    float yDiff = d * sin(deg2rad(oppAng));
+    
+    //printf("d %f, opp %f, diff %f \r\n", d, oppAng, yDiff);
+    //printf("lat1 %f long1 %f lat2 %f long2 %f ", latLong1.latitude, latLong1.longitude, latLong2.latitude, latLong2.longitude);
+    //printf(" Bearing %f", brng);
+    //printf(" Distance %f", d);
+    //printf(" X %f, Y %f\r\n", xDiff, yDiff);   
+    
+    Difference difference = Difference();
+    difference.x = xDiff;
+    difference.y = yDiff;
+    difference.bearing = brng;
+    difference.distance = d;
+    
+    return difference;  
+}
+
+double Gps::deg2rad(double deg)
+{
+    return (deg * PI / 180.0);
+}
+
+double Gps::rad2deg(double rad) 
+{
+    return (rad * 180.0 / PI);
 }
\ No newline at end of file

--- a/Sensors/Gps/Gps.h	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Gps/Gps.h	Fri Apr 24 16:50:20 2015 +0000
@@ -20,12 +20,23 @@
         bool fix;
     };
     
+    struct Difference
+    {
+        double x;
+        double y;
+        double bearing;
+        double distance;
+    };
+    
     Value getValues();
+    Difference getDifference(Value latLong1, Value latLong2);
     
   private:
     MODSERIAL *_gps;
     TinyGPS _tinyGPS;
     Value _values;
+    double deg2rad(double deg);
+    double rad2deg(double rad);
 };
 
 #endif
\ No newline at end of file

--- a/Sensors/Imu/FreeIMU_external_magnetometer.lib	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Imu/FreeIMU_external_magnetometer.lib	Fri Apr 24 16:50:20 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/joe4465/code/FreeIMU_external_magnetometer/#0065ffbcd39b
+http://developer.mbed.org/users/joe4465/code/FreeIMU_external_magnetometer/#ab5318ef31c4

--- a/Sensors/Imu/Imu.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Imu/Imu.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -4,6 +4,7 @@
 {
     _rate = Rate();
     _angle = Angle();
+    _velocity = Velocity();
     
     _accelZeroPitch = _configFileWrapper.getAccelZeroPitch();
     _accelZeroRoll = _configFileWrapper.getAccelZeroRoll();
@@ -13,13 +14,14 @@
     Thread::wait(500);
     
     _barometerZeroFilter = new filter(100);
-    _barometerFilter = new filter(5);
-    
+    _barometerFilter = new filter(50);
     _barometerZero = 0;
     zeroBarometer();
     
-    _velocity = 0;
-    _kalmanFilter = new Kalman(0.1, 32, 1, 0);
+    
+    _kalmanXVelFilter = new Kalman(0.1, 32, 1, 0);
+    _kalmanYVelFilter = new Kalman(0.1, 32, 1, 0);
+    _kalmanZVelFilter = new Kalman(0.1, 32, 1, 0);
     
     DEBUG("IMU initialised\r\n");
 }
@@ -47,17 +49,20 @@
 
 Imu::Angle Imu::getAngle(bool bias)
 {
-    //Filter Z accel data
+    //Get raw accel data
     float values[9];
     _freeImu.getValues(values);
-    _kalmanFilter->update(1, values[2]);
     
-    //printf("%1.6f, %1.6f, %1.6f\n", values[0], values[1], values[2]);
+    //Update kalman filter with raw accel data
+    _kalmanXVelFilter->update(1, values[0]);
+    _kalmanYVelFilter->update(1, values[1]);
+    _kalmanZVelFilter->update(1, values[2]);
     
     //Get angle
     float angle[3];
     _freeImu.getYawPitchRoll(angle);
     
+    //Swap orientation
     float yaw = -angle[0];
     float pitch = angle[2];
     float roll = -angle[1];
@@ -88,30 +93,38 @@
     return (normalAltitude * 100);
 }
 
-float Imu::getVelocity(float time)
+Imu::Velocity Imu::getVelocity(float time)
 {
-    //Get quat
+    //Get quaternion
     float q[4];
     _freeImu.getQ(q);
 
     //Extract accelerometer data
     float acc[3];
-    acc[0]= 0; //x
-    acc[1]= 0; //y
-    acc[2]= _kalmanFilter->getEstimated();
+    acc[0]= _kalmanXVelFilter->getEstimated();
+    acc[1]= _kalmanYVelFilter->getEstimated();
+    acc[2]= _kalmanZVelFilter->getEstimated();
     
     //Gravity compensate
     _freeImu.gravityCompensateAcc(acc, q);
     
+    //Convert acceleration to velocity
+    float xAcceleration = 0;
+    if(acc[0] < -0.01 ||  acc[0] > 0.01) xAcceleration = acc[0] * 9.8 * 100;
+    _velocity.x += xAcceleration * time;
+    
+    float yAcceleration = 0;
+    if(acc[1] < -0.01 ||  acc[1] > 0.01) yAcceleration = acc[1] * 9.8 * 100;
+    _velocity.y += yAcceleration * time;
+    
     float zAcceleration = 0;
     if(acc[2] < -0.01 ||  acc[2] > 0.01) zAcceleration = acc[2] * 9.8 * 100;
-    
-    _velocity += zAcceleration * time;
+    _velocity.z += zAcceleration * time;
     
     return _velocity; //cm/s/s
 }
 
-float Imu::getVelocity()
+Imu::Velocity Imu::getVelocity()
 {
     return _velocity; //cm/s/s
 }
@@ -121,14 +134,17 @@
     _freeImu.zeroGyro();
 }
 
-void Imu::setCurrentVelocity(float velocity)
+void Imu::setCurrentVelocity(Velocity velocity)
 {
     _velocity = velocity;
 }
 
 void Imu::zeroBarometer()
 {
-    for(int i = 0; i < 100; i++)
+    //DEBUG("About to start Barometer zero\r\n");
+    //Thread::wait(5000);
+    
+    for(int i = 0; i < 1000; i++)
     {
         _barometerZero = _barometerFilter->process(_freeImu.getBaroAlt());
     }

--- a/Sensors/Imu/Imu.h	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Imu/Imu.h	Fri Apr 24 16:50:20 2015 +0000
@@ -39,14 +39,14 @@
     
     Rate getRate();
     Angle getAngle(bool bias = true);
-    float getVelocity(float time);
-    float getVelocity();
+    Velocity getVelocity(float time);
+    Velocity getVelocity();
     double getAltitude();
     
     void zeroGyro();
     void zeroBarometer();
     void zeroAccel();
-    void setCurrentVelocity(float velocity);
+    void setCurrentVelocity(Velocity velocity);
     
   private:
     FreeIMU _freeImu;
@@ -54,12 +54,14 @@
     filter* _barometerFilter;
     Rate _rate;
     Angle _angle;
-    float _velocity;
+    Velocity _velocity;
     float _barometerZero;
     ConfigFileWrapper& _configFileWrapper;
     float _accelZeroPitch;
     float _accelZeroRoll;
-    Kalman* _kalmanFilter;
+    Kalman* _kalmanXVelFilter;
+    Kalman* _kalmanYVelFilter;
+    Kalman* _kalmanZVelFilter;
 };
 
 #endif
\ No newline at end of file

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

--- a/Sensors/Sensors.h	Wed Apr 01 11:19:21 2015 +0000
+++ b/Sensors/Sensors.h	Fri Apr 24 16:50:20 2015 +0000
@@ -6,11 +6,14 @@
 #include "Status.h"
 #include "LidarLitePwm.h"
 #include "Kalman.h"
+#include <math.h>
 
 #ifndef Sensors_H
 #define Sensors_H
 
-#define PI 3.14159265
+//#define PI 3.14159265
+
+using namespace std;
 
 class Sensors                
 {
@@ -20,11 +23,7 @@
     
     struct Position
     {
-        double accelX;
-        double accelY;
-        double accelZ;
-        double latitude;
-        double longitude;
+        Gps::Value gpsPosition;
         double computedX;
         double computedY;  
     };
@@ -41,13 +40,14 @@
     Imu::Rate getRate();
     Imu::Angle getAngle();
     Gps::Value getGpsValues();
-    double getZVelocity();
+    Imu::Velocity getImuVelocity();
     Sensors::Altitude getAltitude();
     Sensors::Position getPosition();
     void enable(bool enable);
     void zeroBarometer();
     void updateImu();
     void zeroAccel();
+    void zeroPos();
     
   private:
     void zeroGyro();
@@ -56,17 +56,21 @@
     void updateVelocity();
     void updatePosition();
     int getLidarAltitude();
+    double toRadian(double deg);
     
     Status& _status;
     Imu::Rate _rate;
     Imu::Angle _angle;
     Position _position;
+    Position _startingPosition;
     Altitude _altitude;
     Gps::Value _gpsValues;
     Imu* _imu;
     Gps* _gps;
     LidarLitePwm* _lidar;
     Kalman* _altitudeKalmanFilter;
+    Kalman* _xPosKalmanFilter;
+    Kalman* _yPosKalmanFilter;
 };
 
 #endif
\ No newline at end of file

--- a/Status/Status.cpp	Wed Apr 01 11:19:21 2015 +0000
+++ b/Status/Status.cpp	Fri Apr 24 16:50:20 2015 +0000
@@ -194,7 +194,7 @@
             DEBUG("Armed set to %d\r\n", _armed);
             return true;
         }
-        else if (armed == true && _navigationMode == Status::ALTITUDE_HOLD && getMotorsSpinning() == false && getDeadZone() == true)
+        else if (armed == true && _navigationMode == Status::ALTITUDE_HOLD && getMotorsSpinning() == false)
         {
             _armed = armed;
             DEBUG("Armed set to %d\r\n", _armed);

--- a/Status/Status.h	Wed Apr 01 11:19:21 2015 +0000
+++ b/Status/Status.h	Fri Apr 24 16:50:20 2015 +0000
@@ -49,7 +49,8 @@
         STAB_TUNING,
         ALTITUDE,
         VELOCITY,
-        ALTITUDE_STATUS
+        ALTITUDE_STATUS,
+        LIDAR
     };
     
     bool update();