Diff: NavigationController/AltitudeController/AltitudeController.cpp

Revision:

3:4823d6750629

Parent:

2:969dfa4f2436

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