Some altitude hold improvement merge into my work

Kenny9999 · Jan 29, 2012 · 8d6b024 · 8d6b024
1 parent 4d1ac6a
commit 8d6b024
Showing 1 changed file with 45 additions and 3 deletions.
diff --git a/AeroQuad/AltitudeControlProcessor.h b/AeroQuad/AltitudeControlProcessor.h
@@ -33,6 +33,24 @@
 #ifndef _AQ_ALTITUDE_CONTROL_PROCESSOR_H_
 #define _AQ_ALTITUDE_CONTROL_PROCESSOR_H_
 
+//void computeEstimatedAltitude(float currentSensorAltitude) {
+//
+//  float altitudeError = currentSensorAltitude - estimatedAltitude;
+//  altitudeIntegratedError += altitudeError;
+//  altitudeIntegratedError = constrain(altitudeIntegratedError,-0.5,0.5);
+//  
+//  // Gravity vector correction and projection to the local Z
+//  float zVelocity = ((filteredAccel[ZAXIS] * (1 - accelOneG * invSqrt(isq(filteredAccel[XAXIS]) + isq(filteredAccel[YAXIS]) + isq(filteredAccel[ZAXIS])))) / 10) + altitudeIntegratedError;
+//  
+//  float altitudeDelta = (zVelocity * G_Dt) + (altitudeError * G_Dt);
+//  estimatedAltitude = ((estimatedZVelocity + altitudeDelta) * G_Dt) +  (altitudeError * G_Dt);
+//  estimatedZVelocity += zVelocity;
+//}
+
+#define BARO 0
+#define SONAR 1
+byte sensorRead = BARO;
+
 /**
  * getAltitudeFromSensors
  *
@@ -47,10 +65,12 @@
   float getAltitudeFromSensors() {
 
     if (rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] != INVALID_ALTITUDE) {
+      sensorRead = SONAR;
       baroGroundAltitude = baroRawAltitude - rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX];  
       return (rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX]); 
     }
     else {
+      sensorRead = BARO;
       return getBaroAltitude();    
     }
   }
@@ -61,6 +81,7 @@
    * @return the baro altitude
    */
   float getAltitudeFromSensors() {
+    sensorRead = BARO;
     return getBaroAltitude();
   }
 
@@ -69,6 +90,7 @@
    * @return the sonar altitude
    */
   float getAltitudeFromSensors() {
+    sensorRead = SONAR;
     return (rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX]);
   }
 
@@ -95,9 +117,30 @@ void processAltitudeHold()
         throttle = receiverCommand[THROTTLE];
         return;
       }
-
+ 
       int altitudeHoldThrottleCorrection = updatePID(altitudeToHoldTarget, currentSensorAltitude, &PID[ALTITUDE_HOLD_PID_IDX]);
       altitudeHoldThrottleCorrection = constrain(altitudeHoldThrottleCorrection, minThrottleAdjust, maxThrottleAdjust);
+
+      /////////// try to prevent any movement on the z axis
+      float zVelocity = (filteredAccel[ZAXIS] * (1 - accelOneG * invSqrt(isq(filteredAccel[XAXIS]) + isq(filteredAccel[YAXIS]) + isq(filteredAccel[ZAXIS])))) - runTimeAccelBias[ZAXIS];
+      int accelVelocityThrottleCorrection = 0;
+//      if (!isSwitched(altitudeHoldThrottleCorrection,zVelocity)) { // only used to slowed down decent or grow up
+        accelVelocityThrottleCorrection = constrain(zVelocity*10, minThrottleAdjust*1.2, maxThrottleAdjust*1.2);  // 10 is gain, no PID
+//        accelVelocityThrottleCorrection = zVelocity*20;  // 10 is gain, no PID
+//      }
+
+      //////////// use previous altitude to compute a kind of z velocity to stabilize altitude variation
+      float sensorZVelocity = currentSensorAltitude - oldSensorAltitude;
+      float estimatedZVelocity = filterSmooth(sensorZVelocity, estimatedZVelocity, 1);
+      oldSensorAltitude = currentSensorAltitude;
+
+      int throttleVelocityCorrection = updatePID(0.0, estimatedZVelocity, &PID[ZDAMPENING_PID_IDX]);
+      if (sensorRead == SONAR) { // if sonar, we can easily multiply by 2 with the senros precision
+        throttleVelocityCorrection *= 2;
+      }
+      throttleVelocityCorrection = constrain(throttleVelocityCorrection, minThrottleAdjust*0.8, maxThrottleAdjust*0.8);
+      ///////////////
+
       if (abs(altitudeHoldThrottle - receiverCommand[THROTTLE]) > altitudeHoldPanicStickMovement) {
         altitudeHoldState = ALTPANIC; // too rapid of stick movement so PANIC out of ALTHOLD
       } else {
@@ -108,7 +151,7 @@ void processAltitudeHold()
           altitudeToHoldTarget -= 0.01;
         }
       }
-      throttle = altitudeHoldThrottle + altitudeHoldThrottleCorrection;
+      throttle = altitudeHoldThrottle + altitudeHoldThrottleCorrection + accelVelocityThrottleCorrection + throttleVelocityCorrection;
     }
     else {
       throttle = receiverCommand[THROTTLE];
@@ -119,6 +162,5 @@ void processAltitudeHold()
 }
 
 
-
 #endif // _AQ_ALTITUDE_CONTROL_PROCESSOR_H_