Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Added a configurable mc_hover_throttle parameter to be used when calculating altitude controller

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-26 10:20:52 +10:00
parent 03bfb70a15
commit 68e60f5c37
4 changed files with 19 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/config/config.c
View file

@ -256,6 +256,9 @@ void resetNavConfig(navConfig_t * navConfig)
navConfig->pos_hold_deadband = 20; navConfig->pos_hold_deadband = 20;
navConfig->alt_hold_deadband = 50; navConfig->alt_hold_deadband = 50;
// MC-specific
navConfig->mc_hover_throttle = 1500;
// Fixed wing // Fixed wing
navConfig->fw_max_bank_angle = 20; navConfig->fw_max_bank_angle = 20;
navConfig->fw_max_climb_angle = 15; navConfig->fw_max_climb_angle = 15;

8
src/main/flight/navigation_rewrite.h
View file

@ -106,12 +106,14 @@ typedef struct navConfig_s {
uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode) uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
uint8_t fw_max_bank_angle; // Fixed wing max banking angle (deg) uint16_t mc_hover_throttle; // multicopter hover throttle
uint8_t fw_max_climb_angle; // Fixed wing max banking angle (deg)
uint8_t fw_max_dive_angle; // Fixed wing max banking angle (deg)
uint16_t fw_cruise_throttle; // Cruise throttle uint16_t fw_cruise_throttle; // Cruise throttle
uint16_t fw_min_throttle; // Minimum allowed throttle in auto mode uint16_t fw_min_throttle; // Minimum allowed throttle in auto mode
uint16_t fw_max_throttle; // Maximum allowed throttle in auto mode uint16_t fw_max_throttle; // Maximum allowed throttle in auto mode
uint8_t fw_max_bank_angle; // Fixed wing max banking angle (deg)
uint8_t fw_max_climb_angle; // Fixed wing max banking angle (deg)
uint8_t fw_max_dive_angle; // Fixed wing max banking angle (deg)
uint8_t fw_pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) uint8_t fw_pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
} navConfig_t; } navConfig_t;

14
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -52,7 +52,7 @@
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Altitude controller for multicopter aircraft * Altitude controller for multicopter aircraft
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static int16_t altholdInitialThrottle; // Throttle input when althold was activated static int16_t altholdInitialRCThrottle; // Throttle input when althold was activated
static int16_t rcCommandAdjustedThrottle; static int16_t rcCommandAdjustedThrottle;
/* Calculate global altitude setpoint based on surface setpoint */ /* Calculate global altitude setpoint based on surface setpoint */
@ -83,8 +83,8 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
static filterStatePt1_t throttleFilterState; static filterStatePt1_t throttleFilterState;
// Calculate min and max throttle boundaries (to compensate for integral windup) // Calculate min and max throttle boundaries (to compensate for integral windup)
int32_t thrAdjustmentMin = posControl.escAndServoConfig->minthrottle - altholdInitialThrottle; int32_t thrAdjustmentMin = posControl.escAndServoConfig->minthrottle - posControl.navConfig->mc_hover_throttle;
int32_t thrAdjustmentMax = posControl.escAndServoConfig->maxthrottle - altholdInitialThrottle; int32_t thrAdjustmentMax = posControl.escAndServoConfig->maxthrottle - posControl.navConfig->mc_hover_throttle;
posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false); posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false);
posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &throttleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &throttleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
@ -93,7 +93,7 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
bool adjustMulticopterAltitudeFromRCInput(void) bool adjustMulticopterAltitudeFromRCInput(void)
{ {
int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altholdInitialThrottle, posControl.navConfig->alt_hold_deadband); int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altholdInitialRCThrottle, posControl.navConfig->alt_hold_deadband);
if (rcThrottleAdjustment) { if (rcThrottleAdjustment) {
// set velocity proportional to stick movement // set velocity proportional to stick movement
float rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / 500; float rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / 500;
@ -114,10 +114,10 @@ bool adjustMulticopterAltitudeFromRCInput(void)
void setupMulticopterAltitudeController(void) void setupMulticopterAltitudeController(void)
{ {
if (posControl.navConfig->flags.use_midrc_for_althold) { if (posControl.navConfig->flags.use_midrc_for_althold) {
altholdInitialThrottle = posControl.rxConfig->midrc; altholdInitialRCThrottle = posControl.rxConfig->midrc;
} }
else { else {
altholdInitialThrottle = rcCommand[THROTTLE]; altholdInitialRCThrottle = rcCommand[THROTTLE];
} }
} }
@ -164,7 +164,7 @@ void applyMulticopterAltitudeController(uint32_t currentTime)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain(altholdInitialThrottle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
// Save processed throttle for future use // Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE]; rcCommandAdjustedThrottle = rcCommand[THROTTLE];

7
src/main/io/serial_cli.c
View file

@ -549,12 +549,13 @@ const clivalue_t valueTable[] = {
{ "nav_min_rth_distance", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0, 5000 }, 0 }, { "nav_min_rth_distance", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0, 5000 }, 0 },
{ "nav_rth_alt_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.flags.rth_alt_control_style, .config.minmax = { 0, 4 }, 0 }, { "nav_rth_alt_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.flags.rth_alt_control_style, .config.minmax = { 0, 4 }, 0 },
{ "nav_rth_altitude", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.rth_altitude, .config.minmax = { 100, 10000 }, 0 }, { "nav_rth_altitude", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.rth_altitude, .config.minmax = { 100, 10000 }, 0 },
{ "nav_fw_bank_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_max_bank_angle, .config.minmax = { 5, 45 }, 0 }, { "nav_mc_hover_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.mc_hover_throttle, .config.minmax = { 1000, 2000 }, 0 },
{ "nav_fw_climb_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_max_climb_angle, .config.minmax = { 5, 45 }, 0 },
{ "nav_fw_dive_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_max_dive_angle, .config.minmax = { 5, 45 }, 0 },
{ "nav_fw_cruise_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_cruise_throttle, .config.minmax = { 1000, 2000 }, 0 }, { "nav_fw_cruise_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_cruise_throttle, .config.minmax = { 1000, 2000 }, 0 },
{ "nav_fw_min_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_min_throttle, .config.minmax = { 1000, 2000 }, 0 }, { "nav_fw_min_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_min_throttle, .config.minmax = { 1000, 2000 }, 0 },
{ "nav_fw_max_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_max_throttle, .config.minmax = { 1000, 2000 }, 0 }, { "nav_fw_max_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_max_throttle, .config.minmax = { 1000, 2000 }, 0 },
{ "nav_fw_bank_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_max_bank_angle, .config.minmax = { 5, 45 }, 0 },
{ "nav_fw_climb_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_max_climb_angle, .config.minmax = { 5, 45 }, 0 },
{ "nav_fw_dive_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_max_dive_angle, .config.minmax = { 5, 45 }, 0 },
{ "nav_fw_pitch2thr", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_pitch_to_throttle, .config.minmax = { 0, 100 }, 0 }, { "nav_fw_pitch2thr", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.fw_pitch_to_throttle, .config.minmax = { 0, 100 }, 0 },
#endif #endif