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

AP_Landing: enable deepstall landing to be compiled out of the code

Browse Source
This commit is contained in:
Peter Barker 2021-07-02 11:06:47 +10:00 committed by Peter Barker
parent 765b71adb6
commit a7ab766fda
4 changed files with 59 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_Landing/AP_Landing.cpp
View File

@ -145,9 +145,11 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE), AP_GROUPINFO("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE),
#if HAL_LANDING_DEEPSTALL_ENABLED
// @Group: DS_ // @Group: DS_
// @Path: AP_Landing_Deepstall.cpp // @Path: AP_Landing_Deepstall.cpp
AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall), AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall),
#endif
// @Param: OPTIONS // @Param: OPTIONS
// @DisplayName: Landing options bitmask // @DisplayName: Landing options bitmask
@ -178,7 +180,9 @@ AP_Landing::AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_
,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn) ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn)
,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn) ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn)
,update_flight_stage_fn(_update_flight_stage_fn) ,update_flight_stage_fn(_update_flight_stage_fn)
#if HAL_LANDING_DEEPSTALL_ENABLED
,deepstall(*this) ,deepstall(*this)
#endif
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -193,9 +197,11 @@ void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float rel
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_do_land(cmd, relative_altitude); type_slope_do_land(cmd, relative_altitude);
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
deepstall.do_land(cmd, relative_altitude); deepstall.do_land(cmd, relative_altitude);
break; break;
#endif
default: default:
// a incorrect type is handled in the verify_land // a incorrect type is handled in the verify_land
break; break;
@ -218,10 +224,12 @@ bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc,
success = type_slope_verify_land(prev_WP_loc, next_WP_loc, current_loc, success = type_slope_verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range); height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
success = deepstall.verify_land(prev_WP_loc, next_WP_loc, current_loc, success = deepstall.verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range); height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
break; break;
#endif
default: default:
// returning TRUE while executing verify_land() will increment the // returning TRUE while executing verify_land() will increment the
// mission index which in many cases will trigger an RTL for end-of-mission // mission index which in many cases will trigger an RTL for end-of-mission
@ -240,9 +248,11 @@ bool AP_Landing::verify_abort_landing(const Location &prev_WP_loc, Location &nex
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
break; break;
#endif
default: default:
break; break;
} }
@ -274,7 +284,9 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm); type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
#endif
default: default:
break; break;
} }
@ -287,8 +299,10 @@ bool AP_Landing::send_landing_message(mavlink_channel_t chan) {
} }
switch (type) { switch (type) {
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.send_deepstall_message(chan); return deepstall.send_deepstall_message(chan);
#endif
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
default: default:
return false; return false;
@ -304,7 +318,9 @@ bool AP_Landing::is_flaring(void) const
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_flaring(); return type_slope_is_flaring();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
#endif
default: default:
return false; return false;
} }
@ -324,8 +340,10 @@ bool AP_Landing::is_on_approach(void) const
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_on_approach(); return type_slope_is_on_approach();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.is_on_approach(); return deepstall.is_on_approach();
#endif
default: default:
return false; return false;
} }
@ -341,8 +359,10 @@ bool AP_Landing::is_ground_steering_allowed(void) const
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_on_approach(); return type_slope_is_on_approach();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return false; return false;
#endif
default: default:
return true; return true;
} }
@ -359,7 +379,9 @@ bool AP_Landing::is_expecting_impact(void) const
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_expecting_impact(); return type_slope_is_expecting_impact();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
#endif
default: default:
return false; return false;
} }
@ -372,8 +394,10 @@ bool AP_Landing::override_servos(void) {
} }
switch (type) { switch (type) {
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.override_servos(); return deepstall.override_servos();
#endif
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
default: default:
return false; return false;
@ -385,8 +409,10 @@ bool AP_Landing::override_servos(void) {
const AP_Logger::PID_Info* AP_Landing::get_pid_info(void) const const AP_Logger::PID_Info* AP_Landing::get_pid_info(void) const
{ {
switch (type) { switch (type) {
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return &deepstall.get_pid_info(); return &deepstall.get_pid_info();
#endif
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
default: default:
return nullptr; return nullptr;
@ -408,7 +434,9 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
#endif
default: default:
break; break;
} }
@ -473,7 +501,9 @@ int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd); return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd);
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
#endif
default: default:
return desired_roll_cd; return desired_roll_cd;
} }
@ -487,8 +517,10 @@ bool AP_Landing::get_target_altitude_location(Location &location)
} }
switch (type) { switch (type) {
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.get_target_altitude_location(location); return deepstall.get_target_altitude_location(location);
#endif
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
default: default:
return false; return false;
@ -535,8 +567,10 @@ int32_t AP_Landing::get_target_airspeed_cm(void)
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_get_target_airspeed_cm(); return type_slope_get_target_airspeed_cm();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.get_target_airspeed_cm(); return deepstall.get_target_airspeed_cm();
#endif
default: default:
// don't return the landing airspeed, because if type is invalid we have // don't return the landing airspeed, because if type is invalid we have
// no postive indication that the land airspeed has been configured or // no postive indication that the land airspeed has been configured or
@ -557,9 +591,11 @@ bool AP_Landing::request_go_around(void)
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
success = type_slope_request_go_around(); success = type_slope_request_go_around();
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
success = deepstall.request_go_around(); success = deepstall.request_go_around();
break; break;
#endif
default: default:
break; break;
} }
@ -584,8 +620,10 @@ bool AP_Landing::is_complete(void) const
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_complete(); return type_slope_is_complete();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return false; return false;
#endif
default: default:
return true; return true;
} }
@ -597,9 +635,11 @@ void AP_Landing::Log(void) const
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_log(); type_slope_log();
break; break;
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
deepstall.Log(); deepstall.Log();
break; break;
#endif
default: default:
break; break;
} }
@ -617,8 +657,10 @@ bool AP_Landing::is_throttle_suppressed(void) const
switch (type) { switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_throttle_suppressed(); return type_slope_is_throttle_suppressed();
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.is_throttle_suppressed(); return deepstall.is_throttle_suppressed();
#endif
default: default:
return false; return false;
} }
@ -640,8 +682,10 @@ bool AP_Landing::is_flying_forward(void) const
} }
switch (type) { switch (type) {
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.is_flying_forward(); return deepstall.is_flying_forward();
#endif
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
default: default:
return true; return true;
@ -654,8 +698,10 @@ bool AP_Landing::is_flying_forward(void) const
*/ */
bool AP_Landing::terminate(void) { bool AP_Landing::terminate(void) {
switch (type) { switch (type) {
#if HAL_LANDING_DEEPSTALL_ENABLED
case TYPE_DEEPSTALL: case TYPE_DEEPSTALL:
return deepstall.terminate(); return deepstall.terminate();
#endif
case TYPE_STANDARD_GLIDE_SLOPE: case TYPE_STANDARD_GLIDE_SLOPE:
default: default:
return false; return false;

4
libraries/AP_Landing/AP_Landing.h
View File

@ -52,7 +52,9 @@ public:
// NOTE: make sure to update is_type_valid() // NOTE: make sure to update is_type_valid()
enum Landing_Type { enum Landing_Type {
TYPE_STANDARD_GLIDE_SLOPE = 0, TYPE_STANDARD_GLIDE_SLOPE = 0,
#if HAL_LANDING_DEEPSTALL_ENABLED
TYPE_DEEPSTALL = 1, TYPE_DEEPSTALL = 1,
#endif
// TODO: TYPE_PARACHUTE, // TODO: TYPE_PARACHUTE,
// TODO: TYPE_HELICAL, // TODO: TYPE_HELICAL,
}; };
@ -143,8 +145,10 @@ private:
disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn; disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn;
update_flight_stage_fn_t update_flight_stage_fn; update_flight_stage_fn_t update_flight_stage_fn;
#if HAL_LANDING_DEEPSTALL_ENABLED
// support for deepstall landings // support for deepstall landings
AP_Landing_Deepstall deepstall; AP_Landing_Deepstall deepstall;
#endif
AP_Int16 pitch_cd; AP_Int16 pitch_cd;
AP_Float flare_alt; AP_Float flare_alt;

5
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -18,6 +18,9 @@
*/ */
#include "AP_Landing.h" #include "AP_Landing.h"
#if HAL_LANDING_DEEPSTALL_ENABLED
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
@ -655,3 +658,5 @@ float AP_Landing_Deepstall::update_steering()
return ds_PID.get_pid(error); return ds_PID.get_pid(error);
} }
#endif // HAL_LANDING_DEEPSTALL_ENABLED

4
libraries/AP_Landing/AP_Landing_Deepstall.h
View File

@ -22,6 +22,10 @@
#include <AP_Navigation/AP_Navigation.h> #include <AP_Navigation/AP_Navigation.h>
#include <PID/PID.h> #include <PID/PID.h>
#ifndef HAL_LANDING_DEEPSTALL_ENABLED
#define HAL_LANDING_DEEPSTALL_ENABLED 1
#endif
class AP_Landing; class AP_Landing;
/// @class AP_Landing_Deepstall /// @class AP_Landing_Deepstall