diff --git a/msg/FwLateralControlSetpoint.msg b/msg/FwLateralControlSetpoint.msg index e17954f67e..253d46ddb8 100644 --- a/msg/FwLateralControlSetpoint.msg +++ b/msg/FwLateralControlSetpoint.msg @@ -1,11 +1,12 @@ uint64 timestamp -float32 course_setpoint -float32 airspeed_reference_direction # angle of desired airspeed vector [-pi, pi] -float32 lateral_acceleration_setpoint +# NOTE: At least one of course_setpoint, airspeed_reference_direction, or lateral_acceleration_setpoint must be finite +float32 course_setpoint # NAN if not controlled directly, [-pi, pi] +float32 airspeed_reference_direction # angle of desired airspeed vector, NAN if not controlled directly, used as feedforward if course setpoint is finite, [-pi, pi], +float32 lateral_acceleration_setpoint # NAN if not controlled directly, used as feedforward if either course setpoint or airspeed_reference_direction is finite, [m/s^2] float32 roll_sp # TODO: remove, only for testing -float32 heading_sp_runway_takeoff -bool reset_integral +float32 heading_sp_runway_takeoff # [-pi, pi] heading setpoint for runway takeoff +bool reset_integral # TODO: remove, resets rate controller integrals # TOPICS fw_lateral_control_setpoint fw_lateral_control_status diff --git a/msg/FwLongitudinalControlSetpoint.msg b/msg/FwLongitudinalControlSetpoint.msg index ab602bedae..eeaefd3e9f 100644 --- a/msg/FwLongitudinalControlSetpoint.msg +++ b/msg/FwLongitudinalControlSetpoint.msg @@ -1,9 +1,11 @@ uint64 timestamp -float32 height_rate_setpoint -float32 altitude_setpoint -float32 equivalent_airspeed_setpoint -float32 pitch_sp -float32 thrust_sp + +# 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 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] # TOPICS fw_longitudinal_control_setpoint fw_longitudinal_control_status diff --git a/msg/LateralControlLimits.msg b/msg/LateralControlLimits.msg index 50c2adb6ec..cca3afc84e 100644 --- a/msg/LateralControlLimits.msg +++ b/msg/LateralControlLimits.msg @@ -1,3 +1,3 @@ uint64 timestamp -float32 lateral_accel_max +float32 lateral_accel_max # maps 1:1 to a maximum roll angle, accel_max = tan(roll_max) * GRAVITY, m/s^2 diff --git a/msg/LongitudinalControlLimits.msg b/msg/LongitudinalControlLimits.msg index f6b813ed77..f869d60714 100644 --- a/msg/LongitudinalControlLimits.msg +++ b/msg/LongitudinalControlLimits.msg @@ -1,15 +1,15 @@ uint64 timestamp -float32 pitch_min -float32 pitch_max -float32 throttle_min -float32 throttle_max -float32 climb_rate_max -float32 sink_rate_max -float32 equivalent_airspeed_min -float32 equivalent_airspeed_max +float32 pitch_min # [rad] +float32 pitch_max # [rad] +float32 throttle_min # [0,1] +float32 throttle_max # [0,1] +float32 climb_rate_target # target climbrate used to change altitude, [m/s] +float32 sink_rate_target # target sinkrate used to change altitude, [m/s] +float32 equivalent_airspeed_min # [m/s] +float32 equivalent_airspeed_max # [m/s] -float32 speed_weight +float32 speed_weight # [0,2], 0=pitch controls altitude only, 2=pitch controls airspeed only -bool enforce_low_height_condition +bool enforce_low_height_condition # if true, total energy controller will use lower altitude control time constant bool disable_underspeed_protection diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index e3068a8dae..3915c00047 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -192,8 +192,8 @@ void FwLateralLongitudinalControl::Run() _long_control_limits_sub.get().pitch_max, _long_control_limits_sub.get().throttle_min, _long_control_limits_sub.get().throttle_max, - _long_control_limits_sub.get().sink_rate_max, - _long_control_limits_sub.get().climb_rate_max, + _long_control_limits_sub.get().sink_rate_target, + _long_control_limits_sub.get().climb_rate_target, _long_control_limits_sub.get().disable_underspeed_protection, longitudinal_sp.height_rate_setpoint ); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5490cd0ade..5bb206a476 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -764,8 +764,8 @@ const longitudinal_control_limits.pitch_max = radians(_param_fw_p_lim_max.get()); longitudinal_control_limits.throttle_min = _param_fw_thr_min.get(); longitudinal_control_limits.throttle_max = _param_fw_thr_max.get(); - longitudinal_control_limits.climb_rate_max = _param_climbrate_target.get(); - longitudinal_control_limits.sink_rate_max = _param_sinkrate_target.get(); + longitudinal_control_limits.climb_rate_target = _param_climbrate_target.get(); + longitudinal_control_limits.sink_rate_target = _param_sinkrate_target.get(); longitudinal_control_limits.disable_underspeed_protection = false; longitudinal_control_limits.enforce_low_height_condition = false; longitudinal_control_limits.speed_weight = _param_fw_t_spdweight.get(); @@ -1406,7 +1406,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo longitudinal_control_limits.pitch_min = pitch_min; longitudinal_control_limits.pitch_max = pitch_max; - longitudinal_control_limits.climb_rate_max = _performance_model.getMaximumClimbRate(_air_density); + longitudinal_control_limits.climb_rate_target = _performance_model.getMaximumClimbRate(_air_density); longitudinal_control_limits.disable_underspeed_protection = true; _flaps_setpoint = _param_fw_flaps_to_scl.get(); @@ -1494,7 +1494,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo longitudinal_control_limits.pitch_min = radians(_takeoff_pitch_min.get()); longitudinal_control_limits.throttle_max = max_takeoff_throttle; - longitudinal_control_limits.climb_rate_max = _performance_model.getMaximumClimbRate(_air_density); + longitudinal_control_limits.climb_rate_target = _performance_model.getMaximumClimbRate(_air_density); longitudinal_control_limits.disable_underspeed_protection = true; //float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -1745,7 +1745,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); - longitudinal_control_limits.sink_rate_max = desired_max_sinkrate; + longitudinal_control_limits.sink_rate_target = desired_max_sinkrate; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -1938,7 +1938,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); - longitudinal_control_limits.sink_rate_max = desired_max_sinkrate; + longitudinal_control_limits.sink_rate_target = desired_max_sinkrate; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); }