Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add yaw-relative-to-heading function to Copter Guided mode #27144

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+162 −22
Open

Add yaw-relative-to-heading function to Copter Guided mode #27144

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7663721
Copter: tidy handling of MAV_CMD_CONDITION_YAW
peterbarker May 23, 2024
a2e139b
Copter: add support for CONDITION_YAW-relative-to-vehicle-heading
peterbarker May 23, 2024
16b3e1c
autotest: add test for heading-relative yaw
peterbarker May 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 32 additions & 10 deletions ArduCopter/GCS_Mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -911,17 +911,39 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_comman
// param2 : speed during change [deg per second]
// param3 : direction (-1:ccw, +1:cw)
// param4 : relative offset (1) or absolute angle (0)
if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) &&
(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
copter.flightmode->auto_yaw.set_fixed_yaw(
packet.param1,
packet.param2,
(int8_t)packet.param3,
is_positive(packet.param4));
return MAV_RESULT_ACCEPTED;
}
const float target_angle = packet.param1;
if (target_angle < 0.0 || target_angle > 360.0) {
return MAV_RESULT_FAILED;
}

const float rate_degs = packet.param2;
const int8_t direction = packet.param3;

switch ((uint8_t)packet.param4) {
case 0:
copter.flightmode->auto_yaw.set_fixed_yaw(
target_angle,
rate_degs,
direction,
false);
return MAV_RESULT_ACCEPTED;
case 1:
copter.flightmode->auto_yaw.set_fixed_yaw(
target_angle,
rate_degs,
direction,
true);
return MAV_RESULT_ACCEPTED;
case 2: {
// relative-to-vehicle-heading
// start by turning angle and direction into a relative yaw:
const float relative_angle = wrap_180(direction * target_angle);
copter.flightmode->auto_yaw.set_vehicle_heading_yaw(relative_angle, rate_degs);
return MAV_RESULT_ACCEPTED;
}
default:
return MAV_RESULT_FAILED;
}
}

MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet)
Expand Down
10 changes: 10 additions & 0 deletions ArduCopter/autoyaw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ float Mode::AutoYaw::look_ahead_yaw()
// Commanded Yaw to automatically look ahead.
if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
_look_ahead_yaw = degrees(atan2f(vel.y,vel.x));
_look_ahead_yaw += _look_ahead_yaw_offset;
}
return _look_ahead_yaw;
}
Expand Down Expand Up @@ -82,6 +83,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
_look_ahead_yaw = copter.ahrs.yaw_sensor * 0.01; // cdeg -> deg
_look_ahead_yaw_offset = 0.0;
break;

case Mode::RESETTOARMEDYAW:
Expand Down Expand Up @@ -134,6 +136,14 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
set_mode(Mode::FIXED);
}

// set_vehicle_heading_yaw - sets the yaw look at heading to be relative to vehicle travel
void Mode::AutoYaw::set_vehicle_heading_yaw(float relative_angle, float turn_rate_ds)
{
// currently ignores turn rate
_look_ahead_yaw_offset = relative_angle;
set_mode(Mode::LOOK_AHEAD);
}

// set_fixed_yaw - sets the yaw look at heading for auto mode
void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
{
Expand Down
5 changes: 4 additions & 1 deletion ArduCopter/mode.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -325,6 +325,7 @@ class Mode {
bool relative_angle);

void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
void set_vehicle_heading_yaw(float relative_angle, float turn_rate_ds);

bool reached_fixed_yaw_target();

Expand Down Expand Up @@ -365,6 +366,7 @@ class Mode {

// heading when in yaw_look_ahead_yaw
float _look_ahead_yaw;
float _look_ahead_yaw_offset; // added to vehicle CoG to produce _look_ahead_yaw

// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float _yaw_angle_cd;
Expand Down Expand Up @@ -1135,7 +1137,8 @@ class ModeGuided : public Mode {
DoNotStabilizePositionXY = (1U << 4),
DoNotStabilizeVelocityXY = (1U << 5),
WPNavUsedForPosControl = (1U << 6),
AllowWeatherVaning = (1U << 7)
AllowWeatherVaning = (1U << 7),
PreserveYaw = (1U << 8),
};

// returns true if the Guided-mode-option is set (see GUID_OPTIONS)
Expand Down
41 changes: 32 additions & 9 deletions ArduCopter/mode_guided.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -344,12 +344,16 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
// if configured to use wpnav for position control
if (use_wpnav_for_position_control()) {
// ensure we are in position control mode
bool init_yaw = !option_is_enabled(Option::PreserveYaw);
if (guided_mode != SubMode::WP) {
wp_control_start();
init_yaw = true;
}

// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
if (init_yaw) {
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
}

// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(destination, terrain_alt);
Expand All @@ -362,10 +366,13 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
return true;
}

bool init_yaw = !option_is_enabled(Option::PreserveYaw);

// if configured to use position controller for position control
// ensure we are in position control mode
if (guided_mode != SubMode::Pos) {
pos_control_start();
init_yaw = true;
}

// initialise terrain following if needed
Expand All @@ -386,8 +393,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
pos_control->set_pos_offset_z_cm(0.0);
}

// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
if (init_yaw) {
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
}

// set position target and zero velocity and acceleration
guided_pos_target_cm = destination.topostype();
Expand Down Expand Up @@ -439,8 +448,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y

// if using wpnav for position control
if (use_wpnav_for_position_control()) {
bool init_yaw = !option_is_enabled(Option::PreserveYaw);
if (guided_mode != SubMode::WP) {
wp_control_start();
init_yaw = true;
}

if (!wp_nav->set_wp_destination_loc(dest_loc)) {
Expand All @@ -450,8 +461,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
return false;
}

// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
if (init_yaw) {
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
}

#if HAL_LOGGING_ENABLED
// log target
Expand All @@ -469,14 +482,19 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
return false;
}

bool init_yaw = !option_is_enabled(Option::PreserveYaw);

// if configured to use position controller for position control
// ensure we are in position control mode
if (guided_mode != SubMode::Pos) {
pos_control_start();
init_yaw = true;
}

// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
if (init_yaw) {
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
}

// initialise terrain following if needed
if (terrain_alt) {
Expand Down Expand Up @@ -589,13 +607,18 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const
}
#endif

bool init_yaw = !option_is_enabled(Option::PreserveYaw);

// check we are in velocity control mode
if (guided_mode != SubMode::PosVelAccel) {
posvelaccel_control_start();
init_yaw = true;
}

// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
if (init_yaw) {
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
}

update_time_ms = millis();
guided_pos_target_cm = destination.topostype();
Expand Down
86 changes: 84 additions & 2 deletions Tools/autotest/arducopter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4599,12 +4599,25 @@ def fly_guided_stop(self,
m.climb < climb_tolerance):
break

def send_set_position_target_global_int(self, lat, lon, alt):
def send_set_position_target_global_int_loc(self, loc):
self.send_set_position_target_global_int(
int(loc.lat*1e7),
int(loc.lng*1e7),
loc.alt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
)

def send_set_position_target_global_int(
self,
lat,
lon,
alt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT):
self.mav.mav.set_position_target_global_int_send(
0, # timestamp
1, # target system_id
1, # target component id
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
frame,
MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # mask specifying use-only-lat-lon-alt
lat, # lat
lon, # lon
Expand Down Expand Up @@ -9576,6 +9589,74 @@ def MAV_CMD_CONDITION_YAW(self):
self._MAV_CMD_CONDITION_YAW(self.run_cmd)
self.context_pop()

def MAV_CMD_CONDITION_YAW_heading_relative(self):
'''check relative-to-CoG yaw'''
self.takeoff(10, mode='GUIDED')
self.progress("First make sure it cancelsan absolute yaw")
for heading in 30, 60:
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
p1=heading, # target angle
p2=360, # degrees/second
p3=1, # -1 is counter-clockwise, 1 clockwise
p4=0, # 1 for relative, 0 for absolute
)
self.wait_heading(heading, minimum_duration=5)

here = self.mav.location()
t = self.offset_location_ne(here, 2000, 0)

self.do_reposition(t, frame=mavutil.mavlink.MAV_FRAME_GLOBAL)

# reposition cancels the condition-yaw:
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
p1=heading, # target angle
p2=360, # degrees/second
p3=1, # -1 is counter-clockwise, 1 clockwise
p4=0, # 1 for relative, 0 for absolute
)
self.wait_heading(60, minimum_duration=10)

self.progress("switch to yaw-looks-ahead")
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
p1=0, # target angle
p2=360, # degrees/second
p3=1, # -1 is counter-clockwise, 1 clockwise
p4=2, # 1 for relative, 0 for absolute, 2 for heading-relative
)
self.wait_heading(0, minimum_duration=10)

# turn on preserve-yaw
self.set_parameter("GUID_OPTIONS", 256)

self.progress("Now point 45-degrees to right of vehicle heading")
self.run_cmd_int(
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
p1=45, # target angle
p2=360, # degrees/second
p3=1, # -1 is counter-clockwise, 1 clockwise
p4=2, # 1 for relative, 0 for absolute, 2 for heading-relative
)
self.wait_heading(45, minimum_duration=10)

self.progress("Heading east, checking yaw remains 45-degrees-to-heading")
here = self.mav.location()
t = self.offset_location_ne(here, 0, 2000)

self.send_set_position_target_global_int_loc(t)
self.wait_heading(135, minimum_duration=10)

self.progress("Heading east, checking yaw remains 45-degrees-to-heading")
here = self.mav.location()
t = self.offset_location_ne(here, 0, -2000)

self.send_set_position_target_global_int_loc(t)
self.wait_heading(315, minimum_duration=10)

self.do_RTL()

def GroundEffectCompensation_touchDownExpected(self):
'''Test EKF's handling of touchdown-expected'''
self.zero_throttle()
Expand Down Expand Up @@ -10415,6 +10496,7 @@ def tests1a(self):
self.NavDelay,
self.GuidedSubModeChange,
self.MAV_CMD_CONDITION_YAW,
self.MAV_CMD_CONDITION_YAW_heading_relative,
self.LoiterToAlt,
self.PayloadPlaceMission,
self.PrecisionLoiterCompanion,
Expand Down
Loading