fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 19:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: apply sp renaming to rover modules

Browse Source
This commit is contained in:
chfriedrich98
2025-09-05 08:41:59 +02:00
parent 7670989769
commit ae65b33ba0
50 changed files with 832 additions and 822 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+29 -26
View File
@@ -38,8 +38,8 @@ using namespace time_literals;
DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent)
{
_pure_pursuit_status_pub.advertise();
_rover_speed_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_surface_vehicle_speed_setpoint_pub.advertise();
_surface_vehicle_attitude_setpoint_pub.advertise();
updateParams();
}
@@ -97,25 +97,25 @@ void DifferentialPosControl::updatePosControl()
speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed);
}
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = speed_setpoint;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
yaw_setpoint + M_PI_F);
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{};
surface_vehicle_speed_setpoint.timestamp = timestamp;
surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint;
_surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint);
surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{};
surface_vehicle_attitude_setpoint.timestamp = timestamp;
surface_vehicle_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
yaw_setpoint + M_PI_F);
_surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint);
} else {
rover_speed_setpoint_s rover_speed_setpoint{};
rover_speed_setpoint.timestamp = timestamp;
rover_speed_setpoint.speed_body_x = 0.f;
_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{};
surface_vehicle_speed_setpoint.timestamp = timestamp;
surface_vehicle_speed_setpoint.speed_body_x = 0.f;
_surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint);
surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{};
surface_vehicle_attitude_setpoint.timestamp = timestamp;
surface_vehicle_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
_surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint);
if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
_stopped = true;
@@ -153,15 +153,18 @@ void DifferentialPosControl::updateSubscriptions()
_vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f;
}
if (_rover_position_setpoint_sub.updated()) {
rover_position_setpoint_s rover_position_setpoint;
_rover_position_setpoint_sub.copy(&rover_position_setpoint);
_start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]);
if (_surface_vehicle_position_setpoint_sub.updated()) {
surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint;
_surface_vehicle_position_setpoint_sub.copy(&surface_vehicle_position_setpoint);
_start_ned = Vector2f(surface_vehicle_position_setpoint.start_ned[0], surface_vehicle_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f;
_cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed :
_arrival_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.arrival_speed) ?
surface_vehicle_position_setpoint.arrival_speed : 0.f;
_cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ?
surface_vehicle_position_setpoint.cruising_speed :
_param_ro_speed_limit.get();
_target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]);
_target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0],
surface_vehicle_position_setpoint.position_ned[1]);
_stopped = false;
}
}