From 83cac93e856b5be5e0ec543cb90d989a51f8c8b5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Sep 2024 12:26:49 +1000 Subject: [PATCH] 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 | 4 ++-- ArduPlane/Parameters.cpp | 9 +++++++++ ArduPlane/Parameters.h | 5 +++++ ArduPlane/Plane.h | 7 +++++++ ArduPlane/altitude.cpp | 38 +++++++++++++++++++++++++++++--------- ArduPlane/quadplane.cpp | 2 +- 6 files changed, 53 insertions(+), 12 deletions(-) diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index baf2cd9c92..a193f44e08 100644 --- 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 index 40148509f7..bfb4f89264 100644 --- 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 index 612de354fc..377eb9ded8 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -581,6 +581,11 @@ public: // 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 index 9978b5f2ea..030131abc3 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -212,6 +212,13 @@ private: #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 index 5b845f38a8..c32f5802a8 100644 --- 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 index 05748796e4..8ba9c06595 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3762,7 +3762,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