diff --git a/msg/RoverPositionSetpoint.msg b/msg/RoverPositionSetpoint.msg index 282eebfd51..d2ac0ac088 100644 --- a/msg/RoverPositionSetpoint.msg +++ b/msg/RoverPositionSetpoint.msg @@ -1,6 +1,8 @@ uint64 timestamp # time since system start (microseconds) float32[2] position_ned # 2-dimensional position setpoint in NED frame [m] -float32 cruising_speed # (Optional) Specify rover speed (Defaults to maximum speed) +float32[2] start_ned # (Optional) 2-dimensional start position in NED frame used to define the line that the rover will track to position_ned [m] (Defaults to vehicle position) +float32 cruising_speed # (Optional) Specify rover speed [m/s] (Defaults to maximum speed) +float32 arrival_speed # (Optional) Specify arrival speed [m/s] (Defaults to zero) -float32 yaw # [rad] [-pi,pi] from North. Optional, NAN if not set. Mecanum only. +float32 yaw # [rad] [-pi,pi] from North. Optional, NAN if not set. Mecanum only. (Defaults to vehicle yaw) diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index 077760b8c4..681d775699 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -41,10 +41,6 @@ AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(pa _rover_position_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise(); - // Initially set to NaN to indicate that the rover has no position setpoint - _rover_position_setpoint.position_ned[0] = NAN; - _rover_position_setpoint.position_ned[1] = NAN; - updateParams(); } @@ -61,21 +57,24 @@ void AckermannPosControl::updateParams() void AckermannPosControl::updatePosControl() { - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - updateSubscriptions(); if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Generate Position Setpoint if (_vehicle_control_mode.flag_control_offboard_enabled) { generatePositionSetpoint(); + + } else if (_vehicle_control_mode.flag_control_manual_enabled) { + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { + autoPositionMode(); } + // Generate Velocity Setpoint generateVelocitySetpoint(); } - } void AckermannPosControl::updateSubscriptions() @@ -121,7 +120,7 @@ void AckermannPosControl::generatePositionSetpoint() // Translate trajectory setpoint to rover position setpoint rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = _timestamp; + 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.cruising_speed = _param_ro_speed_limit.get(); @@ -130,31 +129,10 @@ void AckermannPosControl::generatePositionSetpoint() } -void AckermannPosControl::generateVelocitySetpoint() -{ - // Manual Position Mode - if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) { - manualPositionMode(); - return; - } - - // Auto Mode - if (_vehicle_control_mode.flag_control_auto_enabled) { - autoPositionMode(); - return; - } - - // Rover Position Setpoint - if (_rover_position_setpoint_sub.copy(&_rover_position_setpoint) - && PX4_ISFINITE(_rover_position_setpoint.position_ned[0]) && PX4_ISFINITE(_rover_position_setpoint.position_ned[1])) { - goToPositionMode(); - return; - } - -} - void AckermannPosControl::manualPositionMode() { + updateSubscriptions(); + manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); @@ -167,12 +145,21 @@ void AckermannPosControl::manualPositionMode() if (fabsf(yaw_delta) > FLT_EPSILON || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control _course_control = false; + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = yaw_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint)); + const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() * + 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 = speed_setpoint; + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); } else { // Course control if the steering input is zero (keep driving on a straight line) if (!_course_control) { @@ -186,24 +173,23 @@ void AckermannPosControl::manualPositionMode() 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; - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, - _curr_pos_ned, fabsf(speed_setpoint)); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( - bearing_setpoint + M_PI_F); - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - + 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); } } void AckermannPosControl::autoPositionMode() { + updateSubscriptions(); + if (_position_setpoint_triplet_sub.updated()) { updateWaypointsAndAcceptanceRadius(); } @@ -214,40 +200,19 @@ void AckermannPosControl::autoPositionMode() const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); - // Check stopping conditions - bool auto_stop{false}; - - if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE - || !_next_wp_ned.isAllFinite()) { - auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - if (auto_stop) { - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = 0.f; - rover_velocity_setpoint.bearing = _vehicle_yaw; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - - } else { // Regular guidance algorithm - const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, - distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), - _param_ro_jerk_limit.get(), _curr_wp_type, _waypoint_transition_angle, _prev_waypoint_transition_angle, - _param_ro_speed_limit.get()); - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - fabsf(speed_setpoint)); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = yaw_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - - } + 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 = autoArrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type, + _waypoint_transition_angle, _max_yaw_rate); + rover_position_setpoint.cruising_speed = autoCruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp, + distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle, + _prev_waypoint_transition_angle, _max_yaw_rate); + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); } void AckermannPosControl::updateWaypointsAndAcceptanceRadius() @@ -298,87 +263,104 @@ float AckermannPosControl::updateAcceptanceRadius(const float waypoint_transitio // Publish updated acceptance radius position_controller_status_s pos_ctrl_status{}; pos_ctrl_status.acceptance_radius = acceptance_radius; - pos_ctrl_status.timestamp = _timestamp; + pos_ctrl_status.timestamp = hrt_absolute_time(); _position_controller_status_pub.publish(pos_ctrl_status); return acceptance_radius; } -float AckermannPosControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min, - const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, - const float prev_acc_rad, const float max_decel, const float max_jerk, const int curr_wp_type, - const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) +float AckermannPosControl::autoArrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad, + const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate) +{ + if (!PX4_ISFINITE(waypoint_transition_angle) + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + return 0.f; // Stop at the waypoint + + } else { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + const float cornering_speed = max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering + } +} + +float AckermannPosControl::autoCruisingSpeed(const float cruising_speed, const float miss_speed_min, + const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad, + const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate) { // Catch improper values if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { return cruising_speed; } - // Upcoming stop - if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { - const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - return math::min(straight_line_speed, cruising_speed); - } - // Cornering slow down effect if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) { const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); - const float cornering_speed = _max_yaw_rate * turning_circle; + const float cornering_speed = max_yaw_rate * turning_circle; return math::constrain(cornering_speed, miss_speed_min, cruising_speed); } if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) { const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - const float cornering_speed = _max_yaw_rate * turning_circle; + const float cornering_speed = max_yaw_rate * turning_circle; return math::constrain(cornering_speed, miss_speed_min, cruising_speed); } - // Straight line speed - if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - float cornering_speed = _max_yaw_rate * turning_circle; - cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); - const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp - acc_rad, cornering_speed); - return math::min(straight_line_speed, cruising_speed); - - } - return cruising_speed; // Fallthrough } -void AckermannPosControl::goToPositionMode() +void AckermannPosControl::generateVelocitySetpoint() { + hrt_abstime timestamp = hrt_absolute_time(); + + if (_rover_position_setpoint_sub.updated()) { + _rover_position_setpoint_sub.copy(&_rover_position_setpoint); + _start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]); + _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; + } + + if (_position_controller_status_sub.updated()) { + position_controller_status_s position_controller_status{}; + _position_controller_status_sub.copy(&position_controller_status); + _acceptance_radius = position_controller_status.acceptance_radius; + } + const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); if (distance_to_target > _param_nav_acc_rad.get()) { + + float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : + 0.f; + const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target; float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, 0.f); - const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ? - _rover_position_setpoint.cruising_speed : - _param_ro_speed_limit.get(); - speed_setpoint = math::min(speed_setpoint, max_speed); + _param_ro_decel_limit.get(), distance, fabsf(arrival_speed)); + speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + + if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { + speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint, + fabsf(_rover_position_setpoint.cruising_speed)); + } + pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; + pure_pursuit_status.timestamp = timestamp; + const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; + rover_velocity_setpoint.timestamp = timestamp; rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = yaw_setpoint; + rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi( + yaw_setpoint + M_PI_F); _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); } else { rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; + rover_velocity_setpoint.timestamp = timestamp; rover_velocity_setpoint.speed = 0.f; rover_velocity_setpoint.bearing = _vehicle_yaw; _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index 8f9e37eb65..fab6c96e30 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -78,10 +78,20 @@ public: ~AckermannPosControl() = default; /** - * @brief Update position controller. + * @brief Update position control */ void updatePosControl(); + /** + * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + protected: /** * @brief Update the parameters of the module. @@ -99,27 +109,6 @@ private: */ void generatePositionSetpoint(); - /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or - * positionSetpointTriplet (Auto Mode) or roverPositionSetpoint. - */ - void generateVelocitySetpoint(); - - /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. - */ - void manualPositionMode(); - - /** - * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. - */ - void autoPositionMode(); - - /** - * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. - */ - void goToPositionMode(); - /** * @brief Update global/NED waypoint coordinates and acceptance radius. */ @@ -140,26 +129,34 @@ private: float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); /** - * @brief Calculate the speed setpoint. During cornering the speed is restricted based on the radius of the corner. - * On straight lines it is based on a speed trajectory such that the rover will arrive at the next corner with the - * desired cornering speed under consideration of the maximum deceleration and jerk. + * @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner. * @param cruising_speed Cruising speed [m/s]. * @param miss_speed_min Minimum speed setpoint [m/s]. - * @param distance_to_prev_wp Distance to the previous waypoint [m]. - * @param distance_to_curr_wp Distance to the current waypoint [m]. * @param acc_rad Acceptance radius of the current waypoint [m]. - * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. - * @param max_decel Maximum allowed deceleration [m/s^2]. - * @param max_jerk Maximum allowed jerk [m/s^3]. * @param curr_wp_type Type of the current waypoint. * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param max_speed Maximum speed setpoint [m/s] + * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] * @return Speed setpoint [m/s]. */ - float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, - float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int curr_wp_type, - float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); + float autoArrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type, + float waypoint_transition_angle, float max_yaw_rate); + + /** + * @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner. + * @param cruising_speed Cruising speed [m/s]. + * @param miss_speed_min Minimum speed setpoint [m/s]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] + * @return Speed setpoint [m/s]. + */ + float autoCruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle, + float prev_waypoint_transition_angle, float max_yaw_rate); /** * @brief Check if the necessary parameters are set. @@ -167,6 +164,11 @@ private: */ bool runSanityChecks(); + /** + * @brief Generate RoverVelocitySetpoint from RoverPositionSetpoint + */ + void generateVelocitySetpoint(); + // uORB subscriptions uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; @@ -176,9 +178,10 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; + uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; + rover_position_setpoint_s _rover_position_setpoint{}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; - rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; @@ -187,15 +190,14 @@ private: uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables - hrt_abstime _timestamp{0}; Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; Vector2f _pos_ctl_course_direction{}; Vector2f _pos_ctl_start_position_ned{}; + Vector2f _start_ned{}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. - float _dt{0.f}; int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. bool _prev_param_check_passed{true};