Browse Source

Rover: use WP_SPEED instead of CRUISE_SPEED to set loiter's maximum speed

mission-4.1.18
Arjun Vinod 6 years ago committed by Randy Mackay
parent
commit
14cb042e89
  1. 2
      APMrover2/mode_loiter.cpp

2
APMrover2/mode_loiter.cpp

@ -30,7 +30,7 @@ void ModeLoiter::update()
} else { } else {
// P controller with hard-coded gain to convert distance to desired speed // P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g2.loit_radius) * 0.5f, g.speed_cruise); _desired_speed = MIN((_distance_to_destination - g2.loit_radius) * 0.5f, g2.wp_nav.get_default_speed());
// calculate bearing to destination // calculate bearing to destination
_desired_yaw_cd = rover.current_loc.get_bearing_to(_destination); _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);

Loading…
Cancel
Save