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/>.
|
|
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <AP_Common/AP_Common.h>
|
2022-02-25 01:06:28 -04:00
|
|
|
#include <AP_Common/Location.h>
|
2018-01-25 08:35:05 -04:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include <AP_Math/AP_Math.h>
|
2022-02-25 01:06:28 -04:00
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
2018-01-25 08:35:05 -04:00
|
|
|
#include <AC_PID/AC_P.h>
|
2018-12-30 05:45:32 -04:00
|
|
|
#include <AP_RTC/JitterCorrection.h>
|
2018-01-25 08:35:05 -04:00
|
|
|
|
|
|
|
class AP_Follow
|
|
|
|
{
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
2023-05-22 01:02:38 -03:00
|
|
|
// enum for FOLLOW_OPTIONS parameter
|
|
|
|
enum class Option {
|
|
|
|
MOUNT_FOLLOW_ON_ENTER = 1
|
|
|
|
};
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
// enum for YAW_BEHAVE parameter
|
|
|
|
enum YawBehave {
|
|
|
|
YAW_BEHAVE_NONE = 0,
|
|
|
|
YAW_BEHAVE_FACE_LEAD_VEHICLE = 1,
|
|
|
|
YAW_BEHAVE_SAME_AS_LEAD_VEHICLE = 2,
|
|
|
|
YAW_BEHAVE_DIR_OF_FLIGHT = 3
|
|
|
|
};
|
|
|
|
|
|
|
|
// constructor
|
2018-03-31 09:27:58 -03:00
|
|
|
AP_Follow();
|
2018-01-25 08:35:05 -04:00
|
|
|
|
2022-01-04 00:52:47 -04:00
|
|
|
// enable as singleton
|
|
|
|
static AP_Follow *get_singleton(void) {
|
|
|
|
return _singleton;
|
|
|
|
}
|
|
|
|
|
2018-12-11 05:36:24 -04:00
|
|
|
// returns true if library is enabled
|
|
|
|
bool enabled() const { return _enabled; }
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
// set which target to follow
|
2022-07-05 00:08:56 -03:00
|
|
|
void set_target_sysid(uint8_t sysid) { _sysid.set(sysid); }
|
2018-01-25 08:35:05 -04:00
|
|
|
|
2019-10-03 22:19:33 -03:00
|
|
|
// restore offsets to zero if necessary, should be called when vehicle exits follow mode
|
|
|
|
void clear_offsets_if_required();
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
//
|
|
|
|
// position tracking related methods
|
|
|
|
//
|
|
|
|
|
|
|
|
// true if we have a valid target location estimate
|
|
|
|
bool have_target() const;
|
|
|
|
|
|
|
|
// get target's estimated location and velocity (in NED)
|
|
|
|
bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const;
|
|
|
|
|
2022-01-04 00:52:47 -04:00
|
|
|
// get target's estimated location and velocity (in NED), with offsets added
|
|
|
|
bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const;
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
// get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
|
|
|
|
bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned);
|
|
|
|
|
2023-05-22 01:02:38 -03:00
|
|
|
// get target sysid
|
|
|
|
uint8_t get_target_sysid() const { return _sysid.get(); }
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
// get position controller. this controller is not used within this library but it is convenient to hold it here
|
|
|
|
const AC_P& get_pos_p() const { return _p_pos; }
|
|
|
|
|
|
|
|
//
|
|
|
|
// yaw/heading related methods
|
|
|
|
//
|
|
|
|
|
|
|
|
// get user defined yaw behaviour
|
|
|
|
YawBehave get_yaw_behave() const { return (YawBehave)_yaw_behave.get(); }
|
|
|
|
|
|
|
|
// get target's heading in degrees (0 = north, 90 = east)
|
2018-02-15 15:16:43 -04:00
|
|
|
bool get_target_heading_deg(float &heading) const;
|
2018-01-25 08:35:05 -04:00
|
|
|
|
|
|
|
// parse mavlink messages which may hold target's position, velocity and attitude
|
|
|
|
void handle_msg(const mavlink_message_t &msg);
|
|
|
|
|
2018-12-11 05:36:58 -04:00
|
|
|
//
|
|
|
|
// GCS reporting functions
|
|
|
|
//
|
|
|
|
|
|
|
|
// get horizontal distance to target (including offset) in meters (for reporting purposes)
|
|
|
|
float get_distance_to_target() const { return _dist_to_target; }
|
|
|
|
|
|
|
|
// get bearing to target (including offset) in degrees (for reporting purposes)
|
|
|
|
float get_bearing_to_target() const { return _bearing_to_target; }
|
|
|
|
|
2022-01-04 00:52:47 -04:00
|
|
|
// get system time of last position update
|
|
|
|
uint32_t get_last_update_ms() const { return _last_location_update_ms; }
|
|
|
|
|
2023-05-22 01:02:38 -03:00
|
|
|
// returns true if a follow option enabled
|
|
|
|
bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; }
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
// parameter list
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
|
|
|
private:
|
2022-01-04 00:52:47 -04:00
|
|
|
static AP_Follow *_singleton;
|
2018-01-25 08:35:05 -04:00
|
|
|
|
|
|
|
// get velocity estimate in m/s in NED frame using dt since last update
|
|
|
|
bool get_velocity_ned(Vector3f &vel_ned, float dt) const;
|
|
|
|
|
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 init_offsets_if_required(const Vector3f &dist_vec_ned);
|
|
|
|
|
|
|
|
// get offsets in meters in NED frame
|
|
|
|
bool get_offsets_ned(Vector3f &offsets) const;
|
|
|
|
|
2018-07-26 01:07:03 -03:00
|
|
|
// rotate 3D vector clockwise by specified angle (in degrees)
|
|
|
|
Vector3f rotate_vector(const Vector3f &vec, float angle_deg) const;
|
|
|
|
|
2018-12-11 05:36:58 -04:00
|
|
|
// set recorded distance and bearing to target to zero
|
|
|
|
void clear_dist_and_bearing_to_target();
|
|
|
|
|
2018-01-25 08:35:05 -04:00
|
|
|
// parameters
|
|
|
|
AP_Int8 _enabled; // 1 if this subsystem is enabled
|
|
|
|
AP_Int16 _sysid; // target's mavlink system id (0 to use first sysid seen)
|
|
|
|
AP_Float _dist_max; // maximum distance to target. targets further than this will be ignored
|
|
|
|
AP_Int8 _offset_type; // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
|
|
|
|
AP_Vector3f _offset; // offset from lead vehicle in meters
|
2018-02-15 15:16:43 -04:00
|
|
|
AP_Int8 _yaw_behave; // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
|
2018-06-03 00:42:25 -03:00
|
|
|
AP_Int8 _alt_type; // altitude source for follow mode
|
2018-01-25 08:35:05 -04:00
|
|
|
AC_P _p_pos; // position error P controller
|
2023-05-22 01:02:38 -03:00
|
|
|
AP_Int16 _options; // options for mount behaviour follow mode
|
2018-01-25 08:35:05 -04:00
|
|
|
|
|
|
|
// local variables
|
|
|
|
uint32_t _last_location_update_ms; // system time of last position update
|
|
|
|
Location _target_location; // last known location 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
|
|
|
|
float _target_heading; // heading in degrees
|
2018-12-06 20:05:06 -04:00
|
|
|
bool _automatic_sysid; // did we lock onto a sysid automatically?
|
2019-10-03 22:19:33 -03:00
|
|
|
float _dist_to_target; // latest distance to target in meters (for reporting purposes)
|
|
|
|
float _bearing_to_target; // latest bearing to target in degrees (for reporting purposes)
|
|
|
|
bool _offsets_were_zero; // true if offsets were originally zero and then initialised to the offset from lead vehicle
|
2018-12-30 05:45:32 -04:00
|
|
|
|
|
|
|
// setup jitter correction with max transport lag of 3s
|
|
|
|
JitterCorrection _jitter{3000};
|
2018-01-25 08:35:05 -04:00
|
|
|
};
|
2022-01-04 00:52:47 -04:00
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
AP_Follow &follow();
|
|
|
|
};
|