Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add min airspeed arming check and constrain for speed scailing

Browse Source
This commit is contained in:
Iampete1 2022-09-05 00:10:01 +01:00 committed by Andrew Tridgell
parent d31f6c2f57
commit 6358a3c962
3 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/AP_Arming.cpp
View File

@ -78,6 +78,11 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false; ret = false;
} }
if (plane.aparm.airspeed_min < MIN_AIRSPEED_MIN) {
check_failed(display_failure, "ARSPD_FBW_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN);
ret = false;
}
if (plane.channel_throttle->get_reverse() && if (plane.channel_throttle->get_reverse() &&
Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled && Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled &&
plane.g.throttle_fs_value < plane.g.throttle_fs_value <

5
ArduPlane/Attitude.cpp
View File

@ -13,8 +13,9 @@ float Plane::calc_speed_scaler(void)
auto_state.highest_airspeed = aspeed; auto_state.highest_airspeed = aspeed;
} }
// ensure we have scaling over the full configured airspeed // ensure we have scaling over the full configured airspeed
const float airspeed_min = MAX(aparm.airspeed_min, MIN_AIRSPEED_MIN);
const float scale_min = MIN(0.5, g.scaling_speed / (2.0 * aparm.airspeed_max)); const float scale_min = MIN(0.5, g.scaling_speed / (2.0 * aparm.airspeed_max));
const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * aparm.airspeed_min)); const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * airspeed_min));
if (aspeed > 0.0001f) { if (aspeed > 0.0001f) {
speed_scaler = g.scaling_speed / aspeed; speed_scaler = g.scaling_speed / aspeed;
} else { } else {
@ -25,7 +26,7 @@ float Plane::calc_speed_scaler(void)
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) { if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) {
// when in VTOL modes limit surface movement at low speed to prevent instability // when in VTOL modes limit surface movement at low speed to prevent instability
float threshold = aparm.airspeed_min * 0.5; float threshold = airspeed_min * 0.5;
if (aspeed < threshold) { if (aspeed < threshold) {
float new_scaler = linear_interpolate(0.001, g.scaling_speed / threshold, aspeed, 0, threshold); float new_scaler = linear_interpolate(0.001, g.scaling_speed / threshold, aspeed, 0, threshold);
speed_scaler = MIN(speed_scaler, new_scaler); speed_scaler = MIN(speed_scaler, new_scaler);

2
ArduPlane/defines.h
View File

@ -6,6 +6,8 @@
#define SERVO_MAX 4500.0 // This value represents 45 degrees and is just an #define SERVO_MAX 4500.0 // This value represents 45 degrees and is just an
// arbitrary representation of servo max travel. // arbitrary representation of servo max travel.
#define MIN_AIRSPEED_MIN 5 // m/s, used for arming check and speed scaling
// failsafe // failsafe
// ---------------------- // ----------------------
enum failsafe_state { enum failsafe_state {