fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 20:49:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

ackermann: streamline flow of information

Browse Source
This commit is contained in:
chfriedrich98 2025-03-31 09:02:46 +02:00 committed by chfriedrich98
parent 68dc1fcd66
commit 8eb873a245
4 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
View File

@ -85,7 +85,7 @@ void AckermannAttControl::updateAttControl()
}
if (_vehicle_control_mode.flag_control_manual_enabled) {
generateAttitudeSetpoint();
generateAttitudeAndThrottleSetpoint();
}
generateRateSetpoint();
@ -99,12 +99,12 @@ void AckermannAttControl::updateAttControl()
rover_attitude_status_s rover_attitude_status;
rover_attitude_status.timestamp = _timestamp;
rover_attitude_status.measured_yaw = _vehicle_yaw;
rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState();
rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState());
_rover_attitude_status_pub.publish(rover_attitude_status);
}
void AckermannAttControl::generateAttitudeSetpoint()
void AckermannAttControl::generateAttitudeAndThrottleSetpoint()
{
const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
@ -120,17 +120,18 @@ void AckermannAttControl::generateAttitudeSetpoint()
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
_max_yaw_rate / _param_ro_yaw_p.get());
if (fabsf(yaw_rate_setpoint) > FLT_EPSILON
if (fabsf(yaw_delta) > FLT_EPSILON
|| fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control
_stab_yaw_ctl = false;
_adjusted_yaw_setpoint.setForcedValue(0.f);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * yaw_rate_setpoint;
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
if (!_stab_yaw_ctl) {

4
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp
View File

@ -81,9 +81,9 @@ protected:
private:
/**
* @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode).
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode).
*/
void generateAttitudeSetpoint();
void generateAttitudeAndThrottleSetpoint();
/**
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.

4
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
View File

@ -82,7 +82,7 @@ void AckermannRateControl::updateRateControl()
}
if (_vehicle_control_mode.flag_control_manual_enabled) {
generateRateSetpoint();
generateRateAndThrottleSetpoint();
}
generateSteeringSetpoint();
@ -102,7 +102,7 @@ void AckermannRateControl::updateRateControl()
}
void AckermannRateControl::generateRateSetpoint()
void AckermannRateControl::generateRateAndThrottleSetpoint()
{
const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled;

4
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
View File

@ -82,9 +82,9 @@ protected:
private:
/**
* @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode).
* @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode).
*/
void generateRateSetpoint();
void generateRateAndThrottleSetpoint();
/**
* @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.