Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'upstream/master' into turtle_mode_tuned

Browse source 
# Conflicts:
#	docs/Settings.md
This commit is contained in:
kernel-machine 2021-04-26 10:53:39 +02:00
commit 0c00b49b4c
38 changed files with 1059 additions and 926 deletions
Download patch file Download diff file Expand all files Collapse all files

22
docs/Safehomes.md
View file

@ -12,14 +12,20 @@ One potential risk when landing is that there might be buildings, trees and othe
## Safehome
Safehomes are a list of GPS coordinates that identify safe landing points. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be selected. Otherwise, it reverts to the old behaviour of using your current GPS position as home.
Safehomes are a list of GPS coordinates that identify safe landing points. You can define up to 8 safehomes for different locations you fly at. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position is identified. The arming home location remains as home.
Be aware that the safehome replaces your arming position as home. When flying, RTH will return to the safehome and OSD elements such as distance to home and direction to home will refer to the selected safehome.
When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the home location. If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return back to arming point.
You can define up to 8 safehomes for different locations you fly at.
The safehome operating mode is set using ```safehome_usage_mode```. If ```OFF```, safehomes will not be used. If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe. If ```RTH_FS```, the safehome will only be used for RX failsafe. This option can be changed using the OSD menu.
If you frequently use RTH to return back to the arming point, you may not want the aircraft to fly to the safehome. Let it do this at least once to confirm safehomes is working as expected. Afterward, `set safehome_usage_mode = RTH_FS` and the safehome will only be used for failsafe.
When using mode `RTH_FS`, you should confirm that your radio's failsafe configuration triggers the iNav failsafe mode. With many receivers, you have the ability to specify what signal to output during failsafe conditions.
When you are choosing safehome locations, ensure that the location is clear of obstructions for a radius more than 50m (`nav_fw_loiter_radius`). As the plane descends, the circles aren't always symmetrical, as wind direction could result in some wider or tighter turns. Also, the direction and length of the final landing stage is also unknown. You want to choose a point that has provides a margin for variation and the final landing.
If your safehome is not visible from your current location, use extra caution. A visual check of the safehome is recommend prior to flying. If the safehome is in use, you can use the OSD menu to disable safehome usage prior to your flight.
## OSD Message when Armed
When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time.
@ -36,8 +42,14 @@ If a safehome is selected, an additional message appears:
CURRENT DATE
CURRENT TIME
```
The GPS details are those of the selected safehome.
To draw your attention to "HOME" being replaced, the message flashes and stays visible longer.
The GPS details are those of the arming location, not the safehome.
To draw your attention to a safehome being selected, the message flashes and stays visible longer.
If a safehome was found, but ``safehome_usage_mode``` is ```OFF```, the message ```SAFEHOME FOUND; MODE OFF``` will appear.
## OSD Message during RTH
If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed.
## CLI command `safehome` to manage safehomes

1072
docs/Settings.md
View file

File diff suppressed because it is too large Load diff

BIN
docs/assets/images/StickPositions.png
View file

Binary file not shown.
Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 696 KiB

After

Width:  |  Height:  |  Size: 1,012 KiB

Before After
Before After

BIN
docs/assets/images/StickPositions_trans.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 696 KiB

4
src/main/cms/cms_menu_navigation.c
View file

@ -75,12 +75,14 @@ static const CMS_Menu cmsx_menuNavSettings = {
OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST),
OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST),
OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING),
OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED),
OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LAND_MINALT_VSPD),
OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LAND_MAXALT_VSPD),
OSD_SETTING_ENTRY("LAND SPEED MIN AT", SETTING_NAV_LAND_SLOWDOWN_MINALT),
OSD_SETTING_ENTRY("LAND SPEED SLOW AT", SETTING_NAV_LAND_SLOWDOWN_MAXALT),
OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE),
OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD),
OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED),
OSD_SETTING_ENTRY("SAFEHOME USAGE MODE", SETTING_SAFEHOME_USAGE_MODE),
OSD_BACK_AND_END_ENTRY,
};

14
src/main/common/time.h
View file

@ -24,10 +24,19 @@
#include "config/parameter_group.h"
// time difference, 32 bits always sufficient
// time difference, signed 32 bits of microseconds overflows at ~35 minutes
// this is worth leaving as int32_t for performance reasons, use timeDeltaLarge_t for deltas that can be big
typedef int32_t timeDelta_t;
#define TIMEDELTA_MAX INT32_MAX
// time difference large, signed 64 bits of microseconds overflows at ~300000 years
typedef int64_t timeDeltaLarge_t;
#define TIMEDELTALARGE_MAX INT64_MAX
// millisecond time
typedef uint32_t timeMs_t ;
typedef uint32_t timeMs_t;
#define TIMEMS_MAX UINT32_MAX
// microsecond time
#ifdef USE_64BIT_TIME
typedef uint64_t timeUs_t;
@ -48,6 +57,7 @@ typedef uint32_t timeUs_t;
#define MS2S(ms) ((ms) * 1e-3f)
#define HZ2S(hz) US2S(HZ2US(hz))
// Use this function only to get small deltas (difference overflows at ~35 minutes)
static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
typedef enum {

5
src/main/drivers/display_font_metadata.h
View file

@ -10,12 +10,11 @@ typedef struct displayFontMetadata_s {
uint16_t charCount;
} displayFontMetadata_t;
// 'I', 'N', 'A', 'V', 1
// 'I', 'N', 'A', 'V'
#define FONT_CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \
(chr)->data[1] == 'N' && \
(chr)->data[2] == 'A' && \
(chr)->data[3] == 'V' && \
(chr)->data[4] == 1)
(chr)->data[3] == 'V')
#define FONT_METADATA_CHR_INDEX 255
// Used for runtime detection of display drivers that might

7
src/main/drivers/osd_symbols.h
View file

@ -153,8 +153,8 @@
#define SYM_HEADING 0xA9 // 169 Compass Heading symbol
#define SYM_ALT 0xAA // 170 ALT
#define SYM_WH 0xAB // 171 WH
#define SYM_WH_KM_0 0xAC // 172 WH/KM left
#define SYM_WH_KM_1 0xAD // 173 WH/KM right
#define SYM_WH_KM 0xAC // 172 WH/KM
#define SYM_WH_MI 0xAD // 173 WH/MI
#define SYM_WATT 0xAE // 174 W
#define SYM_SCALE 0xAF // 175 Map scale
#define SYM_MPH 0xB0 // 176 MPH
@ -246,6 +246,9 @@
#define SYM_SNR 0xEE // SNR
#define SYM_MW 0xED // mW
#define SYM_KILOWATT 0xEF // 239 kW
#else
#define TEMP_SENSOR_SYM_COUNT 0

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

@ -294,10 +294,6 @@ void validateAndFixConfig(void)
}
#endif
#if !defined(USE_MPU_DATA_READY_SIGNAL)
gyroConfigMutable()->gyroSync = false;
#endif
// Call target-specific validation function
validateAndFixTargetConfig();

15
src/main/fc/controlrate_profile.h
View file

@ -22,21 +22,6 @@
#include "config/parameter_group.h"
#define MAX_CONTROL_RATE_PROFILE_COUNT 3
/*
Max and min available values for rates are now stored as absolute
tenths of degrees-per-second [dsp/10]
That means, max. rotation rate 180 equals 1800dps
New defaults of 200dps for pitch,roll and yaw are more less
equivalent of rates 0 from previous versions of iNav, Cleanflight, Baseflight
and so on.
*/
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6
#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180
#define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2
#define CONTROL_RATE_CONFIG_TPA_MAX 100
typedef struct controlRateConfig_s {

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

@ -101,9 +101,6 @@ enum {
ALIGN_MAG = 2
};
#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro
#define GYRO_SYNC_MAX_CONSECUTIVE_FAILURES 100 // After this many consecutive missed interrupts disable gyro sync and fall back to scheduled updates
#define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
#define EMERGENCY_ARMING_COUNTER_STEP_MS 100
@ -129,7 +126,6 @@ int16_t headFreeModeHold;
uint8_t motorControlEnable = false;
static bool isRXDataNew;
static uint32_t gyroSyncFailureCount;
static disarmReason_t lastDisarmReason = DISARM_NONE;
timeUs_t lastDisarmTimeUs = 0;
static emergencyArmingState_t emergencyArming;
@ -810,36 +806,17 @@ void processRx(timeUs_t currentTimeUs)
// Function for loop trigger
void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
UNUSED(currentTimeUs);
// getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
timeUs_t gyroUpdateUs = currentTimeUs;
if (gyroConfig()->gyroSync) {
while (true) {
gyroUpdateUs = micros();
if (gyroSyncCheckUpdate()) {
gyroSyncFailureCount = 0;
break;
}
else if ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY)) {
gyroSyncFailureCount++;
break;
}
}
// If we detect gyro sync failure - disable gyro sync
if (gyroSyncFailureCount > GYRO_SYNC_MAX_CONSECUTIVE_FAILURES) {
gyroConfigMutable()->gyroSync = false;
}
}
/* Update actual hardware readings */
gyroUpdate();
#ifdef USE_OPFLOW
if (sensors(SENSOR_OPFLOW)) {
opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs));
opflowGyroUpdateCallback(currentDeltaTime);
}
#endif
}

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

@ -1229,7 +1229,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->motorPwmRate);
sbufWriteU16(dst, servoConfig()->servoPwmRate);
sbufWriteU8(dst, gyroConfig()->gyroSync);
sbufWriteU8(dst, 0);
break;
case MSP_FILTER_CONFIG :
@ -1334,7 +1334,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode);
sbufWriteU16(dst, navConfig()->general.rth_abort_threshold);
sbufWriteU16(dst, navConfig()->general.rth_altitude);
sbufWriteU16(dst, navConfig()->general.land_descent_rate);
sbufWriteU16(dst, navConfig()->general.land_minalt_vspd);
sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd);
sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt);
sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt);
sbufWriteU16(dst, navConfig()->general.emerg_descent_rate);
@ -1798,14 +1799,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
for (int i = 0; i < 3; i++) {
tmp_u8 = sbufReadU8(src);
if (i == FD_YAW) {
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
}
else {
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}
tmp_u8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, CONTROL_RATE_CONFIG_TPA_MAX);
((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX);
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src);
((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src);
@ -1835,9 +1836,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
for (uint8_t i = 0; i < 3; ++i) {
tmp_u8 = sbufReadU8(src);
if (i == FD_YAW) {
currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
} else {
currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}
@ -1847,9 +1848,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
for (uint8_t i = 0; i < 3; ++i) {
tmp_u8 = sbufReadU8(src);
if (i == FD_YAW) {
currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
} else {
currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
}
}
@ -2163,7 +2164,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
motorConfigMutable()->motorPwmProtocol = sbufReadU8(src);
motorConfigMutable()->motorPwmRate = sbufReadU16(src);
servoConfigMutable()->servoPwmRate = sbufReadU16(src);
gyroConfigMutable()->gyroSync = sbufReadU8(src);
sbufReadU8(src); //Was gyroSync
} else
return MSP_RESULT_ERROR;
break;
@ -2307,7 +2308,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src);
navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src);
navConfigMutable()->general.rth_altitude = sbufReadU16(src);
navConfigMutable()->general.land_descent_rate = sbufReadU16(src);
navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src);
navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src);
navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src);
navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src);
navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src);

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

@ -39,6 +39,7 @@
#include "fc/controlrate_profile.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_curves.h"
#include "fc/settings.h"
#include "navigation/navigation.h"
@ -61,15 +62,6 @@ static uint8_t adjustmentStateMask = 0;
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
#define RC_ADJUSTMENT_EXPO_MIN 0
#define RC_ADJUSTMENT_EXPO_MAX 100
#define RC_ADJUSTMENT_MANUAL_RATE_MIN 0
#define RC_ADJUSTMENT_MANUAL_RATE_MAX 100
#define RC_ADJUSTMENT_PID_MIN 0
#define RC_ADJUSTMENT_PID_MAX 200
// sync with adjustmentFunction_e
static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
{
@ -368,17 +360,17 @@ static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t
static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
{
applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_EXPO_MIN, RC_ADJUSTMENT_EXPO_MAX);
applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_EXPO_MAX);
}
static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
{
return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX);
return applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_CONSTANT_MANUAL_RATE_MIN, SETTING_CONSTANT_MANUAL_RATE_MAX);
}
static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
{
applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
applyAdjustmentU16(adjustmentFunction, val, delta, SETTING_CONSTANT_RPYL_PID_MIN, SETTING_CONSTANT_RPYL_PID_MAX);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
@ -406,7 +398,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
break;
case ADJUSTMENT_PITCH_ROLL_RATE:
case ADJUSTMENT_PITCH_RATE:
applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX);
if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
schedulePidGainsUpdate();
break;
@ -415,7 +407,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
FALLTHROUGH;
case ADJUSTMENT_ROLL_RATE:
applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
@ -429,7 +421,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta);
break;
case ADJUSTMENT_YAW_RATE:
applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_MANUAL_YAW_RATE:
@ -508,10 +500,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
schedulePidGainsUpdate();
break;
case ADJUSTMENT_NAV_FW_CRUISE_THR:
applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000);
applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX);
break;
case ADJUSTMENT_NAV_FW_PITCH2THR:
applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100);
applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX);
break;
case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
updateBoardAlignment(delta, 0);
@ -586,7 +578,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
navigationUsePIDs();
break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX);
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
break;
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
case ADJUSTMENT_VTX_POWER_LEVEL:
@ -599,13 +591,13 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
break;
#endif
case ADJUSTMENT_TPA:
applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX);
applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX);
break;
case ADJUSTMENT_TPA_BREAKPOINT:
applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX);
break;
case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, 0, 9);
applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX);
break;
default:
break;

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

@ -69,6 +69,9 @@ tables:
- name: osd_stats_energy_unit
values: ["MAH", "WH"]
enum: osd_stats_energy_unit_e
- name: osd_stats_min_voltage_unit
values: ["BATTERY", "CELL"]
enum: osd_stats_min_voltage_unit_e
- name: osd_video_system
values: ["AUTO", "PAL", "NTSC"]
enum: videoSystem_e
@ -104,7 +107,7 @@ tables:
enum: navRTHAllowLanding_e
- name: bat_capacity_unit
values: ["MAH", "MWH"]
enum: batCapacityUnit_e
enum: batCapacityUnit_e
- name: bat_voltage_source
values: ["RAW", "SAG_COMP"]
enum: batVoltageSource_e
@ -163,10 +166,23 @@ tables:
values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"]
- name: osd_plus_code_short
values: ["0", "2", "4", "6"]
- name: safehome_usage_mode
values: ["OFF", "RTH", "RTH_FS"]
enum: safehomeUsageMode_e
- name: nav_rth_climb_first
enum: navRTHClimbFirst_e
values: ["OFF", "ON", "ON_FW_SPIRAL"]
constants:
RPYL_PID_MIN: 0
RPYL_PID_MAX: 200
MANUAL_RATE_MIN: 0
MANUAL_RATE_MAX: 100
ROLL_PITCH_RATE_MIN: 6
ROLL_PITCH_RATE_MAX: 180
groups:
- name: PG_GYRO_CONFIG
type: gyroConfig_t
@ -176,11 +192,6 @@ groups:
description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible."
default_value: 1000
max: 9000
- name: gyro_sync
description: "This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf"
default_value: ON
field: gyroSync
type: bool
- name: align_gyro
description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP."
default_value: "DEFAULT"
@ -1202,7 +1213,7 @@ groups:
default_value: 0
field: throttle.dynPID
min: 0
max: CONTROL_RATE_CONFIG_TPA_MAX
max: 100
- name: tpa_breakpoint
description: "See tpa_rate."
default_value: 1500
@ -1233,20 +1244,20 @@ groups:
description: "Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
default_value: 20
field: stabilized.rates[FD_ROLL]
min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
min: ROLL_PITCH_RATE_MIN
max: ROLL_PITCH_RATE_MAX
- name: pitch_rate
description: "Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
default_value: 20
field: stabilized.rates[FD_PITCH]
min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN
max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX
min: ROLL_PITCH_RATE_MIN
max: ROLL_PITCH_RATE_MAX
- name: yaw_rate
description: "Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure."
default_value: 20
field: stabilized.rates[FD_YAW]
min: CONTROL_RATE_CONFIG_YAW_RATE_MIN
max: CONTROL_RATE_CONFIG_YAW_RATE_MAX
min: 2
max: 180
- name: manual_rc_expo
description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]"
default_value: 70
@ -1263,20 +1274,20 @@ groups:
description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%"
default_value: 100
field: manual.rates[FD_ROLL]
min: 0
max: 100
min: MANUAL_RATE_MIN
max: MANUAL_RATE_MAX
- name: manual_pitch_rate
description: "Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]%"
default_value: 100
field: manual.rates[FD_PITCH]
min: 0
max: 100
min: MANUAL_RATE_MIN
max: MANUAL_RATE_MAX
- name: manual_yaw_rate
description: "Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]%"
default_value: 100
field: manual.rates[FD_YAW]
min: 0
max: 100
min: MANUAL_RATE_MIN
max: MANUAL_RATE_MAX
- name: fpv_mix_degrees
field: misc.fpvCamAngleDegrees
min: 0
@ -1497,182 +1508,182 @@ groups:
description: "Multicopter rate stabilisation P-gain for PITCH"
default_value: 40
field: bank_mc.pid[PID_PITCH].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_i_pitch
description: "Multicopter rate stabilisation I-gain for PITCH"
default_value: 30
field: bank_mc.pid[PID_PITCH].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_d_pitch
description: "Multicopter rate stabilisation D-gain for PITCH"
default_value: 23
field: bank_mc.pid[PID_PITCH].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_cd_pitch
description: "Multicopter Control Derivative gain for PITCH"
default_value: 60
field: bank_mc.pid[PID_PITCH].FF
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_p_roll
description: "Multicopter rate stabilisation P-gain for ROLL"
default_value: 40
field: bank_mc.pid[PID_ROLL].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_i_roll
description: "Multicopter rate stabilisation I-gain for ROLL"
default_value: 30
field: bank_mc.pid[PID_ROLL].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_d_roll
description: "Multicopter rate stabilisation D-gain for ROLL"
default_value: 23
field: bank_mc.pid[PID_ROLL].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_cd_roll
description: "Multicopter Control Derivative gain for ROLL"
default_value: 60
field: bank_mc.pid[PID_ROLL].FF
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_p_yaw
description: "Multicopter rate stabilisation P-gain for YAW"
default_value: 85
field: bank_mc.pid[PID_YAW].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_i_yaw
description: "Multicopter rate stabilisation I-gain for YAW"
default_value: 45
field: bank_mc.pid[PID_YAW].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_d_yaw
description: "Multicopter rate stabilisation D-gain for YAW"
default_value: 0
field: bank_mc.pid[PID_YAW].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_cd_yaw
description: "Multicopter Control Derivative gain for YAW"
default_value: 60
field: bank_mc.pid[PID_YAW].FF
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_p_level
description: "Multicopter attitude stabilisation P-gain"
default_value: 20
field: bank_mc.pid[PID_LEVEL].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_i_level
description: "Multicopter attitude stabilisation low-pass filter cutoff"
default_value: 15
field: bank_mc.pid[PID_LEVEL].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: mc_d_level
description: "Multicopter attitude stabilisation HORIZON transition point"
default_value: 75
field: bank_mc.pid[PID_LEVEL].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_p_pitch
description: "Fixed-wing rate stabilisation P-gain for PITCH"
default_value: 5
field: bank_fw.pid[PID_PITCH].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_i_pitch
description: "Fixed-wing rate stabilisation I-gain for PITCH"
default_value: 7
field: bank_fw.pid[PID_PITCH].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_d_pitch
description: "Fixed wing rate stabilisation D-gain for PITCH"
default_value: 0
field: bank_fw.pid[PID_PITCH].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_ff_pitch
description: "Fixed-wing rate stabilisation FF-gain for PITCH"
default_value: 50
field: bank_fw.pid[PID_PITCH].FF
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_p_roll
description: "Fixed-wing rate stabilisation P-gain for ROLL"
default_value: 5
field: bank_fw.pid[PID_ROLL].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_i_roll
description: "Fixed-wing rate stabilisation I-gain for ROLL"
default_value: 7
field: bank_fw.pid[PID_ROLL].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_d_roll
description: "Fixed wing rate stabilisation D-gain for ROLL"
default_value: 0
field: bank_fw.pid[PID_ROLL].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_ff_roll
description: "Fixed-wing rate stabilisation FF-gain for ROLL"
default_value: 50
field: bank_fw.pid[PID_ROLL].FF
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_p_yaw
description: "Fixed-wing rate stabilisation P-gain for YAW"
default_value: 6
field: bank_fw.pid[PID_YAW].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_i_yaw
description: "Fixed-wing rate stabilisation I-gain for YAW"
default_value: 10
field: bank_fw.pid[PID_YAW].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_d_yaw
description: "Fixed wing rate stabilisation D-gain for YAW"
default_value: 0
field: bank_fw.pid[PID_YAW].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_ff_yaw
description: "Fixed-wing rate stabilisation FF-gain for YAW"
default_value: 60
field: bank_fw.pid[PID_YAW].FF
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_p_level
description: "Fixed-wing attitude stabilisation P-gain"
default_value: 20
field: bank_fw.pid[PID_LEVEL].P
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_i_level
description: "Fixed-wing attitude stabilisation low-pass filter cutoff"
default_value: 5
field: bank_fw.pid[PID_LEVEL].I
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: fw_d_level
description: "Fixed-wing attitude stabilisation HORIZON transition point"
default_value: 75
field: bank_fw.pid[PID_LEVEL].D
min: 0
max: 200
min: RPYL_PID_MIN
max: RPYL_PID_MAX
- name: max_angle_inclination_rll
description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°"
default_value: 300
@ -1724,10 +1735,10 @@ groups:
table: direction
- name: fw_reference_airspeed
description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
default_value: 1000
default_value: 1500
field: fixedWingReferenceAirspeed
min: 1
max: 5000
min: 300
max: 6000
- name: fw_turn_assist_yaw_gain
description: "Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter"
default_value: 1
@ -2280,20 +2291,26 @@ groups:
field: general.max_manual_climb_rate
min: 10
max: 2000
- name: nav_landing_speed
description: "Vertical descent velocity during the RTH landing phase. [cm/s]"
- name: nav_land_minalt_vspd
description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]"
default_value: 50
field: general.land_minalt_vspd
min: 50
max: 500
- name: nav_land_maxalt_vspd
description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]"
default_value: 200
field: general.land_descent_rate
field: general.land_maxalt_vspd
min: 100
max: 2000
- name: nav_land_slowdown_minalt
description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]"
description: "Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm]"
default_value: 500
field: general.land_slowdown_minalt
min: 50
max: 1000
- name: nav_land_slowdown_maxalt
description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]"
description: "Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm]"
default_value: 2000
field: general.land_slowdown_maxalt
min: 500
@ -2372,6 +2389,11 @@ groups:
field: general.safehome_max_distance
min: 0
max: 65000
- name: safehome_usage_mode
description: "Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information."
default_value: "RTH"
field: general.flags.safehome_usage_mode
table: safehome_usage_mode
- name: nav_mc_bank_angle
description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
default_value: 30
@ -2902,6 +2924,12 @@ groups:
field: stats_energy_unit
table: osd_stats_energy_unit
type: uint8_t
- name: osd_stats_min_voltage_unit
description: "Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats."
default_value: "BATTERY"
field: stats_min_voltage_unit
table: osd_stats_min_voltage_unit
type: uint8_t
- name: osd_rssi_alarm
description: "Value below which to make the OSD RSSI indicator blink"

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

@ -47,6 +47,7 @@
#include "flight/pid.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "rx/rx.h"
@ -362,12 +363,15 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set
if ((failsafeConfig()->failsafe_min_distance > 0) &&
((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) &&
sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
if (failsafeConfig()->failsafe_min_distance > 0 &&
sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
// Use the alternate, minimum distance failsafe procedure instead
return failsafeConfig()->failsafe_min_distance_procedure;
// get the distance to the original arming point
uint32_t distance = calculateDistanceToDestination(&original_rth_home);
if (distance < failsafeConfig()->failsafe_min_distance) {
// Use the alternate, minimum distance failsafe procedure instead
return failsafeConfig()->failsafe_min_distance_procedure;
}
}
return failsafeConfig()->failsafe_procedure;

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

@ -25,11 +25,6 @@
#define MAX_SUPPORTED_MOTORS 12
#endif
#define YAW_JUMP_PREVENTION_LIMIT_LOW 80
#define YAW_JUMP_PREVENTION_LIMIT_HIGH 500
#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450
// Digital protocol has fixed values
#define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48
@ -131,4 +126,4 @@ void stopMotors(void);
void stopPwmAllMotors(void);
void loadPrimaryMotorMixer(void);
bool areMotorsRunning(void);
bool areMotorsRunning(void);

2
src/main/io/ledstrip.c
View file

@ -935,7 +935,7 @@ void ledStripUpdate(timeUs_t currentTimeUs)
for (timId_e timId = 0; timId < timTimerCount; timId++) {
// sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
// max delay is limited to 5s
int32_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
if (delta < 0 && delta > -LED_STRIP_MS(5000))
continue; // not ready yet
timActive |= 1 << timId;

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

@ -144,7 +144,7 @@ FILE_COMPILE_FOR_SPEED
#define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2)
#define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s))
#define OSD_MIN_FONT_VERSION 1
#define OSD_MIN_FONT_VERSION 2
static unsigned currentLayout = 0;
static int layoutOverride = -1;
@ -156,8 +156,8 @@ static float GForce, GForceAxis[XYZ_AXIS_COUNT];
typedef struct statistic_s {
uint16_t max_speed;
uint16_t min_voltage; // /100
int16_t max_current; // /100
int16_t max_power; // /100
int16_t max_current;
int32_t max_power;
int16_t min_rssi;
int16_t min_lq; // for CRSF
int16_t min_rssi_dbm; // for CRSF
@ -193,7 +193,7 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
static int digitCount(int32_t value)
@ -804,6 +804,15 @@ static const char * osdFailsafeInfoMessage(void)
}
return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST);
}
#if defined(USE_SAFE_HOME)
static const char * divertingToSafehomeMessage(void)
{
if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) {
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
}
return NULL;
}
#endif
static const char * navigationStateMessage(void)
{
@ -839,6 +848,11 @@ static const char * navigationStateMessage(void)
return OSD_MESSAGE_STR(OSD_MSG_LANDING);
case MW_NAV_STATE_HOVER_ABOVE_HOME:
if (STATE(FIXED_WING_LEGACY)) {
#if defined(USE_SAFE_HOME)
if (safehome_applied) {
return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME);
}
#endif
return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME);
}
return OSD_MESSAGE_STR(OSD_MSG_HOVERING);
@ -970,7 +984,7 @@ static bool osdIsHeadingValid(void)
} else {
return isImuHeadingValid();
}
#else
#else
return isImuHeadingValid();
#endif
}
@ -983,7 +997,7 @@ int16_t osdGetHeading(void)
} else {
return attitude.values.yaw;
}
#else
#else
return attitude.values.yaw;
#endif
}
@ -1630,7 +1644,7 @@ static bool osdDrawSingleElement(uint8_t item)
/*static int32_t updatedTimeSeconds = 0;*/
timeUs_t currentTimeUs = micros();
static int32_t timeSeconds = -1;
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) {
#ifdef USE_WIND_ESTIMATOR
timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
#else
@ -1661,7 +1675,7 @@ static bool osdDrawSingleElement(uint8_t item)
static timeUs_t updatedTimestamp = 0;
timeUs_t currentTimeUs = micros();
static int32_t distanceMeters = -1;
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) {
#ifdef USE_WIND_ESTIMATOR
distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
#else
@ -1927,7 +1941,7 @@ static bool osdDrawSingleElement(uint8_t item)
pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch);
} else {
rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
}
#else
rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
@ -2160,8 +2174,8 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_POWER:
{
osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3);
buff[3] = SYM_WATT;
bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3);
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
buff[4] = '\0';
uint8_t current_alarm = osdConfig()->current_alarm;
@ -2248,7 +2262,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, efficiencyTimeDelta * 1e-6f);
1, US2S(efficiencyTimeDelta));
efficiencyUpdated = currentTimeUs;
} else {
@ -2278,7 +2292,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
1, efficiencyTimeDelta * 1e-6f);
1, US2S(efficiencyTimeDelta));
efficiencyUpdated = currentTimeUs;
} else {
@ -2290,9 +2304,8 @@ static bool osdDrawSingleElement(uint8_t item)
} else {
buff[0] = buff[1] = buff[2] = '-';
}
buff[3] = SYM_WH_KM_0;
buff[4] = SYM_WH_KM_1;
buff[5] = '\0';
buff[3] = SYM_WH_KM;
buff[4] = '\0';
break;
}
@ -2760,7 +2773,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.force_grid = SETTING_OSD_FORCE_GRID_DEFAULT,
.stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT
.stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT,
.stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT
);
void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
@ -3009,9 +3023,8 @@ static void osdCompleteAsyncInitialization(void)
osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3);
} else
strcpy(string_buffer, "---");
string_buffer[3] = SYM_WH_KM_0;
string_buffer[4] = SYM_WH_KM_1;
string_buffer[5] = '\0';
string_buffer[3] = SYM_WH_KM;
string_buffer[4] = '\0';
displayWrite(osdDisplayPort, STATS_VALUE_X_POS-3, y, string_buffer);
}
#endif // USE_ADC
@ -3054,7 +3067,7 @@ static void osdResetStats(void)
static void osdUpdateStats(void)
{
int16_t value;
int32_t value;
if (feature(FEATURE_GPS)) {
value = osdGet3DSpeed();
@ -3069,11 +3082,11 @@ static void osdUpdateStats(void)
if (stats.min_voltage > value)
stats.min_voltage = value;
value = abs(getAmperage() / 100);
value = abs(getAmperage());
if (stats.max_current < value)
stats.max_current = value;
value = abs(getPower() / 100);
value = labs(getPower());
if (stats.max_power < value)
stats.max_power = value;
@ -3104,7 +3117,7 @@ static void osdShowStatsPage1(void)
displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2");
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->");
if (feature(FEATURE_GPS)) {
displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :");
@ -3125,22 +3138,24 @@ static void osdShowStatsPage1(void)
osdFormatAltitudeStr(buff, stats.max_altitude);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
#if defined(USE_SERIALRX_CRSF)
displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :");
itoa(stats.min_lq, buff, 10);
strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff);
switch (rxConfig()->serialrx_provider) {
case SERIALRX_CRSF:
displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :");
itoa(stats.min_lq, buff, 10);
strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi_dbm, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
#else
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi, buff, 10);
strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff);
#endif
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi_dbm, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
break;
default:
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi, buff, 10);
strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff);
}
displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :");
uint16_t flySeconds = getFlightTime();
@ -3167,22 +3182,28 @@ static void osdShowStatsPage2(void)
displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 2/2");
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2");
displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) {
displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
} else {
displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :");
osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3);
}
tfp_sprintf(buff, "%s%c", buff, SYM_VOLT);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
if (feature(FEATURE_CURRENT_METER)) {
displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :");
itoa(stats.max_current, buff, 10);
osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3);
tfp_sprintf(buff, "%s%c", buff, SYM_AMP);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :");
itoa(stats.max_power, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_WATT);
bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3);
buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
buff[4] = '\0';
displayWrite(osdDisplayPort, statValuesX, top++, buff);
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
@ -3203,9 +3224,8 @@ static void osdShowStatsPage2(void)
SYM_MAH_KM_0, SYM_MAH_KM_1);
else {
osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3);
buff[3] = SYM_WH_KM_0;
buff[4] = SYM_WH_KM_1;
buff[5] = '\0';
buff[3] = SYM_WH_KM;
buff[4] = '\0';
}
// If traveled distance is less than 100 meters efficiency numbers are useless and unreliable so display --- instead
if (totalDistance < 10000) {
@ -3215,9 +3235,8 @@ static void osdShowStatsPage2(void)
buff[4] = SYM_MAH_KM_1;
buff[5] = '\0';
} else {
buff[3] = SYM_WH_KM_0;
buff[4] = SYM_WH_KM_1;
buff[5] = '\0';
buff[3] = SYM_WH_KM;
buff[4] = '\0';
}
}
displayWrite(osdDisplayPort, statValuesX, top++, buff);
@ -3280,13 +3299,17 @@ static void osdShowArmed(void)
}
y += 4;
#if defined (USE_SAFE_HOME)
if (isSafeHomeInUse()) {
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
char buf2[12]; // format the distance first
osdFormatDistanceStr(buf2, safehome_distance);
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used);
// write this message above the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
if (safehome_distance) { // safehome found during arming
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
strcpy(buf, "SAFEHOME FOUND; MODE OFF");
} else {
char buf2[12]; // format the distance first
osdFormatDistanceStr(buf2, safehome_distance);
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
}
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
// write this message above the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
}
#endif
} else {
@ -3312,7 +3335,7 @@ static void osdShowArmed(void)
static void osdFilterData(timeUs_t currentTimeUs) {
static timeUs_t lastRefresh = 0;
float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6;
float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
@ -3355,7 +3378,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
statsPagesCheck = 0;
#if defined(USE_SAFE_HOME)
if (isSafeHomeInUse())
if (safehome_distance)
delay *= 3;
#endif
osdSetNextRefreshIn(delay);
@ -3557,6 +3580,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
const char *failsafePhaseMessage = osdFailsafePhaseMessage();
const char *failsafeInfoMessage = osdFailsafeInfoMessage();
const char *navStateFSMessage = navigationStateMessage();
if (failsafePhaseMessage) {
messages[messageCount++] = failsafePhaseMessage;
}
@ -3566,6 +3590,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (navStateFSMessage) {
messages[messageCount++] = navStateFSMessage;
}
#if defined(USE_SAFE_HOME)
const char *safehomeMessage = divertingToSafehomeMessage();
if (safehomeMessage) {
messages[messageCount++] = safehomeMessage;
}
#endif
if (messageCount > 0) {
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
if (message == failsafeInfoMessage) {
@ -3600,6 +3630,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
messages[messageCount++] = navStateMessage;
}
}
#if defined(USE_SAFE_HOME)
const char *safehomeMessage = divertingToSafehomeMessage();
if (safehomeMessage) {
messages[messageCount++] = safehomeMessage;
}
#endif
} else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
const char *launchStateMessage = fixedWingLaunchStateMessage();

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

@ -100,6 +100,11 @@
#define OSD_MSG_HEADFREE "(HEADFREE)"
#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM"
#if defined(USE_SAFE_HOME)
#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME"
#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME"
#endif
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,
@ -238,6 +243,11 @@ typedef enum {
OSD_STATS_ENERGY_UNIT_WH,
} osd_stats_energy_unit_e;
typedef enum {
OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY,
OSD_STATS_MIN_VOLTAGE_UNIT_CELL,
} osd_stats_min_voltage_unit_e;
typedef enum {
OSD_CROSSHAIRS_STYLE_DEFAULT,
OSD_CROSSHAIRS_STYLE_AIRCRAFT,
@ -334,6 +344,7 @@ typedef struct osdConfig_s {
uint8_t units; // from osd_unit_e
uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e
#ifdef USE_WIND_ESTIMATOR
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance

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

@ -112,7 +112,7 @@
})
/*
/*
* DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0
* DJI uses a subset of messages and assume fixed bit positions for flight modes
*
@ -687,7 +687,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
// Show feet when dist < 0.5mi
tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
}
}
else {
// Show miles when dist >= 0.5mi
tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)),
@ -724,7 +724,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, efficiencyTimeDelta * 1e-6f);
1, US2S(efficiencyTimeDelta));
efficiencyUpdated = currentTimeUs;
}
@ -757,7 +757,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
if (enabledElements[0] == 'W') {
enabledElements += 1;
}
int elemLen = strlen(enabledElements);
if (elemLen > 0) {
@ -827,14 +827,14 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
// during a lost aircraft recovery and blinking
// will cause it to be missing from some frames.
}
}
}
else {
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
const char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
}
}
else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = "AUTOLAUNCH";
}
@ -996,7 +996,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
sbufWriteU16(dst, rxGetChannelValue(i));
}
break;
break;
case DJI_MSP_RAW_GPS:
sbufWriteU8(dst, gpsSol.fixType);

2
src/main/io/smartport_master.c
View file

@ -550,7 +550,7 @@ void smartportMasterHandle(timeUs_t currentTimeUs)
return;
}
if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) {
if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) {
if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one
smartportMasterForwardNextPayload();
} else {

90
src/main/navigation/navigation.c Normal file → Executable file
View file

@ -74,10 +74,14 @@ gpsLocation_t GPS_home;
uint32_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home point in degrees
fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
radar_pois_t radar_pois[RADAR_MAX_POIS];
#if defined(USE_SAFE_HOME)
int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
uint32_t safehome_distance; // distance to the selected safehome
int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
uint32_t safehome_distance = 0; // distance to the nearest safehome
fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming
bool safehome_applied = false; // whether the safehome has been applied to home.
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
@ -92,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -109,6 +113,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT,
.rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
.nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
.safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
},
// General navigation parameters
@ -119,9 +124,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
.max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
.land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT, // centimeters/s
.land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
.land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters
.land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
.land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT, // centimeters/s
.emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s
.min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately
.rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters
@ -235,6 +241,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void);
void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void);
void updateHomePosition(void);
static bool rthAltControlStickOverrideCheck(unsigned axis);
@ -1407,22 +1414,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
// 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.
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
descentVelLimited = navConfig()->general.land_minalt_vspd;
}
else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->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
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
// 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);
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
return NAV_FSM_EVENT_NONE;
}
@ -1446,7 +1451,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
UNUSED(previousState);
if (STATE(ALTITUDE_CONTROL)) {
updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME
}
// Prevent I-terms growing when already landed
@ -1474,6 +1479,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
*/
setupJumpCounters();
posControl.activeWaypointIndex = 0;
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
}
}
@ -2350,26 +2356,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
#if defined(USE_SAFE_HOME)
/*******************************************************
* Is a safehome being used instead of the arming point?
*******************************************************/
bool isSafeHomeInUse(void)
void checkSafeHomeState(bool shouldBeEnabled)
{
return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
shouldBeEnabled = false;
} else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) {
// if safehomes are only used with failsafe and we're trying to enable safehome
// then enable the safehome only with failsafe
shouldBeEnabled = posControl.flags.forcedRTHActivated;
}
// no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
return;
}
if (shouldBeEnabled) {
// set home to safehome
setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
safehome_applied = true;
} else {
// set home to original arming point
setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
safehome_applied = false;
}
// if we've changed the home position, update the distance and direction
updateHomePosition();
}
/***********************************************************
* See if there are any safehomes near where we are arming.
* If so, use it instead of the arming point for home.
* Select the nearest safehome
* If so, save the nearest one in case we need it later for RTH.
**********************************************************/
bool foundNearbySafeHome(void)
bool findNearestSafeHome(void)
{
safehome_used = -1;
safehome_index = -1;
uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
uint32_t distance_to_current;
fpVector3_t currentSafeHome;
fpVector3_t nearestSafeHome;
gpsLocation_t shLLH;
shLLH.alt = 0;
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
@ -2382,20 +2404,19 @@ bool foundNearbySafeHome(void)
distance_to_current = calculateDistanceToDestination(&currentSafeHome);
if (distance_to_current < nearest_safehome_distance) {
// this safehome is the nearest so far - keep track of it.
safehome_used = i;
safehome_index = i;
nearest_safehome_distance = distance_to_current;
nearestSafeHome.x = currentSafeHome.x;
nearestSafeHome.y = currentSafeHome.y;
nearestSafeHome.z = currentSafeHome.z;
}
}
if (safehome_used >= 0) {
if (safehome_index >= 0) {
safehome_distance = nearest_safehome_distance;
setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
return true;
} else {
safehome_distance = 0;
}
safehome_distance = 0;
return false;
return safehome_distance > 0;
}
#endif
@ -2421,9 +2442,13 @@ void updateHomePosition(void)
}
if (setHome) {
#if defined(USE_SAFE_HOME)
if (!foundNearbySafeHome())
findNearestSafeHome();
#endif
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
// save the current location in case it is replaced by a safehome or HOME_RESET
original_rth_home.x = posControl.rthState.homePosition.pos.x;
original_rth_home.y = posControl.rthState.homePosition.pos.y;
original_rth_home.z = posControl.rthState.homePosition.pos.z;
}
}
}
@ -3165,6 +3190,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
const bool canActivatePosHold = canActivatePosHoldMode();
const bool canActivateNavigation = canActivateNavigationModes();
const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
// Keep canActivateWaypoint flag at FALSE if there is no mission loaded
// Also block WP mission if we are executing RTH
@ -3601,6 +3627,7 @@ void activateForcedRTH(void)
{
abortFixedWingLaunch();
posControl.flags.forcedRTHActivated = true;
checkSafeHomeState(true);
navProcessFSMEvents(selectNavEventFromBoxModeInput());
}
@ -3609,6 +3636,7 @@ void abortForcedRTH(void)
// Disable failsafe RTH and make sure we back out of navigation mode to IDLE
// If any navigation mode was active prior to RTH it will be re-enabled with next RX update
posControl.flags.forcedRTHActivated = false;
checkSafeHomeState(false);
navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
}

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

@ -35,6 +35,7 @@
extern gpsLocation_t GPS_home;
extern uint32_t GPS_distanceToHome; // distance to home point in meters
extern int16_t GPS_directionToHome; // direction to home point in degrees
extern fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
extern bool autoThrottleManuallyIncreased;
@ -50,14 +51,20 @@ typedef struct {
int32_t lon;
} navSafeHome_t;
typedef enum {
SAFEHOME_USAGE_OFF = 0, // Don't use safehomes
SAFEHOME_USAGE_RTH = 1, // Default - use safehome for RTH
SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only
} safehomeUsageMode_e;
PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
extern int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
extern uint32_t safehome_distance; // distance to the selected safehome
extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
extern uint32_t safehome_distance; // distance to the nearest safehome
extern bool safehome_applied; // whether the safehome has been applied to home.
void resetSafeHomes(void); // remove all safehomes
bool isSafeHomeInUse(void); // Are we using a safehome instead of the arming point?
bool foundNearbySafeHome(void); // Did we find a safehome nearby?
bool findNearestSafeHome(void); // Find nearest safehome
#endif // defined(USE_SAFE_HOME)
@ -191,6 +198,7 @@ typedef struct navConfig_s {
uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH
uint8_t rth_alt_control_override; // Override RTH Altitude and Climb First settings using Pitch and Roll stick
uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor
uint8_t safehome_usage_mode; // Controls when safehomes are used
} flags;
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
@ -200,7 +208,8 @@ typedef struct navConfig_s {
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_manual_speed; // manual velocity control max horizontal speed
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
uint16_t land_descent_rate; // normal RTH landing descent rate
uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt
uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt
uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend
uint16_t land_slowdown_maxalt; // Altitude to start lowering descent rate during RTH descend
uint16_t emerg_descent_rate; // emergency landing descent rate

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

@ -171,11 +171,11 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
if ((posControl.flags.estAltStatus >= EST_USABLE)) {
if (posControl.flags.verticalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
}
else {
@ -401,10 +401,10 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
// Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
// Account for pilot's roll input (move position target left/right at max of max_manual_speed)
// POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
@ -440,10 +440,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
// If we are in the deadband of 50cm/s - don't update speed boost
@ -473,7 +473,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
{
static timeUs_t previousTimePitchToThrCorr = 0;
const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr;
const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr;
previousTimePitchToThrCorr = currentTimeUs;
static pt1Filter_t pitchToThrFilterState;

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

@ -209,11 +209,11 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
// If we have an update on vertical position data - update velocity and accel targets
if (posControl.flags.verticalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
// If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump
if (prepareForTakeoffOnReset) {
const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity();
@ -477,8 +477,8 @@ static float computeNormalizedVelocity(const float value, const float maxValue)
}
static float computeVelocityScale(
const float value,
const float maxValue,
const float value,
const float maxValue,
const float attenuationFactor,
const float attenuationStart,
const float attenuationEnd
@ -491,7 +491,7 @@ static float computeVelocityScale(
}
static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
{
{
const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
@ -539,15 +539,15 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
* Scale down dTerm with 2D speed
*/
const float setpointScale = computeVelocityScale(
setpointXY,
maxSpeed,
setpointXY,
maxSpeed,
multicopterPosXyCoefficients.dTermAttenuation,
multicopterPosXyCoefficients.dTermAttenuationStart,
multicopterPosXyCoefficients.dTermAttenuationEnd
);
const float measurementScale = computeVelocityScale(
posControl.actualState.velXY,
maxSpeed,
posControl.actualState.velXY,
maxSpeed,
multicopterPosXyCoefficients.dTermAttenuation,
multicopterPosXyCoefficients.dTermAttenuationStart,
multicopterPosXyCoefficients.dTermAttenuationEnd
@ -560,23 +560,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
// Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
// Thus we don't need to do anything else with calculated acceleration
float newAccelX = navPidApply3(
&posControl.pids.vel[X],
setpointX,
measurementX,
US2S(deltaMicros),
accelLimitXMin,
accelLimitXMax,
&posControl.pids.vel[X],
setpointX,
measurementX,
US2S(deltaMicros),
accelLimitXMin,
accelLimitXMax,
0, // Flags
1.0f, // Total gain scale
dtermScale // Additional dTerm scale
);
float newAccelY = navPidApply3(
&posControl.pids.vel[Y],
setpointY,
measurementY,
US2S(deltaMicros),
accelLimitYMin,
accelLimitYMax,
&posControl.pids.vel[Y],
setpointY,
measurementY,
US2S(deltaMicros),
accelLimitYMin,
accelLimitYMax,
0, // Flags
1.0f, // Total gain scale
dtermScale // Additional dTerm scale
@ -638,14 +638,14 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
if (!bypassPositionController) {
// Update position controller
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
// Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
const float maxSpeed = getActiveWaypointSpeed();
const float maxSpeed = getActiveWaypointSpeed();
updatePositionVelocityController_MC(maxSpeed);
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
}
@ -761,11 +761,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Normal sensor data
if (posControl.flags.verticalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);

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

@ -37,6 +37,9 @@
#define INAV_SURFACE_MAX_DISTANCE 40
#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro
_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
typedef enum {
NAV_POS_UPDATE_NONE = 0,
NAV_POS_UPDATE_Z = 1 << 1,
@ -197,7 +200,7 @@ typedef enum {
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36,
NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37,
NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37,
} navigationPersistentId_e;

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

@ -71,11 +71,11 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs)
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
const timeDeltaLarge_t deltaMicros = currentTimeUs - previousTimeUpdate;
previousTimeUpdate = currentTimeUs;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
if (deltaMicros > MAX_POSITION_UPDATE_INTERVAL_US) {
previousTimeUpdate = currentTimeUs;
previousTimePositionUpdate = currentTimeUs;
resetFixedWingPositionController();
@ -86,10 +86,10 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs)
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
} else {
resetFixedWingPositionController();
@ -143,4 +143,4 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
}
}
#endif
#endif

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

@ -531,7 +531,7 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
}
#endif
if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > FPORT2_MAX_TELEMETRY_AGE_MS * 1000)) {
if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > MS2US(FPORT2_MAX_TELEMETRY_AGE_MS))) {
lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
frameReceivedTimestamp = 0;
}
@ -593,7 +593,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
}
timeUs_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp);
timeDelta_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp);
if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash)
writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload);
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME, otaResponseTime);

10
src/main/rx/ghst.c
View file

@ -119,7 +119,7 @@ void ghstRxWriteTelemetryData(const void *data, int len)
}
void ghstRxSendTelemetryData(void)
{
{
// if there is telemetry data to write
if (telemetryBufLen > 0) {
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
@ -178,7 +178,7 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
static bool shouldSendTelemetryFrame(void)
{
const timeUs_t now = micros();
const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US;
}
@ -191,7 +191,7 @@ static void ghstIdle(void)
static void ghstUpdateFailsafe(unsigned pktIdx)
{
// pktIdx is an offset of RC channel packet,
// pktIdx is an offset of RC channel packet,
// We'll track arrival time of each of the frame types we ever saw arriving from this receiver
if (pktIdx < GHST_UL_RC_CHANS_FRAME_COUNT) {
if (ghstFsTracker[pktIdx].onTimePacketCounter < GHST_RC_FRAME_COUNT_THRESHOLD) {
@ -282,7 +282,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
int startIdx = 0;
if (
ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST &&
ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST &&
ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST
) {
const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload;
@ -300,7 +300,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
case GHST_UL_RC_CHANS_HS4_RSSI: {
const ghstPayloadPulsesRSSI_t* const rssiFrame = (ghstPayloadPulsesRSSI_t*)&ghstValidatedFrame.frame.payload;
lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiFrame->lq, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
break;
}

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

@ -79,7 +79,7 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData)
static uint8_t sumdIndex;
sumdTime = micros();
if (cmpTimeUs(sumdTime, sumdTimeLast) > 4000)
if (cmpTimeUs(sumdTime, sumdTimeLast) > MS2US(4))
sumdIndex = 0;
sumdTimeLast = sumdTime;

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

@ -69,7 +69,7 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData)
sumhTime = micros();
sumhTimeInterval = cmpTimeUs(sumhTime, sumhTimeLast);
sumhTimeLast = sumhTime;
if (sumhTimeInterval > 5000) {
if (sumhTimeInterval > MS2US(5)) {
sumhFramePosition = 0;
}

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

@ -570,7 +570,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
} else {
if (cmpTimeUs(currentTime, recordTimestamp) > 500000)
if (cmpTimeUs(currentTime, recordTimestamp) > MS2US(500))
recordTimestamp = 0;
if (!recordTimestamp) {
@ -587,7 +587,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
if (impedanceFilterState.state) {
pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5);
pt1FilterApply3(&impedanceFilterState, impedanceSample, timeDelta * 1e-6f);
pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta));
} else {
pt1FilterReset(&impedanceFilterState, impedanceSample);
}
@ -601,7 +601,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000);
pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500);
sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f));
sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta)));
}
DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance);

18
src/main/sensors/gyro.c
View file

@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -113,7 +113,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_align = SETTING_ALIGN_GYRO_DEFAULT,
.gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT,
.looptime = SETTING_LOOPTIME_DEFAULT,
.gyroSync = SETTING_GYRO_SYNC_DEFAULT,
#ifdef USE_DUAL_GYRO
.gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
#endif
@ -318,7 +317,7 @@ bool gyroInit(void)
gyroDev[0].initFn(&gyroDev[0]);
// initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime;
gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs;
// At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record
// If configuration says different - override
@ -518,19 +517,6 @@ int16_t gyroRateDps(int axis)
return lrintf(gyro.gyroADCf[axis]);
}
bool gyroSyncCheckUpdate(void)
{
if (!gyro.initialized) {
return false;
}
if (!gyroDev[0].intStatusFn) {
return false;
}
return gyroDev[0].intStatusFn(&gyroDev[0]);
}
void gyroUpdateDynamicLpf(float cutoffFreq) {
if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {

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

@ -61,7 +61,6 @@ extern gyro_t gyro;
typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
bool gyroSync; // Enable interrupt based loop
uint16_t looptime; // imu loop time in us
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_hz;
@ -95,5 +94,4 @@ bool gyroIsCalibrationComplete(void);
bool gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);
bool gyroSyncCheckUpdate(void);
void gyroUpdateDynamicLpf(float cutoffFreq);

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

@ -227,7 +227,7 @@ timeDelta_t rangefinderUpdate(void)
rangefinder.dev.update(&rangefinder.dev);
}
return rangefinder.dev.delayMs * 1000; // to microseconds
return MS2US(rangefinder.dev.delayMs);
}
/**

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

@ -59,7 +59,6 @@ void targetConfiguration(void)
gyroConfigMutable()->looptime = 1000;
gyroConfigMutable()->gyroSync = 1;
gyroConfigMutable()->gyro_lpf = 0; // 256 Hz
gyroConfigMutable()->gyro_soft_lpf_hz = 90;
gyroConfigMutable()->gyro_notch_hz = 150;

33
src/utils/settings.rb
View file

@ -301,6 +301,7 @@ class Generator
check_member_default_values_presence
sanitize_fields
resolv_min_max_and_default_values_if_possible
initialize_name_encoder
initialize_value_encoder
validate_default_values
@ -359,6 +360,7 @@ class Generator
@data = YAML.load_file(@settings_file)
initialize_tables
initialize_constants
check_conditions
end
@ -415,6 +417,11 @@ class Generator
buf << "extern const char * const #{table_variable_name(name)}[];\n"
end
# Write setting constants from settings file
@constants.each do |name, value|
buf << "#define SETTING_CONSTANT_#{name.upcase} #{value.inspect}\n"
end
# Write #define'd constants for referencing each setting
ii = 0
foreach_enabled_member do |group, member|
@ -715,6 +722,10 @@ class Generator
end
end
def initialize_constants
@constants = @data["constants"]
end
def ordered_table_names
@enabled_tables.to_a().sort()
end
@ -877,6 +888,8 @@ class Generator
foreach_enabled_member do |_, member|
name = member["name"]
type = member["type"]
min = member["min"] || 0
max = member["max"]
default_value = member["default_value"]
next if %i[ zero target ].include? default_value
@ -894,13 +907,17 @@ class Generator
unsigned = !$~[:unsigned].empty?
bitsize = $~[:bitsize].to_i
type_range = unsigned ? 0..(2**bitsize-1) : (-2**(bitsize-1)+1)..(2**(bitsize-1)-1)
raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max'
min = type_range.min if min =~ /\AU?INT\d+_MIN\Z/
max = type_range.max if max =~ /\AU?INT\d+_MAX\Z/
raise "Member #{name} default value has an invalid type, integer or symbol expected" unless default_value.is_a? Integer or default_value.is_a? Symbol
raise "Member #{name} default value is outside type's storage range, min #{type_range.min}, max #{type_range.max}" unless default_value.is_a? Symbol or type_range === default_value
raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max'
raise "Member #{name} default value is outside of the allowed range" if default_value.is_a? Numeric and min.is_a? Numeric and max.is_a? Numeric and not (min..max) === default_value
when type == "float"
raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max'
raise "Member #{name} default value has an invalid type, numeric or symbol expected" unless default_value.is_a? Numeric or default_value.is_a? Symbol
raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max'
raise "Member #{name} default value is outside of the allowed range" if default_value.is_a? Numeric and min.is_a? Numeric and max.is_a? Numeric and not (min..max) === default_value
when type == "string"
max = member["max"].to_i
@ -1041,6 +1058,18 @@ class Generator
raise "Missing default value for #{missing_default_value_names.count} member#{"s" unless missing_default_value_names.one?}: #{missing_default_value_names * ", "}" unless missing_default_value_names.empty?
end
def resolv_min_max_and_default_values_if_possible
foreach_member do |_, member|
%w[ min max default_value ].each do |value_type|
member_value = member[value_type]
if member_value.is_a? String
constant_value = @constants[member_value]
member[value_type] = constant_value unless constant_value.nil?
end
end
end
end
def resolve_constants(constants)
return nil unless constants.length > 0
s = Set.new

75
src/utils/update_cli_docs.py
View file

@ -22,6 +22,16 @@ DEFAULTS_BLACKLIST = [
'serialrx_provider',
]
MIN_MAX_REPLACEMENTS = {
'INT16_MIN': -32768,
'INT16_MAX': 32767,
'INT32_MIN': -2147483648,
'INT32_MAX': 2147483647,
'UINT8_MAX': 255,
'UINT16_MAX': 65535,
'UINT32_MAX': 4294967295,
}
def parse_settings_yaml():
"""Parse the YAML settings specs"""
@ -32,42 +42,59 @@ def generate_md_table_from_yaml(settings_yaml):
"""Generate a sorted markdown table with description & default value for each setting"""
params = {}
# Extract description and default value of each setting from the YAML specs (if present)
# Extract description, default/min/max values of each setting from the YAML specs (if present)
for group in settings_yaml['groups']:
for member in group['members']:
if not any(key in member for key in ["description", "default_value"]) and not options.quiet:
print("Setting \"{}\" has no description or default value specified".format(member['name']))
# Handle edge cases of YAML autogeneration
if "default_value" in member:
# Replace booleans with "ON"/"OFF"
if type(member["default_value"]) == bool:
member["default_value"] = "ON" if member["default_value"] else "OFF"
# Replace zero placeholder with actual zero
elif member["default_value"] == ":zero":
member["default_value"] = 0
# Replace target-default placeholder with extended definition
elif member["default_value"] == ":target":
member["default_value"] = "_target default_"
# Replace empty strings with more evident marker
elif member["default_value"] == "":
member["default_value"] = "_empty_"
# Reformat direct code references
elif str(member["default_value"])[0] == ":":
member["default_value"] = f'`{member["default_value"][1:]}`'
if not any(key in member for key in ["description", "default_value", "min", "max"]) and not options.quiet:
print("Setting \"{}\" has an incomplete specification".format(member['name']))
# Handle default/min/max fields for each setting
for key in ["default_value", "min", "max"]:
# Basing on the check above, not all fields may be present
if key in member:
### Fetch actual values from the `constants` block if present
if ('constants' in settings_yaml) and (member[key] in settings_yaml['constants']):
member[key] = settings_yaml['constants'][member[key]]
### Fetch actual values from hardcoded min/max replacements
elif member[key] in MIN_MAX_REPLACEMENTS:
member[key] = MIN_MAX_REPLACEMENTS[member[key]]
### Handle edge cases of YAML autogeneration and prettify some values
# Replace booleans with "ON"/"OFF"
if type(member[key]) == bool:
member[key] = "ON" if member[key] else "OFF"
# Replace zero placeholder with actual zero
elif member[key] == ":zero":
member[key] = 0
# Replace target-default placeholder with extended definition
elif member[key] == ":target":
member[key] = "_target default_"
# Replace empty strings with more evident marker
elif member[key] == "":
member[key] = "_empty_"
# Reformat direct code references
elif str(member[key])[0] == ":":
member[key] = f'`{member[key][1:]}`'
params[member['name']] = {
"description": member["description"] if "description" in member else "",
"default": member["default_value"] if "default_value" in member else ""
"default": member["default_value"] if "default_value" in member else "",
"min": member["min"] if "min" in member else "",
"max": member["max"] if "max" in member else ""
}
# MD table header
md_table_lines = [
"| Variable Name | Default Value | Description |\n",
"| ------------- | ------------- | ----------- |\n",
"| Variable Name | Default Value | Min | Max | Description |\n",
"| ------------- | ------------- | --- | --- | ----------- |\n",
]
# Sort the settings by name and build the rows of the table
for param in sorted(params.items()):
md_table_lines.append("| {} | {} | {} |\n".format(param[0], param[1]['default'], param[1]['description']))
md_table_lines.append("| {} | {} | {} | {} | {} |\n".format(
param[0], param[1]['default'], param[1]['min'], param[1]['max'], param[1]['description']
))
# Return the assembled table
return md_table_lines