Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

rover: clean up velocity setpoint

Browse Source
This commit is contained in:
chfriedrich98
2025-04-29 15:43:00 +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
msg/AckermannVelocitySetpoint.msg
-4
View File
@@ -1,4 +0,0 @@
uint64 timestamp # time since system start (microseconds)
float32[2] velocity_ned # 2-dimensional velocity setpoint in NED frame [m/s]
bool backwards # Flag for backwards driving
msg/CMakeLists.txt
+1 -3
View File
@@ -37,7 +37,6 @@ cmake_policy(SET CMP0057 NEW)
include(px4_list_make_absolute) include(px4_list_make_absolute)
set(msg_files set(msg_files
AckermannVelocitySetpoint.msg
ActionRequest.msg ActionRequest.msg
ActuatorArmed.msg ActuatorArmed.msg
ActuatorControlsStatus.msg ActuatorControlsStatus.msg
@@ -64,7 +63,6 @@ set(msg_files
DebugValue.msg DebugValue.msg
DebugVect.msg DebugVect.msg
DifferentialPressure.msg DifferentialPressure.msg
DifferentialVelocitySetpoint.msg
DistanceSensor.msg DistanceSensor.msg
DistanceSensorModeChangeRequest.msg DistanceSensorModeChangeRequest.msg
Ekf2Timestamps.msg Ekf2Timestamps.msg
@@ -131,7 +129,6 @@ set(msg_files
ManualControlSwitches.msg ManualControlSwitches.msg
MavlinkLog.msg MavlinkLog.msg
MavlinkTunnel.msg MavlinkTunnel.msg
MecanumVelocitySetpoint.msg
MessageFormatRequest.msg MessageFormatRequest.msg
MessageFormatResponse.msg MessageFormatResponse.msg
Mission.msg Mission.msg
@@ -181,6 +178,7 @@ set(msg_files
RoverRateStatus.msg RoverRateStatus.msg
RoverSteeringSetpoint.msg RoverSteeringSetpoint.msg
RoverThrottleSetpoint.msg RoverThrottleSetpoint.msg
RoverVelocitySetpoint.msg
RoverVelocityStatus.msg RoverVelocityStatus.msg
Rpm.msg Rpm.msg
RtlStatus.msg RtlStatus.msg
msg/MecanumVelocitySetpoint.msg
-5
View File
@@ -1,5 +0,0 @@
uint64 timestamp # time since system start (microseconds)
float32 speed # [m/s] [-inf, inf] Speed setpoint
float32 bearing # [rad] [-pi, pi] from North.
float32 yaw # [rad] [-pi, pi] (Optional, defaults to current vehicle yaw) Vehicle yaw setpoint in NED frame
msg/DifferentialVelocitySetpoint.msg → msg/RoverVelocitySetpoint.msg
+1
View File
@@ -2,3 +2,4 @@ uint64 timestamp # time since system start (microseconds)
float32 speed # [m/s] [-inf, inf] Speed setpoint (Backwards driving if negative) 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.
float32 yaw # [rad] [-pi, pi] (Mecanum only, Optional, defaults to current vehicle yaw) Vehicle yaw setpoint in NED frame
src/modules/logger/logged_topics.cpp
+1 -3
View File
@@ -45,7 +45,6 @@ using namespace px4::logger;
void LoggedTopics::add_default_topics() void LoggedTopics::add_default_topics()
{ {
add_optional_topic("ackermann_velocity_setpoint", 100);
add_topic("action_request"); add_topic("action_request");
add_topic("actuator_armed"); add_topic("actuator_armed");
add_optional_topic("actuator_controls_status_0", 300); add_optional_topic("actuator_controls_status_0", 300);
@@ -58,7 +57,6 @@ void LoggedTopics::add_default_topics()
add_topic("commander_state"); add_topic("commander_state");
add_topic("config_overrides"); add_topic("config_overrides");
add_topic("cpuload"); add_topic("cpuload");
add_optional_topic("differential_velocity_setpoint", 100);
add_topic("distance_sensor_mode_change_request"); add_topic("distance_sensor_mode_change_request");
add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_attitude");
add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_global_position");
@@ -93,7 +91,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic("magnetometer_bias_estimate", 200); add_optional_topic("magnetometer_bias_estimate", 200);
add_topic("manual_control_setpoint", 200); add_topic("manual_control_setpoint", 200);
add_topic("manual_control_switches"); add_topic("manual_control_switches");
add_optional_topic("mecanum_velocity_setpoint", 100);
add_topic("mission_result"); add_topic("mission_result");
add_topic("navigator_mission_item"); add_topic("navigator_mission_item");
add_topic("navigator_status"); add_topic("navigator_status");
@@ -115,6 +112,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_rate_status", 100);
add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_steering_setpoint", 100);
add_optional_topic("rover_throttle_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100);
add_optional_topic("rover_velocity_setpoint", 100);
add_optional_topic("rover_velocity_status", 100); add_optional_topic("rover_velocity_status", 100);
add_topic("rtl_time_estimate", 1000); add_topic("rtl_time_estimate", 1000);
add_topic("rtl_status", 2000); add_topic("rtl_status", 2000);
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(); _pure_pursuit_status_pub.advertise();
_rover_position_setpoint_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 // Initially set to NaN to indicate that the rover has no position setpoint
_rover_position_setpoint.position_ned[0] = NAN; _rover_position_setpoint.position_ned[0] = NAN;
@@ -167,13 +167,12 @@ void AckermannPosControl::manualPositionMode()
if (fabsf(yaw_delta) > FLT_EPSILON if (fabsf(yaw_delta) > FLT_EPSILON
|| fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
_course_control = false; _course_control = false;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta); const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); rover_velocity_setpoint.speed = speed_setpoint;
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); rover_velocity_setpoint.bearing = yaw_setpoint;
ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line) } else { // Course control if the steering input is zero (keep driving on a straight line)
if (!_course_control) { if (!_course_control) {
@@ -189,16 +188,17 @@ void AckermannPosControl::manualPositionMode()
vector_scaling * _pos_ctl_course_direction; vector_scaling * _pos_ctl_course_direction;
pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp; pure_pursuit_status.timestamp = _timestamp;
const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), 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, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(speed_setpoint)); _curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); rover_velocity_setpoint.speed = speed_setpoint;
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON; bearing_setpoint + M_PI_F);
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
@@ -224,12 +224,11 @@ void AckermannPosControl::autoPositionMode()
} }
if (auto_stop) { if (auto_stop) {
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = 0.f; rover_velocity_setpoint.speed = 0.f;
ackermann_velocity_setpoint.velocity_ned[1] = 0.f; rover_velocity_setpoint.bearing = _vehicle_yaw;
ackermann_velocity_setpoint.backwards = false; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} else { // Regular guidance algorithm } else { // Regular guidance algorithm
const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, 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, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(speed_setpoint)); fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); rover_velocity_setpoint.speed = speed_setpoint;
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); rover_velocity_setpoint.bearing = yaw_setpoint;
ackermann_velocity_setpoint.backwards = false; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_ackermann_velocity_setpoint_pub.publish(ackermann_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, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(speed_setpoint)); _curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); rover_velocity_setpoint.speed = speed_setpoint;
ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); rover_velocity_setpoint.bearing = yaw_setpoint;
ackermann_velocity_setpoint.backwards = false; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} else { } else {
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = 0.f; rover_velocity_setpoint.speed = 0.f;
ackermann_velocity_setpoint.velocity_ned[1] = 0.f; rover_velocity_setpoint.bearing = _vehicle_yaw;
ackermann_velocity_setpoint.backwards = false; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} }
} }
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
+5 -5
View File
@@ -50,7 +50,7 @@
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/rover_position_setpoint.h> #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/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h> #include <uORB/topics/trajectory_setpoint.h>
@@ -181,10 +181,10 @@ private:
rover_position_setpoint_s _rover_position_setpoint{}; rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications // uORB publications
uORB::Publication<ackermann_velocity_setpoint_s> _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_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<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<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_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
// Variables // Variables
hrt_abstime _timestamp{0}; 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_throttle_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise(); _rover_velocity_status_pub.advertise();
_ackermann_velocity_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise();
updateParams(); updateParams();
} }
@@ -128,31 +128,22 @@ void AckermannVelControl::generateVelocitySetpoint()
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
ackermann_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
ackermann_velocity_setpoint.velocity_ned[0] = velocity_in_local_frame(0); rover_velocity_setpoint.speed = velocity_in_local_frame.norm();
ackermann_velocity_setpoint.velocity_ned[1] = velocity_in_local_frame(1); rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
ackermann_velocity_setpoint.backwards = false; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
} }
} }
void AckermannVelControl::generateAttitudeAndThrottleSetpoint() void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
{ {
if (_ackermann_velocity_setpoint_sub.updated()) { if (_rover_velocity_setpoint_sub.updated()) {
_ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint); _rover_velocity_setpoint_sub.copy(&_rover_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;
} }
// Attitude 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_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp; rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
@@ -161,15 +152,13 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
} else { } else {
rover_attitude_setpoint_s rover_attitude_setpoint{}; rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp; rover_attitude_setpoint.timestamp = _timestamp;
const float yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;
rover_attitude_setpoint.yaw_setpoint = _ackermann_velocity_setpoint.backwards ? matrix::wrap_pi(
yaw_setpoint + M_PI_F) : yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} }
// Throttle Setpoint // Throttle Setpoint
const float speed_magnitude = math::min(velocity_ned.norm(), _param_ro_speed_limit.get()); const float speed_setpoint = math::constrain(_rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
const float speed_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude; _param_ro_speed_limit.get());
rover_throttle_setpoint_s rover_throttle_setpoint{}; rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp; rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, 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/Publication.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/rover_throttle_setpoint.h> #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_velocity_status.h>
#include <uORB/topics/rover_attitude_setpoint.h> #include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.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 _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; 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{}; vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_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_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_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<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)}; uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
ackermann_velocity_setpoint_s _ackermann_velocity_setpoint{}; rover_velocity_setpoint_s _rover_velocity_setpoint{};
// Variables // Variables
hrt_abstime _timestamp{0}; hrt_abstime _timestamp{0};
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+37 -37
View File
@@ -37,7 +37,7 @@ using namespace time_literals;
DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent) DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent)
{ {
_differential_velocity_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise();
_pure_pursuit_status_pub.advertise(); _pure_pursuit_status_pub.advertise();
@@ -159,20 +159,20 @@ void DifferentialPosControl::manualPositionMode()
if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot
_course_control = false; _course_control = false;
const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = 0.f; rover_velocity_setpoint.speed = 0.f;
differential_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else if (fabsf(bearing_delta) > FLT_EPSILON) { // Closed loop yaw rate control } else if (fabsf(bearing_delta) > FLT_EPSILON) { // Closed loop yaw rate control
_course_control = false; _course_control = false;
const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_setpoint; rover_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line) } else { // Course control if the steering input is zero (keep driving on a straight line)
if (!_course_control) { if (!_course_control) {
@@ -192,12 +192,12 @@ void DifferentialPosControl::manualPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, fabsf(speed_setpoint)); _curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_setpoint; rover_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
bearing_setpoint + M_PI_F); bearing_setpoint + M_PI_F);
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
@@ -232,11 +232,11 @@ void DifferentialPosControl::autoPositionMode()
} }
if (auto_stop) { if (auto_stop) {
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = 0.f; rover_velocity_setpoint.speed = 0.f;
differential_velocity_setpoint.bearing = _vehicle_yaw; rover_velocity_setpoint.bearing = _vehicle_yaw;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { } else {
const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
@@ -248,11 +248,11 @@ void DifferentialPosControl::autoPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
fabsf(speed_setpoint)); fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_setpoint; rover_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
@@ -302,18 +302,18 @@ void DifferentialPosControl::goToPositionMode()
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(speed_setpoint)); _curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = speed_setpoint; rover_velocity_setpoint.speed = speed_setpoint;
differential_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { } else {
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = 0.f; rover_velocity_setpoint.speed = 0.f;
differential_velocity_setpoint.bearing = _vehicle_yaw; rover_velocity_setpoint.bearing = _vehicle_yaw;
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
+6 -6
View File
@@ -47,7 +47,7 @@
// uORB includes // uORB includes
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/differential_velocity_setpoint.h> #include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/pure_pursuit_status.h> #include <uORB/topics/pure_pursuit_status.h>
#include <uORB/topics/rover_position_setpoint.h> #include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/rover_velocity_status.h> #include <uORB/topics/rover_velocity_status.h>
@@ -152,7 +152,7 @@ private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _differential_velocity_setpoint_sub{ORB_ID(differential_velocity_setpoint)}; uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{}; vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{}; offboard_control_mode_s _offboard_control_mode{};
@@ -160,10 +160,10 @@ private:
// uORB publications // uORB publications
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<differential_velocity_setpoint_s> _differential_velocity_setpoint_pub{ORB_ID(differential_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<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_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
// Variables // Variables
hrt_abstime _timestamp{0}; hrt_abstime _timestamp{0};
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp
+11 -11
View File
@@ -39,7 +39,7 @@ DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModulePar
{ {
_rover_throttle_setpoint_pub.advertise(); _rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise();
_differential_velocity_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise(); _rover_velocity_status_pub.advertise();
updateParams(); updateParams();
} }
@@ -125,28 +125,28 @@ void DifferentialVelControl::generateVelocitySetpoint()
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
differential_velocity_setpoint_s differential_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
differential_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
differential_velocity_setpoint.speed = velocity_in_local_frame.norm(); rover_velocity_setpoint.speed = velocity_in_local_frame.norm();
differential_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
void DifferentialVelControl::generateAttitudeAndThrottleSetpoint() void DifferentialVelControl::generateAttitudeAndThrottleSetpoint()
{ {
if (_differential_velocity_setpoint_sub.updated()) { if (_rover_velocity_setpoint_sub.updated()) {
_differential_velocity_setpoint_sub.copy(&_differential_velocity_setpoint); _rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
} }
// Attitude Setpoint // Attitude Setpoint
rover_attitude_setpoint_s rover_attitude_setpoint{}; rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp; rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _differential_velocity_setpoint.bearing; rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
// Throttle Setpoint // Throttle Setpoint
const float heading_error = matrix::wrap_pi(_differential_velocity_setpoint.bearing - _vehicle_yaw); const float heading_error = matrix::wrap_pi(_rover_velocity_setpoint.bearing - _vehicle_yaw);
if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
_current_state = DrivingState::SPOT_TURNING; _current_state = DrivingState::SPOT_TURNING;
@@ -158,7 +158,7 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint()
float speed_setpoint = 0.f; float speed_setpoint = 0.f;
if (_current_state == DrivingState::DRIVING) { if (_current_state == DrivingState::DRIVING) {
speed_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(), speed_setpoint = math::constrain(_rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get()); _param_ro_speed_limit.get());
const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint, const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint,
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp
+4 -4
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/rover_steering_setpoint.h> #include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h> #include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_status.h> #include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/differential_velocity_setpoint.h> #include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h> #include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h> #include <uORB/topics/trajectory_setpoint.h>
@@ -121,7 +121,7 @@ private:
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _differential_velocity_setpoint_sub{ORB_ID(differential_velocity_setpoint)}; uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{}; vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{}; offboard_control_mode_s _offboard_control_mode{};
@@ -131,8 +131,8 @@ private:
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; 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_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<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<differential_velocity_setpoint_s> _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)}; uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
differential_velocity_setpoint_s _differential_velocity_setpoint{}; rover_velocity_setpoint_s _rover_velocity_setpoint{};
// Variables // Variables
hrt_abstime _timestamp{0}; hrt_abstime _timestamp{0};
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) MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent)
{ {
_mecanum_velocity_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise();
_rover_position_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise();
_pure_pursuit_status_pub.advertise(); _pure_pursuit_status_pub.advertise();
@@ -158,12 +158,12 @@ void MecanumPosControl::manualPositionMode()
_pos_ctl_yaw_setpoint = NAN; _pos_ctl_yaw_setpoint = NAN;
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta); const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
const Vector3f velocity_setpoint_local = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body); const Vector3f velocity_setpoint_local = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_setpoint_body.norm(); rover_velocity_setpoint.speed = velocity_setpoint_body.norm();
mecanum_velocity_setpoint.bearing = atan2f(velocity_setpoint_local(1), velocity_setpoint_local(0)); rover_velocity_setpoint.bearing = atan2f(velocity_setpoint_local(1), velocity_setpoint_local(0));
mecanum_velocity_setpoint.yaw = yaw_setpoint; rover_velocity_setpoint.yaw = yaw_setpoint;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { // Course control if the steering input is zero (keep driving on a straight line) } 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); 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, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, velocity_magnitude_setpoint); _curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_magnitude_setpoint; rover_velocity_setpoint.speed = velocity_magnitude_setpoint;
mecanum_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint; rover_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
@@ -241,12 +241,12 @@ void MecanumPosControl::autoPositionMode()
} }
if (auto_stop) { if (auto_stop) {
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = 0.f; rover_velocity_setpoint.speed = 0.f;
mecanum_velocity_setpoint.bearing = 0.f; rover_velocity_setpoint.bearing = 0.f;
mecanum_velocity_setpoint.yaw = _vehicle_yaw; rover_velocity_setpoint.yaw = _vehicle_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { // Regular guidance algorithm } else { // Regular guidance algorithm
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), 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, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
velocity_magnitude); velocity_magnitude);
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_magnitude; rover_velocity_setpoint.speed = velocity_magnitude;
mecanum_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
mecanum_velocity_setpoint.yaw = _auto_yaw; rover_velocity_setpoint.yaw = _auto_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _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, _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(speed_setpoint)); _curr_pos_ned, fabsf(speed_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status); _pure_pursuit_status_pub.publish(pure_pursuit_status);
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = speed_setpoint; rover_velocity_setpoint.speed = speed_setpoint;
mecanum_velocity_setpoint.bearing = bearing_setpoint; rover_velocity_setpoint.bearing = bearing_setpoint;
mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint; rover_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} else { } else {
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = 0.f; rover_velocity_setpoint.speed = 0.f;
mecanum_velocity_setpoint.bearing = 0.f; rover_velocity_setpoint.bearing = 0.f;
mecanum_velocity_setpoint.yaw = _vehicle_yaw; rover_velocity_setpoint.yaw = _vehicle_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _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 // uORB includes
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/Subscription.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/rover_position_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
@@ -162,7 +162,7 @@ private:
rover_position_setpoint_s _rover_position_setpoint{}; rover_position_setpoint_s _rover_position_setpoint{};
// uORB publications // 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<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_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_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise();
_mecanum_velocity_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise(); _rover_velocity_status_pub.advertise();
updateParams(); updateParams();
} }
@@ -131,27 +131,27 @@ void MecanumVelControl::generateVelocitySetpoint()
const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; rover_velocity_setpoint_s rover_velocity_setpoint{};
mecanum_velocity_setpoint.timestamp = _timestamp; rover_velocity_setpoint.timestamp = _timestamp;
mecanum_velocity_setpoint.speed = velocity_in_local_frame.norm(); rover_velocity_setpoint.speed = velocity_in_local_frame.norm();
mecanum_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
mecanum_velocity_setpoint.yaw = _vehicle_yaw; rover_velocity_setpoint.yaw = _vehicle_yaw;
_mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
} }
} }
void MecanumVelControl::generateAttitudeAndThrottleSetpoint() void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
{ {
if (_mecanum_velocity_setpoint_sub.updated()) { if (_rover_velocity_setpoint_sub.updated()) {
_mecanum_velocity_setpoint_sub.copy(&_mecanum_velocity_setpoint); _rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
} }
// Attitude 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_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp; 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); _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
_last_attitude_setpoint_update = _timestamp; _last_attitude_setpoint_update = _timestamp;
@@ -167,10 +167,10 @@ void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
float speed_body_x_setpoint{0.f}; float speed_body_x_setpoint{0.f};
float speed_body_y_setpoint{0.f}; float speed_body_y_setpoint{0.f};
if (fabsf(_mecanum_velocity_setpoint.speed) > FLT_EPSILON) { if (fabsf(_rover_velocity_setpoint.speed) > FLT_EPSILON) {
const Vector3f velocity_in_local_frame(_mecanum_velocity_setpoint.speed * cosf( const Vector3f velocity_in_local_frame(_rover_velocity_setpoint.speed * cosf(
_mecanum_velocity_setpoint.bearing), _rover_velocity_setpoint.bearing),
_mecanum_velocity_setpoint.speed * sinf(_mecanum_velocity_setpoint.bearing), 0.f); _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); 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_x_setpoint = velocity_in_body_frame(0);
speed_body_y_setpoint = velocity_in_body_frame(1); 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_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h> #include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_status.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/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/trajectory_setpoint.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 _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; 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_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{}; 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_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_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<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)}; uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
mecanum_velocity_setpoint_s _mecanum_velocity_setpoint{}; rover_velocity_setpoint_s _rover_velocity_setpoint{};
// Variables // Variables
hrt_abstime _timestamp{0}; hrt_abstime _timestamp{0};