From 4f47581159e8c2beae3514c7ef7cf88459691215 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 22 Feb 2024 23:11:07 +0000 Subject: [PATCH] Merge fixes CR97 --- src/main/fc/fc_msp.c | 20 ++++++++++++++------ src/main/fc/settings.yaml | 12 ++++++++++-- src/main/navigation/navigation.c | 17 ++++++++--------- src/main/navigation/navigation.h | 3 ++- 4 files changed, 34 insertions(+), 18 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bd02e4def78..846b567b31f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1344,9 +1344,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_NAV_POSHOLD: sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); sbufWriteU16(dst, navConfig()->general.max_auto_speed); - sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); + // sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); // CR97A + sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_speed); - sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate); + sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_manual_climb_rate : navConfig()->mc.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.althold_throttle_type); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); @@ -2398,12 +2399,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize == 13) { navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.max_auto_speed = sbufReadU16(src); - navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + // navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + // CR97A + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { + navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src); + } else { + navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + } + // CR97A navConfigMutable()->general.max_manual_speed = sbufReadU16(src); - if (mixerConfig()->platformType != PLATFORM_AIRPLANE) { - navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); - }else{ + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src); + } else { + navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); } navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d22eaf22201..57e288c2a99 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2356,7 +2356,7 @@ groups: field: w_z_surface_p min: 0 max: 100 - default_value: 3.5 + default_value: 3.5 - name: inav_w_z_surface_v field: w_z_surface_v min: 0 @@ -2823,6 +2823,14 @@ groups: field: fw.max_bank_angle min: 5 max: 80 + # CR97A + - name: nav_fw_auto_climb_rate + description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" + default_value: 500 + field: fw.max_auto_climb_rate + min: 10 + max: 2000 + # CR97A - name: nav_fw_manual_climb_rate description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" default_value: 300 @@ -4172,7 +4180,7 @@ groups: type: navFwAutolandConfig_t headers: ["navigation/navigation.h"] condition: USE_FW_AUTOLAND - members: + members: - name: nav_fw_land_approach_length description: "Length of the final approach" default_value: 35000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 348bcef9452..fa653876a18 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -121,7 +121,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -183,8 +183,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // MC-specific .mc = { - .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees - .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s + .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT, #ifdef USE_MR_BRAKING_MODE @@ -195,7 +195,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s - .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle + .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle #endif .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 @@ -207,6 +207,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // Fixed wing .fw = { .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s CR97A .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees @@ -305,8 +306,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(bool launchBypass); void updateHomePosition(void); bool abortLaunchAllowed(void); -static bool rthAltControlStickOverrideCheck(unsigned axis); - #ifdef USE_FW_AUTOLAND static float getLandAltitude(void); static int32_t calcWindDiff(int32_t heading, int32_t windHeading); @@ -1899,7 +1898,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na (posControl.wpDistance - 0.1f * posControl.wpInitialDistance); } - if (fabsf(climbRate) >= navConfig()->general.max_auto_climb_rate) { + if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) { // CR97A climbRate = 0; } updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET); @@ -3481,13 +3480,13 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) float targetVel = 0.0f; const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); - float maxClimbRate = navConfig()->general.max_auto_climb_rate; + float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; // CR97A if (posControl.desiredState.climbRateDemand) { maxClimbRate = ABS(posControl.desiredState.climbRateDemand); } else if (emergLandingIsActive) { maxClimbRate = navConfig()->general.emerg_descent_rate; } else if (posControl.flags.isAdjustingAltitude) { - maxClimbRate = navConfig()->general.max_manual_climb_rate; + maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate; // CR97A } const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5e408aacf1d..1fa485cb42f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -265,7 +265,7 @@ typedef struct positionEstimationConfig_s { #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; -#endif +#endif } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); @@ -351,6 +351,7 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // Fixed wing max banking angle (deg) + uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec // CR97A uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg)