MultiWii

Code: Select all

//#define I2C_SPEED 400000L   //400kHz fast mode, it works only with some WMP clones

Code: Select all

 
   case 'K': //receive RC data from Bluetooth Serial adapter as a remote
         rcData[THROTTLE] = (Serial.read() * 4) + 1000;
         rcData[ROLL]     = (Serial.read() * 4) + 1000;
         rcData[PITCH]    = (Serial.read() * 4) + 1000;
         rcData[YAW]      = (Serial.read() * 4) + 1000;
         rcData[AUX1]     = (Serial.read() * 4) + 1000;
         Serial.write('K');
      break;

Code: Select all

#define FAILSAFE             // Alex: comment this line if you want to deactivate the failsafe function

Code: Select all

'K\xff\x00\x00\x00\x00'

Code: Select all

#ifdef BTSERIAL
    case 'K': //receive RC data from Bluetooth Serial adapter as a remote
      rcData[THROTTLE] = (SerialRead(0) * 4) + 1000;
      rcData[ROLL]     = (SerialRead(0) * 4) + 1000;
      rcData[PITCH]    = (SerialRead(0) * 4) + 1000;
      rcData[YAW]      = (SerialRead(0) * 4) + 1000;
      rcData[AUX1]     = (SerialRead(0) * 4) + 1000;
      rcData[AUX2]     = (SerialRead(0) * 4) + 1000;
      rcData[AUX3]     = (SerialRead(0) * 4) + 1000;
      rcData[AUX4]     = (SerialRead(0) * 4) + 1000;
      break;

Code: Select all

#!/usr/bin/env python

import time
from datetime import datetime
import serial

# Allow for multiple joysticks
joy = []

# usbport = '/dev/tty.usbserial-A70064Mh'
usbport = '/dev/tty.usbmodem621'

MIN_PPM = 0
MAX_PPM = 255

AXIS_ROLL = 0
AXIS_PITCH = 1
AXIS_THROTTLE = 3
AXIS_YAW = 2

BUTTON_AUX1 = 0 # Trigger
BUTTON_AUX2 = 1 # Button 2
BUTTON_AUX3 = 2 # Button 3
BUTTON_AUX4 = 3 # Button 4

class ControllerState(object):
    throttle = MIN_PPM
    roll     = MIN_PPM
    pitch    = MIN_PPM
    yaw      = MIN_PPM
    aux1     = MIN_PPM
    aux2     = MIN_PPM
    aux3     = MIN_PPM
    aux4     = MIN_PPM

    def __str__(self):
        return 'Throttle:{0.throttle} Roll:{0.roll} Pitch:{0.pitch} Yaw:{0.yaw} A1:{0.aux1} A2:{0.aux2} A3:{0.aux3} A4:{0.aux4}'.format(self)

    def serial_format(self):
        return 'K' + chr(self.throttle) + chr(self.roll) + chr(self.pitch) + chr(self.yaw) + chr(self.aux1) + \
            chr(self.aux2) + chr(self.aux3) + chr(self.aux4)

    # Joystick Movement
    def handleJoyEvent(self, e):
        # Identify joystick axes and assign events
        if e.type == pygame.JOYAXISMOTION:
            axis = e.dict['axis']
            # Convert -1.0 - +1.0 to 0 - 255
            value = int(e.dict['value'] * (MAX_PPM / 2)) + (MAX_PPM / 2)

            print 'Axis: {}, value: {}'.format(axis, value)

            if axis == AXIS_ROLL:
                self.roll = value
            elif axis == AXIS_PITCH:
                self.pitch = value
            elif axis == AXIS_THROTTLE:
                self.throttle = MAX_PPM - value
            elif axis == AXIS_YAW:
                self.yaw = value

        # Button Presses (toggle)
        elif e.type == pygame.JOYBUTTONDOWN:
            button = e.dict['button']

            print 'Button Down: {}'.format(button)

            if button == BUTTON_AUX1:
                self.aux1 = MAX_PPM if self.aux1 == MIN_PPM else MIN_PPM
            elif button == BUTTON_AUX2:
                self.aux2 = MAX_PPM if self.aux2 == MIN_PPM else MIN_PPM
            elif button == BUTTON_AUX3:
                self.aux3 = MAX_PPM if self.aux3 == MIN_PPM else MIN_PPM
            elif button == BUTTON_AUX4:
                self.aux4 = MAX_PPM if self.aux4 == MIN_PPM else MIN_PPM

        # # Assign actions for Coolie Hat Switch events
        # elif e.type == pygame.JOYHATMOTION:
        #     if (e.dict['value'][0] == -1):
        #         print "Hat Left"
        #         servo.move(4, 0)
        #     if (e.dict['value'][0] == 1):
        #         print "Hat Right"
        #         servo.move(4, 180)
        #     if (e.dict['value'][1] == -1):
        #         print "Hat Down"
        #     if (e.dict['value'][1] == 1):
        #         print "Hat Up"
        #     if (e.dict['value'][0] == 0 and e.dict['value'][1] == 0):
        #         print "Hat Centered"
        #         servo.move(4, 90)

        print self

# Main method
def main():
    with serial.Serial(usbport, 9600, timeout=1) as ser:
        frequency = 1. / 50
        controller_state = ControllerState()

        # Initialize pygame
        pygame.joystick.init()
        pygame.display.init()

        if not pygame.joystick.get_count():
            print "\nPlease connect a joystick and run again.\n"
            quit()
        print "\n%d joystick(s) detected." % pygame.joystick.get_count()
        for i in range(pygame.joystick.get_count()):
            myjoy = pygame.joystick.Joystick(i)
            myjoy.init()
            joy.append(myjoy)
            print "Joystick %d: " % (i) + joy[i].get_name()

        # Run joystick listener loop
        while True:
            poll_started = datetime.now()
            while True:
                e = pygame.event.poll()
                if e.type in (pygame.JOYAXISMOTION, pygame.JOYBUTTONDOWN, pygame.JOYBUTTONUP, pygame.JOYHATMOTION):
                    controller_state.handleJoyEvent(e)
                if e.type == pygame.NOEVENT:
                    break
            poll_ended = datetime.now()
            sleep_time = frequency - ((poll_ended - poll_started).microseconds / 1000000.)

            if sleep_time > 0.:
                time.sleep(sleep_time)
                # print controller_state
                ser.write(controller_state.serial_format())
            write_ended = datetime.now()

            # print 'Loop Time:', (write_ended - poll_started).microseconds

# Allow use as a module or standalone script
if __name__ == "__main__":
    main()

Code: Select all

Throttle:0 Roll:127 Pitch:127 Yaw:127 A1:0 A2:0 A3:0 A4:0
Axis: 3, value: 229
Throttle:26 Roll:127 Pitch:127 Yaw:127 A1:0 A2:0 A3:0 A4:0
Button Down: 0
Throttle:26 Roll:127 Pitch:127 Yaw:127 A1:255 A2:0 A3:0 A4:0
Button Down: 1
Throttle:26 Roll:127 Pitch:127 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 1, value: 127
Throttle:26 Roll:127 Pitch:127 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 1, value: 132
Throttle:26 Roll:127 Pitch:132 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 1, value: 137
Throttle:26 Roll:127 Pitch:137 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 0, value: 123
Throttle:26 Roll:123 Pitch:137 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 1, value: 145
Throttle:26 Roll:123 Pitch:145 Yaw:127 A1:255 A2:255 A3:0 A4:0
Throttle:26 Roll:123 Pitch:145 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 0, value: 118
Throttle:26 Roll:118 Pitch:145 Yaw:127 A1:255 A2:255 A3:0 A4:0
Axis: 1, value: 147