Diff: NavigationController/NavigationController.cpp

Revision:

3:4823d6750629

Parent:

2:969dfa4f2436

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