mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 22:35:19 +03:00
Add battery voltage/weight/power density influenced settings to battery profiles (#7074)
This commit is contained in:
Michel Pastor
GitHub
parent
9565a8a01c
commit
7dad0bfa68
23 changed files with 362 additions and 297 deletions
|
|
@ -774,7 +774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
||||
sbufWriteU16(dst, motorConfig()->mincommand);
|
||||
|
||||
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
|
||||
|
||||
#ifdef USE_GPS
|
||||
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()->mincommand);
|
||||
|
||||
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
|
||||
|
||||
#ifdef USE_GPS
|
||||
sbufWriteU8(dst, gpsConfig()->provider); // gps_type
|
||||
|
|
@ -1041,7 +1041,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
case MSP_FAILSAFE_CONFIG:
|
||||
sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
|
||||
sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
|
||||
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle);
|
||||
sbufWriteU8(dst, 0); // was failsafe_kill_switch
|
||||
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
|
||||
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);
|
||||
sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
|
||||
sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
|
||||
sbufWriteU16(dst, navConfig()->mc.hover_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
|
||||
break;
|
||||
|
||||
case MSP_RTH_AND_LAND_CONFIG:
|
||||
|
|
@ -1342,13 +1342,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
break;
|
||||
|
||||
case MSP_FW_CONFIG:
|
||||
sbufWriteU16(dst, navConfig()->fw.cruise_throttle);
|
||||
sbufWriteU16(dst, navConfig()->fw.min_throttle);
|
||||
sbufWriteU16(dst, navConfig()->fw.max_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->nav.fw.cruise_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->nav.fw.min_throttle);
|
||||
sbufWriteU16(dst, currentBatteryProfile->nav.fw.max_throttle);
|
||||
sbufWriteU8(dst, navConfig()->fw.max_bank_angle);
|
||||
sbufWriteU8(dst, navConfig()->fw.max_climb_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);
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -1664,7 +1664,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
{
|
||||
uint8_t tmp_u8;
|
||||
uint16_t tmp_u16;
|
||||
batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile;
|
||||
|
||||
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()->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
|
||||
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()->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
|
||||
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()->mc.max_bank_angle = 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
|
||||
return MSP_RESULT_ERROR;
|
||||
break;
|
||||
|
|
@ -2319,13 +2318,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
|
||||
case MSP_SET_FW_CONFIG:
|
||||
if (dataSize >= 12) {
|
||||
navConfigMutable()->fw.cruise_throttle = sbufReadU16(src);
|
||||
navConfigMutable()->fw.min_throttle = sbufReadU16(src);
|
||||
navConfigMutable()->fw.max_throttle = sbufReadU16(src);
|
||||
currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src);
|
||||
currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src);
|
||||
currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src);
|
||||
navConfigMutable()->fw.max_bank_angle = sbufReadU8(src);
|
||||
navConfigMutable()->fw.max_climb_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);
|
||||
} else
|
||||
return MSP_RESULT_ERROR;
|
||||
|
|
@ -2695,7 +2694,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
if (dataSize >= 20) {
|
||||
failsafeConfigMutable()->failsafe_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
|
||||
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
|
||||
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@
|
|||
#include "io/beeper.h"
|
||||
#include "io/vtx.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
|
@ -500,10 +501,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
|
|||
schedulePidGainsUpdate();
|
||||
break;
|
||||
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, ¤tBatteryProfileMutable->nav.fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
|
||||
break;
|
||||
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, ¤tBatteryProfileMutable->nav.fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
|
||||
break;
|
||||
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
|
||||
updateBoardAlignment(delta, 0);
|
||||
|
|
@ -578,7 +579,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
|
|||
navigationUsePIDs();
|
||||
break;
|
||||
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, ¤tBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
|
||||
break;
|
||||
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
|
||||
case ADJUSTMENT_VTX_POWER_LEVEL:
|
||||
|
|
|
|||
|
|
@ -865,31 +865,12 @@ groups:
|
|||
default_value: "ONESHOT125"
|
||||
field: motorPwmProtocol
|
||||
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
|
||||
field: motorPoleCount
|
||||
description: "The number of motor poles. Required to compute motor RPM"
|
||||
min: 4
|
||||
max: 255
|
||||
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
|
||||
type: failsafeConfig_t
|
||||
|
|
@ -910,11 +891,6 @@ groups:
|
|||
default_value: 200
|
||||
min: 0
|
||||
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
|
||||
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
|
||||
|
|
@ -1142,6 +1118,127 @@ groups:
|
|||
min: 0
|
||||
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
|
||||
type: mixerConfig_t
|
||||
members:
|
||||
|
|
@ -1167,12 +1264,6 @@ groups:
|
|||
field: appliedMixerPreset
|
||||
min: -1
|
||||
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
|
||||
type: reversibleMotorsConfig_t
|
||||
|
|
@ -2496,12 +2587,6 @@ groups:
|
|||
field: mc.max_bank_angle
|
||||
min: 15
|
||||
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
|
||||
description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)"
|
||||
default_value: 2000
|
||||
|
|
@ -2583,24 +2668,6 @@ groups:
|
|||
default_value: ON
|
||||
field: mc.slowDownForTurning
|
||||
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
|
||||
description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
|
||||
default_value: 35
|
||||
|
|
@ -2619,12 +2686,6 @@ groups:
|
|||
field: fw.max_dive_angle
|
||||
min: 5
|
||||
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
|
||||
description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
|
||||
default_value: 6
|
||||
|
|
@ -2688,18 +2749,6 @@ groups:
|
|||
field: fw.launch_time_thresh
|
||||
min: 10
|
||||
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
|
||||
description: "Delay between raising throttle and motor starting at idle throttle (ms)"
|
||||
default_value: 0
|
||||
|
|
@ -3705,50 +3754,6 @@ groups:
|
|||
headers: ["flight/power_limits.h"]
|
||||
condition: USE_POWER_LIMITS
|
||||
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
|
||||
description: "Throttle attenuation PI control P term"
|
||||
default_value: 100
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
/*
|
||||
|
|
@ -66,13 +67,12 @@
|
|||
|
||||
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,
|
||||
.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_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_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure
|
||||
.failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left
|
||||
|
|
@ -218,7 +218,7 @@ bool failsafeRequiresMotorStop(void)
|
|||
{
|
||||
return failsafeState.active &&
|
||||
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
|
||||
failsafeConfig()->failsafe_throttle < getThrottleIdleValue();
|
||||
currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
|
||||
}
|
||||
|
||||
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[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[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
}
|
||||
else {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
autoRcCommand[i] = 0;
|
||||
}
|
||||
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
}
|
||||
|
||||
// Apply channel values
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
|
||||
|
||||
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)
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -83,14 +83,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
|
|||
.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,
|
||||
.motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
|
||||
.platformType = SETTING_PLATFORM_TYPE_DEFAULT,
|
||||
.hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
|
||||
.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
|
||||
|
|
@ -103,7 +102,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
|
|||
|
||||
#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,
|
||||
.motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
|
||||
|
|
@ -112,12 +111,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
|||
.mincommand = SETTING_MIN_COMMAND_DEFAULT,
|
||||
.motorAccelTimeMs = SETTING_MOTOR_ACCEL_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
|
||||
#ifdef USE_DSHOT
|
||||
.turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
|
||||
#endif
|
||||
);
|
||||
|
||||
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)
|
||||
{
|
||||
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;
|
||||
|
|
@ -330,7 +324,7 @@ static uint16_t handleOutputScaling(
|
|||
static void applyTurtleModeToMotors(void) {
|
||||
|
||||
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 stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 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
|
||||
#ifdef USE_PROGRAMMING_FRAMEWORK
|
||||
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin;
|
||||
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
|
||||
#else
|
||||
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
|
||||
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
|
||||
#endif
|
||||
// Throttle compensation based on battery voltage
|
||||
if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
|
||||
|
|
|
|||
|
|
@ -62,7 +62,6 @@ typedef struct mixerConfig_s {
|
|||
uint8_t platformType;
|
||||
bool hasFlaps;
|
||||
int16_t appliedMixerPreset;
|
||||
uint16_t fwMinThrottleDownPitchAngle;
|
||||
} mixerConfig_t;
|
||||
|
||||
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 motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
|
||||
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 turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash
|
||||
} motorConfig_t;
|
||||
|
||||
PG_DECLARE(motorConfig_t, motorConfig);
|
||||
|
|
|
|||
|
|
@ -56,6 +56,7 @@ FILE_COMPILE_FOR_SPEED
|
|||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/acceleration.h"
|
||||
|
|
@ -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
|
||||
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
|
||||
|
|
|
|||
|
|
@ -45,22 +45,12 @@ FILE_COMPILE_FOR_SPEED
|
|||
|
||||
#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,
|
||||
.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,
|
||||
.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
|
||||
|
|
@ -84,29 +74,29 @@ static bool wasLimitingPower = false;
|
|||
#endif
|
||||
|
||||
void powerLimiterInit(void) {
|
||||
if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) {
|
||||
powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent;
|
||||
if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) {
|
||||
currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent;
|
||||
}
|
||||
|
||||
activeCurrentLimit = powerLimitsConfig()->burstCurrent;
|
||||
activeCurrentLimit = currentBatteryProfile->powerLimits.burstCurrent;
|
||||
|
||||
uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
|
||||
burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6;
|
||||
burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6;
|
||||
uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
|
||||
burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
|
||||
burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
|
||||
|
||||
pt1FilterInit(¤tThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
|
||||
pt1FilterInitRC(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
|
||||
|
||||
#ifdef USE_ADC
|
||||
if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) {
|
||||
powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower;
|
||||
if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) {
|
||||
currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower;
|
||||
}
|
||||
|
||||
activePowerLimit = powerLimitsConfig()->burstPower;
|
||||
activePowerLimit = currentBatteryProfile->powerLimits.burstPower;
|
||||
|
||||
uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
|
||||
burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6;
|
||||
burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6;
|
||||
uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
|
||||
burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
|
||||
burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
|
||||
|
||||
pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 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;
|
||||
*burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
|
||||
|
||||
if (powerLimitsConfig()->burstCurrentFalldownTime) {
|
||||
if (currentBatteryProfile->powerLimits.burstCurrentFalldownTime) {
|
||||
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) {
|
||||
activeCurrentLimit = calculateActiveLimit(getAmperage(),
|
||||
powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent,
|
||||
currentBatteryProfile->powerLimits.continuousCurrent, currentBatteryProfile->powerLimits.burstCurrent,
|
||||
&burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
|
||||
timeDelta);
|
||||
}
|
||||
|
|
@ -135,7 +125,7 @@ void currentLimiterUpdate(timeDelta_t timeDelta) {
|
|||
#ifdef USE_ADC
|
||||
void powerLimiterUpdate(timeDelta_t timeDelta) {
|
||||
activePowerLimit = calculateActiveLimit(getPower(),
|
||||
powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower,
|
||||
currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower,
|
||||
&burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
|
||||
timeDelta);
|
||||
}
|
||||
|
|
@ -254,18 +244,18 @@ bool powerLimiterIsLimitingPower(void) {
|
|||
|
||||
// returns seconds
|
||||
float powerLimiterGetRemainingBurstTime(void) {
|
||||
uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
|
||||
uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
|
||||
float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
|
||||
|
||||
#ifdef USE_ADC
|
||||
uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
|
||||
uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
|
||||
float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
|
||||
|
||||
if (!powerLimitsConfig()->continuousCurrent) {
|
||||
if (!currentBatteryProfile->powerLimits.continuousCurrent) {
|
||||
return remainingPowerBurstTime;
|
||||
}
|
||||
|
||||
if (!powerLimitsConfig()->continuousPower) {
|
||||
if (!currentBatteryProfile->powerLimits.continuousPower) {
|
||||
return remainingCurrentBurstTime;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -25,23 +25,13 @@
|
|||
|
||||
#include <common/time.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
#if defined(USE_POWER_LIMITS)
|
||||
|
||||
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 piI;
|
||||
|
||||
|
|
|
|||
|
|
@ -77,9 +77,9 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
|
|||
// pitch in degrees
|
||||
// output in Watt
|
||||
static float estimatePitchPower(float pitch) {
|
||||
int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle;
|
||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
|
||||
int16_t altitudeChangeThrottle = (int16_t)pitch * currentBatteryProfile->nav.fw.pitch_to_throttle;
|
||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (currentBatteryProfile->nav.fw.cruise_throttle - getThrottleIdleValue());
|
||||
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2238,15 +2238,15 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
return true;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
case OSD_FW_ALT_PID_OUTPUTS:
|
||||
|
|
@ -2774,7 +2774,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
break;
|
||||
|
||||
case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT:
|
||||
if (powerLimitsConfig()->continuousCurrent) {
|
||||
if (currentBatteryProfile->powerLimits.continuousCurrent) {
|
||||
osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3);
|
||||
buff[3] = SYM_AMP;
|
||||
buff[4] = '\0';
|
||||
|
|
@ -2788,7 +2788,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
#ifdef USE_ADC
|
||||
case OSD_PLIMIT_ACTIVE_POWER_LIMIT:
|
||||
{
|
||||
if (powerLimitsConfig()->continuousPower) {
|
||||
if (currentBatteryProfile->powerLimits.continuousPower) {
|
||||
bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3);
|
||||
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
|
||||
buff[4] = '\0';
|
||||
|
|
|
|||
|
|
@ -867,10 +867,10 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
|
|||
tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
|
||||
break;
|
||||
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;
|
||||
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;
|
||||
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
|
||||
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);
|
||||
break;
|
||||
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
|
||||
tfp_sprintf(buff, "MTDPA %4d", mixerConfigMutable()->fwMinThrottleDownPitchAngle);
|
||||
tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle);
|
||||
break;
|
||||
case ADJUSTMENT_TPA:
|
||||
tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
#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,
|
||||
.general = {
|
||||
|
|
@ -142,7 +142,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
// MC-specific
|
||||
.mc = {
|
||||
.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
|
||||
|
||||
#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_climb_angle = SETTING_NAV_FW_CLIMB_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
|
||||
.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_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
|
||||
.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_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_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_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
|
||||
|
|
|
|||
|
|
@ -225,7 +225,6 @@ typedef struct navConfig_s {
|
|||
|
||||
struct {
|
||||
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
|
||||
|
||||
#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_climb_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
|
||||
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.
|
||||
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
|
||||
|
|
@ -261,8 +256,6 @@ typedef struct navConfig_s {
|
|||
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_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_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)
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
// Base frequencies for smoothing pitch and roll
|
||||
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.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) {
|
||||
// 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 {
|
||||
// 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)
|
||||
{
|
||||
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
|
||||
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle;
|
||||
int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
|
||||
int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
|
||||
|
||||
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
|
||||
// ROLL >0 right, <0 left
|
||||
|
|
@ -529,15 +531,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
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
|
||||
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)
|
||||
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle);
|
||||
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
|
||||
else
|
||||
correctedThrottleValue = motorConfig()->maxthrottle;
|
||||
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle);
|
||||
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
|
||||
} else {
|
||||
isAutoThrottleManuallyIncreased = false;
|
||||
}
|
||||
|
|
@ -600,7 +602,7 @@ void applyFixedWingEmergencyLandingController(void)
|
|||
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[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -50,6 +50,8 @@
|
|||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
|
||||
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
|
||||
#define UNUSED(x) ((void)(x))
|
||||
|
|
@ -225,13 +227,13 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
|
|||
|
||||
static bool isThrottleIdleEnabled(void)
|
||||
{
|
||||
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
|
||||
return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
|
||||
}
|
||||
|
||||
static void applyThrottleIdleLogic(bool forceMixerIdle)
|
||||
{
|
||||
if (isThrottleIdleEnabled() && !forceMixerIdle) {
|
||||
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
|
||||
}
|
||||
else {
|
||||
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;
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -397,14 +399,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
|
|||
|
||||
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
|
||||
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) {
|
||||
rcCommand[THROTTLE] = launchThrottle;
|
||||
return FW_LAUNCH_EVENT_SUCCESS;
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -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)
|
||||
{
|
||||
rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_throttle;
|
||||
|
||||
if (isLaunchMaxAltitudeReached()) {
|
||||
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 {
|
||||
// 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]);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@
|
|||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Altitude controller for multicopter aircraft
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -104,8 +106,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
|||
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
||||
{
|
||||
// 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 thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (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)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);
|
||||
|
||||
|
|
@ -239,7 +241,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// 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
|
||||
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
|
||||
|
|
@ -753,7 +755,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
}
|
||||
else {
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
}
|
||||
|
||||
return;
|
||||
|
|
@ -780,7 +782,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -29,13 +29,19 @@ FILE_COMPILE_FOR_SIZE
|
|||
#ifdef USE_NAV
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/config.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
static bool isYawAdjustmentValid = false;
|
||||
static int32_t navHeadingError;
|
||||
|
||||
|
|
@ -125,7 +131,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
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[PITCH] = 0;
|
||||
rcCommand[YAW] = 0;
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
} else if (navStateFlags & NAV_CTL_POS) {
|
||||
applyRoverBoatPositionController(currentTimeUs);
|
||||
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
|
||||
|
|
|
|||
|
|
@ -119,6 +119,51 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
|
|||
},
|
||||
|
||||
.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
|
||||
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@ PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles);
|
|||
|
||||
extern const batteryProfile_t *currentBatteryProfile;
|
||||
|
||||
#define currentBatteryProfileMutable ((batteryProfile_t*)currentBatteryProfile)
|
||||
|
||||
typedef enum {
|
||||
BATTERY_OK = 0,
|
||||
BATTERY_WARNING,
|
||||
|
|
|
|||
|
|
@ -88,12 +88,57 @@ typedef struct batteryProfile_s {
|
|||
#endif
|
||||
|
||||
struct {
|
||||
uint32_t value; // mAh or mWh (see capacity.unit)
|
||||
uint32_t warning; // 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
|
||||
uint32_t value; // mAh or mWh (see capacity.unit)
|
||||
uint32_t warning; // 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
|
||||
} capacity;
|
||||
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ void targetConfiguration(void)
|
|||
failsafeConfigMutable()->failsafe_delay = 5;
|
||||
failsafeConfigMutable()->failsafe_recovery_delay = 5;
|
||||
failsafeConfigMutable()->failsafe_off_delay = 200;
|
||||
failsafeConfigMutable()->failsafe_throttle = 1200;
|
||||
currentBatteryProfile->failsafe_throttle = 1200;
|
||||
failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH;
|
||||
|
||||
boardAlignmentMutable()->rollDeciDegrees = 0;
|
||||
|
|
@ -120,7 +120,7 @@ void targetConfiguration(void)
|
|||
navConfigMutable()->general.rth_altitude = 1000;
|
||||
|
||||
navConfigMutable()->mc.max_bank_angle = 30;
|
||||
navConfigMutable()->mc.hover_throttle = 1500;
|
||||
currentBatteryProfile->nav.mc.hover_throttle = 1500;
|
||||
navConfigMutable()->mc.auto_disarm_delay = 2000;
|
||||
|
||||
/*
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue