OP-1848 altvario TxPID support

flight/modules/Stabilization/altitudeloop.cpp

@@ -185,7 +185,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 
     // initialise limits on thrust but note the FSM can override.
     controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
-
 }
 
 

flight/modules/TxPID/txpid.c

@@ -55,6 +55,7 @@
 #include "accessorydesired.h"
 #include "manualcontrolcommand.h"
 #include "stabilizationsettings.h"
+#include "altitudeholdsettings.h"
 #include "stabilizationbank.h"
 #include "stabilizationsettingsbank1.h"
 #include "stabilizationsettingsbank2.h"
@@ -95,6 +96,8 @@ int32_t TxPIDInitialize(void)
     bool txPIDEnabled;
     HwSettingsOptionalModulesData optionalModules;
 
+    AltitudeHoldSettingsInitialize();
+
     HwSettingsInitialize();
     HwSettingsOptionalModulesGet(&optionalModules);
 
@@ -188,10 +191,13 @@ static void updatePIDs(UAVObjEvent *ev)
     }
     StabilizationSettingsData stab;
     StabilizationSettingsGet(&stab);
+    AltitudeHoldSettingsData altitude;
+    AltitudeHoldSettingsGet(&altitude);
     AccessoryDesiredData accessory;
 
-    uint8_t needsUpdateBank = 0;
-    uint8_t needsUpdateStab = 0;
+    uint8_t needsUpdateBank     = 0;
+    uint8_t needsUpdateStab     = 0;
+    uint8_t needsUpdateAltitude = 0;
 
     // Loop through every enabled instance
     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@@ -216,140 +222,155 @@ static void updatePIDs(UAVObjEvent *ev)
 
             switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
             case TXPIDSETTINGS_PIDS_ROLLRATEKP:
-                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATEKI:
-                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATEKD:
-                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
-                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLRATERESP:
-                needsUpdateBank |= update(&bank.ManualRate.Roll, value);
+                needsUpdateBank     |= update(&bank.ManualRate.Roll, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
-                needsUpdateBank |= update(&bank.RollPI.Kp, value);
+                needsUpdateBank     |= update(&bank.RollPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
-                needsUpdateBank |= update(&bank.RollPI.Ki, value);
+                needsUpdateBank     |= update(&bank.RollPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
-                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
+                needsUpdateBank     |= update(&bank.RollPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
-                needsUpdateBank |= updateUint8(&bank.RollMax, value);
+                needsUpdateBank     |= updateUint8(&bank.RollMax, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKP:
-                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKI:
-                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEKD:
-                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
-                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHRATERESP:
-                needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
+                needsUpdateBank     |= update(&bank.ManualRate.Pitch, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
-                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
+                needsUpdateBank     |= update(&bank.PitchPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
-                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
+                needsUpdateBank     |= update(&bank.PitchPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
-                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
+                needsUpdateBank     |= update(&bank.PitchPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
-                needsUpdateBank |= updateUint8(&bank.PitchMax, value);
+                needsUpdateBank     |= updateUint8(&bank.PitchMax, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
-                needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
-                needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.Kp, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
-                needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
-                needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.Ki, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
-                needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
-                needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.Kd, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
-                needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
-                needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
+                needsUpdateBank     |= update(&bank.RollRatePID.ILimit, value);
+                needsUpdateBank     |= update(&bank.PitchRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
-                needsUpdateBank |= update(&bank.ManualRate.Roll, value);
-                needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
+                needsUpdateBank     |= update(&bank.ManualRate.Roll, value);
+                needsUpdateBank     |= update(&bank.ManualRate.Pitch, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
-                needsUpdateBank |= update(&bank.RollPI.Kp, value);
-                needsUpdateBank |= update(&bank.PitchPI.Kp, value);
+                needsUpdateBank     |= update(&bank.RollPI.Kp, value);
+                needsUpdateBank     |= update(&bank.PitchPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
-                needsUpdateBank |= update(&bank.RollPI.Ki, value);
-                needsUpdateBank |= update(&bank.PitchPI.Ki, value);
+                needsUpdateBank     |= update(&bank.RollPI.Ki, value);
+                needsUpdateBank     |= update(&bank.PitchPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
-                needsUpdateBank |= update(&bank.RollPI.ILimit, value);
-                needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
+                needsUpdateBank     |= update(&bank.RollPI.ILimit, value);
+                needsUpdateBank     |= update(&bank.PitchPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
-                needsUpdateBank |= updateUint8(&bank.RollMax, value);
-                needsUpdateBank |= updateUint8(&bank.PitchMax, value);
+                needsUpdateBank     |= updateUint8(&bank.RollMax, value);
+                needsUpdateBank     |= updateUint8(&bank.PitchMax, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEKP:
-                needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
+                needsUpdateBank     |= update(&bank.YawRatePID.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEKI:
-                needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
+                needsUpdateBank     |= update(&bank.YawRatePID.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEKD:
-                needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
+                needsUpdateBank     |= update(&bank.YawRatePID.Kd, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
-                needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
+                needsUpdateBank     |= update(&bank.YawRatePID.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWRATERESP:
-                needsUpdateBank |= update(&bank.ManualRate.Yaw, value);
+                needsUpdateBank     |= update(&bank.ManualRate.Yaw, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
-                needsUpdateBank |= update(&bank.YawPI.Kp, value);
+                needsUpdateBank     |= update(&bank.YawPI.Kp, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
-                needsUpdateBank |= update(&bank.YawPI.Ki, value);
+                needsUpdateBank     |= update(&bank.YawPI.Ki, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
-                needsUpdateBank |= update(&bank.YawPI.ILimit, value);
+                needsUpdateBank     |= update(&bank.YawPI.ILimit, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
-                needsUpdateBank |= updateUint8(&bank.YawMax, value);
+                needsUpdateBank     |= updateUint8(&bank.YawMax, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLEXPO:
-                needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
+                needsUpdateBank     |= updateInt8(&bank.StickExpo.Roll, value);
                 break;
             case TXPIDSETTINGS_PIDS_PITCHEXPO:
-                needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
+                needsUpdateBank     |= updateInt8(&bank.StickExpo.Pitch, value);
                 break;
             case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
-                needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
-                needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
+                needsUpdateBank     |= updateInt8(&bank.StickExpo.Roll, value);
+                needsUpdateBank     |= updateInt8(&bank.StickExpo.Pitch, value);
                 break;
             case TXPIDSETTINGS_PIDS_YAWEXPO:
-                needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
+                needsUpdateBank     |= updateInt8(&bank.StickExpo.Yaw, value);
                 break;
             case TXPIDSETTINGS_PIDS_GYROTAU:
-                needsUpdateStab |= update(&stab.GyroTau, value);
+                needsUpdateStab     |= update(&stab.GyroTau, value);
                 break;
             case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
-                needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
+                needsUpdateBank     |= update(&bank.AcroInsanityFactor, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
+                needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
+                break;
+            case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
+                needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
                 break;
             default:
                 PIOS_Assert(0);
@@ -359,6 +380,9 @@ static void updatePIDs(UAVObjEvent *ev)
     if (needsUpdateStab) {
         StabilizationSettingsSet(&stab);
     }
+    if (needsUpdateAltitude) {
+        AltitudeHoldSettingsSet(&altitude);
+    }
     if (needsUpdateBank) {
         switch (inst.BankNumber) {
         case 0:

shared/uavobjectdefinition/txpidsettings.xml

@@ -20,7 +20,7 @@
 			Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp,
 			Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp,
 			Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo,
-			GyroTau,AcroPlusFactor"
+			GyroTau,AcroPlusFactor, Altitude Pos.Kp, Altitude Velocity.Kp, Altitude Velocity.Ki, Altitude Velocity.Kd, Altitude Velocity.Beta"
 		defaultvalue="Disabled"/>
         <field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>
         <field name="MaxPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>