aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: add functions to update dynamic notch frequency. Periodically log the frequency.

Browse Source 
tradheli- make RPM sensor input to harmonic notch in hertz
This commit is contained in:
Andy Piper 2019-07-10 22:52:38 +02:00 committed by Randy Mackay
parent 4a1a5e81ea
commit 8ce7f02167
4 changed files with 33 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.cpp
View File

@ -307,6 +307,8 @@ void Copter::throttle_loop()
// compensate for ground effect (if enabled)
update_ground_effect_detector();
update_dynamic_notch();
}
// update_batt_compass - read battery and compass

1
ArduCopter/Copter.h
View File

@ -835,6 +835,7 @@ private:
// system.cpp
void init_ardupilot();
void startup_INS_ground();
void update_dynamic_notch();
bool position_ok() const;
bool ekf_position_ok() const;
bool optflow_position_ok() const;

6
ArduCopter/Log.cpp
View File

@ -20,6 +20,7 @@ struct PACKED log_Control_Tuning {
float terr_alt;
int16_t target_climb_rate;
int16_t climb_rate;
float dynamic_notch_freq;
};
// Write a control tuning packet
@ -60,7 +61,8 @@ void Copter::Log_Write_Control_Tuning()
rangefinder_alt : rangefinder_alt,
terr_alt : terr_alt,
target_climb_rate : target_climb_rate_cms,
climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
climb_rate : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t
dynamic_notch_freq : ins.get_gyro_dynamic_notch_center_freq_hz()
};
logger.WriteBlock(&pkt, sizeof(pkt));
}
@ -393,7 +395,7 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
"PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" },
"CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B0BBBB-" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),

26
ArduCopter/system.cpp
View File

@ -283,6 +283,32 @@ void Copter::startup_INS_ground()
ahrs.reset();
}
// update the harmonic notch filter center frequency dynamically
void Copter::update_dynamic_notch()
{
const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz();
const float ref = ins.get_gyro_harmonic_notch_reference();
if (is_zero(ref)) {
ins.update_harmonic_notch_freq_hz(ref_freq);
return;
}
if (is_tradheli()) {
#if RPM_ENABLED == ENABLED
if (rpm_sensor.healthy(0)) {
// set the harmonic notch filter frequency from the main rotor rpm
ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f));
} else {
ins.update_harmonic_notch_freq_hz(ref_freq);
}
#endif
} else {
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)));
}
}
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Copter::position_ok() const
{