Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

separate the nav climb rate settings for fw and mc

Browse source
This commit is contained in:
shota 2023-10-29 17:04:56 +09:00
parent 692c61cff9
commit a0f353eb49
7 changed files with 44 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/cms/cms_menu_navigation.c
View file

@ -45,8 +45,9 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED),
OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED),
OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED),
OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_MC_AUTO_CLIMB_RATE),
OSD_SETTING_ENTRY("MAX MC AH CLIMB RATE", SETTING_NAV_MC_MANUAL_CLIMB_RATE),
OSD_SETTING_ENTRY("MAX FW AH CLIMB RATE", SETTING_NAV_FW_MANUAL_CLIMB_RATE),
OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE),
OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),

12
src/main/fc/fc_msp.c
View file

@ -1310,9 +1310,9 @@ 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()->general.max_auto_climb_rate);
sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate);
sbufWriteU16(dst, navConfig()->general.max_manual_speed);
sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.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);
@ -2321,9 +2321,13 @@ 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()->general.max_auto_climb_rate = sbufReadU16(src);
navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
if (mixerConfig()->platformType != PLATFORM_AIRPLANE) {
navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
}else{
navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src);
}
navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);

30
src/main/fc/settings.yaml
View file

@ -2489,24 +2489,12 @@ groups:
field: general.max_auto_speed
min: 10
max: 2000
- name: nav_auto_climb_rate
description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
default_value: 500
field: general.max_auto_climb_rate
min: 10
max: 2000
- name: nav_manual_speed
description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
default_value: 500
field: general.max_manual_speed
min: 10
max: 2000
- name: nav_manual_climb_rate
description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
default_value: 200
field: general.max_manual_climb_rate
min: 10
max: 2000
- name: nav_land_minalt_vspd
description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]"
default_value: 50
@ -2677,6 +2665,18 @@ groups:
field: mc.max_bank_angle
min: 15
max: 45
- name: nav_mc_auto_climb_rate
description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
default_value: 500
field: mc.max_auto_climb_rate
min: 10
max: 2000
- name: nav_mc_manual_climb_rate
description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
default_value: 200
field: mc.max_manual_climb_rate
min: 10
max: 2000
- name: nav_auto_disarm_delay
description: "Delay before craft disarms when `nav_disarm_on_landing` is set (ms)"
default_value: 1000
@ -2762,6 +2762,12 @@ groups:
field: fw.max_bank_angle
min: 5
max: 80
- 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
field: fw.max_manual_climb_rate
min: 10
max: 2500
- name: nav_fw_climb_angle
description: "Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit"
default_value: 20

5
src/main/navigation/navigation.c
View file

@ -133,9 +133,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
.auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
.max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
.max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
.land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
.land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters
.land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
@ -160,6 +158,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_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT,
#ifdef USE_MR_BRAKING_MODE
.braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s
@ -181,6 +181,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// Fixed wing
.fw = {
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.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
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s

5
src/main/navigation/navigation.h
View file

@ -246,9 +246,7 @@ typedef struct navConfig_s {
bool waypoint_load_on_boot; // load waypoints automatically during boot
uint16_t auto_speed; // autonomous navigation speed cm/sec
uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_manual_speed; // manual velocity control max horizontal speed
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt
uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt
uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend
@ -272,6 +270,8 @@ typedef struct navConfig_s {
struct {
uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
#ifdef USE_MR_BRAKING_MODE
uint16_t braking_speed_threshold; // above this speed braking routine might kick in
@ -292,6 +292,7 @@ typedef struct navConfig_s {
struct {
uint8_t max_bank_angle; // Fixed wing max banking angle (deg)
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)
uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH

2
src/main/navigation/navigation_fixedwing.c
View file

@ -116,7 +116,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
if (rcAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
float rcClimbRate = -rcAdjustment * navConfig()->fw.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
return true;
}

16
src/main/navigation/navigation_multicopter.c
View file

@ -77,9 +77,9 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
float vel_max_z = 0.0f;
if (posControl.flags.isAdjustingAltitude) {
vel_max_z = navConfig()->general.max_manual_climb_rate;
vel_max_z = navConfig()->mc.max_manual_climb_rate;
} else {
vel_max_z = navConfig()->general.max_auto_climb_rate;
vel_max_z = navConfig()->mc.max_auto_climb_rate;
}
targetVel = constrainf(targetVel, -vel_max_z, vel_max_z);
@ -151,11 +151,11 @@ bool adjustMulticopterAltitudeFromRCInput(void)
// Make sure we can satisfy max_manual_climb_rate in both up and down directions
if (rcThrottleAdjustment > 0) {
// Scaling from altHoldThrottleRCZero to maxthrottle
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband);
rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband);
}
else {
// Scaling from minthrottle to altHoldThrottleRCZero
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
}
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
@ -221,11 +221,11 @@ void resetMulticopterAltitudeController(void)
const float maxSpeed = getActiveSpeed();
nav_speed_up = maxSpeed;
nav_accel_z = maxSpeed;
nav_speed_down = navConfig()->general.max_auto_climb_rate;
nav_speed_down = navConfig()->mc.max_auto_climb_rate;
} else {
nav_speed_up = navConfig()->general.max_manual_speed;
nav_accel_z = navConfig()->general.max_manual_speed;
nav_speed_down = navConfig()->general.max_manual_climb_rate;
nav_speed_down = navConfig()->mc.max_manual_climb_rate;
}
sqrtControllerInit(
@ -252,8 +252,8 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
if (prepareForTakeoffOnReset) {
const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate;
posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->mc.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
posControl.pids.vel[Z].integrator = -500.0f;
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
prepareForTakeoffOnReset = false;