diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b1eaaa0fdd020..2938048b1c41b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index f7042b4406cf8..20a1ff8237b8e 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -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; } @@ -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: @@ -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) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index e50f045b67480..dec3e95eef3fa 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -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(); @@ -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; @@ -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) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 8ea52985fd17e..69ace8c3ab0a8 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -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); @@ -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 @@ -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(); @@ -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)) { @@ -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 @@ -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) { @@ -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(); diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index fd9b8fa666be5..698812d3e7152 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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 @@ -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() @@ -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,