diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index c20069ca87..fa59829028 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -145,9 +145,11 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @User: Standard AP_GROUPINFO("TYPE", 14, AP_Landing, type, TYPE_STANDARD_GLIDE_SLOPE), +#if HAL_LANDING_DEEPSTALL_ENABLED // @Group: DS_ // @Path: AP_Landing_Deepstall.cpp AP_SUBGROUPINFO(deepstall, "DS_", 15, AP_Landing, AP_Landing_Deepstall), +#endif // @Param: OPTIONS // @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) ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn) ,update_flight_stage_fn(_update_flight_stage_fn) +#if HAL_LANDING_DEEPSTALL_ENABLED ,deepstall(*this) +#endif { 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: type_slope_do_land(cmd, relative_altitude); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: deepstall.do_land(cmd, relative_altitude); break; +#endif default: // a incorrect type is handled in the verify_land 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, height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: 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); break; +#endif default: // returning TRUE while executing verify_land() will increment the // 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: type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: deepstall.verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed); break; +#endif default: break; } @@ -274,7 +284,9 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing 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); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: +#endif default: break; } @@ -287,8 +299,10 @@ bool AP_Landing::send_landing_message(mavlink_channel_t chan) { } switch (type) { +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.send_deepstall_message(chan); +#endif case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -304,7 +318,9 @@ bool AP_Landing::is_flaring(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_flaring(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: +#endif default: return false; } @@ -324,8 +340,10 @@ bool AP_Landing::is_on_approach(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_on_approach(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.is_on_approach(); +#endif default: return false; } @@ -341,8 +359,10 @@ bool AP_Landing::is_ground_steering_allowed(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_on_approach(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return false; +#endif default: return true; } @@ -359,7 +379,9 @@ bool AP_Landing::is_expecting_impact(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_expecting_impact(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: +#endif default: return false; } @@ -372,8 +394,10 @@ bool AP_Landing::override_servos(void) { } switch (type) { +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.override_servos(); +#endif case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -385,8 +409,10 @@ bool AP_Landing::override_servos(void) { const AP_Logger::PID_Info* AP_Landing::get_pid_info(void) const { switch (type) { +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return &deepstall.get_pid_info(); +#endif case TYPE_STANDARD_GLIDE_SLOPE: default: 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: type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: +#endif default: break; } @@ -473,7 +501,9 @@ int32_t AP_Landing::constrain_roll(const int32_t desired_roll_cd, const int32_t switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_constrain_roll(desired_roll_cd, level_roll_limit_cd); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: +#endif default: return desired_roll_cd; } @@ -487,8 +517,10 @@ bool AP_Landing::get_target_altitude_location(Location &location) } switch (type) { +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.get_target_altitude_location(location); +#endif case TYPE_STANDARD_GLIDE_SLOPE: default: return false; @@ -535,8 +567,10 @@ int32_t AP_Landing::get_target_airspeed_cm(void) switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_get_target_airspeed_cm(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.get_target_airspeed_cm(); +#endif default: // don't return the landing airspeed, because if type is invalid we have // 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: success = type_slope_request_go_around(); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: success = deepstall.request_go_around(); break; +#endif default: break; } @@ -584,8 +620,10 @@ bool AP_Landing::is_complete(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_complete(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return false; +#endif default: return true; } @@ -597,9 +635,11 @@ void AP_Landing::Log(void) const case TYPE_STANDARD_GLIDE_SLOPE: type_slope_log(); break; +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: deepstall.Log(); break; +#endif default: break; } @@ -617,8 +657,10 @@ bool AP_Landing::is_throttle_suppressed(void) const switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: return type_slope_is_throttle_suppressed(); +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.is_throttle_suppressed(); +#endif default: return false; } @@ -640,8 +682,10 @@ bool AP_Landing::is_flying_forward(void) const } switch (type) { +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.is_flying_forward(); +#endif case TYPE_STANDARD_GLIDE_SLOPE: default: return true; @@ -654,8 +698,10 @@ bool AP_Landing::is_flying_forward(void) const */ bool AP_Landing::terminate(void) { switch (type) { +#if HAL_LANDING_DEEPSTALL_ENABLED case TYPE_DEEPSTALL: return deepstall.terminate(); +#endif case TYPE_STANDARD_GLIDE_SLOPE: default: return false; diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 82d447bbef..2970f85787 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -52,7 +52,9 @@ public: // NOTE: make sure to update is_type_valid() enum Landing_Type { TYPE_STANDARD_GLIDE_SLOPE = 0, +#if HAL_LANDING_DEEPSTALL_ENABLED TYPE_DEEPSTALL = 1, +#endif // TODO: TYPE_PARACHUTE, // TODO: TYPE_HELICAL, }; @@ -143,8 +145,10 @@ private: disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn; update_flight_stage_fn_t update_flight_stage_fn; +#if HAL_LANDING_DEEPSTALL_ENABLED // support for deepstall landings AP_Landing_Deepstall deepstall; +#endif AP_Int16 pitch_cd; AP_Float flare_alt; diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 00310f7f5c..7034b320e3 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -18,6 +18,9 @@ */ #include "AP_Landing.h" + +#if HAL_LANDING_DEEPSTALL_ENABLED + #include #include #include @@ -655,3 +658,5 @@ float AP_Landing_Deepstall::update_steering() return ds_PID.get_pid(error); } + +#endif // HAL_LANDING_DEEPSTALL_ENABLED diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.h b/libraries/AP_Landing/AP_Landing_Deepstall.h index 948da12333..7d83e9ecd3 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.h +++ b/libraries/AP_Landing/AP_Landing_Deepstall.h @@ -22,6 +22,10 @@ #include #include +#ifndef HAL_LANDING_DEEPSTALL_ENABLED +#define HAL_LANDING_DEEPSTALL_ENABLED 1 +#endif + class AP_Landing; /// @class AP_Landing_Deepstall