From 36d755a48aef232b408b85a0933b0bc4c4df5338 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Mon, 25 Feb 2019 01:10:39 +0100 Subject: [PATCH] Rover: replace location_offset() and get_distance() function calls with Location object member function calls This allows removing duplicated code --- APMrover2/GCS_Mavlink.cpp | 6 +++--- APMrover2/commands.cpp | 2 +- APMrover2/mode.cpp | 6 +++--- APMrover2/mode_auto.cpp | 2 +- APMrover2/mode_guided.cpp | 2 +- APMrover2/mode_loiter.cpp | 2 +- APMrover2/mode_rtl.cpp | 2 +- APMrover2/mode_smart_rtl.cpp | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 1ab4928a92..ffe7a12ece 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -810,19 +810,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw(); const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw(); // add offset to current location - location_offset(target_loc, ne_x, ne_y); + target_loc.offset(ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location - location_offset(target_loc, packet.x, packet.y); + target_loc.offset(packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home target_loc = rover.ahrs.get_home(); - location_offset(target_loc, packet.x, packet.y); + target_loc.offset(packet.x, packet.y); break; } } diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index a88a992101..a7569a8b1b 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -71,7 +71,7 @@ void Rover::update_home() barometer.update_calibration(); if (ahrs.home_is_set() && - get_distance(loc, ahrs.get_home()) < DISTANCE_HOME_MINCHANGE) { + loc.get_distance(ahrs.get_home()) < DISTANCE_HOME_MINCHANGE) { // insufficiently moved from current home - don't change it return; } diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index c129acf001..2cf3450830 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -199,7 +199,7 @@ void Mode::set_desired_location(const struct Location& destination, float next_l _destination = destination; // initialise distance - _distance_to_destination = get_distance(_origin, _destination); + _distance_to_destination = _origin.get_distance(_destination); _reached_destination = false; // set final desired speed @@ -231,7 +231,7 @@ bool Mode::set_desired_location_NED(const Vector3f& destination, float next_leg_ return false; } // apply offset - location_offset(destination_ned, destination.x, destination.y); + destination_ned.offset(destination.x, destination.y); set_desired_location(destination_ned, next_leg_bearing_cd); return true; } @@ -552,7 +552,7 @@ void Mode::calc_stopping_location(Location& stopping_loc) // calculate stopping position from current location in meters const Vector2f stopping_offset = velocity.normalized() * stopping_dist; - location_offset(stopping_loc, stopping_offset.x, stopping_offset.y); + stopping_loc.offset(stopping_offset.x, stopping_offset.y); } Mode *Rover::mode_from_mode_num(const enum Mode::Number num) diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index cf5ce29bc2..8264dc2381 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -36,7 +36,7 @@ void ModeAuto::update() switch (_submode) { case Auto_WP: { - _distance_to_destination = get_distance(rover.current_loc, _destination); + _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 7f48e44596..985530a62f 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -18,7 +18,7 @@ void ModeGuided::update() switch (_guided_mode) { case Guided_WP: { - _distance_to_destination = get_distance(rover.current_loc, _destination); + _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index 30b877d0f3..4b5d7d9833 100644 --- a/APMrover2/mode_loiter.cpp +++ b/APMrover2/mode_loiter.cpp @@ -21,7 +21,7 @@ bool ModeLoiter::_enter() void ModeLoiter::update() { // get distance (in meters) to destination - _distance_to_destination = get_distance(rover.current_loc, _destination); + _distance_to_destination = rover.current_loc.get_distance(_destination); // if within loiter radius slew desired speed towards zero and use existing desired heading if (_distance_to_destination <= g2.loit_radius) { diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 6ba23ef4b7..2bae9a8634 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -25,7 +25,7 @@ bool ModeRTL::_enter() void ModeRTL::update() { // calculate distance to home - _distance_to_destination = get_distance(rover.current_loc, _destination); + _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp index 56a6597810..5acf1faf1a 100644 --- a/APMrover2/mode_smart_rtl.cpp +++ b/APMrover2/mode_smart_rtl.cpp @@ -59,7 +59,7 @@ void ModeSmartRTL::update() } } // check if we've reached the next point - _distance_to_destination = get_distance(rover.current_loc, _destination); + _distance_to_destination = rover.current_loc.get_distance(_destination); if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) { _load_point = true; }