From c4c6c2061774c46b00284cc66385ca68f88cfac3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 6 Jun 2024 16:22:12 +1000 Subject: [PATCH] AP_Follow: change to using position-from-origin internally --- libraries/AP_Follow/AP_Follow.cpp | 84 +++++++++++++++++++++++++------ libraries/AP_Follow/AP_Follow.h | 16 ++++-- 2 files changed, 81 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index b4acb2fc04..b770741b86 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -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) { @@ -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 @@ -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; @@ -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 @@ -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 @@ -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 @@ -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; } diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 48c43fc9ff..aeff6f8a88 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -68,10 +68,20 @@ public: // 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 @@ -156,7 +166,7 @@ private: // 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