From 3649ff40732bef675ef0ad11c375106a5aa2b382 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 baf2cd9c924f6c..a193f44e0858d3 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 40148509f714de..bfb4f892642f88 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 612de354fc8c12..377eb9ded865f2 100644 --- 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 index 9978b5f2eac0ba..030131abc308f1 100644 --- 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 index 5b845f38a86646..c32f5802a81d6a 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 3c0d2e18fd259f..d0f912376dc520 100644 --- 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