diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 7ce211192e..3cf8725ec0 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -32,7 +32,7 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) : _ahrs(ahrs), _inav(inav), - _have_estimate(false), + _last_update_ms(0), _backend(NULL) { // set parameters to defaults @@ -92,29 +92,37 @@ void AC_PrecLand::update(float alt_above_terrain_cm) } } -// get_target_shift - returns 3D vector of earth-frame position adjustments to target -Vector3f AC_PrecLand::get_target_shift(const Vector3f &orig_target) +bool AC_PrecLand::target_acquired() { - Vector3f shift; // default shift initialised to zero + return (AP_HAL::millis()-_last_update_ms) < 1000; +} - // do not shift target if not enabled or no position estimate - if (_backend == NULL || !_have_estimate) { - return shift; +bool AC_PrecLand::get_target_position(Vector3f& ret) +{ + if (!target_acquired()) { + return false; } - // shift is target_offset - (original target - current position) - Vector3f curr_offset_from_target = orig_target - _inav.get_position(); - shift = _target_pos_offset - curr_offset_from_target; - shift.z = 0.0f; + ret = _target_pos; + return true; +} + +bool AC_PrecLand::get_target_position_relative(Vector3f& ret) +{ + if (!target_acquired()) { + return false; + } - // record we have consumed this reading (perhaps there is a cleaner way to do this using timestamps) - _have_estimate = false; + ret = _target_pos_rel; + return true; +} - // return adjusted target - return shift; +bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret) +{ + return false; } -// calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate +// converts sensor's body-frame angles to earth-frame angles and position estimate // raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame) // earth-frame angles stored in _ef_angle_to_target // position estimate is stored in _target_pos @@ -122,42 +130,56 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) { // exit immediately if not enabled if (_backend == NULL) { - _have_estimate = false; return; } // get angles to target from backend if (!_backend->get_angle_to_target(_angle_to_target.x, _angle_to_target.y)) { - _have_estimate = false; return; } - float x_rad; - float y_rad; - - if(_backend->get_frame_of_reference() == MAV_FRAME_LOCAL_NED){ - //don't subtract vehicle lean angles - x_rad = _angle_to_target.x; - y_rad = -_angle_to_target.y; - }else{ // assume MAV_FRAME_BODY_NED (i.e. a hard-mounted sensor) - // subtract vehicle lean angles - x_rad = _angle_to_target.x - _ahrs.roll; - y_rad = -_angle_to_target.y + _ahrs.pitch; - } + _angle_to_target.x = -_angle_to_target.x; + _angle_to_target.y = -_angle_to_target.y; - // rotate to earth-frame angles - _ef_angle_to_target.x = y_rad*_ahrs.cos_yaw() - x_rad*_ahrs.sin_yaw(); - _ef_angle_to_target.y = y_rad*_ahrs.sin_yaw() + x_rad*_ahrs.cos_yaw(); + // compensate for delay + _angle_to_target.x -= _ahrs.get_gyro().x*4.0e-2f; + _angle_to_target.y -= _ahrs.get_gyro().y*4.0e-2f; - // get current altitude (constrained to no lower than 50cm) - float alt = MAX(alt_above_terrain_cm, 50.0f); + Vector3f target_vec_ned; - // convert earth-frame angles to earth-frame position offset - _target_pos_offset.x = alt*tanf(_ef_angle_to_target.x); - _target_pos_offset.y = alt*tanf(_ef_angle_to_target.y); - _target_pos_offset.z = 0; // not used + if (_angle_to_target.is_zero()) { + target_vec_ned = Vector3f(0.0f,0.0f,1.0f); + } else { + float theta = _angle_to_target.length(); + Vector3f axis = Vector3f(_angle_to_target.x, _angle_to_target.y, 0.0f)/theta; + float sin_theta = sinf(theta); + float cos_theta = cosf(theta); - _have_estimate = true; + target_vec_ned.x = axis.x*axis.z*(1.0f - cos_theta) + axis.y*sin_theta; + target_vec_ned.y = axis.y*axis.z*(1.0f - cos_theta) - axis.x*sin_theta; + target_vec_ned.z = cos_theta + sq(axis.z)*(1.0f-cos_theta); + } + + // rotate into NED frame + target_vec_ned = _ahrs.get_rotation_body_to_ned()*target_vec_ned; + + // extract the angles to target (logging only) + _ef_angle_to_target.x = atan2f(target_vec_ned.z,target_vec_ned.x); + _ef_angle_to_target.y = atan2f(target_vec_ned.z,target_vec_ned.y); + + // ensure that the angle to target is no more than 70 degrees + if (target_vec_ned.z > 0.26f) { + // get current altitude (constrained to be positive) + float alt = MAX(alt_above_terrain_cm, 0.0f); + float dist = alt/target_vec_ned.z; + // + _target_pos_rel.x = target_vec_ned.x*dist; + _target_pos_rel.y = target_vec_ned.y*dist; + _target_pos_rel.z = alt; // not used + _target_pos = _inav.get_position()+_target_pos_rel; + + _last_update_ms = AP_HAL::millis(); + } } // handle_msg - Process a LANDING_TARGET mavlink message diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 48ddb1cb62..12ab889167 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -4,6 +4,8 @@ #include #include #include +#include +#include // declare backend classes class AC_PrecLand_Backend; @@ -33,42 +35,57 @@ public: PRECLAND_TYPE_IRLOCK }; - // Constructor + // constructor AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav); - // init - perform any required initialisation of landing controllers + // perform any required initialisation of landing controllers void init(); - // healthy - returns true if precision landing is healthy + // returns true if precision landing is healthy bool healthy() { return _backend_state.healthy; } - // update - give chance to driver to get updates from sensor + // returns time of last update + uint32_t last_update_ms() { return _last_update_ms; } + + // give chance to driver to get updates from sensor void update(float alt_above_terrain_cm); - // get_target_shift - returns 3D vector of earth-frame position adjustments to target + // returns 3D vector of earth-frame position adjustments to target Vector3f get_target_shift(const Vector3f& orig_target); - // handle_msg - Process a LANDING_TARGET mavlink message + // returns target position relative to origin + bool get_target_position(Vector3f& ret); + + // returns target position relative to vehicle + bool get_target_position_relative(Vector3f& ret); + + // returns target velocity relative to vehicle + bool get_target_velocity_relative(Vector3f& ret); + + // returns true when the landing target has been detected + bool target_acquired(); + + // process a LANDING_TARGET mavlink message void handle_msg(mavlink_message_t* msg); // accessors for logging bool enabled() const { return _enabled; } const Vector2f& last_bf_angle_to_target() const { return _angle_to_target; } const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; } - const Vector3f& last_target_pos_offset() const { return _target_pos_offset; } + const Vector3f& last_target_pos_offset() const { return _target_pos_rel; } // parameter var table static const struct AP_Param::GroupInfo var_info[]; private: - // calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate + // converts sensor's body-frame angles to earth-frame angles and position estimate // angles stored in _angle_to_target // earth-frame angles stored in _ef_angle_to_target // position estimate is stored in _target_pos void calc_angles_and_pos(float alt_above_terrain_cm); - // get_behaviour - returns enabled parameter as an behaviour + // returns enabled parameter as an behaviour enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); } // references to inertial nav and ahrs libraries @@ -79,13 +96,15 @@ private: AP_Int8 _enabled; // enabled/disabled and behaviour AP_Int8 _type; // precision landing controller type + uint32_t _last_update_ms; // epoch time in millisecond when update is called + // output from sensor (stored for logging) Vector2f _angle_to_target; // last raw sensor angle to target Vector2f _ef_angle_to_target;// last earth-frame angle to target - // output from controller - bool _have_estimate; // true if we have a recent estimated position offset - Vector3f _target_pos_offset; // estimate target position offset from vehicle in earth-frame + // estimator output + Vector3f _target_pos_rel; // estimate target position relative to vehicle in NEU cm + Vector3f _target_pos; // estimate target position in NEU cm // backend state struct precland_state {