Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: remove SpdHgt and use TECS direct

This commit is contained in:
Iampete1 2021-11-12 17:53:28 +00:00 committed by Andrew Tridgell
parent eb6da9512f
commit b9d240460b
10 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -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,

4
ArduPlane/Attitude.cpp
View File

@ -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() &&

2
ArduPlane/Log.cpp
View File

@ -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(),

6
ArduPlane/Plane.h
View File

@ -62,7 +62,6 @@
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_TECS/AP_TECS.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
@ -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),

4
ArduPlane/altitude.cpp
View File

@ -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

2
ArduPlane/mode_qrtl.cpp
View File

@ -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;

6
ArduPlane/navigation.cpp
View File

@ -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()

8
ArduPlane/quadplane.cpp
View File

@ -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;
}

4
ArduPlane/takeoff.cpp
View File

@ -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;
}
}

1
ArduPlane/wscript
View File

@ -15,7 +15,6 @@ def build(bld):
'AP_L1_Control',
'AP_Navigation',
'AP_RCMapper',
'AP_SpdHgtControl',
'AP_TECS',
'AP_InertialNav',
'AC_WPNav',