def __init__(self, device='MPU-9150'):
     BaseThread.__init__(self)
     log.d("Init serialThread")
     # configure and retrieve IMU scales
     self.imu = IMU(device)
     self.csvScale = self.imu.getScaleVector()
     self.csvSize = len(self.csvScale)
     self.range_accel = self.imu.getRangeAccelerometer()
     self.range_gyro = self.imu.getRangeGyroscope()
     self.range_mag = self.imu.getRangeMagnetometer()
     self.range_euler = self.imu.getRangeEuler()
     # star sensor fusion
     self.fusion = SensorFusion()
Ejemplo n.º 2
0
import time

from remote import AndroidRemote
from motors import Motors
from imu import IMU
from pid import PID

remote_ctl = AndroidRemote()
drone_motors = Motors()
drone_imu = IMU()
pid = PID()

print('start_server:: Ready!')
drone_motors.start()
print('start_motors:: Ok!')

# power = 0
power = {'UR': 30, 'UL': 30, 'BR': 30, 'BL': 30}

t1 = time.time()
try:
    while True:
        # Get data from android orientation
        target_state = remote_ctl.get_data()

        # Get data from local sensor
        t2 = time.time()
        cur_state = drone_imu.get_data(t2 - t1)

        # Find difference between actual and target values.
        error_state = {}
Ejemplo n.º 3
0
class GCS:
    def __init__(self):

        # No communications or arming yet
        self.comms = None
        self.armed = False
        self.gotimu = False

        # Do basic Tk initialization
        self.root = tk.Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' %
                           (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = tk.Frame(self.root)

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w,
                              tk.PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1,
                                               self._connect_callback)
        self.button_imu = self._add_button('IMU', self.pane2,
                                           self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2,
                                              self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2,
                                                self._receiver_button_callback)
        #self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
        #self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = tk.StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = tk.Canvas(self.root,
                                width=DISPLAY_WIDTH,
                                height=DISPLAY_HEIGHT,
                                background='black')
        self.canvas.pack()

        # Set up a text label for reporting errors
        errmsg = 'No response from board.  Possible reasons:\n\n' + \
                 '    * You connected to the wrong port.\n\n' + \
                 '    * Firmware uses serial receiver\n' + \
                 '      (DSMX, SBUS), but receiver is\n' + \
                 '      not connected.'
        self.error_label = tk.Label(self.canvas,
                                    text=errmsg,
                                    bg='black',
                                    fg='red',
                                    font=(None, 24),
                                    justify=tk.LEFT)
        self.hide(self.error_label)

        # Add widgets for motor-testing dialog; hide them immediately
        self.motors = Motors(self)
        self.motors.stop()

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create messages dialog
        #self.messages = Messages(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a maps dialog
        #self.maps = Maps(self, yoffset=-30)

        # Create a splash image
        self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
        self._show_splash()

        # Create a message parser
        self.parser = msppg.MSP_Parser()

        # Set up parser's request strings
        self.attitude_request = msppg.serialize_ATTITUDE_RADIANS_Request()
        self.rc_request = msppg.serialize_RC_NORMAL_Request()

        # No messages yet
        self.roll_pitch_yaw = [0] * 3
        self.rxchannels = [0] * 6

        # A hack to support display in IMU dialog
        self.active_axis = 0

    def quit(self):
        self.motors.stop()
        self.root.destroy()

    def hide(self, widget):

        widget.place(x=-9999)

    def getChannels(self):

        return self.rxchannels

    def getRollPitchYaw(self):

        # configure button to show connected
        self._enable_buttons()
        self.button_connect['text'] = 'Disconnect'
        self._enable_button(self.button_connect)

        return self.roll_pitch_yaw

    def checkArmed(self):

        if self.armed:

            self._show_armed(self.root)
            self._show_armed(self.pane1)
            self._show_armed(self.pane2)

            self._disable_button(self.button_motors)

        else:

            self._show_disarmed(self.root)
            self._show_disarmed(self.pane1)
            self._show_disarmed(self.pane2)

    def scheduleTask(self, delay_msec, task):

        self.root.after(delay_msec, task)

    def _add_pane(self):

        pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
        pane.pack(fill=tk.BOTH, expand=1)
        return pane

    def _add_button(self, label, parent, callback, disabled=True):

        button = tk.Button(parent, text=label, command=callback)
        button.pack(side=tk.LEFT)
        button.config(state='disabled' if disabled else 'normal')
        return button

    # Callback for IMU button
    def _imu_callback(self):

        self._clear()

        self.motors.stop()
        self.receiver.stop()
        #self.messages.stop()
        #self.maps.stop()

        self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
        self._send_attitude_request()
        self.imu.start()

    def _start(self):

        self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
        self._send_attitude_request()
        self.imu.start()

        self.parser.set_RC_NORMAL_Handler(self._handle_rc)

        self.gotimu = False
        self.hide(self.error_label)
        self.scheduleTask(BOARD_REPLY_DELAY_MSEC, self._checkimu)

    def _checkimu(self):

        if not self.gotimu:

            self.imu.stop()
            self.error_label.place(x=50, y=50)
            self._disable_button(self.button_imu)
            self._disable_button(self.button_motors)
            self._disable_button(self.button_receiver)

    # Sends Attitude request to FC
    def _send_attitude_request(self):

        self.comms.send_request(self.attitude_request)

    # Sends RC request to FC
    def _send_rc_request(self):

        self.comms.send_request(self.rc_request)

    # Callback for Motors button
    def _motors_button_callback(self):

        self._clear()

        self.imu.stop()
        self.receiver.stop()
        #self.messages.stop()
        #self.maps.stop()
        self.motors.start()

    def _clear(self):

        self.canvas.delete(tk.ALL)

    # Callback for Receiver button
    def _receiver_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors.stop()
        #self.messages.stop()
        #self.maps.stop()

        self._send_rc_request()
        self.receiver.start()

    # Callback for Messages button
    def _messages_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors.stop()
        #self.maps.stop()
        self.receiver.stop()

        self.messages.start()

    def _getting_messages(self):

        return self.button_connect['text'] == 'Disconnect'

    # Callback for Maps button
    def _maps_button_callback(self):

        self._clear()

        if self._getting_messages():

            self.receiver.stop()
            self.messages.stop()
            self.imu.stop()
            self.motors.stop()

        #self.maps.start()

    # Callback for Connect / Disconnect button
    def _connect_callback(self):

        if self.connected:

            self.imu.stop()
            #self.maps.stop()
            self.motors.stop()
            #self.messages.stop()
            self.receiver.stop()

            if not self.comms is None:

                self.comms.stop()

            self._clear()

            self._disable_buttons()

            self.button_connect['text'] = 'Connect'

            self.hide(self.error_label)

            self._show_splash()

        else:

            #self.maps.stop()

            self.comms = Comms(self)
            self.comms.start()

            self.button_connect['text'] = 'Connecting ...'
            self._disable_button(self.button_connect)

            self._hide_splash()

            self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)

        self.connected = not self.connected

    # Gets available ports
    def _getports(self):

        allports = comports()

        ports = []

        for port in allports:

            portname = port[0]

            if 'usbmodem' in portname or 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
                if not portname in ['COM1', 'COM2']:
                    ports.append(portname)

        return ports

    # Checks for changes in port status (hot-plugging USB cables)
    def _connection_task(self):

        ports = self._getports()

        if ports != self.ports:

            if self.portsmenu is None:

                self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
                                               *ports)

            else:

                for port in ports:

                    self.portsmenu['menu'].add_command(label=port)

            self.portsmenu.pack(side=tk.LEFT)

            # Disconnected
            if ports == []:

                self.portsmenu['menu'].delete(0, 'end')
                self.portsvar.set('')
                self._disable_button(self.button_connect)
                self._disable_buttons()

            # Connected
            else:
                self.portsvar.set(ports[0])  # default value
                self._enable_button(self.button_connect)

            self.ports = ports

        self._schedule_connection_task()

    # Mutually recursive with above
    def _schedule_connection_task(self):

        self.root.after(USB_UPDATE_MSEC, self._connection_task)

    def _disable_buttons(self):

        self._disable_button(self.button_imu)
        self._disable_button(self.button_motors)
        self._disable_button(self.button_receiver)
        #self._disable_button(self.button_messages)

    def _enable_buttons(self):

        self._enable_button(self.button_imu)
        self._enable_button(self.button_motors)
        self._enable_button(self.button_receiver)
        #self._enable_button(self.button_messages)

    def _enable_button(self, button):

        button['state'] = 'normal'

    def _disable_button(self, button):

        button['state'] = 'disabled'

    def sendMotorMessage(self, index, percent):

        values = [0] * 4
        values[index - 1] = percent / 100.
        self.comms.send_message(msppg.serialize_SET_MOTOR_NORMAL, values)

    def _show_splash(self):

        self.splash = self.canvas.create_image(SPLASH_LOCATION,
                                               image=self.splashimage)

    def _hide_splash(self):

        self.canvas.delete(self.splash)

    def _show_armed(self, widget):

        widget.configure(bg='red')

    def _show_disarmed(self, widget):

        widget.configure(bg=BACKGROUND_COLOR)

        if self._getting_messages():

            self._enable_button(self.button_motors)

    def _handle_calibrate_response(self):

        self.imu.showCalibrated()

    def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):

        # Only handle parms from firmware on a fresh connection
        if self.newconnect:

            self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)

        self.newconnect = False

    def _handle_attitude(self, x, y, z):

        self.roll_pitch_yaw = x, -y, z

        self.gotimu = True

        #self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)

        # As soon as we handle the callback from one request, send another request, if IMU dialog is running
        if self.imu.running:
            self._send_attitude_request()

    def _handle_rc(self, c1, c2, c3, c4, c5, c6):

        # Display throttle as [0,1], other channels as [-1,+1]
        self.rxchannels = c1 / 2. + .5, c2, c3, c4, c5, c6

        # As soon as we handle the callback from one request, send another request, if receiver dialog is running
        if self.receiver.running:
            self._send_rc_request()

        #self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))

    def _handle_arm_status(self, armed):

        self.armed = armed

        #self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))

    def _handle_battery_status(self, volts, amps):

        return
Ejemplo n.º 4
0
        SS(inst_id=2,com_port='COM4',baudrate=115200),
        SS(inst_id=3,com_port='COM7',baudrate=115200)]
    
    #List of sun sensors to read data from
    ss_read = [2,3]
    
    #List of sun sensors to use for tracking
    ss_track = [2,3]
    
#    ss_eshim_x = [-1.763, -1.547, -1.578]          #Specify electronic shims (x-dir) for sun sensors
#    ss_eshim_y = [-2.290, -2.377, -2.215]          #Specify electronic shims (y-dir) for sun sensors
    ss_eshim_x = [0.0,0.0,0.0]          #Specify electronic shims (x-dir) for sun sensors
    ss_eshim_y = [0.0,0.0,0.0]          #Specify electronic shims (y-dir) for sun sensors

    #Establish communication with IMU
    imu=IMU(com_port='COM5',baudrate=115200)
    
    #Establish communication with PTU
    ptu_cmd_delay=0.025
    ptu = PTU(com_port='COM6',baudrate=9600,cmd_delay=ptu_cmd_delay)
    #Set latitude, longitude and altitude to Blacksburg, VA for sun pointing
    ptu.lat, ptu.lon, ptu.alt = '37.205144','-80.417560', 634
    ptu.utc_off=4   #Set UTC time offset of EST

    #Find the Sun and the moon from your location
    lat,lon,alt='37.205144','-80.417560',634    #Blacksburg
    utc_datetime = datetime.now()   #Use current time (can also set to custom datetime= '2018/5/7 16:04:56')
    ptu.ephem_point(ep,imu=imu,target='sun',init=False,ptu_cmd=False)
    ptu.ephem_point(ep,imu=imu,target='moon',init=False,ptu_cmd=False)
    
    #Define Default Modes  
Ejemplo n.º 5
0
def main():
    # read arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-v", "--video",
                    help="path to the (optional) video file")
    ap.add_argument("-c", "--camera",
                    help="index of camera")
    ap.add_argument("-o", "--output",
                    help="path to save the video")
    ap.add_argument("-s", "--speed")
    args = vars(ap.parse_args())

    if args.get("video", False):
        vs = args.get("video", False)
    elif args.get("camera", False):
        vs = int(args.get("camera", False))
    else:
        vs = 0
    
    set_speed = args.get('speed', 0)
    if set_speed is None:
        set_speed = 0
    set_speed = float(set_speed)
    speed = 0

    # inits CV
    cv = CVManager(vs,                  # choose the first web camera as the source
                   enable_imshow=True,  # enable image show windows
                   server_port=3333,    # start stream server at port 3333
                   delay=5,
                   outputfolder=args.get("output"))
    cv.add_core("GateTracker", GateTrackerV3(), True)

    # inits MCU
    mcu = MCU(2222)

    # inits IMU
    imu = IMU("/dev/ttyUSB_IMU")

    # start subprocess
    cv.start()
    imu.start()
    mcu.start()

    mcu.wait()

    pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5
    pidP = pidPitch(0.6, 0, 0)# 5 ,0.1 ,8
    pidD = pidDepth(1, 0, 0)
    pidY = pidYaw(1, 0.3, 0)
    motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0

    try:
        motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
        counter = 0
        last_cv_gate = 0
        gate_passed = False
        while True:
            counter += 1
            gate, _, gate_size = cv.get_result("GateTracker")
            depth = mcu.get_depth()
            pinger = mcu.get_angle()
            pitch = imu.get_pitch()
            roll = imu.get_roll()

            if gate_passed or gate is None: # just go straight
                yaw = imu.get_yaw2(0)
            else:
                if gate != last_cv_gate:
                    imu.reset_yaw2(-gate * 0.1, 1)
                    last_cv_gate = gate
                else:
                    yaw = imu.get_yaw2(1)

                if gate_size > 350:
                    gate_passed = True

            if abs(yaw) > 10:
                speed = 0
            else:
                speed = set_speed

            pidR.getSetValues(roll)
            pidP.getSetValues(pitch)
            pidD.getSetValues(70-depth)
            pidY.getSetValues(-yaw)
            finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start())

            sentValues  = []
            for values in finalPidValues:
                subValues = values
                sentValues.append(subValues)

            motor_fl = sentValues[0]
            motor_fr = sentValues[1]
            motor_bl = sentValues[2] + set_speed
            motor_br = sentValues[3] + set_speed
            motor_t = sentValues[4]

            # Put control codes here

            mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)

            if counter % 20 == 0:
                print('Gate', gate)
                print('GateSize', gate_size)
                print('Passed?', gate_passed)
                print('Depth:', depth)
                print('Pinger:', pinger)
                print('Pitch:', pitch)
                print('Roll:', roll)
                print('Yaw:', imu.get_yaw2())
                print('Yaw_sent:', yaw)
                print('Motors: %.2f %.2f %.2f %.2f %.2f'%(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
                print()
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping remaining threads...")
        cv.stop()
        imu.stop()
        mcu.stop()
Ejemplo n.º 6
0
        ss_track.append(2)
    if params.ss3_track:
        ss_track.append(3)

#    ss_eshim_x = [-1.763, -1.547, -1.578]          #Specify electronic shims (x-dir) for sun sensors
#    ss_eshim_y = [-2.290, -2.377, -2.215]          #Specify electronic shims (y-dir) for sun sensors
    ss_eshim_x = [params.ss1_eshim_x, params.ss2_eshim_x, params.ss3_eshim_x
                  ]  #Specify electronic shims (x-dir) for sun sensors
    ss_eshim_y = [params.ss1_eshim_y, params.ss2_eshim_y, params.ss3_eshim_y
                  ]  #Specify electronic shims (y-dir) for sun sensors

    print('eshims_x', ss_eshim_x)
    print('eshims_y', ss_eshim_y)

    #Establish communication with IMU
    imu = IMU(com_port=params.imu_com_port, baudrate=params.imu_baud_rate)

    #Establish communication with PTU
    ptu_cmd_delay = params.ptu_cmd_delay  #0.010
    ptu = PTU(com_port=params.ptu_com_port,
              baudrate=params.ptu_baud_rate,
              cmd_delay=params.ptu_cmd_delay)
    #Set latitude, longitude and altitude to Blacksburg, VA for sun pointing
    ptu.lat, ptu.lon, ptu.alt = params.ptu_lat, params.ptu_lon, params.ptu_alt  #'37.205144','-80.417560', 634
    ptu.utc_off = params.ptu_utc_off  #4   #Set UTC time offset of EST

    #Find the Sun and the moon from your location
    #    lat,lon,alt='37.205144','-80.417560',634    #Blacksburg
    #    utc_datetime = datetime.now()   #Use current time (can also set to custom datetime= '2018/5/7 16:04:56')
    ptu.ephem_point(ep, imu=imu, target='sun', init=False, ptu_cmd=False)
    ptu.ephem_point(ep, imu=imu, target='moon', init=False, ptu_cmd=False)
Ejemplo n.º 7
0
    # Now append a new rotation
    pi_2 = math.pi / 2
    x_rotations.append(-accelerometer['z'] * pi_2)
    z_rotations.append(accelerometer['x'] * pi_2)
    # Calculate rotation matrix
    return createRotationMatrix(
        # this averaging isn't correct in the first <smoothing> frames, but who cares
        sum(x_rotations) / smoothing, 
        math.radians(y_rotation),
        sum(z_rotations) / smoothing
    )        

# Initialise hardware
ugfx.init()
ugfx.clear(ugfx.BLACK)
imu=IMU()
buttons.init()

# Enable tear detection for vsync
ugfx.enable_tear()
tear = pyb.Pin("TEAR", pyb.Pin.IN)
ugfx.set_tear_line(1)

# Set up static rendering matrices
camera_transform = createCameraMatrix(0, 0, -5.0)
proj = createProjectionMatrix(45.0, 100.0, 0.1)
camera_projection = proj * camera_transform

# Get the list of available objects, and load the first one
obj_vertices = []
obj_faces = []
Ejemplo n.º 8
0
from imu import IMU

i = IMU('/dev/ttyACM4')

print("Connected to IMU")

name = input("Enter a new name: ").rstrip()

i.rename(name)

print("The IMU is now %s" % i.get_identifier())
Ejemplo n.º 9
0
class SerialThread(BaseThread):
    ser = serial.Serial()
    A = [.0] * 3
    G = [.0] * 3
    M = [.0] * 3
    E_dmp = [.3] * 3
    E = [.0] * 3
    LA = [.0] * 3

    ## Constructor
    #
    # Enables the basic thread functions and logs
    # @param self The object pointer
    def __init__(self, device='MPU-9150'):
        BaseThread.__init__(self)
        log.d("Init serialThread")
        # configure and retrieve IMU scales
        self.imu = IMU(device)
        self.csvScale = self.imu.getScaleVector()
        self.csvSize = len(self.csvScale)
        self.range_accel = self.imu.getRangeAccelerometer()
        self.range_gyro = self.imu.getRangeGyroscope()
        self.range_mag = self.imu.getRangeMagnetometer()
        self.range_euler = self.imu.getRangeEuler()
        # star sensor fusion
        self.fusion = SensorFusion()

    ## Open the defined point
    #
    # Configures the serial port, address and speed, and verifies
    # correct initialization and status
    # @param self The object pointer
    def openPort(self, port='/dev/ttyUSB0', baud=115200):
        try:
            # configure port
            self.ser.port = port
            self.ser.baudrate = baud
            self.ser.stopbits = serial.STOPBITS_ONE
            self.ser.bytesize = serial.EIGHTBITS
            self.ser.rtscts = 1
            self.ser.timeout = 0.5

            # open port
            if self.ser.isOpen():
                log.w("Port is already open, closing it")
                self.ser.close()
            self.ser.open()
            # clear input
            self.ser.flushInput()
            # verify that port is open
            if self.ser.isOpen():
                log.d("Port " + port + " opened at " + str(baud))
        except serial.SerialException as se:
            log.e("Unable to open port %s" % port)
        except:
            raise

    ## The thread process itself
    #
    # Defines the "loop" method of the thread
    # + Obtain self.data from the defined self.serial port
    # + Separate in CSV format
    # + Assuming the input from a MPU-9150, scales self.data
    # + Manage incorrect CSV format (by size)
    # + Manage incomplete self.data transfer
    # + Notify main thread (Qt4) with a signal
    # @param self The objetc pointer
    def run(self):
        while not self.exiting and self.ser.isOpen():
            try:
                # check if there is self.data avaliable
                if self.ser.inWaiting() > 0:
                    raw = self.ser.readline().strip()
                    log.v(raw)
                    try:
                        # convert received data to a float list
                        data = map(float, raw.split(','))
                        # if got correct data size, scale it
                        if len(data) == self.csvSize:
                            for i in range(0, self.csvSize):
                                data[i] *= self.csvScale[i]

                            # accelerometer
                            for i in self.range_accel:
                                self.A[i - self.range_accel[0]] = data[i]
                            # gyroscope
                            for i in self.range_gyro:
                                self.G[i - self.range_gyro[0]] = data[i]
                            # magnetometer
                            for i in self.range_mag:
                                self.M[i - self.range_mag[0]] = data[i]
                            # euler if its avaliable
                            if self.E_dmp[0] != -1:
                                for i in self.range_euler:
                                    self.E_dmp[i -
                                               self.range_euler[0]] = data[i]

                            # do sensor fusion
                            self.fusion.update9DOF(self.G, self.A, self.M)
                            self.LA = self.fusion.getLinearAcceleration()
                            self.E = self.fusion.getYawPitchRoll(True)

                            # notify new data is ready
                            self.emit(QtCore.SIGNAL('newData()'))

                        else:
                            log.d('Incorrect data format')
                            self.ser.flushInput()
                            pass
                    except:
                        self.ser.flushInput()
                        log.v("Skipping incomplete transfer")
                        pass
            except:
                self.closePort()
                log.e("Thread Stopped by unknow error")
                return
        log.d("Thread Stopped Normally")
        return

    ## Closes the self.serial port and waits for the thread to stop
    #
    # @param self The objet pointer#
    def closePort(self):
        log.i("Waiting Thread to stop...")
        while self.isRunning():
            self.exiting = True
            self.exit()
        self.ser.close()
        log.d("Port closed")

    def getAcceleration(self):
        return self.A

    def getLinearAccel(self):
        return self.LA

    def getGyroscope(self):
        return self.G

    def getMagnetometer(self):
        return self.M

    def getEulerDMP(self):
        return self.E_dmp

    def getEuler(self):
        return self.E
Ejemplo n.º 10
0
from imu import IMU

i = IMU('/dev/ttyACM3')

print("Connected to IMU")
print("Identifier %s" % i.get_identifier())
Ejemplo n.º 11
0
class GCS:
    def __init__(self):

        # No communications or arming yet
        self.comms = None
        self.armed = False

        # Do basic Tk initialization
        self.root = Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' %
                           (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = Frame(self.root)

        self.root.wm_iconbitmap(bitmap="@media/icon.xbm")

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w,
                              PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1,
                                               self._connect_callback)
        self.button_imu = self._add_button('IMU', self.pane2,
                                           self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2,
                                              self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2,
                                                self._receiver_button_callback)
        #self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
        #self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = Canvas(self.root,
                             width=DISPLAY_WIDTH,
                             height=DISPLAY_HEIGHT,
                             background='black')
        self.canvas.pack()

        # Add widgets for motor-testing dialog; hide them immediately
        self.motors = Motors(self)
        self.motors.stop()

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create messages dialog
        #self.messages = Messages(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a maps dialog
        #self.maps = Maps(self, yoffset=-30)

        # Create a splash image
        self.splashimage = PhotoImage(file='media/splash.gif')
        self._show_splash()

        # Create a message parser
        self.parser = MSP_Parser()

        # Set up parser's request strings
        self.attitude_request = serialize_ATTITUDE_Request()
        self.rc_request = serialize_RC_Request()

        # No messages yet
        self.roll_pitch_yaw = 0, 0, 0
        self.rxchannels = 0, 0, 0, 0, 0

        # A hack to support display in IMU dialog
        self.active_axis = 0

    def quit(self):
        self.motors.stop()
        self.root.destroy()

    def hide(self, widget):

        widget.place(x=-9999)

    def getChannels(self):

        return self.rxchannels

    def getRollPitchYaw(self):

        # configure button to show connected
        self._enable_buttons()
        self.button_connect['text'] = 'Disconnect'
        self._enable_button(self.button_connect)

        return self.roll_pitch_yaw

    def checkArmed(self):

        if self.armed:

            self._show_armed(self.root)
            self._show_armed(self.pane1)
            self._show_armed(self.pane2)

            self._disable_button(self.button_motors)

        else:

            self._show_disarmed(self.root)
            self._show_disarmed(self.pane1)
            self._show_disarmed(self.pane2)

    def scheduleTask(self, delay_msec, task):

        self.root.after(delay_msec, task)

    def _add_pane(self):

        pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
        pane.pack(fill=BOTH, expand=1)
        return pane

    def _add_button(self, label, parent, callback, disabled=True):

        button = Button(parent, text=label, command=callback)
        button.pack(side=LEFT)
        button.config(state='disabled' if disabled else 'normal')
        return button

    # Callback for IMU button
    def _imu_callback(self):

        self._clear()

        self.motors.stop()
        self.receiver.stop()
        #self.messages.stop()
        #self.maps.stop()

        self.parser.set_ATTITUDE_Handler(self._handle_attitude)
        self._send_attitude_request()
        self.imu.start()

    def _start(self):

        self.parser.set_ATTITUDE_Handler(self._handle_attitude)
        self._send_attitude_request()
        self.imu.start()

        self.parser.set_RC_Handler(self._handle_rc)

    # Sends Attitude request to FC
    def _send_attitude_request(self):

        self.comms.send_request(self.attitude_request)

    # Sends RC request to FC
    def _send_rc_request(self):

        self.comms.send_request(self.rc_request)

    # Callback for Motors button
    def _motors_button_callback(self):

        self._clear()

        self.imu.stop()
        self.receiver.stop()
        #self.messages.stop()
        #self.maps.stop()
        self.motors.start()

    def _clear(self):

        self.canvas.delete(ALL)

    # Callback for Receiver button
    def _receiver_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors.stop()
        #self.messages.stop()
        #self.maps.stop()

        self._send_rc_request()
        self.receiver.start()

    # Callback for Messages button
    def _messages_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors.stop()
        #self.maps.stop()
        self.receiver.stop()

        self.messages.start()

    def _getting_messages(self):

        return self.button_connect['text'] == 'Disconnect'

    # Callback for Maps button
    def _maps_button_callback(self):

        self._clear()

        if self._getting_messages():

            self.receiver.stop()
            self.messages.stop()
            self.imu.stop()
            self.motors.stop()

        #self.maps.start()

    # Callback for Connect / Disconnect button
    def _connect_callback(self):

        if self.connected:

            self.imu.stop()
            #self.maps.stop()
            self.motors.stop()
            #self.messages.stop()
            self.receiver.stop()

            if not self.comms is None:

                self.comms.stop()

            self._clear()

            self._disable_buttons()

            self.button_connect['text'] = 'Connect'

            self._show_splash()

        else:

            #self.maps.stop()

            self.comms = Comms(self)
            self.comms.start()

            self.button_connect['text'] = 'Connecting ...'
            self._disable_button(self.button_connect)

            self._hide_splash()

            self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)

        self.connected = not self.connected

    # Gets available ports
    def _getports(self):

        allports = comports()

        ports = []

        for port in allports:

            portname = port[0]

            if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
                ports.append(portname)

        return ports

    # Checks for changes in port status (hot-plugging USB cables)
    def _connection_task(self):

        ports = self._getports()

        if ports != self.ports:

            if self.portsmenu is None:

                self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)

            else:

                for port in ports:

                    self.portsmenu['menu'].add_command(label=port)

            self.portsmenu.pack(side=LEFT)

            if ports == []:

                self._disable_button(self.button_connect)
                self._disable_buttons()

            else:
                self.portsvar.set(ports[0])  # default value
                self._enable_button(self.button_connect)

            self.ports = ports

        self._schedule_connection_task()

    # Mutually recursive with above
    def _schedule_connection_task(self):

        self.root.after(USB_UPDATE_MSEC, self._connection_task)

    def _disable_buttons(self):

        self._disable_button(self.button_imu)
        self._disable_button(self.button_motors)
        self._disable_button(self.button_receiver)
        #self._disable_button(self.button_messages)

    def _enable_buttons(self):

        self._enable_button(self.button_imu)
        self._enable_button(self.button_motors)
        self._enable_button(self.button_receiver)
        #self._enable_button(self.button_messages)

    def _enable_button(self, button):

        button['state'] = 'normal'

    def _disable_button(self, button):

        button['state'] = 'disabled'

    def sendMotorMessage(self, index, percent):

        values = [PWM_MIN] * 4
        values[index - 1] += int(percent / 100. * (PWM_MAX - PWM_MIN))
        self.comms.send_message(serialize_SET_MOTOR, values)

    def _show_splash(self):

        self.splash = self.canvas.create_image((400, 250),
                                               image=self.splashimage)

    def _hide_splash(self):

        self.canvas.delete(self.splash)

    def _show_armed(self, widget):

        widget.configure(bg='red')

    def _show_disarmed(self, widget):

        widget.configure(bg=BACKGROUND_COLOR)

        if self._getting_messages():

            self._enable_button(self.button_motors)

    def _handle_calibrate_response(self):

        self.imu.showCalibrated()

    def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):

        # Only handle parms from firmware on a fresh connection
        if self.newconnect:

            self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)

        self.newconnect = False

    def _handle_attitude(self, x, y, z):

        self.roll_pitch_yaw = x / 10., -y / 10., z

        #self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)

        # As soon as we handle the callback from one request, send another request, if IMU dialog is running
        if self.imu.running:
            self._send_attitude_request()

    def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):

        self.rxchannels = c1, c2, c3, c4, c5

        # As soon as we handle the callback from one request, send another request, if receiver dialog is running
        if self.receiver.running:
            self._send_rc_request()

        #self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))

    def _handle_arm_status(self, armed):

        self.armed = armed

        #self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))

    def _handle_battery_status(self, volts, amps):

        return
class SerialThread(BaseThread):
    ser = serial.Serial()
    A = [.0]*3
    G = [.0]*3
    M = [.0]*3
    E_dmp = [.3]*3
    E = [.0]*3
    LA = [.0]*3

    ## Constructor
    #
    # Enables the basic thread functions and logs
    # @param self The object pointer
    def __init__(self, device='MPU-9150'):
        BaseThread.__init__(self)
        log.d("Init serialThread")
        # configure and retrieve IMU scales
        self.imu = IMU(device)
        self.csvScale = self.imu.getScaleVector()
        self.csvSize = len(self.csvScale)
        self.range_accel = self.imu.getRangeAccelerometer()
        self.range_gyro = self.imu.getRangeGyroscope()
        self.range_mag = self.imu.getRangeMagnetometer()
        self.range_euler = self.imu.getRangeEuler()
        # star sensor fusion
        self.fusion = SensorFusion()

    ## Open the defined point
    #
    # Configures the serial port, address and speed, and verifies
    # correct initialization and status
    # @param self The object pointer
    def openPort(self, port='/dev/ttyUSB0', baud=115200):
        try:
            # configure port
            self.ser.port = port
            self.ser.baudrate = baud
            self.ser.stopbits = serial.STOPBITS_ONE
            self.ser.bytesize = serial.EIGHTBITS
            self.ser.rtscts = 1
            self.ser.timeout = 0.5

            # open port
            if self.ser.isOpen():
                log.w("Port is already open, closing it")
                self.ser.close()
            self.ser.open()
            # clear input
            self.ser.flushInput()
            # verify that port is open
            if self.ser.isOpen():
                log.d("Port "+port+" opened at "+str(baud))
        except serial.SerialException as se:
            log.e("Unable to open port %s" % port)
        except:
            raise

    ## The thread process itself
    #
    # Defines the "loop" method of the thread
    # + Obtain self.data from the defined self.serial port
    # + Separate in CSV format
    # + Assuming the input from a MPU-9150, scales self.data
    # + Manage incorrect CSV format (by size)
    # + Manage incomplete self.data transfer
    # + Notify main thread (Qt4) with a signal
    # @param self The objetc pointer
    def run(self):
        while not self.exiting and self.ser.isOpen():
            try:
                # check if there is self.data avaliable
                if self.ser.inWaiting() > 0:
                    raw = self.ser.readline().strip()
                    log.v(raw)
                    try:
                        # convert received data to a float list
                        data = map(float, raw.split(','))
                        # if got correct data size, scale it
                        if len(data) == self.csvSize:
                            for i in range(0, self.csvSize):
                                data[i] *= self.csvScale[i]
                            
                            # accelerometer
                            for i in self.range_accel:
                                self.A[i-self.range_accel[0]] = data[i]
                            # gyroscope
                            for i in self.range_gyro:
                                self.G[i-self.range_gyro[0]] = data[i]
                            # magnetometer
                            for i in self.range_mag:
                                self.M[i-self.range_mag[0]] = data[i]
                            # euler if its avaliable
                            if self.E_dmp[0] != -1:
                                for i in self.range_euler:
                                    self.E_dmp[i-self.range_euler[0]] = data[i]

                            # do sensor fusion
                            self.fusion.update9DOF(self.G, self.A, self.M)
                            self.LA = self.fusion.getLinearAcceleration()
                            self.E = self.fusion.getYawPitchRoll(True)

                            # notify new data is ready
                            self.emit(QtCore.SIGNAL('newData()'))

                        else:
                            log.d('Incorrect data format')
                            self.ser.flushInput()
                            pass
                    except:
                        self.ser.flushInput()
                        log.v("Skipping incomplete transfer")
                        pass
            except:
                self.closePort()
                log.e("Thread Stopped by unknow error")
                return
        log.d("Thread Stopped Normally")
        return

    ## Closes the self.serial port and waits for the thread to stop
    #
    # @param self The objet pointer#
    def closePort(self):
        log.i("Waiting Thread to stop...")
        while self.isRunning():
            self.exiting = True
            self.exit()
        self.ser.close()
        log.d("Port closed")

    def getAcceleration(self):
        return self.A

    def getLinearAccel(self):
        return self.LA

    def getGyroscope(self):
        return self.G

    def getMagnetometer(self):
        return self.M

    def getEulerDMP(self):
        return self.E_dmp

    def getEuler(self):
        return self.E
Ejemplo n.º 13
0
	elif (l > 20):
		ugfx.backlight(70)
	else:
		ugfx.backlight(30)

# Finds all locally installed apps that have an external.py
def get_external_hook_paths():
	return ["%s/external" % app.folder_path for app in get_local_apps() if is_file("%s/external.py" % app.folder_path)]

def low_power():
	ugfx.backlight(0)
	ugfx.power_mode(ugfx.POWER_OFF)


ugfx.init()
imu=IMU()
neo = pyb.Neopix(pyb.Pin("PB13"))
neo.display(0x04040404)
ledg = pyb.LED(2)
ival = imu.get_acceleration()
if ival['y'] < 0:
	ugfx.orientation(0)
else:
	ugfx.orientation(180)


buttons.init()
if not onboard.is_splash_hidden():
	splashes = ["splash1.bmp"]
	for s in splashes:
		ugfx.display_image(0,0,s)
Ejemplo n.º 14
0
from imu import IMU

i = IMU('/dev/ttyACM4')

print("Connected to IMU")
print("Identifier %s" % i.get_identifier())
Ejemplo n.º 15
0
#!/usr/bin/env python

from imu import IMU
import sys

myImu = IMU()
chr = sys.stdin.read(1)

Ejemplo n.º 16
0
#!/usr/bin/python
from imu import IMU
from bmp085 import BMP085
import smbus
import math
import serial

port = serial.Serial("/dev/ttyAMA0", baudrate=115200, timeout=3.0)

if __name__ == "__main__":

    bus = smbus.SMBus(1)
    imu_controller = IMU(bus, 0x69, 0x53, 0x1E, 0x68, "IMU")

    counter = 0

    # The main loop of IMU
    while True:
        imu_data = imu_controller.read_all()
        # if not (counter%200):
        # barometer = BMP085(bus, 0x77, "-barometer")
        # pressure = barometer.read_pressure()
        # temperature = barometer.read_temperature()

        #        port.write(str(counter) + "," + str(imu_data) + "," + str(pressure) + "," + str(temperature) + "\r\n")
        port.write(str(counter) + "," + str(imu_data) + "\r\n")
        counter += 1
Ejemplo n.º 17
0
    def __init__(self):

        # No communications or arming yet
        self.comms = None
        self.armed = False

        # Do basic Tk initialization
        self.root = Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = Frame(self.root)

        self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
        self.button_imu  = self._add_button('IMU',  self.pane2, self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
        #self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
        #self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
        self.canvas.pack()

        # Add widgets for motor-testing dialog; hide them immediately
        self.motors = Motors(self)
        self.motors.stop()

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create messages dialog
        #self.messages = Messages(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a maps dialog
        #self.maps = Maps(self, yoffset=-30)

        # Create a splash image
        self.splashimage = PhotoImage(file='media/splash.gif')
        self._show_splash()

        # Create a message parser 
        self.parser = MSP_Parser()

        # Set up parser's request strings
        self.attitude_request = serialize_ATTITUDE_RADIANS_Request()
        self.rc_request = serialize_RC_NORMAL_Request()

        # No messages yet
        self.roll_pitch_yaw = 0,0,0
        self.rxchannels = 0,0,0,0,0,0,0,0

        # A hack to support display in IMU dialog
        self.active_axis = 0
Ejemplo n.º 18
0
from imu import IMU
import time

imu = IMU("/dev/ttyUSB0")
integration1_result = 0
integration2_result = 0

def get_depth():

    #fast integration
    last_time = time.time()
    acceleration = imu.get_accz()
    acc_diff_time = time.time() -last_time
    integration1 = acceleration*acc_diff_time
    global integration1_result += integration1
    last_time = time.time()
    #2nd integration

    vel_diff_time = acc_diff_time
    integration2 = integration1_result *acc_diff_time
    global integration2_result += integration2

    depth = integration2_result

    return depth

while True:
    print("Depth:" +str( get_depth()))

Ejemplo n.º 19
0
from pid import PID
from time import sleep

""" Limits the thrust passed to the motors
    in the range (-100,100)
"""
def limitThrust(thrust, upperLimit = 100, lowerLimit = 0):
    if thrust > upperLimit:
        thrust = upperLimit
    elif thrust < lowerLimit:
        thrust = lowerLimit
    return thrust

#instantiate IMU
#TODO see how to import C interface to python
imu=IMU() 
#MyKalman=KalmanFilter(....)

#instantiate motors and put them together in a list
motor1=Motor(1)
motor2=Motor(2)
motor3=Motor(3)
motor4=Motor(4)
motors=[motor1,motor2,motor3,motor4]

# instantiate PID controllers and init them
rollPID=PID(0.9, 0.2, 0.3) # Kp, Kd, Ki
pitchPID=PID(0.9, 0.2, 0.3)
yawPID=PID(0.06, 0.02, 0.01)
#zPID=PID(.....)
#xposPID=PID(.....)
Ejemplo n.º 20
0
# -*- coding: utf-8 -*-
"""
Created on Thu May 31 10:57:21 2018

@author: Bailey group PC
"""
from imu import IMU
import time
import pandas as pd
from datetime import datetime
import os

#Create serial connection to IMU
imu = IMU(com_port='COM7', baudrate=115200)  #921600

#Set recording parameters
hz = 20
delay = 1.0 / hz
record_time = 5
save_dir = 'C:/git_repos/GLO/Tutorials/tracking/'

#Initiate pandas dataframe to store IMU data in
data = pd.DataFrame(columns=[
    'accel_x', 'accel_y', 'accel_z', 'angr_x', 'angr_y', 'angr_z', 'mag_x',
    'mag_y', 'mag_z', 'elapsed'
])

t_start = time.time()
print('Recording VN100 IMU data for', record_time, 'seconds')
while (time.time() - t_start) < record_time:
Ejemplo n.º 21
0
                    justification=ugfx.Label.LEFT,
                    style=styVal,
                    parent=c)

c.show()

# Unused, broadcast and multicasts are blocked on the wifi
#s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#s.bind(('', 14612))
#s.setblocking(True)

#while True:
#data, addr = s.recvfrom(1024)
#curOut.text(data)

imu = IMU()

connectto = usocket.getaddrinfo('monitor1.emf.camp', 80)

oldorientation = ugfx.orientation()

looped = 0


def updateStats():
    # Download the JSON file
    s = usocket.socket()
    try:
        s.connect(connectto[0][4])

        s.send("GET /api/\r\n")
Ejemplo n.º 22
0
### author: Laurie James
### description: Goes in a bin. Plays r2d2 noises when the bin shuts. Useful to probably no one but me.
### license: MIT
### reboot-before-run: True
### Appname: R2D2-bin

from http_client import get
import pyb
from imu import IMU
import wifi

TILT_THRESHOLD = -0.4
TILT_PLAY = -0.1

imu = IMU()
host = 'http://192.168.0.12:8001'

if not wifi.nic().is_connected():
    wifi.connect(timeout=20)

triggered = False

while(True):
    y = imu.get_acceleration()['y']

    if(int(y) < TILT_THRESHOLD):
	triggered = True
    elif(y > TILT_PLAY and triggered): 
        try:
            print('foobar')
            get(host, timeout=10).raise_for_status()
Ejemplo n.º 23
0
from imu import IMU
import pyb

imu = IMU()

while True:
    print(imu.get_acceleration())
    pyb.delay(1000)
Ejemplo n.º 24
0
from gridmap import GridMap
from imu import IMU
from lidar import Lidar

if __name__ == "__main__":
    lidar_data = Lidar("lidar")
    imu_data = IMU("imu", "speed")
    map = GridMap()

    print(lidar_data)
    print(imu_data)
    print(map)
    # lidarData.show_all()
Ejemplo n.º 25
0
#!/usr/bin/env python

# dependencies
from flask import Flask, render_template, copy_current_request_context
from flask_socketio import SocketIO, emit
from flask_apscheduler import APScheduler

# local files
from imu import IMU

imu = IMU()  # unique instance of IMU


class Config(object):
    JOBS = [{
        'id': 'run',
        'func': imu.run,
        'args': (),
        'trigger': 'interval',
        'seconds': imu.thread_update_delay
    }]

    SCHEDULER_API_ENABLED = True
    SECRET_KEY = 'secret!'


# Set this variable to "threading", "eventlet" or "gevent" to test the
# different async modes, or leave it set to None for the application to choose
# the best option based on installed packages.
async_mode = None  # should probably always be gevent
Ejemplo n.º 26
0
def main():
    # read arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-p", "--port",
                    help="path for serial port")
    args = vars(ap.parse_args())

    # inits IMU
    port = args.get('port', None)
    if port is None:
        port = '/dev/ttyUSB_IMU'
    imu = IMU(port)

    # start subprocess
    imu.start()

    try:
        while True:
            print("yaw = ", imu.get_yaw())
            print("yaw2 = ", imu.get_yaw2())
            print("pitch = ", imu.get_pitch())
            print("roll = ", imu.get_roll())

            print("angx = ", imu.get_angx())
            print("angy = ", imu.get_angy())
            print("angz = ", imu.get_angz())

            print("accx = ", imu.get_accx())
            print("accy = ", imu.get_accy())
            print("accz = ", imu.get_accz())

            print("magx = ", imu.get_magx())
            print("magy = ", imu.get_magy())
            print("magz = ", imu.get_magz())

            print()
            time.sleep(1)
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping remaining threads...")
        imu.stop()
IMU positioned on top/center of PTU
Multiple runs are saved sending a step re

"""
from imu import IMU
from ptu_newmark import PTU
import numpy as np
import time
import pandas as pd
import matplotlib.pyplot as plt
import os
cwd = os.getcwd()

ptu = PTU(com_port='COM9', baudrate=9600)
time.sleep(2)
imu = IMU(com_port='COM5', baudrate=921600)
time.sleep(0.3)
#imu.change_baudrate(921600)
#imu.imu.disconnect()
#imu = IMU(com_port='COM7',baudrate=921600)
#time.sleep(0.5)

N = 1000
t0 = time.time()
data = {}
data['date'] = '20180710'
data['samples'] = N
cnt = 0
test_vel = [15, 100, 500, 2500, 15000, 80000]

for run in test_vel:
Ejemplo n.º 28
0
def main():
    # read arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-s", "--speed")
    args = vars(ap.parse_args())

    set_speed = args.get('speed', 0)
    if set_speed is None:
        set_speed = 0
    set_speed = float(set_speed)
    speed = 0

    # inits MCU
    mcu = MCU(2222)

    # inits IMU
    imu = IMU("/dev/ttyUSB_IMU")

    # start subprocess
    imu.start()
    mcu.start()

    pidR = pidRoll(1, 0.2, 0)  # 1, 0 , 0
    pidP = pidPitch(0.6, 0, 0)  # 5 ,0.1 ,8
    pidD = pidDepth(1, 0, 0)
    pidY = pidYaw(1, 0.4, 0)
    motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0

    try:
        motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
        counter = 0
        while True:
            counter += 1
            depth = mcu.get_depth()
            pinger = mcu.get_angle()
            pitch = imu.get_pitch()
            roll = imu.get_roll()
            yaw = imu.get_yaw2()

            pidR.getSetValues(roll)
            pidP.getSetValues(pitch)
            pidD.getSetValues(70 - depth)
            pidY.getSetValues(-yaw)
            finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
                                      pidY.start())

            sentValues = []
            for values in finalPidValues:
                subValues = values
                sentValues.append(subValues)

            motor_fl = sentValues[0]
            motor_fr = sentValues[1]
            motor_bl = sentValues[2] + set_speed
            motor_br = sentValues[3] + set_speed
            motor_t = sentValues[4]

            mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)

            if counter % 5 == 0:
                print('Depth:', depth)
                print('Pinger:', pinger)
                print('Pitch:', pitch)
                print('Roll:', roll)
                print('Yaw:', imu.get_yaw2())
                print('Yaw_sent:', yaw)
                print('Motors: %.2f %.2f %.2f %.2f %.2f' %
                      (motor_fl, motor_fr, motor_bl, motor_br, motor_t))
                print()
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping remaining threads...")
        imu.stop()
        mcu.stop()
Ejemplo n.º 29
0
    def __init__(self):

        # No communications or arming yet
        self.comms = None
        self.armed = False
        self.gotimu = False

        # Do basic Tk initialization
        self.root = tk.Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' %
                           (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = tk.Frame(self.root)

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w,
                              tk.PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1,
                                               self._connect_callback)
        self.button_imu = self._add_button('IMU', self.pane2,
                                           self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2,
                                              self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2,
                                                self._receiver_button_callback)
        #self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
        #self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = tk.StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = tk.Canvas(self.root,
                                width=DISPLAY_WIDTH,
                                height=DISPLAY_HEIGHT,
                                background='black')
        self.canvas.pack()

        # Set up a text label for reporting errors
        errmsg = 'No response from board.  Possible reasons:\n\n' + \
                 '    * You connected to the wrong port.\n\n' + \
                 '    * Firmware uses serial receiver\n' + \
                 '      (DSMX, SBUS), but receiver is\n' + \
                 '      not connected.'
        self.error_label = tk.Label(self.canvas,
                                    text=errmsg,
                                    bg='black',
                                    fg='red',
                                    font=(None, 24),
                                    justify=tk.LEFT)
        self.hide(self.error_label)

        # Add widgets for motor-testing dialog; hide them immediately
        self.motors = Motors(self)
        self.motors.stop()

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create messages dialog
        #self.messages = Messages(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a maps dialog
        #self.maps = Maps(self, yoffset=-30)

        # Create a splash image
        self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
        self._show_splash()

        # Create a message parser
        self.parser = msppg.MSP_Parser()

        # Set up parser's request strings
        self.attitude_request = msppg.serialize_ATTITUDE_RADIANS_Request()
        self.rc_request = msppg.serialize_RC_NORMAL_Request()

        # No messages yet
        self.roll_pitch_yaw = [0] * 3
        self.rxchannels = [0] * 6

        # A hack to support display in IMU dialog
        self.active_axis = 0
Ejemplo n.º 30
0
    def __init__(self):

        # No communications or arming yet
        self.comms = None
        self.armed = False

        # Do basic Tk initialization
        self.root = Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = Frame(self.root)

        self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
        self.button_imu  = self._add_button('IMU',  self.pane2, self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
        #self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
        #self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
        self.canvas.pack()

        # Add widgets for motor-testing dialog; hide them immediately
        self.motors = Motors(self)
        self.motors.stop()

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create messages dialog
        #self.messages = Messages(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a maps dialog
        #self.maps = Maps(self, yoffset=-30)

        # Create a splash image
        self.splashimage = PhotoImage(file='media/splash.gif')
        self._show_splash()

        # Create a message parser 
        self.parser = MSP_Parser()

        # Set up parser's request strings
        self.attitude_request = serialize_ATTITUDE_Request()
        self.rc_request = serialize_RC_Request()

        # No messages yet
        self.roll_pitch_yaw = 0,0,0
        self.rxchannels = 0,0,0,0,0

        # A hack to support display in IMU dialog
        self.active_axis = 0
Ejemplo n.º 31
0
    Ki = float(sys.argv[3])

else:
    Kp = 0.9
    Kd = 0.2
    Ki = 0

############################

#instantiate the BT controller
bt = BT_Controller(z_d)
bt.run()

#instantiate IMU
#TODO see how to import C interface to python
imu = IMU()
#MyKalman=KalmanFilter(....)

# dynamical model instance
dyn_model = Dynamical_Model()

#instantiate motors and put them together in a list
motor1 = Motor(1)
motor2 = Motor(2)
motor3 = Motor(3)
motor4 = Motor(4)
motors = [motor1, motor2, motor3, motor4]

# instantiate PID controllers
rollPID = PID(Kp, Kd, Ki)  # Kp, Kd, Ki
pitchPID = PID(Kp, Kd, Ki)
Ejemplo n.º 32
0
def main():
    # read arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-o", "--output")
    ap.add_argument("-s", "--speed")
    ap.add_argument("-t", "--time")  # time to turn after passing
    ap.add_argument("-ft",
                    "--forceturn")  # time to force turn after launching, 30
    ap.add_argument("-a", "--angle")
    ap.add_argument("-d", "--depth")  # 80
    args = vars(ap.parse_args())

    set_speed = float(args.get('speed', 0))
    set_depth = float(args.get('depth', 0))
    time_after_passing = float(args.get('time', 0))
    time_force_turn = float(args.get('forceturn', 0))
    angle_to_turn = float(args.get('angle', 0))

    # inits CV
    cv = CVManager(
        0,  # choose the first web camera as the source
        enable_imshow=True,  # enable image show windows
        server_port=3333,  # start stream server at port 3333
        delay=5,
        outputfolder=args.get("output"))
    cv.add_core("GateTracker", GateTrackerV3(), False)
    cv.add_core("Flare", Flare(), False)

    # inits MCU
    mcu = MCU(2222)

    # inits IMU
    imu = IMU("/dev/ttyUSB_IMU")

    # start subprocess
    cv.start()
    imu.start()
    mcu.start()

    imu.reset_yaw2(-angle_to_turn, 2)  # for state 3

    mcu.wait()

    cv.enable_core("GateTracker")

    pidR = pidRoll(1, 0.2, 0)  # 5, 0.1 , 5
    pidP = pidPitch(0.6, 0, 0)  # 5 ,0.1 ,8
    pidD = pidDepth(1, 0, 0)
    pidY = pidYaw(1, 0.3, 0)
    motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0

    try:
        motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
        counter = 0
        last_cv_gate = 0
        yaw = 0
        speed = 0
        state = 0
        timer_for_state0 = time.time()
        timer_for_state1 = 0
        timer_for_state2 = 0
        # 0 -> go find the gate
        # 1 -> continue after passing the gate
        # 2 -> find flare
        # 3 -> surfacing
        while True:
            counter += 1
            if state == 0:
                gate, _, gate_size = cv.get_result("GateTracker")
                depth = mcu.get_depth()
                pitch = imu.get_pitch()
                roll = imu.get_roll()
                speed = set_speed

                if gate is None:  # just go straight
                    yaw = imu.get_yaw2(0)  # original heading
                else:
                    if gate != last_cv_gate:
                        imu.reset_yaw2(-gate * 0.1, 1)
                        last_cv_gate = gate
                    else:
                        yaw = imu.get_yaw2(1)  # heading with CV

                    if gate_size > 350:
                        state = 1
                if time.time() - timer_for_state0 > time_force_turn:
                    state = 1
                print('Gate', gate)
                print('GateSize', gate_size)
            # go straight
            elif state == 1:
                if timer_for_state1 == 0:  # first time
                    cv.disable_core('GateTracker')
                    timer_for_state1 = time.time()
                elif time.time() - timer_for_state1 > time_after_passing:
                    state = 2
                    cv.enable_core('Flare')
                depth = mcu.get_depth()
                pitch = imu.get_pitch()
                roll = imu.get_roll()
                yaw = imu.get_yaw2(0)  # original heading
                speed = set_speed
            # go to the flare
            elif state == 2:
                gate, _, gate_size = cv.get_result("Flare")
                depth = mcu.get_depth()
                pitch = imu.get_pitch()
                roll = imu.get_roll()
                speed = set_speed
                print(gate, gate_size)

                if gate is None:  # just go straight
                    yaw = imu.get_yaw2(2)
                else:
                    if gate != last_cv_gate:
                        imu.reset_yaw2(-gate * 0.1, 1)
                        last_cv_gate = gate
                    else:
                        yaw = imu.get_yaw2(1)

                if not gate_size is None:
                    if gate_size > 200:
                        timer_for_state2 = time.time()
                if timer_for_state2 != 0 and time.time(
                ) - timer_for_state2 > 10:
                    state = 3
            # surfacing
            else:
                depth = 500
                pitch = imu.get_pitch()
                roll = imu.get_roll()
                yaw = 0
                speed = 0

            pidR.getSetValues(roll)
            pidP.getSetValues(pitch)
            pidD.getSetValues(set_depth - depth)
            pidY.getSetValues(-yaw)
            finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
                                      pidY.start())

            sentValues = []
            for values in finalPidValues:
                subValues = values
                sentValues.append(subValues)

            motor_fl = sentValues[0]
            motor_fr = sentValues[1]
            motor_bl = sentValues[2] + speed
            motor_br = sentValues[3] + speed
            motor_t = sentValues[4]

            mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)

            if counter % 20 == 0:
                print('State:', state)
                print('Depth:', depth)
                print('Pitch:', pitch)
                print('Roll:', roll)
                print('Yaw:', imu.get_yaw2())
                print('Yaw_sent:', yaw)
                print('Motors: %.2f %.2f %.2f %.2f %.2f' %
                      (motor_fl, motor_fr, motor_bl, motor_br, motor_t))
                print()
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping remaining threads...")
        cv.stop()
        imu.stop()
        mcu.stop()
Ejemplo n.º 33
0
class GCS:

    def __init__(self):

        # No communications or arming yet
        self.comms = None
        self.armed = False

        # Do basic Tk initialization
        self.root = Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = Frame(self.root)

        self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
        self.button_imu  = self._add_button('IMU',  self.pane2, self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
        #self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
        #self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
        self.canvas.pack()

        # Add widgets for motor-testing dialog; hide them immediately
        self.motors = Motors(self)
        self.motors.stop()

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create messages dialog
        #self.messages = Messages(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a maps dialog
        #self.maps = Maps(self, yoffset=-30)

        # Create a splash image
        self.splashimage = PhotoImage(file='media/splash.gif')
        self._show_splash()

        # Create a message parser 
        self.parser = MSP_Parser()

        # Set up parser's request strings
        self.attitude_request = serialize_ATTITUDE_RADIANS_Request()
        self.rc_request = serialize_RC_NORMAL_Request()

        # No messages yet
        self.roll_pitch_yaw = 0,0,0
        self.rxchannels = 0,0,0,0,0,0,0,0

        # A hack to support display in IMU dialog
        self.active_axis = 0

    def quit(self):
        self.motors.stop()
        self.root.destroy()

    def hide(self, widget):

        widget.place(x=-9999)

    def getChannels(self):

        return self.rxchannels

    def getRollPitchYaw(self):

        # configure button to show connected
        self._enable_buttons()
        self.button_connect['text'] = 'Disconnect'
        self._enable_button(self.button_connect)

        return self.roll_pitch_yaw

    def checkArmed(self):

        if self.armed:

            self._show_armed(self.root)
            self._show_armed(self.pane1)
            self._show_armed(self.pane2)

            self._disable_button(self.button_motors)

        else:

            self._show_disarmed(self.root)
            self._show_disarmed(self.pane1)
            self._show_disarmed(self.pane2)

    def scheduleTask(self, delay_msec, task):

        self.root.after(delay_msec, task)

    def _add_pane(self):

        pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
        pane.pack(fill=BOTH, expand=1)
        return pane

    def _add_button(self, label, parent, callback, disabled=True):

        button = Button(parent, text=label, command=callback)
        button.pack(side=LEFT)
        button.config(state = 'disabled' if disabled else 'normal')
        return button

    # Callback for IMU button
    def _imu_callback(self):

        self._clear()

        self.motors.stop()
        self.receiver.stop()
        #self.messages.stop()
        #self.maps.stop()

        self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
        self._send_attitude_request()
        self.imu.start()

    def _start(self):

        self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
        self._send_attitude_request()
        self.imu.start()

        self.parser.set_RC_NORMAL_Handler(self._handle_rc)

    # Sends Attitude request to FC
    def _send_attitude_request(self):

        self.comms.send_request(self.attitude_request)

    # Sends RC request to FC
    def _send_rc_request(self):

        self.comms.send_request(self.rc_request)

    # Callback for Motors button
    def _motors_button_callback(self):

        self._clear()

        self.imu.stop()
        self.receiver.stop()
        #self.messages.stop()
        #self.maps.stop()
        self.motors.start()

    def _clear(self):

        self.canvas.delete(ALL)

    # Callback for Receiver button
    def _receiver_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors.stop()
        #self.messages.stop()
        #self.maps.stop()

        self._send_rc_request()
        self.receiver.start()

    # Callback for Messages button
    def _messages_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors.stop()
        #self.maps.stop()
        self.receiver.stop()

        self.messages.start()

    def _getting_messages(self):

        return self.button_connect['text'] == 'Disconnect'

    # Callback for Maps button
    def _maps_button_callback(self):

        self._clear()

        if self._getting_messages():

            self.receiver.stop()
            self.messages.stop()
            self.imu.stop()
            self.motors.stop()

        #self.maps.start()

    # Callback for Connect / Disconnect button
    def _connect_callback(self):

        if self.connected:

            self.imu.stop()
            #self.maps.stop()
            self.motors.stop()
            #self.messages.stop()
            self.receiver.stop()

            if not self.comms is None:

                self.comms.stop()

            self._clear()

            self._disable_buttons()

            self.button_connect['text'] = 'Connect'

            self._show_splash()

        else:

            #self.maps.stop()

            self.comms = Comms(self)
            self.comms.start()

            self.button_connect['text'] = 'Connecting ...'
            self._disable_button(self.button_connect)

            self._hide_splash()

            self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)

        self.connected = not self.connected

    # Gets available ports
    def _getports(self):

        allports = comports()

        ports = []

        for port in allports:
            
            portname = port[0]

            if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
                ports.append(portname)

        return ports

    # Checks for changes in port status (hot-plugging USB cables)
    def _connection_task(self):

        ports = self._getports()

        if ports != self.ports:

            if self.portsmenu is None:

                self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)

            else:

                for port in ports:

                    self.portsmenu['menu'].add_command(label=port)

            self.portsmenu.pack(side=LEFT)

            if ports == []:

                self._disable_button(self.button_connect)
                self._disable_buttons()

            else:
                self.portsvar.set(ports[0]) # default value
                self._enable_button(self.button_connect)

            self.ports = ports

        self._schedule_connection_task()

    # Mutually recursive with above
    def _schedule_connection_task(self):

        self.root.after(USB_UPDATE_MSEC, self._connection_task)

    def _disable_buttons(self):

        self._disable_button(self.button_imu)
        self._disable_button(self.button_motors)
        self._disable_button(self.button_receiver)
        #self._disable_button(self.button_messages)

    def _enable_buttons(self):

        self._enable_button(self.button_imu)
        self._enable_button(self.button_motors)
        self._enable_button(self.button_receiver)
        #self._enable_button(self.button_messages)

    def _enable_button(self, button):

        button['state'] = 'normal'

    def _disable_button(self, button):

        button['state'] = 'disabled'

    def sendMotorMessage(self, index, percent):

        values = [0]*4
        values[index-1] = percent / 100.
        self.comms.send_message(serialize_SET_MOTOR_NORMAL, values)

    def _show_splash(self):

        self.splash = self.canvas.create_image((400,250), image=self.splashimage)

    def _hide_splash(self):

        self.canvas.delete(self.splash)


    def _show_armed(self, widget):

        widget.configure(bg='red')

    def _show_disarmed(self, widget):

        widget.configure(bg=BACKGROUND_COLOR)

        if self._getting_messages():

            self._enable_button(self.button_motors)

    def _handle_calibrate_response(self):

        self.imu.showCalibrated()

    def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):

        # Only handle parms from firmware on a fresh connection
        if self.newconnect:

            self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)

        self.newconnect = False

    def _handle_attitude(self, x, y, z):

        print(x, y, z)

        self.roll_pitch_yaw = x, -y, z  

        #self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)

        # As soon as we handle the callback from one request, send another request, if IMU dialog is running
        if self.imu.running:
            self._send_attitude_request()

    def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):

        # Display throttle as [0,1], other channels as [-1,+1]
        self.rxchannels = c1/2.+.5, c2, c3, c4, c5

        # As soon as we handle the callback from one request, send another request, if receiver dialog is running
        if self.receiver.running:
            self._send_rc_request()

        #self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))

    def _handle_arm_status(self, armed):

        self.armed = armed

        #self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))

    def _handle_battery_status(self, volts, amps):

        return
Ejemplo n.º 34
0
class GCS(MspParser):
    def __init__(self):

        MspParser.__init__(self)

        # No communications or arming yet
        self.comms = None
        self.armed = False
        self.gotimu = False

        # Do basic Tk initialization
        self.root = tk.Tk()
        self.root.configure(bg=BACKGROUND_COLOR)
        self.root.resizable(False, False)
        self.root.title('Hackflight Ground Control Station')
        left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
        top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
        self.root.geometry('%dx%d+%d+%d' %
                           (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
        self.frame = tk.Frame(self.root)

        # Too much hassle on Windows
        if 'nt' != os.name:
            self.root.tk.call('wm', 'iconphoto', self.root._w,
                              tk.PhotoImage('icon.xbm'))

        self.root.protocol('WM_DELETE_WINDOW', self.quit)

        # Create panes for two rows of widgets
        self.pane1 = self._add_pane()
        self.pane2 = self._add_pane()

        # Add a buttons
        self.button_connect = self._add_button('Connect', self.pane1,
                                               self._connect_callback)
        self.button_imu = self._add_button('IMU', self.pane2,
                                           self._imu_callback)
        self.button_motors = self._add_button('Motors', self.pane2,
                                              self._motors_button_callback)
        self.button_receiver = self._add_button('Receiver', self.pane2,
                                                self._receiver_button_callback)

        # Prepare for adding ports as they are detected by our timer task
        self.portsvar = tk.StringVar(self.root)
        self.portsmenu = None
        self.connected = False
        self.ports = []

        # Finalize Tk stuff
        self.frame.pack()
        self.canvas = tk.Canvas(self.root,
                                width=DISPLAY_WIDTH,
                                height=DISPLAY_HEIGHT,
                                background='black')
        self.canvas.pack()

        # Set up a text label for reporting errors
        errmsg = ('No response from board.  Possible reasons:\n\n' +
                  '    * You connected to the wrong port.\n\n' +
                  '    * IMU is not responding.\n\n')
        self.error_label = tk.Label(self.canvas,
                                    text=errmsg,
                                    bg='black',
                                    fg='red',
                                    font=(None, 24),
                                    justify=tk.LEFT)
        self.hide(self.error_label)

        # Add widgets for motor-testing dialogs; hide them immediately
        self.motors_quadxmw = self._add_motor_dialog(MotorsQuadXMW)
        self.motors_coaxial = self._add_motor_dialog(MotorsCoaxial)

        # Create receiver dialog
        self.receiver = Receiver(self)

        # Create IMU dialog
        self.imu = IMU(self)
        self._schedule_connection_task()

        # Create a splash image
        self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
        self._show_splash()

        # Set up parser's request strings
        self.attitude_request = MspParser.serialize_ATTITUDE_Request()
        self.rc_request = MspParser.serialize_RC_Request()

        # No messages yet
        self.roll_pitch_yaw = [0] * 3
        self.rxchannels = [0] * 6

    def quit(self):
        self.motors_quadxmw.stop()
        self.motors_coaxial.stop()
        self.root.destroy()

    def hide(self, widget):

        widget.place(x=-9999)

    def getChannels(self):

        return self.rxchannels

    def getRollPitchYaw(self):

        # configure button to show connected
        self._enable_buttons()
        self.button_connect['text'] = 'Disconnect'
        self._enable_button(self.button_connect)

        return self.roll_pitch_yaw

    def checkArmed(self):

        if self.armed:

            self._show_armed(self.root)
            self._show_armed(self.pane1)
            self._show_armed(self.pane2)

            self._disable_button(self.button_motors)

        else:

            self._show_disarmed(self.root)
            self._show_disarmed(self.pane1)
            self._show_disarmed(self.pane2)

    def scheduleTask(self, delay_msec, task):

        self.root.after(delay_msec, task)

    def handle_RC(self, c1, c2, c3, c4, c5, c6):
        def norm(x):

            MIN = 987
            MAX = 2011

            return (x - MIN) / (MAX - MIN)

        def scale(x):

            return 2 * norm(x) - 1

        # Scale throttle from [-1,+1] to [0,1]
        self.rxchannels = norm(c1), scale(c2), scale(c3), scale(c4), scale(
            c5), scale(c6)

        # As soon as we handle the callback from one request, send another
        # request, if receiver dialog is running
        if self.receiver.running:
            self._send_rc_request()

    def handle_ATTITUDE(self, angx, angy, heading):

        self.roll_pitch_yaw = rad(angx / 10), -rad(angy / 10), rad(heading)

        self.gotimu = True

        # As soon as we handle the callback from one request, send another
        # request, if receiver dialog is running
        if self.imu.running:
            self._send_attitude_request()

    def _add_pane(self):

        pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
        pane.pack(fill=tk.BOTH, expand=1)
        return pane

    def _add_button(self, label, parent, callback, disabled=True):

        button = tk.Button(parent, text=label, command=callback)
        button.pack(side=tk.LEFT)
        button.config(state=('disabled' if disabled else 'normal'))
        return button

    def _add_motor_dialog(self, dialog):
        d = dialog(self)
        d.stop()
        return d

    # unsigned int => signed float conversion
    def _float(self, intval):
        return intval / 1000 - 2

    # Callback for IMU button
    def _imu_callback(self):

        self._clear()

        self.motors_quadxmw.stop()
        self.motors_coaxial.stop()
        self.receiver.stop()
        self._send_attitude_request()
        self.imu.start()

    def _start(self):

        self._send_attitude_request()
        self.imu.start()

        self.gotimu = False
        self.hide(self.error_label)
        self.scheduleTask(CONNECTION_DELAY_MSEC, self._checkimu)

    def _checkimu(self):

        if not self.gotimu:

            self.imu.stop()
            self.error_label.place(x=50, y=50)
            self._disable_button(self.button_imu)
            self._disable_button(self.button_motors)
            self._disable_button(self.button_receiver)

    # Sends state request to FC
    def _send_attitude_request(self):

        self.comms.send_request(self.attitude_request)

    # Sends RC request to FC
    def _send_rc_request(self):

        self.comms.send_request(self.rc_request)

    # Callback for Motors button
    def _motors_button_callback(self):

        self._clear()

        # self.comms.send_request(self.actuator_type_request)

        self.imu.stop()
        self.receiver.stop()
        self.motors_quadxmw.start()

    def _clear(self):

        self.canvas.delete(tk.ALL)

    # Callback for Receiver button
    def _receiver_button_callback(self):

        self._clear()

        self.imu.stop()
        self.motors_quadxmw.stop()
        self.motors_coaxial.stop()
        self._send_rc_request()
        self.receiver.start()

    # Callback for Connect / Disconnect button
    def _connect_callback(self):

        if self.connected:

            self.imu.stop()
            self.motors_quadxmw.stop()
            self.motors_coaxial.stop()
            self.receiver.stop()

            if self.comms is not None:

                self.comms.stop()

            self._clear()

            self._disable_buttons()

            self.button_connect['text'] = 'Connect'

            self.hide(self.error_label)

            self._show_splash()

        else:

            self.comms = Comms(self)
            self.comms.start()

            self.button_connect['text'] = 'Connecting ...'
            self._disable_button(self.button_connect)

            self._hide_splash()

            self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)

        self.connected = not self.connected

    # Gets available ports
    def _getports(self):

        allports = comports()

        ports = []

        for port in allports:

            portname = port[0]

            if any(name in portname
                   for name in ('usbmodem', 'ttyACM', 'ttyUSB', 'COM')):
                if portname not in ('COM1', 'COM2'):
                    ports.append(portname)

        return ports

    # Checks for changes in port status (hot-plugging USB cables)
    def _connection_task(self):

        ports = self._getports()

        if ports != self.ports:

            if self.portsmenu is None:

                self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
                                               *ports)
            else:

                for port in ports:

                    self.portsmenu['menu'].add_command(label=port)

            self.portsmenu.pack(side=tk.LEFT)

            # Disconnected
            if ports == []:

                self.portsmenu['menu'].delete(0, 'end')
                self.portsvar.set('')
                self._disable_button(self.button_connect)
                self._disable_buttons()

            # Connected
            else:
                self.portsvar.set(ports[0])  # default value
                self._enable_button(self.button_connect)

            self.ports = ports

        self._schedule_connection_task()

    # Mutually recursive with above
    def _schedule_connection_task(self):

        self.root.after(USB_UPDATE_MSEC, self._connection_task)

    def _disable_buttons(self):

        self._disable_button(self.button_imu)
        self._disable_button(self.button_motors)
        self._disable_button(self.button_receiver)

    def _enable_buttons(self):

        self._enable_button(self.button_imu)
        self._enable_button(self.button_motors)
        self._enable_button(self.button_receiver)

    def _enable_button(self, button):

        button['state'] = 'normal'

    def _disable_button(self, button):

        button['state'] = 'disabled'

    def sendMotorMessage(self, motors):

        if self.comms is not None:  # Guard at startup
            self.comms.send_message(
                MspParser.serialize_SET_MOTOR,
                (motors[0], motors[1], motors[2], motors[3]))

    def _show_splash(self):

        self.splash = self.canvas.create_image(SPLASH_LOCATION,
                                               image=self.splashimage)

    def _hide_splash(self):

        self.canvas.delete(self.splash)

    def _show_armed(self, widget):

        widget.configure(bg='red')

    def _show_disarmed(self, widget):

        widget.configure(bg=BACKGROUND_COLOR)

    def _handle_calibrate_response(self):

        self.imu.showCalibrated()

    def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):

        # Only handle parms from firmware on a fresh connection
        if self.newconnect:

            self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)

        self.newconnect = False

    def _handle_arm_status(self, armed):

        self.armed = armed

    def _handle_battery_status(self, volts, amps):

        return
Ejemplo n.º 35
0
from imu import IMU
import pyb

imu = IMU()

while True:
    print(imu.get_acceleration())
    pyb.delay(1000);
Ejemplo n.º 36
0
class Bike():
    def __init__(self, log=False):
        if str(log) == "True":
            dir_path = os.getcwd() + "/"
            file_name = time.strftime("%Y.%m.%d.%H.%M.%S") + ".log"
            log = dir_path + file_name
        self.log = log

        self.mtr = BigBoy()
        self.imu = IMU()
        self.fps = FPS(1.0)

        self.t0 = time.time()
        self.steps = 5
        self.max_steps = 60
        self.pos = 0
        self.threshold = 1.5
        self.tilt_constant = -25.0

        # record parameters at the top of the log

    def balance(self):
        x, y, self.tilt = self.imu.get_acceleration()
        self.goal = int(self.tilt_constant * self.tilt)
        self.response = 'Goal: ' + str(self.goal)
        self.try_step(self.goal - self.pos)
        time.sleep(0.005)

    def balance_initial(self):
        t = time.time() - self.t0
        x, y, self.tilt = self.imu.get_acceleration()

        if tilt < -self.threshold:
            self.response = 'leaning left'
            self.try_step(self.steps)
        elif tilt > self.threshold:
            self.response = 'leaning right'
            self.try_step(-self.steps)
        elif self.pos > 0:
            self.response = 'recentering right'
            self.try_step(-self.steps)
        elif self.pos < 0:
            self.response = 'recentering left'
            self.try_step(self.steps)

        if self.log:
            data = [
                t,
                x,
                y,
                self.pos,
                self.tilt,
                self.response,
            ]
            with open(self.log, 'a') as f:
                f.write(",".join(data) + "\n")

    def try_step(self, steps):
        if abs(self.pos + steps) <= self.max_steps:
            self.pos += steps
            self.mtr.step(steps)

    def updateFPS(self):
        self.fps.update(verbose=True)

    def cleanup(self):
        self.mtr.cleanup()
Ejemplo n.º 37
0
from imu import IMU
import sys, time

wantsToQuit = False

if len(sys.argv) == 1:
    print("No devices defined")
    sys.exit(255)

while not wantsToQuit:
    i = IMU(sys.argv[1])
    print("Connected to IMU")

    print("Waiting for the IMU to wake up")
    #time.sleep(5)

    print("Calibrating the accelerometer")
    if (i.calibrate_accelerometer()):
        print(" - successful")

    print("Calibrating the magnometer")
    if (i.calibrate_magnometer()):
        print(" - successful")
    
    name = input("Enter a new name: ").rstrip()

    i.rename(name)

    print("The IMU is now %s" % i.get_identifier())

    i.close()
Ejemplo n.º 38
0
def loop():
    system('clear')
    IMU.printData()
    return
Ejemplo n.º 39
0
	elif (l > 20):
		ugfx.backlight(70)
	else:
		ugfx.backlight(30)

# Finds all locally installed apps that have an external.py
def get_external_hook_paths():
	return ["%s/external" % app.folder_path for app in get_local_apps() if is_file("%s/external.py" % app.folder_path)]

def low_power():
	ugfx.backlight(0)
	ugfx.power_mode(ugfx.POWER_OFF)


ugfx.init()
imu=IMU()
neo = pyb.Neopix(pyb.Pin("PB13"))
neo.display(0x04040404)
ledg = pyb.LED(2)
ival = imu.get_acceleration()
if ival['y'] < 0:
	ugfx.orientation(0)
else:
	ugfx.orientation(180)


buttons.init()
if not onboard.is_splash_hidden():
	splashes = ["splash1.bmp"]
	for s in splashes:
		ugfx.display_image(0,0,s)
Ejemplo n.º 40
0
import time

from imu import IMU
from pid import PID
from radio import Radio
from servos import Servos

# r = Radio('/dev/ttyUSB0')
# while True:
#   if r.bytes_available() > 16:
#       data = r.read()
#       print(data.decode())
# r.close()

if __name__ == '__main__':
    imu = IMU()
    servos = Servos()

    pid_roll = PID(600., 0., 0., control_range=[-250, 250])
    pid_pitch = PID(-600., 0., 0., control_range=[-250, 250])
    pid_yaw = PID(10., 0., 0)
    pids = [pid_roll, pid_pitch, pid_yaw]

    rpy = np.zeros(3)
    sp_rpy = np.zeros(3)
    cmd = np.array([1000, 1000, 1500, 1500])

    throttle = 1100

    tt = t = time.time()
    running = True