diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 2c3161058b..12bf2e55a9 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -180,7 +180,7 @@ void Plane::update_speed_height(void) // Call TECS 50Hz update. Note that we call this regardless of // throttle suppressed, as this needs to be running for // takeoff detection - SpdHgt_Controller->update_50hz(); + TECS_controller.update_50hz(); } #if HAL_QUADPLANE_ENABLED @@ -525,7 +525,7 @@ void Plane::update_alt() target_alt = MAX(target_alt, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100; } - SpdHgt_Controller->update_pitch_throttle(target_alt, + TECS_controller.update_pitch_throttle(target_alt, target_airspeed_cm, flight_stage, distance_beyond_land_wp, diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 3daccd0c29..492557540a 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -554,7 +554,7 @@ void Plane::calc_throttle() return; } - float commanded_throttle = SpdHgt_Controller->get_throttle_demand(); + float commanded_throttle = TECS_controller.get_throttle_demand(); // Received an external msg that guides throttle in the last 3 seconds? if (control_mode->is_guided_mode() && @@ -666,7 +666,7 @@ void Plane::calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand(); + int32_t commanded_pitch = TECS_controller.get_pitch_demand(); // Received an external msg that guides roll in the last 3 seconds? if (control_mode->is_guided_mode() && diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 1fa3065244..41a3b1bc8b 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -117,7 +117,7 @@ void Plane::Log_Write_Control_Tuning() pitch : (int16_t)ahrs.pitch_sensor, throttle_out : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), - throttle_dem : SpdHgt_Controller->get_throttle_demand(), + throttle_dem : TECS_controller.get_throttle_demand(), airspeed_estimate : est_airspeed, synthetic_airspeed : synthetic_airspeed, EAS2TAS : ahrs.get_EAS2TAS(), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ae073014d6..83a3e34da2 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -62,7 +62,6 @@ #include // RC input mapping library #include -#include #include #include #include @@ -237,9 +236,6 @@ private: // selected navigation controller AP_Navigation *nav_controller = &L1_controller; - // selected navigation controller - AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller; - // Camera #if CAMERA == ENABLED AP_Camera camera{MASK_LOG_CAMERA}; @@ -647,7 +643,7 @@ private: AP_Terrain terrain{mission}; #endif - AP_Landing landing{mission,ahrs,SpdHgt_Controller,nav_controller,aparm, + AP_Landing landing{mission,ahrs,&TECS_controller,nav_controller,aparm, FUNCTOR_BIND_MEMBER(&Plane::set_target_altitude_proportion, void, const Location&, float), FUNCTOR_BIND_MEMBER(&Plane::constrain_target_altitude_location, void, const Location&, const Location&), FUNCTOR_BIND_MEMBER(&Plane::adjusted_altitude_cm, int32_t), diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index f23ee16010..9dbc21c2ef 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -96,7 +96,7 @@ void Plane::setup_glide_slope(void) // for calculating out rate of change of altitude auto_state.wp_distance = current_loc.get_distance(next_WP_loc); auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc); - SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion); + TECS_controller.set_path_proportion(auto_state.wp_proportion); update_flight_stage(); /* @@ -570,7 +570,7 @@ float Plane::lookahead_adjustment(void) // we need to know the climb ratio. We use 50% of the maximum // climb rate so we are not constantly at 100% throttle and to // give a bit more margin on terrain - float climb_ratio = 0.5f * SpdHgt_Controller->get_max_climbrate() / groundspeed; + float climb_ratio = 0.5f * TECS_controller.get_max_climbrate() / groundspeed; if (climb_ratio <= 0) { // lookahead makes no sense for negative climb rates diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index c7b850533a..94fcf0c352 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -69,7 +69,7 @@ bool ModeQRTL::update_target_altitude() */ const float radius = MAX(fabsf(plane.aparm.loiter_radius), fabsf(plane.g.rtl_radius)); const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt); - const float sink_time = rtl_alt_delta / MAX(0.6*plane.SpdHgt_Controller->get_max_sinkrate(), 1); + const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1); const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time; const float dist = plane.auto_state.wp_distance; const float rad_min = 2*radius; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 471a45f518..d9171f9e11 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -99,7 +99,7 @@ void Plane::navigate() // ---------------------------- auto_state.wp_distance = current_loc.get_distance(next_WP_loc); auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc); - SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion); + TECS_controller.set_path_proportion(auto_state.wp_proportion); // update total loiter angle loiter_angle_update(); @@ -116,7 +116,7 @@ float Plane::mode_auto_target_airspeed_cm() if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && ((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) || (vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) { - const float land_airspeed = SpdHgt_Controller->get_land_airspeed(); + const float land_airspeed = TECS_controller.get_land_airspeed(); if (is_positive(land_airspeed)) { return land_airspeed * 100; } @@ -239,7 +239,7 @@ void Plane::calc_airspeed_errors() // use the TECS view of the target airspeed for reporting, to take // account of the landing speed - airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured; + airspeed_error = TECS_controller.get_target_airspeed() - airspeed_measured; } void Plane::calc_gndspeed_undershoot() diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b81b8217bb..a18afa56e3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2158,10 +2158,10 @@ void QuadPlane::vtol_position_controller(void) plane.nav_controller->update_waypoint(plane.current_loc, loc); // use TECS for throttle - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.SpdHgt_Controller->get_throttle_demand()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.TECS_controller.get_throttle_demand()); // use TECS for pitch - int32_t commanded_pitch = plane.SpdHgt_Controller->get_pitch_demand(); + int32_t commanded_pitch = plane.TECS_controller.get_pitch_demand(); plane.nav_pitch_cd = constrain_int32(commanded_pitch, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); if (poscontrol.get_state() == QPOS_AIRBRAKE) { // don't allow down pitch in airbrake @@ -2857,7 +2857,7 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) // we reset TECS so that the target height filter is not // constrained by the climb and sink rates from the initial // takeoff height. - plane.SpdHgt_Controller->reset(); + plane.TECS_controller.reset(); } // don't crosstrack on next WP @@ -3616,7 +3616,7 @@ float QuadPlane::get_land_airspeed(void) { if (poscontrol.get_state() == QPOS_APPROACH || plane.control_mode == &plane.mode_rtl) { - float land_airspeed = plane.SpdHgt_Controller->get_land_airspeed(); + float land_airspeed = plane.TECS_controller.get_land_airspeed(); if (!is_positive(land_airspeed)) { land_airspeed = plane.aparm.airspeed_cruise_cm * 0.01; } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 7b994f0af9..b0cf80c9c0 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -44,7 +44,7 @@ bool Plane::auto_takeoff_check(void) if (!takeoff_state.launchTimerStarted && !is_zero(g.takeoff_throttle_min_accel)) { // we are requiring an X acceleration event to launch - float xaccel = SpdHgt_Controller->get_VXdot(); + float xaccel = TECS_controller.get_VXdot(); if (g2.takeoff_throttle_accel_count <= 1) { if (xaccel < g.takeoff_throttle_min_accel) { goto no_launch; @@ -72,7 +72,7 @@ bool Plane::auto_takeoff_check(void) takeoff_state.last_tkoff_arm_time = now; if (now - takeoff_state.last_report_ms > 2000) { gcs().send_text(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec", - (double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f)); + (double)TECS_controller.get_VXdot(), (double)(wait_time_ms*0.001f)); takeoff_state.last_report_ms = now; } } diff --git a/ArduPlane/wscript b/ArduPlane/wscript index dde7456ca2..4ce4e0f02c 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -15,7 +15,6 @@ def build(bld): 'AP_L1_Control', 'AP_Navigation', 'AP_RCMapper', - 'AP_SpdHgtControl', 'AP_TECS', 'AP_InertialNav', 'AC_WPNav',