CR133

breadoven · Sep 22, 2024 · ecf0222 · ecf0222
1 parent 8d035d5
commit ecf0222
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 33 deletions.
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
@@ -227,6 +227,7 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
+    OTHER_PIDFF_ENTRY("ALT FF", &cmsx_pidPosZ.FF),  // CR133
 
     OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P),
     OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I),

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
@@ -509,8 +509,7 @@ bool emergInflightRearmEnabled(void)
     timeMs_t currentTimeMs = millis();
     emergRearmStabiliseTimeout = 0;
 
-    if ((lastDisarmReason != DISARM_SWITCH) ||
-        (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
+    if ((lastDisarmReason != DISARM_SWITCH) || (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
         return false;
     }
 

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -634,6 +634,13 @@ groups:
         field: baro_calibration_tolerance
         min: 0
         max: 1000
+        # CR131
+      - name: inav_baro_temp_correction
+        description: "Baro temperature correction factor"
+        field: baro_temp_correction
+        min: -100
+        max: 1000
+        default_value: 0
 
   - name: PG_PITOTMETER_CONFIG
     type: pitotmeterConfig_t
@@ -2152,6 +2159,12 @@ groups:
         default_value: 10
         field: bank_fw.pid[PID_POS_Z].D
         min: 0
+        max: 255
+      - name: nav_fw_pos_z_ff   # CR133 add FF
+        description: "FF gain of altitude PID controller (Fixedwing)"
+        default_value: 10
+        field: bank_fw.pid[PID_POS_Z].FF
+        min: 0
         max: 255  # CR133 add FF
       - name: nav_fw_alt_control_response
         description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude."
@@ -2431,7 +2444,6 @@ groups:
         min: 0
         max: 10
         default_value: 0.1
-        # CR131
       - name: inav_w_z_gps_p
         description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -243,7 +243,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                     .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 100
                     .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 100
                     .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 100
-                    .FF = 0,  // CR133
+                    .FF = SETTING_NAV_FW_POS_Z_FF_DEFAULT,    // CR133
                 },
                 [PID_POS_XY] = {
                     .P = SETTING_NAV_FW_POS_XY_P_DEFAULT,     // FW_POS_XY_P * 100

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -1936,32 +1936,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance),
                                                 posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance,
                                                 posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
-// DEBUG_SET(DEBUG_ALWAYS, 0, tmpWaypoint.z);
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
 
-
-// targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
-
-
-                    // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
-                    // Update climb rate until within 100cm of total climb xy distance to WP
-                    // static bool latched = false;
-                    // if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f && posControl.wpDistance < 0.9f * posControl.wpInitialDistance) {
-                        // float climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
-                                          // (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
-
-                        // if (latched || fabsf(climbRate) > 50.0f) {
-                            // latched = true;
-                            // climbRate = posControl.desiredState.vel.z + 0.1f * (climbRate - posControl.desiredState.vel.z);
-                            // updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
-                            // DEBUG_SET(DEBUG_ALWAYS, 1, climbRate);
-                        // }
-                    // } else if (latched) {
-                        // updateClimbRateToAltitudeController(0.0f, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
-                        // latched = false;
-                    // }
-                    // DEBUG_SET(DEBUG_ALWAYS, 2, latched);
-                    // CR133
                     if(STATE(MULTIROTOR)) {
                         switch (wpHeadingControl.mode) {
                             case NAV_WP_HEAD_MODE_NONE:
@@ -4791,9 +4767,9 @@ void navigationUsePIDs(void)
     );
                                                                                                 // CR133
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,  // 0.6 All for response factor of 50
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 30.0f,  // 0.333
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 50.0f,  // 0.333
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 50.0f,  // 0.15 to 0.3
-                                        0.0f,   //(float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,  // poss new FF, original was 0.0f
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 50.0f,  // poss new FF, original was 0.0f
                                         NAV_DTERM_CUT_HZ,
                                         0.0f
     );

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
@@ -149,10 +149,9 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 
     // CR133
     // ****************** TEST FILTER VERT VEL
-    // DEBUG_SET(DEBUG_ALWAYS, 0, currentClimbRate);
     static pt1Filter_t velz2FilterState;
-    currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, 0.1 * navConfig()->fw.wp_tracking_accuracy, US2S(deltaMicros));
-    // DEBUG_SET(DEBUG_ALWAYS, 1, currentClimbRate);
+    currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, 1.0f, US2S(deltaMicros));
+    DEBUG_SET(DEBUG_ALWAYS, 2, currentClimbRate);
     // ******************
 
     // ****************** TEST PID INTEGRATOR
@@ -200,6 +199,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     // **********************8
 
     float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
+    DEBUG_SET(DEBUG_ALWAYS, 3, posControl.pids.fw_alt.feedForward);
     // DEBUG_SET(DEBUG_ALWAYS, 6, targetPitchAngle);  // CR133
 
     // Apply low-pass filter to prevent rapid correction