TECS: some cosmetics

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/tecs/TECS.cpp

@@ -254,10 +254,10 @@ void TECS::_update_energy_estimates()
 
 	// Calculate the specific energy balance demand which specifies how the available total
 	// energy should be allocated to speed (kinetic energy) and height (potential energy)
-	float SEB_setpoint = _SPE_setpoint * _SPE_weight - _SKE_setpoint * _SKE_weight;
+	float SEB_setpoint = _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting;
 
 	// Calculate the specific energy balance error
-	_SEB_error = SEB_setpoint - (_SPE_estimate * _SPE_weight - _SKE_estimate * _SKE_weight);
+	_SEB_error = SEB_setpoint - (_SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting);
 
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
 	_SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
@@ -416,10 +416,10 @@ void TECS::_update_pitch_setpoint()
 	*/
 
 	// Calculate the specific energy balance rate demand
-	const float SEB_rate_setpoint = _SPE_rate_setpoint * _SPE_weight - _SKE_rate_setpoint * _SKE_weight;
+	const float SEB_rate_setpoint = _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting;
 
 	// Calculate the specific energy balance rate error
-	_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * _SPE_weight - _SKE_rate * _SKE_weight);
+	_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting);
 
 	// Calculate derivative from change in climb angle to rate of change of specific energy balance
 	const float climb_angle_to_SEB_rate = _tas_state * CONSTANTS_ONE_G;
@@ -444,26 +444,27 @@ void TECS::_update_pitch_setpoint()
 	}
 
 	// Calculate a specific energy correction that doesn't include the integrator contribution
-	float SEB_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint;
+	float SEB_rate_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint;
 
 	// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
 	// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
 	// having to catch up before the nose can be raised to reduce excess speed during climbout.
 	if (_climbout_mode_active) {
-		SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
+		SEB_rate_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
 	}
 
-	// Convert the specific energy balance correction to a target pitch angle. This calculation assumes:
+	// Convert the specific energy balance rate correction to a target pitch angle. This calculation assumes:
 	// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
 	// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
 	// pitch transients due to control action or turbulence.
-	_pitch_setpoint_unc = SEB_correction / climb_angle_to_SEB_rate;
-	_pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
+	_pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate;
+
+	float pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
 
 	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
 	const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
-	_pitch_setpoint = constrain(_pitch_setpoint, _last_pitch_setpoint - ptchRateIncr, _last_pitch_setpoint + ptchRateIncr);
-	_last_pitch_setpoint = _pitch_setpoint;
+	_last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - ptchRateIncr,
+					 _last_pitch_setpoint + ptchRateIncr);
 }
 
 void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
@@ -612,37 +613,37 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float
 void TECS::_update_speed_height_weights()
 {
 	// Calculate the weight applied to control of specific kinetic energy error
-	_SKE_weight = constrain(_pitch_speed_weight, 0.0f, 2.0f);
+	_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
 
 	if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
-		_SKE_weight = 2.0f;
+		_SKE_weighting = 2.0f;
 
 	} else if (!airspeed_sensor_enabled()) {
-		_SKE_weight = 0.0f;
+		_SKE_weighting = 0.0f;
 	}
 
 	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
 	// loop time constant and therefore can lead to a destabilization of that control loop
-	_SPE_weight = constrain(2.0f - _SKE_weight, 0.f, 1.f);
-	_SKE_weight = constrain(_SKE_weight, 0.f, 1.f);
+	_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
+	_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
 }
 
 float TECS::SEB()
 {
-	return _SPE_estimate * _SPE_weight - _SKE_estimate * _SKE_weight;
+	return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting;
 }
 
 float TECS::SEB_setpoint()
 {
-	return _SPE_setpoint * _SPE_weight - _SKE_setpoint * _SKE_weight;
+	return _SPE_setpoint * _SPE_weighting - _SKE_setpoint * _SKE_weighting;
 }
 
 float TECS::SEB_rate()
 {
-	return _SPE_rate * _SPE_weight - _SKE_rate * _SKE_weight;
+	return _SPE_rate * _SPE_weighting - _SKE_rate * _SKE_weighting;
 }
 
 float TECS::SEB_rate_setpoint()
 {
-	return _SPE_rate_setpoint * _SPE_weight - _SKE_rate_setpoint * _SKE_weight;
+	return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting;
 }

src/lib/tecs/TECS.hpp

@@ -87,7 +87,7 @@ public:
 				   float pitch_limit_min, float pitch_limit_max);
 
 	float get_throttle_setpoint() { return _last_throttle_setpoint; }
-	float get_pitch_setpoint() { return _pitch_setpoint; }
+	float get_pitch_setpoint() { return _last_pitch_setpoint; }
 	float get_speed_weight() { return _pitch_speed_weight; }
 
 	void reset_state() { _states_initialized = false; }
@@ -221,9 +221,6 @@ private:
 	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
 	float _speed_derivative_time_const{0.01f};		///< speed derivative filter time constant (s)
 
-	// controller outputs
-	float _pitch_setpoint{0.0f};					///< pitch angle demand (radians)
-
 	// complimentary filter states
 	float _vert_vel_state{0.0f};					///< complimentary filter state - height rate (m/sec)
 	float _vert_pos_state{0.0f};					///< complimentary filter state - height (m)
@@ -280,9 +277,9 @@ private:
 	float _SEB_error{0.0f};						///< specific energy balance error (m**2/sec**2)
 	float _SEB_rate_error{0.0f};					///< specific energy balance rate error (m**2/sec**3)
 
-	// speed height weights
-	float _SPE_weight{1.0f};
-	float _SKE_weight{1.0f};
+	// speed height weighting
+	float _SPE_weighting{1.0f};
+	float _SKE_weighting{1.0f};
 
 	// time steps (non-fixed)
 	float _dt{DT_DEFAULT};						///< Time since last update of main TECS loop (sec)