2018-01-25 08:35:05 -04:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2023-08-11 09:27:17 -03:00
# include "AP_Follow_config.h"
# if AP_FOLLOW_ENABLED
2018-01-25 08:35:05 -04:00
# include <AP_HAL/AP_HAL.h>
# include "AP_Follow.h"
# include <ctype.h>
# include <stdio.h>
2019-02-11 01:11:51 -04:00
# include <AP_AHRS/AP_AHRS.h>
2019-04-04 08:42:24 -03:00
# include <AP_Logger/AP_Logger.h>
2022-08-14 23:18:30 -03:00
# include <GCS_MAVLink/GCS.h>
2022-10-27 22:38:07 -03:00
# include <AP_Vehicle/AP_Vehicle_Type.h>
2019-02-11 01:11:51 -04:00
2018-01-25 08:35:05 -04:00
extern const AP_HAL : : HAL & hal ;
# define AP_FOLLOW_TIMEOUT_MS 3000 // position estimate timeout after 1 second
2023-10-11 04:41:53 -03:00
# define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we have not heard from them in 10 seconds
2018-01-25 08:35:05 -04:00
# define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame
2018-07-20 06:20:35 -03:00
# define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading
2018-01-25 08:35:05 -04:00
2018-06-03 00:42:25 -03:00
# define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE 1 // relative altitude is used by default
2018-01-25 08:35:05 -04:00
# define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default
2022-01-04 00:52:47 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
# define AP_FOLLOW_ALT_TYPE_DEFAULT 0
# else
# define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
# endif
AP_Follow * AP_Follow : : _singleton ;
2018-01-25 08:35:05 -04:00
// table of user settable parameters
const AP_Param : : GroupInfo AP_Follow : : var_info [ ] = {
// @Param: _ENABLE
// @DisplayName: Follow enable/disable
// @Description: Enabled/disable following a target
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO_FLAGS ( " _ENABLE " , 1 , AP_Follow , _enabled , 0 , AP_PARAM_FLAG_ENABLE ) ,
// 2 is reserved for TYPE parameter
// @Param: _SYSID
// @DisplayName: Follow target's mavlink system id
// @Description: Follow target's mavlink system id
// @Range: 0 255
// @User: Standard
AP_GROUPINFO ( " _SYSID " , 3 , AP_Follow , _sysid , 0 ) ,
// 4 is reserved for MARGIN parameter
// @Param: _DIST_MAX
// @DisplayName: Follow distance maximum
// @Description: Follow distance maximum. targets further than this will be ignored
// @Units: m
// @Range: 1 1000
// @User: Standard
AP_GROUPINFO ( " _DIST_MAX " , 5 , AP_Follow , _dist_max , 100 ) ,
// @Param: _OFS_TYPE
// @DisplayName: Follow offset type
// @Description: Follow offset type
// @Values: 0:North-East-Down, 1:Relative to lead vehicle heading
// @User: Standard
AP_GROUPINFO ( " _OFS_TYPE " , 6 , AP_Follow , _offset_type , AP_FOLLOW_OFFSET_TYPE_NED ) ,
// @Param: _OFS_X
// @DisplayName: Follow offsets in meters north/forward
// @Description: Follow offsets in meters north/forward. If positive, this vehicle fly ahead or north of lead vehicle. Depends on FOLL_OFS_TYPE
// @Range: -100 100
// @Units: m
// @Increment: 1
// @User: Standard
// @Param: _OFS_Y
// @DisplayName: Follow offsets in meters east/right
2018-12-24 01:39:01 -04:00
// @Description: Follow offsets in meters east/right. If positive, this vehicle will fly to the right or east of lead vehicle. Depends on FOLL_OFS_TYPE
2018-01-25 08:35:05 -04:00
// @Range: -100 100
// @Units: m
// @Increment: 1
// @User: Standard
// @Param: _OFS_Z
// @DisplayName: Follow offsets in meters down
2018-12-24 01:39:01 -04:00
// @Description: Follow offsets in meters down. If positive, this vehicle will fly below the lead vehicle
2018-01-25 08:35:05 -04:00
// @Range: -100 100
// @Units: m
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " _OFS " , 7 , AP_Follow , _offset , 0 ) ,
2020-03-26 21:51:15 -03:00
# if !(APM_BUILD_TYPE(APM_BUILD_Rover))
2018-01-25 08:35:05 -04:00
// @Param: _YAW_BEHAVE
// @DisplayName: Follow yaw behaviour
// @Description: Follow yaw behaviour
// @Values: 0:None,1:Face Lead Vehicle,2:Same as Lead vehicle,3:Direction of Flight
// @User: Standard
AP_GROUPINFO ( " _YAW_BEHAVE " , 8 , AP_Follow , _yaw_behave , 1 ) ,
2019-10-02 18:09:02 -03:00
# endif
2018-01-25 08:35:05 -04:00
// @Param: _POS_P
// @DisplayName: Follow position error P gain
// @Description: Follow position error P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
// @Range: 0.01 1.00
// @Increment: 0.01
// @User: Standard
AP_SUBGROUPINFO ( _p_pos , " _POS_ " , 9 , AP_Follow , AC_P ) ,
2020-03-26 21:51:15 -03:00
# if !(APM_BUILD_TYPE(APM_BUILD_Rover))
2018-06-03 00:42:25 -03:00
// @Param: _ALT_TYPE
// @DisplayName: Follow altitude type
// @Description: Follow altitude type
2020-02-16 23:26:56 -04:00
// @Values: 0:absolute, 1:relative
2018-06-03 00:42:25 -03:00
// @User: Standard
2022-01-04 00:52:47 -04:00
AP_GROUPINFO ( " _ALT_TYPE " , 10 , AP_Follow , _alt_type , AP_FOLLOW_ALT_TYPE_DEFAULT ) ,
2019-10-02 18:09:02 -03:00
# endif
2018-06-03 00:42:25 -03:00
2023-05-22 01:02:38 -03:00
// @Param: _OPTIONS
// @DisplayName: Follow options
// @Description: Follow options bitmask
// @Values: 0:None,1: Mount Follows lead vehicle on mode enter
// @User: Standard
AP_GROUPINFO ( " _OPTIONS " , 11 , AP_Follow , _options , 0 ) ,
2018-01-25 08:35:05 -04:00
AP_GROUPEND
} ;
/*
The constructor also initialises the proximity sensor . Note that this
constructor is not called until detect ( ) returns true , so we
already know that we should setup the proximity sensor
*/
2018-03-31 09:27:58 -03:00
AP_Follow : : AP_Follow ( ) :
2018-01-25 08:35:05 -04:00
_p_pos ( AP_FOLLOW_POS_P_DEFAULT )
{
2022-01-04 00:52:47 -04:00
_singleton = this ;
2018-01-25 08:35:05 -04:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
2019-10-03 22:19:33 -03:00
// restore offsets to zero if necessary, should be called when vehicle exits follow mode
void AP_Follow : : clear_offsets_if_required ( )
{
if ( _offsets_were_zero ) {
2022-07-20 06:43:20 -03:00
_offset . set ( Vector3f ( ) ) ;
2019-10-03 22:19:33 -03:00
}
_offsets_were_zero = false ;
}
2018-01-25 08:35:05 -04:00
// get target's estimated location
bool AP_Follow : : get_target_location_and_velocity ( Location & loc , Vector3f & vel_ned ) const
{
// exit immediately if not enabled
if ( ! _enabled ) {
return false ;
}
// check for timeout
if ( ( _last_location_update_ms = = 0 ) | | ( AP_HAL : : millis ( ) - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS ) ) {
return false ;
}
// calculate time since last actual position update
const float dt = ( AP_HAL : : millis ( ) - _last_location_update_ms ) * 0.001f ;
// get velocity estimate
if ( ! get_velocity_ned ( vel_ned , dt ) ) {
return false ;
}
// project the vehicle position
Location last_loc = _target_location ;
2019-02-24 20:16:20 -04:00
last_loc . offset ( vel_ned . x * dt , vel_ned . y * dt ) ;
2018-12-30 05:56:25 -04:00
last_loc . alt - = vel_ned . z * 100.0f * dt ; // convert m/s to cm/s, multiply by dt. minus because NED
2018-01-25 08:35:05 -04:00
// return latest position estimate
loc = last_loc ;
return true ;
}
// get distance vector to target (in meters) and target's velocity all in NED frame
bool AP_Follow : : get_target_dist_and_vel_ned ( Vector3f & dist_ned , Vector3f & dist_with_offs , Vector3f & vel_ned )
{
// get our location
Location current_loc ;
2022-01-20 19:42:40 -04:00
if ( ! AP : : ahrs ( ) . get_location ( current_loc ) ) {
2018-12-11 05:36:58 -04:00
clear_dist_and_bearing_to_target ( ) ;
2018-01-25 08:35:05 -04:00
return false ;
2018-06-03 00:42:25 -03:00
}
2018-01-25 08:35:05 -04:00
// get target location and velocity
Location target_loc ;
Vector3f veh_vel ;
if ( ! get_target_location_and_velocity ( target_loc , veh_vel ) ) {
2018-12-11 05:36:58 -04:00
clear_dist_and_bearing_to_target ( ) ;
2018-01-25 08:35:05 -04:00
return false ;
}
2018-06-03 00:42:25 -03:00
// change to altitude above home if relative altitude is being used
2019-01-01 20:00:02 -04:00
if ( target_loc . relative_alt = = 1 ) {
2018-06-03 00:42:25 -03:00
current_loc . alt - = AP : : ahrs ( ) . get_home ( ) . alt ;
}
2018-01-25 08:35:05 -04:00
// calculate difference
2019-04-08 10:29:02 -03:00
const Vector3f dist_vec = current_loc . get_distance_NED ( target_loc ) ;
2018-01-25 08:35:05 -04:00
// fail if too far
if ( is_positive ( _dist_max . get ( ) ) & & ( dist_vec . length ( ) > _dist_max ) ) {
2018-12-11 05:36:58 -04:00
clear_dist_and_bearing_to_target ( ) ;
2018-01-25 08:35:05 -04:00
return false ;
}
// initialise offsets from distance vector if required
init_offsets_if_required ( dist_vec ) ;
// get offsets
Vector3f offsets ;
if ( ! get_offsets_ned ( offsets ) ) {
2018-12-11 05:36:58 -04:00
clear_dist_and_bearing_to_target ( ) ;
2018-01-25 08:35:05 -04:00
return false ;
}
2018-12-11 05:36:58 -04:00
// calculate results
2018-01-25 08:35:05 -04:00
dist_ned = dist_vec ;
dist_with_offs = dist_vec + offsets ;
vel_ned = veh_vel ;
2018-12-11 05:36:58 -04:00
// record distance and heading for reporting purposes
if ( is_zero ( dist_with_offs . x ) & & is_zero ( dist_with_offs . y ) ) {
clear_dist_and_bearing_to_target ( ) ;
} else {
_dist_to_target = safe_sqrt ( sq ( dist_with_offs . x ) + sq ( dist_with_offs . y ) ) ;
_bearing_to_target = degrees ( atan2f ( dist_with_offs . y , dist_with_offs . x ) ) ;
}
2018-01-25 08:35:05 -04:00
return true ;
}
// get target's heading in degrees (0 = north, 90 = east)
2018-02-15 15:16:43 -04:00
bool AP_Follow : : get_target_heading_deg ( float & heading ) const
2018-01-25 08:35:05 -04:00
{
// exit immediately if not enabled
if ( ! _enabled ) {
return false ;
}
// check for timeout
if ( ( _last_heading_update_ms = = 0 ) | | ( AP_HAL : : millis ( ) - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS ) ) {
return false ;
}
// return latest heading estimate
heading = _target_heading ;
return true ;
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow : : handle_msg ( const mavlink_message_t & msg )
{
// exit immediately if not enabled
if ( ! _enabled ) {
return ;
}
// skip our own messages
if ( msg . sysid = = mavlink_system . sysid ) {
return ;
}
// skip message if not from our target
2018-12-06 20:05:06 -04:00
if ( _sysid ! = 0 & & msg . sysid ! = _sysid ) {
if ( _automatic_sysid ) {
2018-01-25 08:35:05 -04:00
// maybe timeout who we were following...
if ( ( _last_location_update_ms = = 0 ) | | ( AP_HAL : : millis ( ) - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS ) ) {
2018-12-06 20:05:06 -04:00
_sysid . set ( 0 ) ;
2018-01-25 08:35:05 -04:00
}
}
return ;
}
// decode global-position-int message
2022-02-25 22:04:46 -04:00
bool updated = false ;
2018-01-25 08:35:05 -04:00
2022-02-25 22:04:46 -04:00
switch ( msg . msgid ) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT : {
2018-01-25 08:35:05 -04:00
// decode message
mavlink_global_position_int_t packet ;
mavlink_msg_global_position_int_decode ( & msg , & packet ) ;
// ignore message if lat and lon are (exactly) zero
if ( ( packet . lat = = 0 & & packet . lon = = 0 ) ) {
return ;
}
_target_location . lat = packet . lat ;
_target_location . lng = packet . lon ;
2018-07-14 02:37:24 -03:00
2018-06-03 00:42:25 -03:00
// select altitude source based on FOLL_ALT_TYPE param
if ( _alt_type = = AP_FOLLOW_ALTITUDE_TYPE_RELATIVE ) {
2022-02-25 22:04:46 -04:00
// above home alt
_target_location . set_alt_cm ( packet . relative_alt / 10 , Location : : AltFrame : : ABOVE_HOME ) ;
2018-06-03 00:42:25 -03:00
} else {
// absolute altitude
2022-02-25 22:04:46 -04:00
_target_location . set_alt_cm ( packet . alt / 10 , Location : : AltFrame : : ABSOLUTE ) ;
2018-06-03 00:42:25 -03:00
}
2018-01-25 08:35:05 -04:00
_target_velocity_ned . x = packet . vx * 0.01f ; // velocity north
_target_velocity_ned . y = packet . vy * 0.01f ; // velocity east
_target_velocity_ned . z = packet . vz * 0.01f ; // velocity down
2018-12-30 05:45:32 -04:00
// get a local timestamp with correction for transport jitter
_last_location_update_ms = _jitter . correct_offboard_timestamp_msec ( packet . time_boot_ms , AP_HAL : : millis ( ) ) ;
2018-01-25 08:35:05 -04:00
if ( packet . hdg < = 36000 ) { // heading (UINT16_MAX if unknown)
_target_heading = packet . hdg * 0.01f ; // convert centi-degrees to degrees
2018-12-30 05:45:32 -04:00
_last_heading_update_ms = _last_location_update_ms ;
2018-01-25 08:35:05 -04:00
}
// initialise _sysid if zero to sender's id
2018-12-06 20:05:06 -04:00
if ( _sysid = = 0 ) {
_sysid . set ( msg . sysid ) ;
_automatic_sysid = true ;
2018-01-25 08:35:05 -04:00
}
2022-02-25 22:04:46 -04:00
updated = true ;
break ;
}
case MAVLINK_MSG_ID_FOLLOW_TARGET : {
// decode message
mavlink_follow_target_t packet ;
mavlink_msg_follow_target_decode ( & msg , & packet ) ;
// ignore message if lat and lon are (exactly) zero
if ( ( packet . lat = = 0 & & packet . lon = = 0 ) ) {
return ;
}
// require at least position
if ( ( packet . est_capabilities & ( 1 < < 0 ) ) = = 0 ) {
return ;
}
Location new_loc = _target_location ;
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 ) ) {
return ;
}
_target_location = new_loc ;
if ( packet . est_capabilities & ( 1 < < 1 ) ) {
_target_velocity_ned . x = packet . vel [ 0 ] ; // velocity north
_target_velocity_ned . y = packet . vel [ 1 ] ; // velocity east
_target_velocity_ned . z = packet . vel [ 2 ] ; // velocity down
2022-03-03 19:53:38 -04:00
} else {
_target_velocity_ned . zero ( ) ;
2022-02-25 22:04:46 -04:00
}
// get a local timestamp with correction for transport jitter
_last_location_update_ms = _jitter . correct_offboard_timestamp_msec ( packet . timestamp , AP_HAL : : millis ( ) ) ;
if ( packet . est_capabilities & ( 1 < < 3 ) ) {
Quaternion q { packet . attitude_q [ 0 ] , packet . attitude_q [ 1 ] , packet . attitude_q [ 2 ] , packet . attitude_q [ 3 ] } ;
float r , p , y ;
q . to_euler ( r , p , y ) ;
_target_heading = degrees ( y ) ;
_last_heading_update_ms = _last_location_update_ms ;
}
// initialise _sysid if zero to sender's id
if ( _sysid = = 0 ) {
_sysid . set ( msg . sysid ) ;
_automatic_sysid = true ;
}
updated = true ;
break ;
}
}
if ( updated ) {
// get estimated location and velocity
Location loc_estimate { } ;
Vector3f vel_estimate ;
UNUSED_RESULT ( get_target_location_and_velocity ( loc_estimate , vel_estimate ) ) ;
2018-01-25 08:35:05 -04:00
// log lead's estimated vs reported position
2020-04-06 23:54:58 -03:00
// @LoggerMessage: FOLL
// @Description: Follow library diagnostic data
// @Field: TimeUS: Time since system startup
// @Field: Lat: Target latitude
// @Field: Lon: Target longitude
// @Field: Alt: Target absolute altitude
// @Field: VelN: Target earth-frame velocity, North
// @Field: VelE: Target earth-frame velocity, East
// @Field: VelD: Target earth-frame velocity, Down
// @Field: LatE: Vehicle latitude
// @Field: LonE: Vehicle longitude
// @Field: AltE: Vehicle absolute altitude
2021-08-17 06:57:41 -03:00
AP : : logger ( ) . WriteStreaming ( " FOLL " ,
2018-07-14 02:37:24 -03:00
" TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE " , // labels
2018-01-25 08:35:05 -04:00
" sDUmnnnDUm " , // units
" F--B000--B " , // mults
" QLLifffLLi " , // fmt
AP_HAL : : micros64 ( ) ,
_target_location . lat ,
_target_location . lng ,
_target_location . alt ,
( double ) _target_velocity_ned . x ,
( double ) _target_velocity_ned . y ,
( double ) _target_velocity_ned . z ,
loc_estimate . lat ,
loc_estimate . lng ,
loc_estimate . alt
) ;
}
}
// get velocity estimate in m/s in NED frame using dt since last update
bool AP_Follow : : get_velocity_ned ( Vector3f & vel_ned , float dt ) const
{
vel_ned = _target_velocity_ned + ( _target_accel_ned * dt ) ;
return true ;
}
2018-07-14 03:42:04 -03:00
// initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required
2018-01-25 08:35:05 -04:00
void AP_Follow : : init_offsets_if_required ( const Vector3f & dist_vec_ned )
{
2018-07-26 01:07:03 -03:00
// return immediately if offsets have already been set
if ( ! _offset . get ( ) . is_zero ( ) ) {
return ;
2018-01-25 08:35:05 -04:00
}
2019-10-03 22:19:33 -03:00
_offsets_were_zero = true ;
2018-07-20 06:20:35 -03:00
2018-07-26 01:07:03 -03:00
float target_heading_deg ;
2018-02-15 15:16:43 -04:00
if ( ( _offset_type = = AP_FOLLOW_OFFSET_TYPE_RELATIVE ) & & get_target_heading_deg ( target_heading_deg ) ) {
2018-07-26 01:07:03 -03:00
// rotate offsets from north facing to vehicle's perspective
2022-07-20 06:43:20 -03:00
_offset . set ( rotate_vector ( - dist_vec_ned , - target_heading_deg ) ) ;
2019-10-03 22:19:33 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Relative follow offset loaded " ) ;
2018-07-26 01:07:03 -03:00
} else {
// initialise offset in NED frame
2022-07-20 06:43:20 -03:00
_offset . set ( - dist_vec_ned ) ;
2018-07-26 01:07:03 -03:00
// ensure offset_type used matches frame of offsets saved
2022-07-05 00:08:56 -03:00
_offset_type . set ( AP_FOLLOW_OFFSET_TYPE_NED ) ;
2019-10-03 22:19:33 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " N-E-D follow offset loaded " ) ;
2018-07-20 06:20:35 -03:00
}
2018-01-25 08:35:05 -04:00
}
// get offsets in meters in NED frame
bool AP_Follow : : get_offsets_ned ( Vector3f & offset ) const
{
const Vector3f & off = _offset . get ( ) ;
2018-07-14 02:37:24 -03:00
// if offsets are zero or type is NED, simply return offset vector
2018-01-25 08:35:05 -04:00
if ( off . is_zero ( ) | | ( _offset_type = = AP_FOLLOW_OFFSET_TYPE_NED ) ) {
offset = off ;
return true ;
}
2018-07-26 01:07:03 -03:00
// offset type is relative, exit if we cannot get vehicle's heading
float target_heading_deg ;
2018-02-15 15:16:43 -04:00
if ( ! get_target_heading_deg ( target_heading_deg ) ) {
2018-07-26 01:07:03 -03:00
return false ;
2018-01-25 08:35:05 -04:00
}
2018-07-26 01:07:03 -03:00
// rotate offsets from vehicle's perspective to NED
offset = rotate_vector ( off , target_heading_deg ) ;
2018-01-25 08:35:05 -04:00
return true ;
2018-07-26 01:07:03 -03:00
}
// rotate 3D vector clockwise by specified angle (in degrees)
Vector3f AP_Follow : : rotate_vector ( const Vector3f & vec , float angle_deg ) const
{
// rotate roll, pitch input from north facing to vehicle's perspective
const float cos_yaw = cosf ( radians ( angle_deg ) ) ;
const float sin_yaw = sinf ( radians ( angle_deg ) ) ;
return Vector3f ( ( vec . x * cos_yaw ) - ( vec . y * sin_yaw ) , ( vec . y * cos_yaw ) + ( vec . x * sin_yaw ) , vec . z ) ;
}
2018-12-11 05:36:58 -04:00
// set recorded distance and bearing to target to zero
void AP_Follow : : clear_dist_and_bearing_to_target ( )
{
_dist_to_target = 0.0f ;
_bearing_to_target = 0.0f ;
}
2022-01-04 00:52:47 -04:00
// 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 ) ) {
return false ;
}
// apply offsets
loc . offset ( ofs . x , ofs . y ) ;
loc . alt - = ofs . z * 100 ;
return true ;
}
// return true if we have a target
bool AP_Follow : : have_target ( void ) const
{
if ( ! _enabled ) {
return false ;
}
// check for timeout
if ( ( _last_location_update_ms = = 0 ) | | ( AP_HAL : : millis ( ) - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS ) ) {
return false ;
}
return true ;
}
namespace AP {
AP_Follow & follow ( )
{
return * AP_Follow : : get_singleton ( ) ;
}
}
2023-08-11 09:27:17 -03:00
# endif