Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Chirp signal generator as flight mode (#13105)

Browse source
This commit is contained in:
pichim 2025-01-21 11:22:35 +01:00 • committed by GitHub
parent cfb4f8fe08
commit 899ce6731d
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
18 changed files with 301 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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