Skip to content

Commit

Permalink
Plane: allow for any orientation of rangefinder for landing
Browse files Browse the repository at this point in the history
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
  • Loading branch information
tridge committed Sep 12, 2024
1 parent 4aa424b commit 3649ff4
Show file tree
Hide file tree
Showing 6 changed files with 53 additions and 12 deletions.
4 changes: 2 additions & 2 deletions ArduPlane/GCS_Plane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
Expand Down
9 changes: 9 additions & 0 deletions ArduPlane/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

Expand Down
5 changes: 5 additions & 0 deletions ArduPlane/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -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[];
7 changes: 7 additions & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
38 changes: 29 additions & 9 deletions ArduPlane/altitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand All @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/quadplane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 3649ff4

Please sign in to comment.