Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: support harmonic notch on 2nd RPM sensor

Browse Source
This commit is contained in:
Andrew Tridgell 2022-04-13 13:52:42 +10:00 committed by Randy Mackay
parent 81ed8598ff
commit fd00fef01a
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/system.cpp
View File

@ -471,14 +471,17 @@ void Plane::update_dynamic_notch(uint8_t idx)
break;
case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
case HarmonicNotchDynamicMode::UpdateRPM2: {
uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1);
float rpm;
if (rpm_sensor.get_rpm(0, rpm)) {
if (rpm_sensor.get_rpm(sensor, rpm)) {
// set the harmonic notch filter frequency from the main rotor rpm
ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1/60.0)));
} else {
ins.update_harmonic_notch_freq_hz(idx, ref_freq);
}
break;
}
#if HAL_WITH_ESC_TELEM
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
// set the harmonic notch filter frequency scaled on measured frequency