From ecf02229822f280b30cc38cda1d8b44c0a131c28 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 22 Sep 2024 23:22:36 +0100 Subject: [PATCH] CR133 --- src/main/cms/cms_menu_imu.c | 1 + src/main/fc/fc_core.c | 3 +-- src/main/fc/settings.yaml | 14 ++++++++++- src/main/flight/pid.c | 2 +- src/main/navigation/navigation.c | 28 ++-------------------- src/main/navigation/navigation_fixedwing.c | 6 ++--- 6 files changed, 21 insertions(+), 33 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 9f8118fe501..8d3eda4f09f 100644 --- 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 index 054899b73cd..a8b31c9c8f5 100644 --- 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 index 4adf307280c..13353d38647 100644 --- 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 index 27979571a55..233e4003a41 100644 --- 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 index faf751f164c..56e0d7335a8 100644 --- 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 index 7b0eac8bdd6..083e0ff6570 100644 --- 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