aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: add protection for new tradheli parameters

Browse Source
This commit is contained in:
Bill Geyer 2021-10-25 22:25:59 -04:00 committed by Bill Geyer
parent 1a50dce206
commit d02097bf1c
5 changed files with 53 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -493,6 +493,22 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const
return false;
}
// returns false if _collective_min_deg is not default value which indicates users set parameter
if (is_equal((float)_collective_min_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MIN_DEG)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_MIN_DEG to actual min blade pitch in deg");
}
return false;
}
// returns false if _collective_max_deg is not default value which indicates users set parameter
if (is_equal((float)_collective_max_deg, (float)AP_MOTORS_HELI_COLLECTIVE_MAX_DEG)) {
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Set H_COL_MAX_DEG to actual max blade pitch in deg");
}
return false;
}
// all other cases parameters are OK
return true;
}

8
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -22,8 +22,8 @@
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN 0.3f // minimum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -10.0f // minimum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 10.0f // maximum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -90.0f // minimum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 90.0f // maximum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN -2.0f // minimum landed collective blade pitch angle in deg for modes using althold
@ -267,8 +267,8 @@ protected:
AP_Float _collective_min_deg; // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN)
// internal variables
float _collective_zero_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
float _collective_land_min_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
float _collective_zero_pct; // collective mid parameter value converted to 0 ~ 1 range
float _collective_land_min_pct; // collective mid parameter value converted to 0 ~ 1 range
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
motor_frame_type _frame_type;

18
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -377,14 +377,20 @@ void AP_MotorsHeli_Dual::calculate_scalars()
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
_collective2_zero_pct = _collective_zero_pct;
_collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
} else {
_collective_zero_pct = 0.0f;
_collective_land_min_pct = 0.0f;
}
_collective2_zero_pct = _collective_zero_pct;
// configure swashplate 1 and update scalars
_swashplate1.configure();

15
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -149,13 +149,18 @@ void AP_MotorsHeli_Quad::calculate_scalars()
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
_collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
} else {
_collective_zero_pct = 0.0f;
_collective_land_min_pct = 0.0f;
}
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();

16
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -324,15 +324,21 @@ void AP_MotorsHeli_Single::calculate_scalars()
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
}
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
_collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
} else {
_collective_zero_pct = 0.0f;
_collective_land_min_pct = 0.0f;
}
// configure swashplate and update scalars
_swashplate.configure();