Add feedforward setpoint term to feedforward

msg/versioned/VehicleAttitudeSetpoint.msg

@@ -7,6 +7,9 @@ float32 yaw_sp_move_rate	# rad/s (commanded by user)
 # For quaternion-based attitude control
 float32[4] q_d			# Desired quaternion for quaternion control
 
+# Feedforwaredbody rate setpoints
+float32[3] body_rates_ff	# Feedforward
+
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body FRD frame [-1,1]

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1456,6 +1456,11 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 	navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);
 
 	float roll_body = getCorrectedNpfgRollSetpoint();
+	float roll_rate_ff{0.0}; ///TODO: Get roll rate FF from new NPFG formulation
+
+	const matrix::Vector3f feedforward_rate{roll_rate_ff, 0.0, 0.0};
+	feedforward_rate.copyTo(_att_sp.body_rates_ff);
+
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -348,6 +348,8 @@ void FixedwingRateControl::Run()
 				_rates_sp_sub.update(&_rates_sp);
 
 				Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
+				const Vector3f feedforward_setpoint = Vector3f(_rates_sp.body_rates_ff[0], _rates_sp.body_rates_ff[1],
+								      _rates_sp.body_rates_ff[2]);
 
 				// Tailsitter: rotate setpoint from hover to fixed-wing frame (controller is in fixed-wing frame, interface in hover)
 				if (_vehicle_status.is_vtol_tailsitter) {
@@ -359,7 +361,7 @@ void FixedwingRateControl::Run()
 						_landed);
 
 				const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get());
-				const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling;
+				const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling + feedforward_setpoint;
 
 				Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward;