Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: seperate acceleration/deceleration limits

Browse Source
This commit is contained in:
Ammarf 2018-05-22 13:30:32 +09:00 committed by Randy Mackay
parent 832778e471
commit d7821635c0
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/mode.cpp
View File

@ -209,9 +209,9 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_
// get acceleration limited target speed // get acceleration limited target speed
target_speed = attitude_control.get_desired_speed_accel_limited(target_speed); target_speed = attitude_control.get_desired_speed_accel_limited(target_speed);
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration // apply object avoidance to desired speed using half vehicle's maximum deceleration
if (avoidance_enabled) { if (avoidance_enabled) {
g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt); g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.yaw, target_speed, rover.G_Dt);
} }
// call throttle controller and convert output to -100 to +100 range // call throttle controller and convert output to -100 to +100 range
@ -332,8 +332,8 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
float speed_scaled = desired_speed * MIN(lateral_accel_speed_scaling, pivot_speed_scaling); float speed_scaled = desired_speed * MIN(lateral_accel_speed_scaling, pivot_speed_scaling);
// limit speed based on distance to waypoint and max acceleration/deceleration // limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(distance_to_waypoint) && is_positive(attitude_control.get_accel_max())) { if (is_positive(distance_to_waypoint) && is_positive(attitude_control.get_decel_max())) {
const float speed_max = safe_sqrt(2.0f * distance_to_waypoint * attitude_control.get_accel_max() + sq(_desired_speed_final)); const float speed_max = safe_sqrt(2.0f * distance_to_waypoint * attitude_control.get_decel_max() + sq(_desired_speed_final));
speed_scaled = constrain_float(speed_scaled, -speed_max, speed_max); speed_scaled = constrain_float(speed_scaled, -speed_max, speed_max);
} }

4
APMrover2/mode_loiter.cpp
View File

@ -29,9 +29,9 @@ void ModeLoiter::update()
// if within waypoint radius slew desired speed towards zero and use existing desired heading // if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) { if (_distance_to_destination <= g.waypoint_radius) {
if (is_negative(_desired_speed)) { if (is_negative(_desired_speed)) {
_desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f); _desired_speed = MIN(_desired_speed + attitude_control.get_decel_max() * rover.G_Dt, 0.0f);
} else { } else {
_desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f); _desired_speed = MAX(_desired_speed - attitude_control.get_decel_max() * rover.G_Dt, 0.0f);
} }
_yaw_error_cd = 0.0f; _yaw_error_cd = 0.0f;
} else { } else {