fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:33:33 +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_ackermann/AckermannPosControl/AckermannPosControl.cpp
+37 -41
View File
@@ -39,7 +39,7 @@ AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(pa
{
_pure_pursuit_status_pub.advertise();
_rover_position_setpoint_pub.advertise();
_ackermann_velocity_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
// Initially set to NaN to indicate that the rover has no position setpoint
_rover_position_setpoint.position_ned[0] = NAN;
@@ -167,13 +167,12 @@ void AckermannPosControl::manualPositionMode()
if (fabsf(yaw_delta) > FLT_EPSILON
|| fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
_course_control = false;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = 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)
if (!_course_control) {
@@ -189,16 +188,17 @@ void AckermannPosControl::manualPositionMode()
vector_scaling * _pos_ctl_course_direction;
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(speed_setpoint));
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
bearing_setpoint + M_PI_F);
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
@@ -224,12 +224,11 @@ void AckermannPosControl::autoPositionMode()
}
if (auto_stop) {
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = 0.f;
ackermann_velocity_setpoint.velocity_ned[1] = 0.f;
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { // Regular guidance algorithm
const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp,
@@ -242,12 +241,11 @@ void AckermannPosControl::autoPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = yaw_setpoint;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
@@ -372,20 +370,18 @@ void AckermannPosControl::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);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint);
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint);
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = speed_setpoint;
rover_velocity_setpoint.bearing = yaw_setpoint;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else {
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = 0.f;
ackermann_velocity_setpoint.velocity_ned[1] = 0.f;
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint{};
rover_velocity_setpoint.timestamp = _timestamp;
rover_velocity_setpoint.speed = 0.f;
rover_velocity_setpoint.bearing = _vehicle_yaw;
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
}
}
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
+5 -5
View File
@@ -50,7 +50,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/ackermann_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>
@@ -181,10 +181,10 @@ private:
rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications
uORB::Publication<ackermann_velocity_setpoint_s> _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
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)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
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)};
// Variables
hrt_abstime _timestamp{0};
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp
+12 -23
View File
@@ -39,7 +39,7 @@ AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(pa
{
_rover_throttle_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_ackermann_velocity_setpoint_pub.advertise();
_rover_velocity_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
updateParams();
}
@@ -128,31 +128,22 @@ void AckermannVelControl::generateVelocitySetpoint()
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = velocity_in_local_frame(0);
ackermann_velocity_setpoint.velocity_ned[1] = velocity_in_local_frame(1);
ackermann_velocity_setpoint.backwards = false;
_ackermann_velocity_setpoint_pub.publish(ackermann_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_pub.publish(rover_velocity_setpoint);
}
}
void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
{
if (_ackermann_velocity_setpoint_sub.updated()) {
_ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint);
}
const Vector2f velocity_ned = Vector2f(_ackermann_velocity_setpoint.velocity_ned[0],
_ackermann_velocity_setpoint.velocity_ned[1]);
// Santitize input
if (!velocity_ned.isAllFinite()) {
return;
if (_rover_velocity_setpoint_sub.updated()) {
_rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
}
// Attitude Setpoint
if (velocity_ned.norm() < FLT_EPSILON) {
if (fabsf(_rover_velocity_setpoint.speed) < FLT_EPSILON) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
@@ -161,15 +152,13 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
} else {
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
const float yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
rover_attitude_setpoint.yaw_setpoint = _ackermann_velocity_setpoint.backwards ? matrix::wrap_pi(
yaw_setpoint + M_PI_F) : yaw_setpoint;
rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
// Throttle Setpoint
const float speed_magnitude = math::min(velocity_ned.norm(), _param_ro_speed_limit.get());
const float speed_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude;
const float speed_setpoint = math::constrain(_rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp
+4 -4
View File
@@ -48,7 +48,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/ackermann_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -112,7 +112,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 _ackermann_velocity_setpoint_sub{ORB_ID(ackermann_velocity_setpoint)};
uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
@@ -120,8 +120,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<ackermann_velocity_setpoint_s> _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)};
ackermann_velocity_setpoint_s _ackermann_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};