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

AP_Follow: Initialize offsets based on offset type

This commit is contained in:
hoangthien94 2018-07-20 17:20:35 +08:00 committed by Randy Mackay
parent d507805891
commit f4255712c1
1 changed files with 30 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Follow/AP_Follow.cpp
View File

@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal;
#define AP_GCS_INTERVAL_MS 1000 // interval between updating GCS on position of vehicle
#define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame
#define AP_FOLLOW_OFFSET_TYPE_RELATIVE 0 // offsets are relative to lead vehicle's heading
#define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading
#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE 1 // relative altitude is used by default
@ -335,6 +335,21 @@ void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned)
if (_offset.get().is_zero()) {
_offset = -dist_vec_ned;
}
// if offset type is relative then the offsets should be rotated appropriately based on the lead vehicle's heading.
if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
const Vector3f &off = _offset.get();
Vector3f off_rotated;
// rotate roll, pitch input from north facing to vehicle's perspective
const float veh_cos_yaw = cosf(radians(_target_heading));
const float veh_sin_yaw = sinf(radians(_target_heading));
off_rotated.x = (off.x * veh_cos_yaw) - (off.y * veh_sin_yaw);
off_rotated.y = (off.y * veh_cos_yaw) + (off.x * veh_sin_yaw);
off_rotated.z = off.z;
_offset.set(off_rotated);
}
}
// get offsets in meters in NED frame
@ -348,17 +363,20 @@ bool AP_Follow::get_offsets_ned(Vector3f &offset) const
return true;
}
// offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE
// check if we have a valid heading for target vehicle
if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
return false;
// if offset type is relative, rotate offsets from NED to vehicle's heading
if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
// check if we have a valid heading for target vehicle
if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
return false;
}
// rotate roll, pitch input from north facing to vehicle's perspective
const float veh_cos_yaw = cosf(radians(_target_heading));
const float veh_sin_yaw = sinf(radians(_target_heading));
offset.x = (off.x * veh_cos_yaw) - (off.y * veh_sin_yaw);
offset.y = (off.y * veh_cos_yaw) + (off.x * veh_sin_yaw);
offset.z = off.z;
}
// rotate roll, pitch input from north facing to vehicle's perspective
const float veh_cos_yaw = cosf(radians(_target_heading));
const float veh_sin_yaw = sinf(radians(_target_heading));
offset.x = (off.x * veh_cos_yaw) - (off.y * veh_sin_yaw);
offset.y = (off.y * veh_cos_yaw) + (off.x * veh_sin_yaw);
offset.z = off.z;
return true;
}