mirror of https://github.com/ArduPilot/ardupilot
AC_PrecLand: added get_target_location and get_target_velocity
This commit is contained in:
parent
155f31a3a2
commit
e5f092482d
|
@ -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.x -= vel_ned_rel_imu.x;
|
||||||
_target_vel_rel_out_NE.y -= vel_ned_rel_imu.y;
|
_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
|
// 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;
|
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;
|
_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();
|
_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
|
#if HAL_LOGGING_ENABLED
|
||||||
// Write a precision landing entry
|
// Write a precision landing entry
|
||||||
void AC_PrecLand::Write_Precland()
|
void AC_PrecLand::Write_Precland()
|
||||||
|
|
|
@ -17,6 +17,7 @@ class AC_PrecLand_Companion;
|
||||||
class AC_PrecLand_IRLock;
|
class AC_PrecLand_IRLock;
|
||||||
class AC_PrecLand_SITL_Gazebo;
|
class AC_PrecLand_SITL_Gazebo;
|
||||||
class AC_PrecLand_SITL;
|
class AC_PrecLand_SITL;
|
||||||
|
class Location;
|
||||||
|
|
||||||
class AC_PrecLand
|
class AC_PrecLand
|
||||||
{
|
{
|
||||||
|
@ -116,6 +117,18 @@ public:
|
||||||
bool allow_precland_after_reposition() const { return _options & PLND_OPTION_PRECLAND_AFTER_REPOSITION; }
|
bool allow_precland_after_reposition() const { return _options & PLND_OPTION_PRECLAND_AFTER_REPOSITION; }
|
||||||
bool do_fast_descend() const { return _options & PLND_OPTION_FAST_DESCEND; }
|
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
|
// parameter var table
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
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_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
|
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
|
TargetState _current_target_state; // Current status of the landing target
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue