dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

cleanup

This commit is contained in:
chfriedrich98 2024-03-22 13:52:28 +01:00
parent dfc58b3dfc
commit 6efca3a196
1 changed files with 1 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/ackermann_drive/AckermannDriveControl.cpp
View File

@ -48,10 +48,6 @@ void AckermannDriveControl::updateParams()
ModuleParams::updateParams(); ModuleParams::updateParams();
_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get(); _max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
// _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
// _differential_drive_kinematics.setMaxSpeed(_max_speed);
// _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
} }
void AckermannDriveControl::Update() void AckermannDriveControl::Update()
@ -74,8 +70,6 @@ void AckermannDriveControl::Update()
} }
} }
// printf("AckermannDriveControl::Update()\n");
if (_manual_driving) { if (_manual_driving) {
// Manual mode // Manual mode
// directly produce setpoints from the manual control setpoint (joystick) // directly produce setpoints from the manual control setpoint (joystick)
@ -94,28 +88,11 @@ void AckermannDriveControl::Update()
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint); _differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
// publish data to actuator_motors (output module)
// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
// Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
// _differential_drive_setpoint.speed,
// _differential_drive_setpoint.yaw_rate);
double steering = _differential_drive_setpoint.yaw_rate; double steering = _differential_drive_setpoint.yaw_rate;
double speed = _differential_drive_setpoint.speed; double speed = _differential_drive_setpoint.speed;
// Check if max_angular_wheel_speed is zero
// const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
// const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
// if (!_armed || setpoint_timeout || !valid_max_speed) {
// wheel_speeds = {}; // stop
// }
// wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
actuator_motors_s actuator_motors{}; actuator_motors_s actuator_motors{};
// actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
// wheel_speeds.copyTo(actuator_motors.control);
actuator_motors.control[0] = speed; actuator_motors.control[0] = speed;
actuator_motors.control[1] = speed; actuator_motors.control[1] = speed;
actuator_motors.timestamp = now; actuator_motors.timestamp = now;