Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Add battery voltage/weight/power density influenced settings to battery profiles (#7074)

Browse source
This commit is contained in:
Michel Pastor 2021-06-15 17:13:57 +02:00 • committed by GitHub
parent 9565a8a01c
commit 7dad0bfa68
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
23 changed files with 362 additions and 297 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -774,7 +774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle);
sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
#ifdef USE_GPS #ifdef USE_GPS
sbufWriteU8(dst, gpsConfig()->provider); // gps_type sbufWriteU8(dst, gpsConfig()->provider); // gps_type
@ -815,7 +815,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle);
sbufWriteU16(dst, motorConfig()->mincommand); sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
#ifdef USE_GPS #ifdef USE_GPS
sbufWriteU8(dst, gpsConfig()->provider); // gps_type sbufWriteU8(dst, gpsConfig()->provider); // gps_type
@ -1041,7 +1041,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_FAILSAFE_CONFIG: case MSP_FAILSAFE_CONFIG:
sbufWriteU8(dst, failsafeConfig()->failsafe_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
sbufWriteU8(dst, 0); // was failsafe_kill_switch sbufWriteU8(dst, 0); // was failsafe_kill_switch
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay); sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_procedure); sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
@ -1322,7 +1322,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold); sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
sbufWriteU16(dst, navConfig()->mc.hover_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
break; break;
case MSP_RTH_AND_LAND_CONFIG: case MSP_RTH_AND_LAND_CONFIG:
@ -1342,13 +1342,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_FW_CONFIG: case MSP_FW_CONFIG:
sbufWriteU16(dst, navConfig()->fw.cruise_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.fw.cruise_throttle);
sbufWriteU16(dst, navConfig()->fw.min_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.fw.min_throttle);
sbufWriteU16(dst, navConfig()->fw.max_throttle); sbufWriteU16(dst, currentBatteryProfile->nav.fw.max_throttle);
sbufWriteU8(dst, navConfig()->fw.max_bank_angle); sbufWriteU8(dst, navConfig()->fw.max_bank_angle);
sbufWriteU8(dst, navConfig()->fw.max_climb_angle); sbufWriteU8(dst, navConfig()->fw.max_climb_angle);
sbufWriteU8(dst, navConfig()->fw.max_dive_angle); sbufWriteU8(dst, navConfig()->fw.max_dive_angle);
sbufWriteU8(dst, navConfig()->fw.pitch_to_throttle); sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle);
sbufWriteU16(dst, navConfig()->fw.loiter_radius); sbufWriteU16(dst, navConfig()->fw.loiter_radius);
break; break;
#endif #endif
@ -1664,7 +1664,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
{ {
uint8_t tmp_u8; uint8_t tmp_u8;
uint16_t tmp_u16; uint16_t tmp_u16;
batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile;
const unsigned int dataSize = sbufBytesRemaining(src); const unsigned int dataSize = sbufBytesRemaining(src);
@ -1867,7 +1866,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
#ifdef USE_GPS #ifdef USE_GPS
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
@ -1915,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
#ifdef USE_GPS #ifdef USE_GPS
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
@ -2293,7 +2292,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src); navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
navConfigMutable()->mc.hover_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -2319,13 +2318,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_FW_CONFIG: case MSP_SET_FW_CONFIG:
if (dataSize >= 12) { if (dataSize >= 12) {
navConfigMutable()->fw.cruise_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src);
navConfigMutable()->fw.min_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src);
navConfigMutable()->fw.max_throttle = sbufReadU16(src); currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src);
navConfigMutable()->fw.max_bank_angle = sbufReadU8(src); navConfigMutable()->fw.max_bank_angle = sbufReadU8(src);
navConfigMutable()->fw.max_climb_angle = sbufReadU8(src); navConfigMutable()->fw.max_climb_angle = sbufReadU8(src);
navConfigMutable()->fw.max_dive_angle = sbufReadU8(src); navConfigMutable()->fw.max_dive_angle = sbufReadU8(src);
navConfigMutable()->fw.pitch_to_throttle = sbufReadU8(src); currentBatteryProfileMutable->nav.fw.pitch_to_throttle = sbufReadU8(src);
navConfigMutable()->fw.loiter_radius = sbufReadU16(src); navConfigMutable()->fw.loiter_radius = sbufReadU16(src);
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
@ -2695,7 +2694,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize >= 20) { if (dataSize >= 20) {
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); currentBatteryProfileMutable->failsafe_throttle = sbufReadU16(src);
sbufReadU8(src); // was failsafe_kill_switch sbufReadU8(src); // was failsafe_kill_switch
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src); failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);

7
src/main/fc/rc_adjustments.c
View file

@ -49,6 +49,7 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -500,10 +501,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_NAV_FW_CRUISE_THR: case ADJUSTMENT_NAV_FW_CRUISE_THR:
applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX); applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &currentBatteryProfileMutable->nav.fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
break; break;
case ADJUSTMENT_NAV_FW_PITCH2THR: case ADJUSTMENT_NAV_FW_PITCH2THR:
applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX); applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &currentBatteryProfileMutable->nav.fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
break; break;
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
updateBoardAlignment(delta, 0); updateBoardAlignment(delta, 0);
@ -578,7 +579,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
navigationUsePIDs(); navigationUsePIDs();
break; break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &currentBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
break; break;
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
case ADJUSTMENT_VTX_POWER_LEVEL: case ADJUSTMENT_VTX_POWER_LEVEL:

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

@ -865,31 +865,12 @@ groups:
default_value: "ONESHOT125" default_value: "ONESHOT125"
field: motorPwmProtocol field: motorPwmProtocol
table: motor_pwm_protocol table: motor_pwm_protocol
- name: throttle_scale
description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%"
default_value: 1.0
field: throttleScale
min: 0
max: 1
- name: throttle_idle
description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
default_value: 15
field: throttleIdle
min: 0
max: 30
- name: motor_poles - name: motor_poles
field: motorPoleCount field: motorPoleCount
description: "The number of motor poles. Required to compute motor RPM" description: "The number of motor poles. Required to compute motor RPM"
min: 4 min: 4
max: 255 max: 255
default_value: 14 default_value: 14
- name: turtle_mode_power_factor
field: turtleModePowerFactor
default_value: 55
description: "Turtle mode power factor"
condition: "USE_DSHOT"
min: 0
max: 100
- name: PG_FAILSAFE_CONFIG - name: PG_FAILSAFE_CONFIG
type: failsafeConfig_t type: failsafeConfig_t
@ -910,11 +891,6 @@ groups:
default_value: 200 default_value: 200
min: 0 min: 0
max: 200 max: 200
- name: failsafe_throttle
description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
default_value: 1000
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: failsafe_throttle_low_delay - name: failsafe_throttle_low_delay
description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout" description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout"
default_value: 0 default_value: 0
@ -1142,6 +1118,127 @@ groups:
min: 0 min: 0
max: MAX_CONTROL_RATE_PROFILE_COUNT max: MAX_CONTROL_RATE_PROFILE_COUNT
- name: throttle_scale
description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%"
default_value: 1.0
field: motor.throttleScale
min: 0
max: 1
- name: throttle_idle
description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle."
default_value: 15
field: motor.throttleIdle
min: 0
max: 30
- name: turtle_mode_power_factor
field: motor.turtleModePowerFactor
default_value: 55
description: "Turtle mode power factor"
condition: "USE_DSHOT"
min: 0
max: 100
- name: failsafe_throttle
description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
default_value: 1000
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: fw_min_throttle_down_pitch
description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
default_value: 0
field: fwMinThrottleDownPitchAngle
min: 0
max: 450
- name: nav_mc_hover_thr
description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
default_value: 1500
field: nav.mc.hover_throttle
min: 1000
max: 2000
- name: nav_fw_cruise_thr
description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
default_value: 1400
field: nav.fw.cruise_throttle
min: 1000
max: 2000
- name: nav_fw_min_thr
description: "Minimum throttle for flying wing in GPS assisted modes"
default_value: 1200
field: nav.fw.min_throttle
min: 1000
max: 2000
- name: nav_fw_max_thr
description: "Maximum throttle for flying wing in GPS assisted modes"
default_value: 1700
field: nav.fw.max_throttle
min: 1000
max: 2000
- name: nav_fw_pitch2thr
description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)"
default_value: 10
field: nav.fw.pitch_to_throttle
min: 0
max: 100
- name: nav_fw_launch_thr
description: "Launch throttle - throttle to be set during launch sequence (pwm units)"
default_value: 1700
field: nav.fw.launch_throttle
min: 1000
max: 2000
- name: nav_fw_launch_idle_thr
description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)"
default_value: 1000
field: nav.fw.launch_idle_throttle
min: 1000
max: 2000
- name: limit_cont_current
description: "Continous current limit (dA), set to 0 to disable"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.continuousCurrent
max: 4000
- name: limit_burst_current
description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.burstCurrent
max: 4000
- name: limit_burst_current_time
description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.burstCurrentTime
max: 3000
- name: limit_burst_current_falldown_time
description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
condition: USE_POWER_LIMITS
default_value: 0
field: powerLimits.burstCurrentFalldownTime
max: 3000
- name: limit_cont_power
description: "Continous power limit (dW), set to 0 to disable"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.continuousPower
max: 40000
- name: limit_burst_power
description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.burstPower
max: 40000
- name: limit_burst_power_time
description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.burstPowerTime
max: 3000
- name: limit_burst_power_falldown_time
description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
condition: USE_POWER_LIMITS && USE_ADC
default_value: 0
field: powerLimits.burstPowerFalldownTime
max: 3000
- name: PG_MIXER_CONFIG - name: PG_MIXER_CONFIG
type: mixerConfig_t type: mixerConfig_t
members: members:
@ -1167,12 +1264,6 @@ groups:
field: appliedMixerPreset field: appliedMixerPreset
min: -1 min: -1
max: INT16_MAX max: INT16_MAX
- name: fw_min_throttle_down_pitch
description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
default_value: 0
field: fwMinThrottleDownPitchAngle
min: 0
max: 450
- name: PG_REVERSIBLE_MOTORS_CONFIG - name: PG_REVERSIBLE_MOTORS_CONFIG
type: reversibleMotorsConfig_t type: reversibleMotorsConfig_t
@ -2496,12 +2587,6 @@ groups:
field: mc.max_bank_angle field: mc.max_bank_angle
min: 15 min: 15
max: 45 max: 45
- name: nav_mc_hover_thr
description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
default_value: 1500
field: mc.hover_throttle
min: 1000
max: 2000
- name: nav_mc_auto_disarm_delay - name: nav_mc_auto_disarm_delay
description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)" description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)"
default_value: 2000 default_value: 2000
@ -2583,24 +2668,6 @@ groups:
default_value: ON default_value: ON
field: mc.slowDownForTurning field: mc.slowDownForTurning
type: bool type: bool
- name: nav_fw_cruise_thr
description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
default_value: 1400
field: fw.cruise_throttle
min: 1000
max: 2000
- name: nav_fw_min_thr
description: "Minimum throttle for flying wing in GPS assisted modes"
default_value: 1200
field: fw.min_throttle
min: 1000
max: 2000
- name: nav_fw_max_thr
description: "Maximum throttle for flying wing in GPS assisted modes"
default_value: 1700
field: fw.max_throttle
min: 1000
max: 2000
- name: nav_fw_bank_angle - name: nav_fw_bank_angle
description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll" description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
default_value: 35 default_value: 35
@ -2619,12 +2686,6 @@ groups:
field: fw.max_dive_angle field: fw.max_dive_angle
min: 5 min: 5
max: 80 max: 80
- name: nav_fw_pitch2thr
description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)"
default_value: 10
field: fw.pitch_to_throttle
min: 0
max: 100
- name: nav_fw_pitch2thr_smoothing - name: nav_fw_pitch2thr_smoothing
description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh." description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
default_value: 6 default_value: 6
@ -2688,18 +2749,6 @@ groups:
field: fw.launch_time_thresh field: fw.launch_time_thresh
min: 10 min: 10
max: 1000 max: 1000
- name: nav_fw_launch_thr
description: "Launch throttle - throttle to be set during launch sequence (pwm units)"
default_value: 1700
field: fw.launch_throttle
min: 1000
max: 2000
- name: nav_fw_launch_idle_thr
description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)"
default_value: 1000
field: fw.launch_idle_throttle
min: 1000
max: 2000
- name: nav_fw_launch_idle_motor_delay - name: nav_fw_launch_idle_motor_delay
description: "Delay between raising throttle and motor starting at idle throttle (ms)" description: "Delay between raising throttle and motor starting at idle throttle (ms)"
default_value: 0 default_value: 0
@ -3705,50 +3754,6 @@ groups:
headers: ["flight/power_limits.h"] headers: ["flight/power_limits.h"]
condition: USE_POWER_LIMITS condition: USE_POWER_LIMITS
members: members:
- name: limit_cont_current
description: "Continous current limit (dA), set to 0 to disable"
default_value: 0
field: continuousCurrent
max: 4000
- name: limit_burst_current
description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
default_value: 0
field: burstCurrent
max: 4000
- name: limit_burst_current_time
description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
default_value: 0
field: burstCurrentTime
max: 3000
- name: limit_burst_current_falldown_time
description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
default_value: 0
field: burstCurrentFalldownTime
max: 3000
- name: limit_cont_power
condition: USE_ADC
description: "Continous power limit (dW), set to 0 to disable"
default_value: 0
field: continuousPower
max: 40000
- name: limit_burst_power
condition: USE_ADC
description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
default_value: 0
field: burstPower
max: 40000
- name: limit_burst_power_time
condition: USE_ADC
description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
default_value: 0
field: burstPowerTime
max: 3000
- name: limit_burst_power_falldown_time
condition: USE_ADC
description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
default_value: 0
field: burstPowerFalldownTime
max: 3000
- name: limit_pi_p - name: limit_pi_p
description: "Throttle attenuation PI control P term" description: "Throttle attenuation PI control P term"
default_value: 100 default_value: 100

10
src/main/flight/failsafe.c
View file

@ -51,6 +51,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
/* /*
@ -66,13 +67,12 @@
static failsafeState_t failsafeState; static failsafeState_t failsafeState;
PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2);
PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec
.failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay) .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay)
.failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec
.failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure
.failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left
@ -218,7 +218,7 @@ bool failsafeRequiresMotorStop(void)
{ {
return failsafeState.active && return failsafeState.active &&
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
failsafeConfig()->failsafe_throttle < getThrottleIdleValue(); currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
} }
void failsafeStartMonitoring(void) void failsafeStartMonitoring(void)
@ -264,13 +264,13 @@ void failsafeApplyControlInput(void)
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
else { else {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
autoRcCommand[i] = 0; autoRcCommand[i] = 0;
} }
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
// Apply channel values // Apply channel values

1
src/main/flight/failsafe.h
View file

@ -30,7 +30,6 @@
typedef struct failsafeConfig_s { typedef struct failsafeConfig_s {
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND) uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)

18
src/main/flight/mixer.c
View file

@ -83,14 +83,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.neutral = SETTING_3D_NEUTRAL_DEFAULT .neutral = SETTING_3D_NEUTRAL_DEFAULT
); );
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 4);
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
.platformType = SETTING_PLATFORM_TYPE_DEFAULT, .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
.hasFlaps = SETTING_HAS_FLAPS_DEFAULT, .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
.appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
.fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT
); );
#ifdef BRUSHED_MOTORS #ifdef BRUSHED_MOTORS
@ -103,7 +102,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
#define DEFAULT_MAX_THROTTLE 1850 #define DEFAULT_MAX_THROTTLE 1850
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 8);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
@ -112,12 +111,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.mincommand = SETTING_MIN_COMMAND_DEFAULT, .mincommand = SETTING_MIN_COMMAND_DEFAULT,
.motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT, .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT,
.motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT, .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT,
.throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
.throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
.motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles
#ifdef USE_DSHOT
.turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
#endif
); );
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
@ -130,7 +124,7 @@ static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
int getThrottleIdleValue(void) int getThrottleIdleValue(void)
{ {
if (!throttleIdleValue) { if (!throttleIdleValue) {
throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle); throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle);
} }
return throttleIdleValue; return throttleIdleValue;
@ -330,7 +324,7 @@ static uint16_t handleOutputScaling(
static void applyTurtleModeToMotors(void) { static void applyTurtleModeToMotors(void) {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f; const float flipPowerFactor = ((float)currentBatteryProfile->motor.turtleModePowerFactor)/100.0f;
const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f); const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f); const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@ -602,9 +596,9 @@ void FAST_CODE mixTable(const float dT)
// Throttle scaling to limit max throttle when battery is full // Throttle scaling to limit max throttle when battery is full
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin; mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
#else #else
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin; mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
#endif #endif
// Throttle compensation based on battery voltage // Throttle compensation based on battery voltage
if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {

4
src/main/flight/mixer.h
View file

@ -62,7 +62,6 @@ typedef struct mixerConfig_s {
uint8_t platformType; uint8_t platformType;
bool hasFlaps; bool hasFlaps;
int16_t appliedMixerPreset; int16_t appliedMixerPreset;
uint16_t fwMinThrottleDownPitchAngle;
} mixerConfig_t; } mixerConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig); PG_DECLARE(mixerConfig_t, mixerConfig);
@ -84,10 +83,7 @@ typedef struct motorConfig_s {
uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
uint16_t digitalIdleOffsetValue; uint16_t digitalIdleOffsetValue;
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
float throttleScale; // Scaling factor for throttle.
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash
} motorConfig_t; } motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig); PG_DECLARE(motorConfig_t, motorConfig);

77
src/main/flight/pid.c
View file

@ -56,6 +56,7 @@ FILE_COMPILE_FOR_SPEED
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
@ -368,24 +369,24 @@ bool pidInitFilters(void)
#ifdef USE_SMITH_PREDICTOR #ifdef USE_SMITH_PREDICTOR
smithPredictorInit( smithPredictorInit(
&pidState[FD_ROLL].smithPredictor, &pidState[FD_ROLL].smithPredictor,
pidProfile()->smithPredictorDelay, pidProfile()->smithPredictorDelay,
pidProfile()->smithPredictorStrength, pidProfile()->smithPredictorStrength,
pidProfile()->smithPredictorFilterHz, pidProfile()->smithPredictorFilterHz,
getLooptime() getLooptime()
); );
smithPredictorInit( smithPredictorInit(
&pidState[FD_PITCH].smithPredictor, &pidState[FD_PITCH].smithPredictor,
pidProfile()->smithPredictorDelay, pidProfile()->smithPredictorDelay,
pidProfile()->smithPredictorStrength, pidProfile()->smithPredictorStrength,
pidProfile()->smithPredictorFilterHz, pidProfile()->smithPredictorFilterHz,
getLooptime() getLooptime()
); );
smithPredictorInit( smithPredictorInit(
&pidState[FD_YAW].smithPredictor, &pidState[FD_YAW].smithPredictor,
pidProfile()->smithPredictorDelay, pidProfile()->smithPredictorDelay,
pidProfile()->smithPredictorStrength, pidProfile()->smithPredictorStrength,
pidProfile()->smithPredictorFilterHz, pidProfile()->smithPredictorFilterHz,
getLooptime() getLooptime()
); );
#endif #endif
@ -412,7 +413,7 @@ void pidResetErrorAccumulators(void)
} }
} }
void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
{ {
pidState[axis].errorGyroIf -= delta; pidState[axis].errorGyroIf -= delta;
pidState[axis].errorGyroIfLimit -= delta; pidState[axis].errorGyroIfLimit -= delta;
@ -423,7 +424,7 @@ float getTotalRateTarget(void)
return fast_fsqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget)); return fast_fsqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
} }
float getAxisIterm(uint8_t axis) float getAxisIterm(uint8_t axis)
{ {
return pidState[axis].errorGyroIf; return pidState[axis].errorGyroIf;
} }
@ -528,7 +529,7 @@ void updatePIDCoefficients()
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
if (usedPidControllerType == PID_TYPE_PID) { if (usedPidControllerType == PID_TYPE_PID) {
antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]); antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain); iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
} }
#endif #endif
@ -606,7 +607,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, currentBatteryProfile->fwMinThrottleDownPitchAngle);
} }
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
@ -615,7 +616,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10); DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z)); DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
/* /*
* fixedWingLevelTrim has opposite sign to rcCommand. * fixedWingLevelTrim has opposite sign to rcCommand.
* Positive rcCommand means nose should point downwards * Positive rcCommand means nose should point downwards
* Negative rcCommand mean nose should point upwards * Negative rcCommand mean nose should point upwards
@ -624,7 +625,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
* Positive fixedWingLevelTrim means nose should point upwards * Positive fixedWingLevelTrim means nose should point upwards
* Negative fixedWingLevelTrim means nose should point downwards * Negative fixedWingLevelTrim means nose should point downwards
*/ */
angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10); DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
} }
@ -702,23 +703,23 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
float dBoost = 1.0f; float dBoost = 1.0f;
if (dBoostFactor > 1) { if (dBoostFactor > 1) {
const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT);
const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration);
dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor);
dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
dBoost = constrainf(dBoost, 1.0f, dBoostFactor); dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
} }
return dBoost; return dBoost;
} }
#else #else
static float applyDBoost(pidState_t *pidState, float dT) { static float applyDBoost(pidState_t *pidState, float dT) {
UNUSED(pidState); UNUSED(pidState);
UNUSED(dT); UNUSED(dT);
@ -747,7 +748,7 @@ static float dTermProcess(pidState_t *pidState, float dT) {
static void applyItermLimiting(pidState_t *pidState) { static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) { if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else } else
{ {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
} }
@ -806,7 +807,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor; return itermErrorRate * itermRelaxFactor;
} }
@ -1051,12 +1052,12 @@ void checkItermLimitingActive(pidState_t *pidState)
bool shouldActivate; bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) { if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else } else
{ {
shouldActivate = mixerIsOutputSaturated(); shouldActivate = mixerIsOutputSaturated();
} }
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
} }
void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
@ -1074,7 +1075,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
{ {
pidState->itermFreezeActive = false; pidState->itermFreezeActive = false;
} }
} }
void FAST_CODE pidController(float dT) void FAST_CODE pidController(float dT)
@ -1102,7 +1103,7 @@ void FAST_CODE pidController(float dT)
} else { } else {
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]); rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]);
#else #else
rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]); rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]);
#endif #endif
} }
@ -1153,7 +1154,7 @@ void FAST_CODE pidController(float dT)
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits // Apply setpoint rate of change limits
pidApplySetpointRateLimiting(&pidState[axis], axis, dT); pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
// Step 4: Run gyro-driven control // Step 4: Run gyro-driven control
checkItermLimitingActive(&pidState[axis]); checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis); checkItermFreezingActive(&pidState[axis], axis);
@ -1165,7 +1166,7 @@ void FAST_CODE pidController(float dT)
pidType_e pidIndexGetType(pidIndex_e pidIndex) pidType_e pidIndexGetType(pidIndex_e pidIndex)
{ {
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) { if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType; return usedPidControllerType;
} }
if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) { if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
@ -1201,7 +1202,7 @@ void pidInit(void)
antigravityGain = pidProfile()->antigravityGain; antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator; antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif #endif
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (axis == FD_YAW) { if (axis == FD_YAW) {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw; pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
@ -1218,7 +1219,7 @@ void pidInit(void)
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) { if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
if ( if (
mixerConfig()->platformType == PLATFORM_AIRPLANE || mixerConfig()->platformType == PLATFORM_AIRPLANE ||
mixerConfig()->platformType == PLATFORM_BOAT || mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER mixerConfig()->platformType == PLATFORM_ROVER
) { ) {
@ -1276,10 +1277,10 @@ void pidInit(void)
} }
const pidBank_t * pidBank(void) { const pidBank_t * pidBank(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
} }
pidBank_t * pidBankMutable(void) { pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
} }
@ -1298,14 +1299,14 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
*/ */
if (ARMING_FLAG(ARMED) && !previousArmingState) { if (ARMING_FLAG(ARMED) && !previousArmingState) {
navPidReset(&fixedWingLevelTrimController); navPidReset(&fixedWingLevelTrimController);
} }
/* /*
* On disarm update the default value * On disarm update the default value
*/ */
if (!ARMING_FLAG(ARMED) && previousArmingState) { if (!ARMING_FLAG(ARMED) && previousArmingState) {
pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE); pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
} }
/* /*
* Prepare flags for the PID controller * Prepare flags for the PID controller

52
src/main/flight/power_limits.c
View file

@ -45,22 +45,12 @@ FILE_COMPILE_FOR_SPEED
#define LIMITING_THR_FILTER_TCONST 50 #define LIMITING_THR_FILTER_TCONST 50
PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 1);
PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig,
.continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA
.burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA
.burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS
.burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS
#ifdef USE_ADC
.continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW
.burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW
.burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS
.burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS
#endif
.piP = SETTING_LIMIT_PI_P_DEFAULT, .piP = SETTING_LIMIT_PI_P_DEFAULT,
.piI = SETTING_LIMIT_PI_I_DEFAULT, .piI = SETTING_LIMIT_PI_I_DEFAULT,
.attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz
); );
static float burstCurrentReserve; // cA.µs static float burstCurrentReserve; // cA.µs
@ -84,29 +74,29 @@ static bool wasLimitingPower = false;
#endif #endif
void powerLimiterInit(void) { void powerLimiterInit(void) {
if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) { if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) {
powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent; currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent;
} }
activeCurrentLimit = powerLimitsConfig()->burstCurrent; activeCurrentLimit = currentBatteryProfile->powerLimits.burstCurrent;
uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6; burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6; burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
#ifdef USE_ADC #ifdef USE_ADC
if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) { if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) {
powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower; currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower;
} }
activePowerLimit = powerLimitsConfig()->burstPower; activePowerLimit = currentBatteryProfile->powerLimits.burstPower;
uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6; burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6; burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
@ -118,7 +108,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui
float spentReserveChunk = continuousDiff * timeDelta; float spentReserveChunk = continuousDiff * timeDelta;
*burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax); *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
if (powerLimitsConfig()->burstCurrentFalldownTime) { if (currentBatteryProfile->powerLimits.burstCurrentFalldownTime) {
return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10; return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10;
} }
@ -127,7 +117,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui
void currentLimiterUpdate(timeDelta_t timeDelta) { void currentLimiterUpdate(timeDelta_t timeDelta) {
activeCurrentLimit = calculateActiveLimit(getAmperage(), activeCurrentLimit = calculateActiveLimit(getAmperage(),
powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent, currentBatteryProfile->powerLimits.continuousCurrent, currentBatteryProfile->powerLimits.burstCurrent,
&burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax, &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
timeDelta); timeDelta);
} }
@ -135,7 +125,7 @@ void currentLimiterUpdate(timeDelta_t timeDelta) {
#ifdef USE_ADC #ifdef USE_ADC
void powerLimiterUpdate(timeDelta_t timeDelta) { void powerLimiterUpdate(timeDelta_t timeDelta) {
activePowerLimit = calculateActiveLimit(getPower(), activePowerLimit = calculateActiveLimit(getPower(),
powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower, currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower,
&burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax, &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
timeDelta); timeDelta);
} }
@ -254,18 +244,18 @@ bool powerLimiterIsLimitingPower(void) {
// returns seconds // returns seconds
float powerLimiterGetRemainingBurstTime(void) { float powerLimiterGetRemainingBurstTime(void) {
uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7; float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
#ifdef USE_ADC #ifdef USE_ADC
uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7; float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
if (!powerLimitsConfig()->continuousCurrent) { if (!currentBatteryProfile->powerLimits.continuousCurrent) {
return remainingPowerBurstTime; return remainingPowerBurstTime;
} }
if (!powerLimitsConfig()->continuousPower) { if (!currentBatteryProfile->powerLimits.continuousPower) {
return remainingCurrentBurstTime; return remainingCurrentBurstTime;
} }

14
src/main/flight/power_limits.h
View file

@ -25,23 +25,13 @@
#include <common/time.h> #include <common/time.h>
#include "platform.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
#if defined(USE_POWER_LIMITS) #if defined(USE_POWER_LIMITS)
typedef struct { typedef struct {
uint16_t continuousCurrent; // dA
uint16_t burstCurrent; // dA
uint16_t burstCurrentTime; // ds
uint16_t burstCurrentFalldownTime; // ds
#ifdef USE_ADC
uint16_t continuousPower; // dW
uint16_t burstPower; // dW
uint16_t burstPowerTime; // ds
uint16_t burstPowerFalldownTime; // ds
#endif
uint16_t piP; uint16_t piP;
uint16_t piI; uint16_t piI;

6
src/main/flight/rth_estimator.c
View file

@ -77,9 +77,9 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
// pitch in degrees // pitch in degrees
// output in Watt // output in Watt
static float estimatePitchPower(float pitch) { static float estimatePitchPower(float pitch) {
int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle; int16_t altitudeChangeThrottle = (int16_t)pitch * currentBatteryProfile->nav.fw.pitch_to_throttle;
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); altitudeChangeThrottle = constrain(altitudeChangeThrottle, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue()); const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (currentBatteryProfile->nav.fw.cruise_throttle - getThrottleIdleValue());
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
} }

10
src/main/io/osd.c
View file

@ -2238,15 +2238,15 @@ static bool osdDrawSingleElement(uint8_t item)
return true; return true;
case OSD_NAV_FW_CRUISE_THR: case OSD_NAV_FW_CRUISE_THR:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
return true; return true;
case OSD_NAV_FW_PITCH2THR: case OSD_NAV_FW_PITCH2THR:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
return true; return true;
case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)currentBatteryProfile->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
return true; return true;
case OSD_FW_ALT_PID_OUTPUTS: case OSD_FW_ALT_PID_OUTPUTS:
@ -2774,7 +2774,7 @@ static bool osdDrawSingleElement(uint8_t item)
break; break;
case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT:
if (powerLimitsConfig()->continuousCurrent) { if (currentBatteryProfile->powerLimits.continuousCurrent) {
osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3);
buff[3] = SYM_AMP; buff[3] = SYM_AMP;
buff[4] = '\0'; buff[4] = '\0';
@ -2788,7 +2788,7 @@ static bool osdDrawSingleElement(uint8_t item)
#ifdef USE_ADC #ifdef USE_ADC
case OSD_PLIMIT_ACTIVE_POWER_LIMIT: case OSD_PLIMIT_ACTIVE_POWER_LIMIT:
{ {
if (powerLimitsConfig()->continuousPower) { if (currentBatteryProfile->powerLimits.continuousPower) {
bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3);
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
buff[4] = '\0'; buff[4] = '\0';

6
src/main/io/osd_dji_hd.c
View file

@ -867,10 +867,10 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF); tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
break; break;
case ADJUSTMENT_NAV_FW_CRUISE_THR: case ADJUSTMENT_NAV_FW_CRUISE_THR:
tfp_sprintf(buff, "CR %4d", navConfigMutable()->fw.cruise_throttle); tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle);
break; break;
case ADJUSTMENT_NAV_FW_PITCH2THR: case ADJUSTMENT_NAV_FW_PITCH2THR:
tfp_sprintf(buff, "P2T %3d", navConfigMutable()->fw.pitch_to_throttle); tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle);
break; break;
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees); tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees);
@ -927,7 +927,7 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D); tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
break; break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
tfp_sprintf(buff, "MTDPA %4d", mixerConfigMutable()->fwMinThrottleDownPitchAngle); tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle);
break; break;
case ADJUSTMENT_TPA: case ADJUSTMENT_TPA:
tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);

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

@ -96,7 +96,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, 0); PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12); PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13);
PG_RESET_TEMPLATE(navConfig_t, navConfig, PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = { .general = {
@ -142,7 +142,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// MC-specific // MC-specific
.mc = { .mc = {
.max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
.hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
.auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed
#ifdef USE_MR_BRAKING_MODE #ifdef USE_MR_BRAKING_MODE
@ -166,12 +165,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT, .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
.max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
.min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
.pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT, .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT, .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
@ -183,8 +178,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G) .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
.launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
.launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch

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

@ -225,7 +225,6 @@ typedef struct navConfig_s {
struct { struct {
uint8_t max_bank_angle; // multicopter max banking angle (deg) uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t hover_throttle; // multicopter hover throttle
uint16_t auto_disarm_delay; // multicopter safety delay for landing detector uint16_t auto_disarm_delay; // multicopter safety delay for landing detector
#ifdef USE_MR_BRAKING_MODE #ifdef USE_MR_BRAKING_MODE
@ -248,12 +247,8 @@ typedef struct navConfig_s {
uint8_t max_bank_angle; // Fixed wing max banking angle (deg) uint8_t max_bank_angle; // Fixed wing max banking angle (deg)
uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_climb_angle; // Fixed wing max banking angle (deg)
uint8_t max_dive_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg)
uint16_t cruise_throttle; // Cruise throttle
uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH
uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation
uint16_t min_throttle; // Minimum allowed throttle in auto mode
uint16_t max_throttle; // Maximum allowed throttle in auto mode
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
@ -261,8 +256,6 @@ typedef struct navConfig_s {
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s) uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s)
uint16_t launch_time_thresh; // Time threshold for launch detection (ms) uint16_t launch_time_thresh; // Time threshold for launch detection (ms)
uint16_t launch_idle_throttle; // Throttle to keep at launch idle
uint16_t launch_throttle; // Launch throttle
uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms)
uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms) uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle throttle (ms)
uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention) uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)

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

@ -51,6 +51,8 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
// Base frequencies for smoothing pitch and roll // Base frequencies for smoothing pitch and roll
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
@ -483,18 +485,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
// Unfiltered throttle correction outside of pitch deadband // Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
} }
else { else {
// Filtered throttle correction inside of pitch deadband // Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle; return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
} }
} }
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{ {
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle; int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle; int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
// ROLL >0 right, <0 left // ROLL >0 right, <0 left
@ -529,15 +531,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
} }
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
// Manual throttle increase // Manual throttle increase
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95) if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95)
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle); correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
else else
correctedThrottleValue = motorConfig()->maxthrottle; correctedThrottleValue = motorConfig()->maxthrottle;
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle); isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
} else { } else {
isAutoThrottleManuallyIncreased = false; isAutoThrottleManuallyIncreased = false;
} }
@ -600,7 +602,7 @@ void applyFixedWingEmergencyLandingController(void)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

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

@ -50,6 +50,8 @@
#include "io/gps.h" #include "io/gps.h"
#include "sensors/battery.h"
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
#define UNUSED(x) ((void)(x)) #define UNUSED(x) ((void)(x))
@ -225,13 +227,13 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
static bool isThrottleIdleEnabled(void) static bool isThrottleIdleEnabled(void)
{ {
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
} }
static void applyThrottleIdleLogic(bool forceMixerIdle) static void applyThrottleIdleLogic(bool forceMixerIdle)
{ {
if (isThrottleIdleEnabled() && !forceMixerIdle) { if (isThrottleIdleEnabled() && !forceMixerIdle) {
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
} }
else { else {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
@ -331,7 +333,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} }
else { else {
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
} }
@ -397,14 +399,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
const uint16_t launchThrottle = navConfig()->fw.launch_throttle; const uint16_t launchThrottle = currentBatteryProfile->nav.fw.launch_throttle;
if (elapsedTimeMs > motorSpinUpMs) { if (elapsedTimeMs > motorSpinUpMs) {
rcCommand[THROTTLE] = launchThrottle; rcCommand[THROTTLE] = launchThrottle;
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} }
else { else {
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle);
} }
@ -413,7 +415,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
{ {
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_throttle;
if (isLaunchMaxAltitudeReached()) { if (isLaunchMaxAltitudeReached()) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
@ -443,7 +445,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
} }
else { else {
// make a smooth transition from the launch state to the current state for throttle and the pitch angle // make a smooth transition from the launch state to the current state for throttle and the pitch angle
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, currentBatteryProfile->nav.fw.launch_throttle, rcCommand[THROTTLE]);
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
} }

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

@ -51,6 +51,8 @@
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "sensors/battery.h"
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Altitude controller for multicopter aircraft * Altitude controller for multicopter aircraft
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -104,8 +106,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
{ {
// Calculate min and max throttle boundaries (to compensate for integral windup) // Calculate min and max throttle boundaries (to compensate for integral windup)
const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
@ -239,7 +241,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
// Save processed throttle for future use // Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE]; rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@ -443,11 +445,11 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
* We override computed speed with max speed in following cases: * We override computed speed with max speed in following cases:
* 1 - computed velocity is > maxSpeed * 1 - computed velocity is > maxSpeed
* 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed
*/ */
if ( if (
(navGetCurrentStateFlags() & NAV_AUTO_WP && (navGetCurrentStateFlags() & NAV_AUTO_WP &&
!isApproachingLastWaypoint() && !isApproachingLastWaypoint() &&
newVelTotal < maxSpeed && newVelTotal < maxSpeed &&
!navConfig()->mc.slowDownForTurning !navConfig()->mc.slowDownForTurning
) || newVelTotal > maxSpeed ) || newVelTotal > maxSpeed
) { ) {
@ -753,7 +755,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = getThrottleIdleValue(); rcCommand[THROTTLE] = getThrottleIdleValue();
} }
else { else {
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} }
return; return;
@ -780,7 +782,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

10
src/main/navigation/navigation_rover_boat.c
View file

@ -29,13 +29,19 @@ FILE_COMPILE_FOR_SIZE
#ifdef USE_NAV #ifdef USE_NAV
#include "build/debug.h" #include "build/debug.h"
#include "common/utils.h" #include "common/utils.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/config.h" #include "fc/config.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "sensors/battery.h"
static bool isYawAdjustmentValid = false; static bool isYawAdjustmentValid = false;
static int32_t navHeadingError; static int32_t navHeadingError;
@ -125,7 +131,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[YAW] = posControl.rcAdjustment[YAW]; rcCommand[YAW] = posControl.rcAdjustment[YAW];
} }
rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
} }
} }
} }
@ -136,7 +142,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0; rcCommand[PITCH] = 0;
rcCommand[YAW] = 0; rcCommand[YAW] = 0;
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
} else if (navStateFlags & NAV_CTL_POS) { } else if (navStateFlags & NAV_CTL_POS) {
applyRoverBoatPositionController(currentTimeUs); applyRoverBoatPositionController(currentTimeUs);
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs); applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);

45
src/main/sensors/battery.c
View file

@ -119,6 +119,51 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
}, },
.controlRateProfile = 0, .controlRateProfile = 0,
.motor = {
.throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
.throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
#ifdef USE_DSHOT
.turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
#endif
},
.failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.nav = {
.mc = {
.hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT,
},
.fw = {
.cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT,
.max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT,
.min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT,
.pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
.launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT,
.launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP
}
},
#if defined(USE_POWER_LIMITS)
.powerLimits = {
.continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA
.burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA
.burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS
.burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS
#ifdef USE_ADC
.continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW
.burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW
.burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS
.burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS
#endif // USE_ADC
}
#endif // USE_POWER_LIMITS
); );
} }
} }

2
src/main/sensors/battery.h
View file

@ -51,6 +51,8 @@ PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles);
extern const batteryProfile_t *currentBatteryProfile; extern const batteryProfile_t *currentBatteryProfile;
#define currentBatteryProfileMutable ((batteryProfile_t*)currentBatteryProfile)
typedef enum { typedef enum {
BATTERY_OK = 0, BATTERY_OK = 0,
BATTERY_WARNING, BATTERY_WARNING,

53
src/main/sensors/battery_config_structs.h
View file

@ -88,12 +88,57 @@ typedef struct batteryProfile_s {
#endif #endif
struct { struct {
uint32_t value; // mAh or mWh (see capacity.unit) uint32_t value; // mAh or mWh (see capacity.unit)
uint32_t warning; // mAh or mWh (see capacity.unit) uint32_t warning; // mAh or mWh (see capacity.unit)
uint32_t critical; // mAh or mWh (see capacity.unit) uint32_t critical; // mAh or mWh (see capacity.unit)
batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical
} capacity; } capacity;
uint8_t controlRateProfile; uint8_t controlRateProfile;
struct {
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
float throttleScale; // Scaling factor for throttle.
#ifdef USE_DSHOT
uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash
#endif
} motor;
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t fwMinThrottleDownPitchAngle;
struct {
struct {
uint16_t hover_throttle; // multicopter hover throttle
} mc;
struct {
uint16_t cruise_throttle; // Cruise throttle
uint16_t min_throttle; // Minimum allowed throttle in auto mode
uint16_t max_throttle; // Maximum allowed throttle in auto mode
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
uint16_t launch_idle_throttle; // Throttle to keep at launch idle
uint16_t launch_throttle; // Launch throttle
} fw;
} nav;
#if defined(USE_POWER_LIMITS)
struct {
uint16_t continuousCurrent; // dA
uint16_t burstCurrent; // dA
uint16_t burstCurrentTime; // ds
uint16_t burstCurrentFalldownTime; // ds
#ifdef USE_ADC
uint16_t continuousPower; // dW
uint16_t burstPower; // dW
uint16_t burstPowerTime; // ds
uint16_t burstPowerFalldownTime; // ds
#endif // USE_ADC
} powerLimits;
#endif // USE_POWER_LIMITS
} batteryProfile_t; } batteryProfile_t;

4
src/main/target/FALCORE/config.c
View file

@ -89,7 +89,7 @@ void targetConfiguration(void)
failsafeConfigMutable()->failsafe_delay = 5; failsafeConfigMutable()->failsafe_delay = 5;
failsafeConfigMutable()->failsafe_recovery_delay = 5; failsafeConfigMutable()->failsafe_recovery_delay = 5;
failsafeConfigMutable()->failsafe_off_delay = 200; failsafeConfigMutable()->failsafe_off_delay = 200;
failsafeConfigMutable()->failsafe_throttle = 1200; currentBatteryProfile->failsafe_throttle = 1200;
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH; failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH;
boardAlignmentMutable()->rollDeciDegrees = 0; boardAlignmentMutable()->rollDeciDegrees = 0;
@ -120,7 +120,7 @@ void targetConfiguration(void)
navConfigMutable()->general.rth_altitude = 1000; navConfigMutable()->general.rth_altitude = 1000;
navConfigMutable()->mc.max_bank_angle = 30; navConfigMutable()->mc.max_bank_angle = 30;
navConfigMutable()->mc.hover_throttle = 1500; currentBatteryProfile->nav.mc.hover_throttle = 1500;
navConfigMutable()->mc.auto_disarm_delay = 2000; navConfigMutable()->mc.auto_disarm_delay = 2000;
/* /*