Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
By default the yaw of the vehicle will follow the direction of travel. After setting
the yaw using this function there is no way to return to the default yaw "follow direction
of travel" behaviour (https://github.com/diydrones/ardupilot/issues/2427)
For more information see:
http://copter.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_condition_yaw
"""
if relative:
is_relative = 1 #yaw relative to direction of travel
else:
is_relative = 0 #yaw is an absolute angle
# create the CONDITION_YAW command using command_long_encode()
msg = vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
is_relative, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
def cmd_condition_yaw(self, args):
'''yaw angle angular_speed angle_mode'''
if ( len(args) != 3):
print("Usage: yaw ANGLE ANGULAR_SPEED MODE:[0 absolute / 1 relative]")
return
if (len(args) == 3):
angle = float(args[0])
angular_speed = float(args[1])
angle_mode = float(args[2])
print("ANGLE %s" % (str(angle)))
self.master.mav.command_long_send(
self.settings.target_system, # target_system
mavutil.mavlink.MAV_COMP_ID_SYSTEM_CONTROL, # target_component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
angle, # param1 (angle value)
angular_speed, # param2 (angular speed value)
0, # param3
angle_mode, # param4 (mode: 0->absolute / 1->relative)
0, # param5
0, # param6
0) # param7
def condition_yaw(heading):
msg = vehicle.message_factory.mission_item_encode(0, 0, # target system, target component
0, # sequence
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
2, # current - set to 2 to make it a guided command
0, # auto continue
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
0, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()
By default the yaw of the vehicle will follow the direction of travel. After setting
the yaw using this function there is no way to return to the default yaw "follow direction
of travel" behaviour (https://github.com/diydrones/ardupilot/issues/2427)
For more information see:
http://copter.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_condition_yaw
"""
if relative:
is_relative = 1 #yaw relative to direction of travel
else:
is_relative = 0 #yaw is an absolute angle
# create the CONDITION_YAW command using command_long_encode()
msg = vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
0, #confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
is_relative, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
if self.vehicle.mount_status[0] is not None:
pointingMsg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
startPitch * 100, # pitch in centidegrees
0.0, # roll not used
startYaw * 100, # yaw in centidegrees
0 # save position
)
self.vehicle.send_mavlink(pointingMsg)
# if not, assume fixed mount
else:
# if we don't have a gimbal, just set CONDITION_YAW
pointingMsg = self.vehicle.message_factory.command_long_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
startYaw, # param 1 - target angle (degrees)
ATTACH_YAW_SPEED, # param 2 - yaw speed (deg/s)
0, # param 3 - direction, always shortest route for now...
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(pointingMsg)
def condition_yaw(heading):
msg = vehicle.message_factory.mission_item_encode(0, 0, # target system, target component
0, # sequence
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
2, # current - set to 2 to make it a guided command
0, # auto continue
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
0, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()
def condition_yaw(self, heading):
# create the CONDITION_YAW command
msg = self.vehicle.message_factory.mission_item_encode(0, 0, # target system, target component
0, # sequence
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
2, # current - set to 2 to make it a guided command
0, # auto continue
heading, 0, 0, 0, 0, 0, 0) # param 1 ~ 7
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
isRelative = 1
else:
isRelative = 0
# The vehicle will rotate in cw or ccw by some degrees
if clock_wise:
direction = 1 # Degree described by 'heading' will be added to current degree
else:
direction = -1 # Degree described by 'heading' will be subscribed from current degree
if not relative:
direction = 0
# create the CONDITION_YAW command
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed (not used)
direction, # param 3, direction
isRelative, # param 4, relative or absolute degrees
0, 0, 0) # param 5-7, not used
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
return True
def vehicle_condition_yaw(self, heading, relative=False):
if relative:
is_relative = 1 # yaw relative to direction of travel
else:
is_relative = 0 # yaw is an absolute angle
# create the CONDITION_YAW command using command_long_encode()
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
is_relative, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
# if we do have a gimbal, use mount_control to set pitch and yaw
if self.vehicle.mount_status[0] is not None:
msg = self.vehicle.message_factory.mount_control_encode(
0, 1, # target system, target component
# pitch is in centidegrees
self.camPitch * 100.0,
0.0, # roll
# yaw is in centidegrees
self.camYaw * 100.0,
0 # save position
)
else:
# if we don't have a gimbal, just set CONDITION_YAW
msg = self.vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
0, # confirmation
self.camYaw, # param 1 - target angle
YAW_SPEED, # param 2 - yaw speed
self.camDir, # param 3 - direction XXX
0.0, # relative offset
0, 0, 0 # params 5-7 (unused)
)
self.vehicle.send_mavlink(msg)