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
tridge · Sep 12, 2024 · 3649ff4 · 3649ff4
1 parent 4aa424b
commit 3649ff4
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 12 deletions.
diff --git a/ArduPlane/GCS_Plane.cpp b/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;            
         }
     }

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1280,6 +1280,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 Down for normal downward facing rangefinder and Back for rearward facing rangefinder for quadplane tailsitters. Custom orientation can be used with Custom1 or Custom2. The orientation must match at least one of the available rangefinders.
+    // @Values: 4: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
 };
 

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -581,6 +581,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[];
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -212,6 +212,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

diff --git a/ArduPlane/altitude.cpp b/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{corrected_distance, 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;
             }

diff --git a/ArduPlane/quadplane.cpp b/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