fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 09:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: handle invalid bearing setpoint

Browse Source
This commit is contained in:
chfriedrich98
2025-05-13 10:23:35 +02:00
committed by chfriedrich98
parent 1857920a5f
commit ce9dd237a9
4 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp
+4 -1
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