FW Position Controller rework

- split up old module into two, one handling setpoint generation, one control
- add lateral and longitudinal control setpoints topics that can also be
injected from companion computer
- add configuration topics that (optionally) configure the controller
with limits and momentary settings

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

msg/CMakeLists.txt

@@ -90,6 +90,10 @@ set(msg_files
 	FollowTargetEstimator.msg
 	FollowTargetStatus.msg
 	FuelTankStatus.msg
+	FixedWingLateralSetpoint.msg
+	FixedWingLongitudinalSetpoint.msg
+	FixedWingLateralGuidanceStatus.msg
+	FixedWingLateralStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GeofenceStatus.msg
@@ -120,10 +124,12 @@ set(msg_files
 	LandingGearWheel.msg
 	LandingTargetInnovations.msg
 	LandingTargetPose.msg
+	LateralControlConfiguration.msg
 	LaunchDetectionStatus.msg
 	LedControl.msg
 	LoggerStatus.msg
 	LogMessage.msg
+	LongitudinalControlConfiguration.msg
 	MagnetometerBiasEstimate.msg
 	MagWorkerData.msg
 	ManualControlSwitches.msg
@@ -137,7 +143,6 @@ set(msg_files
 	NavigatorMissionItem.msg
 	NavigatorStatus.msg
 	NormalizedUnsignedSetpoint.msg
-	NpfgStatus.msg
 	ObstacleDistance.msg
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg

msg/FixedWingLateralGuidanceStatus.msg

@@ -0,0 +1,10 @@
+uint64 timestamp                # time since system start (microseconds)
+
+float32 course_setpoint         # bearing angle (same as course) [rad]
+float32 lateral_acceleration_ff # lateral acceleration demand only for maintaining curvature [m/s^2]
+float32 bearing_feas            # bearing feasibility [0,1]
+float32 bearing_feas_on_track   # on-track bearing feasibility [0,1]
+float32 signed_track_error      # signed track error [m]
+float32 track_error_bound       # track error bound [m]
+float32 adapted_period          # adapted period (if auto-tuning enabled) [s]
+uint8 wind_est_valid            # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid)

msg/FixedWingLateralSetpoint.msg

@@ -0,0 +1,7 @@
+uint64 timestamp
+
+# NOTE: At least one of course, airspeed_direction, or lateral_acceleration must be finite
+
+float32 course 				# [-pi, pi] Course over ground setpoint, w.r.t. North. NAN if not controlled directly.
+float32 airspeed_direction    		# [-pi, pi] Angle projected to ground of desired airspeed vector, w.r.t. North. NAN if not controlled directly, used as feedforward if course setpoint is finite.
+float32 lateral_acceleration 		# [m/s^2] Lateral acceleration setpoint in FRD frame. NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_direction is finite.

msg/FixedWingLateralStatus.msg

@@ -0,0 +1,4 @@
+uint64 timestamp                # time since system start (microseconds)
+
+float32 lateral_acceleration    # resultant lateral acceleration reference [m/s^2]
+float32 can_run_factor 	 	# estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]

msg/FixedWingLongitudinalSetpoint.msg

@@ -0,0 +1,9 @@
+uint64 timestamp
+
+# Note: If not both pitch_direct and throttle_direct are finite, then either altitude or height_rate must be finite
+
+float32 altitude  			# [m] Altitude setpoint AMSL, NAN if not controlled
+float32 height_rate 			# [m/s] NAN if not controlled directly, used as feedforward if altitude is finite
+float32 equivalent_airspeed 		# [m/s] NAN if system default should be used
+float32 pitch_direct 			# [rad] NAN if not controlled, overrides total energy controller
+float32 throttle_direct 		# [0,1] NAN if not controlled, overrides total energy controller

msg/LateralControlConfiguration.msg

@@ -0,0 +1,3 @@
+uint64 timestamp
+
+float32 lateral_accel_max # [m/s^2] maps 1:1 to a maximum roll angle, accel_max = tan(roll_max) * GRAVITY

msg/LongitudinalControlConfiguration.msg

@@ -0,0 +1,11 @@
+uint64 timestamp
+
+float32 pitch_min   			# [rad]
+float32 pitch_max   			# [rad]
+float32 throttle_min 			# [0,1]
+float32 throttle_max 			# [0,1]
+float32 climb_rate_target 		# [m/s] target climbrate used to change altitude
+float32 sink_rate_target 		# [m/s] target sinkrate used to change altitude
+float32 speed_weight 			# [0,2], 0=pitch controls altitude only, 2=pitch controls airspeed only
+bool enforce_low_height_condition 	# if true, total energy controller will use lower altitude control time constant
+bool disable_underspeed_protection 	# if true, underspeed handling is disabled in the total energy controller

msg/NpfgStatus.msg

@@ -1,17 +0,0 @@
-uint64 timestamp                # time since system start (microseconds)
-
-uint8 wind_est_valid            # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid)
-float32 lat_accel               # resultant lateral acceleration reference [m/s^2]
-float32 lat_accel_ff            # lateral acceleration demand only for maintaining curvature [m/s^2]
-float32 bearing_feas            # bearing feasibility [0,1]
-float32 bearing_feas_on_track   # on-track bearing feasibility [0,1]
-float32 signed_track_error      # signed track error [m]
-float32 track_error_bound       # track error bound [m]
-float32 airspeed_ref            # (true) airspeed reference [m/s]
-float32 bearing                 # bearing angle [rad]
-float32 heading_ref             # heading angle reference [rad]
-float32 min_ground_speed_ref    # minimum forward ground speed reference [m/s]
-float32 adapted_period          # adapted period (if auto-tuning enabled) [s]
-float32 p_gain                  # controller proportional gain [rad/s]
-float32 time_const              # controller time constant [s]
-float32 can_run_factor 	 	# estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1]