Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Follow: add clear_offsets_if_required method 

this restores the offsets to zero if they were initialised from zero when the lead vehicle was first spotted
This commit is contained in:
Randy Mackay 2019-10-04 10:19:33 +09:00
parent c8216c51b4
commit e4336551cd
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Follow/AP_Follow.cpp
View File

@ -134,6 +134,15 @@ AP_Follow::AP_Follow() :
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
// 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) {
_offset = Vector3f();
}
_offsets_were_zero = false;
}
// get target's estimated location // get target's estimated location
bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
{ {
@ -346,16 +355,19 @@ void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned)
if (!_offset.get().is_zero()) { if (!_offset.get().is_zero()) {
return; return;
} }
_offsets_were_zero = true;
float target_heading_deg; float target_heading_deg;
if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) && get_target_heading_deg(target_heading_deg)) { if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) && get_target_heading_deg(target_heading_deg)) {
// rotate offsets from north facing to vehicle's perspective // rotate offsets from north facing to vehicle's perspective
_offset = rotate_vector(-dist_vec_ned, -target_heading_deg); _offset = rotate_vector(-dist_vec_ned, -target_heading_deg);
gcs().send_text(MAV_SEVERITY_INFO, "Relative follow offset loaded");
} else { } else {
// initialise offset in NED frame // initialise offset in NED frame
_offset = -dist_vec_ned; _offset = -dist_vec_ned;
// ensure offset_type used matches frame of offsets saved // ensure offset_type used matches frame of offsets saved
_offset_type = AP_FOLLOW_OFFSET_TYPE_NED; _offset_type = AP_FOLLOW_OFFSET_TYPE_NED;
gcs().send_text(MAV_SEVERITY_INFO, "N-E-D follow offset loaded");
} }
} }

8
libraries/AP_Follow/AP_Follow.h
View File

@ -44,6 +44,9 @@ public:
// set which target to follow // set which target to follow
void set_target_sysid(uint8_t sysid) { _sysid = sysid; } void set_target_sysid(uint8_t sysid) { _sysid = sysid; }
// restore offsets to zero if necessary, should be called when vehicle exits follow mode
void clear_offsets_if_required();
// //
// position tracking related methods // position tracking related methods
// //
@ -122,8 +125,9 @@ private:
uint32_t _last_heading_update_ms; // system time of last heading update uint32_t _last_heading_update_ms; // system time of last heading update
float _target_heading; // heading in degrees float _target_heading; // heading in degrees
bool _automatic_sysid; // did we lock onto a sysid automatically? bool _automatic_sysid; // did we lock onto a sysid automatically?
float _dist_to_target; // latest distance to target in meters (for reporting purposes) 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) 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
// setup jitter correction with max transport lag of 3s // setup jitter correction with max transport lag of 3s
JitterCorrection _jitter{3000}; JitterCorrection _jitter{3000};