diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 49c7a11060..ece8653436 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -727,6 +727,9 @@ void AC_PrecLand::run_output_prediction() _target_vel_rel_out_NE.x -= vel_ned_rel_imu.x; _target_vel_rel_out_NE.y -= vel_ned_rel_imu.y; + // remember vehicle velocity + UNUSED_RESULT(_ahrs.get_velocity_NED(_last_veh_velocity_NED_ms)); + // Apply land offset Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f; _target_pos_rel_out_NE.x += land_ofs_ned_m.x; @@ -742,6 +745,45 @@ void AC_PrecLand::run_output_prediction() _last_valid_target_ms = AP_HAL::millis(); } +/* + get target location lat/lon. Note that altitude in returned + location is not reliable + */ +bool AC_PrecLand::get_target_location(Location &loc) +{ + if (!target_acquired()) { + return false; + } + if (!AP::ahrs().get_origin(loc)) { + return false; + } + loc.offset(_last_target_pos_rel_origin_NED.x, _last_target_pos_rel_origin_NED.y); + loc.alt -= _last_target_pos_rel_origin_NED.z*100; + return true; +} + +/* + get the absolute velocity of the target in m/s. + return false if we cannot estimate target velocity or if the target is not acquired +*/ +bool AC_PrecLand::get_target_velocity(Vector2f& target_vel) +{ + if (!(_options & PLND_OPTION_MOVING_TARGET)) { + // the target should not be moving + return false; + } + if ((EstimatorType)_estimator_type.get() == EstimatorType::RAW_SENSOR) { + return false; + } + Vector2f target_vel_rel_cms; + if (!get_target_velocity_relative_cms(target_vel_rel_cms)) { + return false; + } + // return the absolute velocity + target_vel = (target_vel_rel_cms*0.01) + _last_veh_velocity_NED_ms.xy(); + return true; +} + #if HAL_LOGGING_ENABLED // Write a precision landing entry void AC_PrecLand::Write_Precland() diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index a25b30c71f..e11a51f81d 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -17,6 +17,7 @@ class AC_PrecLand_Companion; class AC_PrecLand_IRLock; class AC_PrecLand_SITL_Gazebo; class AC_PrecLand_SITL; +class Location; class AC_PrecLand { @@ -116,6 +117,18 @@ public: bool allow_precland_after_reposition() const { return _options & PLND_OPTION_PRECLAND_AFTER_REPOSITION; } bool do_fast_descend() const { return _options & PLND_OPTION_FAST_DESCEND; } + /* + get target location lat/lon. Note that altitude in returned + location is not reliable + */ + bool get_target_location(Location &loc); + + /* + get the absolute velocity of the target in m/s. + return false if we cannot estimate target velocity or if the target is not acquired + */ + bool get_target_velocity(Vector2f& ret); + // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -215,6 +228,7 @@ private: Vector2f _target_pos_rel_out_NE; // target's position relative to the camera, fed into position controller Vector2f _target_vel_rel_out_NE; // target's velocity relative to the CG, fed into position controller + Vector3f _last_veh_velocity_NED_ms; // AHRS velocity at last estimate TargetState _current_target_state; // Current status of the landing target