From 0a271bdc59ba8c03e1a749dedf69cdd2bba085f5 Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Sat, 30 Jul 2016 14:56:16 -0400 Subject: [PATCH 1/7] Updates book/concept-smartshot.md Auto commit by GitBook Editor --- book/concept-smartshot.md | 84 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 81 insertions(+), 3 deletions(-) diff --git a/book/concept-smartshot.md b/book/concept-smartshot.md index 5d554be..440d49a 100644 --- a/book/concept-smartshot.md +++ b/book/concept-smartshot.md @@ -9,9 +9,87 @@ The most important functions in the `solo.smartshot` are: * *Handling RC input.* Remapping the control sticks to perform alternate functions. For example, this is used in Cable Cam to allow the right stick to control Solo's position along the virtual cable instead of its position relative to earth. * *Buttons.* A and B can be mapped to provide shortcuts like setting fixed waypoints or recording a position. - +Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, will override the Sport Flight Mode. + +Prerequisites + +* Enable Advanced Flight Modes +* Map Sport to Button A or Button B + +To install a custom smart shot requires modification to the following files: + +* modes.py +* shotManager.py +* shots.py + +You also need to install your custom smart shot python script (i.e. autopan.py) to /usr/bin on the Solo. + +**Note**: When updating solo to a newer version, the update process will ignore modified files, so be sure to backup any changed files and restore prior to updating. Otherwise you will have to perform a factory reset. Also, depending on the firmware version installed on your Solo, the code in modes.py, shotManager.py and shots.py may vary from this example. + +Step 1. Modify modes.py + +modes.py is the mapping between Pixhawk Flight Modes and what is displayed on the controller. Change **Sport** to **Autopan**. + +``` +#!/usr/bin/env python + +# mode names - These are what Artoo displays + +MODE_NAMES = { + -1 : "None\0", + 0 : 'Stabilize\0', + 1 : 'Acro\0', + 2 : 'FLY: Manual\0', + 3 : 'Autonomous\0', + 4 : 'Takeoff\0', + 5 : 'FLY\0', + 6 : 'Return Home\0', + 7 : 'Circle\0', + 8 : 'Position\0', + 9 : 'Land\0', + 10 : 'OF_LOITER\0', + 11 : 'Drift\0', + 13 : 'AutoPan\0', + 14 : 'Flip\0', + 15 : 'Auto-tune\0', + 16 : 'Position Hold\0', + 17 : 'Brake\0', + } +``` + +Step 2. Modify shotManager.py + +shotManager.py manages the SmartShots. Add an import for your custom smart shot, add an entry to handle the button press mapped to your custom smart shot, and an entry in the handleButtons function trigger the smart shot. + +Add an import for autopan at the top of shotManager.py. It can be anywhere at the top as long as it is with the other imports. + +``` + from GoProConstants import * + import GoProManager + import infiniCable + import location_helpers + import modes + import orbit + import autopan + import RCRemapper + import selfie + import shotLogger + import shots + from shotManagerConstants import * +``` + +Add an entry in def handleButtons to handle the button press mapped to Autopan. + +``` +def handleButtons(self): + buttonEvent = self.buttonManger.checkButtons() + + if buttonEvent is None: + return + + button, event = buttonEvent. + +``` ```py import solo From 4a61957ecfc61a5d33700892df6bf44614c2310c Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Sat, 30 Jul 2016 15:19:58 -0400 Subject: [PATCH 2/7] Updates book/concept-smartshot.md Auto commit by GitBook Editor --- book/concept-smartshot.md | 76 ++++++++++++++++++++++++--------------- 1 file changed, 48 insertions(+), 28 deletions(-) diff --git a/book/concept-smartshot.md b/book/concept-smartshot.md index 440d49a..36505b6 100644 --- a/book/concept-smartshot.md +++ b/book/concept-smartshot.md @@ -2,12 +2,12 @@ [Smart shots](http://3drobotics.com/smart-shots/) allow you to pre-program complicated flight paths, enabling users to concentrate on when to take the shot rather than how to fly the vehicle. -The Smart Shot framework builds upon DroneKit, and adds support to pause and resume shots (storing the flight state) and re-map controller buttons and sticks during a shot. +The Smart Shot framework builds upon DroneKit, and adds support to pause and resume shots \(storing the flight state\) and re-map controller buttons and sticks during a shot. The most important functions in the `solo.smartshot` are: -* *Handling RC input.* Remapping the control sticks to perform alternate functions. For example, this is used in Cable Cam to allow the right stick to control Solo's position along the virtual cable instead of its position relative to earth. -* *Buttons.* A and B can be mapped to provide shortcuts like setting fixed waypoints or recording a position. +* _Handling RC input._ Remapping the control sticks to perform alternate functions. For example, this is used in Cable Cam to allow the right stick to control Solo's position along the virtual cable instead of its position relative to earth. +* _Buttons._ A and B can be mapped to provide shortcuts like setting fixed waypoints or recording a position. Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, will override the Sport Flight Mode. @@ -22,15 +22,15 @@ To install a custom smart shot requires modification to the following files: * shotManager.py * shots.py -You also need to install your custom smart shot python script (i.e. autopan.py) to /usr/bin on the Solo. +You also need to install your custom smart shot python script \(i.e. autopan.py\) to \/usr\/bin on the Solo. **Note**: When updating solo to a newer version, the update process will ignore modified files, so be sure to backup any changed files and restore prior to updating. Otherwise you will have to perform a factory reset. Also, depending on the firmware version installed on your Solo, the code in modes.py, shotManager.py and shots.py may vary from this example. -Step 1. Modify modes.py +**Step 1. Modify modes.py** modes.py is the mapping between Pixhawk Flight Modes and what is displayed on the controller. Change **Sport** to **Autopan**. -``` +```py #!/usr/bin/env python # mode names - These are what Artoo displays @@ -57,40 +57,59 @@ MODE_NAMES = { } ``` -Step 2. Modify shotManager.py +**Step 2. Modify shotManager.py** -shotManager.py manages the SmartShots. Add an import for your custom smart shot, add an entry to handle the button press mapped to your custom smart shot, and an entry in the handleButtons function trigger the smart shot. +shotManager.py manages the SmartShots. Add an import for your custom smart shot, add an entry to handleButtons function to handle the button press mapped to your custom smart shot, and an entry to the triggerShot function the handleButtons function trigger the smart shot. Add an import for autopan at the top of shotManager.py. It can be anywhere at the top as long as it is with the other imports. -``` - from GoProConstants import * - import GoProManager - import infiniCable - import location_helpers - import modes - import orbit - import autopan - import RCRemapper - import selfie - import shotLogger - import shots - from shotManagerConstants import * +```py +... +import modes +import orbit +import autopan +import RCRemapper +import selfie ``` -Add an entry in def handleButtons to handle the button press mapped to Autopan. +Add an entry to the handleButtons function to handle the button press mapped to Autopan. -``` +```py def handleButtons(self): - buttonEvent = self.buttonManger.checkButtons() - - if buttonEvent is None: - return + ... + if self.currentShot == shots.APP_SHOT_NONE: + if event == btn_msg.Press: + if button == btn_msg.ButtonA or button == btn_msg.ButtonB: + # see what the button is mapped to + (shot, mode) = self.buttonManager.getFreeButtonMapping(button) + + # hack to get pano + if (mode == 13): + logger.log("hack - mode: %s" % modes.MODE_NAMES[mode]) + shot = shots.APP_SHOT_AUTOPAN + + # only allow entry into these shots if the app is attached + allowedShots = [shots.APP_SHOT_ORBIT, shots.APP_SHOT_CABLECAM, shots.APP_SHOT_AUTOPAN] + ... +``` - button, event = buttonEvent. +Add an entry to triggerShot function +```py +def triggerShot(self, shot): + # invalid shot! + if shot not in shots.SHOT_NAMES: + logger.log("[triggerShot]: Error, trying to trigger an invalid shot! %d"%(shot)) + return + ... + elif self.currentShot == shots.APP_SHOT_AUTOPAN: + self.initStreamRates() + self.curController = autopan.AutoPanController(self.vehicle, self) + ... ``` +TBD + ```py import solo from dronekit.lib import VehicleMode, Location @@ -106,3 +125,4 @@ class FlipShot(solo.smartshot): def handle_button(self, button, event): pass ``` + From f958e9399c98a871fb79245200517727766e95c0 Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Sat, 30 Jul 2016 15:35:17 -0400 Subject: [PATCH 3/7] Updates book/concept-smartshot.md Auto commit by GitBook Editor --- book/concept-smartshot.md | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/book/concept-smartshot.md b/book/concept-smartshot.md index 36505b6..3c1688d 100644 --- a/book/concept-smartshot.md +++ b/book/concept-smartshot.md @@ -9,9 +9,9 @@ The most important functions in the `solo.smartshot` are: * _Handling RC input._ Remapping the control sticks to perform alternate functions. For example, this is used in Cable Cam to allow the right stick to control Solo's position along the virtual cable instead of its position relative to earth. * _Buttons._ A and B can be mapped to provide shortcuts like setting fixed waypoints or recording a position. -Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, will override the Sport Flight Mode. +Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, will automatically yaw the Solo in 45 degreee increments and take a picture at each heading. It will override the Sport Flight Mode. -Prerequisites +\*\*Prerequisites:\*\* * Enable Advanced Flight Modes * Map Sport to Button A or Button B @@ -57,7 +57,27 @@ MODE_NAMES = { } ``` -**Step 2. Modify shotManager.py** +**Step 2. Modify shots.py** + +shots.py contains the definition of shots. Add and Autopan constant and add the constant to the SHOT\_NAMES array. + +``` +APP_SHOT_RECORD = 4 +APP_SHOT_FOLLOW = 5 +APP_SHOT_AUTOPAN = 6 + +# NULL terminated for sending to Artoo +SHOT_NAMES = { + APP_SHOT_NONE : "FLY\0", + APP_SHOT_SELFIE : "Selfie\0", + APP_SHOT_ORBIT : "Orbit\0", + APP_SHOT_CABLECAM : "Cable Cam\0", + APP_SHOT_FOLLOW : "Follow\0", + APP_SHOT_PANO : "Pano\0" +} +``` + +**Step 3. Modify shotManager.py** shotManager.py manages the SmartShots. Add an import for your custom smart shot, add an entry to handleButtons function to handle the button press mapped to your custom smart shot, and an entry to the triggerShot function the handleButtons function trigger the smart shot. @@ -70,6 +90,7 @@ import orbit import autopan import RCRemapper import selfie +... ``` Add an entry to the handleButtons function to handle the button press mapped to Autopan. @@ -83,7 +104,7 @@ def handleButtons(self): # see what the button is mapped to (shot, mode) = self.buttonManager.getFreeButtonMapping(button) - # hack to get pano + # hack to get autopan if (mode == 13): logger.log("hack - mode: %s" % modes.MODE_NAMES[mode]) shot = shots.APP_SHOT_AUTOPAN @@ -108,7 +129,9 @@ def triggerShot(self, shot): ... ``` -TBD +Step 4. Add your custom smart shot dronekit-python script. + +This example SmartShot, AutoPan will yaw the Solo and take a picture at each heading. ```py import solo From 29c21c855f8e4ca76d407282c692f571843e8da3 Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Sun, 31 Jul 2016 15:36:54 -0400 Subject: [PATCH 4/7] Updates book/concept-smartshot.md Auto commit by GitBook Editor --- book/concept-smartshot.md | 316 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 303 insertions(+), 13 deletions(-) diff --git a/book/concept-smartshot.md b/book/concept-smartshot.md index 3c1688d..2de672b 100644 --- a/book/concept-smartshot.md +++ b/book/concept-smartshot.md @@ -11,7 +11,7 @@ The most important functions in the `solo.smartshot` are: Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, will automatically yaw the Solo in 45 degreee increments and take a picture at each heading. It will override the Sport Flight Mode. -\*\*Prerequisites:\*\* +**Prerequisites:** * Enable Advanced Flight Modes * Map Sport to Button A or Button B @@ -73,7 +73,7 @@ SHOT_NAMES = { APP_SHOT_ORBIT : "Orbit\0", APP_SHOT_CABLECAM : "Cable Cam\0", APP_SHOT_FOLLOW : "Follow\0", - APP_SHOT_PANO : "Pano\0" + APP_SHOT_AUTOPAN : "AutoPan\0" } ``` @@ -81,7 +81,7 @@ SHOT_NAMES = { shotManager.py manages the SmartShots. Add an import for your custom smart shot, add an entry to handleButtons function to handle the button press mapped to your custom smart shot, and an entry to the triggerShot function the handleButtons function trigger the smart shot. -Add an import for autopan at the top of shotManager.py. It can be anywhere at the top as long as it is with the other imports. +Add an import for autopan at the top of shotManager.py. It can be anywhere at the top as long as it is with the other import statements. ```py ... @@ -93,7 +93,7 @@ import selfie ... ``` -Add an entry to the handleButtons function to handle the button press mapped to Autopan. +Add an entry to the handleButtons function to handle the controller button mapped to Autopan. ```py def handleButtons(self): @@ -114,7 +114,7 @@ def handleButtons(self): ... ``` -Add an entry to triggerShot function +Add an entry to triggerShot function to trigger the Autopan Smart Shot. ```py def triggerShot(self, shot): @@ -125,27 +125,317 @@ def triggerShot(self, shot): ... elif self.currentShot == shots.APP_SHOT_AUTOPAN: self.initStreamRates() - self.curController = autopan.AutoPanController(self.vehicle, self) + self.curController = autopan.AutoPanShot(self.vehicle, self) ... ``` -Step 4. Add your custom smart shot dronekit-python script. +**Step 4. Add your custom smart shot dronekit-python script.** This example SmartShot, AutoPan will yaw the Solo and take a picture at each heading. ```py -import solo -from dronekit.lib import VehicleMode, Location - -class FlipShot(solo.smartshot): +test +## autopan.py +# Implement autopan using DroneKit +# This is a shot where the copter takes a picture while automatically +# yawing. +# Created by Ahmed Agbabiaka# Created: 2015/11/15 +# Last Modified: 2016/01/01 +# Copyright (c) 2016 Viresity, Inc. All Rights Reserved. + +from dronekit import Vehicle, LocationGlobalRelative, VehicleMode + +from pymavlink import mavutil +import os +from os import sys, path +import math +import struct +import time + +sys.path.append(os.path.realpath('')) +import app_packet +import camera +import location_helpers +import shotLogger +import shots +from shotManagerConstants import * +import GoProManager +from GoProConstants import * +from sololink import btn_msg + +YAW_SPEED = 10.0 #deg/s + +# modes +AUTOPAN_PHOTO = 0 +AUTOPAN_VIDEO = 1 + +AUTOPAN_SETUP = 0 +AUTOPAN_RUN = 1 +AUTOPAN_EXIT = 2 + +logger = shotLogger.logger + +MAVLINK_GIMBAL_SYSTEM_ID = 1 +MAVLINK_GIMBAL_COMPONENT_ID = 154 +VIDEO_MODE = 0 +PHOTO_MODE = 1 + +class AutoPanShot(): def __init__(self, vehicle, shotmgr): + #vehicle object self.vehicle = vehicle + + #shotmanager object self.shotmgr = shotmgr + # state machine + self.state = AUTOPAN_SETUP + + # default mode + self.autoPanType = AUTOPAN_PHOTO + + # default camPitch + self.camPitch = camera.getPitch(self.vehicle) + + # default camYaw to current heading + self.camYaw = camera.getYaw(self.vehicle) + + # set button mappings on Artoo + self.setButtonMappings() + + # switch vehicle into GUIDED mode + self.vehicle.mode = VehicleMode("GUIDED") + + # switch gimbal into MAVLINK TARGETING mode + self.setupTargeting() + + # take over RC + self.shotmgr.rcMgr.enableRemapping( True ) + + # channels are expected to be floating point values in the (-1.0, 1.0) range def handle_rcs(self, channels): + if self.autoPanType == AUTOPAN_VIDEO: + SwitchGoProMode(VIDEO_MODE) + self.takeVideo(channels) + self.manualPitch(channels) + self.handlePitchYaw() + else: + if self.state == AUTOPAN_SETUP: + self.manualPitch(channels) + self.manualYaw(channels[YAW]) + self.handlePitchYaw() + else: + if self.autoPanType == AUTOPAN_PHOTO: + self.TakePicture() + self.handlePitchYaw() + + SwitchGoProMode(PHOTO_MODE) + + self.Pan( 90, -1, True) + self.TakePicture() + time.sleep(2) + + self.Pan( 45, 1, True) + self.TakePicture() + time.sleep(2) + + self.Pan( 0, 1, True) + self.TakePicture() + time.sleep(2) + + self.Pan( 45, 1, True) + self.TakePicture() + time.sleep(2) + + self.Pan( 90, 1, True) + self.TakePicture() + time.sleep(2) + self.handlePitchYaw() + + # Always call flush to guarantee that previous writes to the vehicle + # have taken place + self.vehicle.flush() + + def initAutoPan(self): + # We reset to the beginning for all states + if self.autoPanType == AUTOPAN_PHOTO: + logger.log("[AUTOPAN]: Init Photo Pan") + self.enterPhotoMode() + self.initPhoto() + else: + logger.log("[PANO]: Init Video Pan") + + def takePhoto(self): + self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) + + def takeVideo(self, channels): + tbd + + def Pan(heading, direction, relative): + if relative: + is_relative=1 #yaw relative to direction of travel + else: + is_relative=0 #yaw is an absolute angle (0-360; 0 is north) + + # 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 + direction, # 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) + + # Always call flush to guarantee that previous writes to the vehicle have taken place + self.vehicle.flush() + + def SwitchGoProMode(mode): + msg = self.vehicle.message_factory.gopro_set_request_encode( + MAVLINK_GIMBAL_SYSTEM_ID, MAVLINK_GIMBAL_COMPONENT_ID, # target system, target component + mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, # command + (mode, 0, 0, 0)) # params 1-4 + + # send command to vehicle + self.vehicle.send_mavlink(msg) + + # Always call flush to guarantee that previous writes to the vehicle have taken place + self.vehicle.flush() + + def TakePicture(): + msg = self.vehicle.message_factory.gopro_set_request_encode( + MAVLINK_GIMBAL_SYSTEM_ID, MAVLINK_GIMBAL_COMPONENT_ID, # target system, target component + mavutil.mavlink.GOPRO_COMMAND_CAPTURE_SHUTTER, # command + (1, 0, 0, 0)) # params 1-4 + + # send command to vehicle + self.vehicle.send_mavlink(msg) + + # Always call flush to guarantee that previous writes to the vehicle have taken place + self.vehicle.flush() + + def setButtonMappings(self): + buttonMgr = self.shotmgr.buttonManager + + if self.autoPanType == AUTOPAN_VIDEO: + buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_AUTOPAN, 0, "\0") + else: + if self.state == AUTOPAN_RUN: + buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Cancel\0") + else: + buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0") + + if self.state == AUTOPAN_SETUP: + if self.panoType == AUTOPAN_VIDEO: + buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Video\0") + elif self.panoType == AUTOPAN_PHOTO: + buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Photo\0") + else: + buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_AUTOPAN, 0, "\0") + + def handleButton(self, button, event): + if button == btn_msg.ButtonA and event == btn_msg.Press: + if self.autoPanType != AUTOPAN_VIDEO: + if self.state == AUTOPAN_SETUP: + self.state = AUTOPAN_RUN + else: + # enter standby + self.resetPano() + self.state = AUTOPAN_SETUP + logger.log("[AUTOPAN]: Cancel AutoPan") + # if we are in video mode, stop Yawing + if self.autoPanType == AUTOPAN_VIDEO: + self.degSecondYaw = 0 + self.setButtonMappings() + + if button == btn_msg.ButtonLoiter and event == btn_msg.Press: + if self.autoPanType == AUTOPAN_VIDEO: + self.degSecondYaw = 0 + + # cycle through options + if self.state == AUTOPAN_SETUP and button == btn_msg.ButtonB and event == btn_msg.Press: + #clear Pano Video yaw speed + self.degSecondYaw = 0 + if self.autoPanType == AUTOPAN_VIDEO: + self.autoPanType = AUTOPAN_PHOTO + elif self.autoPanType == AUTOPAN_PHOTO: + self.autoPanType = AUTOPAN_VIDEO + self.setButtonMappings() + + def setupTargeting(self): + # set gimbal targeting mode + msg = self.vehicle.message_factory.mount_configure_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode + 1, # stabilize roll + 1, # stabilize pitch + 1, # stabilize yaw + ) + self.vehicle.send_mavlink(msg) + + def manualPitch(self, channels): + if abs(channels[THROTTLE]) > abs(channels[RAW_PADDLE]): + value = channels[THROTTLE] + else: + value = channels[RAW_PADDLE] + + self.camPitch += value * PANO_PITCH_SPEED * UPDATE_TIME + + if self.camPitch > 0.0: + self.camPitch = 0.0 + elif self.camPitch < -90: + self.camPitch = -90 + + def manualYaw(self, stick): + if stick == 0: + return + self.camYaw += stick * PANO_YAW_SPEED * UPDATE_TIME + if stick > 0: + self.camDir = 1 + else: + self.camDir = -1 + + self.camYaw = location_helpers.wrapTo360(self.camYaw) + + def handlePitchYaw(self): + '''Send pitch and yaw commands to gimbal or fixed mount''' + # 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 + self.camYaw * 100, + 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 + 0.0, # relative offset + 0, 0, 0 # params 5-7 (unused) + ) + self.vehicle.send_mavlink(msg) + + def enterPhotoMode(self): + # switch into photo mode if we aren't already in it + self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0)) + + def resume_from_brake(self): pass - def handle_button(self, button, event): - pass ``` +TBD + From 66545c11971361e152cee8949cf6c06fe1179be3 Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Mon, 1 Aug 2016 22:42:59 -0400 Subject: [PATCH 5/7] Updates book/hardware-accessorybay.md Auto commit by GitBook Editor --- book/hardware-accessorybay.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/book/hardware-accessorybay.md b/book/hardware-accessorybay.md index f1731bf..cb95dc5 100644 --- a/book/hardware-accessorybay.md +++ b/book/hardware-accessorybay.md @@ -100,7 +100,7 @@ The accessory port breaks out a subset of the available peripherals of the Pixha | HAL | System | ArduCopter Parameter | Pixhawk2 | Solo Peripheral | |-------------|--------------|-----------------------|----------|-----------------| | px4io/sbus | /dev/ttyS0 | | | | -| uartA | /dev/ttyACM0 | | USB | | +| uartA | /dev/ttyACM0 | | USB | Acc. Port | | uartB | /dev/ttyS3 | SERIAL3 | GPS | Internal GPS | | uartC | /dev/ttyS1 | SERIAL1 | Telem1 | | | uartD | /dev/ttyS2 | SERIAL2 | Telem2 | Acc. Port | From e0939a825463570ad0c800b8c6c654cd08d94f05 Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Tue, 2 Aug 2016 22:30:16 -0400 Subject: [PATCH 6/7] Updates book/concept-smartshot.md Auto commit by GitBook Editor --- book/concept-smartshot.md | 387 ++++++++++++++++++++------------------ 1 file changed, 209 insertions(+), 178 deletions(-) diff --git a/book/concept-smartshot.md b/book/concept-smartshot.md index 2de672b..f633e98 100644 --- a/book/concept-smartshot.md +++ b/book/concept-smartshot.md @@ -9,7 +9,7 @@ The most important functions in the `solo.smartshot` are: * _Handling RC input._ Remapping the control sticks to perform alternate functions. For example, this is used in Cable Cam to allow the right stick to control Solo's position along the virtual cable instead of its position relative to earth. * _Buttons._ A and B can be mapped to provide shortcuts like setting fixed waypoints or recording a position. -Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, will automatically yaw the Solo in 45 degreee increments and take a picture at each heading. It will override the Sport Flight Mode. +Since there isn't a framework to implement custom Smart Shots, to intall a custom Smart Shot, you must override an existing flight mode. In this example, our custom Smart Shot called AutoPan, which will automatically yaw the Solo in increments and take a picture at each heading. It will override the Sport Flight Mode. **Prerequisites:** @@ -19,7 +19,7 @@ Since there isn't a framework to implement custom Smart Shots, to intall a custo To install a custom smart shot requires modification to the following files: * modes.py -* shotManager.py +* shotFactory.py * shots.py You also need to install your custom smart shot python script \(i.e. autopan.py\) to \/usr\/bin on the Solo. @@ -59,7 +59,7 @@ MODE_NAMES = { **Step 2. Modify shots.py** -shots.py contains the definition of shots. Add and Autopan constant and add the constant to the SHOT\_NAMES array. +shots.py contains the definition of shots. Add an Autopan constant and add the constant to the SHOT\_NAMES array. ``` APP_SHOT_RECORD = 4 @@ -77,71 +77,52 @@ SHOT_NAMES = { } ``` -**Step 3. Modify shotManager.py** +**Step 3. Modify shotFactory.py** -shotManager.py manages the SmartShots. Add an import for your custom smart shot, add an entry to handleButtons function to handle the button press mapped to your custom smart shot, and an entry to the triggerShot function the handleButtons function trigger the smart shot. +shotFactory.py defines the list of Smart Shot classes. Add AutoPan to imports and APP\_SHOT\_AUTOPAN to ShotFactory object. -Add an import for autopan at the top of shotManager.py. It can be anywhere at the top as long as it is with the other import statements. +Add import autopan to imports section. -```py +``` ... -import modes -import orbit +import pano import autopan -import RCRemapper -import selfie ... ``` -Add an entry to the handleButtons function to handle the controller button mapped to Autopan. +Add APP\_SHOT\_AUTOPAN to ShotFactory object. -```py -def handleButtons(self): - ... - if self.currentShot == shots.APP_SHOT_NONE: - if event == btn_msg.Press: - if button == btn_msg.ButtonA or button == btn_msg.ButtonB: - # see what the button is mapped to - (shot, mode) = self.buttonManager.getFreeButtonMapping(button) - - # hack to get autopan - if (mode == 13): - logger.log("hack - mode: %s" % modes.MODE_NAMES[mode]) - shot = shots.APP_SHOT_AUTOPAN - - # only allow entry into these shots if the app is attached - allowedShots = [shots.APP_SHOT_ORBIT, shots.APP_SHOT_CABLECAM, shots.APP_SHOT_AUTOPAN] - ... ``` - -Add an entry to triggerShot function to trigger the Autopan Smart Shot. - -```py -def triggerShot(self, shot): - # invalid shot! - if shot not in shots.SHOT_NAMES: - logger.log("[triggerShot]: Error, trying to trigger an invalid shot! %d"%(shot)) - return - ... - elif self.currentShot == shots.APP_SHOT_AUTOPAN: - self.initStreamRates() - self.curController = autopan.AutoPanShot(self.vehicle, self) - ... +class ShotFactory(object): + __shot_classes = { + shots.APP_SHOT_SELFIE : selfie.SelfieShot, + shots.APP_SHOT_ORBIT: orbit.OrbitShot, + shots.APP_SHOT_CABLECAM : cable_cam.CableCamShot, + shots.APP_SHOT_ZIPLINE : zipline.ZiplineShot, + shots.APP_SHOT_FOLLOW : follow.FollowShot, + shots.APP_SHOT_MULTIPOINT : multipoint.MultipointShot, + shots.APP_SHOT_PANO : pano.PanoShot, + shots.APP_SHOT_AUTOPAN : autopan.AutoPanShot, + shots.APP_SHOT_REWIND : rewind.RewindShot, + shots.APP_SHOT_TRANSECT : transect.TransectShot, + shots.APP_SHOT_RTL : returnHome.returnHomeShot, + } ``` **Step 4. Add your custom smart shot dronekit-python script.** -This example SmartShot, AutoPan will yaw the Solo and take a picture at each heading. - ```py -test -## autopan.py +# +# autopan.py # Implement autopan using DroneKit # This is a shot where the copter takes a picture while automatically # yawing. -# Created by Ahmed Agbabiaka# Created: 2015/11/15 +# Created by Ahmed Agbabiaka +# Sport Mode override, Controller Button integration and +# pan angle math by Jason Short and Will Silva +# Created: 2015/11/15 # Last Modified: 2016/01/01 -# Copyright (c) 2016 Viresity, Inc. All Rights Reserved. +# Copyright (c) 2015 Viresity, Inc. All Rights Reserved. from dronekit import Vehicle, LocationGlobalRelative, VehicleMode @@ -165,6 +146,19 @@ from sololink import btn_msg YAW_SPEED = 10.0 #deg/s +CYLINDER_LENS_ANGLE = 160.0 +AUTOPAN_CAPTURE_DELAY = int(UPDATE_RATE * 2.5) +AUTOPAN_MOVE_DELAY = int(UPDATE_RATE * 3.5) + +AUTOPAN_YAW_SPEED = 60.0 # deg/s +AUTOPAN_PITCH_SPEED = 60.0 # deg/s + +AUTOPAN_DEFAULT_FOV = 220 +AUTOPAN_DEFAULT_VIDEO_YAW_RATE = 2.0 + +MAX_YAW_RATE = 60 # deg/s +MAX_YAW_ACCEL_PER_TICK = (MAX_YAW_RATE) / (4 * UPDATE_RATE) # deg/s^2/tick + # modes AUTOPAN_PHOTO = 0 AUTOPAN_VIDEO = 1 @@ -173,12 +167,11 @@ AUTOPAN_SETUP = 0 AUTOPAN_RUN = 1 AUTOPAN_EXIT = 2 -logger = shotLogger.logger +VIDEO_COUNTER = 15 + +TICKS_TO_BEGIN = -25 -MAVLINK_GIMBAL_SYSTEM_ID = 1 -MAVLINK_GIMBAL_COMPONENT_ID = 154 -VIDEO_MODE = 0 -PHOTO_MODE = 1 +logger = shotLogger.logger class AutoPanShot(): def __init__(self, vehicle, shotmgr): @@ -188,16 +181,33 @@ class AutoPanShot(): #shotmanager object self.shotmgr = shotmgr + # ticks to track timing in shot + self.ticks = 0 + # state machine self.state = AUTOPAN_SETUP # default mode self.autoPanType = AUTOPAN_PHOTO + # steps to track incrementing in shot + self.step = 0 + self.stepsTotal = 0 + + # Yaw rate for Video AutoPan shot + self.degSecondYaw = AUTOPAN_DEFAULT_VIDEO_YAW_RATE + + # default FOV for Photo AutoPan shot + self.cylinder_fov = AUTOPAN_DEFAULT_FOV + self.lensFOV = CYLINDER_LENS_ANGLE + + # list of angles in photo AutoPan shot + self.cylinderAngles = None + # default camPitch self.camPitch = camera.getPitch(self.vehicle) - # default camYaw to current heading + # default camYaw to current pointing self.camYaw = camera.getYaw(self.vehicle) # set button mappings on Artoo @@ -214,111 +224,126 @@ class AutoPanShot(): # channels are expected to be floating point values in the (-1.0, 1.0) range def handle_rcs(self, channels): - if self.autoPanType == AUTOPAN_VIDEO: - SwitchGoProMode(VIDEO_MODE) - self.takeVideo(channels) - self.manualPitch(channels) - self.handlePitchYaw() - else: - if self.state == AUTOPAN_SETUP: - self.manualPitch(channels) - self.manualYaw(channels[YAW]) - self.handlePitchYaw() - else: - if self.autoPanType == AUTOPAN_PHOTO: - self.TakePicture() - self.handlePitchYaw() - - SwitchGoProMode(PHOTO_MODE) - - self.Pan( 90, -1, True) - self.TakePicture() - time.sleep(2) - - self.Pan( 45, 1, True) - self.TakePicture() - time.sleep(2) - - self.Pan( 0, 1, True) - self.TakePicture() - time.sleep(2) - - self.Pan( 45, 1, True) - self.TakePicture() - time.sleep(2) - - self.Pan( 90, 1, True) - self.TakePicture() - time.sleep(2) - self.handlePitchYaw() - - # Always call flush to guarantee that previous writes to the vehicle - # have taken place - self.vehicle.flush() + if self.autoPanType == AUTOPAN_VIDEO: + self.takeVideo(channels) + self.manualPitch(channels) + self.handlePitchYaw() + else: + if self.state == AUTOPAN_SETUP: + self.manualPitch(channels) + self.manualYaw(channels[YAW]) + self.handlePitchYaw() + else: + if self.autoPanType == AUTOPAN_PHOTO: + self.takePicture() + self.handlePitchYaw() def initAutoPan(self): - # We reset to the beginning for all states + # switch gopro mode and initialize AutoPan mode if self.autoPanType == AUTOPAN_PHOTO: - logger.log("[AUTOPAN]: Init Photo Pan") - self.enterPhotoMode() + logger.log("[AUTOPAN]: Init Photo AutoPan") + self.switchGoProMode(CAPTURE_MODE_PHOTO) self.initPhoto() else: - logger.log("[PANO]: Init Video Pan") - + logger.log("[AUTOPAN]: Init Video AutoPan") + # GoProManager will automatically switch to video on RECORD_COMMAND_TOGGLE + self.initVideo() + + def resetAutoPan(self): + self.cylinderAngles = None + + def initPhoto(self): + # Initialize the photo AutoPan shot + + self.cylinder_fov = max(self.cylinder_fov, 91.0) + self.cylinder_fov = min(self.cylinder_fov, 360.0) + + self.yaw_total = self.cylinder_fov - (self.lensFOV / 4) + steps = math.ceil(self.cylinder_fov / (self.lensFOV / 4)) + num_photos = math.ceil(self.cylinder_fov / (self.lensFOV / 4)) + yawDelta = self.yaw_total / (num_photos - 1) + + self.origYaw = camera.getYaw(self.vehicle) + yawStart = self.origYaw - (self.yaw_total / 2.0) + + self.cylinderAngles = [] + + for x in range(0, int(steps)): + tmp = yawStart + (x * yawDelta) + if tmp < 0: + tmp += 360 + elif tmp > 360: + tmp -= 360 + self.cylinderAngles.append(int(tmp)) + + self.stepsTotal = len(self.cylinderAngles) + + # go to first angle + self.camYaw = self.cylinderAngles.pop() + + # give first move an extra second to get there + self.ticks = TICKS_TO_BEGIN + self.updateAutoPanStatus(0, self.stepsTotal) + def takePhoto(self): - self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) - + self.ticks += 1 + + if self.cylinderAngles is None: + self.initAutoPan() + + if self.state == AUTOPAN_RUN: + # time delay between camera moves + if self.ticks == AUTOPAN_CAPTURE_DELAY: + self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) + + if self.ticks > AUTOPAN_MOVE_DELAY: + self.ticks = 0 + + if len(self.cylinderAngles) == 0: + self.state = AUTOPAN_EXIT + else: + # select new angle + self.camYaw = self.cylinderAngles.pop() + + elif self.state == AUTOPAN_EXIT: + self.camYaw = self.origYaw + # let user take a new Pano + self.state = AUTOPAN_SETUP + self.resetAutoPan() + self.setButtonMappings() + def takeVideo(self, channels): - tbd - - def Pan(heading, direction, relative): - if relative: - is_relative=1 #yaw relative to direction of travel - else: - is_relative=0 #yaw is an absolute angle (0-360; 0 is north) - - # 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 - direction, # 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) - - # Always call flush to guarantee that previous writes to the vehicle have taken place - self.vehicle.flush() - - def SwitchGoProMode(mode): - msg = self.vehicle.message_factory.gopro_set_request_encode( - MAVLINK_GIMBAL_SYSTEM_ID, MAVLINK_GIMBAL_COMPONENT_ID, # target system, target component - mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, # command - (mode, 0, 0, 0)) # params 1-4 - - # send command to vehicle - self.vehicle.send_mavlink(msg) - - # Always call flush to guarantee that previous writes to the vehicle have taken place - self.vehicle.flush() - - def TakePicture(): - msg = self.vehicle.message_factory.gopro_set_request_encode( - MAVLINK_GIMBAL_SYSTEM_ID, MAVLINK_GIMBAL_COMPONENT_ID, # target system, target component - mavutil.mavlink.GOPRO_COMMAND_CAPTURE_SHUTTER, # command - (1, 0, 0, 0)) # params 1-4 - - # send command to vehicle - self.vehicle.send_mavlink(msg) - - # Always call flush to guarantee that previous writes to the vehicle have taken place - self.vehicle.flush() - - def setButtonMappings(self): + if self.state == AUTOPAN_RUN: + # start recording + self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) + + # modulate yaw rate based on yaw stick input + if channels[YAW] != 0: + self.degSecondYaw = self.degSecondYaw + (channels[YAW] * MAX_YAW_ACCEL_PER_TICK) + + # limit yaw rate + self.degSecondYaw = min(self.degSecondYaw, MAX_YAW_RATE) + self.degSecondYaw = max(self.degSecondYaw, -MAX_YAW_RATE) + + # increment desired yaw angle + self.camYaw += (self.degSecondYaw * UPDATE_TIME) + self.camYaw %= 360.0 + + self.counter += 1 + + if self.counter > VIDEO_COUNTER: + self.counter = 0 + self.state = AUTOPAN_EXIT + # stop recording + self.shotmgr.goproManager.handleRecordCommand(self.shotmgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) + + elif self.state == AUTOPAN_EXIT: + self.camYaw = self.origYaw + self.state = AUTOPAN_SETUP + self.restAutoPan() + self.setButtonMappings() + + def setButtonMappings(self): buttonMgr = self.shotmgr.buttonManager if self.autoPanType == AUTOPAN_VIDEO: @@ -330,13 +355,13 @@ class AutoPanShot(): buttonMgr.setArtooButton(btn_msg.ButtonA, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Begin\0") if self.state == AUTOPAN_SETUP: - if self.panoType == AUTOPAN_VIDEO: + if self.autoPanType == AUTOPAN_VIDEO: buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Video\0") - elif self.panoType == AUTOPAN_PHOTO: + elif self.autoPanType == AUTOPAN_PHOTO: buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_AUTOPAN, btn_msg.ARTOO_BITMASK_ENABLED, "Photo\0") else: buttonMgr.setArtooButton(btn_msg.ButtonB, shots.APP_SHOT_AUTOPAN, 0, "\0") - + def handleButton(self, button, event): if button == btn_msg.ButtonA and event == btn_msg.Press: if self.autoPanType != AUTOPAN_VIDEO: @@ -344,12 +369,13 @@ class AutoPanShot(): self.state = AUTOPAN_RUN else: # enter standby - self.resetPano() + self.resetAutoPan() self.state = AUTOPAN_SETUP logger.log("[AUTOPAN]: Cancel AutoPan") # if we are in video mode, stop Yawing if self.autoPanType == AUTOPAN_VIDEO: self.degSecondYaw = 0 + self.setButtonMappings() if button == btn_msg.ButtonLoiter and event == btn_msg.Press: @@ -358,51 +384,50 @@ class AutoPanShot(): # cycle through options if self.state == AUTOPAN_SETUP and button == btn_msg.ButtonB and event == btn_msg.Press: - #clear Pano Video yaw speed + #clear AutoPan Video yaw speed self.degSecondYaw = 0 if self.autoPanType == AUTOPAN_VIDEO: self.autoPanType = AUTOPAN_PHOTO elif self.autoPanType == AUTOPAN_PHOTO: self.autoPanType = AUTOPAN_VIDEO self.setButtonMappings() - - def setupTargeting(self): - # set gimbal targeting mode - msg = self.vehicle.message_factory.mount_configure_encode( + + def setupTargeting(self): + # set gimbal targeting mode + msg = self.vehicle.message_factory.mount_configure_encode( 0, 1, # target system, target component mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, #mount_mode 1, # stabilize roll 1, # stabilize pitch 1, # stabilize yaw ) - self.vehicle.send_mavlink(msg) - + self.vehicle.send_mavlink(msg) + def manualPitch(self, channels): if abs(channels[THROTTLE]) > abs(channels[RAW_PADDLE]): value = channels[THROTTLE] else: value = channels[RAW_PADDLE] - self.camPitch += value * PANO_PITCH_SPEED * UPDATE_TIME - + self.camPitch += value * AUTOPAN_PITCH_SPEED * UPDATE_TIME + if self.camPitch > 0.0: self.camPitch = 0.0 elif self.camPitch < -90: self.camPitch = -90 - + def manualYaw(self, stick): if stick == 0: return - self.camYaw += stick * PANO_YAW_SPEED * UPDATE_TIME + self.camYaw += stick * AUTOPAN_YAW_SPEED * UPDATE_TIME if stick > 0: self.camDir = 1 else: self.camDir = -1 - + self.camYaw = location_helpers.wrapTo360(self.camYaw) - + def handlePitchYaw(self): - '''Send pitch and yaw commands to gimbal or fixed mount''' # 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( @@ -426,16 +451,22 @@ class AutoPanShot(): 0.0, # relative offset 0, 0, 0 # params 5-7 (unused) ) + self.vehicle.send_mavlink(msg) - - def enterPhotoMode(self): - # switch into photo mode if we aren't already in it - self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0)) - def resume_from_brake(self): - pass + def switchGoProMode(mode): + # switch gopro mode CAPTURE_MODE_PHOTO, CAPTURE_MODE_VIDEO + self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (mode, 0 ,0 , 0)) ``` -TBD +Put autopan.py on the solo in \/usr\/bin + +**Step 5. Reboot Solo and the Controller** + +Reboot Solo and the controller. When the controller is rebooted, AutoPan should be displayed on the controller as Mapped to Button A or Button B. + +**Step 6. Start Custom Smart Shot** + +Press Fly to take off and press the button mapped to AutoPan to start the AutoPan. Choose Photo or Video to choose a Photo or Video pan. From 7893a1a3fd8973eebf49aade34570fa7ca44c8c0 Mon Sep 17 00:00:00 2001 From: franknitty69 Date: Tue, 2 Aug 2016 22:31:06 -0400 Subject: [PATCH 7/7] Updates book/concept-smartshot.md Auto commit by GitBook Editor