def enterFailsafe(self): ''' called when we loose RC link or have Batt FS event ''' # dont enter failsafe on the ground if not self.vehicle.armed or self.vehicle.system_status != 'ACTIVE': return # dont enter failsafe if we are already rewinding home if self.currentShot == shots.APP_SHOT_REWIND: self.curController.exitToRTL = True return if self.currentShot == shots.APP_SHOT_RTL: return if self.last_ekf_ok is False: # no GPS - force an emergency land self.vehicle.mode = VehicleMode("LAND") return # ignore FS while in Auto mode if self.vehicle.mode.name == 'AUTO' and self.rewindManager.fs_thr == 2: return if self.rewindManager.enabled: #self.enterShot(shots.APP_SHOT_REWIND) #Disabled by Matt #self.curController.exitToRTL = True self.vehicle.mode = VehicleMode("RTL") else: self.vehicle.mode = VehicleMode("RTL")
def armed_callback(self, vehicle, name, armed): try: if armed != self.last_armed: self.last_armed = armed logger.log("[callback]: armed status changed to %d" % (armed)) self.buttonManager.setButtonMappings() # clear Rewind manager cache self.rewindManager.resetSpline() if not armed and self.currentShot not in shots.CAN_START_BEFORE_ARMING: self.enterShot(shots.APP_SHOT_NONE) self.vehicle.mode = VehicleMode("LOITER") # stop recording upon disarm (landing, hopefully) if not armed: self.goproManager.handleRecordCommand( self.goproManager.captureMode, RECORD_COMMAND_STOP) # read home loc from vehicle if armed: self.rewindManager.loadHomeLocation() except Exception as e: logger.log('[callback]: armed callback error, %s' % e)
def Start(self, vehicle): logger.log("+-+-+-+-+-+-+ Starting up %s +-+-+-+-+-+-+" % VERSION) ### initialize dronekit vehicle ### self.vehicle = vehicle ### switch vehicle to loiter mode ### self.vehicle.mode = VehicleMode("LOITER") ### initialize rc manager ### self.rcMgr = rcManager.rcManager(self) ### initialize app manager ### self.appMgr = appManager.appManager(self) ### initialize button manager ### self.buttonManager = buttonManager.buttonManager(self) ### initialize extended functions ### self.extFunctions = extFunctions.extFunctions(self.vehicle, self) ### initialize gopro manager ### self.goproManager = GoProManager.GoProManager(self) ### Initialize GeoFence manager ### self.geoFenceManager = GeoFenceManager.GeoFenceManager(self) # instantiate rewindManager self.rewindManager = rewindManager.RewindManager(self.vehicle, self) ### init APM stream rates ### self.initStreamRates() ### register callbacks ### self.registerCallbacks() # Try to maintain a constant tick rate self.timeOfLastTick = monotonic.monotonic() # how many ticks have we performed since an RC update? # register all connections (gopro manager communicates via appMgr's socket) self.inputs = [self.rcMgr.server, self.appMgr.server] self.outputs = [] #check if gimbal is present if self.vehicle.gimbal.yaw is not None: logger.log("[shot]: Gimbal detected.") # Initialize gimbal to RC targeting self.vehicle.gimbal.release() else: logger.log("[shot]: No gimbal detected.") # mark first tick time self.timeOfLastTick = monotonic.monotonic() # check for In-Air start from Shotmanager crash if self.vehicle.system_status == 'ACTIVE': logger.log("[shot]: Restart in air.") # load vehicle home self.rewindManager.loadHomeLocation()
def arm_and_takeoff(aTargetAltitude): """ Arms vehicle and fly to aTargetAltitude. """ print "Basic pre-arm checks" # Don't let the user try to fly autopilot is booting if vehicle.mode.name == "INITIALISING": print "Waiting for vehicle to initialise" time.sleep(1) while vehicle.gps_0.fix_type < 2: print "Waiting for GPS...:", vehicle.gps_0.fix_type time.sleep(1) print "Arming motors" # Copter should arm in GUIDED mode vehicle.mode = VehicleMode("GUIDED") vehicle.armed = True while not vehicle.armed: print " Waiting for arming..." time.sleep(1) print "Taking off!" vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude # Wait until the vehicle reaches a safe height before processing the goto (otherwise the command # after Vehicle.commands.takeoff will execute immediately). while True: print " Altitude: ", vehicle.location.global_frame.alt if vehicle.location.global_frame.alt >= aTargetAltitude * 0.95: #Trigger just below target alt. print "Reached target altitude" break time.sleep(1)
def test_110(connpath): v = connect(connpath, await_params=True) # NOTE these are *very inappropriate settings* # to make on a real vehicle. They are leveraged # exclusively for simulation. Take heed!!! v.parameters['ARMING_CHECK'] = 0 v.parameters['FS_THR_ENABLE'] = 0 v.parameters['FS_GCS_ENABLE'] = 0 v.parameters['EKF_CHECK_THRESH'] = 0 # Change the vehicle into STABILIZE mode v.mode = VehicleMode("STABILIZE") # NOTE wait crudely for ACK on mode update time.sleep(3) # Define example callback for mode def armed_callback(attribute): armed_callback.called += 1 armed_callback.called = 0 # When the same (event, callback) pair is passed to add_attribute_observer, # only one instance of the observer callback should be added. v.add_attribute_observer('armed', armed_callback) v.add_attribute_observer('armed', armed_callback) v.add_attribute_observer('armed', armed_callback) v.add_attribute_observer('armed', armed_callback) v.add_attribute_observer('armed', armed_callback) # Disarm and see update. v.armed = False v.flush() # Wait for ACK. time.sleep(3) # Ensure the callback was called. assert armed_callback.called > 0, "Callback should have been called." # Rmove all observers. The first call should remove all listeners # we've added; the second call should be ignored and not throw. # NOTE: We test if armed_callback were treating adding each additional callback # and remove_attribute_observer were removing them one at a time; in this # case, there would be three callbacks still attached. v.remove_attribute_observer('armed', armed_callback) v.remove_attribute_observer('armed', armed_callback) callcount = armed_callback.called # Re-arm and see update. v.armed = True v.flush() # Wait for ack time.sleep(3) # Ensure the callback was called zero times. assert_equals(armed_callback.called, callcount, "Callback should not have been called once removed.")
def checkForObstacle(self): # check if beam will hit the ground at altitudes lower than 3 meters then disable scan self.pitch_angle = abs(self.vehicle.attitude.pitch) self.check_altitude = self.vehicle.location.global_relative_frame.alt #logger.log("pitch: %s" % self.pitch_angle) if (self.vehicle.attitude.pitch < 0 and self.check_altitude < 3): self.beamreach = (self.check_altitude / math.sin(self.pitch_angle)) else: self.beamreach = 1000 if (self.beamreach > SCAN_BEAM_DISTANCE): if (self.led_beam_angle_state == 1): self.led_beam_angle_state = 0 if (self.arduinoBoard.digital_read(COLL_CENTER) == 1): if (self.led_center_state == 0): logger.log("[objavoid]: Obstacle in center") # when we are not in a shot, nor in RTL or LAND and flying higher than 1 meter if (self.currentShot == shots.APP_SHOT_NONE and (self.vehicle.mode.name != 'LAND' or self.vehicle.mode.name != 'RTL')): if (self.check_altitude > 1): self.vehicle.mode = VehicleMode("BRAKE") else: logger.log( "[objavoid]: Must be 1M above the ground") self.led_center_state = 1 else: logger.log("[objavoid]: no obstacle in sight reset LED") self.LEDrgb(2, 2, 0, 255, 0) self.LEDrgb(3, 2, 255, 0, 0) #self.led_right_state = 0 #self.led_left_state = 0 self.led_center_state = 0 #self.led_beam_angle_state = 0 else: if (self.led_beam_angle_state == 0): logger.log( "[objavoid]: pitch/altitude too low - obstacle detection disabled" ) self.LEDrgb(2, 4, 255, 100, 0) self.LEDrgb(3, 4, 255, 100, 0) self.led_beam_angle_state = 1 self.led_right_state = 0 self.led_left_state = 0 self.led_center_state = 0 # send status to app if self.appMgr.isAppConnected(): exceptStr = "Solo altitude too low for scan" packet = struct.pack( '<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr) self.appMgr.client.send(packet) # sleep to make sure the packet goes out time.sleep(0.1)
def arm_and_takeoff(aTargetAltitude): """ Arms vehicle and fly to aTargetAltitude. """ # print "Basic pre-arm checks" # Don't let the user try to fly autopilot is booting if vehicle.mode.name == "INITIALISING": # print "Waiting for vehicle to initialise" time.sleep(1) while vehicle.gps_0.fix_type < 2: # print "Waiting for GPS...:", vehicle.gps_0.fix_type time.sleep(1) # print "Arming motors" # Copter should arm in GUIDED mode vehicle.mode = VehicleMode("GUIDED") vehicle.flush() i = 60 while vehicle.mode.name != 'GUIDED' and i > 0: # print " Waiting for guided %s seconds..." % (i,) time.sleep(1) i = i - 1 vehicle.armed = True vehicle.flush() i = 60 while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0: # print " Waiting for arming %s seconds..." % (i,) time.sleep(1) i = i - 1 # Failure will result in arming but immediately landing assert vehicle.armed assert_equals(vehicle.mode.name, 'GUIDED') # print "Taking off!" vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude vehicle.flush() # Wait until the vehicle reaches a safe height before # processing the goto (otherwise the command after # Vehicle.commands.takeoff will execute immediately). while True: # print " Altitude: ", vehicle.location.alt # Test for altitude just below target, in case of undershoot. if vehicle.location.alt >= aTargetAltitude * 0.95: # print "Reached target altitude" break assert_equals(vehicle.mode.name, 'GUIDED') time.sleep(1)
def start_vehicle(self, altitude): if self.vehicle.mode.name == "INITIALISING": time.sleep(1) while self.vehicle.gps_0.fix_type < 2: time.sleep(1) self.vehicle.armed = True self.vehicle.flush() while not self.vehicle.armed: time.sleep(1) self.vehicle.mode = VehicleMode("GUIDED") self.vehicle.commands.takeoff(altitude) self.vehicle.flush()
def test_115(connpath): v = connect(connpath, await_params=True) # Dummy callback def mavlink_callback(*args): mavlink_callback.count += 1 mavlink_callback.count = 0 # Set the callback. v.set_mavlink_callback(mavlink_callback) # Change the vehicle into STABILIZE mode v.mode = VehicleMode("STABILIZE") # NOTE wait crudely for ACK on mode update time.sleep(3) # Expect the callback to have been called assert mavlink_callback.count > 0 # Unset the callback. v.unset_mavlink_callback() savecount = mavlink_callback.count # Disarm. A callback of None should not throw errors v.armed = False v.flush() # NOTE wait crudely for ACK on mode update time.sleep(3) # Expect the callback to have been called assert_equals(savecount, mavlink_callback.count) # Re-arm should not throw errors. v.armed = True v.flush() # NOTE wait crudely for ACK on mode update time.sleep(3)
def Run(self): while True: try: #print "in shotManager server loop" # handle TCP/RC packets # we set a timeout of UPDATE_TIME, so we roughly do this every UPDATE_TIME rl, wl, el = select.select( self.inputs, self.outputs, [], UPDATE_TIME ) # handle reads for s in rl: if s is self.appMgr.server: # if read is a connection attempt self.appMgr.connectClient() elif s is self.appMgr.client: # if read is from app self.appMgr.parse() elif s is self.rcMgr.server: # if read is an RC packet self.rcMgr.parse() elif s is self.buttonManager.client: # if read is from buttons self.buttonManager.parse() # now handle writes (sololink.btn_msg handles all button writes) for s in wl: if s is self.appMgr.client: # if write is for app self.appMgr.write() # exceptions for s in el: if s is self.appMgr.client: # if its the app socket throwing an exception self.appMgr.exception() else: # otherwise remove whichever socket is excepting if s in self.inputs: self.inputs.remove(s) if s in self.outputs: self.outputs.remove(s) s.close() self.buttonManager.checkButtonConnection() # Check for obstacles when the lidar is active if (self.buttonManager.scanactive == 1): self.checkForObstacle() # Check if copter is outside fence or will be self.geoFenceManager.activateGeoFenceIfNecessary() # call main control/planning loop at UPDATE_RATE if time.time() - self.timeOfLastTick > UPDATE_TIME: self.Tick() except Exception as ex: # reset any RC timeouts and stop any stick remapping self.rcMgr.detach() # try to put vehicle into LOITER self.vehicle.mode = VehicleMode("LOITER") exceptStr = traceback.format_exc() print exceptStr strlist = exceptStr.split('\n') for i in strlist: logger.log(i) if self.appMgr.isAppConnected(): # send error to app packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr) self.appMgr.client.send(packet) # sleep to make sure the packet goes out (for some reason # setting client.setblocking(1) doesn't work) time.sleep(0.4) # cleanup for socket in self.inputs: socket.close() os._exit(1)
def setMode(mode): # Now change the vehicle into auto mode vehicle.mode = VehicleMode(mode)
def change_mode(self, mode): self._log("Mode: {0}".format(mode)) self.vehicle.mode = VehicleMode(mode)
def test_goto(connpath): vehicle = connect(connpath, await_params=True) # NOTE these are *very inappropriate settings* # to make on a real vehicle. They are leveraged # exclusively for simulation. Take heed!!! vehicle.parameters['ARMING_CHECK'] = 0 vehicle.parameters['FS_THR_ENABLE'] = 0 vehicle.parameters['FS_GCS_ENABLE'] = 0 vehicle.parameters['EKF_CHECK_THRESH'] = 0 def arm_and_takeoff(aTargetAltitude): """ Arms vehicle and fly to aTargetAltitude. """ # print "Basic pre-arm checks" # Don't let the user try to fly autopilot is booting if vehicle.mode.name == "INITIALISING": # print "Waiting for vehicle to initialise" time.sleep(1) while vehicle.gps_0.fix_type < 2: # print "Waiting for GPS...:", vehicle.gps_0.fix_type time.sleep(1) # print "Arming motors" # Copter should arm in GUIDED mode vehicle.mode = VehicleMode("GUIDED") vehicle.flush() i = 60 while vehicle.mode.name != 'GUIDED' and i > 0: # print " Waiting for guided %s seconds..." % (i,) time.sleep(1) i = i - 1 vehicle.armed = True vehicle.flush() i = 60 while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0: # print " Waiting for arming %s seconds..." % (i,) time.sleep(1) i = i - 1 # Failure will result in arming but immediately landing assert vehicle.armed assert_equals(vehicle.mode.name, 'GUIDED') # print "Taking off!" vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude vehicle.flush() # Wait until the vehicle reaches a safe height before # processing the goto (otherwise the command after # Vehicle.commands.takeoff will execute immediately). while True: # print " Altitude: ", vehicle.location.alt # Test for altitude just below target, in case of undershoot. if vehicle.location.alt >= aTargetAltitude * 0.95: # print "Reached target altitude" break assert_equals(vehicle.mode.name, 'GUIDED') time.sleep(1) arm_and_takeoff(10) # print "Going to first point..." point1 = Location(-35.361354, 149.165218, 20, is_relative=True) vehicle.commands.goto(point1) vehicle.flush() # sleep so we can see the change in map time.sleep(3) # print "Going to second point..." point2 = Location(-35.363244, 149.168801, 20, is_relative=True) vehicle.commands.goto(point2) vehicle.flush() # sleep so we can see the change in map time.sleep(3) # print "Returning to Launch" vehicle.mode = VehicleMode("RTL") vehicle.flush()
while True: print " Altitude: ", vehicle.location.global_frame.alt if vehicle.location.global_frame.alt >= aTargetAltitude * 0.95: #Trigger just below target alt. print "Reached target altitude" break time.sleep(1) arm_and_takeoff(20) print "Going to first point..." point1 = LocationGlobal(-35.361354, 149.165218, 20, is_relative=True) vehicle.commands.goto(point1) # sleep so we can see the change in map time.sleep(30) print "Going to second point..." point2 = LocationGlobal(-35.363244, 149.168801, 20, is_relative=True) vehicle.commands.goto(point2) # sleep so we can see the change in map time.sleep(30) print "Returning to Launch" vehicle.mode = VehicleMode("RTL") #Close vehicle object before exiting script print "Close vehicle object" vehicle.close()
def recordLocation(self): degreesYaw = camera.getYaw(self.vehicle) pitch = camera.getPitch(self.vehicle) # don't allow two waypoints near each other if len(self.waypoints) == 1: if location_helpers.getDistanceFromPoints3d(self.waypoints[0].loc, self.vehicle.location.global_relative_frame) < WAYPOINT_NEARNESS_THRESHOLD: logger.log("[cable cam]: attempted to record a point too near the first point") # update our waypoint in case yaw, pitch has changed self.waypoints[0].yaw = degreesYaw self.waypoints[0].pitch = pitch # force the app to exit and restart cable cam packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_NONE) self.shotmgr.appMgr.sendPacket(packet) packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shots.APP_SHOT_CABLECAM) self.shotmgr.appMgr.sendPacket(packet) packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT, 28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon, self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch) self.shotmgr.appMgr.sendPacket(packet) return # create a waypoint and add it to our list waypt = Waypoint(self.vehicle.location.global_relative_frame, degreesYaw, pitch) logger.log("[cable cam]: Recorded location %f, %f, %f. Yaw = %f" % ( self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon, self.vehicle.location.global_relative_frame.alt, degreesYaw)) logger.log("[cable cam]: gimbal pitch is " + str(pitch)) self.waypoints.append(waypt) self.setButtonMappings() # send this waypoint to the app packet = struct.pack('<IIddfff', app_packet.SOLO_CABLE_CAM_WAYPOINT, 28, self.vehicle.location.global_relative_frame.lat, self.vehicle.location.global_relative_frame.lon, self.vehicle.location.global_relative_frame.alt, degreesYaw, pitch) self.shotmgr.appMgr.sendPacket(packet) #start monitoring heading changes if len(self.waypoints) == 1: self.aggregateYaw = 0.0 self.lastYaw = degreesYaw elif len(self.waypoints) == 2: # Now change the vehicle into guided mode self.vehicle.mode = VehicleMode("GUIDED") logger.log("[cable cam]: Got second cable point. Should be in guided %s" % (str(self.vehicle.mode))) self.totalDistance = location_helpers.getDistanceFromPoints3d( self.waypoints[0].loc, self.waypoints[1].loc) # handle the 0-360 border if self.waypoints[1].yaw - self.waypoints[0].yaw > 180.0: self.waypoints[0].yaw += 360.0 elif self.waypoints[1].yaw - self.waypoints[0].yaw < -180.0: self.waypoints[1].yaw += 360.0 #disregard aggregate yaw if it's less than 180 if abs(self.aggregateYaw) < 180.0: self.aggregateYaw = self.waypoints[1].yaw - self.waypoints[0].yaw self.yawDirection = 1 if self.aggregateYaw > 0.0 else 0 logger.log("[cable cam]: Aggregate yaw = %f. Yaw direction saved as %s" % (self.aggregateYaw, "CCW" if self.yawDirection == 1 else "CW")) # send this yawDir to the app self.updateAppOptions() self.pathHandler = pathHandler.TwoPointPathHandler( self.waypoints[1].loc, self.waypoints[0].loc, self.vehicle, self.shotmgr )
def demo(local_connect): # This example shows how to use DroneKit-Python to get and set vehicle state, parameter and channel-override information. # It also demonstrates how to observe vehicle attribute (state) changes. # # Usage: # * mavproxy.py # * module load api # * api start vehicle-state.py # from dronekit.lib import VehicleMode from pymavlink import mavutil import time # First get an instance of the API endpoint api = local_connect() # Get the connected vehicle (currently only one vehicle can be returned). v = api.get_vehicles()[0] # Get all vehicle attributes (state) print "\nGet all vehicle attribute values:" print " Location: %s" % v.location print " Attitude: %s" % v.attitude print " Velocity: %s" % v.velocity print " GPS: %s" % v.gps_0 print " Groundspeed: %s" % v.groundspeed print " Airspeed: %s" % v.airspeed print " Mount status: %s" % v.mount_status print " Battery: %s" % v.battery print " Mode: %s" % v.mode.name # settable print " Armed: %s" % v.armed # settable # Set vehicle mode and armed attributes (the only settable attributes) print "Set Vehicle.mode=GUIDED (currently: %s)" % v.mode.name v.mode = VehicleMode("GUIDED") v.flush() # Flush to guarantee that previous writes to the vehicle have taken place while not v.mode.name=='GUIDED' and not api.exit: #Wait until mode has changed print " Waiting for mode change ..." time.sleep(1) print "Set Vehicle.armed=True (currently: %s)" % v.armed v.armed = True v.flush() while not v.armed and not api.exit: print " Waiting for arming..." time.sleep(1) # Show how to add and remove and attribute observer callbacks (using mode as example) def mode_callback(attribute): print " CALLBACK: Mode changed to: ", v.mode.name print "\nAdd mode attribute observer for Vehicle.mode" v.add_attribute_observer('mode', mode_callback) print " Set mode=STABILIZE (currently: %s)" % v.mode.name v.mode = VehicleMode("STABILIZE") v.flush() print " Wait 2s so callback invoked before observer removed" time.sleep(2) # Remove observer - specifying the attribute and previously registered callback function v.remove_attribute_observer('mode', mode_callback) # # Get Vehicle Home location ((0 index in Vehicle.commands) # print "\nGet home location" # cmds = v.commands # cmds.download() # cmds.wait_valid() # print " Home WP: %s" % cmds[0] # Get/Set Vehicle Parameters print "\nRead vehicle param 'THR_MIN': %s" % v.parameters['THR_MIN'] print "Write vehicle param 'THR_MIN' : 10" v.parameters['THR_MIN']=10 v.flush() print "Read new value of param 'THR_MIN': %s" % v.parameters['THR_MIN'] # # Overriding an RC channel # # NOTE: CHANNEL OVERRIDES may be useful for simulating user input and when implementing certain types of joystick control. # #DO NOT use unless there is no other choice (there almost always is!) # print "\nOverriding RC channels for roll and yaw" # v.channel_override = { "1" : 900, "4" : 1000 } # v.flush() # print " Current overrides are:", v.channel_override # print " Channel default values:", v.channel_readback # All channel values before override # # Cancel override by setting channels to 0 # print " Cancelling override" # v.channel_override = { "1" : 0, "4" : 0 } # v.flush() ## Reset variables to sensible values. print "\nReset vehicle atributes/parameters and exit" v.mode = VehicleMode("STABILIZE") v.armed = False v.parameters['THR_MIN']=130 v.flush()
print " Velocity: %s" % vehicle.velocity print " GPS: %s" % vehicle.gps_0 print " Groundspeed: %s" % vehicle.groundspeed print " Airspeed: %s" % vehicle.airspeed print " Mount status: %s" % vehicle.mount_status print " Battery: %s" % vehicle.battery print " Rangefinder: %s" % vehicle.rangefinder print " Rangefinder distance: %s" % vehicle.rangefinder.distance print " Rangefinder voltage: %s" % vehicle.rangefinder.voltage print " Mode: %s" % vehicle.mode.name # settable print " Armed: %s" % vehicle.armed # settable # Set vehicle mode and armed attributes (the only settable attributes) print "\nSet Vehicle.mode=GUIDED (currently: %s)" % vehicle.mode.name vehicle.mode = VehicleMode("GUIDED") while not vehicle.mode.name=='GUIDED': #Wait until mode has changed print " Waiting for mode change ..." time.sleep(1) # Check we have a good gps fix (required to arm) while vehicle.gps_0.fix_type < 2: print "Waiting for GPS fix=3 (needed to arm):", vehicle.gps_0.fix_type time.sleep(1) print "\nSet Vehicle.armed=True (currently: %s)" % vehicle.armed vehicle.armed = True while not vehicle.armed: print " Waiting for arming..."
def checkForObstacle(self): # check if beam will hit the ground at altitudes lower than 3 meters then disable scan self.pitch_angle = abs(self.vehicle.attitude.pitch) self.check_altitude = self.vehicle.location.global_relative_frame.alt #logger.log("pitch: %s" % self.pitch_angle) if (self.vehicle.attitude.pitch < 0 and self.check_altitude < 3): self.beamreach = (self.check_altitude / math.sin(self.pitch_angle)) else: self.beamreach = 1000 if (self.beamreach > SCAN_BEAM_DISTANCE): if (self.led_beam_angle_state == 1): self.led_beam_angle_state = 0 self.LEDrgb(2, 2, 0, 255, 0) self.LEDrgb(3, 2, 255, 0, 0) if (self.arduinoBoard.digital_read(COLL_RIGHT) == 1): if (self.led_right_state == 0): logger.log("[objavoid]: Obstacle on the right") # send info to app if self.appMgr.isAppConnected(): exceptStr = "Obstacle to the right" packet = struct.pack( '<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr) self.appMgr.client.send(packet) # sleep to make sure the packet goes out time.sleep(0.1) # LED right_front set to strobe magenta self.LEDrgb(3, 2, 255, 0, 0) self.LEDrgb(2, 4, 255, 0, 255) self.led_right_state = 1 self.led_left_state = 0 self.led_center_state = 0 self.led_beam_angle_state = 0 elif (self.arduinoBoard.digital_read(COLL_CENTER) == 1): if (self.led_center_state == 0): logger.log("[objavoid]: Obstacle in center") # when we are not in a shot, nor in RTL or LAND and flying higher than 1 meter if (self.currentShot == shots.APP_SHOT_NONE and (self.vehicle.mode.name != 'LAND' or self.vehicle.mode.name != 'RTL')): if (self.check_altitude > 1): self.vehicle.mode = VehicleMode("BRAKE") # all other shots and modes trigger an audio and visual warning only to not interfere with shots # send status to app if self.appMgr.isAppConnected(): exceptStr = "Obstacle ahead in %.1f" % self.coll_distance( ) + " meters" packet = struct.pack( '<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr) self.appMgr.client.send(packet) # sleep to make sure the packet goes out time.sleep(0.2) # both LED front set to strobe magenta self.LEDrgb(2, 4, 255, 0, 255) self.LEDrgb(3, 4, 255, 0, 255) self.led_right_state = 0 self.led_left_state = 0 self.led_center_state = 1 self.led_beam_angle_state = 0 elif (self.arduinoBoard.digital_read(COLL_LEFT) == 1): if (self.led_left_state == 0): logger.log("[objavoid]: Obstacle on the left") # send status to app if self.appMgr.isAppConnected(): exceptStr = "Obstacle to the left" packet = struct.pack( '<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr) self.appMgr.client.send(packet) # sleep to make sure the packet goes out time.sleep(0.1) # LED left_front set to strobe magenta self.LEDrgb(2, 2, 0, 255, 0) self.LEDrgb(3, 4, 255, 0, 255) self.led_right_state = 0 self.led_left_state = 1 self.led_center_state = 0 self.led_beam_angle_state = 0 else: # no obstacle in sight reset everything if (self.led_left_state == 1 or self.led_right_state == 1 or self.led_center_state == 1): logger.log("[objavoid]: no obstacle in sight reset LED") self.LEDrgb(2, 2, 0, 255, 0) self.LEDrgb(3, 2, 255, 0, 0) self.led_right_state = 0 self.led_left_state = 0 self.led_center_state = 0 self.led_beam_angle_state = 0 else: if (self.led_beam_angle_state == 0): logger.log( "[objavoid]: pitch/altitude too low - obstacle detection disabled" ) self.LEDrgb(2, 4, 255, 100, 0) self.LEDrgb(3, 4, 255, 100, 0) self.led_beam_angle_state = 1 self.led_right_state = 0 self.led_left_state = 0 self.led_center_state = 0 # send status to app if self.appMgr.isAppConnected(): exceptStr = "Solo altitude too low for scan" packet = struct.pack( '<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr) self.appMgr.client.send(packet) # sleep to make sure the packet goes out time.sleep(0.1)
def test_vehicle_mode_eq(): assert_equals(VehicleMode('GUIDED'), VehicleMode('GUIDED'))
def setMode(mode): # Now change the vehicle into auto mode v.mode = VehicleMode(mode) # Always call flush to guarantee that previous writes to the vehicle have taken place v.flush()
print 'Clear the current mission' clear_mission() print 'Create a new mission' adds_square_mission(vehicle.location, 50) time.sleep( 2) # This is here so that mission being sent is displayed on console # From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently). arm_and_takeoff(10) print "Starting mission" # Set mode to AUTO to start mission vehicle.mode = VehicleMode("AUTO") vehicle.flush() # Monitor mission. # Demonstrates getting and setting the command number # Uses distance_to_current_waypoint(), a convenience function for finding the # distance to the next waypoint. while True: nextwaypoint = vehicle.commands.next if nextwaypoint > 1: print 'Distance to waypoint (%s): %s' % ( nextwaypoint, distance_to_current_waypoint()) if nextwaypoint == 3: #Skip to next waypoint print 'Skipping to Waypoint 4 when reach waypoint 3' vehicle.commands.next = 4
print("Set new Home location to current location") set_home(vehicle.location) print "Get new home location" #This reloads the home location in GCSs cmds = vehicle.commands cmds.download() cmds.wait_valid() print " Home WP: %s" % cmds[0] print("Velocity South and West") send_global_velocity(NORTH, EAST, 0) print("Yaw 90 relative (to previous yaw heading)") condition_yaw(90, relative=True) time.sleep(DURATION) send_global_velocity(0, 0, 0) print("Velocity North and West") send_global_velocity(SOUTH, EAST, 0) print("Yaw 90 relative (to previous yaw heading)") condition_yaw(90, relative=True) time.sleep(DURATION) send_global_velocity(0, 0, 0) """ The example is completing. LAND at current location. """ print("Setting LAND mode...") vehicle.mode = VehicleMode("LAND") vehicle.flush() print("Completed")
def enterShot(self, shot): if shot not in shots.SHOT_NAMES: logger.log("[shot]: Shot not recognized. (%d)" % shot) return if shot == shots.APP_SHOT_NONE: pass # check our EKF - if it's bad and that we can't init the shot prior to EKF being OK, reject shot entry attempt elif self.last_ekf_ok is False and shot not in shots.CAN_START_BEFORE_EKF: logger.log('[shot]: Vehicle EKF quality is poor, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot]) # set shot to APP_SHOT_NONE shot = shots.APP_SHOT_NONE # notify the app of shot entry failure packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_BAD_EKF) self.appMgr.sendPacket(packet) # check vehicle system status - if it's CRITICAL or EMERGENCY, reject shot entry attempt elif self.vehicle.system_status in ['CRITICAL', 'EMERGENCY']: logger.log('[shot]: Vehicle in %s, shot entry into %s disallowed.' % (self.vehicle.system_status, shots.SHOT_NAMES[shot])) # set shot to APP_SHOT_NONE shot = shots.APP_SHOT_NONE # notify the app of shot entry failure packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_RTL) self.appMgr.sendPacket(packet) # check if vehicle is not armed or in STANDBY and that we can't init the shot prior to arm, reject shot entry attempt elif (self.vehicle.armed is False or self.vehicle.system_status == 'STANDBY') and shot not in shots.CAN_START_BEFORE_ARMING: logger.log('[shot]: Vehicle is unarmed, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot]) self.vehicle.mode = VehicleMode("LOITER") # set shot to APP_SHOT_NONE shot = shots.APP_SHOT_NONE # notify the app of shot entry failure packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED) self.appMgr.sendPacket(packet) # OK fine, you get to start the shot. if self.currentShot != shot: logger.log('[shot]: Entering shot %s.' % shots.SHOT_NAMES[shot]) if self.currentShot == shots.APP_SHOT_REWIND: # we are exiting Rewind self.rewindManager.resetSpline() # APP_SHOT_NONE if shot == shots.APP_SHOT_NONE: # mark curController for garbage collection del self.curController # set curController to None (should also mark for garbage collection) self.curController = None # re-enable manual gimbal controls (RC Targeting mode) self.vehicle.gimbal.release() # disable the stick re-mapper self.rcMgr.enableRemapping( False ) # disable scanning self.arduinoBoard.digital_write(6, 0) # if the Rewind shot put us into RTL, lets stay there if self.vehicle.mode.name == 'RTL': logger.log("[shot]: Leaving vehicle in mode RTL") # if vehicle mode is in another mode such as GUIDED or AUTO, then switch to LOITER elif self.vehicle.mode.name in shots.SHOT_MODES: logger.log("[shot]: Changing vehicle mode to LOITER.") self.vehicle.mode = VehicleMode("LOITER") else: self.curController = ShotFactory.get_shot_obj(shot, self.vehicle, self) # update currentShot self.currentShot = shot logger.log("[shot]: Successfully entered %s." % shots.SHOT_NAMES[shot]) # already in that shot else: logger.log('[shot]: Already in shot %s.' % shots.SHOT_NAMES[shot]) # let the world know if self.appMgr.isAppConnected(): self.appMgr.broadcastShotToApp(shot) # let Artoo know too self.buttonManager.setArtooShot(shot) # set new button mappings appropriately self.buttonManager.setButtonMappings()
def Start(self, vehicle): logger.log("+-+-+-+-+-+-+ Starting up %s +-+-+-+-+-+-+" % VERSION) ### initialize dronekit vehicle ### self.vehicle = vehicle ### switch vehicle to loiter mode ### self.vehicle.mode = VehicleMode("LOITER") ### initialize rc manager ### self.rcMgr = rcManager.rcManager(self) ### initialize app manager ### self.appMgr = appManager.appManager(self) ### initialize button manager ### self.buttonManager = buttonManager.buttonManager(self) ### initialize gopro manager ### self.goproManager = GoProManager.GoProManager(self) ### Initialize GeoFence manager ### self.geoFenceManager = GeoFenceManager.GeoFenceManager(self) # instantiate rewindManager self.rewindManager = rewindManager.RewindManager(self.vehicle, self) # Create a PyMata instance and initialize the object avoidance toggles self.led_left_state = 0 self.led_right_state = 0 self.led_center_state = 0 self.led_beam_angle_state = 0 self.center_collision_state = 0 try: self.arduinoBoard = PyMata("/dev/ttyACM0", verbose=True) self.arduinoBoard.set_pin_mode(COLL_RIGHT, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) self.arduinoBoard.set_pin_mode(COLL_CENTER, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) self.arduinoBoard.set_pin_mode(COLL_LEFT, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) self.arduinoBoard.set_pin_mode(SCAN_BIT1, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) self.arduinoBoard.set_pin_mode(SCAN_BIT2, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) self.arduinoBoard.set_pin_mode(SCAN_BIT3, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) self.arduinoBoard.set_pin_mode(SCAN_BIT4, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL) except: logger.log("[shotmanager]: Error in communication to Arduino") ### init APM stream rates ### self.initStreamRates() ### register callbacks ### self.registerCallbacks() # Try to maintain a constant tick rate self.timeOfLastTick = monotonic.monotonic() # how many ticks have we performed since an RC update? # register all connections (gopro manager communicates via appMgr's socket) self.inputs = [self.rcMgr.server, self.appMgr.server] self.outputs = [] #check if gimbal is present if self.vehicle.gimbal.yaw is not None: logger.log("[shot]: Gimbal detected.") # Initialize gimbal to RC targeting self.vehicle.gimbal.release() else: logger.log("[shot]: No gimbal detected.") # mark first tick time self.timeOfLastTick = monotonic.monotonic() # check for In-Air start from Shotmanager crash if self.vehicle.system_status == 'ACTIVE': logger.log("[shot]: Restart in air.") # load vehicle home self.rewindManager.loadHomeLocation()
def __get_mode(self): """Private method to read current vehicle mode without polling""" return VehicleMode(self.__module.status.flightmode)