Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Throw mode check altitude within params

Browse Source
This commit is contained in:
JanMaciuk 2024-01-02 23:49:18 +01:00 committed by Randy Mackay
parent b65313303f
commit 511659e6ee
3 changed files with 33 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/Parameters.cpp
View File

@ -696,6 +696,20 @@ const AP_Param::Info Copter::var_info[] = {
// @Values: 0:Stopped,1:Running
// @User: Standard
GSCALAR(throw_motor_start, "THROW_MOT_START", (float)ModeThrow::PreThrowMotorState::STOPPED),
// @Param: THROW_ALT_MIN
// @DisplayName: Throw mode minimum altitude
// @Description: Minimum altitude above which Throw mode will detect a throw or a drop - 0 to disable the check
// @Units: m
// @User: Advanced
GSCALAR(throw_altitude_min, "THROW_ALT_MIN", 0),
// @Param: THROW_ALT_MAX
// @DisplayName: Throw mode maximum altitude
// @Description: Maximum altitude under which Throw mode will detect a throw or a drop - 0 to disable the check
// @Units: m
// @User: Advanced
GSCALAR(throw_altitude_max, "THROW_ALT_MAX", 0),
#endif
#if OSD_ENABLED || OSD_PARAM_ENABLED

4
ArduCopter/Parameters.h
View File

@ -383,6 +383,8 @@ public:
// 254,255: reserved
k_param_vehicle = 257, // vehicle common block of parameters
k_param_throw_altitude_min,
k_param_throw_altitude_max,
// the k_param_* space is 9-bits in size
// 511: reserved
@ -463,6 +465,8 @@ public:
#if MODE_THROW_ENABLED == ENABLED
AP_Enum<ModeThrow::PreThrowMotorState> throw_motor_start;
AP_Int16 throw_altitude_min; // minimum altitude in m above which a throw can be detected
AP_Int16 throw_altitude_max; // maximum altitude in m below which a throw can be detected
#endif
AP_Int16 rc_speed; // speed of fast RC Channels in Hz

15
ArduCopter/mode_throw.cpp
View File

@ -272,8 +272,21 @@ bool ModeThrow::throw_detected()
// Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
// fetch the altitude above home
float altitude_above_home; // Use altitude above home if it is set, otherwise relative to EKF origin
if (ahrs.home_is_set()) {
ahrs.get_relative_position_D_home(altitude_above_home);
altitude_above_home = -altitude_above_home; // altitude above home is returned as negative
} else {
altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters
}
// Check that the altitude is within user defined limits
const bool height_within_params = (g.throw_altitude_min == 0 || altitude_above_home > g.throw_altitude_min) && (g.throw_altitude_max == 0 || (altitude_above_home < g.throw_altitude_max));
// High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;
bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params;
// Record time and vertical velocity when we detect the possible throw
if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {