diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 7fd0efbaa8..29619f3e9d 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -141,9 +141,18 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) return; } - // subtract vehicle lean angles - float x_rad = _bf_angle_to_target.x - _ahrs.roll; - float y_rad = -_bf_angle_to_target.y + _ahrs.pitch; + float x_rad; + float y_rad; + + if(_backend->get_frame_of_reference() == MAV_FRAME_LOCAL_NED){ + //don't subtract vehicle lean angles + x_rad = _angle_to_target.x; + y_rad = -_angle_to_target.y; + }else{ // assume MAV_FRAME_BODY_NED (i.e. a hard-mounted sensor) + // subtract vehicle lean angles + x_rad = _angle_to_target.x - _ahrs.roll; + y_rad = -_angle_to_target.y + _ahrs.pitch; + } // rotate to earth-frame angles _ef_angle_to_target.x = y_rad*_ahrs.cos_yaw() - x_rad*_ahrs.sin_yaw(); diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index ed75f3b3bf..5c5ffdfc7d 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -26,6 +26,8 @@ public: // update - give chance to driver to get updates from sensor // returns true if new data available virtual bool update() = 0; + // what frame of reference is our sensor reporting in? + virtual MAV_FRAME get_frame_of_reference(); // get_angle_to_target - returns body frame angles (in radians) to target // returns true if angles are available, false if not (i.e. no target) diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index a079870ae2..8b91488188 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -16,6 +16,7 @@ void AC_PrecLand_Companion::init() // set healthy _state.healthy = true; _new_estimate = false; + _frame = MAV_FRAME_BODY_NED; // assume body until we get our first message that says otherwise } // update - give chance to driver to get updates from sensor @@ -26,6 +27,11 @@ bool AC_PrecLand_Companion::update() return _new_estimate; } +MAV_FRAME AC_PrecLand_Companion::get_frame_of_reference() +{ + return _frame; +} + // get_angle_to_target - returns body frame angles (in radians) to target // returns true if angles are available, false if not (i.e. no target) // x_angle_rad : body-frame roll direction, positive = target is to right (looking down) @@ -51,6 +57,7 @@ void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg) mavlink_msg_landing_target_decode(msg, &packet); _timestamp_us = packet.time_usec; + _frame = (MAV_FRAME) packet.frame; _angle_to_target.x = packet.angle_x; _angle_to_target.y = packet.angle_y; _distance_to_target = packet.distance; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 8b6cc77d3e..a7c56a0c3e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -24,6 +24,8 @@ public: // update - give chance to driver to get updates from sensor // returns true if new data available bool update(); + // what frame of reference is our sensor reporting in? + MAV_FRAME get_frame_of_reference(); // get_angle_to_target - returns body frame angles (in radians) to target // returns true if angles are available, false if not (i.e. no target) @@ -37,6 +39,7 @@ public: private: // output from camera + MAV_FRAME _frame; // what frame of reference is our sensor reporting in? 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) diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index 16964c83fa..f7f5e102f0 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -28,6 +28,8 @@ public: // update - give chance to driver to get updates from sensor // returns true if new data available bool update(); + // IRLock is hard-mounted to the frame of the vehicle, so it will always be in body-frame + MAV_FRAME get_frame_of_reference() { return MAV_FRAME_BODY_NED; } // get_angle_to_target - returns body frame angles (in radians) to target // returns true if angles are available, false if not (i.e. no target)