diff --git a/msg/AckermannVelocitySetpoint.msg b/msg/AckermannVelocitySetpoint.msg deleted file mode 100644 index 4567255782..0000000000 --- a/msg/AckermannVelocitySetpoint.msg +++ /dev/null @@ -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 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index be9e55f0f9..6c374f2c35 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -37,7 +37,6 @@ cmake_policy(SET CMP0057 NEW) include(px4_list_make_absolute) set(msg_files - AckermannVelocitySetpoint.msg ActionRequest.msg ActuatorArmed.msg ActuatorControlsStatus.msg @@ -64,7 +63,6 @@ set(msg_files DebugValue.msg DebugVect.msg DifferentialPressure.msg - DifferentialVelocitySetpoint.msg DistanceSensor.msg DistanceSensorModeChangeRequest.msg Ekf2Timestamps.msg @@ -131,7 +129,6 @@ set(msg_files ManualControlSwitches.msg MavlinkLog.msg MavlinkTunnel.msg - MecanumVelocitySetpoint.msg MessageFormatRequest.msg MessageFormatResponse.msg Mission.msg @@ -181,6 +178,7 @@ set(msg_files RoverRateStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg + RoverVelocitySetpoint.msg RoverVelocityStatus.msg Rpm.msg RtlStatus.msg diff --git a/msg/MecanumVelocitySetpoint.msg b/msg/MecanumVelocitySetpoint.msg deleted file mode 100644 index 0af422f313..0000000000 --- a/msg/MecanumVelocitySetpoint.msg +++ /dev/null @@ -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 diff --git a/msg/DifferentialVelocitySetpoint.msg b/msg/RoverVelocitySetpoint.msg similarity index 60% rename from msg/DifferentialVelocitySetpoint.msg rename to msg/RoverVelocitySetpoint.msg index 2cd19cf77b..83ec173798 100644 --- a/msg/DifferentialVelocitySetpoint.msg +++ b/msg/RoverVelocitySetpoint.msg @@ -2,3 +2,4 @@ uint64 timestamp # time since system start (microseconds) float32 speed # [m/s] [-inf, inf] Speed setpoint (Backwards driving if negative) 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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 89bb5f0d75..544ecae871 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -45,7 +45,6 @@ using namespace px4::logger; void LoggedTopics::add_default_topics() { - add_optional_topic("ackermann_velocity_setpoint", 100); add_topic("action_request"); add_topic("actuator_armed"); add_optional_topic("actuator_controls_status_0", 300); @@ -58,7 +57,6 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); - add_optional_topic("differential_velocity_setpoint", 100); add_topic("distance_sensor_mode_change_request"); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); @@ -93,7 +91,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("magnetometer_bias_estimate", 200); add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); - add_optional_topic("mecanum_velocity_setpoint", 100); add_topic("mission_result"); add_topic("navigator_mission_item"); add_topic("navigator_status"); @@ -115,6 +112,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100); + add_optional_topic("rover_velocity_setpoint", 100); add_optional_topic("rover_velocity_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index 64ec81d94b..077760b8c4 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -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); } } diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index e566e49513..8f9e37eb65 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include @@ -181,10 +181,10 @@ private: rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications - uORB::Publication _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)}; - uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp index 46b86fbf96..3b6f0fa500 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp @@ -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, diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp index deac0edd62..74dea1693c 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -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_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)}; - ackermann_velocity_setpoint_s _ackermann_velocity_setpoint{}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + rover_velocity_setpoint_s _rover_velocity_setpoint{}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index 446bdcb9d0..f2f37d0a0a 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -37,7 +37,7 @@ using namespace time_literals; DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent) { - _differential_velocity_setpoint_pub.advertise(); + _rover_velocity_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); _pure_pursuit_status_pub.advertise(); @@ -159,20 +159,20 @@ void DifferentialPosControl::manualPositionMode() if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot _course_control = false; const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = 0.f; - differential_velocity_setpoint.bearing = bearing_setpoint; - _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); + rover_velocity_setpoint_s rover_velocity_setpoint{}; + rover_velocity_setpoint.timestamp = _timestamp; + rover_velocity_setpoint.speed = 0.f; + rover_velocity_setpoint.bearing = bearing_setpoint; + _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); } else if (fabsf(bearing_delta) > FLT_EPSILON) { // Closed loop yaw rate control _course_control = false; const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_setpoint; - differential_velocity_setpoint.bearing = bearing_setpoint; - _differential_velocity_setpoint_pub.publish(differential_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_pub.publish(rover_velocity_setpoint); } else { // Course control if the steering input is zero (keep driving on a straight line) 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, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_setpoint; - differential_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( - bearing_setpoint + M_PI_F); - _differential_velocity_setpoint_pub.publish(differential_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); } } @@ -232,11 +232,11 @@ void DifferentialPosControl::autoPositionMode() } if (auto_stop) { - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = 0.f; - differential_velocity_setpoint.bearing = _vehicle_yaw; - _differential_velocity_setpoint_pub.publish(differential_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 { 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, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_setpoint; - differential_velocity_setpoint.bearing = bearing_setpoint; - _differential_velocity_setpoint_pub.publish(differential_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_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, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_setpoint; - differential_velocity_setpoint.bearing = bearing_setpoint; - _differential_velocity_setpoint_pub.publish(differential_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_pub.publish(rover_velocity_setpoint); } else { - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = 0.f; - differential_velocity_setpoint.bearing = _vehicle_yaw; - _differential_velocity_setpoint_pub.publish(differential_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); } } diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 05ac34c7f7..4cf34ac7f3 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -47,7 +47,7 @@ // uORB includes #include #include -#include +#include #include #include #include @@ -152,7 +152,7 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; 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)}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; @@ -160,10 +160,10 @@ private: // uORB publications - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)}; - uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp index 0ce9113bc0..912510f83b 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp @@ -39,7 +39,7 @@ DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModulePar { _rover_throttle_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _differential_velocity_setpoint_pub.advertise(); + _rover_velocity_setpoint_pub.advertise(); _rover_velocity_status_pub.advertise(); updateParams(); } @@ -125,28 +125,28 @@ void DifferentialVelControl::generateVelocitySetpoint() const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = velocity_in_local_frame.norm(); - differential_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_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 DifferentialVelControl::generateAttitudeAndThrottleSetpoint() { - if (_differential_velocity_setpoint_sub.updated()) { - _differential_velocity_setpoint_sub.copy(&_differential_velocity_setpoint); + if (_rover_velocity_setpoint_sub.updated()) { + _rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint); } // Attitude Setpoint rover_attitude_setpoint_s rover_attitude_setpoint{}; 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); // 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()) { _current_state = DrivingState::SPOT_TURNING; @@ -158,7 +158,7 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint() float speed_setpoint = 0.f; 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()); const float speed_setpoint_normalized = math::interpolate(speed_setpoint, diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp index 42150a49a4..c962382653 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include @@ -121,7 +121,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 _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)}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; @@ -131,8 +131,8 @@ private: uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)}; - differential_velocity_setpoint_s _differential_velocity_setpoint{}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + rover_velocity_setpoint_s _rover_velocity_setpoint{}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index 6e2146be7b..284725d59f 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -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); } } diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index 3d98b39e98..40a4e955a8 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -49,7 +49,7 @@ // uORB includes #include #include -#include +#include #include #include #include @@ -162,7 +162,7 @@ private: rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications - uORB::Publication _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp index 894a5cfe12..e33ea5cafc 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp +++ b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp @@ -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); diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp index a124af06a0..9dd433b3b4 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp +++ b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include @@ -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_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)}; - mecanum_velocity_setpoint_s _mecanum_velocity_setpoint{}; + uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + rover_velocity_setpoint_s _rover_velocity_setpoint{}; // Variables hrt_abstime _timestamp{0};