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

tradheli- make RPM sensor input to harmonic notch in hertz

ArduCopter/Copter.cpp

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

ArduCopter/Copter.h

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

ArduCopter/Log.cpp

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

ArduCopter/system.cpp

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