AC_PrecLand: implement frame of reference

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();

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)

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;

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)

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)