diff --git a/msg/DifferentialVelocitySetpoint.msg b/msg/DifferentialVelocitySetpoint.msg index c43f36ad58..2cd19cf77b 100644 --- a/msg/DifferentialVelocitySetpoint.msg +++ b/msg/DifferentialVelocitySetpoint.msg @@ -1,5 +1,4 @@ uint64 timestamp # time since system start (microseconds) -float32[2] velocity_ned # 2-dimensional velocity setpoint in NED frame -bool backwards # Flag for backwards driving -float32 yaw # [rad] [-pi,pi] from North. Optional, NAN if not set. (Exclusive with velocity_ned, the rover will turn on the spot to reach the yaw setpoint) +float32 speed # [m/s] [-inf, inf] Speed setpoint (Backwards driving if negative) +float32 bearing # [rad] [-pi,pi] from North. diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index bef98df2bb..954068ab3f 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -130,23 +130,15 @@ void DifferentialPosControl::generatePositionSetpoint() void DifferentialPosControl::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) { + } else 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])) { + } else 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; } } @@ -158,19 +150,18 @@ void DifferentialPosControl::manualPositionMode() const float speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float bearing_scaling = math::min(_max_yaw_rate / _param_ro_yaw_p.get(), + _param_rd_trans_drv_trn.get() - FLT_EPSILON); const float bearing_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(), - _max_yaw_rate / _param_ro_yaw_p.get()); + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -bearing_scaling, bearing_scaling); if (fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Turn on spot _course_control = false; - const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); + const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = 0.f; - differential_velocity_setpoint.velocity_ned[1] = 0.f; - differential_velocity_setpoint.backwards = false; - differential_velocity_setpoint.yaw = yaw_setpoint; + differential_velocity_setpoint.speed = 0.f; + differential_velocity_setpoint.bearing = bearing_setpoint; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } else if (fabsf(bearing_delta) > FLT_EPSILON) { // Closed loop yaw rate control @@ -178,10 +169,8 @@ void DifferentialPosControl::manualPositionMode() const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = speed_body_x_setpoint * cosf(bearing_setpoint); - differential_velocity_setpoint.velocity_ned[1] = speed_body_x_setpoint * sinf(bearing_setpoint); - differential_velocity_setpoint.yaw = NAN; - differential_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON; + differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.bearing = bearing_setpoint; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } else { // Course control if the steering input is zero (keep driving on a straight line) @@ -198,16 +187,15 @@ void DifferentialPosControl::manualPositionMode() vector_scaling * _pos_ctl_course_direction; pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; - float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, - _curr_pos_ned, fabsf(speed_body_x_setpoint)); + const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(speed_body_x_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); - differential_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); - differential_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON; - differential_velocity_setpoint.yaw = NAN; + differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.bearing = speed_body_x_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( + bearing_setpoint + M_PI_F); _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } } @@ -242,69 +230,28 @@ void DifferentialPosControl::autoPositionMode() auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); } - // State machine - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - 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(_vehicle_speed_body_x)); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); - - if (!auto_stop) { - if (_currentState == GuidanceState::STOPPED) { - _currentState = GuidanceState::DRIVING; - } - - if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - _currentState = GuidanceState::SPOT_TURNING; - - } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _currentState = GuidanceState::DRIVING; - } - - } else { // Mission finished or delay command - _currentState = GuidanceState::STOPPED; - } - - // Guidance logic - switch (_currentState) { - case GuidanceState::DRIVING: { - // Calculate desired speed in body x direction - const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), - _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), - _param_rd_miss_spd_gain.get(), _curr_wp_type); - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); - differential_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); - differential_velocity_setpoint.backwards = false; - differential_velocity_setpoint.yaw = NAN; - _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); - - } break; - - case GuidanceState::SPOT_TURNING: { - differential_velocity_setpoint_s differential_velocity_setpoint{}; - differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = 0.f; - differential_velocity_setpoint.velocity_ned[1] = 0.f; - differential_velocity_setpoint.backwards = false; - differential_velocity_setpoint.yaw = yaw_setpoint; - _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); - } break; - - case GuidanceState::STOPPED: - default: + if (auto_stop) { differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = 0.f; - differential_velocity_setpoint.velocity_ned[1] = 0.f; - differential_velocity_setpoint.backwards = false; - differential_velocity_setpoint.yaw = NAN; + differential_velocity_setpoint.speed = 0.f; + differential_velocity_setpoint.bearing = _vehicle_yaw; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); - break; + } else { + const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get(), _curr_wp_type); + 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(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + fabsf(speed_body_x_setpoint)); + _pure_pursuit_status_pub.publish(pure_pursuit_status); + differential_velocity_setpoint_s differential_velocity_setpoint{}; + differential_velocity_setpoint.timestamp = _timestamp; + differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.bearing = bearing_setpoint; + _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } } @@ -350,25 +297,21 @@ void DifferentialPosControl::goToPositionMode() const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; - const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, - _curr_pos_ned, fabsf(speed_body_x_setpoint)); + const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(speed_body_x_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); - differential_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); - differential_velocity_setpoint.backwards = false; - differential_velocity_setpoint.yaw = NAN; + differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.bearing = bearing_setpoint; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } else { differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = 0.f; - differential_velocity_setpoint.velocity_ned[1] = 0.f; - differential_velocity_setpoint.backwards = false; - differential_velocity_setpoint.yaw = NAN; + differential_velocity_setpoint.speed = 0.f; + differential_velocity_setpoint.bearing = _vehicle_yaw; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } } diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 96bb6fb1af..51f6ccc855 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -62,15 +62,6 @@ using namespace matrix; -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - SPOT_TURNING, // The vehicle is currently turning on the spot. - DRIVING, // The vehicle is currently driving. - STOPPED // The vehicle is stopped. -}; - /** * @brief Class for differential position control. */ @@ -197,10 +188,8 @@ private: // Class Instances MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. DEFINE_PARAMETERS( - (ParamFloat) _param_rd_trans_trn_drv, (ParamFloat) _param_rd_trans_drv_trn, (ParamFloat) _param_rd_miss_spd_gain, (ParamFloat) _param_ro_max_thr_speed, @@ -215,6 +204,5 @@ private: (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_nav_acc_rad - ) }; diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp index 0f7bc43e20..cf9376c70b 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp @@ -127,9 +127,8 @@ void DifferentialVelControl::generateVelocitySetpoint() if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.velocity_ned[0] = velocity_in_local_frame(0); - differential_velocity_setpoint.velocity_ned[1] = velocity_in_local_frame(1); - differential_velocity_setpoint.backwards = false; + differential_velocity_setpoint.speed = velocity_in_local_frame.norm(); + differential_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } } @@ -140,61 +139,43 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint() _differential_velocity_setpoint_sub.copy(&_differential_velocity_setpoint); } - // Catch spot turning - if (PX4_ISFINITE(_differential_velocity_setpoint.yaw)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _differential_velocity_setpoint.yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, - 0.f, _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 = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - return; - } - // Attitude Setpoint - if (fabsf(_differential_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON - && fabsf(_differential_velocity_setpoint.velocity_ned[0]) < FLT_EPSILON) { - 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); - - } else { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - const float yaw_setpoint = atan2f(_differential_velocity_setpoint.velocity_ned[1], - _differential_velocity_setpoint.velocity_ned[0]); - rover_attitude_setpoint.yaw_setpoint = _differential_velocity_setpoint.backwards ? matrix::wrap_pi( - yaw_setpoint + M_PI_F) : yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _differential_velocity_setpoint.bearing; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); // Throttle Setpoint - float speed_magnitude = math::min(sqrtf(powf(_differential_velocity_setpoint.velocity_ned[0], - 2) + powf(_differential_velocity_setpoint.velocity_ned[1], 2)), _param_ro_speed_limit.get()); + const float heading_error = matrix::wrap_pi(_differential_velocity_setpoint.bearing - _vehicle_yaw); - if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { + if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + _current_state = DrivingState::SPOT_TURNING; - const float speed_body_x_setpoint_normalized = math::interpolate(speed_magnitude, + } else if (_current_state == DrivingState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _current_state = DrivingState::DRIVING; + } + + float speed_body_x_setpoint = 0.f; + + if (_current_state == DrivingState::DRIVING) { + speed_body_x_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(), + _param_ro_speed_limit.get()); + + const float speed_body_x_setpoint_normalized = math::interpolate(speed_body_x_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_sub.copy(&_rover_steering_setpoint); } - if (fabsf(speed_body_x_setpoint_normalized) > 1.f - - fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - speed_magnitude = math::interpolate(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + if (fabsf(speed_body_x_setpoint_normalized) > 1.f - fabsf( + _rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + speed_body_x_setpoint = math::interpolate(sign(speed_body_x_setpoint_normalized) * (1.f - fabsf( + _rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f, + - _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); } } - const float speed_body_x_setpoint = _differential_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude; rover_throttle_setpoint_s rover_throttle_setpoint{}; rover_throttle_setpoint.timestamp = _timestamp; rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp index 35f69c4767..fcdcfbba7e 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp @@ -60,6 +60,14 @@ using namespace matrix; +/** + * @brief Enum class for the different states of driving. + */ +enum class DrivingState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING // The vehicle is currently driving. +}; + /** * @brief Class for differential velocity control. */ @@ -134,12 +142,16 @@ private: float _vehicle_yaw{0.f}; float _dt{0.f}; bool _prev_param_check_passed{false}; + DrivingState _current_state{DrivingState::DRIVING}; + // Controllers PID _pid_speed; SlewRate _speed_setpoint; DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, (ParamFloat) _param_ro_max_thr_speed, (ParamFloat) _param_ro_speed_p, (ParamFloat) _param_ro_speed_i,