mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
differential: migrate state machine to velocity control
This commit is contained in:
chfriedrich98
committed by
chfriedrich98
chfriedrich98
parent
ca01d9e37c
commit
39fa8b5550
@@ -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.
|
||||
|
||||
@@ -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<float>(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<float>(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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
|
||||
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
|
||||
(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain,
|
||||
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
|
||||
@@ -215,6 +204,5 @@ private:
|
||||
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
|
||||
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
|
||||
|
||||
)
|
||||
};
|
||||
|
||||
@@ -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<float>(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<float>(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<float>(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<float>(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,
|
||||
|
||||
@@ -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<float> _speed_setpoint;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
|
||||
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
|
||||
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
|
||||
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
|
||||
(ParamFloat<px4::params::RO_SPEED_I>) _param_ro_speed_i,
|
||||
|
||||
Reference in New Issue
Block a user