mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 01:05:21 +03:00
Merge branch 'master' into avs-automatic-servo-trim
# Conflicts: # src/main/build/debug.h # src/main/fc/settings.yaml
This commit is contained in:
Alexander van Saase
commit
1a6f96ce32
30 changed files with 325 additions and 98 deletions
|
|
@ -103,7 +103,6 @@
|
|||
| failsafe_throttle_low_delay | 0 | 0 | 300 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
|
||||
| fixed_wing_auto_arm | OFF | | | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
|
||||
| flaperon_throw_offset | 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
|
||||
| flip_over_after_crash_power_factor | 65 | 0 | 100 | flip over after crash power factor |
|
||||
| fpv_mix_degrees | 0 | 0 | 50 | |
|
||||
| frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
|
||||
| frsky_default_latitude | 0 | -90 | 90 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
|
||||
|
|
@ -148,6 +147,9 @@
|
|||
| gps_provider | UBLOX | | | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
|
||||
| gps_sbas_mode | NONE | | | Which SBAS mode to be used |
|
||||
| gps_ublox_use_galileo | OFF | | | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
|
||||
| gyro_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter |
|
||||
| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter |
|
||||
| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter |
|
||||
| gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
|
||||
| gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF |
|
||||
| gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF |
|
||||
|
|
@ -289,7 +291,7 @@
|
|||
| nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) |
|
||||
| nav_fw_land_dive_angle | 2 | -20 | 20 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
|
||||
| nav_fw_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
|
||||
| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
|
||||
| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
|
||||
| nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] |
|
||||
| nav_fw_launch_end_time | 3000 | 0 | 5000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
|
||||
| nav_fw_launch_idle_thr | 1000 | 1000 | 2000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
|
||||
|
|
@ -424,6 +426,7 @@
|
|||
| osd_right_sidebar_scroll_step | 0 | | 255 | Same as left_sidebar_scroll_step, but for the right sidebar |
|
||||
| osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) |
|
||||
| osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink |
|
||||
| osd_sidebar_height | 3 | 0 | 5 | Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) |
|
||||
| osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. |
|
||||
| osd_sidebar_scroll_arrows | OFF | | | |
|
||||
| osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) |
|
||||
|
|
@ -522,6 +525,7 @@
|
|||
| tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. |
|
||||
| tpa_rate | 0 | 0 | 100 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. |
|
||||
| tri_unarmed_servo | ON | | | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
|
||||
| turtle_mode_power_factor | 55 | 0 | 100 | Turtle mode power factor |
|
||||
| tz_automatic_dst | OFF | | | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. |
|
||||
| tz_offset | 0 | -1440 | 1440 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
|
||||
| vbat_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled |
|
||||
|
|
|
|||
|
|
@ -83,7 +83,8 @@ typedef enum {
|
|||
DEBUG_FW_D,
|
||||
DEBUG_IMU2,
|
||||
DEBUG_ALTITUDE,
|
||||
DEBUG_SMITH_COMPENSATOR,
|
||||
DEBUG_GYRO_ALPHA_BETA_GAMMA,
|
||||
DEBUG_SMITH_PREDICTOR,
|
||||
DEBUG_AUTOTRIM,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
|
|
|||
|
|
@ -250,3 +250,69 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
|
|||
filter->y1 = y1;
|
||||
filter->y2 = y2;
|
||||
}
|
||||
|
||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
||||
void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
|
||||
// beta, gamma, and eta gains all derived from
|
||||
// http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
|
||||
|
||||
const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
|
||||
filter->xk = 0.0f;
|
||||
filter->vk = 0.0f;
|
||||
filter->ak = 0.0f;
|
||||
filter->jk = 0.0f;
|
||||
filter->a = alpha;
|
||||
filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
|
||||
filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
|
||||
filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
|
||||
filter->dT = dT;
|
||||
filter->dT2 = dT * dT;
|
||||
filter->dT3 = dT * dT * dT;
|
||||
pt1FilterInit(&filter->boostFilter, 100, dT);
|
||||
|
||||
const float boost = boostGain * 100;
|
||||
|
||||
filter->boost = (boost * boost / 10000) * 0.003;
|
||||
filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
|
||||
}
|
||||
|
||||
FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
|
||||
//xk - current system state (ie: position)
|
||||
//vk - derivative of system state (ie: velocity)
|
||||
//ak - derivative of system velociy (ie: acceleration)
|
||||
//jk - derivative of system acceleration (ie: jerk)
|
||||
float rk; // residual error
|
||||
|
||||
// give the filter limited history
|
||||
filter->xk *= filter->halfLife;
|
||||
filter->vk *= filter->halfLife;
|
||||
filter->ak *= filter->halfLife;
|
||||
filter->jk *= filter->halfLife;
|
||||
|
||||
// update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
|
||||
filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
|
||||
|
||||
// update (estimated) velocity (also estimated dterm from measurement)
|
||||
filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
|
||||
filter->ak += filter->dT * filter->jk;
|
||||
|
||||
// what is our residual error (measured - estimated)
|
||||
rk = input - filter->xk;
|
||||
|
||||
// artificially boost the error to increase the response of the filter
|
||||
rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
|
||||
if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
|
||||
rk = (input - filter->xk) / filter->a;
|
||||
}
|
||||
filter->rk = rk; // for logging
|
||||
|
||||
// update our estimates given the residual error.
|
||||
filter->xk += filter->a * rk;
|
||||
filter->vk += filter->b / filter->dT * rk;
|
||||
filter->ak += filter->g / (2.0f * filter->dT2) * rk;
|
||||
filter->jk += filter->e / (6.0f * filter->dT3) * rk;
|
||||
|
||||
return filter->xk;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -56,6 +56,18 @@ typedef struct firFilter_s {
|
|||
uint8_t coeffsLength;
|
||||
} firFilter_t;
|
||||
|
||||
typedef struct alphaBetaGammaFilter_s {
|
||||
float a, b, g, e;
|
||||
float ak; // derivative of system velociy (ie: acceleration)
|
||||
float vk; // derivative of system state (ie: velocity)
|
||||
float xk; // current system state (ie: position)
|
||||
float jk; // derivative of system acceleration (ie: jerk)
|
||||
float rk; // residual error
|
||||
float dT, dT2, dT3;
|
||||
float halfLife, boost;
|
||||
pt1Filter_t boostFilter;
|
||||
} alphaBetaGammaFilter_t;
|
||||
|
||||
typedef float (*filterApplyFnPtr)(void *filter, float input);
|
||||
typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
|
||||
|
||||
|
|
@ -86,3 +98,6 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
|
|||
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
|
||||
float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
|
||||
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
|
||||
|
||||
void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
|
||||
float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
|
||||
|
|
@ -169,7 +169,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
|
|||
|
||||
static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud)
|
||||
{
|
||||
uint32_t baseClock = SystemCoreClock / timerClockDivisor(tch->timHw->tim);
|
||||
uint32_t baseClock = timerClock(tch->timHw->tim);
|
||||
uint32_t clock = baseClock;
|
||||
uint32_t timerPeriod;
|
||||
|
||||
|
|
|
|||
|
|
@ -216,6 +216,12 @@ void usbVcpInitHardware(void)
|
|||
|
||||
/* Start Device Process */
|
||||
USBD_Start(&USBD_Device);
|
||||
|
||||
#ifdef STM32H7
|
||||
HAL_PWREx_EnableUSBVoltageDetector();
|
||||
delay(100); // Cold boot failures observed without this, even when USB cable is not connected
|
||||
#endif
|
||||
|
||||
#else
|
||||
Set_System();
|
||||
Set_USBClock();
|
||||
|
|
|
|||
|
|
@ -51,10 +51,13 @@ bool isMPUSoftReset(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1ff09800)
|
||||
uint32_t systemBootloaderAddress(void)
|
||||
{
|
||||
return SYSMEMBOOT_VECTOR_TABLE[1];
|
||||
#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
|
||||
return 0x1ff09800;
|
||||
#else
|
||||
#error Unknown MCU
|
||||
#endif
|
||||
}
|
||||
|
||||
void systemClockSetup(uint8_t cpuUnderclock)
|
||||
|
|
|
|||
|
|
@ -240,7 +240,7 @@ uint32_t timerGetBaseClock(TCH_t * tch)
|
|||
|
||||
uint32_t timerGetBaseClockHW(const timerHardware_t * timHw)
|
||||
{
|
||||
return SystemCoreClock / timerClockDivisor(timHw->tim);
|
||||
return timerClock(timHw->tim);
|
||||
}
|
||||
|
||||
bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
|
||||
|
|
|
|||
|
|
@ -157,7 +157,7 @@ typedef enum {
|
|||
TYPE_TIMER
|
||||
} channelType_t;
|
||||
|
||||
uint8_t timerClockDivisor(TIM_TypeDef *tim);
|
||||
uint32_t timerClock(TIM_TypeDef *tim);
|
||||
uint32_t timerGetBaseClockHW(const timerHardware_t * timHw);
|
||||
|
||||
const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag);
|
||||
|
|
|
|||
|
|
@ -48,10 +48,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
|||
[16] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM1_TRG_COM_TIM17_IRQn },
|
||||
};
|
||||
|
||||
uint8_t timerClockDivisor(TIM_TypeDef *tim)
|
||||
uint32_t timerClock(TIM_TypeDef *tim)
|
||||
{
|
||||
UNUSED(tim);
|
||||
return 1;
|
||||
return SystemCoreClock;
|
||||
}
|
||||
|
||||
_TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
|
||||
|
|
|
|||
|
|
@ -95,16 +95,16 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
|||
#endif
|
||||
};
|
||||
|
||||
uint8_t timerClockDivisor(TIM_TypeDef *tim)
|
||||
uint32_t timerClock(TIM_TypeDef *tim)
|
||||
{
|
||||
#if defined (STM32F411xE)
|
||||
UNUSED(tim);
|
||||
return 1;
|
||||
return SystemCoreClock;
|
||||
#elif defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F446xx)
|
||||
if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
|
||||
return 1;
|
||||
return SystemCoreClock;
|
||||
} else {
|
||||
return 2;
|
||||
return SystemCoreClock / 2;
|
||||
}
|
||||
#else
|
||||
#error "No timer clock defined correctly for the MCU"
|
||||
|
|
|
|||
|
|
@ -48,12 +48,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
|||
[13] = { .tim = TIM14, .rcc = RCC_APB1(TIM14), .irq = TIM8_TRG_COM_TIM14_IRQn},
|
||||
};
|
||||
|
||||
uint8_t timerClockDivisor(TIM_TypeDef *tim)
|
||||
{
|
||||
UNUSED(tim);
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint32_t timerClock(TIM_TypeDef *tim)
|
||||
{
|
||||
UNUSED(tim);
|
||||
|
|
|
|||
|
|
@ -48,16 +48,48 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
|||
[13] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM17_IRQn},
|
||||
};
|
||||
|
||||
uint8_t timerClockDivisor(TIM_TypeDef *tim)
|
||||
{
|
||||
UNUSED(tim);
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint32_t timerClock(TIM_TypeDef *tim)
|
||||
{
|
||||
UNUSED(tim);
|
||||
return SystemCoreClock;
|
||||
int timpre;
|
||||
uint32_t pclk;
|
||||
uint32_t ppre;
|
||||
|
||||
// Implement the table:
|
||||
// RM0433 (Rev 6) Table 52.
|
||||
// RM0455 (Rev 3) Table 55.
|
||||
// "Ratio between clock timer and pclk"
|
||||
// (Tables are the same, just D2 or CD difference)
|
||||
|
||||
#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
|
||||
#define PERIPH_PRESCALER(bus) ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE ## bus) >> RCC_D2CFGR_D2PPRE ## bus ## _Pos)
|
||||
#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ)
|
||||
#define PERIPH_PRESCALER(bus) ((RCC->CDCFGR2 & RCC_CDCFGR2_CDPPRE ## bus) >> RCC_CDCFGR2_CDPPRE ## bus ## _Pos)
|
||||
#else
|
||||
#error Unknown MCU type
|
||||
#endif
|
||||
|
||||
if (tim == TIM1 || tim == TIM8 || tim == TIM15 || tim == TIM16 || tim == TIM17) {
|
||||
// Timers on APB2
|
||||
pclk = HAL_RCC_GetPCLK2Freq();
|
||||
ppre = PERIPH_PRESCALER(2);
|
||||
} else {
|
||||
// Timers on APB1
|
||||
pclk = HAL_RCC_GetPCLK1Freq();
|
||||
ppre = PERIPH_PRESCALER(1);
|
||||
}
|
||||
|
||||
timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0;
|
||||
|
||||
int index = (timpre << 3) | ppre;
|
||||
|
||||
static uint8_t periphToKernel[16] = { // The mutiplier table
|
||||
1, 1, 1, 1, 2, 2, 2, 2, // TIMPRE = 0
|
||||
1, 1, 1, 1, 2, 4, 4, 4 // TIMPRE = 1
|
||||
};
|
||||
|
||||
return pclk * periphToKernel[index];
|
||||
|
||||
#undef PERIPH_PRESCALER
|
||||
}
|
||||
|
||||
_TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
|
||||
|
|
@ -66,8 +98,6 @@ _TIM_IRQ_HANDLER(TIM3_IRQHandler, 3);
|
|||
_TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
|
||||
_TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
|
||||
_TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
|
||||
//_TIM_IRQ_HANDLER(TIM1_BRK_TIM9_IRQHandler, 9);
|
||||
//_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM11_IRQHandler, 11);
|
||||
_TIM_IRQ_HANDLER(TIM8_BRK_TIM12_IRQHandler, 12);
|
||||
_TIM_IRQ_HANDLER(TIM15_IRQHandler, 15);
|
||||
_TIM_IRQ_HANDLER(TIM16_IRQHandler, 16);
|
||||
|
|
|
|||
|
|
@ -420,9 +420,9 @@ void disarm(disarmReason_t disarmReason)
|
|||
}
|
||||
#endif
|
||||
#ifdef USE_DSHOT
|
||||
if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
|
||||
if (FLIGHT_MODE(TURTLE_MODE)) {
|
||||
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
|
||||
DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
|
||||
DISABLE_FLIGHT_MODE(TURTLE_MODE);
|
||||
}
|
||||
#endif
|
||||
statsOnDisarm();
|
||||
|
|
@ -494,6 +494,22 @@ void tryArm(void)
|
|||
{
|
||||
updateArmingStatus();
|
||||
|
||||
#ifdef USE_DSHOT
|
||||
if (
|
||||
STATE(MULTIROTOR) &&
|
||||
IS_RC_MODE_ACTIVE(BOXTURTLE) &&
|
||||
emergencyArmingCanOverrideArmingDisabled() &&
|
||||
isMotorProtocolDshot() &&
|
||||
!ARMING_FLAG(ARMED) &&
|
||||
!FLIGHT_MODE(TURTLE_MODE)
|
||||
) {
|
||||
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
enableFlightMode(TURTLE_MODE);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_PROGRAMMING_FRAMEWORK
|
||||
if (
|
||||
!isArmingDisabled() ||
|
||||
|
|
@ -510,21 +526,6 @@ void tryArm(void)
|
|||
return;
|
||||
}
|
||||
|
||||
#ifdef USE_DSHOT
|
||||
if (
|
||||
STATE(MULTIROTOR) &&
|
||||
IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
|
||||
emergencyArmingCanOverrideArmingDisabled() &&
|
||||
isMotorProtocolDshot() &&
|
||||
!FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
|
||||
) {
|
||||
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
enableFlightMode(FLIP_OVER_AFTER_CRASH);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_NAV)
|
||||
// If nav_extra_arming_safety was bypassed we always
|
||||
// allow bypassing it even without the sticks set
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
|
||||
{ BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
|
||||
{ BOXPREARM, "PREARM", 51 },
|
||||
{ BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
|
||||
{ BOXTURTLE, "TURTLE", 52 },
|
||||
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
||||
};
|
||||
|
||||
|
|
@ -316,7 +316,7 @@ void initActiveBoxIds(void)
|
|||
|
||||
#ifdef USE_DSHOT
|
||||
if(STATE(MULTIROTOR) && isMotorProtocolDshot())
|
||||
activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXTURTLE;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ typedef enum {
|
|||
BOXLOITERDIRCHN = 40,
|
||||
BOXMSPRCOVERRIDE = 41,
|
||||
BOXPREARM = 42,
|
||||
BOXFLIPOVERAFTERCRASH = 43,
|
||||
BOXTURTLE = 43,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ typedef enum {
|
|||
NAV_COURSE_HOLD_MODE = (1 << 12),
|
||||
FLAPERON = (1 << 13),
|
||||
TURN_ASSISTANT = (1 << 14),
|
||||
FLIP_OVER_AFTER_CRASH = (1 << 15),
|
||||
TURTLE_MODE = (1 << 15),
|
||||
} flightModeFlags_e;
|
||||
|
||||
extern uint32_t flightModeFlags;
|
||||
|
|
|
|||
|
|
@ -94,7 +94,7 @@ tables:
|
|||
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
|
||||
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
|
||||
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
|
||||
"SMITH_COMPENSATOR", "AUTOTRIM"]
|
||||
"GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM"]
|
||||
- name: async_mode
|
||||
values: ["NONE", "GYRO", "ALL"]
|
||||
- name: aux_operator
|
||||
|
|
@ -296,6 +296,27 @@ groups:
|
|||
min: 0
|
||||
max: 1
|
||||
default_value: 0
|
||||
- name: gyro_abg_alpha
|
||||
description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
|
||||
default_value: 0
|
||||
field: alphaBetaGammaAlpha
|
||||
condition: USE_ALPHA_BETA_GAMMA_FILTER
|
||||
min: 0
|
||||
max: 1
|
||||
- name: gyro_abg_boost
|
||||
description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
|
||||
default_value: 0.35
|
||||
field: alphaBetaGammaBoost
|
||||
condition: USE_ALPHA_BETA_GAMMA_FILTER
|
||||
min: 0
|
||||
max: 2
|
||||
- name: gyro_abg_half_life
|
||||
description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
|
||||
default_value: 0.5
|
||||
field: alphaBetaGammaHalfLife
|
||||
condition: USE_ALPHA_BETA_GAMMA_FILTER
|
||||
min: 0
|
||||
max: 10
|
||||
|
||||
- name: PG_ADC_CHANNEL_CONFIG
|
||||
type: adcChannelConfig_t
|
||||
|
|
@ -851,10 +872,10 @@ groups:
|
|||
min: 4
|
||||
max: 255
|
||||
default_value: 14
|
||||
- name: flip_over_after_crash_power_factor
|
||||
field: flipOverAfterPowerFactor
|
||||
default_value: 65
|
||||
description: "flip over after crash power factor"
|
||||
- name: turtle_mode_power_factor
|
||||
field: turtleModePowerFactor
|
||||
default_value: 55
|
||||
description: "Turtle mode power factor"
|
||||
condition: "USE_DSHOT"
|
||||
min: 0
|
||||
max: 100
|
||||
|
|
@ -2670,7 +2691,7 @@ groups:
|
|||
min: 0
|
||||
max: 60000
|
||||
- name: nav_fw_launch_climb_angle
|
||||
description: "Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
|
||||
description: "Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
|
||||
default_value: 18
|
||||
field: fw.launch_climb_angle
|
||||
min: 5
|
||||
|
|
@ -3288,6 +3309,13 @@ groups:
|
|||
default_value: 0
|
||||
description: Same as left_sidebar_scroll_step, but for the right sidebar
|
||||
|
||||
- name: osd_sidebar_height
|
||||
field: sidebar_height
|
||||
min: 0
|
||||
max: 5
|
||||
default_value: 3
|
||||
description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
|
||||
|
||||
- name: osd_home_position_arm_screen
|
||||
type: bool
|
||||
default_value: ON
|
||||
|
|
|
|||
|
|
@ -116,13 +116,12 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
|||
.throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
|
||||
.motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles
|
||||
#ifdef USE_DSHOT
|
||||
.flipOverAfterPowerFactor = SETTING_FLIP_OVER_AFTER_CRASH_POWER_FACTOR_DEFAULT,
|
||||
.turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
|
||||
#endif
|
||||
);
|
||||
|
||||
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
|
||||
|
||||
#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
|
||||
#define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
|
||||
|
||||
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
|
||||
|
|
@ -328,10 +327,10 @@ static uint16_t handleOutputScaling(
|
|||
}
|
||||
return value;
|
||||
}
|
||||
static void applyFlipOverAfterCrashModeToMotors(void) {
|
||||
static void applyTurtleModeToMotors(void) {
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
|
||||
const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f;
|
||||
const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
|
||||
const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
|
||||
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
|
||||
|
|
@ -394,13 +393,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) {
|
|||
|
||||
motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
|
||||
|
||||
float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
|
||||
|
||||
// Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
|
||||
motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
|
||||
motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
|
||||
|
||||
motor[i] = motorOutput;
|
||||
motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle);
|
||||
}
|
||||
} else {
|
||||
// Disarmed mode
|
||||
|
|
@ -532,8 +525,8 @@ static int getReversibleMotorsThrottleDeadband(void)
|
|||
void FAST_CODE mixTable(const float dT)
|
||||
{
|
||||
#ifdef USE_DSHOT
|
||||
if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
|
||||
applyFlipOverAfterCrashModeToMotors();
|
||||
if (FLIGHT_MODE(TURTLE_MODE)) {
|
||||
applyTurtleModeToMotors();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -87,7 +87,7 @@ typedef struct motorConfig_s {
|
|||
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
|
||||
float throttleScale; // Scaling factor for throttle.
|
||||
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
|
||||
uint8_t flipOverAfterPowerFactor; // Power factor from 0 to 100% of flip over after crash
|
||||
uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash
|
||||
} motorConfig_t;
|
||||
|
||||
PG_DECLARE(motorConfig_t, motorConfig);
|
||||
|
|
|
|||
|
|
@ -1103,9 +1103,9 @@ void FAST_CODE pidController(float dT)
|
|||
#endif
|
||||
|
||||
#ifdef USE_SMITH_PREDICTOR
|
||||
DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis, pidState[axis].gyroRate);
|
||||
DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis, pidState[axis].gyroRate);
|
||||
pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
|
||||
DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis + 3, pidState[axis].gyroRate);
|
||||
DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis + 3, pidState[axis].gyroRate);
|
||||
#endif
|
||||
|
||||
DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate);
|
||||
|
|
|
|||
|
|
@ -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, 0);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 1);
|
||||
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
|
||||
|
||||
static int digitCount(int32_t value)
|
||||
|
|
@ -1770,6 +1770,8 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
p = "!FS!";
|
||||
else if (FLIGHT_MODE(MANUAL_MODE))
|
||||
p = "MANU";
|
||||
else if (FLIGHT_MODE(TURTLE_MODE))
|
||||
p = "TURT";
|
||||
else if (FLIGHT_MODE(NAV_RTH_MODE))
|
||||
p = isWaypointMissionRTHActive() ? "WRTH" : "RTH ";
|
||||
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
||||
|
|
@ -1790,8 +1792,6 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
p = "ANGL";
|
||||
else if (FLIGHT_MODE(HORIZON_MODE))
|
||||
p = "HOR ";
|
||||
else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
|
||||
p = "TURT";
|
||||
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
|
||||
return true;
|
||||
|
|
@ -1963,9 +1963,10 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
wp2.lon = posControl.waypointList[j].lon;
|
||||
wp2.alt = posControl.waypointList[j].alt;
|
||||
fpVector3_t poi;
|
||||
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
|
||||
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3));
|
||||
int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude();
|
||||
while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
|
||||
osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
|
||||
osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2811,6 +2812,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
|
|||
.sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT,
|
||||
.left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT,
|
||||
.right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT,
|
||||
.sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT,
|
||||
.osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
|
||||
.pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
|
||||
.pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
|
||||
|
|
|
|||
|
|
@ -369,6 +369,7 @@ typedef struct osdConfig_s {
|
|||
uint8_t pan_servo_index; // Index of the pan servo used for home direction offset
|
||||
int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm
|
||||
uint8_t crsf_lq_format;
|
||||
uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
|
||||
uint8_t telemetry; // use telemetry on displayed pixel line 0
|
||||
|
||||
} osdConfig_t;
|
||||
|
|
|
|||
|
|
@ -295,7 +295,7 @@ void osdGridDrawSidebars(displayPort_t *display)
|
|||
uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
|
||||
|
||||
const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS;
|
||||
const int hudheight = OSD_AH_SIDEBAR_HEIGHT_POS;
|
||||
const int hudheight = osdConfig()->sidebar_height;
|
||||
|
||||
// Arrows
|
||||
if (osdConfig()->sidebar_scroll_arrows) {
|
||||
|
|
@ -315,11 +315,12 @@ void osdGridDrawSidebars(displayPort_t *display)
|
|||
// Draw AH sides
|
||||
int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0);
|
||||
int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1);
|
||||
if (osdConfig()->sidebar_height) {
|
||||
for (int y = -hudheight; y <= hudheight; y++) {
|
||||
displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
|
||||
displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
|
||||
}
|
||||
|
||||
}
|
||||
// AH level indicators
|
||||
displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT);
|
||||
displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT);
|
||||
|
|
|
|||
|
|
@ -236,7 +236,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
|
|||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
|
||||
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
|
||||
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
|
||||
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
|
||||
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
|
||||
static navigationFSMEvent_t nextForNonGeoStates(void);
|
||||
static bool isWaypointMissionValid(void);
|
||||
|
||||
|
|
@ -1502,7 +1502,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
if (STATE(MULTIROTOR)) {
|
||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
|
||||
mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
|
||||
&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||
&posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
|
||||
}
|
||||
return nextForNonGeoStates();
|
||||
|
||||
|
|
@ -1875,6 +1875,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
|
|||
break;
|
||||
|
||||
case RTH_HOME_FINAL_LAND:
|
||||
// if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
|
||||
posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm
|
||||
if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
|
||||
posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -2890,7 +2897,7 @@ void resetSafeHomes(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
|
||||
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
|
||||
{
|
||||
gpsLocation_t wpLLH;
|
||||
|
||||
|
|
@ -2898,7 +2905,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
|
|||
wpLLH.lon = waypoint->lon;
|
||||
wpLLH.alt = waypoint->alt;
|
||||
|
||||
geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
|
||||
geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
|
||||
}
|
||||
|
||||
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
|
||||
|
|
@ -2912,10 +2919,15 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
|
|||
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
}
|
||||
|
||||
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
|
||||
{
|
||||
return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
|
||||
}
|
||||
|
||||
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
||||
{
|
||||
fpVector3_t localPos;
|
||||
mapWaypointToLocalPosition(&localPos, waypoint);
|
||||
mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
|
||||
calculateAndSetActiveWaypointToLocalPosition(&localPos);
|
||||
}
|
||||
|
||||
|
|
@ -3321,7 +3333,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
|||
// Don't allow arming if first waypoint is farther than configured safe distance
|
||||
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
|
||||
fpVector3_t startingWaypointPos;
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
|
||||
|
||||
const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
|
||||
|
||||
|
|
|
|||
|
|
@ -481,6 +481,11 @@ typedef enum {
|
|||
GEO_ORIGIN_RESET_ALTITUDE
|
||||
} geoOriginResetMode_e;
|
||||
|
||||
typedef enum {
|
||||
NAV_WP_TAKEOFF_DATUM,
|
||||
NAV_WP_MSL_DATUM
|
||||
} geoAltitudeDatumFlag_e;
|
||||
|
||||
// geoSetOrigin stores the location provided in llh as a GPS origin in the
|
||||
// provided origin parameter. resetMode indicates wether all origin coordinates
|
||||
// should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
|
||||
|
|
@ -501,6 +506,8 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh
|
|||
// the provided origin is valid and the conversion could be performed.
|
||||
bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
|
||||
float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
|
||||
// Select absolute or relative altitude based on WP mission flag setting
|
||||
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
|
||||
|
||||
/* Distance/bearing calculation */
|
||||
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
|
||||
|
|
|
|||
|
|
@ -104,7 +104,14 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
|
|||
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
|
||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
||||
|
||||
STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn;
|
||||
STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
|
||||
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13);
|
||||
|
||||
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
||||
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
|
||||
|
|
@ -130,6 +137,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
|||
.dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
|
||||
.dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
|
||||
#endif
|
||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
||||
.alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT,
|
||||
.alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
|
||||
.alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
|
||||
#endif
|
||||
);
|
||||
|
||||
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
|
||||
|
|
@ -284,6 +296,24 @@ static void gyroInitFilters(void)
|
|||
biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
||||
|
||||
abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply;
|
||||
|
||||
if (gyroConfig()->alphaBetaGammaAlpha > 0) {
|
||||
abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
alphaBetaGammaFilterInit(
|
||||
&abgFilter[axis],
|
||||
gyroConfig()->alphaBetaGammaAlpha,
|
||||
gyroConfig()->alphaBetaGammaBoost,
|
||||
gyroConfig()->alphaBetaGammaHalfLife,
|
||||
getLooptime() * 1e-6f
|
||||
);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
bool gyroInit(void)
|
||||
|
|
@ -458,6 +488,12 @@ void FAST_CODE NOINLINE gyroUpdate()
|
|||
gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
|
||||
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
|
||||
|
||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
||||
DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf);
|
||||
gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf);
|
||||
DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf);
|
||||
#endif
|
||||
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
if (dynamicGyroNotchState.enabled) {
|
||||
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
|
||||
|
|
|
|||
|
|
@ -82,6 +82,11 @@ typedef struct gyroConfig_s {
|
|||
uint16_t dynamicGyroNotchMinHz;
|
||||
uint8_t dynamicGyroNotchEnabled;
|
||||
#endif
|
||||
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
|
||||
float alphaBetaGammaAlpha;
|
||||
float alphaBetaGammaBoost;
|
||||
float alphaBetaGammaHalfLife;
|
||||
#endif
|
||||
} gyroConfig_t;
|
||||
|
||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||
|
|
|
|||
|
|
@ -33,8 +33,29 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500, DEVHW_MPU6500, IMU2_SPI_BUS, IMU2
|
|||
//BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6000, DEVHW_MPU6000, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN);
|
||||
|
||||
const timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
|
||||
DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S1
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S2
|
||||
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
|
||||
DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
|
||||
|
||||
DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S7
|
||||
DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S8
|
||||
DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S9
|
||||
DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE
|
||||
|
||||
DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S11
|
||||
DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S12
|
||||
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_2812
|
||||
DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM
|
||||
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM
|
||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6
|
||||
DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0, 0), // RX4
|
||||
DEF_TIM(TIM17, CH1, PB9, TIM_USE_ANY, 0, 0), // TX4
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
|
|
|
|||
|
|
@ -84,6 +84,7 @@
|
|||
#define USE_PITOT_ADC
|
||||
#define USE_PITOT_VIRTUAL
|
||||
|
||||
#define USE_ALPHA_BETA_GAMMA_FILTER
|
||||
#define USE_DYNAMIC_FILTERS
|
||||
#define USE_GYRO_KALMAN
|
||||
#define USE_SMITH_PREDICTOR
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue