diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index de3b844e5e..7fd0efbaa8 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -124,7 +124,7 @@ Vector3f AC_PrecLand::get_target_shift(const Vector3f &orig_target) } // calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate -// body-frame angles stored in _bf_angle_to_target +// raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame) // earth-frame angles stored in _ef_angle_to_target // position estimate is stored in _target_pos void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) @@ -136,7 +136,7 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) } // get body-frame angles to target from backend - if (!_backend->get_angle_to_target(_bf_angle_to_target.x, _bf_angle_to_target.y)) { + if (!_backend->get_angle_to_target(_angle_to_target.x, _angle_to_target.y)) { _have_estimate = false; return; } diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 86eb5b5979..17cd41b8c0 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -62,7 +62,7 @@ public: // accessors for logging bool enabled() const { return _enabled; } - const Vector2f& last_bf_angle_to_target() const { return _bf_angle_to_target; } + const Vector2f& last_bf_angle_to_target() const { return _angle_to_target; } const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; } const Vector3f& last_target_pos_offset() const { return _target_pos_offset; } @@ -94,7 +94,7 @@ private: float _dt; // time difference (in seconds) between calls from the main program // output from sensor (stored for logging) - Vector2f _bf_angle_to_target;// last body-frame angle to target + Vector2f _angle_to_target; // last raw sensor angle to target Vector2f _ef_angle_to_target;// last earth-frame angle to target // output from controller diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index aeb8eaac7f..a079870ae2 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -33,8 +33,8 @@ bool AC_PrecLand_Companion::update() bool AC_PrecLand_Companion::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) { if (_new_estimate){ - x_angle_rad = _bf_angle_to_target.x; - y_angle_rad = _bf_angle_to_target.y; + x_angle_rad = _angle_to_target.x; + y_angle_rad = _angle_to_target.y; // reset and wait for new data _new_estimate = false; @@ -51,8 +51,8 @@ void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg) mavlink_msg_landing_target_decode(msg, &packet); _timestamp_us = packet.time_usec; - _bf_angle_to_target.x = packet.angle_x; - _bf_angle_to_target.y = packet.angle_y; + _angle_to_target.x = packet.angle_x; + _angle_to_target.y = packet.angle_y; _distance_to_target = packet.distance; _state.healthy = true; _new_estimate = true; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 6e48991c00..8b6cc77d3e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -37,7 +37,7 @@ public: private: // output from camera - Vector2f _bf_angle_to_target; // last body-frame angle to target + Vector2f _angle_to_target; // last body-frame angle to target float _distance_to_target; // distance from the camera to target in meters uint64_t _timestamp_us; // timestamp when the image was captured(synced via UAVCAN) bool _new_estimate; // true if new data from the camera has been received