Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 06:45:14 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/development' into dzikuvx-mr-cruise-experiments

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-06-13 11:56:51 +02:00
commit 8132e59b7e
44 changed files with 458 additions and 361 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitignore vendored
View file

@ -20,6 +20,4 @@ docs/Manual.pdf
README.pdf README.pdf
# build generated files # build generated files
/src/main/fc/settings_generated.h
/src/main/fc/settings_generated.c
/settings.json /settings.json

35
Makefile
View file

@ -274,9 +274,10 @@ TARGET_BIN = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).bin
TARGET_HEX = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).hex TARGET_HEX = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).hex
endif endif
TARGET_OBJ_DIR = $(OBJECT_DIR)/$(TARGET)
TARGET_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf TARGET_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf
TARGET_OBJS = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(TARGET_SRC)))) TARGET_OBJS = $(addsuffix .o,$(addprefix $(TARGET_OBJ_DIR)/,$(basename $(TARGET_SRC))))
TARGET_DEPS = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $(TARGET_SRC)))) TARGET_DEPS = $(addsuffix .d,$(addprefix $(TARGET_OBJ_DIR)/,$(basename $(TARGET_SRC))))
TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map TARGET_MAP = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map
@ -285,20 +286,23 @@ CLEAN_ARTIFACTS += $(TARGET_HEX)
CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
# Make sure build date and revision is updated on every incremental build # Make sure build date and revision is updated on every incremental build
$(OBJECT_DIR)/$(TARGET)/build/version.o : $(TARGET_SRC) $(TARGET_OBJ_DIR)/build/version.o : $(TARGET_SRC)
# Settings generator # Settings generator
.PHONY: .FORCE settings clean-settings .PHONY: .FORCE settings clean-settings
UTILS_DIR = $(ROOT)/src/utils UTILS_DIR = $(ROOT)/src/utils
SETTINGS_GENERATOR = $(UTILS_DIR)/settings.rb SETTINGS_GENERATOR = $(UTILS_DIR)/settings.rb
BUILD_STAMP = $(UTILS_DIR)/build_stamp.rb BUILD_STAMP = $(UTILS_DIR)/build_stamp.rb
STAMP = $(BIN_DIR)/build.stamp STAMP = $(TARGET_OBJ_DIR)/build.stamp
GENERATED_SETTINGS = $(SRC_DIR)/fc/settings_generated.h $(SRC_DIR)/fc/settings_generated.c GENERATED_SETTINGS = $(TARGET_OBJ_DIR)/settings_generated.h $(TARGET_OBJ_DIR)/settings_generated.c
SETTINGS_FILE = $(SRC_DIR)/fc/settings.yaml SETTINGS_FILE = $(SRC_DIR)/fc/settings.yaml
GENERATED_FILES = $(GENERATED_SETTINGS) GENERATED_FILES = $(GENERATED_SETTINGS)
$(GENERATED_SETTINGS): $(SETTINGS_GENERATOR) $(SETTINGS_FILE) $(STAMP) $(GENERATED_SETTINGS): $(SETTINGS_GENERATOR) $(SETTINGS_FILE) $(STAMP)
# Make sure the generated files are in the include path
CFLAGS += -I$(TARGET_OBJ_DIR)
$(STAMP): .FORCE $(STAMP): .FORCE
$(V1) CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(BUILD_STAMP) $(SETTINGS_FILE) $(STAMP) $(V1) CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(BUILD_STAMP) $(SETTINGS_FILE) $(STAMP)
@ -306,7 +310,7 @@ $(STAMP): .FORCE
# See https://www.gnu.org/software/make/manual/make.html#Pattern-Examples # See https://www.gnu.org/software/make/manual/make.html#Pattern-Examples
%generated.h %generated.c: %generated.h %generated.c:
$(V1) echo "settings.yaml -> settings_generated.h, settings_generated.c" "$(STDOUT)" $(V1) echo "settings.yaml -> settings_generated.h, settings_generated.c" "$(STDOUT)"
$(V1) CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) $(V1) CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) -o $(TARGET_OBJ_DIR)
settings-json: settings-json:
$(V0) CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) --json settings.json $(V0) CFLAGS="$(CFLAGS)" TARGET=$(TARGET) ruby $(SETTINGS_GENERATOR) . $(SETTINGS_FILE) --json settings.json
@ -329,18 +333,18 @@ $(TARGET_ELF): $(TARGET_OBJS)
$(V0) $(SIZE) $(TARGET_ELF) $(V0) $(SIZE) $(TARGET_ELF)
# Compile # Compile
$(OBJECT_DIR)/$(TARGET)/%.o: %.c $(TARGET_OBJ_DIR)/%.o: %.c
$(V1) mkdir -p $(dir $@) $(V1) mkdir -p $(dir $@)
$(V1) echo %% $(notdir $<) "$(STDOUT)" $(V1) echo %% $(notdir $<) "$(STDOUT)"
$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $< $(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $<
# Assemble # Assemble
$(OBJECT_DIR)/$(TARGET)/%.o: %.s $(TARGET_OBJ_DIR)/%.o: %.s
$(V1) mkdir -p $(dir $@) $(V1) mkdir -p $(dir $@)
$(V1) echo %% $(notdir $<) "$(STDOUT)" $(V1) echo %% $(notdir $<) "$(STDOUT)"
$(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $<
$(OBJECT_DIR)/$(TARGET)/%.o: %.S $(TARGET_OBJ_DIR)/%.o: %.S
$(V1) mkdir -p $(dir $@) $(V1) mkdir -p $(dir $@)
$(V1) echo %% $(notdir $<) "$(STDOUT)" $(V1) echo %% $(notdir $<) "$(STDOUT)"
$(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $< $(V1) $(CROSS_CC) -c -o $@ $(ASFLAGS) $<
@ -377,8 +381,7 @@ $(VALID_TARGETS):
clean: clean:
$(V0) echo "Cleaning $(TARGET)" $(V0) echo "Cleaning $(TARGET)"
$(V0) rm -f $(CLEAN_ARTIFACTS) $(V0) rm -f $(CLEAN_ARTIFACTS)
$(V0) rm -rf $(OBJECT_DIR)/$(TARGET) $(V0) rm -rf $(TARGET_OBJ_DIR)
$(V0) rm -f $(GENERATED_SETTINGS)
$(V0) echo "Cleaning $(TARGET) succeeded." $(V0) echo "Cleaning $(TARGET) succeeded."
## clean_test : clean up all temporary / machine-generated files (tests) ## clean_test : clean up all temporary / machine-generated files (tests)

12
docs/Rangefinder.md
View file

@ -8,8 +8,7 @@ Current support of rangefinders in INAV is very limited. They are used only to:
* landing detection for multirotors * landing detection for multirotors
* automated landing support for fixed wings * automated landing support for fixed wings
* _Experimental_ terrain following (Surface) flight mode activated with _SURFACE_ and _ALTHOLD_ flight mode
INAV currently does not support _terrain following_ or _rangefinder supported altitude hold_ flight modes.
## Hardware ## Hardware
@ -18,15 +17,16 @@ Following rangefinders are supported:
* HC-SR04 - ***DO NOT USE*** HC-SR04 while most popular is not suited to use in noise reach environments (like multirotors). It is proven that this sonar rangefinder start to report random values already at 1.5m above solid concrete surface. Reported altitude is valid up to only 75cm above concrete. ***DO NOT USE*** * HC-SR04 - ***DO NOT USE*** HC-SR04 while most popular is not suited to use in noise reach environments (like multirotors). It is proven that this sonar rangefinder start to report random values already at 1.5m above solid concrete surface. Reported altitude is valid up to only 75cm above concrete. ***DO NOT USE***
* US-100 in trigger-echo mode - Can be used as direct replacement of _HC-SR04_ when `rangefinder_hardware` is set to _HCSR04_. Useful up to 2m over concrete and correctly reporting _out of range_ when out of range * US-100 in trigger-echo mode - Can be used as direct replacement of _HC-SR04_ when `rangefinder_hardware` is set to _HCSR04_. Useful up to 2m over concrete and correctly reporting _out of range_ when out of range
* SRF10 - experimental * SRF10 - experimental
* HCSR04I2C - is a simple [DIY rangefinder interface](https://github.com/iNavFlight/INAV-Rangefinder-I2C-interface). Can be used to connect when flight controller has no Trigger-Echo ports. * INAV_I2C - is a simple [DIY rangefinder interface with Arduino Pro Mini 3.3V](https://github.com/iNavFlight/inav-rangefinder). Can be used to connect when flight controller has no Trigger-Echo ports.
* VL53L0X - simple laser rangefinder usable up to 75cm * VL53L0X - simple laser rangefinder usable up to 75cm
* UIB - experimental * UIB - experimental
* MSP - experimental
## Connections ## Connections
Target dependent in case of Trigger/Echo solutions like HC-SR04 and US-100. Target dependent in case of Trigger/Echo solutions like `HC-SR04` and `US-100`.
I2C solutions like VL53L0X or HCSR04I2C can be connected to I2C port and used as any other I2C device. I2C solutions like `VL53L0X` or `INAV_I2C` can be connected to I2C port and used as any other I2C device.
#### Constraints #### Constraints
Target dependent in case of Trigger/Echo solutions like HC-SR04 and US-100. No constrains for I2C like VL53L0X or HCSR04I2C. Target dependent in case of Trigger/Echo solutions like `HC-SR04` and `US-100`. No constrains for I2C like `VL53L0X` or `INAV_I2C`.

3
src/main/build/debug.h
View file

@ -59,6 +59,7 @@ typedef enum {
DEBUG_ALWAYS, DEBUG_ALWAYS,
DEBUG_STAGE2, DEBUG_STAGE2,
DEBUG_WIND_ESTIMATOR, DEBUG_WIND_ESTIMATOR,
DEBUG_SAG_COMP_VOLTAGE,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;
@ -75,4 +76,4 @@ void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size)
#define DEBUG_TRACE_SYNC(fmt, ...) #define DEBUG_TRACE_SYNC(fmt, ...)
#define DEBUG_TRACE_BUFFER_HEX(buf, size) #define DEBUG_TRACE_BUFFER_HEX(buf, size)
#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size) #define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size)
#endif #endif

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

@ -775,8 +775,13 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration) void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration)
{ {
// Check if we're already yielding, in that case just extend
// the yield time without releasing the display again, otherwise
// the yield/grab become unbalanced.
if (cmsYieldUntil == 0) {
displayRelease(pPort);
}
cmsYieldUntil = millis() + duration; cmsYieldUntil = millis() + duration;
displayRelease(pPort);
} }
// Stick/key detection and key codes // Stick/key detection and key codes
@ -1209,6 +1214,8 @@ void cmsUpdate(uint32_t currentTimeUs)
// Only scan keys and draw if we're not yielding // Only scan keys and draw if we're not yielding
if (cmsYieldUntil == 0) { if (cmsYieldUntil == 0) {
// XXX: Note that one call to cmsScanKeys() might generate multiple keypresses
// when repeating, that's why cmsYieldDisplay() has to check for multiple calls.
rcDelayMs = cmsScanKeys(currentTimeMs, lastCalledMs, rcDelayMs); rcDelayMs = cmsScanKeys(currentTimeMs, lastCalledMs, rcDelayMs);
// Check again, the keypress might have produced a yield // Check again, the keypress might have produced a yield
if (cmsYieldUntil == 0) { if (cmsYieldUntil == 0) {

1
src/main/cms/cms_menu_osd.c
View file

@ -148,6 +148,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("RSSI", OSD_RSSI_VALUE), OSD_ELEMENT_ENTRY("RSSI", OSD_RSSI_VALUE),
OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE), OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE),
OSD_ELEMENT_ENTRY("MAIN BATT SC", OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE),
OSD_ELEMENT_ENTRY("CELL VOLTAGE", OSD_MAIN_BATT_CELL_VOLTAGE), OSD_ELEMENT_ENTRY("CELL VOLTAGE", OSD_MAIN_BATT_CELL_VOLTAGE),
OSD_ELEMENT_ENTRY("CROSSHAIRS", OSD_CROSSHAIRS), OSD_ELEMENT_ENTRY("CROSSHAIRS", OSD_CROSSHAIRS),
OSD_ELEMENT_ENTRY("HORIZON", OSD_ARTIFICIAL_HORIZON), OSD_ELEMENT_ENTRY("HORIZON", OSD_ARTIFICIAL_HORIZON),

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

@ -141,7 +141,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
{ {
int16_t stickDeflection; int16_t stickDeflection;
stickDeflection = constrain(rawData - rxConfig()->midrc, -500, 500); stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
stickDeflection = applyDeadband(stickDeflection, deadband); stickDeflection = applyDeadband(stickDeflection, deadband);
return rcLookup(stickDeflection, rate); return rcLookup(stickDeflection, rate);
@ -401,14 +401,6 @@ void tryArm(void)
#endif #endif
statsOnArm(); statsOnArm();
#ifdef USE_RANGEFINDER
/*
* Since each arm can happen over different surface type, we have to reset
* previously computed max. dynamic range threshold
*/
rangefinderResetDynamicThreshold();
#endif
return; return;
} }
@ -488,7 +480,7 @@ void processRx(timeUs_t currentTimeUs)
updateActivatedModes(); updateActivatedModes();
if (!cliMode) { if ((!cliMode) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
updateAdjustmentStates(); updateAdjustmentStates();
processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile)); processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile));
} }

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

@ -647,7 +647,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_MISC: case MSP_MISC:
sbufWriteU16(dst, rxConfig()->midrc); sbufWriteU16(dst, PWM_RANGE_MIDDLE);
sbufWriteU16(dst, motorConfig()->minthrottle); sbufWriteU16(dst, motorConfig()->minthrottle);
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle);
@ -677,7 +677,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP2_INAV_MISC: case MSP2_INAV_MISC:
sbufWriteU16(dst, rxConfig()->midrc); sbufWriteU16(dst, PWM_RANGE_MIDDLE);
sbufWriteU16(dst, motorConfig()->minthrottle); sbufWriteU16(dst, motorConfig()->minthrottle);
sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle);
@ -829,7 +829,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_RX_CONFIG: case MSP_RX_CONFIG:
sbufWriteU8(dst, rxConfig()->serialrx_provider); sbufWriteU8(dst, rxConfig()->serialrx_provider);
sbufWriteU16(dst, rxConfig()->maxcheck); sbufWriteU16(dst, rxConfig()->maxcheck);
sbufWriteU16(dst, rxConfig()->midrc); sbufWriteU16(dst, PWM_RANGE_MIDDLE);
sbufWriteU16(dst, rxConfig()->mincheck); sbufWriteU16(dst, rxConfig()->mincheck);
sbufWriteU8(dst, rxConfig()->spektrum_sat_bind); sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
sbufWriteU16(dst, rxConfig()->rx_min_usec); sbufWriteU16(dst, rxConfig()->rx_min_usec);
@ -1595,7 +1595,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_MISC: case MSP_SET_MISC:
if (dataSize >= 22) { if (dataSize >= 22) {
rxConfigMutable()->midrc = constrain(sbufReadU16(src), MIDRC_MIN, MIDRC_MAX); sbufReadU16(src); // midrc
motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
@ -1635,7 +1635,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_INAV_SET_MISC: case MSP2_INAV_SET_MISC:
if (dataSize == 39) { if (dataSize == 39) {
rxConfigMutable()->midrc = constrain(sbufReadU16(src), MIDRC_MIN, MIDRC_MAX); sbufReadU16(src); // midrc
motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
@ -2264,7 +2264,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (dataSize >= 24) { if (dataSize >= 24) {
rxConfigMutable()->serialrx_provider = sbufReadU8(src); rxConfigMutable()->serialrx_provider = sbufReadU8(src);
rxConfigMutable()->maxcheck = sbufReadU16(src); rxConfigMutable()->maxcheck = sbufReadU16(src);
rxConfigMutable()->midrc = sbufReadU16(src); sbufReadU16(src); // midrc
rxConfigMutable()->mincheck = sbufReadU16(src); rxConfigMutable()->mincheck = sbufReadU16(src);
rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
rxConfigMutable()->rx_min_usec = sbufReadU16(src); rxConfigMutable()->rx_min_usec = sbufReadU16(src);

4
src/main/fc/fc_msp_box.c
View file

@ -58,6 +58,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXBEEPERON, "BEEPER", 13 }, { BOXBEEPERON, "BEEPER", 13 },
{ BOXLEDLOW, "LEDLOW", 15 }, { BOXLEDLOW, "LEDLOW", 15 },
{ BOXLIGHTS, "LIGHTS", 16 }, { BOXLIGHTS, "LIGHTS", 16 },
{ BOXOSD, "OSD SW", 19 },
{ BOXTELEMETRY, "TELEMETRY", 20 }, { BOXTELEMETRY, "TELEMETRY", 20 },
{ BOXAUTOTUNE, "AUTO TUNE", 21 }, { BOXAUTOTUNE, "AUTO TUNE", 21 },
{ BOXBLACKBOX, "BLACKBOX", 26 }, { BOXBLACKBOX, "BLACKBOX", 26 },
@ -228,6 +229,8 @@ void initActiveBoxIds(void)
} }
#endif #endif
activeBoxIds[activeBoxIdCount++] = BOXOSD;
#ifdef USE_TELEMETRY #ifdef USE_TELEMETRY
if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch)
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
@ -282,6 +285,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY);
CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM); CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX);

4
src/main/fc/fc_tasks.c
View file

@ -106,8 +106,10 @@ void taskUpdateBattery(timeUs_t currentTimeUs)
#ifdef USE_ADC #ifdef USE_ADC
if (feature(FEATURE_VBAT)) if (feature(FEATURE_VBAT))
batteryUpdate(BatMonitoringTimeSinceLastServiced); batteryUpdate(BatMonitoringTimeSinceLastServiced);
if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
powerMeterUpdate(BatMonitoringTimeSinceLastServiced); powerMeterUpdate(BatMonitoringTimeSinceLastServiced);
sagCompensatedVBatUpdate(currentTimeUs);
}
#endif #endif
batMonitoringLastServiced = currentTimeUs; batMonitoringLastServiced = currentTimeUs;
} }

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

@ -475,9 +475,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig)
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta; int delta;
if (rcData[channelIndex] > rxConfig()->midrc + 200) { if (rcData[channelIndex] > PWM_RANGE_MIDDLE + 200) {
delta = adjustmentState->config->data.stepConfig.step; delta = adjustmentState->config->data.stepConfig.step;
} else if (rcData[channelIndex] < rxConfig()->midrc - 200) { } else if (rcData[channelIndex] < PWM_RANGE_MIDDLE - 200) {
delta = 0 - adjustmentState->config->data.stepConfig.step; delta = 0 - adjustmentState->config->data.stepConfig.step;
} else { } else {
// returning the switch to the middle immediately resets the ready state // returning the switch to the middle immediately resets the ready state

10
src/main/fc/rc_controls.c
View file

@ -101,7 +101,7 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
throttleStatus_e calculateThrottleStatus(void) throttleStatus_e calculateThrottleStatus(void)
{ {
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle;
if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig()->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + deadband3d_throttle))) if (feature(FEATURE_3D) && (rcData[THROTTLE] > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rcData[THROTTLE] < (PWM_RANGE_MIDDLE + deadband3d_throttle)))
return THROTTLE_LOW; return THROTTLE_LOW;
else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck)) else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck))
return THROTTLE_LOW; return THROTTLE_LOW;
@ -111,8 +111,8 @@ throttleStatus_e calculateThrottleStatus(void)
rollPitchStatus_e calculateRollPitchCenterStatus(void) rollPitchStatus_e calculateRollPitchCenterStatus(void)
{ {
if (((rcData[PITCH] < (rxConfig()->midrc + AIRMODE_DEADBAND)) && (rcData[PITCH] > (rxConfig()->midrc -AIRMODE_DEADBAND))) if (((rcData[PITCH] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[PITCH] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))
&& ((rcData[ROLL] < (rxConfig()->midrc + AIRMODE_DEADBAND)) && (rcData[ROLL] > (rxConfig()->midrc -AIRMODE_DEADBAND)))) && ((rcData[ROLL] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[ROLL] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))))
return CENTERED; return CENTERED;
return NOT_CENTERED; return NOT_CENTERED;
@ -328,7 +328,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
} }
} }
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { int32_t getRcStickDeflection(int32_t axis) {
return MIN(ABS(rcData[axis] - midrc), 500); return MIN(ABS(rcData[axis] - PWM_RANGE_MIDDLE), 500);
} }

2
src/main/fc/rc_controls.h
View file

@ -99,4 +99,4 @@ throttleStatus_e calculateThrottleStatus(void);
rollPitchStatus_e calculateRollPitchCenterStatus(void); rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus); void processRcStickPositions(throttleStatus_e throttleStatus);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); int32_t getRcStickDeflection(int32_t axis);

2
src/main/fc/rc_modes.h
View file

@ -39,7 +39,7 @@ typedef enum {
BOXLEDLOW = 12, BOXLEDLOW = 12,
BOXLIGHTS = 13, BOXLIGHTS = 13,
BOXNAVLAUNCH = 14, BOXNAVLAUNCH = 14,
// BOXOSD = 15, BOXOSD = 15,
BOXTELEMETRY = 16, BOXTELEMETRY = 16,
BOXBLACKBOX = 17, BOXBLACKBOX = 17,
BOXFAILSAFE = 18, BOXFAILSAFE = 18,

5
src/main/fc/settings.c
View file

@ -4,10 +4,11 @@
#include "common/string_light.h" #include "common/string_light.h"
#include "common/utils.h" #include "common/utils.h"
#include "fc/settings_generated.h" #include "settings_generated.h"
#include "fc/settings.h" #include "fc/settings.h"
#include "fc/settings_generated.c" #include "settings_generated.c"
void setting_get_name(const setting_t *val, char *buf) void setting_get_name(const setting_t *val, char *buf)
{ {

2
src/main/fc/settings.h
View file

@ -6,7 +6,7 @@
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "fc/settings_generated.h" #include "settings_generated.h"
typedef struct lookupTableEntry_s { typedef struct lookupTableEntry_s {
const char * const *values; const char * const *values;

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

@ -7,7 +7,7 @@ tables:
values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "FAKE"] values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "FAKE"]
enum: accelerationSensor_e enum: accelerationSensor_e
- name: rangefinder_hardware - name: rangefinder_hardware
values: ["NONE", "HCSR04", "SRF10", "HCSR04I2C", "VL53L0X", "MSP", "UIB"] values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB"]
enum: rangefinderType_e enum: rangefinderType_e
- name: mag_hardware - name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "FAKE"] values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "FAKE"]
@ -67,7 +67,7 @@ tables:
- name: i2c_speed - name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes - name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR"] values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
- name: async_mode - name: async_mode
values: ["NONE", "GYRO", "ALL"] values: ["NONE", "GYRO", "ALL"]
- name: aux_operator - name: aux_operator
@ -321,10 +321,6 @@ groups:
- name: receiver_type - name: receiver_type
field: receiverType field: receiverType
table: receiver_type table: receiver_type
- name: mid_rc
field: midrc
min: MIDRC_MIN
max: MIDRC_MAX
- name: min_check - name: min_check
field: mincheck field: mincheck
min: PWM_RANGE_MIN min: PWM_RANGE_MIN
@ -427,6 +423,9 @@ groups:
- name: motor_pwm_protocol - name: motor_pwm_protocol
field: motorPwmProtocol field: motorPwmProtocol
table: motor_pwm_protocol table: motor_pwm_protocol
- name: throttle_vbat_compensation
field: throttleVBatCompensation
type: bool
- name: PG_FAILSAFE_CONFIG - name: PG_FAILSAFE_CONFIG
type: failsafeConfig_t type: failsafeConfig_t

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

@ -275,7 +275,7 @@ void failsafeApplyControlInput(void)
break; break;
case THROTTLE: case THROTTLE:
rcCommand[idx] = feature(FEATURE_3D) ? rxConfig()->midrc : motorConfig()->minthrottle; rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->minthrottle;
break; break;
} }
break; break;
@ -330,9 +330,9 @@ static bool failsafeCheckStickMotion(void)
if (failsafeConfig()->failsafe_stick_motion_threshold > 0) { if (failsafeConfig()->failsafe_stick_motion_threshold > 0) {
uint32_t totalRcDelta = 0; uint32_t totalRcDelta = 0;
totalRcDelta += ABS(rcData[ROLL] - rxConfig()->midrc); totalRcDelta += ABS(rcData[ROLL] - PWM_RANGE_MIDDLE);
totalRcDelta += ABS(rcData[PITCH] - rxConfig()->midrc); totalRcDelta += ABS(rcData[PITCH] - PWM_RANGE_MIDDLE);
totalRcDelta += ABS(rcData[YAW] - rxConfig()->midrc); totalRcDelta += ABS(rcData[YAW] - PWM_RANGE_MIDDLE);
return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold; return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold;
} }

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

@ -50,6 +50,8 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h"
//#define MIXER_DEBUG //#define MIXER_DEBUG
@ -87,7 +89,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
#define DEFAULT_MIN_THROTTLE 1150 #define DEFAULT_MIN_THROTTLE 1150
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 2);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.minthrottle = DEFAULT_MIN_THROTTLE, .minthrottle = DEFAULT_MIN_THROTTLE,
@ -96,7 +98,8 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.maxthrottle = 1850, .maxthrottle = 1850,
.mincommand = 1000, .mincommand = 1000,
.motorAccelTimeMs = 0, .motorAccelTimeMs = 0,
.motorDecelTimeMs = 0 .motorDecelTimeMs = 0,
.throttleVBatCompensation = false
); );
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -284,17 +287,17 @@ void mixTable(const float dT)
// Find min and max throttle based on condition. // Find min and max throttle based on condition.
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig()->deadband3d_low; throttleMax = flight3DConfig()->deadband3d_low;
throttleMin = motorConfig()->minthrottle; throttleMin = motorConfig()->minthrottle;
throttlePrevious = throttleCommand = rcCommand[THROTTLE]; throttlePrevious = throttleCommand = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle)) { // Positive handling } else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
throttleMax = motorConfig()->maxthrottle; throttleMax = motorConfig()->maxthrottle;
throttleMin = flight3DConfig()->deadband3d_high; throttleMin = flight3DConfig()->deadband3d_high;
throttlePrevious = throttleCommand = rcCommand[THROTTLE]; throttlePrevious = throttleCommand = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive } else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
throttleCommand = throttleMax = flight3DConfig()->deadband3d_low; throttleCommand = throttleMax = flight3DConfig()->deadband3d_low;
throttleMin = motorConfig()->minthrottle; throttleMin = motorConfig()->minthrottle;
} else { // Deadband handling from positive to negative } else { // Deadband handling from positive to negative
@ -305,6 +308,10 @@ void mixTable(const float dT)
throttleCommand = rcCommand[THROTTLE]; throttleCommand = rcCommand[THROTTLE];
throttleMin = motorConfig()->minthrottle; throttleMin = motorConfig()->minthrottle;
throttleMax = motorConfig()->maxthrottle; throttleMax = motorConfig()->maxthrottle;
// Throttle compensation based on battery voltage
if (motorConfig()->throttleVBatCompensation && STATE(FIXED_WING) && isAmperageConfigured() && feature(FEATURE_VBAT))
throttleCommand = MIN(throttleCommand * calculateThrottleCompensationFactor(), throttleMax);
} }
throttleRange = throttleMax - throttleMin; throttleRange = throttleMax - throttleMin;
@ -333,7 +340,7 @@ void mixTable(const float dT)
if (failsafeIsActive()) { if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
} else if (feature(FEATURE_3D)) { } else if (feature(FEATURE_3D)) {
if (throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)) { if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) {
motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low); motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low);
} else { } else {
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
@ -349,7 +356,7 @@ void mixTable(const float dT)
bool userMotorStop = !navigationIsFlyingAutonomousMode() && !failsafeIsActive() && (rcData[THROTTLE] < rxConfig()->mincheck); bool userMotorStop = !navigationIsFlyingAutonomousMode() && !failsafeIsActive() && (rcData[THROTTLE] < rxConfig()->mincheck);
if (failsafeMotorStop || navMotorStop || userMotorStop) { if (failsafeMotorStop || navMotorStop || userMotorStop) {
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
motor[i] = rxConfig()->midrc; motor[i] = PWM_RANGE_MIDDLE;
} }
else { else {
motor[i] = motorConfig()->mincommand; motor[i] = motorConfig()->mincommand;

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

@ -80,6 +80,7 @@ typedef struct motorConfig_s {
uint8_t motorPwmProtocol; uint8_t motorPwmProtocol;
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]
bool throttleVBatCompensation;
} motorConfig_t; } motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig); PG_DECLARE(motorConfig_t, motorConfig);

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

@ -392,8 +392,8 @@ void updatePIDCoefficients(void)
static float calcHorizonRateMagnitude(void) static float calcHorizonRateMagnitude(void)
{ {
// Figure out the raw stick positions // Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig()->midrc)); const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL));
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig()->midrc)); const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH));
const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f; const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f;
const float modeTransitionStickPos = constrain(pidBank()->pid[PID_LEVEL].D, 0, 100) / 100.0f; const float modeTransitionStickPos = constrain(pidBank()->pid[PID_LEVEL].D, 0, 100) / 100.0f;

34
src/main/flight/servos.c
View file

@ -230,7 +230,7 @@ void servoMixer(float dT)
input[INPUT_STABILIZED_YAW] = axisPID[YAW]; input[INPUT_STABILIZED_YAW] = axisPID[YAW];
// Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc) && if (feature(FEATURE_3D) && (rcData[THROTTLE] < PWM_RANGE_MIDDLE) &&
(mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) { (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) {
input[INPUT_STABILIZED_YAW] *= -1; input[INPUT_STABILIZED_YAW] *= -1;
} }
@ -254,22 +254,22 @@ void servoMixer(float dT)
// 2000 - 1500 = +500 // 2000 - 1500 = +500
// 1500 - 1500 = 0 // 1500 - 1500 = 0
// 1000 - 1500 = -500 // 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc; input[INPUT_RC_ROLL] = rcData[ROLL] - PWM_RANGE_MIDDLE;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc; input[INPUT_RC_PITCH] = rcData[PITCH] - PWM_RANGE_MIDDLE;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc; input[INPUT_RC_YAW] = rcData[YAW] - PWM_RANGE_MIDDLE;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc; input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH5] = rcData[AUX1] - rxConfig()->midrc; input[INPUT_RC_CH5] = rcData[AUX1] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH6] = rcData[AUX2] - rxConfig()->midrc; input[INPUT_RC_CH6] = rcData[AUX2] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH7] = rcData[AUX3] - rxConfig()->midrc; input[INPUT_RC_CH7] = rcData[AUX3] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH8] = rcData[AUX4] - rxConfig()->midrc; input[INPUT_RC_CH8] = rcData[AUX4] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH9] = rcData[AUX5] - rxConfig()->midrc; input[INPUT_RC_CH9] = rcData[AUX5] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH10] = rcData[AUX6] - rxConfig()->midrc; input[INPUT_RC_CH10] = rcData[AUX6] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH11] = rcData[AUX7] - rxConfig()->midrc; input[INPUT_RC_CH11] = rcData[AUX7] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH12] = rcData[AUX8] - rxConfig()->midrc; input[INPUT_RC_CH12] = rcData[AUX8] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH13] = rcData[AUX9] - rxConfig()->midrc; input[INPUT_RC_CH13] = rcData[AUX9] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH14] = rcData[AUX10] - rxConfig()->midrc; input[INPUT_RC_CH14] = rcData[AUX10] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH15] = rcData[AUX11] - rxConfig()->midrc; input[INPUT_RC_CH15] = rcData[AUX11] - PWM_RANGE_MIDDLE;
input[INPUT_RC_CH16] = rcData[AUX12] - rxConfig()->midrc; input[INPUT_RC_CH16] = rcData[AUX12] - PWM_RANGE_MIDDLE;
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = 0; servo[i] = 0;

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

@ -107,7 +107,7 @@
// Adjust OSD_MESSAGE's default position when // Adjust OSD_MESSAGE's default position when
// changing OSD_MESSAGE_LENGTH // changing OSD_MESSAGE_LENGTH
#define OSD_MESSAGE_LENGTH 28 #define OSD_MESSAGE_LENGTH 28
#define OSD_ALTERNATING_TEXT(ms, num_choices) ((millis() / ms) % num_choices) #define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices)
#define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0' #define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0'
// Wrap all string constants intenteded for display as messages with // Wrap all string constants intenteded for display as messages with
// this macro to ensure compile time length validation. // this macro to ensure compile time length validation.
@ -678,13 +678,12 @@ static const char * navigationStateMessage(void)
case MW_NAV_STATE_LAND_START: case MW_NAV_STATE_LAND_START:
return OSD_MESSAGE_STR("STARTING EMERGENCY LANDING"); return OSD_MESSAGE_STR("STARTING EMERGENCY LANDING");
case MW_NAV_STATE_LAND_IN_PROGRESS: case MW_NAV_STATE_LAND_IN_PROGRESS:
if (!navigationRTHAllowsLanding()) {
if (STATE(FIXED_WING)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
}
return OSD_MESSAGE_STR("HOVERING");
}
return OSD_MESSAGE_STR("LANDING"); return OSD_MESSAGE_STR("LANDING");
case MW_NAV_STATE_HOVER_ABOVE_HOME:
if (STATE(FIXED_WING)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
}
return OSD_MESSAGE_STR("HOVERING");
case MW_NAV_STATE_LANDED: case MW_NAV_STATE_LANDED:
return OSD_MESSAGE_STR("LANDED"); return OSD_MESSAGE_STR("LANDED");
case MW_NAV_STATE_LAND_SETTLE: case MW_NAV_STATE_LAND_SETTLE:
@ -934,9 +933,9 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
break; break;
} }
if (STATE(GPS_FIX) && poiDistance > scale) { if (STATE(GPS_FIX)) {
int directionToPoi = osdGetHeadingAngle(poiDirection + referenceHeading); int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
float poiAngle = DEGREES_TO_RADIANS(directionToPoi); float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
float poiSin = sin_approx(poiAngle); float poiSin = sin_approx(poiAngle);
float poiCos = cos_approx(poiAngle); float poiCos = cos_approx(poiAngle);
@ -947,7 +946,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
int points = poiDistance / (float)(scale / charHeight); int points = poiDistance / (float)(scale / charHeight);
float pointsX = points * poiSin; float pointsX = points * poiSin;
int poiX = midX + roundf(pointsX / charWidth); int poiX = midX - roundf(pointsX / charWidth);
if (poiX < minX || poiX > maxX) { if (poiX < minX || poiX > maxX) {
continue; continue;
} }
@ -958,18 +957,21 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
continue; continue;
} }
if (poiX == midX && poiY == midY && centerSym != SYM_BLANK) { if (poiX == midX && poiY == midY) {
// We're over the map center symbol, so we would be drawing // We're over the map center symbol, so we would be drawing
// over it even if we increased the scale. No reason to run // over it even if we increased the scale. Alternate between
// this loop 50 times. // drawing the center symbol or drawing the POI.
break; if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
} break;
}
} else {
uint8_t c; uint8_t c;
if (displayReadCharWithAttr(osdDisplayPort, poiY, poiY, &c, NULL) && c != SYM_BLANK) { if (displayReadCharWithAttr(osdDisplayPort, poiY, poiY, &c, NULL) && c != SYM_BLANK) {
// Something else written here, increase scale. If the display doesn't support reading // Something else written here, increase scale. If the display doesn't support reading
// back characters, we assume there's nothing. // back characters, we assume there's nothing.
continue; continue;
}
} }
// Draw the point on the map // Draw the point on the map
@ -1050,6 +1052,19 @@ static bool osdDrawSingleElement(uint8_t item)
displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
return true; return true;
case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE:
osdFormatBatteryChargeSymbol(buff);
buff[1] = '\0';
osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
osdFormatCentiNumber(buff, getSagCompensatedBatteryVoltage(), 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
strcat(buff, "V");
if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getSagCompensatedBatteryVoltage() <= getBatteryWarningVoltage()))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
return true;
case OSD_CURRENT_DRAW: case OSD_CURRENT_DRAW:
buff[0] = SYM_AMP; buff[0] = SYM_AMP;
osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3); osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3);
@ -1590,7 +1605,7 @@ static bool osdDrawSingleElement(uint8_t item)
messages[messageCount++] = navStateFSMessage; messages[messageCount++] = navStateFSMessage;
} }
if (messageCount > 0) { if (messageCount > 0) {
message = messages[OSD_ALTERNATING_TEXT(1000, messageCount)]; message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
if (message == failsafeInfoMessage) { if (message == failsafeInfoMessage) {
// failsafeInfoMessage is not useful for recovering // failsafeInfoMessage is not useful for recovering
// a lost model, but might help avoiding a crash. // a lost model, but might help avoiding a crash.
@ -1630,12 +1645,12 @@ static bool osdDrawSingleElement(uint8_t item)
// Pick one of the available messages. Each message lasts // Pick one of the available messages. Each message lasts
// a second. // a second.
if (messageCount > 0) { if (messageCount > 0) {
message = messages[OSD_ALTERNATING_TEXT(1000, messageCount)]; message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
} }
} }
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
// Check if we're unable to arm for some reason // Check if we're unable to arm for some reason
if (OSD_ALTERNATING_TEXT(1000, 2) == 0) { if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
message = "UNABLE TO ARM"; message = "UNABLE TO ARM";
TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
} else { } else {
@ -1895,6 +1910,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
{ {
osdConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG;
osdConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG;
osdConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1);
osdConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG;
//line 2 //line 2
osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);
@ -2224,6 +2241,12 @@ static void osdRefresh(timeUs_t currentTimeUs)
{ {
static timeUs_t lastTimeUs = 0; static timeUs_t lastTimeUs = 0;
if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu)) {
displayClearScreen(osdDisplayPort);
armState = ARMING_FLAG(ARMED);
return;
}
// detect arm/disarm // detect arm/disarm
if (armState != ARMING_FLAG(ARMED)) { if (armState != ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {

1
src/main/io/osd.h
View file

@ -83,6 +83,7 @@ typedef enum {
OSD_DEBUG, // Number 46. Intentionally absent from configurator and CMS. Set it from CLI. OSD_DEBUG, // Number 46. Intentionally absent from configurator and CMS. Set it from CLI.
OSD_WIND_SPEED_HORIZONTAL, OSD_WIND_SPEED_HORIZONTAL,
OSD_WIND_SPEED_VERTICAL, OSD_WIND_SPEED_VERTICAL,
OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
OSD_ITEM_COUNT // MUST BE LAST OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e; } osd_items_e;

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

@ -199,6 +199,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
@ -368,6 +369,24 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
.timeoutMs = 500,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -784,7 +803,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
// If close to home - reset home position and land // If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) { if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
@ -932,12 +951,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// Wait until target heading is reached (with 15 deg margin for error) // Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
resetLandingDetector(); resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else { else {
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
resetLandingDetector(); resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else if (!validateRTHSanityChecker()) { else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover // Continue to check for RTH sanity during pre-landing hover
@ -954,6 +973,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
} }
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
@ -970,28 +999,26 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
if (navigationRTHAllowsLanding()) { float descentVelLimited = 0;
float descentVelLimited = 0;
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
// Do not allow descent velocity slower than -30cm/s so the landing detector works. // Do not allow descent velocity slower than -30cm/s so the landing detector works.
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
}
else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
// Do not allow descent velocity slower than -50cm/s so the landing detector works.
descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
}
updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
} }
else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
// Do not allow descent velocity slower than -50cm/s so the landing detector works.
descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
}
updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -1331,12 +1358,16 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
newProportional = error * pid->param.kP * gainScaler; newProportional = error * pid->param.kP * gainScaler;
/* D-term */ /* D-term */
if (pid->reset) {
pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
pid->reset = false;
}
if (pidFlags & PID_DTERM_FROM_ERROR) { if (pidFlags & PID_DTERM_FROM_ERROR) {
/* Error-tracking D-term */ /* Error-tracking D-term */
newDerivative = (error - pid->last_input) / dt; newDerivative = (error - pid->last_input) / dt;
pid->last_input = error; pid->last_input = error;
} } else {
else {
/* Measurement tracking D-term */ /* Measurement tracking D-term */
newDerivative = -(measurement - pid->last_input) / dt; newDerivative = -(measurement - pid->last_input) / dt;
pid->last_input = measurement; pid->last_input = measurement;
@ -1378,6 +1409,7 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
void navPidReset(pidController_t *pid) void navPidReset(pidController_t *pid)
{ {
pid->reset = true;
pid->integrator = 0.0f; pid->integrator = 0.0f;
pid->last_input = 0.0f; pid->last_input = 0.0f;
pid->dterm_filter_state.state = 0.0f; pid->dterm_filter_state.state = 0.0f;
@ -1532,9 +1564,30 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateActualHeading(bool headingValid, int32_t newHeading) void updateActualHeading(bool headingValid, int32_t newHeading)
{ {
/* Update heading */ /* Update heading. Check if we're acquiring a valid heading for the
* first time and update home heading accordingly.
*/
navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE;
if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE &&
(posControl.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) &&
(posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
// Home was stored using the fake heading (assuming boot as 0deg). Calculate
// the offset from the fake to the actual yaw and apply the same rotation
// to the home point.
int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
posControl.homePosition.yaw += fakeToRealYawOffset;
if (posControl.homePosition.yaw < 0) {
posControl.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
}
if (posControl.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
posControl.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
}
posControl.homeFlags |= NAV_HOME_VALID_HEADING;
}
posControl.actualState.yaw = newHeading; posControl.actualState.yaw = newHeading;
posControl.flags.estHeadingStatus = headingValid ? EST_TRUSTED : EST_NONE; posControl.flags.estHeadingStatus = newEstHeading;
/* Precompute sin/cos of yaw angle */ /* Precompute sin/cos of yaw angle */
posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading)); posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
@ -1678,30 +1731,43 @@ bool validateRTHSanityChecker(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Reset home position to current position * Reset home position to current position
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask) void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
{ {
// XY-position // XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) { if ((useMask & NAV_POS_UPDATE_XY) != 0) {
posControl.homePosition.pos.x = pos->x; posControl.homePosition.pos.x = pos->x;
posControl.homePosition.pos.y = pos->y; posControl.homePosition.pos.y = pos->y;
if (homeFlags & NAV_HOME_VALID_XY) {
posControl.homeFlags |= NAV_HOME_VALID_XY;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_XY;
}
} }
// Z-position // Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) { if ((useMask & NAV_POS_UPDATE_Z) != 0) {
posControl.homePosition.pos.z = pos->z; posControl.homePosition.pos.z = pos->z;
if (homeFlags & NAV_HOME_VALID_Z) {
posControl.homeFlags |= NAV_HOME_VALID_Z;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_Z;
}
} }
// Heading // Heading
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) { if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
// Heading // Heading
posControl.homePosition.yaw = yaw; posControl.homePosition.yaw = yaw;
if (homeFlags & NAV_HOME_VALID_HEADING) {
posControl.homeFlags |= NAV_HOME_VALID_HEADING;
} else {
posControl.homeFlags &= ~NAV_HOME_VALID_HEADING;
}
} }
posControl.homeDistance = 0; posControl.homeDistance = 0;
posControl.homeDirection = 0; posControl.homeDirection = 0;
posControl.flags.isHomeValid = true;
// Update target RTH altitude as a waypoint above home // Update target RTH altitude as a waypoint above home
posControl.homeWaypointAbove = posControl.homePosition; posControl.homeWaypointAbove = posControl.homePosition;
updateDesiredRTHAltitude(); updateDesiredRTHAltitude();
@ -1710,6 +1776,21 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
ENABLE_STATE(GPS_FIX_HOME); ENABLE_STATE(GPS_FIX_HOME);
} }
static navigationHomeFlags_t navigationActualStateHomeValidity(void)
{
navigationHomeFlags_t flags = 0;
if (posControl.flags.estPosStatus >= EST_USABLE) {
flags |= NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
}
if (posControl.flags.estHeadingStatus >= EST_USABLE) {
flags |= NAV_HOME_VALID_HEADING;
}
return flags;
}
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Update home position, calculate distance and bearing to home * Update home position, calculate distance and bearing to home
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -1718,7 +1799,8 @@ void updateHomePosition(void)
// Disarmed and have a valid position, constantly update home // Disarmed and have a valid position, constantly update home
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
if (posControl.flags.estPosStatus >= EST_USABLE) { if (posControl.flags.estPosStatus >= EST_USABLE) {
bool setHome = !posControl.flags.isHomeValid; const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
bool setHome = (posControl.homeFlags & validHomeFlags) != validHomeFlags;
switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
case NAV_RESET_NEVER: case NAV_RESET_NEVER:
break; break;
@ -1730,7 +1812,7 @@ void updateHomePosition(void)
break; break;
} }
if (setHome) { if (setHome) {
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING ); setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
} }
} }
} }
@ -1741,7 +1823,7 @@ void updateHomePosition(void)
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags); setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
isHomeResetAllowed = false; isHomeResetAllowed = false;
} }
} }
@ -2120,7 +2202,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) { if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
} }
// WP #255 - special waypoint - directly set desiredPosition // WP #255 - special waypoint - directly set desiredPosition
// Only valid when armed and in poshold mode // Only valid when armed and in poshold mode
@ -2441,9 +2523,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
} }
} }
else { else {
// If we were in LAUNCH mode - force switch to IDLE // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
if (FLIGHT_MODE(NAV_LAUNCH_MODE)) { if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE; throttleStatus_e throttleStatus = calculateThrottleStatus();
if (throttleStatus != THROTTLE_LOW)
return NAV_FSM_EVENT_NONE;
else
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} }
} }
} }
@ -2554,8 +2640,8 @@ bool navigationTerrainFollowingEnabled(void)
bool navigationBlockArming(void) bool navigationBlockArming(void)
{ {
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD); const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP)); const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
bool shouldBlockArming = false; bool shouldBlockArming = false;
if (!navConfig()->general.flags.extra_arming_safety) if (!navConfig()->general.flags.extra_arming_safety)
@ -2767,7 +2853,7 @@ rthState_e getStateOfForcedRTH(void)
bool navigationIsControllingThrottle(void) bool navigationIsControllingThrottle(void)
{ {
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) || (STATE(FIXED_WING) && (stateFlags & (NAV_CTL_POS))); return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
} }
bool navigationIsFlyingAutonomousMode(void) bool navigationIsFlyingAutonomousMode(void)

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

@ -230,7 +230,8 @@ typedef enum {
MW_NAV_STATE_LAND_IN_PROGRESS, // Land in Progress MW_NAV_STATE_LAND_IN_PROGRESS, // Land in Progress
MW_NAV_STATE_LANDED, // Landed MW_NAV_STATE_LANDED, // Landed
MW_NAV_STATE_LAND_SETTLE, // Settling before land MW_NAV_STATE_LAND_SETTLE, // Settling before land
MW_NAV_STATE_LAND_START_DESCENT // Start descent MW_NAV_STATE_LAND_START_DESCENT, // Start descent
MW_NAV_STATE_HOVER_ABOVE_HOME // Hover/Loitering above home
} navSystemStatus_State_e; } navSystemStatus_State_e;
typedef enum { typedef enum {

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

@ -412,28 +412,24 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{ {
int16_t pitchCorrection = 0; // >0 climb, <0 dive
int16_t rollCorrection = 0; // >0 right, <0 left
int16_t throttleCorrection = 0; // raw throttle
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle; int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle; int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle;
// Mix Pitch/Roll/Throttle
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection += posControl.rcAdjustment[ROLL]; // ROLL >0 right, <0 left
int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
} }
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) { if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
pitchCorrection += posControl.rcAdjustment[PITCH]; // PITCH >0 dive, <0 climb
throttleCorrection += DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle; int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
int16_t throttleCorrection = DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle;
#ifdef NAV_FIXED_WING_LANDING #ifdef NAV_FIXED_WING_LANDING
if (navStateFlags & NAV_CTL_LAND) { if (navStateFlags & NAV_CTL_LAND) {
/* // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
* During LAND we do not allow to raise THROTTLE when nose is up
* to reduce speed
*/
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0); throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
} else { } else {
#endif #endif
@ -441,28 +437,13 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
#ifdef NAV_FIXED_WING_LANDING #ifdef NAV_FIXED_WING_LANDING
} }
#endif #endif
}
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND // Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) { if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs); throttleCorrection += applyFixedWingMinSpeedController(currentTimeUs);
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
} }
// Limit and apply
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH correction is measured according to altitude: <0 - dive/lose altitude, >0 - climb/gain altitude
// PITCH angle is measured in opposite direction ( >0 - dive, <0 - climb)
pitchCorrection = constrain(pitchCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
}
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rollCorrection = constrain(rollCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
}
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle); rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
} }
@ -475,23 +456,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (navStateFlags & NAV_CTL_LAND) { if (navStateFlags & NAV_CTL_LAND) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) || if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) { ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
/*
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
*/
rcCommand[THROTTLE] = motorConfig()->minthrottle; rcCommand[THROTTLE] = motorConfig()->minthrottle;
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
/* // Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
* Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
*/
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
/* // Stabilize PITCH angle into shallow dive as specified by the nav_fw_land_dive_angle setting (default value is 2 - defined in navigation.c).
* Stabilize PITCH angle into shallow dive as specified by the
* nav_fw_land_dive_angle setting (default value is 2 - defined
* in navigation.c).
* PITCH angle is measured: >0 - dive, <0 - climb)
*/
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
} }
} }

75
src/main/navigation/navigation_private.h
View file

@ -62,6 +62,14 @@ typedef enum {
EST_TRUSTED = 2 // Estimate is usable and based on actual sensor data EST_TRUSTED = 2 // Estimate is usable and based on actual sensor data
} navigationEstimateStatus_e; } navigationEstimateStatus_e;
typedef enum {
NAV_HOME_INVALID = 0,
NAV_HOME_VALID_XY = 1 << 0,
NAV_HOME_VALID_Z = 1 << 1,
NAV_HOME_VALID_HEADING = 1 << 2,
NAV_HOME_VALID_ALL = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z | NAV_HOME_VALID_HEADING,
} navigationHomeFlags_t;
typedef struct navigationFlags_s { typedef struct navigationFlags_s {
bool horizontalPositionDataNew; bool horizontalPositionDataNew;
bool verticalPositionDataNew; bool verticalPositionDataNew;
@ -85,8 +93,6 @@ typedef struct navigationFlags_s {
bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings)
bool forcedRTHActivated; bool forcedRTHActivated;
bool isHomeValid;
} navigationFlags_t; } navigationFlags_t;
typedef struct { typedef struct {
@ -107,6 +113,7 @@ typedef enum {
} pidControllerFlags_e; } pidControllerFlags_e;
typedef struct { typedef struct {
bool reset;
pidControllerParam_t param; pidControllerParam_t param;
pt1Filter_t dterm_filter_state; // last derivative for low-pass filter pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
float integrator; // integrator value float integrator; // integrator value
@ -163,6 +170,7 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_ALTHOLD, NAV_FSM_EVENT_SWITCH_TO_ALTHOLD,
NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D, NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D,
NAV_FSM_EVENT_SWITCH_TO_RTH, NAV_FSM_EVENT_SWITCH_TO_RTH,
NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT,
NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING, NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING,
NAV_FSM_EVENT_SWITCH_TO_LAUNCH, NAV_FSM_EVENT_SWITCH_TO_LAUNCH,
@ -179,43 +187,44 @@ typedef enum {
// This enum is used to keep values in blackbox logs stable, so we can // This enum is used to keep values in blackbox logs stable, so we can
// freely change navigationFSMState_t. // freely change navigationFSMState_t.
typedef enum { typedef enum {
NAV_PERSISTENT_ID_UNDEFINED = 0, // 0 NAV_PERSISTENT_ID_UNDEFINED = 0,
NAV_PERSISTENT_ID_IDLE, // 1 NAV_PERSISTENT_ID_IDLE = 1,
NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE, // 2 NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE = 2,
NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS, // 3 NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS = 3,
NAV_PERSISTENT_ID_UNUSED_1, // 4, was NAV_STATE_POSHOLD_2D_INITIALIZE NAV_PERSISTENT_ID_UNUSED_1 = 4, // was NAV_STATE_POSHOLD_2D_INITIALIZE
NAV_PERSISTENT_ID_UNUSED_2, // 5, was NAV_STATE_POSHOLD_2D_IN_PROGRESS NAV_PERSISTENT_ID_UNUSED_2 = 5, // was NAV_STATE_POSHOLD_2D_IN_PROGRESS
NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE, // 6 NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE = 6,
NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS, // 7 NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS = 7,
NAV_PERSISTENT_ID_RTH_INITIALIZE, // 8 NAV_PERSISTENT_ID_RTH_INITIALIZE = 8,
NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, // 9 NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9,
NAV_PERSISTENT_ID_RTH_HEAD_HOME, // 10 NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10,
NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING, // 11 NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11,
NAV_PERSISTENT_ID_RTH_LANDING, // 12 NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 29,
NAV_PERSISTENT_ID_RTH_FINISHING, // 13 NAV_PERSISTENT_ID_RTH_LANDING = 12,
NAV_PERSISTENT_ID_RTH_FINISHED, // 14 NAV_PERSISTENT_ID_RTH_FINISHING = 13,
NAV_PERSISTENT_ID_RTH_FINISHED = 14,
NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE, // 15 NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE = 15,
NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION, // 16 NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION = 16,
NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS, // 17 NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS = 17,
NAV_PERSISTENT_ID_WAYPOINT_REACHED, // 18 NAV_PERSISTENT_ID_WAYPOINT_REACHED = 18,
NAV_PERSISTENT_ID_WAYPOINT_NEXT, // 19 NAV_PERSISTENT_ID_WAYPOINT_NEXT = 19,
NAV_PERSISTENT_ID_WAYPOINT_FINISHED, // 20 NAV_PERSISTENT_ID_WAYPOINT_FINISHED = 20,
NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND, // 21 NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND = 21,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, // 22 NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE = 22,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS, // 23 NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS = 23,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED, // 24 NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED = 24,
NAV_PERSISTENT_ID_LAUNCH_INITIALIZE, // 25 NAV_PERSISTENT_ID_LAUNCH_INITIALIZE = 25,
NAV_PERSISTENT_ID_LAUNCH_WAIT, // 26 NAV_PERSISTENT_ID_LAUNCH_WAIT = 26,
NAV_PERSISTENT_ID_UNUSED_3, // 27, was NAV_STATE_LAUNCH_MOTOR_DELAY NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS, // 28 NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28,
} navigationPersistentId_e; } navigationPersistentId_e;
typedef enum { typedef enum {
@ -233,6 +242,7 @@ typedef enum {
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
NAV_STATE_RTH_HEAD_HOME, NAV_STATE_RTH_HEAD_HOME,
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
NAV_STATE_RTH_HOVER_ABOVE_HOME,
NAV_STATE_RTH_LANDING, NAV_STATE_RTH_LANDING,
NAV_STATE_RTH_FINISHING, NAV_STATE_RTH_FINISHING,
NAV_STATE_RTH_FINISHED, NAV_STATE_RTH_FINISHED,
@ -326,6 +336,7 @@ typedef struct {
rthSanityChecker_t rthSanityChecker; rthSanityChecker_t rthSanityChecker;
navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched) navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched)
navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude
navigationHomeFlags_t homeFlags;
uint32_t homeDistance; // cm uint32_t homeDistance; // cm
int32_t homeDirection; // deg*100 int32_t homeDirection; // deg*100
@ -362,7 +373,7 @@ bool isLandingDetected(void);
navigationFSMStateFlags_t navGetCurrentStateFlags(void); navigationFSMStateFlags_t navGetCurrentStateFlags(void);
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);

2
src/main/rx/crsf.c
View file

@ -257,7 +257,7 @@ void crsfRxSendTelemetryData(void)
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408; crsfChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
} }
rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL; rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;

17
src/main/rx/fport.c
View file

@ -48,6 +48,7 @@
#define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define FPORT_MAX_TELEMETRY_AGE_MS 500 #define FPORT_MAX_TELEMETRY_AGE_MS 500
#define FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS 2 // Needed to avoid lost sensors on FPort, see #3198
#define FPORT_FRAME_MARKER 0x7E #define FPORT_FRAME_MARKER 0x7E
@ -340,7 +341,17 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
if (clearToSend) { if (clearToSend) {
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
processSmartPortTelemetry(mspPayload, &clearToSend, NULL); uint8_t *consecutiveSensorCount = rxRuntimeConfig->frameData;
if (*consecutiveSensorCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS && !smartPortPayloadContainsMSP(mspPayload)) {
// Stop one cycle to avoid saturating the buffer in the RX, since the
// downstream bandwidth doesn't allow sensor sensors on every cycle.
// We allow MSP frames to run over the resting period, expecting that
// the caller won't flood us with requests.
*consecutiveSensorCount = 0;
} else {
(*consecutiveSensorCount)++;
processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
}
if (clearToSend) { if (clearToSend) {
smartPortWriteFrameFport(&emptySmartPortFrame); smartPortWriteFrameFport(&emptySmartPortFrame);
@ -358,8 +369,10 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
static uint8_t consecutiveSensorCount = 0;
rxRuntimeConfig->channelData = sbusChannelData; rxRuntimeConfig->channelData = sbusChannelData;
sbusChannelsInit(rxConfig, rxRuntimeConfig); rxRuntimeConfig->frameData = &consecutiveSensorCount;
sbusChannelsInit(rxRuntimeConfig);
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000; rxRuntimeConfig->rxRefreshRate = 11000;

6
src/main/rx/rx.c
View file

@ -113,7 +113,6 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 4);
#define DEFAULT_RX_TYPE RX_TYPE_NONE #define DEFAULT_RX_TYPE RX_TYPE_NONE
#endif #endif
#define RX_MIDRC 1500
#define RX_MIN_USEX 885 #define RX_MIN_USEX 885
PG_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.receiverType = DEFAULT_RX_TYPE, .receiverType = DEFAULT_RX_TYPE,
@ -123,7 +122,6 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL, .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
.spektrum_sat_bind = 0, .spektrum_sat_bind = 0,
.serialrx_inverted = 0, .serialrx_inverted = 0,
.midrc = RX_MIDRC,
.mincheck = 1100, .mincheck = 1100,
.maxcheck = 1900, .maxcheck = 1900,
.rx_min_usec = RX_MIN_USEX, // any of first 4 channels below this value will trigger rx loss detection .rx_min_usec = RX_MIN_USEX, // any of first 4 channels below this value will trigger rx loss detection
@ -244,11 +242,11 @@ void rxInit(void)
rcSampleIndex = 0; rcSampleIndex = 0;
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig()->midrc; rcData[i] = PWM_RANGE_MIDDLE;
rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
} }
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; rcData[THROTTLE] = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec;
// Initialize ARM switch to OFF position when arming via switch is defined // Initialize ARM switch to OFF position when arming via switch is defined
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {

2
src/main/rx/rx.h
View file

@ -121,7 +121,7 @@ typedef struct rxConfig_s {
uint8_t rssi_channel; uint8_t rssi_channel;
uint8_t rssi_scale; uint8_t rssi_scale;
uint8_t rssiInvert; uint8_t rssiInvert;
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t __reserved; // was micrd
uint16_t mincheck; // minimum rc end uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end uint16_t maxcheck; // maximum rc end
uint16_t rx_min_usec; uint16_t rx_min_usec;

2
src/main/rx/sbus.c
View file

@ -193,7 +193,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
rxRuntimeConfig->channelData = sbusChannelData; rxRuntimeConfig->channelData = sbusChannelData;
rxRuntimeConfig->frameData = &sbusFrameData; rxRuntimeConfig->frameData = &sbusFrameData;
sbusChannelsInit(rxConfig, rxRuntimeConfig); sbusChannelsInit(rxRuntimeConfig);
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000; rxRuntimeConfig->rxRefreshRate = 11000;

4
src/main/rx/sbus_channels.c
View file

@ -84,11 +84,11 @@ static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig,
return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880; return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
} }
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC; rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) { for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
rxRuntimeConfig->channelData[b] = (16 * rxConfig->midrc) / 10 - 1408; rxRuntimeConfig->channelData[b] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
} }
} }
#endif #endif

2
src/main/rx/sbus_channels.h
View file

@ -49,4 +49,4 @@ typedef struct sbusChannels_s {
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels); uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig);

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

@ -42,6 +42,7 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "build/debug.h"
#define ADCVREF 3300 // in mV (3300 = 3.3V) #define ADCVREF 3300 // in mV (3300 = 3.3V)
@ -65,6 +66,8 @@ static bool batteryFullWhenPluggedIn = false;
static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
static uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC static uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC
static uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC static uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC
static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm
static uint16_t sagCompensatedVBat = 0; // calculated no load vbat
static int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) static int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
static int32_t power = 0; // power draw in cW (0.01W resolution) static int32_t power = 0; // power draw in cW (0.01W resolution)
@ -122,20 +125,20 @@ void batteryInit(void)
batteryCriticalVoltage = 0; batteryCriticalVoltage = 0;
} }
static void updateBatteryVoltage(uint32_t vbatTimeDelta) static void updateBatteryVoltage(timeUs_t timeDelta)
{ {
uint16_t vbatSample; uint16_t vbatSample;
static pt1Filter_t vbatFilterState; static pt1Filter_t vbatFilterState;
// store the battery voltage with some other recent battery voltage readings // store the battery voltage with some other recent battery voltage readings
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f); vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, timeDelta * 1e-6f);
vbat = batteryAdcToVoltage(vbatSample); vbat = batteryAdcToVoltage(vbatSample);
} }
void batteryUpdate(uint32_t vbatTimeDelta) void batteryUpdate(timeUs_t timeDelta)
{ {
updateBatteryVoltage(vbatTimeDelta); updateBatteryVoltage(timeDelta);
/* battery has just been connected*/ /* battery has just been connected*/
if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD)
@ -147,7 +150,7 @@ void batteryUpdate(uint32_t vbatTimeDelta)
We only do this on the ground so don't care if we do block, not We only do this on the ground so don't care if we do block, not
worse than original code anyway*/ worse than original code anyway*/
delay(VBATT_STABLE_DELAY); delay(VBATT_STABLE_DELAY);
updateBatteryVoltage(vbatTimeDelta); updateBatteryVoltage(timeDelta);
unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig()->voltage.cellDetect) + 1; unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig()->voltage.cellDetect) + 1;
if (cells > 8) cells = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) if (cells > 8) cells = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
@ -245,6 +248,16 @@ uint16_t getBatteryVoltage(void)
return vbat; return vbat;
} }
uint16_t getSagCompensatedBatteryVoltage(void)
{
return sagCompensatedVBat;
}
float calculateThrottleCompensationFactor(void)
{
return batteryFullVoltage / sagCompensatedVBat;
}
uint16_t getBatteryVoltageLatestADC(void) uint16_t getBatteryVoltageLatestADC(void)
{ {
return vbatLatestADC; return vbatLatestADC;
@ -275,7 +288,7 @@ uint32_t getBatteryRemainingCapacity(void)
bool isAmperageConfigured(void) bool isAmperageConfigured(void)
{ {
return batteryConfig()->current.type != CURRENT_SENSOR_NONE; return feature(FEATURE_CURRENT_METER) && batteryConfig()->current.type != CURRENT_SENSOR_NONE;
} }
int32_t getAmperage(void) int32_t getAmperage(void)
@ -304,7 +317,7 @@ int32_t getMWhDrawn(void)
} }
void currentMeterUpdate(int32_t timeDelta) void currentMeterUpdate(timeUs_t timeDelta)
{ {
static pt1Filter_t amperageFilterState; static pt1Filter_t amperageFilterState;
static int64_t mAhdrawnRaw = 0; static int64_t mAhdrawnRaw = 0;
@ -333,15 +346,51 @@ void currentMeterUpdate(int32_t timeDelta)
mAhDrawn = mAhdrawnRaw / (3600 * 100); mAhDrawn = mAhdrawnRaw / (3600 * 100);
} }
void powerMeterUpdate(int32_t timeDelta) void powerMeterUpdate(timeUs_t timeDelta)
{ {
static int64_t mWhDrawnRaw = 0; static int64_t mWhDrawnRaw = 0;
uint32_t power_mW = amperage * vbat / 10;
power = amperage * vbat / 100; // power unit is cW (0.01W resolution) power = amperage * vbat / 100; // power unit is cW (0.01W resolution)
mWhDrawnRaw += (power_mW * timeDelta) / 10000; int32_t heatLossesCompensatedPower_mW = amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000;
mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000;
mWhDrawn = mWhDrawnRaw / (3600 * 100); mWhDrawn = mWhDrawnRaw / (3600 * 100);
} }
void sagCompensatedVBatUpdate(timeUs_t currentTime)
{
static timeUs_t recordTimestamp = 0;
static int32_t amperageRecord;
static uint16_t vbatRecord;
if (batteryState == BATTERY_NOT_PRESENT) {
recordTimestamp = 0;
powerSupplyImpedance = 0;
sagCompensatedVBat = vbat;
} else {
if (cmpTimeUs(currentTime, recordTimestamp) > 20000000)
recordTimestamp = 0;
if (!recordTimestamp) {
amperageRecord = amperage;
vbatRecord = vbat;
recordTimestamp = currentTime;
} else if ((amperage - amperageRecord >= 400) && ((int16_t)vbatRecord - vbat >= 10)) {
powerSupplyImpedance = (int32_t)(vbatRecord - vbat) * 1000 / (amperage - amperageRecord);
amperageRecord = amperage;
vbatRecord = vbat;
recordTimestamp = currentTime;
}
sagCompensatedVBat = MIN(batteryFullVoltage, vbat + powerSupplyImpedance * amperage / 1000);
}
DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance);
DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 1, sagCompensatedVBat);
}
uint8_t calculateBatteryPercentage(void) uint8_t calculateBatteryPercentage(void)
{ {
if (batteryState == BATTERY_NOT_PRESENT) if (batteryState == BATTERY_NOT_PRESENT)

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

@ -18,6 +18,7 @@
#pragma once #pragma once
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "drivers/time.h"
#ifndef VBAT_SCALE_DEFAULT #ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 1100 #define VBAT_SCALE_DEFAULT 1100
@ -83,11 +84,11 @@ uint16_t batteryAdcToVoltage(uint16_t src);
batteryState_e getBatteryState(void); batteryState_e getBatteryState(void);
bool batteryWasFullWhenPluggedIn(void); bool batteryWasFullWhenPluggedIn(void);
bool batteryUsesCapacityThresholds(void); bool batteryUsesCapacityThresholds(void);
void batteryUpdate(uint32_t vbatTimeDelta);
void batteryInit(void); void batteryInit(void);
bool isBatteryVoltageConfigured(void); bool isBatteryVoltageConfigured(void);
uint16_t getBatteryVoltage(void); uint16_t getBatteryVoltage(void);
uint16_t getSagCompensatedBatteryVoltage(void);
uint16_t getBatteryVoltageLatestADC(void); uint16_t getBatteryVoltageLatestADC(void);
uint16_t getBatteryWarningVoltage(void); uint16_t getBatteryWarningVoltage(void);
uint8_t getBatteryCellCount(void); uint8_t getBatteryCellCount(void);
@ -101,8 +102,10 @@ int32_t getPower(void);
int32_t getMAhDrawn(void); int32_t getMAhDrawn(void);
int32_t getMWhDrawn(void); int32_t getMWhDrawn(void);
void currentMeterUpdate(int32_t lastUpdateAt); void batteryUpdate(timeUs_t timeDelta);
void currentMeterUpdate(timeUs_t timeDelta);
void powerMeterUpdate(int32_t lastUpdateAt); void sagCompensatedVBatUpdate(timeUs_t currentTime);
void powerMeterUpdate(timeUs_t timeDelta);
uint8_t calculateBatteryPercentage(void); uint8_t calculateBatteryPercentage(void);
float calculateThrottleCompensationFactor(void);

80
src/main/sensors/rangefinder.c
View file

@ -169,12 +169,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
return true; return true;
} }
void rangefinderResetDynamicThreshold(void)
{
rangefinder.snrThresholdReached = false;
rangefinder.dynamicDistanceThreshold = 0;
}
bool rangefinderInit(void) bool rangefinderInit(void)
{ {
if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) { if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
@ -186,9 +180,6 @@ bool rangefinderInit(void)
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE; rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f)); rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
rangefinder.lastValidResponseTimeMs = millis(); rangefinder.lastValidResponseTimeMs = millis();
rangefinder.snr = 0;
rangefinderResetDynamicThreshold();
return true; return true;
} }
@ -211,35 +202,6 @@ static int32_t applyMedianFilter(int32_t newReading)
return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading; return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading;
} }
static int16_t computePseudoSnr(int32_t newReading) {
#define SNR_SAMPLES 5
static int16_t snrSamples[SNR_SAMPLES];
static uint8_t snrSampleIndex = 0;
static int32_t previousReading = RANGEFINDER_OUT_OF_RANGE;
static bool snrReady = false;
int16_t pseudoSnr = 0;
snrSamples[snrSampleIndex] = constrain((int)(pow(newReading - previousReading, 2) / 10), 0, 6400);
++snrSampleIndex;
if (snrSampleIndex == SNR_SAMPLES) {
snrSampleIndex = 0;
snrReady = true;
}
previousReading = newReading;
if (snrReady) {
for (uint8_t i = 0; i < SNR_SAMPLES; i++) {
pseudoSnr += snrSamples[i];
}
return constrain(pseudoSnr, 0, 32000);
} else {
return RANGEFINDER_OUT_OF_RANGE;
}
}
/* /*
* This is called periodically by the scheduler * This is called periodically by the scheduler
*/ */
@ -252,34 +214,6 @@ timeDelta_t rangefinderUpdate(void)
return rangefinder.dev.delayMs * 1000; // to microseconds return rangefinder.dev.delayMs * 1000; // to microseconds
} }
bool isSurfaceAltitudeValid() {
/*
* Preconditions: raw and calculated altidude > 0
* SNR lower than threshold
*/
if (
rangefinder.calculatedAltitude > 0 &&
rangefinder.rawAltitude > 0 &&
rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD
) {
/*
* When critical altitude was determined, distance reported by rangefinder
* has to be lower than it to assume healthy readout
*/
if (rangefinder.snrThresholdReached) {
return (rangefinder.rawAltitude < rangefinder.dynamicDistanceThreshold);
} else {
return true;
}
} else {
return false;
}
}
/** /**
* Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned. * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
*/ */
@ -309,20 +243,6 @@ bool rangefinderProcess(float cosTiltAngle)
// Invalid response / hardware failure // Invalid response / hardware failure
rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE; rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
} }
rangefinder.snr = computePseudoSnr(distance);
if (rangefinder.snrThresholdReached == false && rangefinder.rawAltitude > 0) {
if (rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD && rangefinder.dynamicDistanceThreshold < rangefinder.rawAltitude) {
rangefinder.dynamicDistanceThreshold = rangefinder.rawAltitude * RANGEFINDER_DYNAMIC_FACTOR / 100;
}
if (rangefinder.snr >= RANGEFINDER_DYNAMIC_THRESHOLD) {
rangefinder.snrThresholdReached = true;
}
}
} }
else { else {
// Bad configuration // Bad configuration

5
src/main/sensors/rangefinder.h
View file

@ -44,17 +44,12 @@ typedef struct rangefinder_s {
int32_t rawAltitude; int32_t rawAltitude;
int32_t calculatedAltitude; int32_t calculatedAltitude;
timeMs_t lastValidResponseTimeMs; timeMs_t lastValidResponseTimeMs;
bool snrThresholdReached;
int32_t dynamicDistanceThreshold;
int16_t snr;
} rangefinder_t; } rangefinder_t;
extern rangefinder_t rangefinder; extern rangefinder_t rangefinder;
const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void); const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void);
void rangefinderResetDynamicThreshold(void);
bool rangefinderInit(void); bool rangefinderInit(void);
int32_t rangefinderGetLatestAltitude(void); int32_t rangefinderGetLatestAltitude(void);

17
src/main/telemetry/smartport.c
View file

@ -59,7 +59,6 @@
#include "telemetry/msp_shared.h" #include "telemetry/msp_shared.h"
#define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define SMARTPORT_REST_PERIOD 3 // Needed to avoid lost sensors on FPort, see #3198
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
enum enum
@ -138,7 +137,6 @@ enum
static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED; static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
static uint8_t smartPortIdCnt = 0; static uint8_t smartPortIdCnt = 0;
static bool smartPortHasRested = false;
typedef struct smartPortFrame_s { typedef struct smartPortFrame_s {
uint8_t sensorId; uint8_t sensorId;
@ -233,6 +231,11 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
} }
} }
bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
{
return payload && (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT);
}
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum) void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
{ {
uint8_t *data = (uint8_t *)payload; uint8_t *data = (uint8_t *)payload;
@ -343,7 +346,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
// unless we start receiving other sensors' packets // unless we start receiving other sensors' packets
#if defined(USE_MSP_OVER_TELEMETRY) #if defined(USE_MSP_OVER_TELEMETRY)
if (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT) { if (smartPortPayloadContainsMSP(payload)) {
// Pass only the payload: skip frameId // Pass only the payload: skip frameId
uint8_t *frameStart = (uint8_t *)&payload->valueId; uint8_t *frameStart = (uint8_t *)&payload->valueId;
smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE); smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
@ -380,14 +383,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
id = frSkyDataIdTable[smartPortIdCnt]; id = frSkyDataIdTable[smartPortIdCnt];
} }
smartPortIdCnt++; smartPortIdCnt++;
if (smartPortIdCnt % SMARTPORT_REST_PERIOD == 0) {
if (!smartPortHasRested) {
smartPortIdCnt--;
smartPortHasRested = true;
return;
}
smartPortHasRested = false;
}
switch (id) { switch (id) {
case FSSP_DATAID_VFAS : case FSSP_DATAID_VFAS :

1
src/main/telemetry/smartport.h
View file

@ -55,3 +55,4 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor
struct serialPort_s; struct serialPort_s;
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum); void smartPortWriteFrameSerial(const smartPortPayload_t *payload, struct serialPort_s *port, uint16_t checksum);
void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port); void smartPortSendByte(uint8_t c, uint16_t *checksum, struct serialPort_s *port);
bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload);

4
src/utils/build_stamp.rb
View file

@ -65,7 +65,9 @@ class Stamper
output = File.join(@stamp_dir, "stamp") output = File.join(@stamp_dir, "stamp")
stdout, stderr = @compiler.run(input, output, ["-dM", "-E"]) stdout, stderr = @compiler.run(input, output, ["-dM", "-E"])
File.delete(input) File.delete(input)
File.delete(output) if File.file?(output)
File.delete(output)
end
return Digest::SHA1.hexdigest(stdout) return Digest::SHA1.hexdigest(stdout)
end end
end end

5
src/utils/compiler.rb
View file

@ -80,7 +80,10 @@ class Compiler
if args if args
all_args.push(*args) all_args.push(*args)
end end
all_args << "-o" << output << input if output
all_args << "-o" << output
end
all_args << input
stdout, stderr, compile_status = Open3.capture3(join_args(all_args)) stdout, stderr, compile_status = Open3.capture3(join_args(all_args))
raise "Compiler error:\n#{all_args.join(' ')}\n#{stderr}" if not options[:noerror] and not compile_status.success? raise "Compiler error:\n#{all_args.join(' ')}\n#{stderr}" if not options[:noerror] and not compile_status.success?
return stdout, stderr return stdout, stderr

19
src/utils/settings.rb
View file

@ -260,10 +260,10 @@ end
OFF_ON_TABLE = Hash["name" => "off_on", "values" => ["OFF", "ON"]] OFF_ON_TABLE = Hash["name" => "off_on", "values" => ["OFF", "ON"]]
class Generator class Generator
def initialize(src_root, settings_file) def initialize(src_root, settings_file, output_dir)
@src_root = src_root @src_root = src_root
@settings_file = settings_file @settings_file = settings_file
@output_dir = File.dirname(settings_file) @output_dir = output_dir || File.dirname(settings_file)
@compiler = Compiler.new @compiler = Compiler.new
@ -416,7 +416,7 @@ class Generator
} }
add_header.call("platform.h") add_header.call("platform.h")
add_header.call("config/parameter_group_ids.h") add_header.call("config/parameter_group_ids.h")
add_header.call("settings.h") add_header.call("fc/settings.h")
foreach_enabled_group do |group| foreach_enabled_group do |group|
(group["headers"] || []).each do |h| (group["headers"] || []).each do |h|
@ -623,10 +623,12 @@ class Generator
# Use a temporary dir reachable by relative path # Use a temporary dir reachable by relative path
# since g++ in cygwin fails to open files # since g++ in cygwin fails to open files
# with absolute paths # with absolute paths
tmp = File.join("obj", "tmp") tmp = File.join(@output_dir, "tmp")
FileUtils.mkdir_p(tmp) unless File.directory?(tmp) FileUtils.mkdir_p(tmp) unless File.directory?(tmp)
value = yield(tmp) value = yield(tmp)
FileUtils.remove_dir(tmp) if File.directory?(tmp)
FileUtils.remove_dir(tmp)
end
value value
end end
@ -882,17 +884,20 @@ if __FILE__ == $0
exit(1) exit(1)
end end
gen = Generator.new(src_root, settings_file)
opts = GetoptLong.new( opts = GetoptLong.new(
[ "--output-dir", "-o", GetoptLong::REQUIRED_ARGUMENT ],
[ "--help", "-h", GetoptLong::NO_ARGUMENT ], [ "--help", "-h", GetoptLong::NO_ARGUMENT ],
[ "--json", "-j", GetoptLong::REQUIRED_ARGUMENT ], [ "--json", "-j", GetoptLong::REQUIRED_ARGUMENT ],
) )
jsonFile = nil jsonFile = nil
output_dir = nil
opts.each do |opt, arg| opts.each do |opt, arg|
case opt case opt
when "--output-dir"
output_dir = arg
when "--help" when "--help"
usage() usage()
exit(0) exit(0)
@ -901,6 +906,8 @@ if __FILE__ == $0
end end
end end
gen = Generator.new(src_root, settings_file, output_dir)
if jsonFile if jsonFile
gen.write_json(jsonFile) gen.write_json(jsonFile)
else else