mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
AP_Follow: NFC small renames and comment improvements
This commit is contained in:
parent
f1e43979ab
commit
8f5a22685a
@ -219,7 +219,7 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w
|
|||||||
}
|
}
|
||||||
|
|
||||||
// get target's heading in degrees (0 = north, 90 = east)
|
// get target's heading in degrees (0 = north, 90 = east)
|
||||||
bool AP_Follow::get_target_heading(float &heading) const
|
bool AP_Follow::get_target_heading_deg(float &heading) const
|
||||||
{
|
{
|
||||||
// exit immediately if not enabled
|
// exit immediately if not enabled
|
||||||
if (!_enabled) {
|
if (!_enabled) {
|
||||||
@ -343,7 +343,7 @@ void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned)
|
|||||||
}
|
}
|
||||||
|
|
||||||
float target_heading_deg;
|
float target_heading_deg;
|
||||||
if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) && get_target_heading(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);
|
||||||
} else {
|
} else {
|
||||||
@ -367,7 +367,7 @@ bool AP_Follow::get_offsets_ned(Vector3f &offset) const
|
|||||||
|
|
||||||
// offset type is relative, exit if we cannot get vehicle's heading
|
// offset type is relative, exit if we cannot get vehicle's heading
|
||||||
float target_heading_deg;
|
float target_heading_deg;
|
||||||
if (!get_target_heading(target_heading_deg)) {
|
if (!get_target_heading_deg(target_heading_deg)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -68,7 +68,7 @@ public:
|
|||||||
YawBehave get_yaw_behave() const { return (YawBehave)_yaw_behave.get(); }
|
YawBehave get_yaw_behave() const { return (YawBehave)_yaw_behave.get(); }
|
||||||
|
|
||||||
// get target's heading in degrees (0 = north, 90 = east)
|
// get target's heading in degrees (0 = north, 90 = east)
|
||||||
bool get_target_heading(float &heading) const;
|
bool get_target_heading_deg(float &heading) const;
|
||||||
|
|
||||||
// parse mavlink messages which may hold target's position, velocity and attitude
|
// parse mavlink messages which may hold target's position, velocity and attitude
|
||||||
void handle_msg(const mavlink_message_t &msg);
|
void handle_msg(const mavlink_message_t &msg);
|
||||||
@ -109,7 +109,7 @@ private:
|
|||||||
AP_Float _dist_max; // maximum distance to target. targets further than this will be ignored
|
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_Int8 _offset_type; // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
|
||||||
AP_Vector3f _offset; // offset from lead vehicle in meters
|
AP_Vector3f _offset; // offset from lead vehicle in meters
|
||||||
AP_Int8 _yaw_behave; // following vehicle's yaw/heading behaviour
|
AP_Int8 _yaw_behave; // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
|
||||||
AP_Int8 _alt_type; // altitude source for follow mode
|
AP_Int8 _alt_type; // altitude source for follow mode
|
||||||
AC_P _p_pos; // position error P controller
|
AC_P _p_pos; // position error P controller
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user