fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: handle invalid bearing setpoint

Browse Source
This commit is contained in:
chfriedrich98 2025-05-12 13:20:42 +02:00 committed by chfriedrich98
parent 1857920a5f
commit ce9dd237a9
4 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/RoverVelocitySetpoint.msg
View File

@ -1,5 +1,5 @@
uint64 timestamp # time since system start (microseconds)
float32 speed # [m/s] [-inf, inf] Speed setpoint (Backwards driving if negative)
float32 bearing # [rad] [-pi,pi] from North.
float32 bearing # [rad] [-pi,pi] from North. [invalid: NAN, speed is defined in body x direction]
float32 yaw # [rad] [-pi, pi] (Mecanum only, Optional, defaults to current vehicle yaw) Vehicle yaw setpoint in NED frame

2
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp
View File

@ -149,7 +149,7 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
} else if (PX4_ISFINITE(_rover_velocity_setpoint.bearing)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;

10
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp
View File

@ -140,10 +140,12 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint()
}
// Attitude Setpoint
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (PX4_ISFINITE(_rover_velocity_setpoint.bearing)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
// Throttle Setpoint
const float heading_error = matrix::wrap_pi(_rover_velocity_setpoint.bearing - _vehicle_yaw);

5
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp
View File

@ -167,7 +167,7 @@ void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
float speed_body_x_setpoint{0.f};
float speed_body_y_setpoint{0.f};
if (fabsf(_rover_velocity_setpoint.speed) > FLT_EPSILON) {
if (fabsf(_rover_velocity_setpoint.speed) > FLT_EPSILON && PX4_ISFINITE(_rover_velocity_setpoint.bearing)) {
const Vector3f velocity_in_local_frame(_rover_velocity_setpoint.speed * cosf(
_rover_velocity_setpoint.bearing),
_rover_velocity_setpoint.speed * sinf(_rover_velocity_setpoint.bearing), 0.f);
@ -175,6 +175,9 @@ void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
speed_body_x_setpoint = velocity_in_body_frame(0);
speed_body_y_setpoint = velocity_in_body_frame(1);
} else {
speed_body_x_setpoint = _rover_velocity_setpoint.speed;
speed_body_y_setpoint = 0.f;
}
if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible