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 bbb8d234486eaf..d7b95e2a94eb40 100644 --- a/ArduPlane/Parameters.cpp +++ b/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 }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 38801d99631cc2..59abb456eeb78e 100644 --- a/ArduPlane/Parameters.h +++ b/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[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..235c56aa3f4be7 100644 --- a/ArduPlane/Plane.h +++ b/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 diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 5b845f38a86646..331ab222bb99af 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{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; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0a41b5551b6e6e..e6ad5ff494a6e2 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 diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index a38e00b9a222a6..104da1e5f62ac6 100644 --- a/ArduPlane/system.cpp +++ b/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