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

SITL: support more rangefinder orientations 

allows for quadplane tailsitter rangefinders
This commit is contained in:
Andrew Tridgell 2024-09-05 12:23:57 +10:00
parent 72a013952c
commit fea279b181
1 changed files with 44 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/SITL/SIM_Aircraft.cpp
View File

@ -33,6 +33,7 @@
#include <AP_Filesystem/AP_Filesystem.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL_SITL/HAL_SITL_Class.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
using namespace SITL;
@ -500,18 +501,27 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
#endif
}
// returns perpendicular height to surface downward-facing rangefinder
// is bouncing off:
/*
rover and copter have special handling for horizontal rangefinders
as distance to obstacles - this takes effect for yaw-only
orientations
*/
#define SITL_RANGEFINDER_AS_OBJECT_SENSOR (APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_Rover))
#define SITL_RANGEFINDER_IS_YAW_ONLY(orientation) (orientation <= ROTATION_YAW_315)
// returns perpendicular height to surface rangefinder is bouncing off
float Aircraft::perpendicular_distance_to_rangefinder_surface() const
{
switch ((Rotation)sitl->sonar_rot.get()) {
case Rotation::ROTATION_PITCH_270:
return sitl->state.height_agl;
case ROTATION_NONE ... ROTATION_YAW_315:
#if SITL_RANGEFINDER_AS_OBJECT_SENSOR
const auto orientation = (Rotation)sitl->sonar_rot.get();
if (SITL_RANGEFINDER_IS_YAW_ONLY(orientation)) {
// assume these are avoidance sensors
return sitl->measure_distance_at_angle_bf(location, sitl->sonar_rot.get()*45);
default:
AP_BoardConfig::config_error("Bad simulated sonar rotation");
}
#endif
// default is ground sensing rangefinders
return sitl->state.height_agl;
}
float Aircraft::rangefinder_range() const
@ -543,11 +553,6 @@ float Aircraft::rangefinder_range() const
}
}
if (fabs(roll) >= 90.0 || fabs(pitch) >= 90.0) {
// not going to hit the ground....
return INFINITY;
}
float altitude = perpendicular_distance_to_rangefinder_surface();
// sensor position offset in body frame
@ -565,8 +570,32 @@ float Aircraft::rangefinder_range() const
altitude -= relPosSensorEF.z;
}
// adjust for apparent altitude with roll
altitude /= cosf(radians(roll)) * cosf(radians(pitch));
const auto orientation = (Rotation)sitl->sonar_rot.get();
#if SITL_RANGEFINDER_AS_OBJECT_SENSOR
/*
rover and copter using SITL rangefinders as obstacle sensors
*/
if (SITL_RANGEFINDER_IS_YAW_ONLY(orientation)) {
if (fabs(roll) >= 90.0 || fabs(pitch) >= 90.0) {
// not going to hit the ground....
return INFINITY;
}
altitude /= cosf(radians(roll)) * cosf(radians(pitch));
} else
#endif
{
// adjust for rotation based on orientation of the sensor
Matrix3f rotmat;
sitl->state.quaternion.rotation_matrix(rotmat);
Vector3f v{1, 0, 0};
v.rotate(orientation);
v = rotmat * v;
if (!is_positive(v.z)) {
return INFINITY;
}
altitude /= v.z;
}
// Add some noise on reading
altitude += sitl->sonar_noise * rand_float();