diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index e51369f9ff..657e62f734 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -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(); diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 26db39b21c..c3b1b0db63 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -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 _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