Plane: allow for any orientation of rangefinder for landing

this is principally for tailsitters where rangefinders would be
orientation with RNGFND1_ORIENT=12 (PITCH_180), but also allows for
custom orientations which will be useful if the rangefinder is tilted
forward

ArduPlane/GCS_Plane.cpp

@@ -116,12 +116,12 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
 
 #if AP_RANGEFINDER_ENABLED
     const RangeFinder *rangefinder = RangeFinder::get_singleton();
-    if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
+    if (rangefinder && rangefinder->has_orientation(plane.rangefinder_orientation())) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         if (plane.g.rangefinder_landing) {
             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
-        if (rangefinder->has_data_orient(ROTATION_PITCH_270)) {
+        if (rangefinder->has_data_orient(plane.rangefinder_orientation())) {
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;            
         }
     }

ArduPlane/Parameters.cpp

@@ -1274,6 +1274,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(precland, "PLND_", 35, ParametersG2, AC_PrecLand),
 #endif
 
+#if AP_RANGEFINDER_ENABLED
+    // @Param: RNGFND_LND_ORNT
+    // @DisplayName: rangefinder landing orientation
+    // @Description: The orientation of rangefinder to use for landing detection. Should be set to rangefinder_orientation() for normal downward facing rangefinder and ROTATION_PITCH_180 for rearward facing rangefinder for quadplane tailsitters. Custom orientation can be used with CUST_ROT_ENABLE. The orientation must match at least one of the available rangefinders.
+    // @Values: 12:Back, 25:Down, 101:Custom1, 102:Custom2
+    // @User: Standard
+    AP_GROUPINFO("RNGFND_LND_ORNT", 36, ParametersG2, rangefinder_land_orient, ROTATION_PITCH_270),
+#endif
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -579,6 +579,11 @@ class ParametersG2 {
 
     // just to make compilation easier when all things are compiled out...
     uint8_t unused_integer;
+
+#if AP_RANGEFINDER_ENABLED
+    // orientation of rangefinder to use for landing
+    AP_Int8 rangefinder_land_orient;
+#endif
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -208,6 +208,13 @@ class Plane : public AP_Vehicle {
 
 #if AP_RANGEFINDER_ENABLED
     AP_FixedWing::Rangefinder_State rangefinder_state;
+
+    /*
+      orientation of rangefinder to use for landing
+     */
+    Rotation rangefinder_orientation(void) const {
+        return Rotation(g2.rangefinder_land_orient.get());
+    }
 #endif
 
 #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED

ArduPlane/altitude.cpp

@@ -138,7 +138,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool us
 
 #if HAL_QUADPLANE_ENABLED && AP_RANGEFINDER_ENABLED
    if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
-       rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow) {
+       rangefinder.status_orient(rangefinder_orientation()) == RangeFinder::Status::OutOfRangeLow) {
        // a special case for quadplane landing when rangefinder goes
        // below minimum. Consider our height above ground to be zero
        return 0;
@@ -679,16 +679,36 @@ void Plane::rangefinder_terrain_correction(float &height)
  */
 void Plane::rangefinder_height_update(void)
 {
-    float distance = rangefinder.distance_orient(ROTATION_PITCH_270);
-
-    if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && ahrs.home_is_set()) {
+    const auto orientation = rangefinder_orientation();
+    bool range_ok = rangefinder.status_orient(orientation) == RangeFinder::Status::Good;
+    float distance = rangefinder.distance_orient(orientation);
+    float corrected_distance = distance;
+
+    /*
+      correct distance for attitude
+     */
+    if (range_ok) {
+        // correct the range for attitude
+        const auto &dcm = ahrs.get_rotation_body_to_ned();
+
+        Vector3f v{1, 0, 0};
+        v.rotate(orientation);
+        v = dcm * v;
+
+        if (!is_positive(v.z)) {
+            // not pointing at the ground
+            range_ok = false;
+        } else {
+            corrected_distance *= v.z;
+        }
+    }
+
+    if (range_ok && ahrs.home_is_set()) {
         if (!rangefinder_state.have_initial_reading) {
             rangefinder_state.have_initial_reading = true;
             rangefinder_state.initial_range = distance;
         }
-        // correct the range for attitude (multiply by DCM.c.z, which
-        // is cos(roll)*cos(pitch))
-        rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
+        rangefinder_state.height_estimate = corrected_distance;
 
         rangefinder_terrain_correction(rangefinder_state.height_estimate);
 
@@ -699,10 +719,10 @@ void Plane::rangefinder_height_update(void)
         // to misconfiguration or a faulty sensor
         if (rangefinder_state.in_range_count < 10) {
             if (!is_equal(distance, rangefinder_state.last_distance) &&
-                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)*0.01f) {
+                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01f) {
                 rangefinder_state.in_range_count++;
             }
-            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)*0.01*0.2) {
+            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01*0.2) {
                 // changes by more than 20% of full range will reset counter
                 rangefinder_state.in_range_count = 0;
             }

ArduPlane/quadplane.cpp

@@ -3752,7 +3752,7 @@ float QuadPlane::forward_throttle_pct()
         vel_forward.last_pct = vel_forward.integrator;
     } else if ((in_vtol_land_final() && motors->limit.throttle_lower) ||
 #if AP_RANGEFINDER_ENABLED
-              (plane.g.rangefinder_landing && (plane.rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::OutOfRangeLow))) {
+              (plane.g.rangefinder_landing && (plane.rangefinder.status_orient(plane.rangefinder_orientation()) == RangeFinder::Status::OutOfRangeLow))) {
 #else
               false) {
 #endif

ArduPlane/system.cpp

@@ -39,7 +39,7 @@ void Plane::init_ardupilot()
 #if AP_RANGEFINDER_ENABLED
     // initialise rangefinder
     rangefinder.set_log_rfnd_bit(MASK_LOG_SONAR);
-    rangefinder.init(ROTATION_PITCH_270);
+    rangefinder.init(rangefinder_orientation());
 #endif
 
     // initialise battery monitoring