Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed wing Nav altitude velocity control #9471

Merged
merged 18 commits into from
May 11, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions docs/Settings.md
Original file line number Diff line number Diff line change
Expand Up @@ -2772,6 +2772,26 @@ Enable the possibility to manually increase the throttle in auto throttle contro

---

### nav_fw_alt_control_response

Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude.

| Default | Min | Max |
| --- | --- | --- |
| 20 | 5 | 100 |

---

### nav_fw_auto_climb_rate

Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]

| Default | Min | Max |
| --- | --- | --- |
| 500 | 10 | 2000 |

---

### nav_fw_bank_angle

Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll
Expand Down
2 changes: 2 additions & 0 deletions src/main/cms/cms_menu_imu.c
Original file line number Diff line number Diff line change
Expand Up @@ -222,6 +222,8 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
{
OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),

OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE),

OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
Expand Down
44 changes: 24 additions & 20 deletions src/main/fc/fc_msp.c
Original file line number Diff line number Diff line change
Expand Up @@ -1109,7 +1109,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
legacyLedConfig |= ledConfig->led_function << shiftCount;
shiftCount += 4;
legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
shiftCount += 6;
shiftCount += 6;
legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
shiftCount += 4;
legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
Expand Down Expand Up @@ -1336,9 +1336,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()->mc.max_auto_climb_rate);
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);
Expand Down Expand Up @@ -1586,7 +1586,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, timerHardware[i].usageFlags);
}
break;

case MSP2_INAV_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
Expand Down Expand Up @@ -2399,12 +2399,16 @@ 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);
if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src);
} else {
navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
}
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);
Expand Down Expand Up @@ -2734,7 +2738,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)

case MSP_SET_WP:
if (dataSize == 21) {

const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
navWaypoint_t msp_wp;
msp_wp.action = sbufReadU8(src); // action
Expand Down Expand Up @@ -2948,7 +2952,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
ledConfig->led_position = legacyConfig & 0xFF;
ledConfig->led_function = (legacyConfig >> 8) & 0xF;
ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
ledConfig->led_color = (legacyConfig >> 18) & 0xF;
ledConfig->led_color = (legacyConfig >> 18) & 0xF;
ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
ledConfig->led_params = (legacyConfig >> 28) & 0xF;

Expand Down Expand Up @@ -3238,7 +3242,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);

int16_t head1 = 0, head2 = 0;
if (sbufReadI16Safe(&head1, src)) {
fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
Expand Down Expand Up @@ -3292,12 +3296,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src);

} else {
return MSP_RESULT_ERROR;
}

break;
break;

#endif
#ifdef USE_PROGRAMMING_FRAMEWORK
Expand Down Expand Up @@ -3591,7 +3595,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
static uint8_t osdPos_x = 0;

//indicate new format hitl 1.4.0
sbufWriteU8(dst, 255);
sbufWriteU8(dst, 255);

if (isOSDTypeSupportedBySimulator())
{
Expand Down Expand Up @@ -3797,7 +3801,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
#ifdef USE_SIMULATOR
case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version

// Check the MSP version of simulator
if (tmp_u8 != SIMULATOR_MSP_VERSION) {
break;
Expand Down Expand Up @@ -3892,15 +3896,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
} else {
sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
}

// Get the acceleration in 1G units
acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accVibeSq[X] = 0.0f;
acc.accVibeSq[Y] = 0.0f;
acc.accVibeSq[Z] = 0.0f;

// Get the angular velocity in DPS
gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
Expand Down Expand Up @@ -3931,7 +3935,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
simulatorData.airSpeed = sbufReadU16(src);
} else {
if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
sbufReadU16(src);
sbufReadU16(src);
}
}

Expand Down Expand Up @@ -4006,8 +4010,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = MSP_RESULT_ERROR;
}
break;
#endif
#endif

default:
// Not handled
return false;
Expand Down
16 changes: 14 additions & 2 deletions src/main/fc/settings.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2075,6 +2075,12 @@ groups:
field: bank_fw.pid[PID_POS_Z].D
min: 0
max: 255
- 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."
default_value: 20
field: fwAltControlResponseFactor
min: 5
max: 100
- name: nav_fw_pos_xy_p
description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH"
default_value: 75
Expand Down Expand Up @@ -2302,7 +2308,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
description: "Weight of rangefinder measurements in estimated climb rate. Setting is used on both airplanes and multirotors when rangefinder is present and Surface mode enabled"
field: w_z_surface_v
Expand Down Expand Up @@ -2772,6 +2778,12 @@ groups:
field: fw.max_bank_angle
min: 5
max: 80
- 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
- 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
Expand Down Expand Up @@ -4115,7 +4127,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
Expand Down
9 changes: 5 additions & 4 deletions src/main/flight/pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
static EXTENDED_FASTRAM float fixedWingLevelTrim;
static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;

PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8);

PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
Expand Down Expand Up @@ -230,9 +230,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
},
[PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 },
[PID_POS_Z] = {
.P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10
.I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10
.D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10
.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,
},
[PID_POS_XY] = {
Expand Down Expand Up @@ -298,6 +298,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
.fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,

.fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT,
#ifdef USE_SMITH_PREDICTOR
.smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
.smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
Expand Down
2 changes: 2 additions & 0 deletions src/main/flight/pid.h
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,8 @@ typedef struct pidProfile_s {

float fixedWingLevelTrim;
float fixedWingLevelTrimGain;

uint8_t fwAltControlResponseFactor;
#ifdef USE_SMITH_PREDICTOR
float smithPredictorStrength;
float smithPredictorDelay;
Expand Down
Loading
Loading