From 5bb4729d61de6457d8f81f998a03fa9936bf2bc1 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Mon, 9 Aug 2021 12:21:52 +0100 Subject: [PATCH] AP_RPM: Restructure params to be per instance of _params --- libraries/AP_RPM/AP_RPM.cpp | 175 +++++++++++++------------ libraries/AP_RPM/AP_RPM.h | 13 +- libraries/AP_RPM/AP_RPM_Params.cpp | 75 +++++++++++ libraries/AP_RPM/AP_RPM_Params.h | 35 +++++ libraries/AP_RPM/RPM_Backend.h | 2 +- libraries/AP_RPM/RPM_EFI.cpp | 2 +- libraries/AP_RPM/RPM_ESC_Telem.cpp | 4 +- libraries/AP_RPM/RPM_HarmonicNotch.cpp | 2 +- libraries/AP_RPM/RPM_Pin.cpp | 6 +- libraries/AP_RPM/RPM_SITL.cpp | 2 +- 10 files changed, 216 insertions(+), 100 deletions(-) create mode 100644 libraries/AP_RPM/AP_RPM_Params.cpp create mode 100644 libraries/AP_RPM/AP_RPM_Params.h diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index f7cdcdc7ab..60a0594a65 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -24,83 +24,16 @@ extern const AP_HAL::HAL& hal; // table of user settable parameters const AP_Param::GroupInfo AP_RPM::var_info[] = { - // @Param: _TYPE - // @DisplayName: RPM type - // @Description: What type of RPM sensor is connected - // @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask - // @User: Standard - AP_GROUPINFO("_TYPE", 0, AP_RPM, _type[0], 0), + // 0-13 used by old param indexes before being moved to AP_RPM_Params - // @Param: _SCALING - // @DisplayName: RPM scaling - // @Description: Scaling factor between sensor reading and RPM. - // @Increment: 0.001 - // @User: Standard - AP_GROUPINFO("_SCALING", 1, AP_RPM, _scaling[0], 1.0f), - - // @Param: _MAX - // @DisplayName: Maximum RPM - // @Description: Maximum RPM to report - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_MAX", 2, AP_RPM, _maximum[0], 100000), - - // @Param: _MIN - // @DisplayName: Minimum RPM - // @Description: Minimum RPM to report - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("_MIN", 3, AP_RPM, _minimum[0], 10), - - // @Param: _MIN_QUAL - // @DisplayName: Minimum Quality - // @Description: Minimum data quality to be used - // @Increment: 0.1 - // @User: Advanced - AP_GROUPINFO("_MIN_QUAL", 4, AP_RPM, _quality_min[0], 0.5), - - // @Param: _PIN - // @DisplayName: Input pin number - // @Description: Which pin to use - // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 - // @User: Standard - AP_GROUPINFO("_PIN", 5, AP_RPM, _pin[0], -1), - - // @Param: _ESC_MASK - // @DisplayName: Bitmask of ESC telemetry channels to average - // @Description: Mask of channels which support ESC rpm telemetry. RPM telemetry of the selected channels will be averaged - // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 - // @User: Advanced - AP_GROUPINFO("_ESC_MASK", 6, AP_RPM, _esc_mask[0], 0), + // @Group: 1_ + // @Path: AP_RPM_Params.cpp + AP_SUBGROUPINFO(_params[0], "1_", 14, AP_RPM, AP_RPM_Params), #if RPM_MAX_INSTANCES > 1 - // @Param: 2_TYPE - // @DisplayName: Second RPM type - // @Description: What type of RPM sensor is connected - // @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask - // @User: Advanced - AP_GROUPINFO("2_TYPE", 10, AP_RPM, _type[1], 0), - - // @Param: 2_SCALING - // @DisplayName: RPM scaling - // @Description: Scaling factor between sensor reading and RPM. - // @Increment: 0.001 - // @User: Advanced - AP_GROUPINFO("2_SCALING", 11, AP_RPM, _scaling[1], 1.0f), - - // @Param: 2_PIN - // @DisplayName: RPM2 input pin number - // @Description: Which pin to use - // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 - // @User: Standard - AP_GROUPINFO("2_PIN", 12, AP_RPM, _pin[1], -1), - - // @Param: 2_ESC_MASK - // @DisplayName: Bitmask of ESC telemetry channels to average - // @Description: Mask of channels which support ESC rpm telemetry. RPM telemetry of the selected channels will be averaged - // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 - // @User: Advanced - AP_GROUPINFO("2_ESC_MASK", 13, AP_RPM, _esc_mask[1], 0), + // @Group: 2_ + // @Path: AP_RPM_Params.cpp + AP_SUBGROUPINFO(_params[1], "2_", 15, AP_RPM, AP_RPM_Params), #endif AP_GROUPEND @@ -125,8 +58,11 @@ void AP_RPM::init(void) // init called a 2nd time? return; } + + convert_params(); + for (uint8_t i=0; isnprintf(param_name, sizeof(param_name), "RPM%X_%s", param_instance, AP_RPM_Params::var_info[destination_index].name); + param_name[sizeof(param_name)-1] = '\0'; + + AP_Param::convert_old_parameter(&info, 1.0f, 0); + } + + // force _params[0].type into storage to flag that conversion has been done + _params[0].type.save(true); +} + /* update RPM state for all instances. This should be called by main loop */ @@ -168,7 +177,7 @@ void AP_RPM::update(void) { for (uint8_t i=0; i= num_instances || _type[instance] == RPM_TYPE_NONE) { + if (instance >= num_instances || _params[instance].type == RPM_TYPE_NONE) { return false; } // check that data quality is above minimum required - if (state[instance].signal_quality < _quality_min[0]) { + if (state[instance].signal_quality < _params[instance].quality_min) { return false; } @@ -205,7 +214,7 @@ bool AP_RPM::enabled(uint8_t instance) const return false; } // if no sensor type is selected, the sensor is not activated. - if (_type[instance] == RPM_TYPE_NONE) { + if (_params[instance].type == RPM_TYPE_NONE) { return false; } return true; @@ -227,15 +236,15 @@ bool AP_RPM::get_rpm(uint8_t instance, float &rpm_value) const bool AP_RPM::arming_checks(size_t buflen, char *buffer) const { for (uint8_t i=0; isnprintf(buffer, buflen, "RPM[%u] no pin set", i + 1); return false; } - if (!hal.gpio->valid_pin(_pin[i])) { - hal.util->snprintf(buffer, buflen, "RPM[%u] pin %d invalid", unsigned(i + 1), int(_pin[i].get())); + if (!hal.gpio->valid_pin(_params[i].pin)) { + hal.util->snprintf(buffer, buflen, "RPM[%u] pin %d invalid", unsigned(i + 1), int(_params[i].pin.get())); return false; } break; diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index ad02878200..5d034b8a62 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -18,6 +18,7 @@ #include #include #include +#include "AP_RPM_Params.h" // Maximum number of RPM measurement instances available on this platform #define RPM_MAX_INSTANCES 2 @@ -57,13 +58,7 @@ public: }; // parameters for each instance - AP_Int8 _type[RPM_MAX_INSTANCES]; - AP_Int8 _pin[RPM_MAX_INSTANCES]; - AP_Float _scaling[RPM_MAX_INSTANCES]; - AP_Float _maximum[RPM_MAX_INSTANCES]; - AP_Float _minimum[RPM_MAX_INSTANCES]; - AP_Float _quality_min[RPM_MAX_INSTANCES]; - AP_Int32 _esc_mask[RPM_MAX_INSTANCES]; + AP_RPM_Params _params[RPM_MAX_INSTANCES]; static const struct AP_Param::GroupInfo var_info[]; @@ -100,11 +95,13 @@ public: bool arming_checks(size_t buflen, char *buffer) const; private: + void convert_params(void); + static AP_RPM *_singleton; RPM_State state[RPM_MAX_INSTANCES]; AP_RPM_Backend *drivers[RPM_MAX_INSTANCES]; - uint8_t num_instances:2; + uint8_t num_instances; void detect_instance(uint8_t instance); }; diff --git a/libraries/AP_RPM/AP_RPM_Params.cpp b/libraries/AP_RPM/AP_RPM_Params.cpp new file mode 100644 index 0000000000..bed2b7a03b --- /dev/null +++ b/libraries/AP_RPM/AP_RPM_Params.cpp @@ -0,0 +1,75 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include + +// table of user settable parameters +const AP_Param::GroupInfo AP_RPM_Params::var_info[] = { + // @Param: TYPE + // @DisplayName: RPM type + // @Description: What type of RPM sensor is connected + // @Values: 0:None,1:PWM,2:AUXPIN,3:EFI,4:Harmonic Notch,5:ESC Telemetry Motors Bitmask + // @User: Standard + AP_GROUPINFO_FLAGS("TYPE", 1, AP_RPM_Params, type, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: SCALING + // @DisplayName: RPM scaling + // @Description: Scaling factor between sensor reading and RPM. + // @Increment: 0.001 + // @User: Standard + AP_GROUPINFO("SCALING", 2, AP_RPM_Params, scaling, 1.0f), + + // @Param: MAX + // @DisplayName: Maximum RPM + // @Description: Maximum RPM to report. Only used on type = PWM. + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("MAX", 3, AP_RPM_Params, maximum, 100000), + + // @Param: MIN + // @DisplayName: Minimum RPM + // @Description: Minimum RPM to report. Only used on type = PWM. + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("MIN", 4, AP_RPM_Params, minimum, 10), + + // @Param: MIN_QUAL + // @DisplayName: Minimum Quality + // @Description: Minimum data quality to be used + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("MIN_QUAL", 5, AP_RPM_Params, quality_min, 0.5), + + // @Param: PIN + // @DisplayName: Input pin number + // @Description: Which pin to use. Only used on type = PWM. + // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 + // @User: Standard + AP_GROUPINFO("PIN", 6, AP_RPM_Params, pin, -1), + + // @Param: ESC_MASK + // @DisplayName: Bitmask of ESC telemetry channels to average + // @Description: Mask of channels which support ESC rpm telemetry. RPM telemetry of the selected channels will be averaged + // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 + // @User: Advanced + AP_GROUPINFO("ESC_MASK", 7, AP_RPM_Params, esc_mask, 0), + + AP_GROUPEND +}; + +AP_RPM_Params::AP_RPM_Params(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_RPM/AP_RPM_Params.h b/libraries/AP_RPM/AP_RPM_Params.h new file mode 100644 index 0000000000..c6f5d77adb --- /dev/null +++ b/libraries/AP_RPM/AP_RPM_Params.h @@ -0,0 +1,35 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once +#include + +class AP_RPM_Params { + +public: + // Constructor + AP_RPM_Params(void); + + // parameters for each instance + AP_Int8 type; + AP_Int8 pin; + AP_Float scaling; + AP_Float maximum; + AP_Float minimum; + AP_Float quality_min; + AP_Int32 esc_mask; + + static const struct AP_Param::GroupInfo var_info[]; + +}; diff --git a/libraries/AP_RPM/RPM_Backend.h b/libraries/AP_RPM/RPM_Backend.h index fee8d9719b..3aeef5471b 100644 --- a/libraries/AP_RPM/RPM_Backend.h +++ b/libraries/AP_RPM/RPM_Backend.h @@ -35,7 +35,7 @@ public: if (state.instance >= RPM_MAX_INSTANCES) { return -1; } - return ap_rpm._pin[state.instance].get(); + return ap_rpm._params[state.instance].pin.get(); } protected: diff --git a/libraries/AP_RPM/RPM_EFI.cpp b/libraries/AP_RPM/RPM_EFI.cpp index c91fcf5560..fcf04cb808 100644 --- a/libraries/AP_RPM/RPM_EFI.cpp +++ b/libraries/AP_RPM/RPM_EFI.cpp @@ -36,7 +36,7 @@ void AP_RPM_EFI::update(void) return; } state.rate_rpm = efi->get_rpm(); - state.rate_rpm *= ap_rpm._scaling[state.instance]; + state.rate_rpm *= ap_rpm._params[state.instance].scaling; state.signal_quality = 0.5f; state.last_reading_ms = AP_HAL::millis(); } diff --git a/libraries/AP_RPM/RPM_ESC_Telem.cpp b/libraries/AP_RPM/RPM_ESC_Telem.cpp index e871e6de17..03885df3f4 100644 --- a/libraries/AP_RPM/RPM_ESC_Telem.cpp +++ b/libraries/AP_RPM/RPM_ESC_Telem.cpp @@ -34,8 +34,8 @@ void AP_RPM_ESC_Telem::update(void) { #if HAL_WITH_ESC_TELEM AP_ESC_Telem &esc_telem = AP::esc_telem(); - float esc_rpm = esc_telem.get_average_motor_rpm(ap_rpm._esc_mask[state.instance]); - state.rate_rpm = esc_rpm * ap_rpm._scaling[state.instance]; + float esc_rpm = esc_telem.get_average_motor_rpm(ap_rpm._params[state.instance].esc_mask); + state.rate_rpm = esc_rpm * ap_rpm._params[state.instance].scaling; state.signal_quality = 0.5f; state.last_reading_ms = AP_HAL::millis(); #endif diff --git a/libraries/AP_RPM/RPM_HarmonicNotch.cpp b/libraries/AP_RPM/RPM_HarmonicNotch.cpp index 7c64c922c9..883c45eb57 100644 --- a/libraries/AP_RPM/RPM_HarmonicNotch.cpp +++ b/libraries/AP_RPM/RPM_HarmonicNotch.cpp @@ -34,7 +34,7 @@ void AP_RPM_HarmonicNotch::update(void) AP_InertialSensor& ins = AP::ins(); if (ins.get_gyro_harmonic_notch_tracking_mode() != HarmonicNotchDynamicMode::Fixed) { state.rate_rpm = ins.get_gyro_dynamic_notch_center_freq_hz() * 60.0f; - state.rate_rpm *= ap_rpm._scaling[state.instance]; + state.rate_rpm *= ap_rpm._params[state.instance].scaling; state.signal_quality = 0.5f; state.last_reading_ms = AP_HAL::millis(); } diff --git a/libraries/AP_RPM/RPM_Pin.cpp b/libraries/AP_RPM/RPM_Pin.cpp index 79b40a9e64..78f9bf4015 100644 --- a/libraries/AP_RPM/RPM_Pin.cpp +++ b/libraries/AP_RPM/RPM_Pin.cpp @@ -80,9 +80,9 @@ void AP_RPM_Pin::update(void) irq_state[state.instance].dt_sum = 0; hal.scheduler->restore_interrupts(irqstate); - const float scaling = ap_rpm._scaling[state.instance]; - float maximum = ap_rpm._maximum[state.instance]; - float minimum = ap_rpm._minimum[state.instance]; + const float scaling = ap_rpm._params[state.instance].scaling; + float maximum = ap_rpm._params[state.instance].maximum; + float minimum = ap_rpm._params[state.instance].minimum; float quality = 0; float rpm = scaling * (1.0e6 / dt_avg) * 60; float filter_value = signal_quality_filter.get(); diff --git a/libraries/AP_RPM/RPM_SITL.cpp b/libraries/AP_RPM/RPM_SITL.cpp index 7800962021..7df2fd03ec 100644 --- a/libraries/AP_RPM/RPM_SITL.cpp +++ b/libraries/AP_RPM/RPM_SITL.cpp @@ -40,7 +40,7 @@ void AP_RPM_SITL::update(void) } else { state.rate_rpm = sitl->state.rpm[1]; } - state.rate_rpm *= ap_rpm._scaling[state.instance]; + state.rate_rpm *= ap_rpm._params[state.instance].scaling; state.signal_quality = 0.5f; state.last_reading_ms = AP_HAL::millis();