AC_PrecLand: add _ORIENT param to precland library

The orient parameter will help us construct a direction of approach vector in vehicle body frame. This vector would help
us rotate the target vector from sensor frame to body frame and determine the horizontal position error of vehicle for
more orientations than just PITCH_270

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -12,6 +12,12 @@
 
 extern const AP_HAL::HAL& hal;
 
+#if APM_BUILD_TYPE(APM_BUILD_Rover)
+ # define AC_PRECLAND_ORIENT_DEFAULT Rotation::ROTATION_NONE
+#else
+ # define AC_PRECLAND_ORIENT_DEFAULT Rotation::ROTATION_PITCH_270
+#endif
+
 static const uint32_t EKF_INIT_TIME_MS = 2000; // EKF initialisation requires this many milliseconds of good sensor data
 static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500; // Sensor must update within this many ms during EKF init, else init will fail
 static const uint32_t LANDING_TARGET_TIMEOUT_MS = 2000; // Sensor must update within this many ms, else prec landing will be switched off
@@ -170,6 +176,14 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 17, AC_PrecLand, _options, 0),
 
+    // @Param: ORIENT
+    // @DisplayName: Camera Orientation
+    // @Description: Orientation of camera/sensor on body
+    // @Values: 0:Forward, 4:Back, 25:Down
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO_FRAME("ORIENT", 18, AC_PrecLand, _orient, AC_PRECLAND_ORIENT_DEFAULT, AP_PARAM_FRAME_ROVER),
+
     AP_GROUPEND
 };
 
@@ -242,6 +256,9 @@ void AC_PrecLand::init(uint16_t update_rate_hz)
     if (_backend != nullptr) {
         _backend->init();
     }
+
+    _approach_vector_body.x = 1;
+    _approach_vector_body.rotate(_orient);
 }
 
 // update - give chance to driver to get updates from sensor
@@ -591,6 +608,21 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
             // Apply sensor yaw alignment rotation
             target_vec_unit_body.rotate_xy(radians(_yaw_align*0.01f));
         }
+
+
+        // rotate vector based on sensor oriention to get correct body frame vector
+        if (_orient != ROTATION_PITCH_270) {
+            // by default, the vector is constructed downwards in body frame
+            // hence, we do not do any rotation if the orientation is downwards
+            // if it is some other orientation, we first bring the vector to forward
+            // and then we rotate it to desired orientation
+            // because the rotations are measured with respect to a vector pointing towards front in body frame
+            // for eg, if orientation is back, i.e., ROTATION_YAW_180, 
+            // the vector is first brought to front and then rotation by YAW 180 to take it to the back of vehicle
+            target_vec_unit_body.rotate(ROTATION_PITCH_90); // bring vector to front
+            target_vec_unit_body.rotate(_orient);           // rotate it to desired orientation
+        }
+
         return true;
     } else {
         return false;
@@ -603,11 +635,13 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
     if (retrieve_los_meas(target_vec_unit_body)) {
         const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
 
+        const bool target_vec_valid = target_vec_unit_body.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f;
         const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body;
-        const bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
+        const Vector3f approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body;
         const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
         if (target_vec_valid && alt_valid) {
-            float dist, alt;
+            // distance to target and distance to target along approach vector
+            float dist_to_target, dist_to_target_along_av;
             // figure out ned camera orientation w.r.t its offset
             Vector3f cam_pos_ned;
             if (!_cam_offset.get().is_zero()) {
@@ -617,13 +651,12 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
             }
             if (_backend->distance_to_target() > 0.0f) {
                 // sensor has provided distance to landing target
-                dist = _backend->distance_to_target();
-                alt = dist * target_vec_unit_ned.z;
+                dist_to_target = _backend->distance_to_target();
             } else {
                 // sensor only knows the horizontal location of the landing target
                 // rely on rangefinder for the vertical target
-                alt = MAX(rangefinder_alt_m - cam_pos_ned.z, 0.0f);
-                dist = alt / target_vec_unit_ned.z;
+                dist_to_target_along_av = MAX(rangefinder_alt_m - cam_pos_ned.projected(approach_vector_NED).length(), 0.0f);
+                dist_to_target = dist_to_target_along_av / target_vec_unit_ned.projected(approach_vector_NED).length();
             }
 
             // Compute camera position relative to IMU
@@ -631,7 +664,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
             const Vector3f cam_pos_ned_rel_imu = cam_pos_ned - accel_pos_ned;
 
             // Compute target position relative to IMU
-            _target_pos_rel_meas_NED = Vector3f{target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt} + cam_pos_ned_rel_imu;
+            _target_pos_rel_meas_NED = (target_vec_unit_ned * dist_to_target) + cam_pos_ned_rel_imu;
 
             // store the current relative down position so that if we need to retry landing, we know at this height landing target can be found
             const AP_AHRS &_ahrs = AP::ahrs();

libraries/AC_PrecLand/AC_PrecLand.h

@@ -54,6 +54,9 @@ public:
     // vehicle has to be closer than this many cm's to the target before descending towards target
     float get_max_xy_error_before_descending_cm() const { return _xy_max_dist_desc * 100.0f; }
 
+    // returns orientation of sensor
+    Rotation get_orient() const { return _orient; }
+
     // returns ekf outlier count
     uint32_t ekf_outlier_count() const { return _outlier_reject_count; }
 
@@ -174,6 +177,7 @@ private:
     AP_Float                    _sensor_min_alt;     // PrecLand minimum height required for detecting target
     AP_Float                    _sensor_max_alt;     // PrecLand maximum height the sensor can detect target
     AP_Int16                    _options;            // Bitmask for extra options
+    AP_Enum<Rotation>           _orient;             // Orientation of camera/sensor
 
     uint32_t                    _last_update_ms;    // system time in millisecond when update was last called
     bool                        _target_acquired;   // true if target has been seen recently after estimator is initialized
@@ -186,6 +190,7 @@ private:
     uint32_t                    _outlier_reject_count;  // mini-EKF's outlier counter (3 consecutive outliers lead to EKF accepting updates)
 
     Vector3f                    _target_pos_rel_meas_NED; // target's relative position as 3D vector
+    Vector3f                    _approach_vector_body;   // unit vector in landing approach direction (in body frame)
 
     Vector3f                    _last_target_pos_rel_origin_NED;  // stores the last known location of the target horizontally, and the height of the vehicle where it detected this target in meters NED
     Vector3f                    _last_vehicle_pos_NED;            // stores the position of the vehicle when landing target was last detected in m and NED