Browse Source

Plane: param conversion for INS_NOTCH to INS_HNTC2

apm_2208
Andrew Tridgell 3 years ago
parent
commit
627f62d23b
  1. 18
      ArduPlane/Parameters.cpp

18
ArduPlane/Parameters.cpp

@ -1502,5 +1502,23 @@ void Plane::load_parameters(void) @@ -1502,5 +1502,23 @@ void Plane::load_parameters(void)
}
#endif
if (!ins.gyro_notch_enabled()) {
// notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch
const AP_Param::ConversionInfo notchfilt_conversion_info[] {
{ Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" },
{ Parameters::k_param_ins, 293, AP_PARAM_FLOAT, "INS_HNTC2_ATT" },
{ Parameters::k_param_ins, 357, AP_PARAM_FLOAT, "INS_HNTC2_FREQ" },
{ Parameters::k_param_ins, 421, AP_PARAM_FLOAT, "INS_HNTC2_BW" },
};
uint8_t notchfilt_table_size = ARRAY_SIZE(notchfilt_conversion_info);
for (uint8_t i=0; i<notchfilt_table_size; i++) {
AP_Param::convert_old_parameters(&notchfilt_conversion_info[i], 1.0f);
}
if (ins.gyro_notch_enabled()) {
AP_Param::set_default_by_name("INS_HNTC2_MODE", 0);
AP_Param::set_default_by_name("INS_HNTC2_HMNCS", 1);
}
}
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

Loading…
Cancel
Save