fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 04:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: clean up velocity setpoint

Browse Source
This commit is contained in:
chfriedrich98
2025-05-13 10:23:35 +02:00
committed by chfriedrich98
parent b727ce86a0
commit 1857920a5f
17 changed files with 177 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
+37 -37
View File
@@ -37,7 +37,7 @@ using namespace time_literals;
MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent)
{
_mecanum_velocity_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise();
_pure_pursuit_status_pub.advertise();
@@ -158,12 +158,12 @@ void MecanumPosControl::manualPositionMode()
_pos_ctl_yaw_setpoint = NAN;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
const Vector3f velocity_setpoint_local = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_setpoint_body.norm();
mecanum_velocity_setpoint.bearing = atan2f(velocity_setpoint_local(1), velocity_setpoint_local(0));
mecanum_velocity_setpoint.yaw = yaw_setpoint;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = velocity_setpoint_body.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_setpoint_local(1), velocity_setpoint_local(0));
rover_velocity_setpoint.yaw = yaw_setpoint;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line)
const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f);
@@ -194,12 +194,12 @@ void MecanumPosControl::manualPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_magnitude_setpoint;
mecanum_velocity_setpoint.bearing = bearing_setpoint;
mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = velocity_magnitude_setpoint;
rover_velocity_setpoint.bearing = bearing_setpoint;
rover_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
@@ -241,12 +241,12 @@ void MecanumPosControl::autoPositionMode()
}
if (auto_stop) {
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = 0.f;
mecanum_velocity_setpoint.bearing = 0.f;
mecanum_velocity_setpoint.yaw = _vehicle_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = 0.f;
rover_velocity_setpoint.yaw = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { // Regular guidance algorithm
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
@@ -258,12 +258,12 @@ void MecanumPosControl::autoPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
velocity_magnitude);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_magnitude;
mecanum_velocity_setpoint.bearing = bearing_setpoint;
mecanum_velocity_setpoint.yaw = _auto_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = velocity_magnitude;
rover_velocity_setpoint.bearing = bearing_setpoint;
rover_velocity_setpoint.yaw = _auto_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
@@ -313,20 +313,20 @@ void MecanumPosControl::goToPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = speed_setpoint;
mecanum_velocity_setpoint.bearing = bearing_setpoint;
mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = bearing_setpoint;
rover_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else {
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = 0.f;
mecanum_velocity_setpoint.bearing = 0.f;
mecanum_velocity_setpoint.yaw = _vehicle_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = 0.f;
rover_velocity_setpoint.yaw = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
+2 -2
View File
@@ -49,7 +49,7 @@
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/mecanum_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -162,7 +162,7 @@ private:
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<mecanum_velocity_setpoint_s> _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp
+15 -15
View File
@@ -39,7 +39,7 @@ MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent
{
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_mecanum_velocity_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
updateParams();
}
@@ -131,27 +131,27 @@ void MecanumVelControl::generateVelocitySetpoint()
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_in_local_frame.norm();
mecanum_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
mecanum_velocity_setpoint.yaw = _vehicle_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = velocity_in_local_frame.norm();
rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
rover_velocity_setpoint.yaw = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
{
if (_mecanum_velocity_setpoint_sub.updated()) {
_mecanum_velocity_setpoint_sub.copy(&_mecanum_velocity_setpoint);
if (_rover_velocity_setpoint_sub.updated()) {
_rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
}
// Attitude Setpoint
if (PX4_ISFINITE(_mecanum_velocity_setpoint.yaw)) {
if (PX4_ISFINITE(_rover_velocity_setpoint.yaw)) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _mecanum_velocity_setpoint.yaw;
rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
_last_attitude_setpoint_update = _timestamp;
@@ -167,10 +167,10 @@ void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
float speed_body_x_setpoint{0.f};
float speed_body_y_setpoint{0.f};
if (fabsf(_mecanum_velocity_setpoint.speed) > FLT_EPSILON) {
const Vector3f velocity_in_local_frame(_mecanum_velocity_setpoint.speed * cosf(
_mecanum_velocity_setpoint.bearing),
_mecanum_velocity_setpoint.speed * sinf(_mecanum_velocity_setpoint.bearing), 0.f);
if (fabsf(_rover_velocity_setpoint.speed) > FLT_EPSILON) {
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);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
speed_body_x_setpoint = velocity_in_body_frame(0);
speed_body_y_setpoint = velocity_in_body_frame(1);
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp
+4 -4
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/mecanum_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
@@ -113,7 +113,7 @@ private:
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _mecanum_velocity_setpoint_sub{ORB_ID(mecanum_velocity_setpoint)};
uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
@@ -124,8 +124,8 @@ private:
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<mecanum_velocity_setpoint_s> _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)};
mecanum_velocity_setpoint_s _mecanum_velocity_setpoint{};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
rover_velocity_setpoint_s _rover_velocity_setpoint{};
// Variables
hrt_abstime _timestamp{0};