addressed review comments

Signed-off-by: RomanBapst <[email protected]>

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -16,6 +16,7 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_pos_control start
+fw_lat_control start
 airspeed_selector start
 
 #

msg/FwLongitudinalControlSetpoint.msg

@@ -2,8 +2,8 @@ uint64 timestamp
 
 
 # Note: If not both pitch_sp and throttle_sp are finite, then either altitude_setpoint or height_rate_setpoint must be finite
-float32 height_rate_setpoint # NAN if not controlled directly, used as feeforward if altitude_setpoint is finite [m/s]
 float32 altitude_setpoint  # NAN if not controlled, MSL [m]
+float32 height_rate_setpoint # NAN if not controlled directly, used as feedforward if altitude_setpoint is finite [m/s]
 float32 equivalent_airspeed_setpoint # [m/s]
 float32 pitch_sp # NAN if not controlled, overrides total energy controller [rad]
 float32 thrust_sp # NAN if not controlled, overrides total energy controller [0,1]

src/lib/npfg/DirectionalGuidance.cpp

@@ -56,67 +56,52 @@ DirectionalGuidance::guideToPath(const Vector2f &curr_pos_local, const Vector2f
 	const float wind_speed = wind_vel.norm();
 
 	const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path};
-	_signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle);
+	signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle);
 
 	// on-track wind triangle projections
 	const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
 	const float wind_dot_upt = wind_vel.dot(unit_path_tangent);
 
 	// calculate the bearing feasibility on the track at the current closest point
-	_feas_on_track = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
+	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
 
-	const float track_error = fabsf(_signed_track_error);
+	const float track_error = fabsf(signed_track_error_);
 
 	// update control parameters considering upper and lower stability bounds (if enabled)
 	// must be called before trackErrorBound() as it updates time_const_
-	_adapted_period = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
-				      path_curvature, wind_vel, unit_path_tangent, _feas_on_track);
+	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
+				      path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
 	//const float p_gain = pGain(adapted_period, damping_);
-	_time_const = timeConst(_adapted_period, damping_);
+	_time_const = timeConst(adapted_period_, damping_);
 
 	// track error bound is dynamic depending on ground speed
-	_track_error_bound = trackErrorBound(ground_speed, _time_const);
-	const float normalized_track_error = normalizedTrackError(track_error, _track_error_bound);
+	track_error_bound_ = trackErrorBound(ground_speed, _time_const);
+	const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
 
 	// look ahead angle based solely on track proximity
 	const float look_ahead_ang = lookAheadAngle(normalized_track_error);
 
-	_track_proximity = trackProximity(look_ahead_ang);
+	track_proximity_ = trackProximity(look_ahead_ang);
 
-	_bearing_vec = bearingVec(unit_path_tangent, look_ahead_ang, _signed_track_error);
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_);
 
 	// wind triangle projections
-	const float wind_cross_bearing = wind_vel.cross(_bearing_vec);
-	const float wind_dot_bearing = wind_vel.dot(_bearing_vec);
+	const float wind_cross_bearing = wind_vel.cross(bearing_vec_);
+	const float wind_dot_bearing = wind_vel.dot(bearing_vec_);
 
 	// continuous representation of the bearing feasibility
-	_feas = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
+	feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
 
 	// we consider feasibility of both the current bearing as well as that on the track at the current closest point
-	const float feas_combined = _feas * _feas_on_track;
+	const float feas_combined = feas_ * feas_on_track_;
 	// lateral acceleration needed to stay on curved track (assuming no heading error)
 	const float lateral_accel_ff = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
-				       wind_cross_upt, airspeed, wind_speed, _signed_track_error, path_curvature) * feas_combined * _track_proximity;
+				       wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature) * feas_combined * track_proximity_;
 
-	return DirectionalGuidanceOutput{.course_setpoint = atan2f(_bearing_vec(1), _bearing_vec(0)),
+	return DirectionalGuidanceOutput{.course_setpoint = atan2f(bearing_vec_(1), bearing_vec_(0)),
 					 .lateral_acceleration_feedforward = lateral_accel_ff};
 }
 
-float
-DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
-					const float wind_speed) const
-{
-	if (wind_dot_bearing < 0.0f) {
-		wind_cross_bearing = wind_speed;
-
-	} else {
-		wind_cross_bearing = fabsf(wind_cross_bearing);
-	}
-
-	float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f));
-	return sin_arg * sin_arg;
-}
-
 float DirectionalGuidance::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
 				       const float track_error, const float path_curvature, const Vector2f &wind_vel,
 				       const Vector2f &unit_path_tangent, const float feas_on_track) const
@@ -178,6 +163,13 @@ float DirectionalGuidance::adaptPeriod(const float ground_speed, const float air
 	return period;
 }
 
+float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const
+{
+	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
+}
+
+
+
 float DirectionalGuidance::windFactor(const float airspeed, const float wind_speed) const
 {
 	// See [TODO: include citation] for definition/elaboration of this approximation.
@@ -189,6 +181,18 @@ float DirectionalGuidance::windFactor(const float airspeed, const float wind_spe
 	}
 }
 
+float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor,
+		const float feas_on_track) const
+{
+	if (air_turn_rate * wind_factor > NPFG_EPSILON) {
+		// multiply air turn rate by feasibility on track to zero out when we anyway
+		// should not consider the curvature
+		return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track);
+	}
+
+	return INFINITY;
+}
+
 float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const float wind_factor,
 		const float feas_on_track) const
 {
@@ -210,21 +214,10 @@ float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const flo
 	}
 }
 
-float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor,
-		const float feas_on_track) const
-{
-	if (air_turn_rate * wind_factor > NPFG_EPSILON) {
-		// multiply air turn rate by feasibility on track to zero out when we anyway
-		// should not consider the curvature
-		return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track);
-	}
-
-	return INFINITY;
-}
-
-float DirectionalGuidance::timeConst(const float period, const float damping) const
+float DirectionalGuidance::trackProximity(const float look_ahead_ang) const
 {
-	return period * damping;
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+	return sin_look_ahead_ang * sin_look_ahead_ang;
 }
 
 float DirectionalGuidance::trackErrorBound(const float ground_speed, const float time_const) const
@@ -239,21 +232,16 @@ float DirectionalGuidance::trackErrorBound(const float ground_speed, const float
 	}
 }
 
-float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const
+float DirectionalGuidance::timeConst(const float period, const float damping) const
 {
-	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
+	return period * damping;
 }
 
 float DirectionalGuidance::lookAheadAngle(const float normalized_track_error) const
 {
 	return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
 }
 
-float DirectionalGuidance::trackProximity(const float look_ahead_ang) const
-{
-	const float sin_look_ahead_ang = sinf(look_ahead_ang);
-	return sin_look_ahead_ang * sin_look_ahead_ang;
-}
 
 matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang,
 		const float signed_track_error) const
@@ -267,6 +255,21 @@ matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tange
 	return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent;
 }
 
+float
+DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+					const float wind_speed) const
+{
+	if (wind_dot_bearing < 0.0f) {
+		wind_cross_bearing = wind_speed;
+
+	} else {
+		wind_cross_bearing = fabsf(wind_cross_bearing);
+	}
+
+	float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f));
+	return sin_arg * sin_arg;
+}
+
 float DirectionalGuidance::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel,
 		const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
 		const float wind_speed, const float signed_track_error,
@@ -308,12 +311,7 @@ float DirectionalGuidance::projectAirspOnBearing(const float airspeed, const flo
 	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
 }
 
-float DirectionalGuidance::getBearing()
-{
-	return atan2f(_bearing_vec(1), _bearing_vec(0));
-}
-
 float DirectionalGuidance::switchDistance(float wp_radius) const
 {
-	return math::min(wp_radius, _track_error_bound * _switch_distance_multiplier);
+	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
 }