Skip to content

Commit

Permalink
AP_Follow: change to using position-from-origin internally
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Jul 1, 2024
1 parent 4ed1b6b commit e7de2cc
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 19 deletions.
84 changes: 68 additions & 16 deletions libraries/AP_Follow/AP_Follow.cpp
View file
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
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

0 comments on commit e7de2cc

Please sign in to comment.