diff --git a/mk/source.mk b/mk/source.mk
index 9f837c9caa..8fe65dde2f 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -57,6 +57,7 @@ COMMON_SRC = \
build/version.c \
main.c \
common/bitarray.c \
+ common/chirp.c \
common/colorconversion.c \
common/crc.c \
common/encoding.c \
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index c8f359b924..acebd8c2c2 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1642,6 +1642,17 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_YAW, "%d", currentPidProfile->pid[PID_YAW].S);
#endif // USE_WING
+#ifdef USE_CHIRP
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_LAG_FREQ_HZ, "%d", currentPidProfile->chirp_lag_freq_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_LEAD_FREQ_HZ, "%d", currentPidProfile->chirp_lead_freq_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_AMPLITUDE_ROLL, "%d", currentPidProfile->chirp_amplitude_roll);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_AMPLITUDE_PITCH, "%d", currentPidProfile->chirp_amplitude_pitch);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_AMPLITUDE_YAW, "%d", currentPidProfile->chirp_amplitude_yaw);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_FREQUENCY_START_DECI_HZ, "%d", currentPidProfile->chirp_frequency_start_deci_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_FREQUENCY_END_DECI_HZ, "%d", currentPidProfile->chirp_frequency_end_deci_hz);
+ BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_CHIRP_TIME_SECONDS, "%d", currentPidProfile->chirp_time_seconds);
+#endif // USE_CHIRP
+
// End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d", rcControlsConfig()->deadband);
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 3d67bae371..fb8795bd42 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -124,4 +124,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
[DEBUG_GIMBAL] = "GIMBAL",
[DEBUG_WING_SETPOINT] = "WING_SETPOINT",
[DEBUG_AUTOPILOT_POSITION] = "AUTOPILOT_POSITION",
+ [DEBUG_CHIRP] = "CHIRP",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 6e6e43a67a..b72ebdb496 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -126,6 +126,7 @@ typedef enum {
DEBUG_GIMBAL,
DEBUG_WING_SETPOINT,
DEBUG_AUTOPILOT_POSITION,
+ DEBUG_CHIRP,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 3e79fc4d78..3db4343db9 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1263,6 +1263,17 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_HORIZON_IGNORE_STICKS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_ignore_sticks) },
{ PARAM_NAME_HORIZON_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 5000 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_delay_ms) },
+#ifdef USE_CHIRP
+ { PARAM_NAME_CHIRP_LAG_FREQ_HZ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_lag_freq_hz) },
+ { PARAM_NAME_CHIRP_LEAD_FREQ_HZ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_lead_freq_hz) },
+ { PARAM_NAME_CHIRP_AMPLITUDE_ROLL, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_amplitude_roll) },
+ { PARAM_NAME_CHIRP_AMPLITUDE_PITCH, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_amplitude_pitch) },
+ { PARAM_NAME_CHIRP_AMPLITUDE_YAW, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_amplitude_yaw) },
+ { PARAM_NAME_CHIRP_FREQUENCY_START_DECI_HZ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_frequency_start_deci_hz) },
+ { PARAM_NAME_CHIRP_FREQUENCY_END_DECI_HZ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 10000 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_frequency_end_deci_hz) },
+ { PARAM_NAME_CHIRP_TIME_SECONDS, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, chirp_time_seconds) },
+#endif
+
#if defined(USE_ABSOLUTE_CONTROL)
{ PARAM_NAME_ABS_CONTROL_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_gain) },
{ "abs_control_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_limit) },
diff --git a/src/main/common/chirp.c b/src/main/common/chirp.c
new file mode 100644
index 0000000000..20aff3db14
--- /dev/null
+++ b/src/main/common/chirp.c
@@ -0,0 +1,94 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+
+#include "maths.h"
+
+#include "chirp.h"
+
+// initialize the chirp signal generator
+// f0: start frequency in Hz
+// f1: end frequency in Hz
+// t1: signal length in seconds
+// looptimeUs: loop time in microseconds
+void chirpInit(chirp_t *chirp, const float f0, const float f1, const float t1, const uint32_t looptimeUs)
+{
+ chirp->f0 = f0;
+ chirp->Ts = looptimeUs * 1e-6f;
+ chirp->N = (uint32_t)(t1 / chirp->Ts);
+ chirp->beta = pow_approx(f1 / f0, 1.0f / t1);
+ chirp->k0 = 2.0f * M_PIf / log_approx(chirp->beta);
+ chirp->k1 = chirp->k0 * f0;
+ chirpReset(chirp);
+}
+
+// reset the chirp signal generator fully
+void chirpReset(chirp_t *chirp)
+{
+ chirp->count = 0;
+ chirp->isFinished = false;
+ chirpResetSignals(chirp);
+}
+
+// reset the chirp signal generator signals
+void chirpResetSignals(chirp_t *chirp)
+{
+ chirp->exc = 0.0f;
+ chirp->fchirp = 0.0f;
+ chirp->sinarg = 0.0f;
+}
+
+// update the chirp signal generator
+bool chirpUpdate(chirp_t *chirp)
+{
+ if (chirp->isFinished) {
+
+ return false;
+
+ } else if (chirp->count == chirp->N) {
+
+ chirp->isFinished = true;
+ chirpResetSignals(chirp);
+ return false;
+
+ } else {
+
+ chirp->fchirp = chirp->f0 * pow_approx(chirp->beta, (float)(chirp->count) * chirp->Ts);
+ chirp->sinarg = chirp->k0 * chirp->fchirp - chirp->k1;
+
+ // wrap sinarg to 0...2*pi
+ chirp->sinarg = fmodf(chirp->sinarg, 2.0f * M_PIf);
+
+ // use cosine so that the angle will oscillate around 0 (integral of gyro)
+ chirp->exc = cos_approx(chirp->sinarg);
+
+ // frequencies below 1 Hz will lead to the same angle magnitude as at 1 Hz (integral of gyro)
+ if (chirp->fchirp < 1.0f) {
+ chirp->exc = chirp->fchirp * chirp->exc;
+ }
+ chirp->count++;
+
+ return true;
+ }
+}
diff --git a/src/main/common/chirp.h b/src/main/common/chirp.h
new file mode 100644
index 0000000000..211a7c66e1
--- /dev/null
+++ b/src/main/common/chirp.h
@@ -0,0 +1,37 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+typedef struct chirp_s {
+ float f0, Ts, beta, k0, k1;
+ uint32_t count, N;
+ float exc, fchirp, sinarg;
+ bool isFinished;
+} chirp_t;
+
+void chirpInit(chirp_t *chirp, const float f0, const float f1, const float t1, const uint32_t looptimeUs);
+void chirpReset(chirp_t *chirp);
+void chirpResetSignals(chirp_t *chirp);
+bool chirpUpdate(chirp_t *chirp);
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 889204938b..75bd3100d0 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -1092,6 +1092,16 @@ void processRxModes(timeUs_t currentTimeUs)
}
#endif
+#ifdef USE_CHIRP
+ if (IS_RC_MODE_ACTIVE(BOXCHIRP) && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
+ if (!FLIGHT_MODE(CHIRP_MODE)) {
+ ENABLE_FLIGHT_MODE(CHIRP_MODE);
+ }
+ } else {
+ DISABLE_FLIGHT_MODE(CHIRP_MODE);
+ }
+#endif
+
if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE | POS_HOLD_MODE | HORIZON_MODE)) {
LED1_ON;
// increase frequency of attitude task to reduce drift when in angle or horizon mode
diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h
index 43624e06ea..e677b6fe26 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -192,6 +192,15 @@
#define PARAM_NAME_HORIZON_IGNORE_STICKS "horizon_ignore_sticks"
#define PARAM_NAME_HORIZON_DELAY_MS "horizon_delay_ms"
+#define PARAM_NAME_CHIRP_LAG_FREQ_HZ "chirp_lag_freq_hz"
+#define PARAM_NAME_CHIRP_LEAD_FREQ_HZ "chirp_lead_freq_hz"
+#define PARAM_NAME_CHIRP_AMPLITUDE_ROLL "chirp_amplitude_roll"
+#define PARAM_NAME_CHIRP_AMPLITUDE_PITCH "chirp_amplitude_pitch"
+#define PARAM_NAME_CHIRP_AMPLITUDE_YAW "chirp_amplitude_yaw"
+#define PARAM_NAME_CHIRP_FREQUENCY_START_DECI_HZ "chirp_frequency_start_deci_hz"
+#define PARAM_NAME_CHIRP_FREQUENCY_END_DECI_HZ "chirp_frequency_end_deci_hz"
+#define PARAM_NAME_CHIRP_TIME_SECONDS "chirp_time_seconds"
+
#ifdef USE_GPS
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
#define PARAM_NAME_GPS_SBAS_MODE "gps_sbas_mode"
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 04bd4f2f3a..13838c1c23 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -35,6 +35,7 @@ typedef enum {
BOXMAG,
BOXALTHOLD,
BOXHEADFREE,
+ BOXCHIRP,
BOXPASSTHRU,
BOXFAILSAFE,
BOXPOSHOLD,
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 9d3512ca41..a1df79b426 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -87,7 +87,7 @@ typedef enum {
// GPS_HOME_MODE = (1 << 4),
POS_HOLD_MODE = (1 << 5),
HEADFREE_MODE = (1 << 6),
-// UNUSED_MODE = (1 << 7), // old autotune
+ CHIRP_MODE = (1 << 7), // old autotune
PASSTHRU_MODE = (1 << 8),
// RANGEFINDER_MODE= (1 << 9),
FAILSAFE_MODE = (1 << 10),
@@ -109,6 +109,7 @@ extern uint16_t flightModeFlags;
[BOXALTHOLD] = LOG2(ALT_HOLD_MODE), \
[BOXPOSHOLD] = LOG2(POS_HOLD_MODE), \
[BOXHEADFREE] = LOG2(HEADFREE_MODE), \
+ [BOXCHIRP] = LOG2(CHIRP_MODE), \
[BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \
[BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \
[BOXGPSRESCUE] = LOG2(GPS_RESCUE_MODE), \
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b0fa8fc43b..071c1feaf2 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -260,6 +260,14 @@ void resetPidProfile(pidProfile_t *pidProfile)
.tpa_speed_pitch_offset = 0,
.yaw_type = YAW_TYPE_RUDDER,
.angle_pitch_offset = 0,
+ .chirp_lag_freq_hz = 3,
+ .chirp_lead_freq_hz = 30,
+ .chirp_amplitude_roll = 230,
+ .chirp_amplitude_pitch = 230,
+ .chirp_amplitude_yaw = 180,
+ .chirp_frequency_start_deci_hz = 2,
+ .chirp_frequency_end_deci_hz = 6000,
+ .chirp_time_seconds = 20,
);
}
@@ -1200,9 +1208,49 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
disarmOnImpact();
}
+#ifdef USE_CHIRP
+
+ static int chirpAxis = 0;
+ static bool shouldChirpAxisToggle = false;
+
+ float chirp = 0.0f;
+ float sinarg = 0.0f;
+ if (FLIGHT_MODE(CHIRP_MODE)) {
+ shouldChirpAxisToggle = true; // advance chirp axis on next !CHIRP_MODE
+ // update chirp signal
+ if (chirpUpdate(&pidRuntime.chirp)) {
+ chirp = pidRuntime.chirp.exc;
+ sinarg = pidRuntime.chirp.sinarg;
+ }
+ } else {
+ if (shouldChirpAxisToggle) {
+ // toggle chirp signal logic and increment to next axis for next run
+ shouldChirpAxisToggle = false;
+ chirpAxis = (++chirpAxis > FD_YAW) ? 0 : chirpAxis;
+ // reset chirp signal generator
+ chirpReset(&pidRuntime.chirp);
+ }
+ }
+
+ // input / excitation shaping
+ float chirpFiltered = phaseCompApply(&pidRuntime.chirpFilter, chirp);
+
+ // ToDo: check if this can be reconstructed offline for rotating filter and if so, remove the debug
+ // fit (0...2*pi) into int16_t (-32768 to 32767)
+ DEBUG_SET(DEBUG_CHIRP, 0, lrintf(5.0e3f * sinarg));
+
+#endif // USE_CHIRP
+
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
+#ifdef USE_CHIRP
+ float currentChirp = 0.0f;
+ if(axis == chirpAxis){
+ currentChirp = pidRuntime.chirpAmplitude[axis] * chirpFiltered;
+ }
+#endif // USE_CHIRP
+
float currentPidSetpoint = getSetpointRate(axis);
if (pidRuntime.maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
@@ -1263,6 +1311,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
+#ifdef USE_CHIRP
+ currentPidSetpoint += currentChirp;
+#endif // USE_CHIRP
float errorRate = currentPidSetpoint - gyroRate; // r - y
#if defined(USE_ACC)
handleCrashRecovery(
@@ -1596,3 +1647,10 @@ float pidGetPidFrequency(void)
{
return pidRuntime.pidFrequency;
}
+
+#ifdef USE_CHIRP
+bool pidChirpIsFinished(void)
+{
+ return pidRuntime.chirp.isFinished;
+}
+#endif
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index f1d077bf6b..8c217dd03c 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -23,6 +23,7 @@
#include
#include "common/axis.h"
+#include "common/chirp.h"
#include "common/filter.h"
#include "common/pwl.h"
#include "common/time.h"
@@ -322,6 +323,15 @@ typedef struct pidProfile_s {
int16_t tpa_speed_pitch_offset; // For wings: pitch offset in degrees*10 for craft speed estimation
uint8_t yaw_type; // For wings: type of yaw (rudder or differential thrust)
int16_t angle_pitch_offset; // For wings: pitch offset for angle modes; in decidegrees; positive values tilting the wing down
+
+ uint8_t chirp_lag_freq_hz; // leadlag1Filter cutoff/pole to shape the excitation signal
+ uint8_t chirp_lead_freq_hz; // leadlag1Filter cutoff/zero
+ uint16_t chirp_amplitude_roll; // amplitude roll in degree/second
+ uint16_t chirp_amplitude_pitch; // amplitude pitch in degree/second
+ uint16_t chirp_amplitude_yaw; // amplitude yaw in degree/second
+ uint16_t chirp_frequency_start_deci_hz; // start frequency in units of 0.1 hz
+ uint16_t chirp_frequency_end_deci_hz; // end frequency in units of 0.1 hz
+ uint8_t chirp_time_seconds; // excitation time
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -529,6 +539,17 @@ typedef struct pidRuntime_s {
float tpaCurvePwl_yValues[TPA_CURVE_PWL_SIZE];
tpaCurveType_t tpaCurveType;
#endif // USE_ADVANCED_TPA
+
+#ifdef USE_CHIRP
+ chirp_t chirp;
+ phaseComp_t chirpFilter;
+ float chirpLagFreqHz;
+ float chirpLeadFreqHz;
+ float chirpAmplitude[3];
+ float chirpFrequencyStartHz;
+ float chirpFrequencyEndHz;
+ float chirpTimeSeconds;
+#endif // USE_CHIRP
} pidRuntime_t;
extern pidRuntime_t pidRuntime;
@@ -585,3 +606,6 @@ float pidGetDT();
float pidGetPidFrequency();
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);
+#ifdef USE_CHIRP
+bool pidChirpIsFinished();
+#endif
diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c
index e8ab4ab91f..d8dc0484a2 100644
--- a/src/main/flight/pid_init.c
+++ b/src/main/flight/pid_init.c
@@ -317,6 +317,14 @@ void pidInitFilters(const pidProfile_t *pidProfile)
pidRuntime.angleYawSetpoint = 0.0f;
#endif
+#ifdef USE_CHIRP
+ const float alpha = pidRuntime.chirpLeadFreqHz / pidRuntime.chirpLagFreqHz;
+ const float centerFreqHz = pidRuntime.chirpLagFreqHz * sqrtf(alpha);
+ const float centerPhaseDeg = asinf( (1.0f - alpha) / (1.0f + alpha) ) / RAD;
+ phaseCompInit(&pidRuntime.chirpFilter, centerFreqHz, centerPhaseDeg, targetPidLooptime);
+ chirpInit(&pidRuntime.chirp, pidRuntime.chirpFrequencyStartHz, pidRuntime.chirpFrequencyEndHz, pidRuntime.chirpTimeSeconds, targetPidLooptime);
+#endif
+
pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
#ifdef USE_WING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -411,6 +419,17 @@ void pidInitConfig(const pidProfile_t *pidProfile)
pidRuntime.horizonDelayMs = pidProfile->horizon_delay_ms;
#endif
+#ifdef USE_CHIRP
+ pidRuntime.chirpLagFreqHz = pidProfile->chirp_lag_freq_hz;
+ pidRuntime.chirpLeadFreqHz = pidProfile->chirp_lead_freq_hz;
+ pidRuntime.chirpAmplitude[FD_ROLL] = pidProfile->chirp_amplitude_roll;
+ pidRuntime.chirpAmplitude[FD_PITCH] = pidProfile->chirp_amplitude_pitch;
+ pidRuntime.chirpAmplitude[FD_YAW]= pidProfile->chirp_amplitude_yaw;
+ pidRuntime.chirpFrequencyStartHz = pidProfile->chirp_frequency_start_deci_hz / 10.0f;
+ pidRuntime.chirpFrequencyEndHz = pidProfile->chirp_frequency_end_deci_hz / 10.0f;
+ pidRuntime.chirpTimeSeconds = pidProfile->chirp_time_seconds;
+#endif
+
pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.antiGravityGain = pidProfile->anti_gravity_gain;
diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c
index 7477a5337d..e816db02d9 100644
--- a/src/main/msp/msp_box.c
+++ b/src/main/msp/msp_box.c
@@ -101,6 +101,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 52},
{ .boxId = BOXREADY, .boxName = "READY", .permanentId = 53},
{ .boxId = BOXLAPTIMERRESET, .boxName = "LAP TIMER RESET", .permanentId = 54},
+ { .boxId = BOXCHIRP, .boxName = "CHIRP", .permanentId = 55}
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -365,6 +366,10 @@ void initActiveBoxIds(void)
BME(BOXLAPTIMERRESET);
#endif
+#if defined(USE_CHIRP)
+ BME(BOXCHIRP);
+#endif
+
#undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 1edd59843b..43855697a1 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -1084,6 +1084,11 @@ static void osdElementFlymode(osdElementParms_t *element)
strcpy(element->buff, "HOR ");
} else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
strcpy(element->buff, "ATRN");
+#ifdef USE_CHIRP
+ // the additional check for pidChirpIsFinished() is to have visual feedback for user that don't have warnings enabled in their googles
+ } else if (FLIGHT_MODE(CHIRP_MODE) && !pidChirpIsFinished()) {
+#endif
+ strcpy(element->buff, "CHIR");
} else if (isAirmodeEnabled()) {
strcpy(element->buff, "AIR ");
} else {
diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c
index bf6b8e1c07..3c983946b0 100644
--- a/src/main/osd/osd_warnings.c
+++ b/src/main/osd/osd_warnings.c
@@ -432,6 +432,16 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
return;
}
+#ifdef USE_CHIRP
+ // Visual info that chirp excitation is finished
+ if (pidChirpIsFinished()) {
+ tfp_sprintf(warningText, "CHIRP EXC FINISHED");
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
+ *blinking = true;
+ return;
+ }
+#endif // USE_CHIRP
+
}
#endif // USE_OSD
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 68d654831b..48140efae8 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -463,6 +463,8 @@ void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "ALTH";
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";
+ } else if (FLIGHT_MODE(CHIRP_MODE)) {
+ flightMode = "CHIR";
} else if (isAirmodeEnabled()) {
flightMode = "AIR";
}