diff --git a/shotmanager/follow.py b/shotmanager/follow.py index 5aba962..a3b0103 100644 --- a/shotmanager/follow.py +++ b/shotmanager/follow.py @@ -625,44 +625,29 @@ class FollowShot(): # 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, # roll - # yaw is in centidegrees - 0, #self.camYaw * 100, (Disabled by Matt due to ArduCopter master mount_control problem) - 0 # save position - ) - self.vehicle.send_mavlink(msg) - - # Use MAV_CMD_CONDITION_YAW since the yaw in MSG_MOUNT_CONTROL is not working in AC35 - 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 - 0.0, # relative offset - 0, 0, 0 # params 5-7 (unused) - ) - self.vehicle.send_mavlink(msg) - - 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 - 0.0, # relative offset - 0, 0, 0 # params 5-7 (unused) + 0, 1, # Target system, target component + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position ) self.vehicle.send_mavlink(msg) - + else: + # if we don't have a gimbal, just set CONDITION_YAW + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # 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 + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(msg) + + def handleLookAtPointing(self, tempROI): # set ROI msg = self.vehicle.message_factory.command_int_encode( diff --git a/shotmanager/multipoint.py b/shotmanager/multipoint.py index 8003347..938d544 100644 --- a/shotmanager/multipoint.py +++ b/shotmanager/multipoint.py @@ -290,49 +290,31 @@ class MultipointShot(): newYaw += self.yawPitchOffsetter.yawOffset # Formulate mavlink message for mount controller. Use mount_control if using a gimbal. Use just condition_yaw if no gimbal. - # Modified by Matt 2017-05-18 for ArducCopter master compatibility - - # mav_cmd_do_mount_control should handle gimbal pitch and copter yaw, but yaw is not working in master. - # So this mount_control command is only going to do the gimbal pitch for now. if self.vehicle.mount_status[0] is not None: pointingMsg = self.vehicle.message_factory.mount_control_encode( - 0, 1, # target system, target component - newPitch * 100, # pitch (centidegrees) - 0.0, # roll (centidegrees) - newYaw * 100, # yaw (centidegrees) - 0 # save position - ) - self.vehicle.send_mavlink(pointingMsg) - - # Temporary fix while the yaw control is not working in the above mount_control command. - pointingMsg = self.vehicle.message_factory.command_long_encode( - 0, 0, # target system, target component - mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command - 0, # confirmation - newYaw, # param 1 - target angle (degrees) - 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) + 0, 1, # target system, target component + newPitch * 100, # pitch in centidegrees + 0.0, # roll not used + newYaw * 100, # yaw in centidegrees + 0 # save position ) self.vehicle.send_mavlink(pointingMsg) - # if no gimbal, assume fixed mount and use condition_yaw only else: # if we don't have a gimbal, just set CONDITION_YAW pointingMsg = self.vehicle.message_factory.command_long_encode( - 0, 0, # target system, target component + 0, 1, # target system, target component mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command - 0, # confirmation - newYaw, # param 1 - target angle (degrees) - 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) + 0, # confirmation + newYaw, # param 1 - target angle (degrees) + 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 interpolateCamera(self): '''Interpolate (linear) pitch and yaw between cable control points''' @@ -847,45 +829,26 @@ class MultipointShot(): if self.vehicle.mount_status[0] is not None: pointingMsg = self.vehicle.message_factory.mount_control_encode( - 0, 1, # target system, target component - newPitch * 100, # pitch (centidegrees) - 0.0, # roll (centidegrees) - newYaw * 100, # yaw (centidegrees) - 0 # save position + 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) - - pointingMsg = self.vehicle.message_factory.command_long_encode( - 0, 0, # 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) - - # pointingMsg = self.vehicle.message_factory.mount_control_encode( - # 0, 1, # target system, target component - # startPitch * 100, # pitch (centidegrees) - # 0.0, # roll (centidegrees) - # startYaw * 100, # yaw (centidegrees) - # 0 # save position - # ) + # 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, 0, # target system, target component + 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) + 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) diff --git a/shotmanager/pano.py b/shotmanager/pano.py index 6c12c6c..8973c9e 100644 --- a/shotmanager/pano.py +++ b/shotmanager/pano.py @@ -469,39 +469,25 @@ class PanoShot(): # 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, # roll - # yaw is in centidegrees - 0, # self.camYaw * 100, (Disabled by Matt for now due to ArduCopter master mount_control bug. Using condition_yaw instead) - 0 # save position + 0, 1, # Target system, target component + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position ) self.vehicle.send_mavlink(msg) - - msg = self.vehicle.message_factory.command_long_encode( # Using condition_yaw temporarily until mount_control yaw issue is fixed - 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 - 0.0, # relative offset - 0, 0, 0 # params 5-7 (unused) - ) - self.vehicle.send_mavlink(msg) - + 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 - 0.0, # relative offset - 0, 0, 0 # params 5-7 (unused) + 0, 1, # 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 + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) ) self.vehicle.send_mavlink(msg) diff --git a/shotmanager/selfie.py b/shotmanager/selfie.py index 179a97b..c42f7e3 100644 --- a/shotmanager/selfie.py +++ b/shotmanager/selfie.py @@ -268,24 +268,24 @@ class SelfieShot(): # 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 + 0, 1, # Target system, target component + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position ) + self.vehicle.send_mavlink(msg) + 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) + 0, 1, # 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 + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) ) - self.vehicle.send_mavlink(msg) + self.vehicle.send_mavlink(msg) diff --git a/shotmanager/zipline.py b/shotmanager/zipline.py index 1dc5b91..4099fa0 100644 --- a/shotmanager/zipline.py +++ b/shotmanager/zipline.py @@ -317,39 +317,25 @@ class ZiplineShot(): # 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 - 0, # self.camYaw * 100.0, # yaw is in centidegrees (Disabled by Matt for now due to ArduCopter master mount_control bug. Using condition_yaw instead) - 0 # save position + 0, 1, # Target system, target component + self.camPitch * 100, # Pitch in centidegrees + 0.0, # Roll not used + self.camYaw * 100, # Yaw in centidegrees + 0 # save position ) self.vehicle.send_mavlink(msg) - - msg = self.vehicle.message_factory.command_long_encode( # Using condition_yaw temporarily until mount_control yaw issue is fixed - 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) - - + 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) + 0, 1, # 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 + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) ) self.vehicle.send_mavlink(msg)