Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: support harmonic notch on 2nd RPM sensor

This commit is contained in:
Andrew Tridgell 2022-04-13 13:52:42 +10:00
parent 7272a29b7b
commit 13107dfa6d
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/system.cpp
View File

@ -256,14 +256,17 @@ void Copter::update_dynamic_notch(uint8_t idx)
#if RPM_ENABLED == ENABLED
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 / 60.0f));
ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1.0/60)));
} else {
ins.update_harmonic_notch_freq_hz(idx, ref_freq);
}
break;
}
#endif
#if HAL_WITH_ESC_TELEM
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking