diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 38d6074d53..0c2906a1c8 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -106,15 +106,15 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); - add_optional_topic("rover_attitude_setpoint", 100); - add_optional_topic("rover_attitude_status", 100); - add_optional_topic("rover_position_setpoint", 100); - add_optional_topic("rover_rate_setpoint", 100); - add_optional_topic("rover_rate_status", 100); - add_optional_topic("rover_speed_setpoint", 100); - add_optional_topic("rover_speed_status", 100); - add_optional_topic("rover_steering_setpoint", 100); - add_optional_topic("rover_throttle_setpoint", 100); + add_optional_topic("surface_vehicle_attitude_setpoint", 100); + add_optional_topic("surface_vehicle_attitude_status", 100); + add_optional_topic("surface_vehicle_position_setpoint", 100); + add_optional_topic("surface_vehicle_rate_setpoint", 100); + add_optional_topic("surface_vehicle_rate_status", 100); + add_optional_topic("surface_vehicle_speed_setpoint", 100); + add_optional_topic("surface_vehicle_speed_status", 100); + add_optional_topic("surface_vehicle_steering_setpoint", 100); + add_optional_topic("surface_vehicle_throttle_setpoint", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp index 391b197bd1..159feeb7f3 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp @@ -60,10 +60,10 @@ void AckermannActControl::updateActControl() const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; // Motor control - if (_rover_throttle_setpoint_sub.updated()) { - rover_throttle_setpoint_s rover_throttle_setpoint{}; - _rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint); - _throttle_setpoint = rover_throttle_setpoint.throttle_body_x; + if (_surface_vehicle_throttle_setpoint_sub.updated()) { + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + _surface_vehicle_throttle_setpoint_sub.copy(&surface_vehicle_throttle_setpoint); + _throttle_setpoint = surface_vehicle_throttle_setpoint.throttle_body_x; } if (PX4_ISFINITE(_throttle_setpoint)) { @@ -80,10 +80,10 @@ void AckermannActControl::updateActControl() } // Servo control - if (_rover_steering_setpoint_sub.updated()) { - rover_steering_setpoint_s rover_steering_setpoint{}; - _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); - _steering_setpoint = rover_steering_setpoint.normalized_steering_setpoint; + if (_surface_vehicle_steering_setpoint_sub.updated()) { + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + _surface_vehicle_steering_setpoint_sub.copy(&surface_vehicle_steering_setpoint); + _steering_setpoint = surface_vehicle_steering_setpoint.normalized_steering_setpoint; } if (PX4_ISFINITE(_steering_setpoint)) { diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp index fb3630c13c..a63266c02e 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp @@ -46,8 +46,8 @@ #include #include #include -#include -#include +#include +#include /** * @brief Class for ackermann actuator control. @@ -63,7 +63,7 @@ public: ~AckermannActControl() = default; /** - * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + * @brief Generate and publish actuatorMotors/actuatorServos setpoints from SurfaceVehicleThrottleSetpoint/SurfaceVehicleSteeringSetpoint. */ void updateActControl(); @@ -83,8 +83,8 @@ private: // uORB subscriptions uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; - uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _surface_vehicle_steering_setpoint_sub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Subscription _surface_vehicle_throttle_setpoint_sub{ORB_ID(surface_vehicle_throttle_setpoint)}; // uORB publications uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp index faaf6a0c37..2e85e5f6c7 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; AckermannAttControl::AckermannAttControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_status_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_status_pub.advertise(); updateParams(); } @@ -76,19 +76,19 @@ void AckermannAttControl::updateAttControl() float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, _vehicle_yaw, _yaw_setpoint, dt); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = _timestamp; + surface_vehicle_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } // Publish attitude controller status (logging only) - rover_attitude_status_s rover_attitude_status; - rover_attitude_status.timestamp = _timestamp; - rover_attitude_status.measured_yaw = _vehicle_yaw; - rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); - _rover_attitude_status_pub.publish(rover_attitude_status); + surface_vehicle_attitude_status_s surface_vehicle_attitude_status; + surface_vehicle_attitude_status.timestamp = _timestamp; + surface_vehicle_attitude_status.measured_yaw = _vehicle_yaw; + surface_vehicle_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); + _surface_vehicle_attitude_status_pub.publish(surface_vehicle_attitude_status); } @@ -109,10 +109,10 @@ void AckermannAttControl::updateSubscriptions() -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); } - if (_rover_attitude_setpoint_sub.updated()) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - _rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint); - _yaw_setpoint = rover_attitude_setpoint.yaw_setpoint; + if (_surface_vehicle_attitude_setpoint_sub.updated()) { + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + _surface_vehicle_attitude_setpoint_sub.copy(&surface_vehicle_attitude_setpoint); + _yaw_setpoint = surface_vehicle_attitude_setpoint.yaw_setpoint; } } diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp index 2d58bae4ce..78f674299f 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -46,10 +46,10 @@ // uORB includes #include #include -#include +#include #include -#include -#include +#include +#include #include /** @@ -66,7 +66,7 @@ public: ~AckermannAttControl() = default; /** - * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + * @brief Generate and publish SurfaceVehicleRateSetpoint from SurfaceVehicleAttitudeSetpoint. */ void updateAttControl(); @@ -96,11 +96,11 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; - uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _surface_vehicle_attitude_setpoint_sub{ORB_ID(surface_vehicle_attitude_setpoint)}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_status_pub{ORB_ID(surface_vehicle_attitude_status)}; // Variables float _vehicle_yaw{0.f}; diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp index 0b8a5d0c75..b9a98d49c4 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp @@ -38,7 +38,7 @@ using namespace time_literals; AckermannAutoMode::AckermannAutoMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void AckermannAutoMode::updateParams() @@ -70,17 +70,18 @@ void AckermannAutoMode::autoControl() updateWaypointsAndAcceptanceRadius(); - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = _curr_wp_ned(0); - rover_position_setpoint.position_ned[1] = _curr_wp_ned(1); - rover_position_setpoint.start_ned[0] = _prev_wp_ned(0); - rover_position_setpoint.start_ned[1] = _prev_wp_ned(1); - rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type, - _waypoint_transition_angle, _max_yaw_rate); - rover_position_setpoint.cruising_speed = _cruising_speed; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = _curr_wp_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = _curr_wp_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = _prev_wp_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = _prev_wp_ned(1); + surface_vehicle_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, + _curr_wp_type, + _waypoint_transition_angle, _max_yaw_rate); + surface_vehicle_position_setpoint.cruising_speed = _cruising_speed; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } } diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp index 91e05477e7..0b0397f5ba 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include /** * @brief Class for ackermann auto mode. @@ -62,7 +62,7 @@ public: ~AckermannAutoMode() = default; /** - * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet. + * @brief Generate and publish SurfaceVehiclePositionSetpoint from positionSetpointTriplet. */ void autoControl(); @@ -110,7 +110,7 @@ private: uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; // uORB publications - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; // Variables diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp index f3eea3f910..a9b97b0df9 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp @@ -38,12 +38,12 @@ using namespace time_literals; AckermannManualMode::AckermannManualMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_throttle_setpoint_pub.advertise(); - _rover_steering_setpoint_pub.advertise(); - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void AckermannManualMode::updateParams() @@ -56,32 +56,32 @@ void AckermannManualMode::manual() { manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = hrt_absolute_time(); - rover_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.roll; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.roll; + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } void AckermannManualMode::acro() { manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * - math::superexpo - (manual_control_setpoint.roll, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * + math::superexpo + (manual_control_setpoint.roll, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } void AckermannManualMode::stab() @@ -95,39 +95,39 @@ void AckermannManualMode::stab() manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON - || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { + || fabsf(surface_vehicle_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { _stab_yaw_setpoint = NAN; // Rate control - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * - math::superexpo(math::deadzone(manual_control_setpoint.roll, - _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * + math::superexpo(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoint invalid - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = NAN; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else { // Heading control if (!PX4_ISFINITE(_stab_yaw_setpoint)) { _stab_yaw_setpoint = _vehicle_yaw; } - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } } @@ -164,35 +164,35 @@ void AckermannManualMode::position() _pos_ctl_course_direction = Vector2f(NAN, NAN); // Speed control - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = hrt_absolute_time(); - rover_speed_setpoint.speed_body_x = speed_setpoint; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); // Rate control - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * - math::superexpo(math::deadzone(manual_control_setpoint.roll, - _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * + math::superexpo(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoints invalid - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = NAN; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = NAN; - rover_position_setpoint.position_ned[1] = NAN; - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = NAN; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = NAN; + surface_vehicle_position_setpoint.position_ned[1] = NAN; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } else { // Course control if (!_pos_ctl_course_direction.isAllFinite()) { @@ -205,16 +205,16 @@ void AckermannManualMode::position() const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) * vector_scaling * _pos_ctl_course_direction; - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = target_waypoint_ned(0); - rover_position_setpoint.position_ned[1] = target_waypoint_ned(1); - rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0); - rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = speed_setpoint; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = target_waypoint_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = target_waypoint_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = speed_setpoint; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } } diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp index 6216ed17ff..9c50a6a70c 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp @@ -47,12 +47,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include /** * @brief Class for ackermann manual mode. @@ -68,22 +68,22 @@ public: ~AckermannManualMode() = default; /** - * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + * @brief Publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleSteeringSetpoint from manualControlSetpoint. */ void manual(); /** - * @brief Generate and publish roverThrottleSetpoint and RoverRateSetpoint from manualControlSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleRateSetpoint from manualControlSetpoint. */ void acro(); /** - * @brief Generate and publish roverThrottleSetpoint and RoverAttitudeSetpoint from manualControlSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleAttitudeSetpoint from manualControlSetpoint. */ void stab(); /** - * @brief Generate and publish roverSpeedSetpoint/roverRateSetpoint or roverPositionSetpoint from manualControlSetpoint. + * @brief Generate and publish SurfaceVehicleSpeedSetpoint/SurfaceVehicleRateSetpoint or SurfaceVehiclePositionSetpoint from manualControlSetpoint. */ void position(); @@ -105,12 +105,12 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // uORB publications - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; // Variables MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp index ccf7f7ba36..8586e0888b 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp @@ -38,9 +38,9 @@ using namespace time_literals; AckermannOffboardMode::AckermannOffboardMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_speed_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); } void AckermannOffboardMode::updateParams() @@ -57,26 +57,26 @@ void AckermannOffboardMode::offboardControl() _trajectory_setpoint_sub.copy(&trajectory_setpoint); if (offboard_control_mode.position) { - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; - rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.cruising_speed = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; + surface_vehicle_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } else if (offboard_control_mode.velocity) { const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = hrt_absolute_time(); - rover_speed_setpoint.speed_body_x = velocity_ned.norm(); - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = velocity_ned.norm(); + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } } diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp index 573289e300..8f8937b582 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp @@ -43,9 +43,9 @@ // uORB includes #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -81,7 +81,7 @@ private: uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; // uORB publications - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; }; diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index dc8f2ae136..94e5dddf27 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -38,8 +38,8 @@ using namespace time_literals; AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(parent) { _pure_pursuit_status_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); updateParams(); } @@ -88,25 +88,25 @@ void AckermannPosControl::updatePosControl() } _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = timestamp; - rover_speed_setpoint.speed_body_x = speed_setpoint; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( - bearing_setpoint + M_PI_F); - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( + bearing_setpoint + M_PI_F); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else { - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = timestamp; - rover_speed_setpoint.speed_body_x = 0.f; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = 0.f; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { _stopped = true; @@ -147,15 +147,18 @@ void AckermannPosControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_position_setpoint_sub.updated()) { - rover_position_setpoint_s rover_position_setpoint; - _rover_position_setpoint_sub.copy(&rover_position_setpoint); - _start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]); + if (_surface_vehicle_position_setpoint_sub.updated()) { + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint; + _surface_vehicle_position_setpoint_sub.copy(&surface_vehicle_position_setpoint); + _start_ned = Vector2f(surface_vehicle_position_setpoint.start_ned[0], surface_vehicle_position_setpoint.start_ned[1]); _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f; - _cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed : + _arrival_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.arrival_speed) ? + surface_vehicle_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? + surface_vehicle_position_setpoint.cruising_speed : _param_ro_speed_limit.get(); - _target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]); + _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], + surface_vehicle_position_setpoint.position_ned[1]); _stopped = false; } diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index e67f83440a..4db169d6ad 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -45,9 +45,9 @@ // uORB includes #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -69,7 +69,7 @@ public: ~AckermannPosControl() = default; /** - * @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint. + * @brief Generate and publish SurfaceVehicleSpeedSetpoint and SurfaceVehicleAttitudeSetpoint from SurfaceVehiclePositionSetpoint. */ void updatePosControl(); @@ -99,12 +99,12 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; + uORB::Subscription _surface_vehicle_position_setpoint_sub{ORB_ID(surface_vehicle_position_setpoint)}; uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; // uORB publications - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp index 5f0f6ee5ee..269040c0b0 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_steering_setpoint_pub.advertise(); - _rover_rate_status_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_status_pub.advertise(); updateParams(); } @@ -95,29 +95,29 @@ void AckermannRateControl::updateRateControl() steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, dt); } - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_steering_setpoint = math::interpolate(steering_setpoint, + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = _timestamp; + surface_vehicle_steering_setpoint.normalized_steering_setpoint = math::interpolate(steering_setpoint, -_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); } else { _pid_yaw_rate.resetIntegral(); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_steering_setpoint = 0.f; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = _timestamp; + surface_vehicle_steering_setpoint.normalized_steering_setpoint = 0.f; + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); } } // Publish rate controller status (logging only) - rover_rate_status_s rover_rate_status; - rover_rate_status.timestamp = _timestamp; - rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; - rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); - rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_rate_status_pub.publish(rover_rate_status); + surface_vehicle_rate_status_s surface_vehicle_rate_status; + surface_vehicle_rate_status.timestamp = _timestamp; + surface_vehicle_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + surface_vehicle_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + surface_vehicle_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _surface_vehicle_rate_status_pub.publish(surface_vehicle_rate_status); } @@ -139,10 +139,10 @@ void AckermannRateControl::updateSubscriptions() _estimated_speed = fabsf(_estimated_speed) > _param_ro_speed_th.get() ? _estimated_speed : 0.f; } - if (_rover_rate_setpoint_sub.updated()) { - rover_rate_setpoint_s rover_rate_setpoint{}; - _rover_rate_setpoint_sub.copy(&rover_rate_setpoint); - _yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint; + if (_surface_vehicle_rate_setpoint_sub.updated()) { + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + _surface_vehicle_rate_setpoint_sub.copy(&surface_vehicle_rate_setpoint); + _yaw_rate_setpoint = surface_vehicle_rate_setpoint.yaw_rate_setpoint; } } diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp index 4a3ef1b2d6..925b0e68d9 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -46,10 +46,10 @@ // uORB includes #include #include -#include +#include #include -#include -#include +#include +#include #include /** @@ -66,7 +66,7 @@ public: ~AckermannRateControl() = default; /** - * @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint. + * @brief Generate and publish SurfaceVehicleSteeringSetpoint from SurfaceVehicleRateSetpoint. */ void updateRateControl(); @@ -94,13 +94,13 @@ private: void updateSubscriptions(); // uORB subscriptions - uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _surface_vehicle_rate_setpoint_sub{ORB_ID(surface_vehicle_rate_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; // uORB publications - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_status_pub{ORB_ID(surface_vehicle_rate_status)}; // Variables float _estimated_speed{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed] diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp index 4dd76e889b..a669d27de3 100644 --- a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; AckermannSpeedControl::AckermannSpeedControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_throttle_setpoint_pub.advertise(); - _rover_speed_status_pub.advertise(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_speed_status_pub.advertise(); updateParams(); } @@ -69,25 +69,25 @@ void AckermannSpeedControl::updateSpeedControl() if (PX4_ISFINITE(_speed_setpoint)) { const float speed_setpoint = math::constrain(_speed_setpoint, -_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(_adjusted_speed_setpoint, _pid_speed, + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = _timestamp; + surface_vehicle_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); - rover_throttle_setpoint.throttle_body_y = NAN; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + surface_vehicle_throttle_setpoint.throttle_body_y = NAN; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } // Publish speed controller status (logging only) - rover_speed_status_s rover_speed_status; - rover_speed_status.timestamp = _timestamp; - rover_speed_status.measured_speed_body_x = _vehicle_speed; - rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); - rover_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); - rover_speed_status.measured_speed_body_y = NAN; - rover_speed_status.adjusted_speed_body_y_setpoint = NAN; - rover_speed_status.pid_throttle_body_y_integral = NAN; - _rover_speed_status_pub.publish(rover_speed_status); + surface_vehicle_speed_status_s surface_vehicle_speed_status; + surface_vehicle_speed_status.timestamp = _timestamp; + surface_vehicle_speed_status.measured_speed_body_x = _vehicle_speed; + surface_vehicle_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); + surface_vehicle_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + surface_vehicle_speed_status.measured_speed_body_y = NAN; + surface_vehicle_speed_status.adjusted_speed_body_y_setpoint = NAN; + surface_vehicle_speed_status.pid_throttle_body_y_integral = NAN; + _surface_vehicle_speed_status_pub.publish(surface_vehicle_speed_status); } void AckermannSpeedControl::updateSubscriptions() @@ -107,10 +107,10 @@ void AckermannSpeedControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_speed_setpoint_sub.updated()) { - rover_speed_setpoint_s rover_speed_setpoint; - _rover_speed_setpoint_sub.copy(&rover_speed_setpoint); - _speed_setpoint = rover_speed_setpoint.speed_body_x; + if (_surface_vehicle_speed_setpoint_sub.updated()) { + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint; + _surface_vehicle_speed_setpoint_sub.copy(&surface_vehicle_speed_setpoint); + _speed_setpoint = surface_vehicle_speed_setpoint.speed_body_x; } } diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp index 70b6192e4f..e65ef2da59 100644 --- a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp @@ -47,9 +47,9 @@ // uORB includes #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -69,7 +69,7 @@ public: ~AckermannSpeedControl() = default; /** - * @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint from SurfaceVehicleSpeedSetpoint. */ void updateSpeedControl(); @@ -99,11 +99,11 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)}; + uORB::Subscription _surface_vehicle_speed_setpoint_sub{ORB_ID(surface_vehicle_speed_setpoint)}; // uORB publications - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_speed_status_pub{ORB_ID(rover_speed_status)}; + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_speed_status_pub{ORB_ID(surface_vehicle_speed_status)}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp index f460aefa3a..c44951fc68 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp @@ -56,16 +56,16 @@ void DifferentialActControl::updateActControl() const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; // Motor control - if (_rover_throttle_setpoint_sub.updated()) { - rover_throttle_setpoint_s rover_throttle_setpoint{}; - _rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint); - _throttle_setpoint = rover_throttle_setpoint.throttle_body_x; + if (_surface_vehicle_throttle_setpoint_sub.updated()) { + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + _surface_vehicle_throttle_setpoint_sub.copy(&surface_vehicle_throttle_setpoint); + _throttle_setpoint = surface_vehicle_throttle_setpoint.throttle_body_x; } - if (_rover_steering_setpoint_sub.updated()) { - rover_steering_setpoint_s rover_steering_setpoint{}; - _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); - _speed_diff_setpoint = rover_steering_setpoint.normalized_steering_setpoint; + if (_surface_vehicle_steering_setpoint_sub.updated()) { + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + _surface_vehicle_steering_setpoint_sub.copy(&surface_vehicle_steering_setpoint); + _speed_diff_setpoint = surface_vehicle_steering_setpoint.normalized_steering_setpoint; } if (PX4_ISFINITE(_throttle_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) { diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp index 2076542e4a..30c84a5cdf 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp @@ -45,8 +45,8 @@ #include #include #include -#include -#include +#include +#include #include /** @@ -63,7 +63,7 @@ public: ~DifferentialActControl() = default; /** - * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + * @brief Generate and publish actuatorMotors setpoints from SurfaceVehicleThrottleSetpoint/SurfaceVehicleSteeringSetpoint. */ void updateActControl(); @@ -89,8 +89,8 @@ private: // uORB subscriptions uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; - uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _surface_vehicle_steering_setpoint_sub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Subscription _surface_vehicle_throttle_setpoint_sub{ORB_ID(surface_vehicle_throttle_setpoint)}; // uORB publications uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp index cbb86944b1..332c7bdd07 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; DifferentialAttControl::DifferentialAttControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_status_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_status_pub.advertise(); updateParams(); } @@ -72,28 +72,28 @@ void DifferentialAttControl::updateAttControl() _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } - if (_rover_attitude_setpoint_sub.updated()) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - _rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint); - _yaw_setpoint = rover_attitude_setpoint.yaw_setpoint; + if (_surface_vehicle_attitude_setpoint_sub.updated()) { + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + _surface_vehicle_attitude_setpoint_sub.copy(&surface_vehicle_attitude_setpoint); + _yaw_setpoint = surface_vehicle_attitude_setpoint.yaw_setpoint; } if (PX4_ISFINITE(_yaw_setpoint)) { const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, _vehicle_yaw, _yaw_setpoint, dt); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = _timestamp; + surface_vehicle_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } // Publish attitude controller status (logging only) - rover_attitude_status_s rover_attitude_status; - rover_attitude_status.timestamp = _timestamp; - rover_attitude_status.measured_yaw = _vehicle_yaw; - rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); - _rover_attitude_status_pub.publish(rover_attitude_status); + surface_vehicle_attitude_status_s surface_vehicle_attitude_status; + surface_vehicle_attitude_status.timestamp = _timestamp; + surface_vehicle_attitude_status.measured_yaw = _vehicle_yaw; + surface_vehicle_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); + _surface_vehicle_attitude_status_pub.publish(surface_vehicle_attitude_status); } diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp index 205d1d3d0e..c48176c923 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -47,10 +47,10 @@ // uORB includes #include #include -#include -#include +#include +#include #include -#include +#include /** * @brief Class for differential attitude control. @@ -66,7 +66,7 @@ public: ~DifferentialAttControl() = default; /** - * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + * @brief Generate and publish SurfaceVehicleRateSetpoint from SurfaceVehicleAttitudeSetpoint. */ void updateAttControl(); @@ -90,11 +90,11 @@ protected: private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _surface_vehicle_attitude_setpoint_sub{ORB_ID(surface_vehicle_attitude_setpoint)}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_status_pub{ORB_ID(surface_vehicle_attitude_status)}; // Variables float _vehicle_yaw{0.f}; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp index cb2c54152b..a2c4e8d81b 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp @@ -38,7 +38,7 @@ using namespace time_literals; DifferentialAutoMode::DifferentialAutoMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void DifferentialAutoMode::updateParams() @@ -78,17 +78,17 @@ void DifferentialAutoMode::autoControl() float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = curr_wp_ned(0); - rover_position_setpoint.position_ned[1] = curr_wp_ned(1); - rover_position_setpoint.start_ned[0] = prev_wp_ned(0); - rover_position_setpoint.start_ned[1] = prev_wp_ned(1); - rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type); - rover_position_setpoint.cruising_speed = cruising_speed; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = curr_wp_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = curr_wp_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = prev_wp_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = prev_wp_ned(1); + surface_vehicle_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, + _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type); + surface_vehicle_position_setpoint.cruising_speed = cruising_speed; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } } diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp index 92cbce1653..dcd3f5c97d 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include /** * @brief Class for differential auto mode. @@ -61,7 +61,7 @@ public: ~DifferentialAutoMode() = default; /** - * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet. + * @brief Generate and publish SurfaceVehiclePositionSetpoint from positionSetpointTriplet. */ void autoControl(); @@ -91,7 +91,7 @@ private: uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; // uORB publications - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; DEFINE_PARAMETERS( (ParamFloat) _param_ro_speed_limit, diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp index f44ae3a703..db38ddfda0 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp @@ -38,12 +38,12 @@ using namespace time_literals; DifferentialManualMode::DifferentialManualMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_throttle_setpoint_pub.advertise(); - _rover_steering_setpoint_pub.advertise(); - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void DifferentialManualMode::updateParams() @@ -56,32 +56,32 @@ void DifferentialManualMode::manual() { manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = hrt_absolute_time(); - rover_steering_setpoint.normalized_steering_setpoint = _param_rd_yaw_stk_gain.get() * math::superexpo + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_steering_setpoint.normalized_steering_setpoint = _param_rd_yaw_stk_gain.get() * math::superexpo (manual_control_setpoint.roll, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } void DifferentialManualMode::acro() { manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(manual_control_setpoint.roll, - _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(manual_control_setpoint.roll, + _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } void DifferentialManualMode::stab() @@ -95,38 +95,38 @@ void DifferentialManualMode::stab() manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON - || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { + || fabsf(surface_vehicle_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { _stab_yaw_setpoint = NAN; // Rate control - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( - manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( + manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoint invalid - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = NAN; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else { // Heading control if (!PX4_ISFINITE(_stab_yaw_setpoint)) { _stab_yaw_setpoint = _vehicle_yaw; } - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } } @@ -156,34 +156,34 @@ void DifferentialManualMode::position() _pos_ctl_course_direction = Vector2f(NAN, NAN); // Speed control - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = hrt_absolute_time(); - rover_speed_setpoint.speed_body_x = speed_setpoint; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); // Rate control - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( - manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( + manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoints invalid - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = NAN; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = NAN; - rover_position_setpoint.position_ned[1] = NAN; - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = NAN; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = NAN; + surface_vehicle_position_setpoint.position_ned[1] = NAN; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } else { // Course control if (!_pos_ctl_course_direction.isAllFinite()) { @@ -196,16 +196,16 @@ void DifferentialManualMode::position() const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) * vector_scaling * _pos_ctl_course_direction; - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = target_waypoint_ned(0); - rover_position_setpoint.position_ned[1] = target_waypoint_ned(1); - rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0); - rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = speed_setpoint; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = target_waypoint_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = target_waypoint_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = speed_setpoint; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } } diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp index cd297b4ce6..85bee48ff1 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp @@ -46,12 +46,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include using namespace matrix; @@ -69,12 +69,12 @@ public: ~DifferentialManualMode() = default; /** - * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + * @brief Publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleSteeringSetpoint from manualControlSetpoint. */ void manual(); /** - * @brief Generate and publish roverThrottleSetpoint/RoverRateSetpoint from manualControlSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint/SurfaceVehicleRateSetpoint from manualControlSetpoint. */ void acro(); @@ -106,12 +106,12 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // uORB publications - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; // Variables Vector2f _pos_ctl_course_direction{NAN, NAN}; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp index c00bcf5b1e..8ec63ec882 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp @@ -38,10 +38,10 @@ using namespace time_literals; DifferentialOffboardMode::DifferentialOffboardMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void DifferentialOffboardMode::updateParams() @@ -58,38 +58,38 @@ void DifferentialOffboardMode::offboardControl() _trajectory_setpoint_sub.copy(&trajectory_setpoint); if (offboard_control_mode.position) { - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; - rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.cruising_speed = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; + surface_vehicle_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } else if (offboard_control_mode.velocity) { const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = hrt_absolute_time(); - rover_speed_setpoint.speed_body_x = velocity_ned.norm(); - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = velocity_ned.norm(); + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else if (offboard_control_mode.attitude) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else if (offboard_control_mode.body_rate) { - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } } diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp index 7317862213..2ead81401c 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp @@ -43,10 +43,10 @@ // uORB includes #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -82,8 +82,8 @@ private: uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; }; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index d7807c2e8d..5e0bfb9977 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -38,8 +38,8 @@ using namespace time_literals; DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent) { _pure_pursuit_status_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); updateParams(); } @@ -97,25 +97,25 @@ void DifferentialPosControl::updatePosControl() speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed); } - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = timestamp; - rover_speed_setpoint.speed_body_x = speed_setpoint; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi( - yaw_setpoint + M_PI_F); - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi( + yaw_setpoint + M_PI_F); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else { - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = timestamp; - rover_speed_setpoint.speed_body_x = 0.f; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = 0.f; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { _stopped = true; @@ -153,15 +153,18 @@ void DifferentialPosControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_position_setpoint_sub.updated()) { - rover_position_setpoint_s rover_position_setpoint; - _rover_position_setpoint_sub.copy(&rover_position_setpoint); - _start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]); + if (_surface_vehicle_position_setpoint_sub.updated()) { + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint; + _surface_vehicle_position_setpoint_sub.copy(&surface_vehicle_position_setpoint); + _start_ned = Vector2f(surface_vehicle_position_setpoint.start_ned[0], surface_vehicle_position_setpoint.start_ned[1]); _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f; - _cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed : + _arrival_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.arrival_speed) ? + surface_vehicle_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? + surface_vehicle_position_setpoint.cruising_speed : _param_ro_speed_limit.get(); - _target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]); + _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], + surface_vehicle_position_setpoint.position_ned[1]); _stopped = false; } } diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index fde8a25515..99cd5a6d1a 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -45,10 +45,10 @@ // uORB includes #include #include -#include -#include +#include +#include #include -#include +#include #include #include @@ -76,7 +76,7 @@ public: ~DifferentialPosControl() = default; /** - * @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint. + * @brief Generate and publish SurfaceVehicleSpeedSetpoint and SurfaceVehicleAttitudeSetpoint from SurfaceVehiclePositionSetpoint. */ void updatePosControl(); @@ -106,12 +106,12 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; + uORB::Subscription _surface_vehicle_position_setpoint_sub{ORB_ID(surface_vehicle_position_setpoint)}; // uORB publications - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; // Variables Quatf _vehicle_attitude_quaternion{}; diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp index 9559b4dbf9..ea2a59b427 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; DifferentialRateControl::DifferentialRateControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_steering_setpoint_pub.advertise(); - _rover_rate_status_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_status_pub.advertise(); updateParams(); } @@ -68,10 +68,10 @@ void DifferentialRateControl::updateRateControl() vehicle_angular_velocity.xyz[2] : 0.f; } - if (_rover_rate_setpoint_sub.updated()) { - rover_rate_setpoint_s rover_rate_setpoint{}; - _rover_rate_setpoint_sub.copy(&rover_rate_setpoint); - _yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint; + if (_surface_vehicle_rate_setpoint_sub.updated()) { + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + _surface_vehicle_rate_setpoint_sub.copy(&surface_vehicle_rate_setpoint); + _yaw_rate_setpoint = surface_vehicle_rate_setpoint.yaw_rate_setpoint; } if (PX4_ISFINITE(_yaw_rate_setpoint)) { @@ -81,22 +81,22 @@ void DifferentialRateControl::updateRateControl() yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_steering_setpoint = speed_diff_normalized; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = _timestamp; + surface_vehicle_steering_setpoint.normalized_steering_setpoint = speed_diff_normalized; + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); } else { _pid_yaw_rate.resetIntegral(); } // Publish rate controller status (logging only) - rover_rate_status_s rover_rate_status; - rover_rate_status.timestamp = _timestamp; - rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; - rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); - rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_rate_status_pub.publish(rover_rate_status); + surface_vehicle_rate_status_s surface_vehicle_rate_status; + surface_vehicle_rate_status.timestamp = _timestamp; + surface_vehicle_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + surface_vehicle_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + surface_vehicle_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _surface_vehicle_rate_status_pub.publish(surface_vehicle_rate_status); } diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp index 83dc2f8dfa..535282fffd 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -46,9 +46,9 @@ // uORB includes #include #include -#include -#include -#include +#include +#include +#include #include /** @@ -65,7 +65,7 @@ public: ~DifferentialRateControl() = default; /** - * @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint. + * @brief Generate and publish SurfaceVehicleSteeringSetpoint from SurfaceVehicleRateSetpoint. */ void updateRateControl(); @@ -89,12 +89,12 @@ protected: private: // uORB subscriptions - uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _surface_vehicle_rate_setpoint_sub{ORB_ID(surface_vehicle_rate_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; // uORB publications - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_status_pub{ORB_ID(surface_vehicle_rate_status)}; // Variables float _vehicle_yaw_rate{0.f}; diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp index ee6c0c0339..5707691b17 100644 --- a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; DifferentialSpeedControl::DifferentialSpeedControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_throttle_setpoint_pub.advertise(); - _rover_speed_status_pub.advertise(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_speed_status_pub.advertise(); updateParams(); } @@ -68,25 +68,25 @@ void DifferentialSpeedControl::updateSpeedControl() // Throttle Setpoint if (PX4_ISFINITE(_speed_setpoint)) { const float speed_setpoint = calcSpeedSetpoint(); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = _timestamp; + surface_vehicle_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } // Publish speed controller status (logging only) - rover_speed_status_s rover_speed_status; - rover_speed_status.timestamp = _timestamp; - rover_speed_status.measured_speed_body_x = _vehicle_speed; - rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); - rover_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); - rover_speed_status.measured_speed_body_y = NAN; - rover_speed_status.adjusted_speed_body_y_setpoint = NAN; - rover_speed_status.pid_throttle_body_y_integral = NAN; - _rover_speed_status_pub.publish(rover_speed_status); + surface_vehicle_speed_status_s surface_vehicle_speed_status; + surface_vehicle_speed_status.timestamp = _timestamp; + surface_vehicle_speed_status.measured_speed_body_x = _vehicle_speed; + surface_vehicle_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); + surface_vehicle_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + surface_vehicle_speed_status.measured_speed_body_y = NAN; + surface_vehicle_speed_status.adjusted_speed_body_y_setpoint = NAN; + surface_vehicle_speed_status.pid_throttle_body_y_integral = NAN; + _surface_vehicle_speed_status_pub.publish(surface_vehicle_speed_status); } void DifferentialSpeedControl::updateSubscriptions() @@ -106,10 +106,10 @@ void DifferentialSpeedControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_speed_setpoint_sub.updated()) { - rover_speed_setpoint_s rover_speed_setpoint; - _rover_speed_setpoint_sub.copy(&rover_speed_setpoint); - _speed_setpoint = rover_speed_setpoint.speed_body_x; + if (_surface_vehicle_speed_setpoint_sub.updated()) { + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint; + _surface_vehicle_speed_setpoint_sub.copy(&surface_vehicle_speed_setpoint); + _speed_setpoint = surface_vehicle_speed_setpoint.speed_body_x; } } @@ -121,10 +121,10 @@ float DifferentialSpeedControl::calcSpeedSetpoint() const float speed_setpoint_normalized = math::interpolate(speed_setpoint, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); - if (_rover_steering_setpoint_sub.updated()) { - rover_steering_setpoint_s rover_steering_setpoint{}; - _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); - _normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint; + if (_surface_vehicle_steering_setpoint_sub.updated()) { + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + _surface_vehicle_steering_setpoint_sub.copy(&surface_vehicle_steering_setpoint); + _normalized_speed_diff = surface_vehicle_steering_setpoint.normalized_steering_setpoint; } if (fabsf(speed_setpoint_normalized) > 1.f - fabsf( diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp index 158b421d9b..3454794709 100644 --- a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp @@ -47,10 +47,10 @@ // uORB includes #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -70,7 +70,7 @@ public: ~DifferentialSpeedControl() = default; /** - * @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint from SurfaceVehicleSpeedSetpoint. */ void updateSpeedControl(); @@ -106,12 +106,12 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _surface_vehicle_speed_setpoint_sub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Subscription _surface_vehicle_steering_setpoint_sub{ORB_ID(surface_vehicle_steering_setpoint)}; // uORB publications - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_speed_status_pub{ORB_ID(rover_speed_status)}; + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_speed_status_pub{ORB_ID(surface_vehicle_speed_status)}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp index 203aadc365..379820d420 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp @@ -57,17 +57,17 @@ void MecanumActControl::updateActControl() const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; // Motor control - if (_rover_throttle_setpoint_sub.updated()) { - rover_throttle_setpoint_s rover_throttle_setpoint{}; - _rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint); - _throttle_x_setpoint = rover_throttle_setpoint.throttle_body_x; - _throttle_y_setpoint = rover_throttle_setpoint.throttle_body_y; + if (_surface_vehicle_throttle_setpoint_sub.updated()) { + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + _surface_vehicle_throttle_setpoint_sub.copy(&surface_vehicle_throttle_setpoint); + _throttle_x_setpoint = surface_vehicle_throttle_setpoint.throttle_body_x; + _throttle_y_setpoint = surface_vehicle_throttle_setpoint.throttle_body_y; } - if (_rover_steering_setpoint_sub.updated()) { - rover_steering_setpoint_s rover_steering_setpoint{}; - _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); - _speed_diff_setpoint = rover_steering_setpoint.normalized_steering_setpoint; + if (_surface_vehicle_steering_setpoint_sub.updated()) { + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + _surface_vehicle_steering_setpoint_sub.copy(&surface_vehicle_steering_setpoint); + _speed_diff_setpoint = surface_vehicle_steering_setpoint.normalized_steering_setpoint; } if (PX4_ISFINITE(_throttle_x_setpoint) && PX4_ISFINITE(_throttle_y_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) { diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp index 63aa155004..8abca5907e 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp @@ -45,8 +45,8 @@ #include #include #include -#include -#include +#include +#include /** * @brief Class for mecanum actuator control. @@ -62,7 +62,7 @@ public: ~MecanumActControl() = default; /** - * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + * @brief Generate and publish actuatorMotors setpoints from SurfaceVehicleThrottleSetpoint/SurfaceVehicleSteeringSetpoint. */ void updateActControl(); @@ -89,8 +89,8 @@ private: // uORB subscriptions uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; - uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _surface_vehicle_steering_setpoint_sub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Subscription _surface_vehicle_throttle_setpoint_sub{ORB_ID(surface_vehicle_throttle_setpoint)}; // uORB publications uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 3d3a1cb6e6..643fc58601 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_status_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_status_pub.advertise(); updateParams(); } @@ -72,28 +72,28 @@ void MecanumAttControl::updateAttControl() _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } - if (_rover_attitude_setpoint_sub.updated()) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - _rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint); - _yaw_setpoint = rover_attitude_setpoint.yaw_setpoint; + if (_surface_vehicle_attitude_setpoint_sub.updated()) { + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + _surface_vehicle_attitude_setpoint_sub.copy(&surface_vehicle_attitude_setpoint); + _yaw_setpoint = surface_vehicle_attitude_setpoint.yaw_setpoint; } if (PX4_ISFINITE(_yaw_setpoint)) { const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, _vehicle_yaw, _yaw_setpoint, dt); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = _timestamp; + surface_vehicle_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } // Publish attitude controller status (logging only) - rover_attitude_status_s rover_attitude_status; - rover_attitude_status.timestamp = _timestamp; - rover_attitude_status.measured_yaw = _vehicle_yaw; - rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); - _rover_attitude_status_pub.publish(rover_attitude_status); + surface_vehicle_attitude_status_s surface_vehicle_attitude_status; + surface_vehicle_attitude_status.timestamp = _timestamp; + surface_vehicle_attitude_status.measured_yaw = _vehicle_yaw; + surface_vehicle_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); + _surface_vehicle_attitude_status_pub.publish(surface_vehicle_attitude_status); } diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index 327b4652ad..8833cf73d0 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -47,10 +47,10 @@ // uORB includes #include #include -#include +#include #include -#include -#include +#include +#include /** * @brief Class for mecanum attitude control. @@ -66,7 +66,7 @@ public: ~MecanumAttControl() = default; /** - * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + * @brief Generate and publish SurfaceVehicleRateSetpoint from SurfaceVehicleAttitudeSetpoint. */ void updateAttControl(); @@ -91,11 +91,11 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _surface_vehicle_attitude_setpoint_sub{ORB_ID(surface_vehicle_attitude_setpoint)}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_status_pub{ORB_ID(surface_vehicle_attitude_status)}; // Variables float _vehicle_yaw{0.f}; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp index 8591b42d25..2db1f91e9f 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp @@ -38,7 +38,7 @@ using namespace time_literals; MecanumAutoMode::MecanumAutoMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void MecanumAutoMode::updateParams() @@ -78,18 +78,18 @@ void MecanumAutoMode::autoControl() float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = curr_wp_ned(0); - rover_position_setpoint.position_ned[1] = curr_wp_ned(1); - rover_position_setpoint.start_ned[0] = prev_wp_ned(0); - rover_position_setpoint.start_ned[1] = prev_wp_ned(1); - rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type); - rover_position_setpoint.cruising_speed = cruising_speed; - rover_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ? - position_setpoint_triplet.current.yaw : NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = curr_wp_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = curr_wp_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = prev_wp_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = prev_wp_ned(1); + surface_vehicle_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, + _param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type); + surface_vehicle_position_setpoint.cruising_speed = cruising_speed; + surface_vehicle_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ? + position_setpoint_triplet.current.yaw : NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } } diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp index 66d8125d9f..f3dd02291d 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include /** * @brief Class for Mecanum auto mode. @@ -61,7 +61,7 @@ public: ~MecanumAutoMode() = default; /** - * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet. + * @brief Generate and publish SurfaceVehiclePositionSetpoint from positionSetpointTriplet. */ void autoControl(); @@ -90,7 +90,7 @@ private: uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; // uORB publications - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; DEFINE_PARAMETERS( (ParamFloat) _param_ro_speed_limit, diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp index 0bacf61f19..9bf7f3c38b 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp @@ -38,12 +38,12 @@ using namespace time_literals; MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_throttle_setpoint_pub.advertise(); - _rover_steering_setpoint_pub.advertise(); - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void MecanumManualMode::updateParams() @@ -56,32 +56,32 @@ void MecanumManualMode::manual() { manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = hrt_absolute_time(); - rover_steering_setpoint.normalized_steering_setpoint = _param_rm_yaw_stk_gain.get() * math::superexpo + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_steering_setpoint.normalized_steering_setpoint = _param_rm_yaw_stk_gain.get() * math::superexpo (manual_control_setpoint.yaw, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } void MecanumManualMode::acro() { manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(manual_control_setpoint.yaw, - _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(manual_control_setpoint.yaw, + _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } void MecanumManualMode::stab() @@ -95,37 +95,37 @@ void MecanumManualMode::stab() manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = hrt_absolute_time(); - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON) { _stab_yaw_setpoint = NAN; // Rate control - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( - manual_control_setpoint.yaw, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( + manual_control_setpoint.yaw, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoint invalid - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = NAN; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else { // Heading control if (!PX4_ISFINITE(_stab_yaw_setpoint)) { _stab_yaw_setpoint = _vehicle_yaw; } - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } } @@ -158,35 +158,35 @@ void MecanumManualMode::position() _pos_ctl_yaw_setpoint = NAN; // Speed control - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = hrt_absolute_time(); - rover_speed_setpoint.speed_body_x = velocity_setpoint_body(0); - rover_speed_setpoint.speed_body_y = velocity_setpoint_body(1); - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = velocity_setpoint_body(0); + surface_vehicle_speed_setpoint.speed_body_y = velocity_setpoint_body(1); + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); // Rate control - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( - manual_control_setpoint.yaw, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(math::deadzone( + manual_control_setpoint.yaw, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoints invalid - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = NAN; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = NAN; - rover_position_setpoint.position_ned[1] = NAN; - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = NAN; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = NAN; + surface_vehicle_position_setpoint.position_ned[1] = NAN; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } else { // Course control const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body); @@ -210,16 +210,16 @@ void MecanumManualMode::position() const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = target_waypoint_ned(0); - rover_position_setpoint.position_ned[1] = target_waypoint_ned(1); - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = velocity_setpoint_ned.norm(); - rover_position_setpoint.yaw = _pos_ctl_yaw_setpoint; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = target_waypoint_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = target_waypoint_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = velocity_setpoint_ned.norm(); + surface_vehicle_position_setpoint.yaw = _pos_ctl_yaw_setpoint; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } } diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp index 9f270fbd46..e726f258ed 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp @@ -46,12 +46,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include using namespace matrix; @@ -69,12 +69,12 @@ public: ~MecanumManualMode() = default; /** - * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + * @brief Publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleSteeringSetpoint from manualControlSetpoint. */ void manual(); /** - * @brief Generate and publish roverThrottleSetpoint/RoverRateSetpoint from manualControlSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint/SurfaceVehicleRateSetpoint from manualControlSetpoint. */ void acro(); @@ -106,12 +106,12 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // uORB publications - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; // Variables Vector2f _pos_ctl_course_direction{NAN, NAN}; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp index f217ada200..b09def5a2d 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp @@ -38,10 +38,10 @@ using namespace time_literals; MecanumOffboardMode::MecanumOffboardMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_rate_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_speed_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); } void MecanumOffboardMode::updateParams() @@ -58,16 +58,16 @@ void MecanumOffboardMode::offboardControl() _trajectory_setpoint_sub.copy(&trajectory_setpoint); if (offboard_control_mode.position) { - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; - rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.cruising_speed = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; + surface_vehicle_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); } else if (offboard_control_mode.velocity) { if (_vehicle_attitude_sub.updated()) { @@ -78,26 +78,26 @@ void MecanumOffboardMode::offboardControl() const Vector3f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1], 0.f); const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = hrt_absolute_time(); - rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0); - rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1); - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = velocity_in_body_frame(0); + surface_vehicle_speed_setpoint.speed_body_y = velocity_in_body_frame(1); + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else if (offboard_control_mode.attitude) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else if (offboard_control_mode.body_rate) { - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = hrt_absolute_time(); - rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } } diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp index 18cc930760..53969a7281 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp @@ -43,10 +43,10 @@ // uORB includes #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -84,10 +84,10 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; Quatf _vehicle_attitude_quaternion{}; }; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index 3ec9be67d3..d6515a1a33 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -37,9 +37,9 @@ using namespace time_literals; MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_speed_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); _pure_pursuit_status_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); updateParams(); } @@ -84,26 +84,26 @@ void MecanumPosControl::updatePosControl() speed_setpoint * sinf(bearing_setpoint), 0.f); const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = timestamp; - rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0); - rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1); - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = velocity_in_body_frame(0); + surface_vehicle_speed_setpoint.speed_body_y = velocity_in_body_frame(1); + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = _yaw_setpoint; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); } else { - rover_speed_setpoint_s rover_speed_setpoint{}; - rover_speed_setpoint.timestamp = timestamp; - rover_speed_setpoint.speed_body_x = 0.f; - rover_speed_setpoint.speed_body_y = 0.f; - _rover_speed_setpoint_pub.publish(rover_speed_setpoint); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = timestamp; - rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = 0.f; + surface_vehicle_speed_setpoint.speed_body_y = 0.f; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { _stopped = true; @@ -138,15 +138,18 @@ void MecanumPosControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_position_setpoint_sub.updated()) { - rover_position_setpoint_s rover_position_setpoint; - _rover_position_setpoint_sub.copy(&rover_position_setpoint); - _start_ned = Vector2f(rover_position_setpoint.start_ned[0], rover_position_setpoint.start_ned[1]); + if (_surface_vehicle_position_setpoint_sub.updated()) { + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint; + _surface_vehicle_position_setpoint_sub.copy(&surface_vehicle_position_setpoint); + _start_ned = Vector2f(surface_vehicle_position_setpoint.start_ned[0], surface_vehicle_position_setpoint.start_ned[1]); _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _arrival_speed = PX4_ISFINITE(rover_position_setpoint.arrival_speed) ? rover_position_setpoint.arrival_speed : 0.f; - _cruising_speed = PX4_ISFINITE(rover_position_setpoint.cruising_speed) ? rover_position_setpoint.cruising_speed : + _arrival_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.arrival_speed) ? + surface_vehicle_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? + surface_vehicle_position_setpoint.cruising_speed : _param_ro_speed_limit.get(); - _target_waypoint_ned = Vector2f(rover_position_setpoint.position_ned[0], rover_position_setpoint.position_ned[1]); + _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], + surface_vehicle_position_setpoint.position_ned[1]); _stopped = false; } } diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index 54880657ec..301da6791e 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -46,9 +46,9 @@ // uORB includes #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -69,7 +69,7 @@ public: ~MecanumPosControl() = default; /** - * @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint. + * @brief Generate and publish SurfaceVehicleSpeedSetpoint and SurfaceVehicleAttitudeSetpoint from SurfaceVehiclePositionSetpoint. */ void updatePosControl(); @@ -101,12 +101,12 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; + uORB::Subscription _surface_vehicle_position_setpoint_sub{ORB_ID(surface_vehicle_position_setpoint)}; // uORB publications - uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; // Variables Quatf _vehicle_attitude_quaternion{}; diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp index 8084380540..b2e80693f2 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_steering_setpoint_pub.advertise(); - _rover_rate_status_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_status_pub.advertise(); updateParams(); } @@ -68,10 +68,10 @@ void MecanumRateControl::updateRateControl() vehicle_angular_velocity.xyz[2] : 0.f; } - if (_rover_rate_setpoint_sub.updated()) { - rover_rate_setpoint_s rover_rate_setpoint{}; - _rover_rate_setpoint_sub.copy(&rover_rate_setpoint); - _yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint; + if (_surface_vehicle_rate_setpoint_sub.updated()) { + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + _surface_vehicle_rate_setpoint_sub.copy(&surface_vehicle_rate_setpoint); + _yaw_rate_setpoint = surface_vehicle_rate_setpoint.yaw_rate_setpoint; } if (PX4_ISFINITE(_yaw_rate_setpoint)) { @@ -81,22 +81,22 @@ void MecanumRateControl::updateRateControl() yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_steering_setpoint = speed_diff_normalized; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = _timestamp; + surface_vehicle_steering_setpoint.normalized_steering_setpoint = speed_diff_normalized; + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); } else { _pid_yaw_rate.resetIntegral(); } // Publish rate controller status (logging only) - rover_rate_status_s rover_rate_status; - rover_rate_status.timestamp = _timestamp; - rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; - rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); - rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_rate_status_pub.publish(rover_rate_status); + surface_vehicle_rate_status_s surface_vehicle_rate_status; + surface_vehicle_rate_status.timestamp = _timestamp; + surface_vehicle_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + surface_vehicle_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + surface_vehicle_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _surface_vehicle_rate_status_pub.publish(surface_vehicle_rate_status); } diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp index 15a4a12b8c..ed0d0bcdf6 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -46,10 +46,10 @@ // uORB includes #include #include -#include +#include #include -#include -#include +#include +#include /** * @brief Class for mecanum rate control. @@ -65,7 +65,7 @@ public: ~MecanumRateControl() = default; /** - * @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint. + * @brief Generate and publish SurfaceVehicleSteeringSetpoint from SurfaceVehicleRateSetpoint. */ void updateRateControl(); @@ -89,12 +89,12 @@ protected: private: // uORB subscriptions - uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _surface_vehicle_rate_setpoint_sub{ORB_ID(surface_vehicle_rate_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; // uORB publications - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_status_pub{ORB_ID(surface_vehicle_rate_status)}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp index eed2bd06d3..0afe207016 100644 --- a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp @@ -37,8 +37,8 @@ using namespace time_literals; MecanumSpeedControl::MecanumSpeedControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_throttle_setpoint_pub.advertise(); - _rover_speed_status_pub.advertise(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_speed_status_pub.advertise(); updateParams(); } @@ -72,29 +72,29 @@ void MecanumSpeedControl::updateSpeedControl() // Throttle Setpoints if (PX4_ISFINITE(_speed_x_setpoint) && PX4_ISFINITE(_speed_y_setpoint)) { Vector2f speed_setpoint = calcSpeedSetpoint(); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_x_setpoint, _pid_speed_x, + surface_vehicle_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_x_setpoint, _pid_speed_x, speed_setpoint(0), _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); - rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_adjusted_speed_y_setpoint, _pid_speed_y, + surface_vehicle_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_adjusted_speed_y_setpoint, _pid_speed_y, speed_setpoint(1), _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } // Publish position controller status (logging only) - rover_speed_status_s rover_speed_status; - rover_speed_status.timestamp = _timestamp; - rover_speed_status.measured_speed_body_x = _vehicle_speed_body_x; - rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState(); - rover_speed_status.measured_speed_body_y = _vehicle_speed_body_y; - rover_speed_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState(); - rover_speed_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); - rover_speed_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); - _rover_speed_status_pub.publish(rover_speed_status); + surface_vehicle_speed_status_s surface_vehicle_speed_status; + surface_vehicle_speed_status.timestamp = _timestamp; + surface_vehicle_speed_status.measured_speed_body_x = _vehicle_speed_body_x; + surface_vehicle_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState(); + surface_vehicle_speed_status.measured_speed_body_y = _vehicle_speed_body_y; + surface_vehicle_speed_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState(); + surface_vehicle_speed_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); + surface_vehicle_speed_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); + _surface_vehicle_speed_status_pub.publish(surface_vehicle_speed_status); } void MecanumSpeedControl::updateSubscriptions() @@ -115,20 +115,20 @@ void MecanumSpeedControl::updateSubscriptions() _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } - if (_rover_speed_setpoint_sub.updated()) { - rover_speed_setpoint_s rover_speed_setpoint; - _rover_speed_setpoint_sub.copy(&rover_speed_setpoint); - _speed_x_setpoint = rover_speed_setpoint.speed_body_x; - _speed_y_setpoint = rover_speed_setpoint.speed_body_y; + if (_surface_vehicle_speed_setpoint_sub.updated()) { + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint; + _surface_vehicle_speed_setpoint_sub.copy(&surface_vehicle_speed_setpoint); + _speed_x_setpoint = surface_vehicle_speed_setpoint.speed_body_x; + _speed_y_setpoint = surface_vehicle_speed_setpoint.speed_body_y; } } Vector2f MecanumSpeedControl::calcSpeedSetpoint() { - if (_rover_steering_setpoint_sub.updated()) { - rover_steering_setpoint_s rover_steering_setpoint{}; - _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); - _normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint; + if (_surface_vehicle_steering_setpoint_sub.updated()) { + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + _surface_vehicle_steering_setpoint_sub.copy(&surface_vehicle_steering_setpoint); + _normalized_speed_diff = surface_vehicle_steering_setpoint.normalized_steering_setpoint; } Vector2f speed_setpoint = Vector2f(_speed_x_setpoint, _speed_y_setpoint); diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp index 7d0b8b5517..479cd0446a 100644 --- a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp @@ -47,10 +47,10 @@ // uORB includes #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -70,7 +70,7 @@ public: ~MecanumSpeedControl() = default; /** - * @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint. + * @brief Generate and publish SurfaceVehicleThrottleSetpoint from SurfaceVehicleSpeedSetpoint. */ void updateSpeedControl(); @@ -106,12 +106,12 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _surface_vehicle_speed_setpoint_sub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Subscription _surface_vehicle_steering_setpoint_sub{ORB_ID(surface_vehicle_steering_setpoint)}; // uORB publications - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_speed_status_pub{ORB_ID(rover_speed_status)}; + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_speed_status_pub{ORB_ID(surface_vehicle_speed_status)}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index e315a7f1d5..94b2119449 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -196,23 +196,23 @@ subscriptions: - topic: /fmu/in/lateral_control_configuration type: px4_msgs::msg::LateralControlConfiguration - - topic: /fmu/in/rover_position_setpoint - type: px4_msgs::msg::RoverPositionSetpoint + - topic: /fmu/in/surface_vehicle_position_setpoint + type: px4_msgs::msg::SurfaceVehiclePositionSetpoint - - topic: /fmu/in/rover_speed_setpoint - type: px4_msgs::msg::RoverSpeedSetpoint + - topic: /fmu/in/surface_vehicle_speed_setpoint + type: px4_msgs::msg::SurfaceVehicleSpeedSetpoint - - topic: /fmu/in/rover_attitude_setpoint - type: px4_msgs::msg::RoverAttitudeSetpoint + - topic: /fmu/in/surface_vehicle_attitude_setpoint + type: px4_msgs::msg::SurfaceVehicleAttitudeSetpoint - - topic: /fmu/in/rover_rate_setpoint - type: px4_msgs::msg::RoverRateSetpoint + - topic: /fmu/in/surface_vehicle_rate_setpoint + type: px4_msgs::msg::SurfaceVehicleRateSetpoint - - topic: /fmu/in/rover_throttle_setpoint - type: px4_msgs::msg::RoverThrottleSetpoint + - topic: /fmu/in/surface_vehicle_throttle_setpoint + type: px4_msgs::msg::SurfaceVehicleThrottleSetpoint - - topic: /fmu/in/rover_steering_setpoint - type: px4_msgs::msg::RoverSteeringSetpoint + - topic: /fmu/in/surface_vehicle_steering_setpoint + type: px4_msgs::msg::SurfaceVehicleSteeringSetpoint - topic: /fmu/in/landing_gear type: px4_msgs::msg::LandingGear