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

Plane: remove SpdHgt and use TECS direct

Browse Source
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 // Call TECS 50Hz update. Note that we call this regardless of
// throttle suppressed, as this needs to be running for // throttle suppressed, as this needs to be running for
// takeoff detection // takeoff detection
SpdHgt_Controller->update_50hz(); TECS_controller.update_50hz();
} }
#if HAL_QUADPLANE_ENABLED #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; 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, target_airspeed_cm,
flight_stage, flight_stage,
distance_beyond_land_wp, distance_beyond_land_wp,

4
ArduPlane/Attitude.cpp
View File

@ -554,7 +554,7 @@ void Plane::calc_throttle()
return; 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? // Received an external msg that guides throttle in the last 3 seconds?
if (control_mode->is_guided_mode() && if (control_mode->is_guided_mode() &&
@ -666,7 +666,7 @@ void Plane::calc_nav_pitch()
{ {
// Calculate the Pitch of the plane // 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? // Received an external msg that guides roll in the last 3 seconds?
if (control_mode->is_guided_mode() && 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, pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), throttle_out : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
rudder_out : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder), 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, airspeed_estimate : est_airspeed,
synthetic_airspeed : synthetic_airspeed, synthetic_airspeed : synthetic_airspeed,
EAS2TAS : ahrs.get_EAS2TAS(), 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_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_TECS/AP_TECS.h> #include <AP_TECS/AP_TECS.h>
#include <AP_NavEKF2/AP_NavEKF2.h> #include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h> #include <AP_NavEKF3/AP_NavEKF3.h>
@ -237,9 +236,6 @@ private:
// selected navigation controller // selected navigation controller
AP_Navigation *nav_controller = &L1_controller; AP_Navigation *nav_controller = &L1_controller;
// selected navigation controller
AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
// Camera // Camera
#if CAMERA == ENABLED #if CAMERA == ENABLED
AP_Camera camera{MASK_LOG_CAMERA}; AP_Camera camera{MASK_LOG_CAMERA};
@ -647,7 +643,7 @@ private:
AP_Terrain terrain{mission}; AP_Terrain terrain{mission};
#endif #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::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::constrain_target_altitude_location, void, const Location&, const Location&),
FUNCTOR_BIND_MEMBER(&Plane::adjusted_altitude_cm, int32_t), 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 // for calculating out rate of change of altitude
auto_state.wp_distance = current_loc.get_distance(next_WP_loc); 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); 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(); 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 // 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 // climb rate so we are not constantly at 100% throttle and to
// give a bit more margin on terrain // 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) { if (climb_ratio <= 0) {
// lookahead makes no sense for negative climb rates // 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 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 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 sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time;
const float dist = plane.auto_state.wp_distance; const float dist = plane.auto_state.wp_distance;
const float rad_min = 2*radius; 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_distance = current_loc.get_distance(next_WP_loc);
auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, 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 // update total loiter angle
loiter_angle_update(); loiter_angle_update();
@ -116,7 +116,7 @@ float Plane::mode_auto_target_airspeed_cm()
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && 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::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) { (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)) { if (is_positive(land_airspeed)) {
return land_airspeed * 100; 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 // use the TECS view of the target airspeed for reporting, to take
// account of the landing speed // 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() 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); plane.nav_controller->update_waypoint(plane.current_loc, loc);
// use TECS for throttle // 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 // 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()); 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) { if (poscontrol.get_state() == QPOS_AIRBRAKE) {
// don't allow down pitch in 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 // we reset TECS so that the target height filter is not
// constrained by the climb and sink rates from the initial // constrained by the climb and sink rates from the initial
// takeoff height. // takeoff height.
plane.SpdHgt_Controller->reset(); plane.TECS_controller.reset();
} }
// don't crosstrack on next WP // don't crosstrack on next WP
@ -3616,7 +3616,7 @@ float QuadPlane::get_land_airspeed(void)
{ {
if (poscontrol.get_state() == QPOS_APPROACH || if (poscontrol.get_state() == QPOS_APPROACH ||
plane.control_mode == &plane.mode_rtl) { 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)) { if (!is_positive(land_airspeed)) {
land_airspeed = plane.aparm.airspeed_cruise_cm * 0.01; 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)) { if (!takeoff_state.launchTimerStarted && !is_zero(g.takeoff_throttle_min_accel)) {
// we are requiring an X acceleration event to launch // 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 (g2.takeoff_throttle_accel_count <= 1) {
if (xaccel < g.takeoff_throttle_min_accel) { if (xaccel < g.takeoff_throttle_min_accel) {
goto no_launch; goto no_launch;
@ -72,7 +72,7 @@ bool Plane::auto_takeoff_check(void)
takeoff_state.last_tkoff_arm_time = now; takeoff_state.last_tkoff_arm_time = now;
if (now - takeoff_state.last_report_ms > 2000) { if (now - takeoff_state.last_report_ms > 2000) {
gcs().send_text(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec", 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; takeoff_state.last_report_ms = now;
} }
} }

1
ArduPlane/wscript
View File

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