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

AC_PrecLand: add _ORIENT param to precland library 

The orient parameter will help us construct a direction of approach vector in vehicle body frame. This vector would help
us rotate the target vector from sensor frame to body frame and determine the horizontal position error of vehicle for
more orientations than just PITCH_270
This commit is contained in:
Shiv Tyagi 2022-06-17 17:32:02 +05:30 committed by Randy Mackay
parent 82d1750e8c
commit bc3ff20871
2 changed files with 45 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -12,6 +12,12 @@
extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_Rover)
# define AC_PRECLAND_ORIENT_DEFAULT Rotation::ROTATION_NONE
#else
# define AC_PRECLAND_ORIENT_DEFAULT Rotation::ROTATION_PITCH_270
#endif
static const uint32_t EKF_INIT_TIME_MS = 2000; // EKF initialisation requires this many milliseconds of good sensor data
static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500; // Sensor must update within this many ms during EKF init, else init will fail
static const uint32_t LANDING_TARGET_TIMEOUT_MS = 2000; // Sensor must update within this many ms, else prec landing will be switched off
@ -170,6 +176,14 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @User: Advanced
AP_GROUPINFO("OPTIONS", 17, AC_PrecLand, _options, 0),
// @Param: ORIENT
// @DisplayName: Camera Orientation
// @Description: Orientation of camera/sensor on body
// @Values: 0:Forward, 4:Back, 25:Down
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FRAME("ORIENT", 18, AC_PrecLand, _orient, AC_PRECLAND_ORIENT_DEFAULT, AP_PARAM_FRAME_ROVER),
AP_GROUPEND
};
@ -242,6 +256,9 @@ void AC_PrecLand::init(uint16_t update_rate_hz)
if (_backend != nullptr) {
_backend->init();
}
_approach_vector_body.x = 1;
_approach_vector_body.rotate(_orient);
}
// update - give chance to driver to get updates from sensor
@ -591,6 +608,21 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
// Apply sensor yaw alignment rotation
target_vec_unit_body.rotate_xy(radians(_yaw_align*0.01f));
}
// rotate vector based on sensor oriention to get correct body frame vector
if (_orient != ROTATION_PITCH_270) {
// by default, the vector is constructed downwards in body frame
// hence, we do not do any rotation if the orientation is downwards
// if it is some other orientation, we first bring the vector to forward
// and then we rotate it to desired orientation
// because the rotations are measured with respect to a vector pointing towards front in body frame
// for eg, if orientation is back, i.e., ROTATION_YAW_180,
// the vector is first brought to front and then rotation by YAW 180 to take it to the back of vehicle
target_vec_unit_body.rotate(ROTATION_PITCH_90); // bring vector to front
target_vec_unit_body.rotate(_orient); // rotate it to desired orientation
}
return true;
} else {
return false;
@ -603,11 +635,13 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
if (retrieve_los_meas(target_vec_unit_body)) {
const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
const bool target_vec_valid = target_vec_unit_body.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f;
const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body;
const bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
const Vector3f approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body;
const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
if (target_vec_valid && alt_valid) {
float dist, alt;
// distance to target and distance to target along approach vector
float dist_to_target, dist_to_target_along_av;
// figure out ned camera orientation w.r.t its offset
Vector3f cam_pos_ned;
if (!_cam_offset.get().is_zero()) {
@ -617,13 +651,12 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
}
if (_backend->distance_to_target() > 0.0f) {
// sensor has provided distance to landing target
dist = _backend->distance_to_target();
alt = dist * target_vec_unit_ned.z;
dist_to_target = _backend->distance_to_target();
} else {
// sensor only knows the horizontal location of the landing target
// rely on rangefinder for the vertical target
alt = MAX(rangefinder_alt_m - cam_pos_ned.z, 0.0f);
dist = alt / target_vec_unit_ned.z;
dist_to_target_along_av = MAX(rangefinder_alt_m - cam_pos_ned.projected(approach_vector_NED).length(), 0.0f);
dist_to_target = dist_to_target_along_av / target_vec_unit_ned.projected(approach_vector_NED).length();
}
// Compute camera position relative to IMU
@ -631,7 +664,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
const Vector3f cam_pos_ned_rel_imu = cam_pos_ned - accel_pos_ned;
// Compute target position relative to IMU
_target_pos_rel_meas_NED = Vector3f{target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt} + cam_pos_ned_rel_imu;
_target_pos_rel_meas_NED = (target_vec_unit_ned * dist_to_target) + cam_pos_ned_rel_imu;
// store the current relative down position so that if we need to retry landing, we know at this height landing target can be found
const AP_AHRS &_ahrs = AP::ahrs();

5
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -54,6 +54,9 @@ public:
// vehicle has to be closer than this many cm's to the target before descending towards target
float get_max_xy_error_before_descending_cm() const { return _xy_max_dist_desc * 100.0f; }
// returns orientation of sensor
Rotation get_orient() const { return _orient; }
// returns ekf outlier count
uint32_t ekf_outlier_count() const { return _outlier_reject_count; }
@ -174,6 +177,7 @@ private:
AP_Float _sensor_min_alt; // PrecLand minimum height required for detecting target
AP_Float _sensor_max_alt; // PrecLand maximum height the sensor can detect target
AP_Int16 _options; // Bitmask for extra options
AP_Enum<Rotation> _orient; // Orientation of camera/sensor
uint32_t _last_update_ms; // system time in millisecond when update was last called
bool _target_acquired; // true if target has been seen recently after estimator is initialized
@ -186,6 +190,7 @@ private:
uint32_t _outlier_reject_count; // mini-EKF's outlier counter (3 consecutive outliers lead to EKF accepting updates)
Vector3f _target_pos_rel_meas_NED; // target's relative position as 3D vector
Vector3f _approach_vector_body; // unit vector in landing approach direction (in body frame)
Vector3f _last_target_pos_rel_origin_NED; // stores the last known location of the target horizontally, and the height of the vehicle where it detected this target in meters NED
Vector3f _last_vehicle_pos_NED; // stores the position of the vehicle when landing target was last detected in m and NED