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

AP_RPM: Restructure params to be per instance of _params

This commit is contained in:
Gone4Dirt 2021-08-09 12:21:52 +01:00 committed by Andrew Tridgell
parent 58e14b2dc3
commit 5bb4729d61
10 changed files with 216 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
libraries/AP_RPM/AP_RPM.cpp
View File

@ -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; i<RPM_MAX_INSTANCES; i++) {
switch (_type[i]) {
switch (_params[i].type) {
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
case RPM_TYPE_PWM:
case RPM_TYPE_PIN:
@ -161,6 +97,79 @@ void AP_RPM::init(void)
}
}
/*
PARAMETER_CONVERSION - Added: Aug-2021
*/
void AP_RPM::convert_params(void)
{
if (_params[0].type.configured_in_storage()) {
// _params[0].type will always be configured in storage after conversion is done the first time
return;
}
// don't do conversion if neither RPM types were set
bool type_set;
uint8_t rpm_type = 0;
uint8_t rpm2_type = 0;
type_set = AP_Param::get_param_by_index(this, 0, AP_PARAM_INT8, &rpm_type);
type_set |= AP_Param::get_param_by_index(this, 10, AP_PARAM_INT8, &rpm2_type);
if (!type_set || (rpm_type == 0 && rpm2_type == 0)) {
return;
}
struct ConversionTable {
uint8_t old_element;
uint8_t new_index;
uint8_t instance;
};
const struct ConversionTable conversionTable[] = {
// RPM 1
{0, 0, 0}, // TYPE
{1, 1, 0}, // SCALING
{2, 2, 0}, // MAX
{3, 3, 0}, // MIN
{4, 4, 0}, // MIN_QUAL
{5, 5, 0}, // PIN
{6, 6, 0}, // ESC_MASK
// RPM 2
{10, 0, 1}, // TYPE
{11, 1, 1}, // SCALING
// MAX (Previous bug meant RPM2_MAX param was never accesible to users. No conversion required.)
// MIN (Previous bug meant RPM2_MIN param was never accesible to users. No conversion required.)
{4, 4, 1}, // MIN_QUAL (Previously the min quality of the 1st RPM instance was used for all RPM instances.)
{12, 5, 1}, // PIN
{13, 6, 1}, // ESC_MASK
};
char param_name[17] = {0};
AP_Param::ConversionInfo info;
info.new_name = param_name;
if (!AP_Param::find_top_level_key_by_pointer(this, info.old_key)) {
_params[0].type.save(true);
return; // no conversion is supported on this platform
}
for (uint8_t i = 0; i < ARRAY_SIZE(conversionTable); i++) {
uint8_t param_instance = conversionTable[i].instance + 1;
uint8_t destination_index = conversionTable[i].new_index;
info.old_group_element = conversionTable[i].old_element;
// The var type of the params has not changed in the conversion so this is ok:
info.type = (ap_var_type)AP_RPM_Params::var_info[destination_index].type;
hal.util->snprintf(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; i++) {
if (drivers[i] != nullptr) {
if (_type[i] == RPM_TYPE_NONE) {
if (_params[i].type == RPM_TYPE_NONE) {
// allow user to disable an RPM sensor at runtime and force it to re-learn the quality if re-enabled.
state[i].signal_quality = 0;
continue;
@ -184,12 +193,12 @@ void AP_RPM::update(void)
*/
bool AP_RPM::healthy(uint8_t instance) const
{
if (instance >= 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; i<RPM_MAX_INSTANCES; i++) {
switch (_type[i]) {
switch (_params[i].type) {
case RPM_TYPE_PWM:
case RPM_TYPE_PIN:
if (_pin[i] == -1) {
if (_params[i].pin == -1) {
hal.util->snprintf(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;

13
libraries/AP_RPM/AP_RPM.h
View File

@ -18,6 +18,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#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);
};

75
libraries/AP_RPM/AP_RPM_Params.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_RPM/AP_RPM_Params.h>
// 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);
}

35
libraries/AP_RPM/AP_RPM_Params.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Param/AP_Param.h>
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[];
};

2
libraries/AP_RPM/RPM_Backend.h
View File

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

2
libraries/AP_RPM/RPM_EFI.cpp
View File

@ -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();
}

4
libraries/AP_RPM/RPM_ESC_Telem.cpp
View File

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

2
libraries/AP_RPM/RPM_HarmonicNotch.cpp
View File

@ -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();
}

6
libraries/AP_RPM/RPM_Pin.cpp
View File

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

2
libraries/AP_RPM/RPM_SITL.cpp
View File

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