diff --git a/msg/versioned/VehicleAttitudeSetpoint.msg b/msg/versioned/VehicleAttitudeSetpoint.msg index 28aa780699..c48a41bce0 100644 --- a/msg/versioned/VehicleAttitudeSetpoint.msg +++ b/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] diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a15f1e8f71..eac1a13553 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/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