Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Follow: change to using position-from-origin internally #27250

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+81 −19
Open

AP_Follow: change to using position-from-origin internally #27250

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 68 additions & 16 deletions libraries/AP_Follow/AP_Follow.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,26 @@ void AP_Follow::clear_offsets_if_required()

// get target's estimated location
bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
{
Vector3p pos_ned;
Vector3f local_vel_ned; // so we either change both return parameters or neither
if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
return false;
}
if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned * 0.01)) {
return false;
}
vel_ned = local_vel_ned;

if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
}

return true;
}

// get target's estimated location and velocity, both NED SI from origin
bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
{
// exit immediately if not enabled
if (!_enabled) {
Expand All @@ -187,16 +207,17 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne
}

// project the vehicle position
Location last_loc = _target_location;
last_loc.offset(vel_ned.x * dt, vel_ned.y * dt);
last_loc.alt -= vel_ned.z * 100.0f * dt; // convert m/s to cm/s, multiply by dt. minus because NED
Vector3p projected_pos_ned = _target_position_ned;
const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
projected_pos_ned += vel_ned_p * dt;

// return latest position estimate
loc = last_loc;
pos_ned = projected_pos_ned;
return true;
}

// get distance vector to target (in meters) and target's velocity all in NED frame
// FIXME: use the target_position_ned vector!
bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
{
// get our location
Expand Down Expand Up @@ -341,6 +362,7 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
return false;
}

Location _target_location;
_target_location.lat = packet.lat;
_target_location.lng = packet.lon;

Expand All @@ -352,6 +374,10 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
// absolute altitude
_target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
}
if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
return false;
}
_target_position_ned.z = -_target_position_ned.z; // NEU->NED

_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
Expand Down Expand Up @@ -386,18 +412,15 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
return false;
}

Location new_loc = _target_location;
Location new_loc;
new_loc.lat = packet.lat;
new_loc.lng = packet.lon;
new_loc.set_alt_cm(packet.alt*100, Location::AltFrame::ABSOLUTE);

// FOLLOW_TARGET is always AMSL, change the provided alt to
// above home if we are configured for relative alt
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
!new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
return false;
}
_target_location = new_loc;
_target_position_ned.z = -_target_position_ned.z; // NEU->NED

if (packet.est_capabilities & (1<<1)) {
_target_velocity_ned.x = packet.vel[0]; // velocity north
Expand Down Expand Up @@ -436,6 +459,12 @@ void AP_Follow::Log_Write_FOLL()
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));

Location _target_location;
UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
_target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
}

// log lead's estimated vs reported position
// @LoggerMessage: FOLL
// @Description: Follow library diagnostic data
Expand Down Expand Up @@ -537,17 +566,40 @@ void AP_Follow::clear_dist_and_bearing_to_target()
_bearing_to_target = 0.0f;
}

// get target's estimated origin-relative-position and velocity (both
// in SI NED), with offsets added
bool AP_Follow::get_target_position_and_velocity_ofs(Vector3p &pos_ned, Vector3f &vel_ned) const
{
Vector3f ofs_ned;
if (!get_offsets_ned(ofs_ned)) {
return false;
}
if (!get_target_position_and_velocity_ofs(pos_ned, vel_ned)) {
return false;
}
// can't add Vector3p and Vector3f directly:
pos_ned.x += ofs_ned.x;
pos_ned.y += ofs_ned.y;
pos_ned.z += ofs_ned.z;
return true;
}

// get target's estimated location and velocity (in NED), with offsets added
bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
{
Vector3f ofs;
if (!get_offsets_ned(ofs) ||
!get_target_location_and_velocity(loc, vel_ned)) {
Vector3p pos_ned;
Vector3f local_vel_ned; // so we either change both return parameters or neither
if (!get_target_position_and_velocity_ofs(pos_ned, local_vel_ned)) {
return false;
}
// apply offsets
loc.offset(ofs.x, ofs.y);
loc.alt -= ofs.z*100;
if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned * 0.01)) {
return false;
}
if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
}

vel_ned = local_vel_ned;
return true;
}

Expand Down
16 changes: 13 additions & 3 deletions libraries/AP_Follow/AP_Follow.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,10 +68,20 @@ class AP_Follow
// true if we have a valid target location estimate
bool have_target() const;

// get target's estimated location and velocity (in NED)
// get target's estimated position (NED-from-origin) and velocity (in NED)
// from position-from-origin. metres and metres/second
bool get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const;

// get target's estimated position (NED-from-origin) and velocity (in NED)
// from position-from-origin with offsets added. metres and metres/second
bool get_target_position_and_velocity_ofs(Vector3p &pos_ned, Vector3f &vel_ned) const;

// get target's estimated location and velocity (in NED). Derived
// from position-from-origin.
bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const;

// get target's estimated location and velocity (in NED), with offsets added
// get target's estimated location and velocity (in NED), with
// offsets added. Derived from position-from-origin.
bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const;

// get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
Expand Down Expand Up @@ -156,7 +166,7 @@ class AP_Follow

// local variables
uint32_t _last_location_update_ms; // system time of last position update
Location _target_location; // last known location of target
Vector3p _target_position_ned; // last known position of target
Vector3f _target_velocity_ned; // last known velocity of target in NED frame in m/s
Vector3f _target_accel_ned; // last known acceleration of target in NED frame in m/s/s
uint32_t _last_heading_update_ms; // system time of last heading update
Expand Down
Loading