Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Motors: add arming check for VOLT_MIN and VOLT_MAX #24228

Closed
wants to merge 2 commits into from
Closed

AP_Motors: add arming check for VOLT_MIN and VOLT_MAX #24228

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e189381
AP_Motors: add arming check for VOLT_MIN and VOLT_MAX
IamPete1 Jul 5, 2023
b469514
Tools: autotest: defualt_params: configure BAT_VOLT MIN and MAX for q…
IamPete1 Jul 5, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions Tools/autotest/default_params/quadplane-tilthvec.parm
View file
Open in desktop
Expand Up @@ -20,3 +20,5 @@ Q_A_RAT_RLL_P 0.15
Q_A_RAT_PIT_P 0.15
Q_A_RAT_RLL_D 0.002
Q_A_RAT_PIT_D 0.002
Q_M_BAT_VOLT_MAX 12.8
Q_M_BAT_VOLT_MIN 9.6
11 changes: 6 additions & 5 deletions libraries/AP_Motors/AP_MotorsMulticopter.cpp
View file
Open in desktop
Expand Up @@ -183,7 +183,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {

// @Param: BAT_IDX
// @DisplayName: Battery compensation index
// @Description: Which battery monitor should be used for doing compensation
// @Description: Which battery monitor should be used for doing compensation. Set to -1 to disable pre-arm warning.
// @Values: 0:First battery, 1:Second battery
// @User: Advanced
AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, thr_lin.batt_idx, 0),
Expand Down Expand Up @@ -765,10 +765,6 @@ bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const
}

// Check param config
if (thr_lin.get_spin_min() > 0.3) {
hal.util->snprintf(buffer, buflen, "%sSPIN_MIN too high %.2f > 0.3", AP_MOTORS_PARAM_PREFIX, thr_lin.get_spin_min());
return false;
}
if (_spin_arm > thr_lin.get_spin_min()) {
hal.util->snprintf(buffer, buflen, "%sSPIN_ARM > %sSPIN_MIN", AP_MOTORS_PARAM_PREFIX, AP_MOTORS_PARAM_PREFIX);
return false;
Expand All @@ -778,6 +774,11 @@ bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const
return false;
}

// Check Linearization setup
if (!thr_lin.arming_checks(buflen, buffer)) {
return false;
}

return true;
}

Expand Down
30 changes: 29 additions & 1 deletion libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp
View file
Open in desktop
Expand Up @@ -30,6 +30,12 @@
#define THRST_LIN_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect)
#endif

#include <AP_Vehicle/AP_Vehicle_Type.h>
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define AP_MOTORS_PARAM_PREFIX "Q_M_"
#else
#define AP_MOTORS_PARAM_PREFIX "MOT_"
#endif

extern const AP_HAL::HAL& hal;

Expand Down Expand Up @@ -154,7 +160,7 @@ void Thrust_Linearization::update_lift_max_from_batt_voltage()
// if disabled or misconfigured exit immediately
float _batt_voltage = motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE) ? AP::battery().voltage(batt_idx) : AP::battery().voltage_resting_estimate(batt_idx);

if ((batt_voltage_max <= 0) || (batt_voltage_min >= batt_voltage_max) || (_batt_voltage < 0.25 * batt_voltage_min)) {
if (!battery_voltage_config_valid() || (_batt_voltage < 0.25 * batt_voltage_min)) {
batt_voltage_filt.reset(1.0);
lift_max = 1.0;
return;
Expand Down Expand Up @@ -197,3 +203,25 @@ float Thrust_Linearization::get_compensation_gain() const
#endif
return ret;
}

// Check the voltage scale configuration
bool Thrust_Linearization::battery_voltage_config_valid() const
{
return (batt_voltage_max > 0) && (batt_voltage_min < batt_voltage_max);
}

bool Thrust_Linearization::arming_checks(size_t buflen, char *buffer) const
{
if (get_spin_min() > 0.3) {
hal.util->snprintf(buffer, buflen, "%sSPIN_MIN too high %.2f > 0.3", AP_MOTORS_PARAM_PREFIX, get_spin_min());
return false;
}

if (is_positive(AP::battery().voltage(batt_idx)) && !battery_voltage_config_valid()) {
// If there is a valid voltage reading battery voltage scaling should be setup
hal.util->snprintf(buffer, buflen, "Set %sBAT_VOLT_MIN and %sBAT_VOLT_MAX", AP_MOTORS_PARAM_PREFIX, AP_MOTORS_PARAM_PREFIX);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if the compiler is smart enough to

Suggested change
hal.util->snprintf(buffer, buflen, "Set %sBAT_VOLT_MIN and %sBAT_VOLT_MAX", AP_MOTORS_PARAM_PREFIX, AP_MOTORS_PARAM_PREFIX);
hal.util->snprintf(buffer, buflen, "Set " AP_MOTORS_PARAM_PREFIX "BAT_VOLT_MIN and " AP_MOTORS_PARAM_PREFIX "BAT_VOLT_MAX");

return false;
}

return true;
}
7 changes: 7 additions & 0 deletions libraries/AP_Motors/AP_Motors_Thrust_Linearization.h
View file
Open in desktop
Expand Up @@ -44,6 +44,9 @@ friend class AP_MotorsHeli_Single;
// Get lift max
float get_lift_max() const { return lift_max; }

// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const;

// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

Expand All @@ -56,6 +59,10 @@ friend class AP_MotorsHeli_Single;
AP_Float batt_voltage_min; // minimum voltage used to scale lift

private:

// Check the voltage scale configuration
bool battery_voltage_config_valid() const;

float lift_max; // maximum lift ratio from battery voltage
float throttle_limit; // ratio of throttle limit between hover and maximum
LowPassFilterFloat batt_voltage_filt; // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
Expand Down