diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index c03193a32d..6420284fb7 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -78,7 +78,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(Log_Write_Fast, 25, 300), SCHED_TASK(update_logging1, 25, 300), SCHED_TASK(update_logging2, 25, 300), +#if SOARING_ENABLED == ENABLED SCHED_TASK(update_soaring, 50, 400), +#endif SCHED_TASK(parachute_check, 10, 200), #if AP_TERRAIN_AVAILABLE SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200), @@ -821,6 +823,13 @@ void Plane::update_alt() distance_beyond_land_wp = get_distance(current_loc, next_WP_loc); } + bool soaring_active = false; +#if SOARING_ENABLED == ENABLED + if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { + soaring_active = true; + } +#endif + SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, @@ -828,7 +837,8 @@ void Plane::update_alt() get_takeoff_pitch_min_cd(), throttle_nudge, tecs_hgt_afe(), - aerodynamic_load_factor); + aerodynamic_load_factor, + soaring_active); } } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 477405f0c6..43344d95a5 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -264,9 +264,11 @@ int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const float GCS_MAVLINK_Plane::vfr_hud_climbrate() const { +#if SOARING_ENABLED == ENABLED if (plane.g2.soaring_controller.is_active()) { return plane.g2.soaring_controller.get_vario_reading(); } +#endif return AP::baro().get_climb_rate(); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 3a1346cf79..c532aa6c8c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1140,9 +1140,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/RC_Channel/RC_Channels.cpp AP_SUBGROUPINFO(rc_channels, "RC", 7, ParametersG2, RC_Channels), +#if SOARING_ENABLED == ENABLED // @Group: SOAR_ // @Path: ../libraries/AP_Soaring/AP_Soaring.cpp AP_SUBGROUPINFO(soaring_controller, "SOAR_", 8, ParametersG2, SoaringController), +#endif // @Param: RUDD_DT_GAIN // @DisplayName: rudder differential thrust gain @@ -1179,8 +1181,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { }; ParametersG2::ParametersG2(void) : - ice_control(plane.rpm_sensor, plane.ahrs), - soaring_controller(plane.ahrs, plane.TECS_controller, plane.aparm) + ice_control(plane.rpm_sensor, plane.ahrs) +#if SOARING_ENABLED == ENABLED + ,soaring_controller(plane.ahrs, plane.TECS_controller, plane.aparm) +#endif { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index e9934b49c0..805e117e7e 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -525,9 +525,11 @@ public: // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; - + +#if SOARING_ENABLED == ENABLED // ArduSoar parameters SoaringController soaring_controller; +#endif // dual motor tailsitter rudder to differential thrust scaling: 0-100% AP_Int8 rudd_dt_gain; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2344b3a651..ad642363cb 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -221,7 +221,7 @@ private: AP_AHRS_DCM ahrs; #endif - AP_TECS TECS_controller{ahrs, aparm, landing, g2.soaring_controller}; + AP_TECS TECS_controller{ahrs, aparm, landing}; AP_L1_Control L1_controller{ahrs, &TECS_controller}; // Attitude to servo controllers @@ -1029,7 +1029,9 @@ private: #endif void accel_cal_update(void); void update_soft_armed(); +#if SOARING_ENABLED == ENABLED void update_soaring(); +#endif // support for AP_Avoidance custom flight mode, AVOID_ADSB bool avoid_adsb_init(bool ignore_checks); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 07bb50a333..234a17d8cb 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -378,3 +378,10 @@ #define OSD_ENABLED DISABLED #endif +#ifndef SOARING_ENABLED +#if HAL_MINIMIZE_FEATURES + #define SOARING_ENABLED DISABLED +#else + #define SOARING_ENABLED ENABLED +#endif +#endif diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 5ea479866e..442ba6b290 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -386,6 +386,7 @@ void Plane::set_servos_controlled(void) constrain_int16(quadplane.forward_throttle_pct(), min_throttle, max_throttle)); } +#if SOARING_ENABLED == ENABLED // suppress throttle when soaring is active if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || control_mode == AUTO || control_mode == LOITER) && @@ -393,6 +394,7 @@ void Plane::set_servos_controlled(void) g2.soaring_controller.get_throttle_suppressed()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); } +#endif } /* diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index beb42945d1..b398a116cc 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -1,5 +1,7 @@ #include "Plane.h" +#if SOARING_ENABLED == ENABLED + /* * ArduSoar support function * @@ -94,3 +96,4 @@ void Plane::update_soaring() { } } +#endif // SOARING_ENABLED diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index c4096650bf..c7f4f6a69f 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -381,9 +381,11 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) auto_navigation_mode = false; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; - + +#if SOARING_ENABLED == ENABLED // for ArduSoar soaring_controller g2.soaring_controller.init_cruising(); +#endif set_target_altitude_current(); break; @@ -392,9 +394,11 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) throttle_allows_nudging = false; auto_throttle_mode = true; auto_navigation_mode = false; - + +#if SOARING_ENABLED == ENABLED // for ArduSoar soaring_controller g2.soaring_controller.init_cruising(); +#endif set_target_altitude_current(); break; @@ -419,8 +423,10 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); - + +#if SOARING_ENABLED == ENABLED g2.soaring_controller.init_cruising(); +#endif break; case RTL: @@ -436,12 +442,14 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) auto_throttle_mode = true; auto_navigation_mode = true; do_loiter_at_location(); - + +#if SOARING_ENABLED == ENABLED if (g2.soaring_controller.is_active() && g2.soaring_controller.suppress_throttle()) { g2.soaring_controller.init_thermalling(); g2.soaring_controller.get_target(next_WP_loc); // ahead on flight path } +#endif break;