Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2024-09-05 12:26:49 +10:00
parent fea279b181
commit 83cac93e85
6 changed files with 53 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/GCS_Plane.cpp
View File

@ -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;
}
}

9
ArduPlane/Parameters.cpp
View File

@ -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
};

5
ArduPlane/Parameters.h
View File

@ -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[];

7
ArduPlane/Plane.h
View File

@ -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

36
ArduPlane/altitude.cpp
View File

@ -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);
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;
if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && ahrs.home_is_set()) {
/*
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;
}

2
ArduPlane/quadplane.cpp
View File

@ -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