mirror of https://github.com/ArduPilot/ardupilot
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:
parent
c8216c51b4
commit
e4336551cd
|
@ -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");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
//
|
//
|
||||||
|
@ -124,6 +127,7 @@ private:
|
||||||
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};
|
||||||
|
|
Loading…
Reference in New Issue