mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 02:36:37 +08:00
rover: streamline rover steering setpoint
This commit is contained in:
committed by
chfriedrich98
parent
4a5c00d0e4
commit
5a430f0ba6
@@ -1,5 +1,3 @@
|
|||||||
uint64 timestamp # time since system start (microseconds)
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Ackermann only, Positiv: Steer right, Negativ: Steer left)
|
float32 normalized_steering_setpoint # [-1, 1] Positiv = Turn right, Negativ: Turn left (Ackermann: Steering angle, Differential/Mecanum: Speed difference between the left and right wheels)
|
||||||
|
|
||||||
float32 normalized_speed_diff # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Differential/Mecanum only, Positiv = Turn right, Negativ: Turn left)
|
|
||||||
|
|||||||
@@ -83,7 +83,7 @@ void AckermannActControl::updateActControl()
|
|||||||
if (_rover_steering_setpoint_sub.updated()) {
|
if (_rover_steering_setpoint_sub.updated()) {
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
||||||
_steering_setpoint = rover_steering_setpoint.normalized_steering_angle;
|
_steering_setpoint = rover_steering_setpoint.normalized_steering_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (PX4_ISFINITE(_steering_setpoint)) {
|
if (PX4_ISFINITE(_steering_setpoint)) {
|
||||||
|
|||||||
@@ -96,7 +96,7 @@ void AckermannRateControl::updateRateControl()
|
|||||||
|
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = _timestamp;
|
rover_steering_setpoint.timestamp = _timestamp;
|
||||||
rover_steering_setpoint.normalized_steering_angle = math::interpolate<float>(steering_setpoint,
|
rover_steering_setpoint.normalized_steering_setpoint = math::interpolate<float>(steering_setpoint,
|
||||||
-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
|
-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
|
|
||||||
@@ -104,7 +104,7 @@ void AckermannRateControl::updateRateControl()
|
|||||||
_pid_yaw_rate.resetIntegral();
|
_pid_yaw_rate.resetIntegral();
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = _timestamp;
|
rover_steering_setpoint.timestamp = _timestamp;
|
||||||
rover_steering_setpoint.normalized_steering_angle = 0.f;
|
rover_steering_setpoint.normalized_steering_setpoint = 0.f;
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ void ManualMode::manual()
|
|||||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
||||||
rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
|
rover_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.roll;
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||||
|
|||||||
@@ -65,7 +65,7 @@ void DifferentialActControl::updateActControl()
|
|||||||
if (_rover_steering_setpoint_sub.updated()) {
|
if (_rover_steering_setpoint_sub.updated()) {
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
||||||
_speed_diff_setpoint = rover_steering_setpoint.normalized_speed_diff;
|
_speed_diff_setpoint = rover_steering_setpoint.normalized_steering_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (PX4_ISFINITE(_throttle_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) {
|
if (PX4_ISFINITE(_throttle_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) {
|
||||||
|
|||||||
+1
-1
@@ -58,7 +58,7 @@ void DifferentialManualMode::manual()
|
|||||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
||||||
rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll;
|
rover_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.roll;
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||||
|
|||||||
@@ -82,7 +82,7 @@ void DifferentialRateControl::updateRateControl()
|
|||||||
_param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt);
|
_param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt);
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = _timestamp;
|
rover_steering_setpoint.timestamp = _timestamp;
|
||||||
rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized;
|
rover_steering_setpoint.normalized_steering_setpoint = speed_diff_normalized;
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -149,7 +149,7 @@ float DifferentialVelControl::calcSpeedSetpoint()
|
|||||||
if (_rover_steering_setpoint_sub.updated()) {
|
if (_rover_steering_setpoint_sub.updated()) {
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
||||||
_normalized_speed_diff = rover_steering_setpoint.normalized_speed_diff;
|
_normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fabsf(speed_setpoint_normalized) > 1.f - fabsf(
|
if (fabsf(speed_setpoint_normalized) > 1.f - fabsf(
|
||||||
|
|||||||
@@ -67,7 +67,7 @@ void MecanumActControl::updateActControl()
|
|||||||
if (_rover_steering_setpoint_sub.updated()) {
|
if (_rover_steering_setpoint_sub.updated()) {
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
||||||
_speed_diff_setpoint = rover_steering_setpoint.normalized_speed_diff;
|
_speed_diff_setpoint = rover_steering_setpoint.normalized_steering_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (PX4_ISFINITE(_throttle_x_setpoint) && PX4_ISFINITE(_throttle_y_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) {
|
if (PX4_ISFINITE(_throttle_x_setpoint) && PX4_ISFINITE(_throttle_y_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) {
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ void MecanumManualMode::manual()
|
|||||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
||||||
rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw;
|
rover_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.yaw;
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||||
|
|||||||
@@ -82,7 +82,7 @@ void MecanumRateControl::updateRateControl()
|
|||||||
_param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt);
|
_param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt);
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
rover_steering_setpoint.timestamp = _timestamp;
|
rover_steering_setpoint.timestamp = _timestamp;
|
||||||
rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized;
|
rover_steering_setpoint.normalized_steering_setpoint = speed_diff_normalized;
|
||||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -154,7 +154,7 @@ Vector2f MecanumVelControl::calcSpeedSetpoint()
|
|||||||
if (_rover_steering_setpoint_sub.updated()) {
|
if (_rover_steering_setpoint_sub.updated()) {
|
||||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||||
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
|
||||||
_normalized_speed_diff = rover_steering_setpoint.normalized_speed_diff;
|
_normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
float speed_x_setpoint_normalized = math::interpolate<float>(_speed_x_setpoint,
|
float speed_x_setpoint_normalized = math::interpolate<float>(_speed_x_setpoint,
|
||||||
|
|||||||
Reference in New Issue
Block a user