Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APMrover2: move get_bearing_cd to Location and rename to get_bearing_to

This commit is contained in:
Pierre Kancir 2019-04-05 15:02:44 +02:00 committed by Peter Barker
parent dfdef294e3
commit 007d317741
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/mode.cpp
View File

@ -205,7 +205,7 @@ void Mode::set_desired_location(const struct Location& destination, float next_l
// set final desired speed // set final desired speed
_desired_speed_final = 0.0f; _desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) { if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination); const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd); const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (fabsf(turn_angle_cd) < 10.0f) { if (fabsf(turn_angle_cd) < 10.0f) {
// if turning less than 0.1 degrees vehicle can continue at full speed // if turning less than 0.1 degrees vehicle can continue at full speed
@ -432,7 +432,7 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f)); const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f));
// calculate distance from vehicle to line + wp_overshoot // calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(get_bearing_cd(_origin, _destination) - heading_cd); const float line_yaw_diff = wrap_180_cd(_origin.get_bearing_to(_destination) - heading_cd);
const float xtrack_error = rover.nav_controller->crosstrack_error(); const float xtrack_error = rover.nav_controller->crosstrack_error();
const float dist_from_line = fabsf(xtrack_error); const float dist_from_line = fabsf(xtrack_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(xtrack_error); const bool heading_away = is_positive(line_yaw_diff) == is_positive(xtrack_error);

2
APMrover2/mode_loiter.cpp
View File

@ -35,7 +35,7 @@ void ModeLoiter::update()
_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, g.speed_cruise);
// calculate bearing to destination // calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination); _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it // if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) { if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {