├── .gitignore ├── LICENSE ├── README.md ├── air ├── buzzer │ ├── buzzerTest.py │ ├── soundEnvelope.py │ ├── testSoundEnvelope.py │ └── wiringPiTest.py ├── droneController │ ├── AirMainLoop.py │ ├── Logger.py │ ├── MultiUART.py │ ├── air.ini │ ├── altitude.py │ ├── failsafe.py │ ├── hoverCalc.py │ ├── lightTelemetry.py │ ├── main.py │ ├── miniterm.py │ ├── mqtt.py │ ├── mqttMultiWii.py │ ├── pidTest.py │ ├── pyMultiwii.py │ ├── quiet.py │ ├── setRadio.py │ ├── sonar.py │ ├── soundEnvelope.py │ ├── spiTest.py │ ├── testUART.py │ └── windowfilter.py └── pyMultiWii │ ├── LICENSE │ ├── README.md │ ├── pyMultiwii.py │ ├── show-attitude.py │ ├── show-imu.py │ ├── test-arm-disarm.py │ ├── test-get.py │ ├── test-imu-plot.py │ ├── test-imu-plot2.py │ └── test-send.py └── ground └── droneController ├── GroundMainLoop.py ├── hd44780.py ├── joystick.py ├── js_linux.py ├── main copy.py ├── main.py ├── mainMQTT.py ├── miniterm.py ├── setRadio.py └── soundEnvelope.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | 49 | # Translations 50 | *.mo 51 | *.pot 52 | 53 | # Django stuff: 54 | *.log 55 | local_settings.py 56 | 57 | # Flask stuff: 58 | instance/ 59 | .webassets-cache 60 | 61 | # Scrapy stuff: 62 | .scrapy 63 | 64 | # Sphinx documentation 65 | docs/_build/ 66 | 67 | # PyBuilder 68 | target/ 69 | 70 | # Jupyter Notebook 71 | .ipynb_checkpoints 72 | 73 | # pyenv 74 | .python-version 75 | 76 | # celery beat schedule file 77 | celerybeat-schedule 78 | 79 | # SageMath parsed files 80 | *.sage.py 81 | 82 | # dotenv 83 | .env 84 | 85 | # virtualenv 86 | .venv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | .spyproject 93 | 94 | # Rope project settings 95 | .ropeproject 96 | 97 | # mkdocs documentation 98 | /site 99 | 100 | # mypy 101 | .mypy_cache/ 102 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Tony Butterfield 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rpi-python-drone 2 | a raspberry pi based drone using Naze32 flight controller 3 | -------------------------------------------------------------------------------- /air/buzzer/buzzerTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #--------------------------------------------------- 3 | # 4 | # This is a program for Passive Buzzer Module 5 | # It will play simple songs. 6 | # You could try to make songs by youselves! 7 | # 8 | # Passive buzzer Pi 9 | # VCC ----------------- 3.3V 10 | # GND ------------------ GND 11 | # SIG ---------------- Pin 11 12 | # 13 | #--------------------------------------------------- 14 | 15 | import RPi.GPIO as GPIO 16 | import time 17 | 18 | Buzzer = 12 19 | 20 | CL = [0, 131, 147, 165, 175, 196, 211, 248] # Frequency of Low C notes 21 | 22 | CM = [0, 262, 294, 330, 350, 393, 441, 495] # Frequency of Middle C notes 23 | 24 | CH = [0, 525, 589, 661, 700, 786, 882, 990] # Frequency of High C notes 25 | 26 | song_1 = [ CM[3], CM[5], CM[6], CM[3], CM[2], CM[3], CM[5], CM[6], # Notes of song1 27 | CH[1], CM[6], CM[5], CM[1], CM[3], CM[2], CM[2], CM[3], 28 | CM[5], CM[2], CM[3], CM[3], CL[6], CL[6], CL[6], CM[1], 29 | CM[2], CM[3], CM[2], CL[7], CL[6], CM[1], CL[5] ] 30 | 31 | beat_1 = [ 1, 1, 3, 1, 1, 3, 1, 1, # Beats of song 1, 1 means 1/8 beats 32 | 1, 1, 1, 1, 1, 1, 3, 1, 33 | 1, 3, 1, 1, 1, 1, 1, 1, 34 | 1, 2, 1, 1, 1, 1, 1, 1, 35 | 1, 1, 3 ] 36 | 37 | song_2 = [ CM[1], CM[1], CM[1], CL[5], CM[3], CM[3], CM[3], CM[1], # Notes of song2 38 | CM[1], CM[3], CM[5], CM[5], CM[4], CM[3], CM[2], CM[2], 39 | CM[3], CM[4], CM[4], CM[3], CM[2], CM[3], CM[1], CM[1], 40 | CM[3], CM[2], CL[5], CL[7], CM[2], CM[1] ] 41 | 42 | beat_2 = [ 1, 1, 2, 2, 1, 1, 2, 2, # Beats of song 2, 1 means 1/8 beats 43 | 1, 1, 2, 2, 1, 1, 3, 1, 44 | 1, 2, 2, 1, 1, 2, 2, 1, 45 | 1, 2, 2, 1, 1, 3 ] 46 | 47 | def setup(): 48 | GPIO.setmode(GPIO.BOARD) # Numbers GPIOs by physical location 49 | GPIO.setup(Buzzer, GPIO.OUT) # Set pins' mode is output 50 | global Buzz # Assign a global variable to replace GPIO.PWM 51 | Buzz = GPIO.PWM(Buzzer, 440) # 440 is initial frequency. 52 | Buzz.start(50) # Start Buzzer pin with 50% duty ration 53 | 54 | def loop(): 55 | while True: 56 | print '\n Playing song 1...' 57 | for i in range(1, len(song_1)): # Play song 1 58 | Buzz.ChangeFrequency(2*song_1[i]) # Change the frequency along the song note 59 | time.sleep(beat_1[i] * 0.5) # delay a note for beat * 0.5s 60 | time.sleep(1) # Wait a second for next song. 61 | 62 | print '\n\n Playing song 2...' 63 | for i in range(1, len(song_2)): # Play song 1 64 | Buzz.ChangeFrequency(song_2[i]) # Change the frequency along the song note 65 | time.sleep(beat_2[i] * 0.5) # delay a note for beat * 0.5s 66 | 67 | def destory(): 68 | Buzz.stop() # Stop the buzzer 69 | GPIO.output(Buzzer, 1) # Set Buzzer pin to High 70 | GPIO.cleanup() # Release resource 71 | 72 | if __name__ == '__main__': # Program start from here 73 | setup() 74 | try: 75 | loop() 76 | except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the child program destroy() will be executed. 77 | destory() -------------------------------------------------------------------------------- /air/buzzer/soundEnvelope.py: -------------------------------------------------------------------------------- 1 | import wiringpi 2 | import time 3 | from threading import Thread 4 | 5 | twelthRootOf2=2**(1.0/12) 6 | 7 | class SoundEnvelope: 8 | 9 | thread=None; 10 | threadInterrupted=False 11 | 12 | def __init__(self,clock=76, pin=1): 13 | self.clock=clock 14 | self.pin=pin 15 | 16 | wiringpi.wiringPiSetup() 17 | wiringpi.pinMode(self.pin,wiringpi.OUTPUT) 18 | wiringpi.pinMode(self.pin,wiringpi.PWM_OUTPUT) 19 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_BAL) 20 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_MS) 21 | wiringpi.pwmSetClock(clock) 22 | self.wiringpi=wiringpi 23 | 24 | def getRangeForNote2(self,note): 25 | freq=65.41*(twelthRootOf2**note) 26 | rg=int(19.2e6/self.clock/freq) 27 | return rg 28 | 29 | def getRangeForNote(self, note, subnote): 30 | scaleVal=scale[int(note%12)] 31 | if subnote!=0: 32 | scaleVal2=scale[1+int(note%12)] 33 | scaleVal=scaleVal2*subnote+scaleVal*(1-subnote) 34 | 35 | octave=1+int(note/12) 36 | freq=65.41*(2**octave)*scaleVal 37 | rg=int(19.2e6/clock/freq) 38 | #print note,freq,range, octave, int(note%12) 39 | return rg 40 | 41 | def setSound(self,note,volume): 42 | rg=self.getRangeForNote2(note) 43 | mr=int(rg*0.5*volume) 44 | self.wiringpi.pwmSetRange(rg) 45 | self.wiringpi.pwmWrite(self.pin,mr) 46 | 47 | def playRun(self,list,pitchOffset,speed,legato): 48 | env=None 49 | for sound in list: 50 | if len(sound)==3: 51 | env=sound[2] 52 | duration=int(sound[1]/speed) 53 | pitch=sound[0] 54 | if pitch==None: 55 | for i in range(0,duration): 56 | time.sleep(0.01) 57 | if self.threadInterrupted: 58 | break 59 | else: 60 | env.start((pitch+pitchOffset)*4,duration*legato) 61 | for i in range(0,duration): 62 | env.process(self) 63 | time.sleep(0.01) 64 | if self.threadInterrupted: 65 | break 66 | if self.threadInterrupted: 67 | break 68 | self.setSound(0,0) 69 | 70 | def play(self,list,pitchOffset=0,speed=1,legato=0.8): 71 | if self.thread!=None: 72 | self.threadInterrupted=True 73 | self.thread.join() 74 | self.threadInterrupted=False 75 | 76 | thread = Thread(target=self.playRun, args=(list,pitchOffset,speed,legato,)) 77 | thread.start() 78 | self.thread=thread 79 | 80 | class Envelope: 81 | 82 | def __init__(self,s,pi1,pi2,pi3,pn1,pn2,pn3,aa,ad,asus,ar,ala,ald): 83 | self.s=s 84 | self.pi=(pi1,pi2,pi3) 85 | self.pn=(pn1,pn2,pn3) 86 | self.aa=aa 87 | self.ad=ad 88 | self.asus=asus 89 | self.ar=ar 90 | self.ala=ala 91 | self.ald=ald 92 | 93 | def start(self,pitch,duration): 94 | self.pitch=pitch 95 | self.volume=0.0 96 | self.div=0 97 | self.phase=-1 98 | self.phaseStepCount=0 99 | self.ampPhase=0 100 | self.duration=duration 101 | 102 | def process(self,soundEnvelope): 103 | 104 | self.div=(self.div+1)%self.s 105 | if self.div==0: 106 | while self.phaseStepCount==0: 107 | self.phase=(self.phase+1)%3 108 | self.phaseStepCount=self.pn[self.phase] 109 | self.pitch+=self.pi[self.phase] 110 | self.phaseStepCount=self.phaseStepCount-1 111 | 112 | 113 | if self.pitch>255: 114 | self.pitch=self.pitch-256 115 | elif self.pitch<0: 116 | self.pitch=self.pitch+256 117 | 118 | if self.duration==0: 119 | self.ampPhase=3 120 | else: 121 | self.duration=self.duration-1 122 | 123 | if self.ampPhase==0: #attach 124 | self.volume+=self.aa 125 | if self.volume>=self.ala: 126 | self.ampPhase=1 127 | elif self.ampPhase==1: #decay 128 | self.volume+=self.ad 129 | if self.volume<=self.ald: 130 | self.ampPhase=2 131 | elif self.ampPhase==2: #sustain 132 | self.volume+=self.asus 133 | if self.volume<=0: 134 | self.ampPhase=4 135 | elif self.ampPhase==3: 136 | self.volume+=self.ar 137 | if self.volume<=0: 138 | self.ampPhase=4 139 | 140 | #print self.ampPhase, self.volume 141 | 142 | 143 | if self.volume<=0: 144 | soundEnvelope.setSound(12+self.pitch/4.0,0) 145 | return True 146 | else: 147 | soundEnvelope.setSound(12+self.pitch/4.0,self.volume/128.0) 148 | return False -------------------------------------------------------------------------------- /air/buzzer/testSoundEnvelope.py: -------------------------------------------------------------------------------- 1 | from soundEnvelope import SoundEnvelope, Envelope 2 | import time 3 | 4 | 5 | def test1(): 6 | print "test1()" 7 | for i in range(0,2): 8 | e3=Envelope(2,1,-1,0,4,4,0,20,-1,0,-1,126,110) 9 | se.play([[24,200,e3]]) 10 | time.sleep(1) 11 | 12 | e3=Envelope(4,48,-48,0,1,1,0,127,0,0,-127,127,8) 13 | se.play([[0,25,e3],[2,25],[4,25],[5,25],[7,25],[9,25],[11,25],[12,50]],pitchOffset=24,speed=2,legato=1.0) 14 | 15 | time.sleep(2) 16 | 17 | def test2(): 18 | print "test2()" 19 | #random 20 | e3=Envelope(4,121,17,171,3,1,4,127,0,0,-127,127,8) 21 | se.play([[0,200,e3]],legato=1) 22 | time.sleep(2.5) 23 | 24 | #updown1 25 | e3=Envelope(4,48,-48,0,5,5,100,127,0,0,-127,127,8) 26 | se.play([[0,10*4,e3]],legato=1) 27 | time.sleep(2) 28 | 29 | #updown2 30 | e3=Envelope(2,24,-24,0,10,10,100,127,0,0,-127,127,8) 31 | se.play([[0,20*2,e3]],legato=1) 32 | time.sleep(2) 33 | 34 | #down 35 | e3=Envelope(8,-48,0,0,5,16,16,127,0,0,-127,127,8) 36 | se.play([[12*5,8*6,e3]]) 37 | 38 | def test3(): 39 | print "test3()" 40 | #no 41 | e3=Envelope(16,0,0,0,1,1,0,127,0,0,-127,127,8) 42 | se.play([[12,32,e3],[0,64]],legato=0.8,speed=1.5,pitchOffset=0) 43 | time.sleep(2) 44 | #yes 45 | se.play([[0,16,e3],[4,16],[7,16],[12,48]],legato=0.9,speed=1.5,pitchOffset=24) 46 | time.sleep(2) 47 | 48 | #beep-beep 49 | se.play([[0,16,e3],[None,16],[0,16]],legato=0.9,speed=1.5,pitchOffset=24) 50 | time.sleep(2) 51 | se.play([[0,16,e3],[None,16],[0,16],[None,16],[0,16]],legato=0.9,speed=1.5,pitchOffset=24) 52 | time.sleep(2) 53 | 54 | def test4(): 55 | print "test4()" 56 | #slow siren 57 | e3=Envelope(1,1,-1,0,48,48,0,127,0,0,-127,127,8) 58 | se.play([[36,256,e3]]) 59 | time.sleep(2) 60 | 61 | #fast siren 62 | e3=Envelope(1,2,-2,0,12,12,0,127,0,0,-127,127,8) 63 | se.play([[48,128,e3]]) 64 | 65 | def test5(): 66 | print "test5()" 67 | #bell 68 | e3=Envelope(1,-48,48,0,2,2,255,127,-3,0,-127,127,0) 69 | e4=Envelope(4,1,-1,0,2,2,0,127,-3,0,-127,127,0) 70 | se.play([[60,128,e3],[54,128],[None,64],[60,128,e4]]) 71 | time.sleep(2) 72 | 73 | def test6(): 74 | print "test6()" 75 | #click 76 | e3=Envelope(1,0,0,0,12,12,0,127,-127,0,-127,127,8) 77 | se.play([[48,4,e3]]) 78 | 79 | def test7(): 80 | print "test7()" 81 | brass=Envelope(8,0,0,0,2,2,4,4,-1,0,-16,127,126) 82 | fill=Envelope(4,48,-48,0,1,1,0,127,0,0,-127,127,8) 83 | 84 | se.play([ 85 | [24,32,brass],[22,8],[None,8],[21,8],[None,8], 86 | [24,32],[22,8],[None,8],[21,8],[None,8], 87 | [24,8],[None,8],[19,16],[7,8,fill],[None,8],[12,8],[None,8], 88 | [16,24],[None,8],[12,24],[None,8], 89 | [26,16,brass],[24,8],[None,8],[26,8],[None,8],[28,32], 90 | [26,8],[None,8],[24,8],[None,8],[22,8],[None,8], 91 | [17,24,fill],[None,8],[16,24],[None,8], 92 | [14,24],[None,8],[12,24],[None,8], 93 | 94 | [24,32,brass],[22,8],[None,8],[21,8],[None,8], 95 | [24,32],[22,8],[None,8],[21,8],[None,8], 96 | [24,8],[None,8],[19,16],[7,8,fill],[None,8],[12,8],[None,8], 97 | [16,24],[None,8],[12,24],[None,8], 98 | [26,16,brass],[24,8],[None,8],[26,8],[None,8],[28,24], 99 | 100 | [None,8],[26,32],[28,8],[None,8], 101 | [29,8],[None,24], 102 | [24,16,fill],[26,8],[28,8],[29,16] 103 | 104 | ],legato=0.8,speed=0.75,pitchOffset=12) 105 | 106 | se=SoundEnvelope() 107 | test1() 108 | test2() 109 | test3() 110 | test4() 111 | test5() 112 | test6() 113 | test7() -------------------------------------------------------------------------------- /air/buzzer/wiringPiTest.py: -------------------------------------------------------------------------------- 1 | # Pulsates an LED connected to GPIO pin 1 with a suitable resistor 4 times using softPwm 2 | # softPwm uses a fixed frequency 3 | import wiringpi 4 | import time 5 | 6 | 7 | PIN_TO_PWM = 1 8 | clock=76 9 | 10 | wiringpi.wiringPiSetup() 11 | 12 | wiringpi.pinMode(PIN_TO_PWM,wiringpi.PWM_OUTPUT) 13 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_MS) 14 | wiringpi.pwmSetClock(clock) 15 | #wiringpi.pwmSetRange(1024) 16 | #wiringpi.pwmWrite(PIN_TO_PWM,512) 17 | 18 | 19 | twelthRootOf2=2**(1.0/12) 20 | 21 | 22 | scale=[1,1.05946,1.12246,1.18921,1.25992,1.33483,1.41421,1.49831,1.58740,1.68179,1.78180,1.88775,2] 23 | 24 | def getRangeForNote2(note): 25 | freq=65.41*(twelthRootOf2**note) 26 | rg=int(19.2e6/clock/freq) 27 | #print note,freq,rg, int(note%12) 28 | return rg 29 | 30 | def getRangeForNote(note, subnote): 31 | global clock 32 | scaleVal=scale[int(note%12)] 33 | if subnote!=0: 34 | scaleVal2=scale[1+int(note%12)] 35 | scaleVal=scaleVal2*subnote+scaleVal*(1-subnote) 36 | 37 | octave=1+int(note/12) 38 | freq=65.41*(2**octave)*scaleVal 39 | rg=int(19.2e6/clock/freq) 40 | #print note,freq,range, octave, int(note%12) 41 | return rg 42 | 43 | def setSound(note,volume): 44 | rg=getRangeForNote2(note) 45 | mr=int(rg*0.5*volume) 46 | wiringpi.pwmSetRange(rg) 47 | wiringpi.pwmWrite(PIN_TO_PWM,mr) 48 | #wiringpi.pwmSetRange(2048) 49 | #wiringpi.pwmWrite(PIN_TO_PWM,1024) 50 | #print rg,mr 51 | 52 | class Envelope: 53 | 54 | def __init__(self,s,pi1,pi2,pi3,pn1,pn2,pn3,aa,ad,asus,ar,ala,ald): 55 | self.s=s 56 | self.pi=(pi1,pi2,pi3) 57 | self.pn=(pn1,pn2,pn3) 58 | self.aa=aa 59 | self.ad=ad 60 | self.asus=asus 61 | self.ar=ar 62 | self.ala=ala 63 | self.ald=ald 64 | 65 | def start(self,pitch,duration): 66 | self.pitch=pitch 67 | self.volume=0.0 68 | self.div=0 69 | self.phase=-1 70 | self.phaseStepCount=0 71 | self.ampPhase=0 72 | self.duration=duration 73 | 74 | def process(self): 75 | 76 | self.div=(self.div+1)%self.s 77 | if self.div==0: 78 | while self.phaseStepCount==0: 79 | self.phase=(self.phase+1)%3 80 | self.phaseStepCount=self.pn[self.phase] 81 | self.pitch+=self.pi[self.phase] 82 | self.phaseStepCount=self.phaseStepCount-1 83 | 84 | 85 | if self.pitch>255: 86 | self.pitch=self.pitch-256 87 | elif self.pitch<0: 88 | self.pitch=self.pitch+256 89 | 90 | if self.duration==0: 91 | self.ampPhase=3 92 | else: 93 | self.duration=self.duration-1 94 | 95 | if self.ampPhase==0: #attach 96 | self.volume+=self.aa 97 | if self.volume>=self.ala: 98 | self.ampPhase=1 99 | elif self.ampPhase==1: #decay 100 | self.volume+=self.ad 101 | if self.volume<=self.ald: 102 | self.ampPhase=2 103 | elif self.ampPhase==2: #sustain 104 | self.volume+=self.asus 105 | if self.volume<=0: 106 | self.ampPhase=4 107 | elif self.ampPhase==3: 108 | self.volume+=self.ar 109 | if self.volume<=0: 110 | self.ampPhase=4 111 | 112 | #print self.ampPhase, self.volume 113 | 114 | 115 | if self.volume<=0: 116 | setSound(12+self.pitch/4.0,0) 117 | return True 118 | else: 119 | setSound(12+self.pitch/4.0,self.volume/128.0) 120 | return False 121 | 122 | def play(self): 123 | while True: 124 | if self.process(): 125 | break 126 | time.sleep(0.01) 127 | 128 | #e=Envelope(1,70,16,2,2,0,0,126,0,0,-126,126,126) 129 | #e=Envelope(4,-1,1,0,2,2,0,126,0,0,-126,126,126) 130 | e=Envelope(2,1,-1,0,4,4,0,20,-1,0,-1,126,110) 131 | e2=Envelope(4,48,-48,0,2,2,0,127,-4,0,-1,127,0) 132 | #e=Envelope(1,-12,0,0,12,12,0,127,-2,0,-1,127,0) 133 | #e=Envelope(1,0,0,0,3,3,2,32,-2,0,-16,127,8) 134 | e=Envelope(1,0,0,0,3,3,2,127,0,0,-127,127,8) 135 | e2=Envelope(2,-1,-1,-1,2,2,2,127,0,0,-1,127,0) 136 | e3=Envelope(4,48,-48,0,2,2,0,127,-4,0,-1,127,0) 137 | 138 | def play(list,pitchOffset=0,speed=1,legato=0.8): 139 | for sound in list: 140 | print sound 141 | env=sound[0] 142 | duration=int(sound[2]/speed) 143 | env.start((sound[1]+pitchOffset)*4,duration*legato) 144 | for i in range(0,duration): 145 | env.process() 146 | time.sleep(0.01) 147 | setSound(0,0) 148 | 149 | 150 | play([[e3,12,25],[e3,16,25],[e3,19,50]],pitchOffset=12,speed=1,legato=0.5) 151 | 152 | #e=Envelope(1,4,-4,0,12,12,0,2,0,0,-127,127,0) 153 | #play([[e,120,200]]) 154 | 155 | ''' 156 | e.start(0,200) 157 | e.play() 158 | e.start(48,200) 159 | e.play() 160 | e.start(96,200) 161 | e.play() 162 | e.start(96+48,200) 163 | e.play() 164 | e.start(96+96,200) 165 | e.play() 166 | e.start(96+96+48,200) 167 | e.play() 168 | ''' 169 | 170 | ''' 171 | time.sleep(0.1) 172 | e.start(48,100) 173 | e.play() 174 | time.sleep(0.1) 175 | e.start(96,100) 176 | e.play() 177 | time.sleep(0.1) 178 | e.start(0,100) 179 | e.play() 180 | ''' 181 | ''' 182 | volume=1.0 183 | for i in range(0,20): 184 | setSound(32+i/4.0,volume) 185 | #volume=volume*0.5 186 | if volume<0.01: 187 | volume=1.0 188 | time.sleep(0.1) 189 | ''' 190 | 191 | 192 | 193 | #wiringpi.pinMode(PIN_TO_PWM,wiringpi.INPUT) 194 | 195 | -------------------------------------------------------------------------------- /air/droneController/AirMainLoop.py: -------------------------------------------------------------------------------- 1 | from threading import Thread 2 | import sys, time, math, traceback, serial, os 3 | from soundEnvelope import SoundEnvelope, Envelope 4 | sys.path.append('/home/pi/dev/mavlink/') 5 | from pymavlink import mavutil 6 | from pyMultiwii import MultiWii 7 | import pymavlink.dialects.v10.ardupilotmega as mavlink 8 | from Logger import Logger 9 | from sonar import Sonar 10 | import configparser 11 | from altitude import Altitude 12 | from windowfilter import WindowFilter 13 | 14 | flatTone=Envelope(16,0,0,0,1,1,0,127,0,0,-127,127,8) 15 | bellTone=Envelope(1,-48,48,0,2,2,255,127,-3,0,-127,127,0) 16 | errorTone=Envelope(8,48,0,0,5,16,16,127,0,0,-127,127,8) 17 | warningTone=Envelope(1,2,-2,0,12,12,0,127,0,0,-127,127,8) 18 | sirenTone=Envelope(1,2,-2,0,12,12,0,127,0,0,-127,127,8) 19 | 20 | def constrain(v,min,max): 21 | if vmax: 24 | r=max 25 | else: 26 | r=v 27 | return r 28 | 29 | 30 | 31 | 32 | class AirMainLoop: 33 | 34 | lastHeartbeatSent=0 35 | threadInterrupted=False 36 | lastRemoteHeartbeat=time.time() 37 | localSNR=0 38 | remoteSNR=0 39 | remoteConnection=False 40 | rcDirty=False 41 | lastRCSend=0 42 | board=None 43 | inhibitMultiWii=False 44 | homeAltitude=0 45 | onGround=False 46 | onGroundCountDown=5 47 | theta=1.0 # ratio of thrust pointing upward 48 | 49 | throttleIn=1000 50 | throttle=1000 51 | yaw=1500 52 | roll=1500 53 | pitch=1500 54 | aux1=1000 55 | pidLookup= ["Roll P", "Roll D", "Roll I", 56 | "Pitch P", "Pitch D", "Pitch I", 57 | "Yaw P", "Yaw D", "Yaw I", 58 | "Alt P", "Alt D", "Alt I", 59 | "Pos P", "Pos D", "Pos I", 60 | "PosR P", "PosR D", "PosR I", 61 | "NavR P", "NavR D", "NavR I", 62 | "Level Angle", "Lev Horizon", "Tran Horizon", 63 | "Mag P", "---", "---", 64 | "Vel P", "Vel D", "Vel I", 65 | "AltitudeA","AltitudeP","AltitudeD","AltitudeI","Alt Throttle" 66 | ] 67 | 68 | altHoldA=0 69 | altHoldP=1.0 70 | altHoldD=0 71 | altHoldI=0 72 | 73 | def __init__(self,radio,multiwii): 74 | os.chdir(sys.path[0]) 75 | self.se=SoundEnvelope() 76 | try: 77 | self.loadInitialisation() 78 | 79 | self.mavconnection = mavutil.mavlink_connection(radio, baud="57600", autoreconnect = True ) 80 | self.mav=self.mavconnection.mav 81 | self.board = MultiWii(multiwii) 82 | #self.board.PRINT=0 83 | #print "init logger" 84 | self.logger = Logger() 85 | self.sonar = Sonar() 86 | self.altitude = Altitude(self) 87 | #self.logger.logTelemetry(self,True) 88 | except Exception, error: 89 | self.se.play([[12,32,flatTone],[0,64]],legato=0.8,speed=1.5,pitchOffset=0) 90 | raise error 91 | 92 | def loadInitialisation(self): 93 | self.config = configparser.ConfigParser() 94 | self.config.read("air.ini") 95 | default=self.config['DEFAULT'] 96 | self.altHoldA=default.getfloat('altHoldA') 97 | self.altHoldP=default.getfloat('altHoldP') 98 | self.altHoldI=default.getfloat('altHoldI') 99 | self.altHoldD=default.getfloat('altHoldD') 100 | self.altHoldOffset=default.getfloat('altHoldOffset') 101 | self.altHoldThrottleMultiplier=default.getfloat('altHoldThrottleMult') 102 | 103 | def saveConfig(self): 104 | default=self.config['DEFAULT'] 105 | default['altHoldA']=str(self.altHoldA) 106 | default['altHoldP']=str(self.altHoldP) 107 | default['altHoldI']=str(self.altHoldI) 108 | default['altHoldD']=str(self.altHoldD) 109 | default['altHoldOffset']=str(self.altHoldOffset) 110 | default['altHoldThrottleMult']=str(self.altHoldThrottleMultiplier) 111 | with open('air.ini', 'w') as configfile: 112 | self.config.write(configfile) 113 | 114 | def updateRC(self,now): 115 | if (self.rcDirty or now-self.lastRCSend>0.05) and self.remoteConnection: 116 | self.rcDirty=False 117 | self.lastRCSend=now 118 | data = [self.roll,self.pitch,self.throttle,self.yaw,1000,1000,1000,1000] 119 | self.board.sendCMD(16,MultiWii.SET_RAW_RC,data) 120 | 121 | def doHeartbeat(self,now): 122 | self.mav.heartbeat_send(0,0,0,0,0) 123 | 124 | def arm(self,now): 125 | self.inhibitMultiWii=True 126 | self.se.play([[48,128,sirenTone]]) 127 | time.sleep(0.05) 128 | self.homeAltitude=self.filterAlt.get() 129 | self.board.arm() 130 | self.inhibitMultiWii=False 131 | def disarm(self,now): 132 | self.inhibitMultiWii=True 133 | time.sleep(0.05) 134 | self.board.disarm() 135 | self.inhibitMultiWii=False 136 | 137 | 138 | def setPIDTuning(self,tuningId,tuningValue): 139 | if tuningId>=0 and tuningId<30: 140 | self.tuningId=tuningId 141 | self.tuningValue=tuningValue 142 | elif tuningId==30: 143 | self.altHoldA=float(tuningValue) 144 | elif tuningId==31: 145 | self.altHoldP=float(tuningValue) 146 | elif tuningId==32: 147 | self.altHoldD=float(tuningValue) 148 | elif tuningId==33: 149 | self.altHoldI=float(tuningValue) 150 | elif tuningId==34: 151 | self.altHoldThrottleMultiplier=float(tuningValue) 152 | 153 | if tuningId>=30: 154 | self.saveConfig() 155 | 156 | def getPIDTuning(self,tuningId): 157 | if tuningId>=0 and tuningId<30: 158 | return self.board.PIDcoef[tuningId] 159 | elif tuningId==30: 160 | return self.altHoldA 161 | elif tuningId==31: 162 | return self.altHoldP 163 | elif tuningId==32: 164 | return self.altHoldD 165 | elif tuningId==33: 166 | return self.altHoldI 167 | elif tuningId==34: 168 | return self.altHoldThrottleMultiplier 169 | 170 | def multiWiiLoop(self): 171 | now=time.time() 172 | self.lastSensorPoll=now 173 | self.filterAlt=WindowFilter(4,0.2) 174 | self.sensorIndex=0 175 | self.multiWiiFailure=False 176 | self.tuningId=None 177 | 178 | loopPeriod=0.02 179 | loopExpectedTime=time.time() 180 | 181 | while not self.threadInterrupted: 182 | 183 | now=time.time() 184 | 185 | try: 186 | 187 | if self.aux1==1100: 188 | self.altitude.doAltitudeHold(now) 189 | self.rcDirty=True 190 | else: 191 | self.altitude.setpoint=0 192 | if self.throttleIn<1000: 193 | self.throttleIn=1000 194 | self.throttle=self.throttleIn 195 | 196 | if not self.inhibitMultiWii: 197 | 198 | self.updateRC(now) 199 | self.sensorPoll(now) 200 | self.multiWiiFailure=False 201 | 202 | #keep loop synchronized to clock period 203 | tdiff=now-loopExpectedTime 204 | loopExpectedTime+=loopPeriod 205 | if tdiff<0: # too quick 206 | time.sleep(-tdiff) 207 | elif tdiff>0.1: #much too slow - skip frames 208 | loopExpectedTime=now+loopPeriod 209 | print "multiWiiLoop skipping ",tdiff 210 | 211 | except (serial.SerialException, serial.portNotOpenError, ValueError) as se: 212 | #deal with disconnect of flight controller from Pi 213 | print se 214 | self.multiWiiFailure=True 215 | time.sleep(0.5) 216 | try: 217 | try: 218 | self.board.ser.close() 219 | except: 220 | pass 221 | multiwii=self.board.ser.port 222 | self.board = MultiWii(multiwii) 223 | except: 224 | pass 225 | 226 | except Exception, err: 227 | print err 228 | traceback.print_exc(file=sys.stdout) 229 | time.sleep(0.1) 230 | 231 | def updateSonarDistance(self,now): 232 | distance=self.sonar.getDistance() 233 | 234 | if distance>0 and abs(self.board.attitude['angx'])<15 and abs(self.board.attitude['angy'])<15: 235 | #print self.board.attitude['angx'] 236 | if now-self.sonarLastReading>1: 237 | self.sonarDistance=distance 238 | else: 239 | self.sonarDistance=distance #self.sonarDistance*0.25+distance*0.75 240 | self.sonarLastReading=now 241 | 242 | if now-self.sonarLastReading>0.5: 243 | self.sonarDistance=-1 244 | 245 | 246 | #print distance,"cm" 247 | 248 | 249 | 250 | 251 | #lastSensorPoll=0 252 | 253 | #called ~50/sec - each step called 5/sec 254 | def sensorPoll(self,now): 255 | #td=now-self.lastSensorPoll 256 | #self.lastSensorPoll=now 257 | #print "sensorPoll",td 258 | 259 | #self.board.flushIn() 260 | while self.board.ser.in_waiting>0: 261 | code=self.board.doRead() 262 | 263 | if code==MultiWii.RAW_IMU: 264 | self.altitude.updateAccAltitude() 265 | elif code==MultiWii.ALTITUDE: 266 | self.altitude.updateBaroAltitude() 267 | elif code==MultiWii.ATTITUDE: 268 | f=math.pi/180; 269 | angx=self.board.attitude['angx']*f 270 | angy=self.board.attitude['angy']*f 271 | self.theta=math.cos(angx)*math.cos(angy) 272 | if self.theta<0: # 90 degrees off level 273 | self.disarm(now) 274 | elif code==MultiWii.RAW_GPS: 275 | print self.board.gps 276 | 277 | self.sensorIndex=(self.sensorIndex+1)%10 278 | index=self.sensorIndex 279 | 280 | if self.aux1==1100: # if in althold 281 | a=index%2 282 | if a==0: 283 | self.board.getData(MultiWii.ALTITUDE) 284 | else: 285 | self.board.getData(MultiWii.RAW_IMU) 286 | else: 287 | if index==4: 288 | self.board.getData(MultiWii.ALTITUDE) 289 | a=index%2 290 | if a==1: 291 | self.board.getData(MultiWii.RAW_IMU) 292 | 293 | 294 | 295 | if index==0: 296 | self.sonar.doTrigger() 297 | self.board.getData(MultiWii.ANALOG) 298 | elif index==1: 299 | self.board.getData(MultiWii.STATUS) 300 | flags=self.board.status['flag'] 301 | self.armed=flags&1!=0 302 | self.failsafe=flags&4096!=0 303 | elif index==2: 304 | self.board.getData(MultiWii.RAW_GPS) 305 | elif index==3: 306 | #self.board.getData(MultiWii.RAW_IMU) 307 | if self.armed: 308 | self.logger.logTelemetry(self) 309 | elif index==4: 310 | #self.board.getData(MultiWii.ALTITUDE) 311 | 312 | #test if on ground 313 | onGround=False 314 | if self.throttle==1000: 315 | gx=self.board.rawIMU['gx'] 316 | gy=self.board.rawIMU['gy'] 317 | gz=self.board.rawIMU['gz'] 318 | gyroMag=math.sqrt(gx*gx+gy*gy+gz*gz) 319 | if gyroMag<100: 320 | onGround=True 321 | if onGround: 322 | if self.onGroundCountDown>0: 323 | self.onGroundCountDown-=1 324 | else: 325 | self.onGround=True 326 | self.homeAltitude=self.filterAlt.get() 327 | else: 328 | self.onGround=False 329 | self.onGroundCountDown=5 330 | self.altitude.updateSonarDistance(now) 331 | elif index==5: 332 | self.board.getData(MultiWii.MOTOR) 333 | self.sonar.doTrigger() 334 | elif index==6: 335 | self.board.getData(MultiWii.ATTITUDE) 336 | elif index==7: 337 | self.board.getData(MultiWii.PID) 338 | if self.tuningId!=None: 339 | self.board.PIDcoef[self.tuningId]=self.tuningValue 340 | self.tuningId=None 341 | self.board.setPID2() 342 | print "changing PID",self.tuningId,self.tuningValue 343 | elif index==8: 344 | pass 345 | #self.board.getData(MultiWii.RAW_IMU) 346 | #self.updateAccAltitude(now) 347 | elif index==9: 348 | #self.board.getData(MultiWii.ALTITUDE) 349 | self.altitude.updateSonarDistance(now) 350 | 351 | 352 | def checkForErrors(self,now): 353 | 354 | errors=[] 355 | warnings=[] 356 | 357 | if now-self.lastRemoteHeartbeat>4: 358 | errors.append("No heartbeat received"); 359 | self.remoteConnection=False 360 | if self.remoteSNR<10: 361 | errors.append("low remoteSNR "+str(self.remoteSNR)) 362 | if self.localSNR<10: 363 | errors.append("low localSNR "+str(self.localSNR)) 364 | if self.board.vbat!=None and self.board.vbat<10: 365 | errors.append("battery critical "+str(self.board.vbat)) 366 | elif self.board.vbat!=None and self.board.vbat<10.8: 367 | warnings.append("battery low "+str(self.board.vbat)) 368 | if self.board.gps['fix']==0: 369 | warnings.append("No GPS Fix {:1.0f}sat".format(self.board.gps['satellites'])) 370 | #self.board.arm() 371 | if self.multiWiiFailure: 372 | errors.append("Lost MultiWii") 373 | 374 | if len(errors)>0: 375 | print errors 376 | #self.se.play([[0,25,errorTone]],pitchOffset=0,speed=1,legato=1.0) 377 | elif len(warnings)>0: 378 | #print warnings 379 | #self.se.play([[0,12,warningTone]],pitchOffset=0,speed=1,legato=1.0) 380 | pass 381 | 382 | def telemetrySend(self,now): 383 | 384 | #send data to ground 385 | self.existingRemoteError=None 386 | remoteError=None 387 | if self.multiWiiFailure: 388 | remoteError="Lost MultiWii" 389 | 390 | if remoteError and self.existingRemoteError!=remoteError: 391 | self.mav.statustext_send(mavlink.MAV_SEVERITY_EMERGENCY,remoteError) 392 | self.existingRemoteError=remoteError 393 | else: 394 | self.mav.statustext_send(mavlink.MAV_SEVERITY_INFO,"") 395 | self.existingRemoteError=None 396 | 397 | batteryPercent=(self.board.vbat-9.6)/(12.6-9.6)*100 398 | if batteryPercent>100: 399 | batteryPercent=100 400 | elif batteryPercent<0: 401 | batteryPercent=0 402 | self.mav.battery_status_send(0,0,0,0,[self.board.vbat*1000,0,0,0,0,0,0,0,0,0],0,0,0,batteryPercent); 403 | ''' 404 | #MAVLINK messages ATTITUDE GLOBAL_POSITION_INT 405 | #attitude_send(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, force_mavlink1=False): 406 | degToRad=math.pi/180 407 | att=self.board.attitude 408 | pitch=att['angy']*degToRad 409 | roll=att['angx']*degToRad 410 | yaw=att['heading']*degToRad 411 | #print pitch,roll,yaw 412 | self.mav.attitude_send(0,roll,pitch,yaw,0,0,0) 413 | 414 | #def gps_raw_int_send(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible, force_mavlink1=False): 415 | self.mav.gps_raw_int_send(0, 416 | self.board.gps['fix'], 417 | self.board.gps['lat']*10000000, 418 | self.board.gps['lon']*10000000, 419 | self.board.gps['altitude']*1000, 420 | 0,0, 421 | self.board.gps['speed'], 422 | self.board.gps['course']*100, 423 | self.board.gps['satellites']); 424 | 425 | altitude #141 426 | ''' 427 | #def high_latency_send(self, base_mode, custom_mode, landed_state, roll, pitch, heading, throttle, heading_sp, latitude, longitude, altitude_amsl, altitude_sp, airspeed, airspeed_sp, groundspeed, climb_rate, gps_nsat, gps_fix_type, battery_remaining, temperature, temperature_air, failsafe, wp_num, wp_distance, force_mavlink1=False): 428 | heading=self.board.attitude['heading'] 429 | if heading>180.0: 430 | heading=360.0-heading 431 | gpsAltitude=self.board.gps['altitude'] 432 | if gpsAltitude>32767: 433 | gpsAltitude=32767 434 | 435 | baroAltitude=self.altitude.getFusionAltitude() #self.filterAlt.get()-self.homeAltitude 436 | #print heading, altitude 437 | self.mav.high_latency_send(0, 438 | self.board.status['flag'], 439 | 0, 440 | self.board.attitude['angx']*100, 441 | self.board.attitude['angy']*100, 442 | heading*100, 443 | 0,0, 444 | self.board.gps['lat']*10000000, 445 | self.board.gps['lon']*10000000, 446 | gpsAltitude, 447 | baroAltitude, 448 | 0,0, 449 | self.board.gps['speed'], 450 | 0, 451 | self.board.gps['satellites'], 452 | self.board.gps['fix'], 453 | batteryPercent, 454 | 0,0,0,0,0) 455 | #possible problem fields: heading, altitude x2, 456 | 457 | 458 | def processMessage(self,msg,now): 459 | msgType=msg.get_type() 460 | #print msgType 461 | 462 | if msg.get_type()=='RC_CHANNELS_OVERRIDE': 463 | self.lastRemoteHeartbeat=now 464 | self.throttleIn=msg.chan1_raw 465 | self.yaw=msg.chan2_raw 466 | self.pitch=msg.chan3_raw 467 | self.roll=msg.chan4_raw 468 | self.aux1=msg.chan5_raw 469 | self.rcDirty=True 470 | elif msgType=='RADIO_STATUS' or msgType=='RADIO': 471 | localSignal=(msg.rssi/1.9-127) 472 | localNoise=(msg.noise/1.9-127) 473 | self.localSNR=localSignal-localNoise 474 | remoteSignal=(msg.remrssi/1.9-127) 475 | remoteNoise=(msg.remnoise/1.9-127) 476 | self.remoteSNR=remoteSignal-remoteNoise 477 | elif msgType=='HEARTBEAT': 478 | self.lastRemoteHeartbeat=now 479 | if not(self.remoteConnection): 480 | self.remoteConnection=True 481 | self.se.play([[60,128,bellTone]]) 482 | elif msgType=='SET_MODE': 483 | #print "mode",msg 484 | mode=msg.base_mode 485 | if mode==mavlink.MAV_MODE_STABILIZE_ARMED: 486 | self.arm(now) 487 | elif mode==mavlink.MAV_MODE_PREFLIGHT: 488 | self.disarm(now) 489 | elif msgType=='PARAM_REQUEST_READ': 490 | paramId=msg.param_index 491 | paramValue=int(self.getPIDTuning(paramId)) 492 | #print "PARAM_REQUEST_READ",paramId,paramValue 493 | self.mav.param_value_send(self.pidLookup[paramId],paramValue,0,0,paramId) 494 | elif msgType=='COMMAND_LONG': 495 | command=msg.command 496 | if command==mavlink.MAV_CMD_DO_SET_PARAMETER: 497 | #print "MAV_CMD_DO_SET_PARAMETER",msg.param1,msg.param2 498 | self.setPIDTuning(int(msg.param1),int(msg.param2)) 499 | self.se.play([[0,16,flatTone],[None,16],[0,16]],legato=0.9,speed=1.5,pitchOffset=24) 500 | if command==mavlink.MAV_CMD_DO_CHANGE_ALTITUDE: 501 | altitude=msg.param1 502 | self.altitude.altHoldSetpoint+=altitude 503 | print "set altHoldSetpoint",altitude 504 | else: 505 | print msgType 506 | 507 | def loop(self): 508 | now=time.time(); 509 | self.se.play([[0,16,flatTone],[4,16],[7,16],[12,48]],legato=0.9,speed=1.5,pitchOffset=24) 510 | 511 | self.lastTelemetrySend=now 512 | 513 | while not self.threadInterrupted: 514 | try: 515 | now=time.time(); 516 | 517 | if now-self.lastHeartbeatSent>1: 518 | self.lastHeartbeatSent=now 519 | self.doHeartbeat(now) 520 | self.checkForErrors(now) 521 | 522 | msg = self.mavconnection.recv_msg() 523 | if msg!=None: 524 | self.processMessage(msg,now) 525 | 526 | if now-self.lastTelemetrySend>1: 527 | self.lastTelemetrySend=now 528 | try: 529 | self.telemetrySend(now) 530 | except Exception, err: 531 | print err 532 | 533 | time.sleep(0.02) 534 | except Exception, err: 535 | print "here3",Exception, err 536 | traceback.print_exc(file=sys.stdout) 537 | time.sleep(0.1) 538 | 539 | #on termination 540 | self.board.disarm() 541 | 542 | def start(self): 543 | self.threadInterrupted=False 544 | 545 | t = Thread(target=self.loop) 546 | t.setDaemon(True) 547 | t.start() 548 | self.threadMain=t 549 | 550 | t = Thread(target=self.multiWiiLoop) 551 | t.setDaemon(True) 552 | t.start() 553 | self.threadMultiWii=t 554 | -------------------------------------------------------------------------------- /air/droneController/Logger.py: -------------------------------------------------------------------------------- 1 | #import time,datetime 2 | from datetime import datetime 3 | import os, os.path, time, random 4 | 5 | class Logger: 6 | 7 | def __init__(self): 8 | while True: 9 | r = random.randint(0,65536) 10 | filename="/home/pi/dev/droneController/log/log_{:4x}.csv".format(r); 11 | if not os.path.isfile(filename): 12 | break; 13 | 14 | #filenameBase=datetime.now().strftime("/home/pi/dev/droneController/log/log_%y-%m-%d_%H-%M.txt") 15 | #while os.path.isfile(filename): 16 | # filename 17 | self.file=open(filename,mode="w+",buffering=1) 18 | self.first=True 19 | print filename 20 | self.lastFlush=time.time() 21 | 22 | def logRaw(self,msg): 23 | timeString=datetime.now().strftime("%H:%M:%S:%f") 24 | line=timeString+","+msg+"\r\n" 25 | #print msg 26 | self.file.write(line) 27 | 28 | now=time.time() 29 | if now-self.lastFlush>10: 30 | self.lastFlush=now 31 | self.file.flush() 32 | os.fsync(self.file.fileno()) 33 | 34 | def logSmart(self,logdef,src): 35 | for d in logdef: 36 | ref=src 37 | for d2 in d: 38 | if d2.endswith("[]"): 39 | #print "array",d2 40 | ref=ref[d2[:-2]] 41 | elif d2.endswith("()"): 42 | #print "function",d2 43 | m=getattr(ref,d2[:-2]) 44 | ref=m() 45 | else: 46 | ref=getattr(ref,d2) 47 | print "value=",str(ref),"for",d2 48 | 49 | def logTelemetry(self,src): 50 | 51 | defn=[ 52 | [src.localSNR,"{:.2f}","localSNR"], 53 | [src.remoteSNR,"{:.2f}","remoteSNR"], 54 | [src.remoteConnection,"{0}","groundConnection"], 55 | [not src.multiWiiFailure,"{0}","nazeConnection"], 56 | [src.board.vbat,"{:.1f}","batteryVoltage"], 57 | [src.board.current,"{:.2f}","batteryCurrent"], 58 | [src.failsafe,"{0}","failsafe"], 59 | 60 | [src.armed,"{0}","armed"], 61 | #[src.throttle,"{:.0f}","throttleRC"], 62 | #[src.yaw,"{:.0f}","yawRC"], 63 | #[src.pitch,"{:.0f}","pitchRC"], 64 | #[src.roll,"{:.0f}","rollRC"], 65 | #[src.aux1,"{:.0f}","aux1RC"], 66 | [src.aux1==1100,"{0}","altHold"], 67 | 68 | [src.board.attitude['angx'],"{:.1f}","pitch"], 69 | [src.board.attitude['angy'],"{:.1f}","roll"], 70 | #[src.board.attitude['heading'],"{:.1f}","heading"], 71 | 72 | #[src.altitude.getBaroAltitude(),"{:.0f}","baroAltitude"], 73 | #[src.altitude.getSonarAltitude(),"{:.0f}","sonarAltitude"], 74 | [(src.throttle-1000)/10,"{:.1f}","throttlePercent"], 75 | [src.altitude.altHoldSetpoint,"{:.0f}","setpoint"], 76 | [src.altitude.getFusionAltitude(),"{:.0f}","fusionAltitude"], 77 | #[src.altitude.getAltitudeVelocity(),"{:.1f}","altitudeVelocityAcc"], 78 | #[src.altitude.baroVelocityFilter.get(),"{:.1f}","altitudeVelocityBaro"], 79 | #[src.altitude.altHoldSetpoint,"{:.0f}","altHoldSetpoint"], 80 | 81 | [src.altitude.hoverpoint,"{:.0f}","hoverpoint"], 82 | [src.altitude.hoverpointLowerBound,"{:.0f}","hpLowerBound"], 83 | [src.altitude.hoverpointUpperBound,"{:.0f}","hpUpperBound"], 84 | [src.altitude.hoverpointLast,"{:.0f}","hoverpointLast"], 85 | [src.altitude.hoverpointCountdown,"{:.0f}","hpCount"], 86 | 87 | 88 | #[src.board.gps['lat'],"{:.7f}","latitude"], 89 | #[src.board.gps['lon'],"{:.7f}","longitude"], 90 | #[src.board.gps['altitude'],"{:.0f}","gpsAltitude"], 91 | #[src.board.gps['speed'],"{:.2f}","speed"], 92 | #[src.board.gps['satellites'],"{:.0f}","satellites"], 93 | #[src.board.gps['fix'],"{:.0f}","fix"], 94 | 95 | #[src.board.motor['m1'],"{:.0f}","motor1-SE-CW"], 96 | #[src.board.motor['m2'],"{:.0f}","motor2-NE-CCW"], 97 | #[src.board.motor['m3'],"{:.0f}","motor3-SW-CCW"], 98 | #[src.board.motor['m4'],"{:.0f}","motor4-NW-CW"], 99 | 100 | #[src.board.rawIMU['ax'],"{:.0f}","ax"], 101 | #[src.board.rawIMU['ay'],"{:.0f}","ay"], 102 | [src.board.rawIMU['az'],"{:.0f}","az"], 103 | #[src.board.rawIMU['gx'],"{:.0f}","gx"], 104 | #[src.board.rawIMU['gy'],"{:.0f}","gy"], 105 | #[src.board.rawIMU['gz'],"{:.0f}","gz"], 106 | #[src.board.rawIMU['mx'],"{:.0f}","mx"], 107 | #[src.board.rawIMU['my'],"{:.0f}","my"], 108 | #[src.board.rawIMU['mz'],"{:.0f}","mz"], 109 | 110 | #[src.altitude.pidAltP,"{:.0f}","altHoldP"], 111 | #[src.altitude.pidAltI,"{:.0f}","altHoldI"], 112 | #[src.altitude.pidAltD,"{:.0f}","altHoldD"], 113 | 114 | ] 115 | 116 | if self.first: 117 | self.first=False 118 | parts=[] 119 | for def1 in defn: 120 | parts.append(def1[2]) 121 | line = ",".join(parts ) 122 | self.logRaw(line) 123 | 124 | parts=[] 125 | for def1 in defn: 126 | v=def1[0] 127 | parts.append(def1[1].format(v)) 128 | line = ", ".join(parts ) 129 | self.logRaw(line) -------------------------------------------------------------------------------- /air/droneController/MultiUART.py: -------------------------------------------------------------------------------- 1 | import spidev 2 | import time 3 | 4 | 5 | class MultiUART: 6 | 7 | spi=None 8 | uart=None 9 | 10 | def __init__(self,UART,SPIDivider): 11 | self.uart=UART 12 | spi = spidev.SpiDev() 13 | spi.open(0,0) 14 | self.spi=spi 15 | spi.lsbfirst=False 16 | #div64 = ??? 250Mhz/64 = 4000000 17 | spi.max_speed_hz=250000000/SPIDivider 18 | self.spi.cshigh=False 19 | spi.loop=False 20 | #spi.bits_per_word=8 21 | return 22 | 23 | def cleanup(self): 24 | self.spi.close() 25 | return 26 | 27 | # Returns the number of received bytes held in queue for the selected channel. 28 | def checkRx(self): 29 | result=0 30 | self.spi.xfer2( [0x10 | self.uart ]); 31 | time.sleep(0.00250) 32 | result=self.spi.readbytes(1)[0] 33 | time.sleep(0.00250) 34 | return result 35 | 36 | def checkTx(self): 37 | result=0 38 | self.spi.xfer2( [0x30 | self.uart ]); 39 | time.sleep(0.00250) 40 | result=self.spi.readbytes(1)[0] 41 | time.sleep(0.00250) 42 | return result 43 | 44 | def receiveByte(self): 45 | self.spi.xfer2( [0x20 | self.uart ]); 46 | time.sleep(0.001) 47 | self.spi.xfer2( [1]); 48 | time.sleep(0.001) 49 | result=self.spi.xfer2([0xFF])[0] 50 | time.sleep(0.001) 51 | return result 52 | 53 | def flushRx(self): 54 | c=self.checkRx() 55 | if c>0: 56 | self.receiveBytes(c) 57 | c=self.checkRx() 58 | if c>0: 59 | self.receiveBytes(c) 60 | 61 | def receiveBytes(self, NUMBYTES): 62 | self.spi.xfer2( [0x20 | self.uart ]); 63 | time.sleep(0.001) 64 | self.spi.xfer2( [NUMBYTES]); 65 | result=[] 66 | for i in range(0,NUMBYTES): 67 | time.sleep(0.0005) 68 | v=self.spi.xfer2([0xFF])[0] 69 | result.append(v) 70 | time.sleep(0.001) 71 | return result 72 | 73 | def transmitByte(self, DATA): 74 | self.spi.xfer2( [0x40 | self.uart ]); 75 | time.sleep(0.001) 76 | self.spi.xfer2( [1]); 77 | time.sleep(0.001) 78 | result=self.spi.xfer2([DATA])[0] 79 | time.sleep(0.001) 80 | return 81 | 82 | def transmitBytes(self, DATA): 83 | self.spi.xfer2( [0x40 | self.uart ]); 84 | time.sleep(0.001) 85 | length=len(DATA) 86 | self.spi.xfer2( [length]); 87 | for i in range(0,length): 88 | time.sleep(0.0005) 89 | self.spi.xfer2([DATA[i]]) 90 | time.sleep(0.001) 91 | return 92 | 93 | # Configures the baud rate of the selected channel. 94 | # Baud : 0=1200, 1=2400, 2=4800, 3=9600, 4=19200, 5=38400, 6=57600, 7=115200 95 | def setBaud(self, BAUD): 96 | self.spi.xfer2( [0x80 | self.uart ]); 97 | time.sleep(0.00250) 98 | result=self.spi.xfer2([ BAUD ]); 99 | time.sleep(0.1) 100 | return 101 | -------------------------------------------------------------------------------- /air/droneController/air.ini: -------------------------------------------------------------------------------- 1 | [DEFAULT] 2 | altholda = 82.0 3 | altholdp = 10.0 4 | altholdd = 63.0 5 | altholdi = 86.0 6 | altholdthrottlemult = 128.0 7 | altholdoffset = 1500.0 8 | 9 | -------------------------------------------------------------------------------- /air/droneController/altitude.py: -------------------------------------------------------------------------------- 1 | import math, random, os, time 2 | from windowfilter import WindowFilter 3 | from hoverCalc import HoverCalc 4 | 5 | def constrain(v,min,max): 6 | if vmax: 9 | r=max 10 | else: 11 | r=v 12 | return r 13 | 14 | class Altitude: 15 | 16 | lastBaroReading=None 17 | lastAccReading=None 18 | lastSonarReading=None 19 | #accZVelocity=0.0 20 | #accHeight=0.0 21 | #velocityDiffIntegral=0.0 22 | sonarAlt=0.0 23 | accZ=0.0 24 | 25 | 26 | baroThrottleOffset=0.0 # -0.1 #how thottle causes pressure drop and perceived altitude to be higher cm/servoVal 27 | 28 | hoverpointInitial=1550 29 | hoverpointAnnealDistrupt=25 30 | hoverpointAnnealPeriod=50 31 | hoverpointAnnealK1=0.04 32 | hoverpointAnnealK2=0.02 33 | hoverpointAnnealK3=0.025 34 | 35 | def __init__(self,airmainloop): 36 | self.airmainloop=airmainloop 37 | self.board=airmainloop.board 38 | self.baroAltFilter=WindowFilter(5,0.2) 39 | self.baroVelocityFilter=WindowFilter(10,0.1) 40 | #self.accVelocityLongTermAverage=WindowFilter(100,0) 41 | 42 | self.hoverpointUpperBound=self.hoverpointInitial+self.hoverpointAnnealDistrupt*4; 43 | self.hoverpointLowerBound=self.hoverpointInitial-self.hoverpointAnnealDistrupt*4; 44 | self.hoverpoint=self.hoverpointInitial 45 | self.hoverpointLast=self.hoverpointInitial 46 | self.hoverpointCountdown=self.hoverpointAnnealPeriod/2; 47 | self.hoverpointInitialRamp=4.0 48 | 49 | self.hoverpointInitial=self.airmainloop.altHoldOffset 50 | 51 | self.hoverpoint2=self.hoverpointInitial 52 | 53 | ''' 54 | filename="/home/pi/dev/droneController/log/hover.csv" 55 | self.file=open(filename,mode="w+",buffering=1) 56 | self.lastFlush=time.time() 57 | ''' 58 | 59 | self.hoverCalc=HoverCalc(airmainloop) 60 | 61 | 62 | 63 | 64 | #lhc=0 65 | def calculateHoverpoint2(self,accZ,throttle): 66 | #print accZ,throttle 67 | #t=time.time() 68 | #print t-self.lhc 69 | #self.lhc=t; 70 | #self.calculateHoverpoint2Count+=1 71 | #print self.calculateHoverpoint2Count 72 | #pass 73 | ''' 74 | line="{:.2f},{:.0f}\r\n".format(accZ,throttle) 75 | self.file.write(line) 76 | 77 | now=time.time() 78 | if now-self.lastFlush>1: 79 | self.lastFlush=now 80 | self.file.flush() 81 | os.fsync(self.file.fileno()) 82 | ''' 83 | 84 | 85 | def calculateHoverpoint(self,accZ,throttle): 86 | 87 | #only attempt to calculate when we have potential hover situations 88 | if throttleself.hoverpointInitial+self.hoverpointAnnealDistrupt*4: 89 | return 90 | if self.getFusionAltitude()<10: 91 | return 92 | 93 | if accZ>0: 94 | k1=self.hoverpointAnnealK1*self.hoverpointInitialRamp 95 | k2=self.hoverpointAnnealK2*self.hoverpointInitialRamp 96 | else: 97 | k1=self.hoverpointAnnealK2*self.hoverpointInitialRamp 98 | k2=self.hoverpointAnnealK1*self.hoverpointInitialRamp 99 | 100 | 101 | 102 | self.hoverpointUpperBound=self.hoverpointUpperBound*(1-k1)+throttle*k1; 103 | self.hoverpointLowerBound=self.hoverpointLowerBound*(1-k2)+throttle*k2; 104 | 105 | self.hoverpointCountdown-=1 106 | if self.hoverpointCountdown<=0 or self.hoverpointLowerBound>self.hoverpointUpperBound: 107 | #process anneal phase 108 | if self.hoverpointUpperBound>=self.hoverpointLast+self.hoverpointAnnealDistrupt*0.9: 109 | #we have maxed out and need to move up fast 110 | self.hoverpointCountdown=self.hoverpointAnnealPeriod/2 111 | self.hoverpointLast=self.hoverpointUpperBound 112 | self.hoverpointUpperBound=self.hoverpointLast+self.hoverpointAnnealDistrupt*4 113 | self.hoverpointLowerBound=self.hoverpointLast-self.hoverpointAnnealDistrupt*0.5 114 | elif self.hoverpointLowerBound<=self.hoverpointLast-self.hoverpointAnnealDistrupt*0.9: 115 | #we have mined out and need to move down fast 116 | self.hoverpointCountdown=self.hoverpointAnnealPeriod/2 117 | self.hoverpointLast=self.hoverpointLowerBound 118 | self.hoverpointUpperBound=self.hoverpointLast+self.hoverpointAnnealDistrupt*0.5 119 | self.hoverpointLowerBound=self.hoverpointLast-self.hoverpointAnnealDistrupt*4 120 | else: 121 | #normal anneal within tolerances 122 | self.hoverpointCountdown=self.hoverpointAnnealPeriod 123 | self.hoverpointLast=(self.hoverpointUpperBound+self.hoverpointLowerBound)*0.5 124 | self.hoverpointUpperBound+=self.hoverpointAnnealDistrupt 125 | self.hoverpointLowerBound-=self.hoverpointAnnealDistrupt 126 | print "hoverpoint",self.hoverpointLast,self.hoverpointInitialRamp 127 | 128 | #ramp down scaling to unity 129 | if self.hoverpointInitialRamp>1.0: 130 | self.hoverpointCountdown=self.hoverpointAnnealPeriod/2 131 | self.hoverpointInitialRamp*=0.8 132 | else: 133 | self.hoverpointInitialRamp=1.0 134 | else: 135 | self.hoverpoint=self.hoverpoint*(1-self.hoverpointAnnealK3)+self.hoverpointLast*self.hoverpointAnnealK3; 136 | 137 | 138 | def updateAccAltitude(self): 139 | az=self.board.rawIMU['az'] 140 | 141 | #correct for pitch/roll 142 | theta=self.airmainloop.theta 143 | az=az/theta 144 | #accZ acceleration in cm/s^2 145 | self.accZ=(az-512)*1.916015625 # /512.0*981 146 | 147 | 148 | if theta>0.96 and self.getFusionAltitude()>10: # only recalculate when tilting from level by <15degrees and over 10cm 149 | #self.hoverCalc.update(self.accZ,self.airmainloop.throttle*theta) 150 | self.calculateHoverpoint(self.accZ,self.airmainloop.throttle*theta) 151 | #self.calculateHoverpoint2(self.accZ,self.airmainloop.throttle*theta) 152 | 153 | ''' 154 | t=self.board.rawIMU['timestamp'] 155 | if self.lastAccReading!=None: 156 | td=t-self.lastAccReading 157 | if td<0 or td>1: #happens if clock changes 158 | self.lastAccReading=t 159 | return 160 | self.accZVelocity+=self.accZ*td 161 | ''' 162 | #pull accZVelocity drift back to baroVelocity 163 | #velocityDiff=self.accZVelocity-self.baroVelocityFilter.get() 164 | #self.accZVelocity-=velocityDiff*0.2 165 | 166 | 167 | #self.accZVelocity=constrain(self.accZVelocity,-200,200) 168 | #self.accZVelocity-=self.accVelocityLongTermAverage.get()*0.1 169 | #self.accVelocityLongTermAverage.add(self.accZVelocity) 170 | 171 | #use integral to fix persistent offset 172 | #self.velocityDiffIntegral=constrain(self.velocityDiffIntegral+velocityDiff,-200,200) 173 | #self.accZVelocity-=self.velocityDiffIntegral*0.002 174 | 175 | #pull accHeight drift back to baroHeight 176 | #heightDiff=self.accHeight-self.baroAltFilter.get() 177 | #self.accHeight-=heightDiff*0.04 178 | 179 | #self.lastAccReading=t 180 | 181 | #print "acc velocity",self.accZVelocity, self.baroVelocity#self.accHeight,self.baroFilter.get() 182 | #print "altitude {:.2f}cm/s {:.2f}cm {:.2f}cm".format(self.accZVelocity,self.baroVelocityFilter.get(),0) 183 | 184 | def updateBaroAltitude(self): 185 | t=self.board.altitude['timestamp'] 186 | alt=self.board.altitude['estalt'] 187 | offset=(self.airmainloop.throttle-1000)*self.baroThrottleOffset 188 | #alt+=offset 189 | #print "baro",alt,alt+offset 190 | 191 | 192 | if self.lastBaroReading!=None: 193 | self.baroAltFilter.add(alt-self.baroOffset) 194 | td=t-self.lastBaroReading 195 | if td<0 or td>1: #happens if clock changes 196 | self.lastBaroReading=t 197 | return 198 | self.baroVelocityFilter.add((alt-self.lastBaroAlt)/td) 199 | 200 | #pull baroAlt drift back to sonar 201 | if self.sonarAlt>0 and self.baroAltFilter.get()<300: 202 | heightDiff=self.baroAltFilter.get()-self.sonarAlt 203 | self.baroOffset+=heightDiff*0.4 204 | 205 | else: 206 | self.baroOffset=alt 207 | 208 | 209 | self.lastBaroAlt=alt 210 | self.lastBaroReading=t 211 | 212 | 213 | def updateSonarDistance(self,now): 214 | distance=self.airmainloop.sonar.getDistance() 215 | 216 | if distance>0 and abs(self.board.attitude['angx'])<15 and abs(self.board.attitude['angy'])<15: 217 | #print self.board.attitude['angx'] 218 | if self.lastSonarReading==None or now-self.lastSonarReading>1: 219 | self.sonarAlt=distance 220 | else: 221 | self.sonarAlt=distance #self.sonarDistance*0.25+distance*0.75 222 | self.lastSonarReading=now 223 | 224 | if self.lastSonarReading!=None and now-self.lastSonarReading>0.5: 225 | self.sonarAlt=-1 226 | 227 | #print "sonar {:.2f}cm {:.2f}cm".format(self.sonarAlt,self.baroAltFilter.get()) 228 | 229 | def getFusionAltitude(self): 230 | baroHeight=self.baroAltFilter.get() 231 | if self.sonarAlt>0 and baroHeight<300: 232 | altitude=self.sonarAlt 233 | else: 234 | altitude=baroHeight 235 | return altitude 236 | 237 | def getBaroAltitude(self): 238 | return self.baroAltFilter.get() 239 | 240 | def getSonarAltitude(self): 241 | return self.sonarAlt 242 | 243 | def getAltitudeVelocity(self): 244 | return self.accZVelocity; 245 | 246 | 247 | altHoldLastTime=0 248 | altHoldOffset=1500 249 | altHoldThrottleMultiplier=1.0 #how throttle alters altitude hold 250 | altHoldSetpoint=0.0 251 | 252 | pidAltP=0 253 | pidAltI=0 254 | pidAltD=0 255 | 256 | def doAltitudeHold(self,now): 257 | if now-self.altHoldLastTime>0.5: 258 | self.altHoldPreviousError=0 # self.throttle/self.altHoldD*dt 259 | self.altHoldIntegral=0 260 | if self.airmainloop.throttleIn<1150: 261 | self.altHoldSetpoint=0 262 | else: 263 | self.altHoldSetpoint=self.getFusionAltitude() 264 | self.altHoldControl=False 265 | else: 266 | 267 | alt=self.getFusionAltitude() 268 | error=constrain(self.altHoldSetpoint-alt,-100,100) 269 | p=constrain(self.airmainloop.altHoldP*0.05*error, -200,200) 270 | vel=self.baroVelocityFilter.get() 271 | d=constrain(self.airmainloop.altHoldD*0.005*vel,-150,150) 272 | 273 | #scale hoverpoint throttle for tilt from level (more tilt - more thrust needed) 274 | throttleHoverpoint=self.hoverpoint-1000.0 275 | throttleHoverpoint/=self.airmainloop.theta 276 | hoverpoint=throttleHoverpoint+1000 277 | 278 | output=constrain(hoverpoint/self.airmainloop.theta+p+d,1350,1650) 279 | self.airmainloop.throttle=output 280 | self.pidAltP=p 281 | self.pidAltD=d 282 | 283 | ''' 284 | alt=self.getFusionAltitude() 285 | altError=constrain(self.altHoldSetpoint-alt,-100,100) 286 | setVel=constrain(self.airmainloop.altHoldA*0.01*altError,-50,50) 287 | 288 | #altitude P control 289 | error=constrain(self.altHoldSetpoint-alt,-100,100) 290 | 291 | 292 | p=constrain(self.airmainloop.altHoldP*0.1*error, -200,200) 293 | 294 | #velocity I control 295 | dt=now-self.altHoldLastTime 296 | self.altHoldIntegral=constrain(self.altHoldIntegral+self.airmainloop.altHoldI*0.01*error*dt, -200,200) 297 | i=self.altHoldIntegral 298 | 299 | #velocity D control 300 | vel=setVel-self.baroVelocityFilter.get() 301 | d=constrain(self.airmainloop.altHoldD*0.01*vel,-150,150) 302 | 303 | output=constrain(self.altHoldOffset+p+i+d,1200,1650) 304 | self.airmainloop.throttle=output 305 | 306 | #print "altHold", altError,output 307 | #print "altitudeHold {:.0f}cm {:.1f}cm/s {:.0f} P{:.0f} I{:.0f} D{:.0f}".format(altError,setVel,output,p,i,d) 308 | self.pidAltP=p 309 | self.pidAltI=i 310 | self.pidAltD=d 311 | ''' 312 | 313 | 314 | # throttle must return to center before control can commence 315 | if abs(self.airmainloop.throttleIn-1000.0)<10 and not self.altHoldControl: 316 | self.altHoldControl=True 317 | 318 | 319 | if self.altHoldControl: 320 | #allow throttle to alter altitude 321 | throttleOffset=(self.airmainloop.throttleIn-1000)/1000.0; 322 | self.altHoldSetpoint+=throttleOffset*self.altHoldThrottleMultiplier*0.1 323 | if self.altHoldSetpoint<0.0: 324 | self.altHoldSetpoint=0.0 325 | 326 | self.altHoldLastTime=now 327 | 328 | def doAltitudeHold2(self,now): 329 | if now-self.altHoldLastTime>0.5: 330 | self.altHoldPreviousError=0 # self.throttle/self.altHoldD*dt 331 | self.altHoldIntegral=0 332 | self.altHoldSetpoint=self.getFusionAltitude() 333 | self.altHoldControl=False 334 | else: 335 | 336 | #altitude P control 337 | alt=self.getFusionAltitude() 338 | altError=constrain(self.altHoldSetpoint-alt,-100,100) 339 | setVel=constrain(self.airmainloop.altHoldA*0.01*altError,-50,50) 340 | 341 | #velocity P control 342 | #error=setVel-self.accZVelocity 343 | error=setVel-self.baroVelocityFilter.get() 344 | 345 | p=constrain(self.airmainloop.altHoldP*0.1*error, -200,200) 346 | 347 | #velocity I control 348 | dt=now-self.altHoldLastTime 349 | self.altHoldIntegral=constrain(self.altHoldIntegral+self.airmainloop.altHoldI*0.01*error*dt, -200,200) 350 | i=self.altHoldIntegral 351 | 352 | #velocity D control 353 | d=constrain(self.airmainloop.altHoldD*0.01*self.accZ,-150,150) 354 | 355 | output=constrain(self.altHoldOffset+p+i+d,1200,1650) 356 | self.airmainloop.throttle=output 357 | 358 | #print "altHold", altError,output 359 | #print "altitudeHold {:.0f}cm {:.1f}cm/s {:.0f} P{:.0f} I{:.0f} D{:.0f}".format(altError,setVel,output,p,i,d) 360 | self.pidAltP=p 361 | self.pidAltI=i 362 | self.pidAltD=d 363 | 364 | # throttle must return to center before control can commence 365 | if abs(self.airmainloop.throttleIn-1000.0)<10 and not self.altHoldControl: 366 | self.altHoldControl=True 367 | 368 | 369 | if self.altHoldControl: 370 | #allow throttle to alter altitude 371 | throttleOffset=(self.airmainloop.throttleIn-1000)/1000.0; 372 | self.altHoldSetpoint+=throttleOffset*self.altHoldThrottleMultiplier*0.1 373 | if self.altHoldSetpoint<0.0: 374 | self.altHoldSetpoint=0.0 375 | 376 | self.altHoldLastTime=now 377 | -------------------------------------------------------------------------------- /air/droneController/failsafe.py: -------------------------------------------------------------------------------- 1 | # Simple demo of of the PCA9685 PWM servo/LED controller library. 2 | # This will move channel 0 from min to max position repeatedly. 3 | # Author: Tony DiCola 4 | # License: Public Domain 5 | from __future__ import division 6 | import time 7 | import paho.mqtt.client as mqtt 8 | import Adafruit_PCA9685 9 | import serial 10 | from lightTelemetry import LightTelemetry 11 | 12 | pwm = Adafruit_PCA9685.PCA9685() 13 | 14 | 15 | # Helper function to make setting a servo pulse width simpler. 16 | def set_servo_pulse(channel, pulse): 17 | if pulse<1000: 18 | pulse=1000 19 | elif pulse>2000: 20 | pulse=2000 21 | t=0.2114*(pulse) 22 | pwm.set_pwm(channel, 0, int(t)) 23 | #print t 24 | 25 | def on_connect(client, userdata, flags, rc): 26 | print("Connected to MQTT broker "+str(rc)) 27 | client.subscribe("test") 28 | 29 | # The callback for when a PUBLISH message is received from the server. 30 | def on_message(client, userdata, msg): 31 | #print(msg.topic+" "+str(msg.payload)) 32 | pl=str(msg.payload) 33 | i=pl.index(":") 34 | channel=pl[:i] 35 | value=int(pl[i+1:]) 36 | print (channel,value) 37 | c=-1 38 | if channel=="throttle": 39 | c=2 40 | elif channel=="yaw": 41 | c=3 42 | elif channel=="pitch": 43 | c=0 44 | elif channel=="roll": 45 | c=1 46 | if c>=0: 47 | set_servo_pulse(c,value) 48 | if channel=="kill": 49 | pwm.set_pwm(2, 0, 0) 50 | 51 | pwm.set_pwm_freq(50) 52 | set_servo_pulse(2,1000) 53 | 54 | set_servo_pulse(0,1500) 55 | set_servo_pulse(1,1500) 56 | set_servo_pulse(3,1500) 57 | 58 | 59 | client = mqtt.Client("", True, None, mqtt.MQTTv31) 60 | client.on_connect = on_connect 61 | client.on_message = on_message 62 | 63 | client.connect("192.168.1.1", 1883, 60) 64 | client.loop_start() 65 | 66 | ser = serial.Serial( 67 | 68 | port='/dev/ttyS0', 69 | baudrate = 19200, 70 | parity=serial.PARITY_NONE, 71 | stopbits=serial.STOPBITS_ONE, 72 | bytesize=serial.EIGHTBITS, 73 | timeout=1 74 | ) 75 | 76 | lt=LightTelemetry() 77 | 78 | while True: 79 | 80 | b=ser.read(); 81 | #print "serial",b 82 | if b: 83 | lt.processByte(b) 84 | 85 | #time.sleep(0.01) 86 | 87 | -------------------------------------------------------------------------------- /air/droneController/hoverCalc.py: -------------------------------------------------------------------------------- 1 | import math, random, os, time 2 | from windowfilter import WindowAverage 3 | 4 | class HoverCalc: 5 | 6 | def __init__(self,airmainloop): 7 | self.airmainloop=airmainloop 8 | self.throttleAverage=WindowAverage(32) 9 | self.accAverage=WindowAverage(32) 10 | self.G=-981 #cm/s^2 11 | self.minThrottle=1150 12 | 13 | 14 | def update(self,acc,throttle): 15 | self.accAverage.add(acc) 16 | self.throttleAverage.add(throttle) 17 | 18 | #calculate hoverpoint throttle 19 | avgAcc=self.accAverage.get() 20 | avgThrottle=self.throttleAverage.get() 21 | c=(avgAcc-self.G)/(avgThrottle-self.minThrottle) 22 | b=self.G-self.minThrottle*c 23 | hoverpoint=-b/c 24 | print "hoverpoint2",hoverpoint,avgThrottle,avgAcc,self.airmainloop.altitude.getFusionAltitude() 25 | 26 | -------------------------------------------------------------------------------- /air/droneController/lightTelemetry.py: -------------------------------------------------------------------------------- 1 | import struct 2 | 3 | class LightTelemetry: 4 | 5 | mBuffer=[] 6 | mPitch=None 7 | mRoll=None 8 | mHeading=None 9 | 10 | def processByte(self,b): 11 | 12 | result=None 13 | if b=='$': 14 | result=self.processBuffer() 15 | self.mBuffer=[] 16 | if len(self.mBuffer)<30: 17 | self.mBuffer.append(b) 18 | else: 19 | print "overflow"; 20 | self.mBuffer=[] 21 | return result 22 | 23 | def verifyChecksum(self,length): 24 | result=False; 25 | if len(self.mBuffer)==4+length: 26 | actualChecksum=0; 27 | for i in range(3,3+length): 28 | actualChecksum^=ord(self.mBuffer[i]) 29 | expectedChecksum=ord(self.mBuffer[3+length]); 30 | result=expectedChecksum==actualChecksum; 31 | return result; 32 | 33 | def getInt16(self,p): 34 | s="".join(self.mBuffer[p:p+2]) 35 | t=struct.unpack( "3 and self.mBuffer[0]=='$' and self.mBuffer[1]=='T': 46 | type=self.mBuffer[2] 47 | if type=='A': 48 | if self.verifyChecksum(6): 49 | result=type 50 | self.mPitch=self.getInt16(3) 51 | self.mRoll=self.getInt16(5) 52 | self.mHeading=self.getInt16(7) 53 | #print "A",self.mPitch, self.mRoll, self.mHeading 54 | elif type=='S': 55 | if self.verifyChecksum(7): 56 | result=type 57 | self.mBattery=self.getInt16(3) 58 | self.mCurrent=self.getInt16(5) 59 | self.mRSSI=ord(self.mBuffer[7]) 60 | self.mAirSpeed=ord(self.mBuffer[8]) 61 | status=ord(self.mBuffer[9]) 62 | self.mArmed=status&0x01 63 | self.mFailsafe=status&0x02 64 | self.mFlightMode=status>>2 65 | print "S",self.mBattery 66 | elif type=='G': 67 | if self.verifyChecksum(14): 68 | result=type 69 | self.mLatitude=self.getInt32(3) 70 | self.mLongitude=self.getInt32(7) 71 | self.mGroundSpeed=ord(self.mBuffer[11]) 72 | self.mAltitude=self.getInt32(12) 73 | satellites=ord(self.mBuffer[16]) 74 | self.mSatellites=satellites>>2 75 | self.mFix=satellites&0x03 76 | print "GPS",self.mLatitude, self.mLongitude, self.mAltitude, self.mGroundSpeed, self.mSatellites 77 | return result; -------------------------------------------------------------------------------- /air/droneController/main.py: -------------------------------------------------------------------------------- 1 | from AirMainLoop import AirMainLoop 2 | import sys, time 3 | #from setRadio import SetRadio 4 | 5 | #SetRadio("/dev/ttyS0") 6 | 7 | loop=AirMainLoop( 8 | multiwii="/dev/ttyUSB0", 9 | radio="/dev/ttyS0" ) 10 | loop.start() 11 | 12 | while True: 13 | time.sleep(1) -------------------------------------------------------------------------------- /air/droneController/miniterm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Very simple serial terminal 4 | # 5 | # This file is part of pySerial. https://github.com/pyserial/pyserial 6 | # (C)2002-2015 Chris Liechti 7 | # 8 | # SPDX-License-Identifier: BSD-3-Clause 9 | 10 | import codecs 11 | import os 12 | import sys 13 | import threading 14 | 15 | import serial 16 | from serial.tools.list_ports import comports 17 | from serial.tools import hexlify_codec 18 | 19 | # pylint: disable=wrong-import-order,wrong-import-position 20 | 21 | codecs.register(lambda c: hexlify_codec.getregentry() if c == 'hexlify' else None) 22 | 23 | try: 24 | raw_input 25 | except NameError: 26 | # pylint: disable=redefined-builtin,invalid-name 27 | raw_input = input # in python3 it's "raw" 28 | unichr = chr 29 | 30 | 31 | def key_description(character): 32 | """generate a readable description for a key""" 33 | ascii_code = ord(character) 34 | if ascii_code < 32: 35 | return 'Ctrl+{:c}'.format(ord('@') + ascii_code) 36 | else: 37 | return repr(character) 38 | 39 | 40 | # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 41 | class ConsoleBase(object): 42 | """OS abstraction for console (input/output codec, no echo)""" 43 | 44 | def __init__(self): 45 | if sys.version_info >= (3, 0): 46 | self.byte_output = sys.stdout.buffer 47 | else: 48 | self.byte_output = sys.stdout 49 | self.output = sys.stdout 50 | 51 | def setup(self): 52 | """Set console to read single characters, no echo""" 53 | 54 | def cleanup(self): 55 | """Restore default console settings""" 56 | 57 | def getkey(self): 58 | """Read a single key from the console""" 59 | return None 60 | 61 | def write_bytes(self, byte_string): 62 | """Write bytes (already encoded)""" 63 | self.byte_output.write(byte_string) 64 | self.byte_output.flush() 65 | 66 | def write(self, text): 67 | """Write string""" 68 | self.output.write(text) 69 | self.output.flush() 70 | 71 | def cancel(self): 72 | """Cancel getkey operation""" 73 | 74 | # - - - - - - - - - - - - - - - - - - - - - - - - 75 | # context manager: 76 | # switch terminal temporary to normal mode (e.g. to get user input) 77 | 78 | def __enter__(self): 79 | self.cleanup() 80 | return self 81 | 82 | def __exit__(self, *args, **kwargs): 83 | self.setup() 84 | 85 | 86 | if os.name == 'nt': # noqa 87 | import msvcrt 88 | import ctypes 89 | 90 | class Out(object): 91 | """file-like wrapper that uses os.write""" 92 | 93 | def __init__(self, fd): 94 | self.fd = fd 95 | 96 | def flush(self): 97 | pass 98 | 99 | def write(self, s): 100 | os.write(self.fd, s) 101 | 102 | class Console(ConsoleBase): 103 | def __init__(self): 104 | super(Console, self).__init__() 105 | self._saved_ocp = ctypes.windll.kernel32.GetConsoleOutputCP() 106 | self._saved_icp = ctypes.windll.kernel32.GetConsoleCP() 107 | ctypes.windll.kernel32.SetConsoleOutputCP(65001) 108 | ctypes.windll.kernel32.SetConsoleCP(65001) 109 | self.output = codecs.getwriter('UTF-8')(Out(sys.stdout.fileno()), 'replace') 110 | # the change of the code page is not propagated to Python, manually fix it 111 | sys.stderr = codecs.getwriter('UTF-8')(Out(sys.stderr.fileno()), 'replace') 112 | sys.stdout = self.output 113 | self.output.encoding = 'UTF-8' # needed for input 114 | 115 | def __del__(self): 116 | ctypes.windll.kernel32.SetConsoleOutputCP(self._saved_ocp) 117 | ctypes.windll.kernel32.SetConsoleCP(self._saved_icp) 118 | 119 | def getkey(self): 120 | while True: 121 | z = msvcrt.getwch() 122 | if z == unichr(13): 123 | return unichr(10) 124 | elif z in (unichr(0), unichr(0x0e)): # functions keys, ignore 125 | msvcrt.getwch() 126 | else: 127 | return z 128 | 129 | def cancel(self): 130 | # CancelIo, CancelSynchronousIo do not seem to work when using 131 | # getwch, so instead, send a key to the window with the console 132 | hwnd = ctypes.windll.kernel32.GetConsoleWindow() 133 | ctypes.windll.user32.PostMessageA(hwnd, 0x100, 0x0d, 0) 134 | 135 | elif os.name == 'posix': 136 | import atexit 137 | import termios 138 | import fcntl 139 | 140 | class Console(ConsoleBase): 141 | def __init__(self): 142 | super(Console, self).__init__() 143 | self.fd = sys.stdin.fileno() 144 | self.old = termios.tcgetattr(self.fd) 145 | atexit.register(self.cleanup) 146 | if sys.version_info < (3, 0): 147 | self.enc_stdin = codecs.getreader(sys.stdin.encoding)(sys.stdin) 148 | else: 149 | self.enc_stdin = sys.stdin 150 | 151 | def setup(self): 152 | new = termios.tcgetattr(self.fd) 153 | new[3] = new[3] & ~termios.ICANON & ~termios.ECHO & ~termios.ISIG 154 | new[6][termios.VMIN] = 1 155 | new[6][termios.VTIME] = 0 156 | termios.tcsetattr(self.fd, termios.TCSANOW, new) 157 | 158 | def getkey(self): 159 | c = self.enc_stdin.read(1) 160 | if c == unichr(0x7f): 161 | c = unichr(8) # map the BS key (which yields DEL) to backspace 162 | return c 163 | 164 | def cancel(self): 165 | fcntl.ioctl(self.fd, termios.TIOCSTI, b'\0') 166 | 167 | def cleanup(self): 168 | termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old) 169 | 170 | else: 171 | raise NotImplementedError( 172 | 'Sorry no implementation for your platform ({}) available.'.format(sys.platform)) 173 | 174 | 175 | # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 176 | 177 | class Transform(object): 178 | """do-nothing: forward all data unchanged""" 179 | def rx(self, text): 180 | """text received from serial port""" 181 | return text 182 | 183 | def tx(self, text): 184 | """text to be sent to serial port""" 185 | return text 186 | 187 | def echo(self, text): 188 | """text to be sent but displayed on console""" 189 | return text 190 | 191 | 192 | class CRLF(Transform): 193 | """ENTER sends CR+LF""" 194 | 195 | def tx(self, text): 196 | return text.replace('\n', '\r\n') 197 | 198 | 199 | class CR(Transform): 200 | """ENTER sends CR""" 201 | 202 | def rx(self, text): 203 | return text.replace('\r', '\n') 204 | 205 | def tx(self, text): 206 | return text.replace('\n', '\r') 207 | 208 | 209 | class LF(Transform): 210 | """ENTER sends LF""" 211 | 212 | 213 | class NoTerminal(Transform): 214 | """remove typical terminal control codes from input""" 215 | 216 | REPLACEMENT_MAP = dict((x, 0x2400 + x) for x in range(32) if unichr(x) not in '\r\n\b\t') 217 | REPLACEMENT_MAP.update( 218 | { 219 | 0x7F: 0x2421, # DEL 220 | 0x9B: 0x2425, # CSI 221 | }) 222 | 223 | def rx(self, text): 224 | return text.translate(self.REPLACEMENT_MAP) 225 | 226 | echo = rx 227 | 228 | 229 | class NoControls(NoTerminal): 230 | """Remove all control codes, incl. CR+LF""" 231 | 232 | REPLACEMENT_MAP = dict((x, 0x2400 + x) for x in range(32)) 233 | REPLACEMENT_MAP.update( 234 | { 235 | 0x20: 0x2423, # visual space 236 | 0x7F: 0x2421, # DEL 237 | 0x9B: 0x2425, # CSI 238 | }) 239 | 240 | 241 | class Printable(Transform): 242 | """Show decimal code for all non-ASCII characters and replace most control codes""" 243 | 244 | def rx(self, text): 245 | r = [] 246 | for c in text: 247 | if ' ' <= c < '\x7f' or c in '\r\n\b\t': 248 | r.append(c) 249 | elif c < ' ': 250 | r.append(unichr(0x2400 + ord(c))) 251 | else: 252 | r.extend(unichr(0x2080 + ord(d) - 48) for d in '{:d}'.format(ord(c))) 253 | r.append(' ') 254 | return ''.join(r) 255 | 256 | echo = rx 257 | 258 | 259 | class Colorize(Transform): 260 | """Apply different colors for received and echo""" 261 | 262 | def __init__(self): 263 | # XXX make it configurable, use colorama? 264 | self.input_color = '\x1b[37m' 265 | self.echo_color = '\x1b[31m' 266 | 267 | def rx(self, text): 268 | return self.input_color + text 269 | 270 | def echo(self, text): 271 | return self.echo_color + text 272 | 273 | 274 | class DebugIO(Transform): 275 | """Print what is sent and received""" 276 | 277 | def rx(self, text): 278 | sys.stderr.write(' [RX:{}] '.format(repr(text))) 279 | sys.stderr.flush() 280 | return text 281 | 282 | def tx(self, text): 283 | sys.stderr.write(' [TX:{}] '.format(repr(text))) 284 | sys.stderr.flush() 285 | return text 286 | 287 | 288 | # other ideas: 289 | # - add date/time for each newline 290 | # - insert newline after: a) timeout b) packet end character 291 | 292 | EOL_TRANSFORMATIONS = { 293 | 'crlf': CRLF, 294 | 'cr': CR, 295 | 'lf': LF, 296 | } 297 | 298 | TRANSFORMATIONS = { 299 | 'direct': Transform, # no transformation 300 | 'default': NoTerminal, 301 | 'nocontrol': NoControls, 302 | 'printable': Printable, 303 | 'colorize': Colorize, 304 | 'debug': DebugIO, 305 | } 306 | 307 | 308 | # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 309 | def ask_for_port(): 310 | """\ 311 | Show a list of ports and ask the user for a choice. To make selection 312 | easier on systems with long device names, also allow the input of an 313 | index. 314 | """ 315 | sys.stderr.write('\n--- Available ports:\n') 316 | ports = [] 317 | for n, (port, desc, hwid) in enumerate(sorted(comports()), 1): 318 | sys.stderr.write('--- {:2}: {:20} {}\n'.format(n, port, desc)) 319 | ports.append(port) 320 | while True: 321 | port = raw_input('--- Enter port index or full name: ') 322 | try: 323 | index = int(port) - 1 324 | if not 0 <= index < len(ports): 325 | sys.stderr.write('--- Invalid index!\n') 326 | continue 327 | except ValueError: 328 | pass 329 | else: 330 | port = ports[index] 331 | return port 332 | 333 | 334 | class Miniterm(object): 335 | """\ 336 | Terminal application. Copy data from serial port to console and vice versa. 337 | Handle special keys from the console to show menu etc. 338 | """ 339 | 340 | def __init__(self, serial_instance, echo=False, eol='crlf', filters=()): 341 | self.console = Console() 342 | self.serial = serial_instance 343 | self.echo = echo 344 | self.raw = False 345 | self.input_encoding = 'UTF-8' 346 | self.output_encoding = 'UTF-8' 347 | self.eol = eol 348 | self.filters = filters 349 | self.update_transformations() 350 | self.exit_character = 0x1d # GS/CTRL+] 351 | self.menu_character = 0x14 # Menu: CTRL+T 352 | self.alive = None 353 | self._reader_alive = None 354 | self.receiver_thread = None 355 | self.rx_decoder = None 356 | self.tx_decoder = None 357 | 358 | def _start_reader(self): 359 | """Start reader thread""" 360 | self._reader_alive = True 361 | # start serial->console thread 362 | self.receiver_thread = threading.Thread(target=self.reader, name='rx') 363 | self.receiver_thread.daemon = True 364 | self.receiver_thread.start() 365 | 366 | def _stop_reader(self): 367 | """Stop reader thread only, wait for clean exit of thread""" 368 | self._reader_alive = False 369 | if hasattr(self.serial, 'cancel_read'): 370 | self.serial.cancel_read() 371 | self.receiver_thread.join() 372 | 373 | def start(self): 374 | """start worker threads""" 375 | self.alive = True 376 | self._start_reader() 377 | # enter console->serial loop 378 | self.transmitter_thread = threading.Thread(target=self.writer, name='tx') 379 | self.transmitter_thread.daemon = True 380 | self.transmitter_thread.start() 381 | self.console.setup() 382 | 383 | def stop(self): 384 | """set flag to stop worker threads""" 385 | self.alive = False 386 | 387 | def join(self, transmit_only=False): 388 | """wait for worker threads to terminate""" 389 | self.transmitter_thread.join() 390 | if not transmit_only: 391 | if hasattr(self.serial, 'cancel_read'): 392 | self.serial.cancel_read() 393 | self.receiver_thread.join() 394 | 395 | def close(self): 396 | self.serial.close() 397 | 398 | def update_transformations(self): 399 | """take list of transformation classes and instantiate them for rx and tx""" 400 | transformations = [EOL_TRANSFORMATIONS[self.eol]] + [TRANSFORMATIONS[f] 401 | for f in self.filters] 402 | self.tx_transformations = [t() for t in transformations] 403 | self.rx_transformations = list(reversed(self.tx_transformations)) 404 | 405 | def set_rx_encoding(self, encoding, errors='replace'): 406 | """set encoding for received data""" 407 | self.input_encoding = encoding 408 | self.rx_decoder = codecs.getincrementaldecoder(encoding)(errors) 409 | 410 | def set_tx_encoding(self, encoding, errors='replace'): 411 | """set encoding for transmitted data""" 412 | self.output_encoding = encoding 413 | self.tx_encoder = codecs.getincrementalencoder(encoding)(errors) 414 | 415 | def dump_port_settings(self): 416 | """Write current settings to sys.stderr""" 417 | sys.stderr.write("\n--- Settings: {p.name} {p.baudrate},{p.bytesize},{p.parity},{p.stopbits}\n".format( 418 | p=self.serial)) 419 | sys.stderr.write('--- RTS: {:8} DTR: {:8} BREAK: {:8}\n'.format( 420 | ('active' if self.serial.rts else 'inactive'), 421 | ('active' if self.serial.dtr else 'inactive'), 422 | ('active' if self.serial.break_condition else 'inactive'))) 423 | try: 424 | sys.stderr.write('--- CTS: {:8} DSR: {:8} RI: {:8} CD: {:8}\n'.format( 425 | ('active' if self.serial.cts else 'inactive'), 426 | ('active' if self.serial.dsr else 'inactive'), 427 | ('active' if self.serial.ri else 'inactive'), 428 | ('active' if self.serial.cd else 'inactive'))) 429 | except serial.SerialException: 430 | # on RFC 2217 ports, it can happen if no modem state notification was 431 | # yet received. ignore this error. 432 | pass 433 | sys.stderr.write('--- software flow control: {}\n'.format('active' if self.serial.xonxoff else 'inactive')) 434 | sys.stderr.write('--- hardware flow control: {}\n'.format('active' if self.serial.rtscts else 'inactive')) 435 | sys.stderr.write('--- serial input encoding: {}\n'.format(self.input_encoding)) 436 | sys.stderr.write('--- serial output encoding: {}\n'.format(self.output_encoding)) 437 | sys.stderr.write('--- EOL: {}\n'.format(self.eol.upper())) 438 | sys.stderr.write('--- filters: {}\n'.format(' '.join(self.filters))) 439 | 440 | def reader(self): 441 | """loop and copy serial->console""" 442 | try: 443 | while self.alive and self._reader_alive: 444 | # read all that is there or wait for one byte 445 | data = self.serial.read(self.serial.in_waiting or 1) 446 | if data: 447 | if self.raw: 448 | self.console.write_bytes(data) 449 | else: 450 | text = self.rx_decoder.decode(data) 451 | for transformation in self.rx_transformations: 452 | text = transformation.rx(text) 453 | self.console.write(text) 454 | except serial.SerialException: 455 | self.alive = False 456 | self.console.cancel() 457 | raise # XXX handle instead of re-raise? 458 | 459 | def writer(self): 460 | """\ 461 | Loop and copy console->serial until self.exit_character character is 462 | found. When self.menu_character is found, interpret the next key 463 | locally. 464 | """ 465 | menu_active = False 466 | try: 467 | while self.alive: 468 | try: 469 | c = self.console.getkey() 470 | except KeyboardInterrupt: 471 | c = '\x03' 472 | if not self.alive: 473 | break 474 | if menu_active: 475 | self.handle_menu_key(c) 476 | menu_active = False 477 | elif c == self.menu_character: 478 | menu_active = True # next char will be for menu 479 | elif c == self.exit_character: 480 | self.stop() # exit app 481 | break 482 | else: 483 | #~ if self.raw: 484 | text = c 485 | for transformation in self.tx_transformations: 486 | text = transformation.tx(text) 487 | self.serial.write(self.tx_encoder.encode(text)) 488 | if self.echo: 489 | echo_text = c 490 | for transformation in self.tx_transformations: 491 | echo_text = transformation.echo(echo_text) 492 | self.console.write(echo_text) 493 | except: 494 | self.alive = False 495 | raise 496 | 497 | def handle_menu_key(self, c): 498 | """Implement a simple menu / settings""" 499 | if c == self.menu_character or c == self.exit_character: 500 | # Menu/exit character again -> send itself 501 | self.serial.write(self.tx_encoder.encode(c)) 502 | if self.echo: 503 | self.console.write(c) 504 | elif c == '\x15': # CTRL+U -> upload file 505 | sys.stderr.write('\n--- File to upload: ') 506 | sys.stderr.flush() 507 | with self.console: 508 | filename = sys.stdin.readline().rstrip('\r\n') 509 | if filename: 510 | try: 511 | with open(filename, 'rb') as f: 512 | sys.stderr.write('--- Sending file {} ---\n'.format(filename)) 513 | while True: 514 | block = f.read(1024) 515 | if not block: 516 | break 517 | self.serial.write(block) 518 | # Wait for output buffer to drain. 519 | self.serial.flush() 520 | sys.stderr.write('.') # Progress indicator. 521 | sys.stderr.write('\n--- File {} sent ---\n'.format(filename)) 522 | except IOError as e: 523 | sys.stderr.write('--- ERROR opening file {}: {} ---\n'.format(filename, e)) 524 | elif c in '\x08hH?': # CTRL+H, h, H, ? -> Show help 525 | sys.stderr.write(self.get_help_text()) 526 | elif c == '\x12': # CTRL+R -> Toggle RTS 527 | self.serial.rts = not self.serial.rts 528 | sys.stderr.write('--- RTS {} ---\n'.format('active' if self.serial.rts else 'inactive')) 529 | elif c == '\x04': # CTRL+D -> Toggle DTR 530 | self.serial.dtr = not self.serial.dtr 531 | sys.stderr.write('--- DTR {} ---\n'.format('active' if self.serial.dtr else 'inactive')) 532 | elif c == '\x02': # CTRL+B -> toggle BREAK condition 533 | self.serial.break_condition = not self.serial.break_condition 534 | sys.stderr.write('--- BREAK {} ---\n'.format('active' if self.serial.break_condition else 'inactive')) 535 | elif c == '\x05': # CTRL+E -> toggle local echo 536 | self.echo = not self.echo 537 | sys.stderr.write('--- local echo {} ---\n'.format('active' if self.echo else 'inactive')) 538 | elif c == '\x06': # CTRL+F -> edit filters 539 | sys.stderr.write('\n--- Available Filters:\n') 540 | sys.stderr.write('\n'.join( 541 | '--- {:<10} = {.__doc__}'.format(k, v) 542 | for k, v in sorted(TRANSFORMATIONS.items()))) 543 | sys.stderr.write('\n--- Enter new filter name(s) [{}]: '.format(' '.join(self.filters))) 544 | with self.console: 545 | new_filters = sys.stdin.readline().lower().split() 546 | if new_filters: 547 | for f in new_filters: 548 | if f not in TRANSFORMATIONS: 549 | sys.stderr.write('--- unknown filter: {}\n'.format(repr(f))) 550 | break 551 | else: 552 | self.filters = new_filters 553 | self.update_transformations() 554 | sys.stderr.write('--- filters: {}\n'.format(' '.join(self.filters))) 555 | elif c == '\x0c': # CTRL+L -> EOL mode 556 | modes = list(EOL_TRANSFORMATIONS) # keys 557 | eol = modes.index(self.eol) + 1 558 | if eol >= len(modes): 559 | eol = 0 560 | self.eol = modes[eol] 561 | sys.stderr.write('--- EOL: {} ---\n'.format(self.eol.upper())) 562 | self.update_transformations() 563 | elif c == '\x01': # CTRL+A -> set encoding 564 | sys.stderr.write('\n--- Enter new encoding name [{}]: '.format(self.input_encoding)) 565 | with self.console: 566 | new_encoding = sys.stdin.readline().strip() 567 | if new_encoding: 568 | try: 569 | codecs.lookup(new_encoding) 570 | except LookupError: 571 | sys.stderr.write('--- invalid encoding name: {}\n'.format(new_encoding)) 572 | else: 573 | self.set_rx_encoding(new_encoding) 574 | self.set_tx_encoding(new_encoding) 575 | sys.stderr.write('--- serial input encoding: {}\n'.format(self.input_encoding)) 576 | sys.stderr.write('--- serial output encoding: {}\n'.format(self.output_encoding)) 577 | elif c == '\x09': # CTRL+I -> info 578 | self.dump_port_settings() 579 | #~ elif c == '\x01': # CTRL+A -> cycle escape mode 580 | #~ elif c == '\x0c': # CTRL+L -> cycle linefeed mode 581 | elif c in 'pP': # P -> change port 582 | with self.console: 583 | try: 584 | port = ask_for_port() 585 | except KeyboardInterrupt: 586 | port = None 587 | if port and port != self.serial.port: 588 | # reader thread needs to be shut down 589 | self._stop_reader() 590 | # save settings 591 | settings = self.serial.getSettingsDict() 592 | try: 593 | new_serial = serial.serial_for_url(port, do_not_open=True) 594 | # restore settings and open 595 | new_serial.applySettingsDict(settings) 596 | new_serial.rts = self.serial.rts 597 | new_serial.dtr = self.serial.dtr 598 | new_serial.open() 599 | new_serial.break_condition = self.serial.break_condition 600 | except Exception as e: 601 | sys.stderr.write('--- ERROR opening new port: {} ---\n'.format(e)) 602 | new_serial.close() 603 | else: 604 | self.serial.close() 605 | self.serial = new_serial 606 | sys.stderr.write('--- Port changed to: {} ---\n'.format(self.serial.port)) 607 | # and restart the reader thread 608 | self._start_reader() 609 | elif c in 'bB': # B -> change baudrate 610 | sys.stderr.write('\n--- Baudrate: ') 611 | sys.stderr.flush() 612 | with self.console: 613 | backup = self.serial.baudrate 614 | try: 615 | self.serial.baudrate = int(sys.stdin.readline().strip()) 616 | except ValueError as e: 617 | sys.stderr.write('--- ERROR setting baudrate: {} ---\n'.format(e)) 618 | self.serial.baudrate = backup 619 | else: 620 | self.dump_port_settings() 621 | elif c == '8': # 8 -> change to 8 bits 622 | self.serial.bytesize = serial.EIGHTBITS 623 | self.dump_port_settings() 624 | elif c == '7': # 7 -> change to 8 bits 625 | self.serial.bytesize = serial.SEVENBITS 626 | self.dump_port_settings() 627 | elif c in 'eE': # E -> change to even parity 628 | self.serial.parity = serial.PARITY_EVEN 629 | self.dump_port_settings() 630 | elif c in 'oO': # O -> change to odd parity 631 | self.serial.parity = serial.PARITY_ODD 632 | self.dump_port_settings() 633 | elif c in 'mM': # M -> change to mark parity 634 | self.serial.parity = serial.PARITY_MARK 635 | self.dump_port_settings() 636 | elif c in 'sS': # S -> change to space parity 637 | self.serial.parity = serial.PARITY_SPACE 638 | self.dump_port_settings() 639 | elif c in 'nN': # N -> change to no parity 640 | self.serial.parity = serial.PARITY_NONE 641 | self.dump_port_settings() 642 | elif c == '1': # 1 -> change to 1 stop bits 643 | self.serial.stopbits = serial.STOPBITS_ONE 644 | self.dump_port_settings() 645 | elif c == '2': # 2 -> change to 2 stop bits 646 | self.serial.stopbits = serial.STOPBITS_TWO 647 | self.dump_port_settings() 648 | elif c == '3': # 3 -> change to 1.5 stop bits 649 | self.serial.stopbits = serial.STOPBITS_ONE_POINT_FIVE 650 | self.dump_port_settings() 651 | elif c in 'xX': # X -> change software flow control 652 | self.serial.xonxoff = (c == 'X') 653 | self.dump_port_settings() 654 | elif c in 'rR': # R -> change hardware flow control 655 | self.serial.rtscts = (c == 'R') 656 | self.dump_port_settings() 657 | else: 658 | sys.stderr.write('--- unknown menu character {} --\n'.format(key_description(c))) 659 | 660 | def get_help_text(self): 661 | """return the help text""" 662 | # help text, starts with blank line! 663 | return """ 664 | --- pySerial ({version}) - miniterm - help 665 | --- 666 | --- {exit:8} Exit program 667 | --- {menu:8} Menu escape key, followed by: 668 | --- Menu keys: 669 | --- {menu:7} Send the menu character itself to remote 670 | --- {exit:7} Send the exit character itself to remote 671 | --- {info:7} Show info 672 | --- {upload:7} Upload file (prompt will be shown) 673 | --- {repr:7} encoding 674 | --- {filter:7} edit filters 675 | --- Toggles: 676 | --- {rts:7} RTS {dtr:7} DTR {brk:7} BREAK 677 | --- {echo:7} echo {eol:7} EOL 678 | --- 679 | --- Port settings ({menu} followed by the following): 680 | --- p change port 681 | --- 7 8 set data bits 682 | --- N E O S M change parity (None, Even, Odd, Space, Mark) 683 | --- 1 2 3 set stop bits (1, 2, 1.5) 684 | --- b change baud rate 685 | --- x X disable/enable software flow control 686 | --- r R disable/enable hardware flow control 687 | """.format(version=getattr(serial, 'VERSION', 'unknown version'), 688 | exit=key_description(self.exit_character), 689 | menu=key_description(self.menu_character), 690 | rts=key_description('\x12'), 691 | dtr=key_description('\x04'), 692 | brk=key_description('\x02'), 693 | echo=key_description('\x05'), 694 | info=key_description('\x09'), 695 | upload=key_description('\x15'), 696 | repr=key_description('\x01'), 697 | filter=key_description('\x06'), 698 | eol=key_description('\x0c')) 699 | 700 | 701 | # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 702 | # default args can be used to override when calling main() from an other script 703 | # e.g to create a miniterm-my-device.py 704 | def main(default_port=None, default_baudrate=9600, default_rts=None, default_dtr=None): 705 | """Command line tool, entry point""" 706 | 707 | import argparse 708 | 709 | parser = argparse.ArgumentParser( 710 | description="Miniterm - A simple terminal program for the serial port.") 711 | 712 | parser.add_argument( 713 | "port", 714 | nargs='?', 715 | help="serial port name ('-' to show port list)", 716 | default=default_port) 717 | 718 | parser.add_argument( 719 | "baudrate", 720 | nargs='?', 721 | type=int, 722 | help="set baud rate, default: %(default)s", 723 | default=default_baudrate) 724 | 725 | group = parser.add_argument_group("port settings") 726 | 727 | group.add_argument( 728 | "--parity", 729 | choices=['N', 'E', 'O', 'S', 'M'], 730 | type=lambda c: c.upper(), 731 | help="set parity, one of {N E O S M}, default: N", 732 | default='N') 733 | 734 | group.add_argument( 735 | "--rtscts", 736 | action="store_true", 737 | help="enable RTS/CTS flow control (default off)", 738 | default=False) 739 | 740 | group.add_argument( 741 | "--xonxoff", 742 | action="store_true", 743 | help="enable software flow control (default off)", 744 | default=False) 745 | 746 | group.add_argument( 747 | "--rts", 748 | type=int, 749 | help="set initial RTS line state (possible values: 0, 1)", 750 | default=default_rts) 751 | 752 | group.add_argument( 753 | "--dtr", 754 | type=int, 755 | help="set initial DTR line state (possible values: 0, 1)", 756 | default=default_dtr) 757 | 758 | group.add_argument( 759 | "--ask", 760 | action="store_true", 761 | help="ask again for port when open fails", 762 | default=False) 763 | 764 | group = parser.add_argument_group("data handling") 765 | 766 | group.add_argument( 767 | "-e", "--echo", 768 | action="store_true", 769 | help="enable local echo (default off)", 770 | default=False) 771 | 772 | group.add_argument( 773 | "--encoding", 774 | dest="serial_port_encoding", 775 | metavar="CODEC", 776 | help="set the encoding for the serial port (e.g. hexlify, Latin1, UTF-8), default: %(default)s", 777 | default='UTF-8') 778 | 779 | group.add_argument( 780 | "-f", "--filter", 781 | action="append", 782 | metavar="NAME", 783 | help="add text transformation", 784 | default=[]) 785 | 786 | group.add_argument( 787 | "--eol", 788 | choices=['CR', 'LF', 'CRLF'], 789 | type=lambda c: c.upper(), 790 | help="end of line mode", 791 | default='CRLF') 792 | 793 | group.add_argument( 794 | "--raw", 795 | action="store_true", 796 | help="Do no apply any encodings/transformations", 797 | default=False) 798 | 799 | group = parser.add_argument_group("hotkeys") 800 | 801 | group.add_argument( 802 | "--exit-char", 803 | type=int, 804 | metavar='NUM', 805 | help="Unicode of special character that is used to exit the application, default: %(default)s", 806 | default=0x1d) # GS/CTRL+] 807 | 808 | group.add_argument( 809 | "--menu-char", 810 | type=int, 811 | metavar='NUM', 812 | help="Unicode code of special character that is used to control miniterm (menu), default: %(default)s", 813 | default=0x14) # Menu: CTRL+T 814 | 815 | group = parser.add_argument_group("diagnostics") 816 | 817 | group.add_argument( 818 | "-q", "--quiet", 819 | action="store_true", 820 | help="suppress non-error messages", 821 | default=False) 822 | 823 | group.add_argument( 824 | "--develop", 825 | action="store_true", 826 | help="show Python traceback on error", 827 | default=False) 828 | 829 | args = parser.parse_args() 830 | 831 | if args.menu_char == args.exit_char: 832 | parser.error('--exit-char can not be the same as --menu-char') 833 | 834 | if args.filter: 835 | if 'help' in args.filter: 836 | sys.stderr.write('Available filters:\n') 837 | sys.stderr.write('\n'.join( 838 | '{:<10} = {.__doc__}'.format(k, v) 839 | for k, v in sorted(TRANSFORMATIONS.items()))) 840 | sys.stderr.write('\n') 841 | sys.exit(1) 842 | filters = args.filter 843 | else: 844 | filters = ['default'] 845 | 846 | while True: 847 | # no port given on command line -> ask user now 848 | if args.port is None or args.port == '-': 849 | try: 850 | args.port = ask_for_port() 851 | except KeyboardInterrupt: 852 | sys.stderr.write('\n') 853 | parser.error('user aborted and port is not given') 854 | else: 855 | if not args.port: 856 | parser.error('port is not given') 857 | try: 858 | serial_instance = serial.serial_for_url( 859 | args.port, 860 | args.baudrate, 861 | parity=args.parity, 862 | rtscts=args.rtscts, 863 | xonxoff=args.xonxoff, 864 | do_not_open=True) 865 | 866 | if not hasattr(serial_instance, 'cancel_read'): 867 | # enable timeout for alive flag polling if cancel_read is not available 868 | serial_instance.timeout = 1 869 | 870 | if args.dtr is not None: 871 | if not args.quiet: 872 | sys.stderr.write('--- forcing DTR {}\n'.format('active' if args.dtr else 'inactive')) 873 | serial_instance.dtr = args.dtr 874 | if args.rts is not None: 875 | if not args.quiet: 876 | sys.stderr.write('--- forcing RTS {}\n'.format('active' if args.rts else 'inactive')) 877 | serial_instance.rts = args.rts 878 | 879 | serial_instance.open() 880 | except serial.SerialException as e: 881 | sys.stderr.write('could not open port {}: {}\n'.format(repr(args.port), e)) 882 | if args.develop: 883 | raise 884 | if not args.ask: 885 | sys.exit(1) 886 | else: 887 | args.port = '-' 888 | else: 889 | break 890 | 891 | miniterm = Miniterm( 892 | serial_instance, 893 | echo=args.echo, 894 | eol=args.eol.lower(), 895 | filters=filters) 896 | miniterm.exit_character = unichr(args.exit_char) 897 | miniterm.menu_character = unichr(args.menu_char) 898 | miniterm.raw = args.raw 899 | miniterm.set_rx_encoding(args.serial_port_encoding) 900 | miniterm.set_tx_encoding(args.serial_port_encoding) 901 | 902 | if not args.quiet: 903 | sys.stderr.write('--- Miniterm on {p.name} {p.baudrate},{p.bytesize},{p.parity},{p.stopbits} ---\n'.format( 904 | p=miniterm.serial)) 905 | sys.stderr.write('--- Quit: {} | Menu: {} | Help: {} followed by {} ---\n'.format( 906 | key_description(miniterm.exit_character), 907 | key_description(miniterm.menu_character), 908 | key_description(miniterm.menu_character), 909 | key_description('\x08'))) 910 | 911 | miniterm.start() 912 | try: 913 | miniterm.join(True) 914 | except KeyboardInterrupt: 915 | pass 916 | if not args.quiet: 917 | sys.stderr.write("\n--- exit ---\n") 918 | miniterm.join() 919 | miniterm.close() 920 | 921 | # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 922 | if __name__ == '__main__': 923 | main() 924 | -------------------------------------------------------------------------------- /air/droneController/mqtt.py: -------------------------------------------------------------------------------- 1 | # Simple demo of of the PCA9685 PWM servo/LED controller library. 2 | # This will move channel 0 from min to max position repeatedly. 3 | # Author: Tony DiCola 4 | # License: Public Domain 5 | from __future__ import division 6 | import time 7 | import paho.mqtt.client as mqtt 8 | import Adafruit_PCA9685 9 | import serial 10 | from lightTelemetry import LightTelemetry 11 | 12 | pwm = Adafruit_PCA9685.PCA9685() 13 | 14 | PIN_THROTTLE=10 15 | PIN_YAW=11 16 | PIN_PITCH=8 17 | PIN_ROLL=9 18 | PIN_AUX1=12 19 | 20 | # Helper function to make setting a servo pulse width simpler. 21 | def set_servo_pulse(channel, pulse): 22 | if pulse<1000: 23 | pulse=1000 24 | elif pulse>2000: 25 | pulse=2000 26 | t=0.2114*(pulse) 27 | pwm.set_pwm(channel, 0, int(t)) 28 | #print t 29 | 30 | def on_connect(client, userdata, flags, rc): 31 | print("Connected to MQTT broker "+str(rc)) 32 | client.subscribe("test") 33 | 34 | # The callback for when a PUBLISH message is received from the server. 35 | def on_message(client, userdata, msg): 36 | #print(msg.topic+" "+str(msg.payload)) 37 | pl=str(msg.payload) 38 | i=pl.index(":") 39 | channel=pl[:i] 40 | value=int(pl[i+1:]) 41 | print (channel,value) 42 | c=-1 43 | if channel=="throttle": 44 | c=PIN_THROTTLE 45 | elif channel=="yaw": 46 | c=PIN_YAW 47 | elif channel=="pitch": 48 | c=PIN_PITCH 49 | elif channel=="roll": 50 | c=PIN_ROLL 51 | if c>=0: 52 | set_servo_pulse(c,value) 53 | if channel=="kill": 54 | pwm.set_pwm(2, 0, 0) 55 | 56 | pwm.set_pwm_freq(50) 57 | set_servo_pulse(PIN_THROTTLE,1000) 58 | 59 | set_servo_pulse(PIN_ROLL,1500) 60 | set_servo_pulse(PIN_PITCH,1500) 61 | set_servo_pulse(PIN_YAW,1500) 62 | set_servo_pulse(PIN_AUX1,1500) 63 | 64 | client = mqtt.Client("", True, None, mqtt.MQTTv31) 65 | client.on_connect = on_connect 66 | client.on_message = on_message 67 | 68 | client.connect("192.168.1.1", 1883, 60) 69 | client.loop_start() 70 | 71 | ser = serial.Serial( 72 | 73 | port='/dev/ttyUSB0', 74 | baudrate = 19200, 75 | parity=serial.PARITY_NONE, 76 | stopbits=serial.STOPBITS_ONE, 77 | bytesize=serial.EIGHTBITS, 78 | timeout=1 79 | ) 80 | 81 | lt=LightTelemetry() 82 | 83 | try: 84 | while True: 85 | 86 | b=ser.read(); 87 | #print "serial",b 88 | if b: 89 | lt.processByte(b) 90 | finally: 91 | pwm.set_pwm(PIN_THROTTLE, 0, 10) 92 | set_servo_pulse(PIN_YAW,1500) 93 | set_servo_pulse(PIN_ROLL,1500) 94 | set_servo_pulse(PIN_PITCH,1500) 95 | #time.sleep(0.01) 96 | 97 | -------------------------------------------------------------------------------- /air/droneController/mqttMultiWii.py: -------------------------------------------------------------------------------- 1 | # Simple demo of of the PCA9685 PWM servo/LED controller library. 2 | # This will move channel 0 from min to max position repeatedly. 3 | # Author: Tony DiCola 4 | # License: Public Domain 5 | from __future__ import division 6 | import time, sys 7 | #import paho.mqtt.client as mqtt 8 | from pyMultiwii import MultiWii 9 | from soundEnvelope import SoundEnvelope, Envelope 10 | sys.path.append('/home/pi/dev/mavlink/') 11 | from pymavlink import mavutil 12 | import pymavlink.dialects.v10.ardupilotmega as mavlink 13 | 14 | 15 | se=SoundEnvelope() 16 | 17 | throttle=1000 18 | yaw=1500 19 | roll=1500 20 | pitch=1500 21 | 22 | def updateRC(): 23 | global throttle,yaw,roll,pitch,board 24 | data = [pitch,roll,throttle,yaw,1000,1000,1000,1000] 25 | board.sendCMD(16,MultiWii.SET_RAW_RC,data) 26 | 27 | 28 | def on_connect(client, userdata, flags, rc): 29 | print("Connected to MQTT broker "+str(rc)) 30 | client.subscribe("test") 31 | 32 | # The callback for when a PUBLISH message is received from the server. 33 | def on_message(client, userdata, msg): 34 | global throttle,yaw,roll,pitch 35 | pl=str(msg.payload) 36 | i=pl.index(":") 37 | channel=pl[:i] 38 | value=int(pl[i+1:]) 39 | print (channel,value) 40 | if channel=="throttle": 41 | throttle=value 42 | elif channel=="yaw": 43 | yaw=value 44 | elif channel=="pitch": 45 | pitch=value 46 | elif channel=="roll": 47 | roll=value 48 | #updateRC() 49 | 50 | 51 | 52 | #client = mqtt.Client("", True, None, mqtt.MQTTv31) 53 | #client.on_connect = on_connect 54 | #client.on_message = on_message 55 | 56 | #client.connect("192.168.1.1", 1883, 60) 57 | #client.loop_start() 58 | 59 | mav = mavutil.mavlink_connection("/dev/ttyS0", baud="57600", autoreconnect = True ) 60 | 61 | board = MultiWii("/dev/ttyUSB0") 62 | try: 63 | board.disarm() 64 | 65 | e3=Envelope(8,48,0,0,5,16,16,127,0,0,-127,127,8) 66 | se.play([[e3,0,25]],pitchOffset=0,speed=1,legato=1.0) 67 | 68 | 69 | 70 | #print "arming..." 71 | #board.arm() 72 | lastHeartbeat=0 73 | 74 | while True: 75 | 76 | now=time.time() 77 | if now-lastHeartbeat>1: 78 | #print "heartbeat" 79 | lastHeartbeat=now 80 | mav.mav.heartbeat_send(0,0,0,0,0) 81 | time.sleep(0.05) 82 | #board.getData(MultiWii.MOTOR) 83 | #time.sleep(0.05) 84 | 85 | board.getData(MultiWii.ANALOG) 86 | #print board.vbat 87 | # def battery_status_send(self, id, battery_function, type, temperature, voltages, current_battery, current_consumed, energy_consumed, battery_remaining, force_mavlink1=False): 88 | millivolt=board.vbat*1000 89 | mav.mav.battery_status_send(0,0,0,0,[millivolt,0,0,0,0,0,0,0,0,0],0,0,0,-1); 90 | 91 | msg = mav.recv_msg() 92 | if msg!=None: 93 | #print msg.get_type() 94 | if msg.get_type()=='RC_CHANNELS_OVERRIDE': 95 | throttle=msg.chan1_raw 96 | yaw=msg.chan2_raw 97 | 98 | time.sleep(0.02) 99 | updateRC() 100 | finally: 101 | #e3=Envelope(4,-4,0,0,4,3,1,127,0,0,-127,127,8) 102 | e3=Envelope(4,48,0,-38,4,0,4,127,0,0,-127,127,8) 103 | se.play([[e3,0,25]],pitchOffset=0,speed=2,legato=1.0) 104 | print "disarming..." 105 | board.disarm() 106 | #time.sleep(1) 107 | 108 | -------------------------------------------------------------------------------- /air/droneController/pidTest.py: -------------------------------------------------------------------------------- 1 | from pyMultiwii import MultiWii 2 | import struct, time 3 | 4 | multiwii="/dev/ttyUSB0" 5 | board = MultiWii(multiwii) 6 | 7 | 8 | pids=board.getData(MultiWii.PID) 9 | time.sleep(0.1) 10 | print pids 11 | board.PIDcoef[1]=40 12 | board.setPID2() 13 | time.sleep(1.0) 14 | pids=board.getData(MultiWii.PID) 15 | pids=board.getData(MultiWii.PID) 16 | ''' 17 | for i in range(0,2): 18 | try: 19 | pids=board.getData(MultiWii.PID) 20 | time.sleep(0.1) 21 | except Error, e: 22 | print e 23 | pass 24 | ''' 25 | print pids 26 | 27 | 28 | ''' 29 | data = [1511,1588,1000,1500,1000,1044,1000,1033] 30 | board.sendCMD(16,MultiWii.SET_RAW_RC,data) 31 | 32 | 33 | dataEncoded=struct.pack('<8H', *data) 34 | board.sendCMDRaw(MultiWii.SET_RAW_RC,dataEncoded) 35 | ''' 36 | 37 | #encoded = struct.pack('<'+'B'*(len(board.PIDcoef)),*board.PIDcoef) 38 | #print encoded 39 | #decoded=struct.unpack('<'+'B'*(len(board.PIDcoef)),encoded) 40 | #print decoded -------------------------------------------------------------------------------- /air/droneController/pyMultiwii.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """multiwii.py: Handles Multiwii Serial Protocol.""" 4 | 5 | __author__ = "Aldo Vargas" 6 | __copyright__ = "Copyright 2017 Altax.net" 7 | 8 | __license__ = "GPL" 9 | __version__ = "1.6" 10 | __maintainer__ = "Aldo Vargas" 11 | __email__ = "alduxvm@gmail.com" 12 | __status__ = "Development" 13 | 14 | 15 | import serial, time, struct 16 | 17 | 18 | class MultiWii: 19 | 20 | """Multiwii Serial Protocol message ID""" 21 | """ notice: just attitude, rc channels and raw imu, set raw rc are implemented at the moment """ 22 | IDENT = 100 23 | STATUS = 101 24 | RAW_IMU = 102 25 | SERVO = 103 26 | MOTOR = 104 27 | RC = 105 28 | RAW_GPS = 106 29 | COMP_GPS = 107 30 | ATTITUDE = 108 31 | ALTITUDE = 109 32 | ANALOG = 110 33 | RC_TUNING = 111 34 | PID = 112 35 | BOX = 113 36 | MISC = 114 37 | MOTOR_PINS = 115 38 | BOXNAMES = 116 39 | PIDNAMES = 117 40 | WP = 118 41 | BOXIDS = 119 42 | RC_RAW_IMU = 121 43 | SET_RAW_RC = 200 44 | SET_RAW_GPS = 201 45 | SET_PID = 202 46 | SET_BOX = 203 47 | SET_RC_TUNING = 204 48 | ACC_CALIBRATION = 205 49 | MAG_CALIBRATION = 206 50 | SET_MISC = 207 51 | RESET_CONF = 208 52 | SET_WP = 209 53 | SWITCH_RC_SERIAL = 210 54 | IS_SERIAL = 211 55 | EEPROM_WRITE = 250 56 | DEBUG = 254 57 | 58 | 59 | """Class initialization""" 60 | def __init__(self, serPort): 61 | 62 | """Global variables of data""" 63 | self.PIDcoef = [] #{'rp':0,'ri':0,'rd':0,'pp':0,'pi':0,'pd':0,'yp':0,'yi':0,'yd':0} 64 | self.rcChannels = {'roll':0,'pitch':0,'yaw':0,'throttle':0,'elapsed':0,'timestamp':0} 65 | self.rawIMU = {'ax':0,'ay':0,'az':0,'gx':0,'gy':0,'gz':0,'mx':0,'my':0,'mz':0,'elapsed':0,'timestamp':0} 66 | self.motor = {'m1':0,'m2':0,'m3':0,'m4':0,'elapsed':0,'timestamp':0} 67 | self.attitude = {'angx':0,'angy':0,'heading':0,'elapsed':0,'timestamp':0} 68 | self.altitude = {'estalt':0,'vario':0,'elapsed':0,'timestamp':0} 69 | self.message = {'angx':0,'angy':0,'heading':0,'roll':0,'pitch':0,'yaw':0,'throttle':0,'elapsed':0,'timestamp':0} 70 | self.gps = {'fix':0,'satellites':0,'lat':0,'lon':0,'altitude':0,'speed':0,'course':0} 71 | self.status = { 'cycleTime':0, 'i2c_errors_count':0, 'sensor':0, 'flag':0, 'currentSet':0, 'unknown1':0, 'unknown2':0 } 72 | self.vbat = 0.0; 73 | self.current = 0.0; 74 | self.temp = (); 75 | self.temp2 = (); 76 | self.elapsed = 0 77 | self.PRINT = 0 78 | 79 | self.ser = serial.Serial() 80 | self.ser.port = serPort 81 | self.ser.baudrate = 115200 82 | self.ser.bytesize = serial.EIGHTBITS 83 | self.ser.parity = serial.PARITY_NONE 84 | self.ser.stopbits = serial.STOPBITS_ONE 85 | self.ser.timeout = 0 86 | self.ser.xonxoff = False 87 | self.ser.rtscts = False 88 | self.ser.dsrdtr = False 89 | self.ser.writeTimeout = 2 90 | 91 | """Time to wait until the board becomes operational""" 92 | wakeup = 2 93 | try: 94 | self.ser.open() 95 | if self.PRINT: 96 | print "Waking up board on "+self.ser.port+"..." 97 | for i in range(1,wakeup): 98 | if self.PRINT: 99 | print wakeup-i 100 | time.sleep(1) 101 | else: 102 | time.sleep(1) 103 | except Exception, error: 104 | print "\n\nError opening "+self.ser.port+" port.\n"+str(error)+"\n\n" 105 | raise error 106 | 107 | 108 | 109 | """Function for sending a command to the board""" 110 | def sendCMD(self, data_length, code, data): 111 | checksum = 0 112 | total_data = ['$', 'M', '<', data_length, code] + data 113 | for i in struct.pack('<2B%dH' % len(data), *total_data[3:len(total_data)]): 114 | checksum = checksum ^ ord(i) 115 | total_data.append(checksum) 116 | try: 117 | b = None 118 | s=struct.pack('<3c2B%dHB' % len(data), *total_data) 119 | b = self.ser.write(s) 120 | except Exception, error: 121 | #print "\n\nError in sendCMD." 122 | #print "("+str(error)+")\n\n" 123 | pass 124 | 125 | """Function for sending a command to the board and receive attitude""" 126 | """ 127 | Modification required on Multiwii firmware to Protocol.cpp in evaluateCommand: 128 | 129 | case MSP_SET_RAW_RC: 130 | s_struct_w((uint8_t*)&rcSerial,16); 131 | rcSerialCount = 50; // 1s transition 132 | s_struct((uint8_t*)&att,6); 133 | break; 134 | 135 | """ 136 | def sendCMDreceiveATT(self, data_length, code, data): 137 | checksum = 0 138 | total_data = ['$', 'M', '<', data_length, code] + data 139 | for i in struct.pack('<2B%dH' % len(data), *total_data[3:len(total_data)]): 140 | checksum = checksum ^ ord(i) 141 | total_data.append(checksum) 142 | try: 143 | start = time.time() 144 | b = None 145 | b = self.ser.write(struct.pack('<3c2B%dHB' % len(data), *total_data)) 146 | while True: 147 | header = self.ser.read() 148 | if header == '$': 149 | header = header+self.ser.read(2) 150 | break 151 | datalength = struct.unpack('1000: 201 | yaw=yaw-250; 202 | #print yaw 203 | self.sendCMD(16,MultiWii.SET_RAW_RC,data) 204 | time.sleep(0.05) 205 | timer = timer + (time.time() - start) 206 | start = time.time() 207 | 208 | 209 | 210 | 211 | 212 | def setPID(self,pd): 213 | nd=[] 214 | for i in np.arange(1,len(pd),2): 215 | nd.append(pd[i]+pd[i+1]*256) 216 | data = pd 217 | print "PID sending:",data 218 | self.sendCMD(30,MultiWii.SET_PID,data) 219 | self.sendCMD(0,MultiWii.EEPROM_WRITE,[]) 220 | 221 | 222 | def sendCMDRaw(self,code,string): 223 | data_length=len(string) 224 | checksum = data_length ^ code 225 | for i in string: 226 | checksum = checksum ^ ord(i) 227 | encoded = struct.pack('<3c2B','$', 'M', '<', data_length, code) 228 | encoded += string 229 | encoded += struct.pack('",cmd 247 | #while self.ser.in_waiting>0: 248 | # self.doRead() 249 | 250 | def flushIn(self): 251 | self.ser.flushInput() 252 | def flushOut(self): 253 | self.ser.flushOutput() 254 | 255 | def doRead(self): 256 | code=None 257 | inwaiting=self.ser.in_waiting 258 | #print "inwaiting",inwaiting 259 | if inwaiting>0: 260 | 261 | now=time.time() 262 | header = self.ser.read() 263 | if header != '$': 264 | #print "header!",header 265 | pass 266 | else: 267 | header = header+self.ser.read(2) 268 | if len(header)!=3: 269 | return 270 | b=self.ser.read() 271 | #print "b=",len(b),len(header) 272 | datalength = struct.unpack('=2: 32 | d=self.p[1]-self.p[0] 33 | dist=d*17150 34 | if dist>400: 35 | dist=-1 36 | return dist 37 | 38 | ''' 39 | sonar=Sonar() 40 | time.sleep(0.5) 41 | while True: 42 | sonar.doTrigger() 43 | time.sleep(0.25) 44 | dist=sonar.getDistance() 45 | time.sleep(0.25) 46 | print dist 47 | ''' -------------------------------------------------------------------------------- /air/droneController/soundEnvelope.py: -------------------------------------------------------------------------------- 1 | import wiringpi 2 | import time 3 | from threading import Thread 4 | 5 | twelthRootOf2=2**(1.0/12) 6 | 7 | class SoundEnvelope: 8 | 9 | thread=None; 10 | threadInterrupted=False 11 | 12 | def __init__(self,clock=76, pin=1): 13 | self.clock=clock 14 | self.pin=pin 15 | 16 | wiringpi.wiringPiSetup() 17 | wiringpi.pinMode(self.pin,wiringpi.OUTPUT) 18 | wiringpi.pinMode(self.pin,wiringpi.PWM_OUTPUT) 19 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_BAL) 20 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_MS) 21 | wiringpi.pwmSetClock(clock) 22 | self.wiringpi=wiringpi 23 | 24 | def getRangeForNote2(self,note): 25 | freq=65.41*(twelthRootOf2**note) 26 | rg=int(19.2e6/self.clock/freq) 27 | return rg 28 | 29 | def getRangeForNote(self, note, subnote): 30 | scaleVal=scale[int(note%12)] 31 | if subnote!=0: 32 | scaleVal2=scale[1+int(note%12)] 33 | scaleVal=scaleVal2*subnote+scaleVal*(1-subnote) 34 | 35 | octave=1+int(note/12) 36 | freq=65.41*(2**octave)*scaleVal 37 | rg=int(19.2e6/clock/freq) 38 | #print note,freq,range, octave, int(note%12) 39 | return rg 40 | 41 | def setSound(self,note,volume): 42 | rg=self.getRangeForNote2(note) 43 | mr=int(rg*0.5*volume) 44 | self.wiringpi.pwmSetRange(rg) 45 | self.wiringpi.pwmWrite(self.pin,mr) 46 | 47 | def playRun(self,list,pitchOffset,speed,legato): 48 | env=None 49 | for sound in list: 50 | if len(sound)==3: 51 | env=sound[2] 52 | duration=int(sound[1]/speed) 53 | pitch=sound[0] 54 | if pitch==None: 55 | for i in range(0,duration): 56 | time.sleep(0.01) 57 | if self.threadInterrupted: 58 | break 59 | else: 60 | env.start((pitch+pitchOffset)*4,duration*legato) 61 | for i in range(0,duration): 62 | env.process(self) 63 | time.sleep(0.01) 64 | if self.threadInterrupted: 65 | break 66 | if self.threadInterrupted: 67 | break 68 | self.setSound(0,0) 69 | 70 | def play(self,list,pitchOffset=0,speed=1,legato=0.8): 71 | if self.thread!=None: 72 | self.threadInterrupted=True 73 | self.thread.join() 74 | self.threadInterrupted=False 75 | 76 | thread = Thread(target=self.playRun, args=(list,pitchOffset,speed,legato,)) 77 | thread.start() 78 | self.thread=thread 79 | 80 | class Envelope: 81 | 82 | def __init__(self,s,pi1,pi2,pi3,pn1,pn2,pn3,aa,ad,asus,ar,ala,ald): 83 | self.s=s 84 | self.pi=(pi1,pi2,pi3) 85 | self.pn=(pn1,pn2,pn3) 86 | self.aa=aa 87 | self.ad=ad 88 | self.asus=asus 89 | self.ar=ar 90 | self.ala=ala 91 | self.ald=ald 92 | 93 | def start(self,pitch,duration): 94 | self.pitch=pitch 95 | self.volume=0.0 96 | self.div=0 97 | self.phase=-1 98 | self.phaseStepCount=0 99 | self.ampPhase=0 100 | self.duration=duration 101 | 102 | def process(self,soundEnvelope): 103 | 104 | self.div=(self.div+1)%self.s 105 | if self.div==0: 106 | while self.phaseStepCount==0: 107 | self.phase=(self.phase+1)%3 108 | self.phaseStepCount=self.pn[self.phase] 109 | self.pitch+=self.pi[self.phase] 110 | self.phaseStepCount=self.phaseStepCount-1 111 | 112 | 113 | if self.pitch>255: 114 | self.pitch=self.pitch-256 115 | elif self.pitch<0: 116 | self.pitch=self.pitch+256 117 | 118 | if self.duration==0: 119 | self.ampPhase=3 120 | else: 121 | self.duration=self.duration-1 122 | 123 | if self.ampPhase==0: #attach 124 | self.volume+=self.aa 125 | if self.volume>=self.ala: 126 | self.ampPhase=1 127 | elif self.ampPhase==1: #decay 128 | self.volume+=self.ad 129 | if self.volume<=self.ald: 130 | self.ampPhase=2 131 | elif self.ampPhase==2: #sustain 132 | self.volume+=self.asus 133 | if self.volume<=0: 134 | self.ampPhase=4 135 | elif self.ampPhase==3: 136 | self.volume+=self.ar 137 | if self.volume<=0: 138 | self.ampPhase=4 139 | 140 | #print self.ampPhase, self.volume 141 | 142 | 143 | if self.volume<=0: 144 | soundEnvelope.setSound(12+self.pitch/4.0,0) 145 | return True 146 | else: 147 | soundEnvelope.setSound(12+self.pitch/4.0,self.volume/128.0) 148 | return False -------------------------------------------------------------------------------- /air/droneController/spiTest.py: -------------------------------------------------------------------------------- 1 | import spidev 2 | import time 3 | 4 | 5 | spi = spidev.SpiDev() 6 | spi.open(0,0) 7 | spi.cshigh=False 8 | spi.loop=False 9 | spi.bits_per_word=8 10 | spi.max_speed_hz=800000 11 | for i in range(0,10): 12 | 13 | spi.xfer2( [0x40] ); 14 | time.sleep(0.001) 15 | spi.xfer2( [0x55] ); 16 | time.sleep(0.001) 17 | 18 | spi.xfer2( [0x11] ); 19 | time.sleep(0.001) 20 | c=spi.readbytes(1) 21 | print c 22 | time.sleep(0.5) 23 | spi.close() -------------------------------------------------------------------------------- /air/droneController/testUART.py: -------------------------------------------------------------------------------- 1 | import time 2 | from MultiUART import MultiUART 3 | 4 | muart=MultiUART(1,64) 5 | muart.setBaud(6) 6 | 7 | #muart=MultiUART(0,64) 8 | #muart.setBaud(4) 9 | 10 | muart.flushRx() 11 | for i in range(1,1000): 12 | c=muart.checkRx() 13 | c2=muart.checkTx() 14 | if c>0: 15 | ch=muart.receiveBytes(c) 16 | print "c=",c,c2,ch 17 | else: 18 | #print "nothing",c,c2 19 | #ch=muart.receiveByte(0) 20 | time.sleep(0.1) 21 | muart.transmitBytes([65,66,67,68,69,70,71,72,73,10,13] ) 22 | muart.cleanup() 23 | -------------------------------------------------------------------------------- /air/droneController/windowfilter.py: -------------------------------------------------------------------------------- 1 | #FIR filter with given size and decay rate 2 | class WindowFilter: 3 | def __init__(self,size,factor): 4 | self.window=[0]*size 5 | self.index=0 6 | self.factor=factor 7 | 8 | def add(self,value): 9 | if self.window[0]==None: 10 | for i in range(0,len(self.window)): 11 | self.window[i]=value 12 | else: 13 | self.index=(self.index+1)%len(self.window) 14 | self.window[self.index]=value 15 | 16 | def get(self): 17 | total=0 18 | m=1 19 | d=0 20 | j=self.index 21 | for i in range(0,len(self.window)): 22 | total+=self.window[j]*m 23 | d=d+m 24 | m=m-self.factor 25 | j=j-1 26 | if j<0: 27 | j=len(self.window)-1 28 | return total/d 29 | 30 | #calculate average over a fixed window size of samples 31 | class WindowAverage: 32 | def __init__(self,size): 33 | self.len=size 34 | self.window=[0]*size 35 | self.index=0 36 | self.total=0 37 | 38 | def add(self,value): 39 | if self.window[0]==None: 40 | for i in range(0,self.len): 41 | self.window[i]=value 42 | self.total=value*self.len 43 | else: 44 | self.index=(self.index+1)%len(self.window) 45 | self.total+=value-self.window[self.index] 46 | self.window[self.index]=value 47 | 48 | def get(self): 49 | return self.total/self.len -------------------------------------------------------------------------------- /air/pyMultiWii/README.md: -------------------------------------------------------------------------------- 1 | ![Altax](https://altax.net/images/altax.png "Altax") 2 | 3 | # pyMultiWii 4 | 5 | 6 | Handles the MultiWii Serial Protocol to send/receive data from boards. 7 | 8 | This is a text based / console, no GUI, it works reading data from the multicopter and/or sending commands from a computer via a serial modem. I use this module for doing different request to my multicopters in order to control them wirelessly via a raspberry pie. 9 | 10 | ## How? 11 | 12 | Just create a MultiWii object that receives the serial port address as parameter and then you can ask for a MSP command by using the function getData, an explicit example looks like this: 13 | 14 | ``` 15 | serialPort = "/dev/tty.usbserial-A101CCVF" 16 | board = MultiWii(serialPort) 17 | while True: 18 | print board.getData(MultiWii.ATTITUDE) 19 | ``` 20 | 21 | With the example above, you will see a stream of data on your terminal. 22 | 23 | [![ScreenShot](http://img.youtube.com/vi/TpcQ-TOuOA0/0.jpg)](https://www.youtube.com/watch?v=TpcQ-TOuOA0) 24 | 25 | ## MultiWii Serial Protocol 26 | 27 | MSP is a protocol designed by the MultiWii community, with the idea to be light, generic, bit wire efficient, secure. The MSP data frames are structured as: 28 | 29 | ``` 30 | $
,,,,$ 31 | ``` 32 | 33 | where: 34 | 35 | * header: the ASCII characters `$\$M$` 36 | * direction: the ASCII character `$<$` if the message goes to the MultiWii board or `$>$` if the message is coming from the board 37 | * size: number of data bytes, binary. Can be zero as in the case of a data request to the board 38 | * command: message id of MSP 39 | * data: values to be sent. UINT16 values are LSB first 40 | * crc: (cyclic redundancy check) checksum, XOR of `$,$` and each data byte into a zero sum 41 | 42 | ### Data Flow 43 | 44 | There is basically three types of messages to interact with a MultiWii board. Those are command, request and response. Command is an incoming message without implicit outgoing response from the board, request is an incoming message with implicit outgoing response while response is the outgoing message resulting from an incoming request. 45 | 46 | If, e.g., the orientation of the board is needed, then a message with type request and ID = 108 must be created and then sent to the board, after being sent, the board will reply with a response. 47 | 48 | ### Performance 49 | 50 | The entire implementation of this module does not include a sleep function, which means that is very fast and efficient, the rate of communication would then depend on the computer and the board capabilities. 51 | 52 | The module is also designed to be extremely simple to use, the next code will request and print (to the host computer) the orientation of the a MultiWii board connected to a USB port: 53 | 54 | ``` 55 | from pyMultiwii import MultiWii 56 | from sys import stdout 57 | 58 | if __name__ == "__main__": 59 | board = MultiWii("/dev/ttyUSB0") 60 | try: 61 | while True: 62 | board.getData(MultiWii.ATTITUDE) 63 | print board.attitude 64 | except Exception,error: 65 | print "Error on Main: "+str(error) 66 | ``` 67 | 68 | This module can achieve communication back and forth of 300hz, this was achieved using a Naze32 (32bits micro-controller) board and a Odroid U3. And around 62.5hz when using a MultiWii AIO 2.0 (8bits micro-controller) board and a Raspberry Pi. 69 | 70 | ## Boards update 71 | 72 | ### 8bit boards 73 | 74 | When using an 8bit MultiWii board, please change the `wakeup` time on the main file at line 84. The old boards need more than 10 seconds to boot up in order to be ready to start asking for data. A safe time would be: 75 | 76 | ``` 77 | """Time to wait until the board becomes operational""" 78 | wakeup = 14 79 | ``` 80 | 81 | ### 32bit boards 82 | 83 | If you're using something similar to a naze32 using either baseflight or cleanflight you will be able to ask for attitude and some other commands, but by default you will not be able to use the MSP_SET_RAW_RC to write pilot commands to the multiwii. In order to do that you need to activate (via the baseflight/cleanflight GUI) the ```SerialRX ``` with the specific type for MSP (MultiWii Serial Protocol). The instructions for doing that on baseflight are: 84 | 85 | - Open the CLI (while on the baseflight configurator) and type: 86 | 87 | ``` 88 | feature SERIALRX 89 | ``` 90 | 91 | and then type the following lines: 92 | 93 | ``` 94 | set serialrx_type=4 95 | ``` 96 | 97 | This will activate "msp" in order to control the multiwii via that protocol. Important: when type=4 is active, standard radio will not work... (at least on the releases I'm using). 98 | 99 | Then you can carefully test my example "test-arm-disarm.py"... You will see the motors spin for 3 seconds. ¡¡BE CAREFUL!! 100 | 101 | ## Example: 102 | 103 | This code has no ```time.sleep()```, so, its very fast and efficient. The output looks like this when asking or ATTITUDE: 104 | 105 | ``` 106 | {'timestamp': 1417432436.878697, 'elapsed': 0.016, 'angx': -26.8, 'angy': -24.8, 'heading': -84.0} 107 | {'timestamp': 1417432436.894663, 'elapsed': 0.016, 'angx': -26.8, 'angy': -24.7, 'heading': -84.0} 108 | {'timestamp': 1417432436.910673, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.8, 'heading': -84.0} 109 | {'timestamp': 1417432436.926812, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.7, 'heading': -84.0} 110 | {'timestamp': 1417432436.942629, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.7, 'heading': -84.0} 111 | {'timestamp': 1417432436.958657, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.6, 'heading': -84.0} 112 | {'timestamp': 1417432436.974627, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.6, 'heading': -84.0} 113 | {'timestamp': 1417432436.990591, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.5, 'heading': -84.0} 114 | {'timestamp': 1417432437.006598, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.5, 'heading': -84.0} 115 | {'timestamp': 1417432437.022676, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.5, 'heading': -84.0} 116 | {'timestamp': 1417432437.038604, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.4, 'heading': -85.0} 117 | {'timestamp': 1417432437.054619, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.4, 'heading': -85.0} 118 | {'timestamp': 1417432437.070593, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.3, 'heading': -85.0} 119 | {'timestamp': 1417432437.086576, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.3, 'heading': -85.0} 120 | {'timestamp': 1417432437.102768, 'elapsed': 0.016, 'angx': -26.7, 'angy': -24.2, 'heading': -85.0} 121 | {'timestamp': 1417432437.118586, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.2, 'heading': -85.0} 122 | {'timestamp': 1417432437.134683, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.2, 'heading': -85.0} 123 | {'timestamp': 1417432437.150524, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.1, 'heading': -85.0} 124 | {'timestamp': 1417432437.166525, 'elapsed': 0.016, 'angx': -26.6, 'angy': -24.1, 'heading': -85.0} 125 | ``` 126 | 127 | Using different devices and newer boards you can achieve greater rates of communication, using an oDroid U3 and a naze32 I have achieved close to 300hz. 128 | 129 | ## Video usages: 130 | 131 | [![Multiwii joystick (naze32)](http://img.youtube.com/vi/XyyfGp-IomE/0.jpg)](http://www.youtube.com/watch?v=XyyfGp-IomE) 132 | 133 | [![Drone Pilot - Position hold controller (raspberry pi + naze32)](http://img.youtube.com/vi/oN2S1qJaQNU/0.jpg)](http://www.youtube.com/watch?v=oN2S1qJaQNU) 134 | 135 | [![Drone Pilot - Trajectory controller (raspberry pi + naze32)](http://img.youtube.com/vi/k6tswW7M_-8/0.jpg)](http://www.youtube.com/watch?v=k6tswW7M_-8) 136 | 137 | 138 | ## Caution 139 | 140 | This code is still somewhat under development, if you found a bug or a improvement, please let me know!! 141 | 142 | ## Why? 143 | 144 | I'm doing systems identification of multicopters being flown by me via a multiwii board. Why do I want to do this? I want to do very precise navigation algorithms using a motion capture system. 145 | 146 | Systems identification is a statistical method to build mathematical models of the multicopters, to have excellent control we require a perfect mathematical model. In order to do a good sysid we require data, lots of data, data coming from the board as well as the pilot and the position in the space (motion capture), so, I require raw imu (accelerometers and gyroscopes), pilot commands (rc channels) and position (x,y,z). 147 | 148 | So far, I have a position controller working with the multiwii board being flow by simulink using data from the motion capture system and just sending rc channels via a 3DR robotics radio (roll, pitch, yaw, throttle), you can see a video about that here [TEGO indoor position control](https://vimeo.com/105761692) 149 | 150 | I works nice... but I want it to work even nicer and better!!! so, we need all the mathematical models and parameters to be as precise as possible, ergo, systems identification required. 151 | 152 | I knew that the 3DR radio was not good enough to send data in a fast way to the ground station... So, I put onboard a Raspberry Pie, this computer ask data to the multwii and also to the motion capture system, and saves it... thats it for now. 153 | 154 | ![MultWii and Raspberry Pie on a quadcopter](http://www.multiwii.com/forum/download/file.php?id=3360&mode=view "MultWii and Raspberry Pie on a quadcopter") 155 | -------------------------------------------------------------------------------- /air/pyMultiWii/pyMultiwii.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """multiwii.py: Handles Multiwii Serial Protocol.""" 4 | 5 | __author__ = "Aldo Vargas" 6 | __copyright__ = "Copyright 2017 Altax.net" 7 | 8 | __license__ = "GPL" 9 | __version__ = "1.6" 10 | __maintainer__ = "Aldo Vargas" 11 | __email__ = "alduxvm@gmail.com" 12 | __status__ = "Development" 13 | 14 | 15 | import serial, time, struct 16 | 17 | 18 | class MultiWii: 19 | 20 | """Multiwii Serial Protocol message ID""" 21 | """ notice: just attitude, rc channels and raw imu, set raw rc are implemented at the moment """ 22 | IDENT = 100 23 | STATUS = 101 24 | RAW_IMU = 102 25 | SERVO = 103 26 | MOTOR = 104 27 | RC = 105 28 | RAW_GPS = 106 29 | COMP_GPS = 107 30 | ATTITUDE = 108 31 | ALTITUDE = 109 32 | ANALOG = 110 33 | RC_TUNING = 111 34 | PID = 112 35 | BOX = 113 36 | MISC = 114 37 | MOTOR_PINS = 115 38 | BOXNAMES = 116 39 | PIDNAMES = 117 40 | WP = 118 41 | BOXIDS = 119 42 | RC_RAW_IMU = 121 43 | SET_RAW_RC = 200 44 | SET_RAW_GPS = 201 45 | SET_PID = 202 46 | SET_BOX = 203 47 | SET_RC_TUNING = 204 48 | ACC_CALIBRATION = 205 49 | MAG_CALIBRATION = 206 50 | SET_MISC = 207 51 | RESET_CONF = 208 52 | SET_WP = 209 53 | SWITCH_RC_SERIAL = 210 54 | IS_SERIAL = 211 55 | DEBUG = 254 56 | 57 | 58 | """Class initialization""" 59 | def __init__(self, serPort): 60 | 61 | """Global variables of data""" 62 | self.PIDcoef = {'rp':0,'ri':0,'rd':0,'pp':0,'pi':0,'pd':0,'yp':0,'yi':0,'yd':0} 63 | self.rcChannels = {'roll':0,'pitch':0,'yaw':0,'throttle':0,'elapsed':0,'timestamp':0} 64 | self.rawIMU = {'ax':0,'ay':0,'az':0,'gx':0,'gy':0,'gz':0,'mx':0,'my':0,'mz':0,'elapsed':0,'timestamp':0} 65 | self.motor = {'m1':0,'m2':0,'m3':0,'m4':0,'elapsed':0,'timestamp':0} 66 | self.attitude = {'angx':0,'angy':0,'heading':0,'elapsed':0,'timestamp':0} 67 | self.altitude = {'estalt':0,'vario':0,'elapsed':0,'timestamp':0} 68 | self.message = {'angx':0,'angy':0,'heading':0,'roll':0,'pitch':0,'yaw':0,'throttle':0,'elapsed':0,'timestamp':0} 69 | self.temp = (); 70 | self.temp2 = (); 71 | self.elapsed = 0 72 | self.PRINT = 1 73 | 74 | self.ser = serial.Serial() 75 | self.ser.port = serPort 76 | self.ser.baudrate = 115200 77 | self.ser.bytesize = serial.EIGHTBITS 78 | self.ser.parity = serial.PARITY_NONE 79 | self.ser.stopbits = serial.STOPBITS_ONE 80 | self.ser.timeout = 0 81 | self.ser.xonxoff = False 82 | self.ser.rtscts = False 83 | self.ser.dsrdtr = False 84 | self.ser.writeTimeout = 2 85 | """Time to wait until the board becomes operational""" 86 | wakeup = 2 87 | try: 88 | self.ser.open() 89 | if self.PRINT: 90 | print "Waking up board on "+self.ser.port+"..." 91 | for i in range(1,wakeup): 92 | if self.PRINT: 93 | print wakeup-i 94 | time.sleep(1) 95 | else: 96 | time.sleep(1) 97 | except Exception, error: 98 | print "\n\nError opening "+self.ser.port+" port.\n"+str(error)+"\n\n" 99 | 100 | """Function for sending a command to the board""" 101 | def sendCMD(self, data_length, code, data): 102 | checksum = 0 103 | total_data = ['$', 'M', '<', data_length, code] + data 104 | for i in struct.pack('<2B%dH' % len(data), *total_data[3:len(total_data)]): 105 | checksum = checksum ^ ord(i) 106 | total_data.append(checksum) 107 | try: 108 | b = None 109 | b = self.ser.write(struct.pack('<3c2B%dHB' % len(data), *total_data)) 110 | except Exception, error: 111 | #print "\n\nError in sendCMD." 112 | #print "("+str(error)+")\n\n" 113 | pass 114 | 115 | """Function for sending a command to the board and receive attitude""" 116 | """ 117 | Modification required on Multiwii firmware to Protocol.cpp in evaluateCommand: 118 | 119 | case MSP_SET_RAW_RC: 120 | s_struct_w((uint8_t*)&rcSerial,16); 121 | rcSerialCount = 50; // 1s transition 122 | s_struct((uint8_t*)&att,6); 123 | break; 124 | 125 | """ 126 | def sendCMDreceiveATT(self, data_length, code, data): 127 | checksum = 0 128 | total_data = ['$', 'M', '<', data_length, code] + data 129 | for i in struct.pack('<2B%dH' % len(data), *total_data[3:len(total_data)]): 130 | checksum = checksum ^ ord(i) 131 | total_data.append(checksum) 132 | try: 133 | start = time.time() 134 | b = None 135 | b = self.ser.write(struct.pack('<3c2B%dHB' % len(data), *total_data)) 136 | while True: 137 | header = self.ser.read() 138 | if header == '$': 139 | header = header+self.ser.read(2) 140 | break 141 | datalength = struct.unpack('1000: 191 | yaw=yaw-100; 192 | #print yaw 193 | self.sendCMD(16,MultiWii.SET_RAW_RC,data) 194 | time.sleep(0.05) 195 | timer = timer + (time.time() - start) 196 | start = time.time() 197 | 198 | 199 | 200 | 201 | 202 | def setPID(self,pd): 203 | nd=[] 204 | for i in np.arange(1,len(pd),2): 205 | nd.append(pd[i]+pd[i+1]*256) 206 | data = pd 207 | print "PID sending:",data 208 | self.sendCMD(30,MultiWii.SET_PID,data) 209 | self.sendCMD(0,MultiWii.EEPROM_WRITE,[]) 210 | 211 | """Function to receive a data packet from the board""" 212 | def getData(self, cmd): 213 | try: 214 | start = time.time() 215 | self.sendCMD(0,cmd,[]) 216 | while True: 217 | header = self.ser.read() 218 | if header == '$': 219 | header = header+self.ser.read(2) 220 | break 221 | datalength = struct.unpack('1: 276 | d=0 277 | for t in temp: 278 | dataPID.append(t%256) 279 | dataPID.append(t/256) 280 | for p in [0,3,6,9]: 281 | dataPID[p]=dataPID[p]/10.0 282 | dataPID[p+1]=dataPID[p+1]/1000.0 283 | self.PIDcoef['rp']= dataPID=[0] 284 | self.PIDcoef['ri']= dataPID=[1] 285 | self.PIDcoef['rd']= dataPID=[2] 286 | self.PIDcoef['pp']= dataPID=[3] 287 | self.PIDcoef['pi']= dataPID=[4] 288 | self.PIDcoef['pd']= dataPID=[5] 289 | self.PIDcoef['yp']= dataPID=[6] 290 | self.PIDcoef['yi']= dataPID=[7] 291 | self.PIDcoef['yd']= dataPID=[8] 292 | return self.PIDcoef 293 | else: 294 | return "No return error!" 295 | except Exception, error: 296 | print error 297 | pass 298 | 299 | """Function to receive a data packet from the board. Note: easier to use on threads""" 300 | def getDataInf(self, cmd): 301 | while True: 302 | try: 303 | start = time.clock() 304 | self.sendCMD(0,cmd,[]) 305 | while True: 306 | header = self.ser.read() 307 | if header == '$': 308 | header = header+self.ser.read(2) 309 | break 310 | datalength = struct.unpack('0.5: 108 | self.trimPitch=0 109 | self.trimRoll=0 110 | self.se.play([[12,16,flatTone]]) 111 | else: 112 | correction=10 113 | if button=="dpad_up": 114 | self.trimPitch+=correction 115 | elif button=="dpad_down": 116 | self.trimPitch-=correction 117 | elif button=="dpad_right": 118 | self.trimRoll+=correction 119 | elif button=="dpad_left": 120 | self.trimRoll-=correction 121 | self.se.play([[36,4,flatTone]]) 122 | 123 | #print "trim correction",self.trimPitch,self.trimRoll 124 | self.setModeDisplay(self.getTrimMessage()) 125 | self.trimPressed=None 126 | 127 | def toggleTiltMode(self): 128 | if self.tiltControl: 129 | self.tiltControl=False 130 | else: 131 | self.tiltControl=True 132 | js=self.joystick 133 | js.poll() 134 | pitchOffset=js.getAxis( 'ry' ) 135 | rollOffset=js.getAxis( 'rx' ) 136 | print "tilt",pitchOffset,rollOffset 137 | 138 | 139 | def onButton(self,button, press): 140 | #print button,press 141 | 142 | if button=="start": 143 | self.onStartButton(press) 144 | elif button.startswith("dpad_"): 145 | self.onPadButton(button,press) 146 | elif button=="cross": 147 | self.hover=press 148 | self.dirtyRC=True 149 | elif button=="select": 150 | self.onSelectButton(press) 151 | elif button=="triangle" and press: 152 | #set altitude diff in cm not metres! 153 | self.mav.command_long_send(0,0,mavlink.MAV_CMD_DO_CHANGE_ALTITUDE,0, 154 | 25,0,0,0,0,0,0,0) 155 | elif button=="square" and press: 156 | #set altitude diff in cm not metres! 157 | self.mav.command_long_send(0,0,mavlink.MAV_CMD_DO_CHANGE_ALTITUDE,0, 158 | -25,0,0,0,0,0,0,0) 159 | elif button=="R2" and press: 160 | self.toggleTiltMode() 161 | else: 162 | print button 163 | 164 | #else: 165 | # print buttonsudo 166 | 167 | def onPIDUpdate(self): 168 | self.mav.command_long_send(0,0,mavlink.MAV_CMD_DO_SET_PARAMETER,0, 169 | self.pidTuneIndex,self.pidTuneValue,0,0,0,0,0,0) 170 | self.setModeDisplay(self.getPidMessage()) 171 | 172 | def onPIDTune(self,button,press): 173 | if press: 174 | if button=="dpad_up": 175 | self.pidTuneIndex=(self.pidTuneIndex+1)%35 176 | print self.pidTuneIndex 177 | self.mav.param_request_read_send(0,0,"",self.pidTuneIndex) 178 | elif button=="dpad_down": 179 | self.pidTuneIndex=(self.pidTuneIndex-1)%35 180 | self.mav.param_request_read_send(0,0,"",self.pidTuneIndex) 181 | elif button=="dpad_right": 182 | self.pidTuneValue+=1 183 | self.onPIDUpdate() 184 | elif button=="dpad_left": 185 | self.pidTuneValue-=1 186 | self.onPIDUpdate() 187 | self.se.play([[36,4,flatTone]]) 188 | 189 | def getPidMessage(self): 190 | return "{:12s}{:4.0f}".format(self.pidTuneMode,self.pidTuneValue); 191 | 192 | def onPIDTuneValueReceived(self,paramId,paramValue): 193 | self.pidTuneMode=paramId 194 | self.pidTuneValue=paramValue 195 | self.setModeDisplay(self.getPidMessage()) 196 | 197 | def doHeartbeat(self,now): 198 | self.mav.heartbeat_send(0,0,0,0,0) 199 | 200 | def doArmCheck(self,now): 201 | sp=self.startPressed 202 | if sp!=None: 203 | d=int(now-sp) 204 | if self.armCheck(): 205 | if d<3: 206 | self.se.play([[36,16,flatTone]]) 207 | elif d==3: 208 | self.se.play([[48,32,flatTone]]) 209 | self.startPressed=None 210 | self.setArmed() 211 | print "armed" 212 | elif self.disarmCheck(): 213 | if d<1: 214 | self.se.play([[36,16,flatTone]]) 215 | elif d==1: 216 | self.se.play([[24,32,flatTone]]) 217 | self.startPressed=None 218 | self.setDisarmed() 219 | print "disarmed" 220 | else: 221 | self.se.play([[12,32,flatTone],[0,64]],legato=0.8,speed=1.5,pitchOffset=0) 222 | 223 | def armCheck(self): 224 | return not self.armed and self.remoteConnection # and self.highLatency.gps_fix_type>0 225 | def disarmCheck(self): 226 | return self.armed 227 | 228 | def setArmed(self): 229 | self.mav.set_mode_send(0,mavlink.MAV_MODE_STABILIZE_ARMED,0) 230 | def setDisarmed(self): 231 | self.mav.set_mode_send(0,mavlink.MAV_MODE_PREFLIGHT,0) 232 | 233 | def doPoll(self,now): 234 | 235 | #print self.remoteSNR,self.localSNR 236 | status="?" 237 | if self.failsafe: 238 | status="F" 239 | elif self.armed: 240 | status="A" 241 | else: 242 | status="0" 243 | 244 | #line1="{:s} {:>4.1f}v {:3.0f} {:2.0f} {:2.0f}".format(status,self.batteryVoltage, self.highLatency.heading/100,self.remoteSNR,self.localSNR); 245 | try: 246 | line1="{:s} {:>4.1f}v{:3.0f} {:4.0f}".format( 247 | status, 248 | self.batteryVoltage, 249 | self.previousAxisValues['throttle'], 250 | self.highLatency.altitude_sp 251 | ) 252 | 253 | self.setLCDMain(line1) 254 | except Exception, error: 255 | print error 256 | 257 | errors=[] 258 | hashcode=0 259 | 260 | if not self.joystick.isReady: 261 | errors.append("Joystick lost"); 262 | hashcode+=1 263 | if now-self.lastRemoteHeartbeat>2: 264 | errors.append("No heartbeat"); 265 | self.remoteConnection=False 266 | hashcode+=2 267 | if self.remoteSNR<10: 268 | errors.append("Low R-SNR {:2.0f}".format(self.remoteSNR)) 269 | hashcode+=3 270 | if self.localSNR<10: 271 | errors.append("Low L-SNR {:2.0f}".format(self.localSNR)) 272 | hashcode+=4 273 | if self.remoteConnection: 274 | if self.batteryVoltage!=None and self.batteryVoltage<10: 275 | errors.append("Bat danger "+str(self.batteryVoltage)+"v") 276 | hashcode+=5 277 | elif self.remoteConnection and self.batteryVoltage!=None and self.batteryVoltage<10.8: 278 | errors.append("Bat low "+str(self.batteryVoltage)+"v") 279 | hashcode+=6 280 | #print self.highLatency 281 | if self.highLatency!=None and self.highLatency.gps_fix_type==0: 282 | errors.append("No GPS Fix {:1.0f}sat".format(self.highLatency.gps_nsat)) 283 | hashcode+=7 284 | if self.remoteError!=None: 285 | errors.append(self.remoteError) 286 | 287 | if len(errors)>0: 288 | if hashcode!=self.lastErrorHash: 289 | self.lastErrorHash=hashcode 290 | self.errorCountDown=1 #10 291 | if self.errorCountDown>0: 292 | self.errorCountDown-=1 293 | self.se.play([[0,25,errorTone]],pitchOffset=0,speed=1,legato=1.0) 294 | print errors 295 | 296 | if self.modeDisplay==None: 297 | self.setLCDStatus(errors[self.errorToggle%len(errors)]) 298 | self.errorToggle=self.errorToggle+1 299 | else: 300 | self.modeDisplay=None 301 | self.lastError=True 302 | elif self.lastError: 303 | if self.modeDisplay==None: 304 | self.setLCDStatus(" "); 305 | else: 306 | self.modeDisplay=None 307 | self.lastError=False 308 | 309 | def processMessage(self,msg,now): 310 | msgType=msg.get_type() 311 | #print msgType 312 | 313 | if msgType=='RADIO_STATUS': 314 | localSignal=(msg.rssi/1.9-127) 315 | localNoise=(msg.noise/1.9-127) 316 | self.localSNR=localSignal-localNoise 317 | remoteSignal=(msg.remrssi/1.9-127) 318 | remoteNoise=(msg.remnoise/1.9-127) 319 | self.remoteSNR=remoteSignal-remoteNoise 320 | 321 | elif msgType=='HEARTBEAT': 322 | self.lastRemoteHeartbeat=now 323 | if not(self.remoteConnection): 324 | self.remoteConnection=True 325 | self.se.play([[60,128,bellTone]]) 326 | 327 | elif msgType=='BATTERY_STATUS': 328 | self.batteryVoltage=msg.voltages[0]/1000.0 329 | #print "battery",self.batteryVoltage 330 | elif msgType=='HIGH_LATENCY': 331 | self.lastRemoteHeartbeat=now 332 | self.highLatency=msg 333 | 334 | #print "mode=",self.highLatency.custom_mode 335 | self.armed=self.highLatency.custom_mode&1>0 336 | self.failsafe=self.highLatency.custom_mode&4096>0 337 | elif msgType=='STATUSTEXT': 338 | if len(msg.text)>0: 339 | self.remoteError=msg.text 340 | else: 341 | self.remoteError=None 342 | elif msgType=='PARAM_VALUE': 343 | #print "param_value", msg.param_id, msg.param_value 344 | self.onPIDTuneValueReceived(msg.param_id,msg.param_value) 345 | #else: 346 | # print msgType, msg 347 | 348 | def updateAxis(self,name,value): 349 | if not(self.previousAxisValues.has_key(name)) or self.previousAxisValues[name]!=value: 350 | self.previousAxisValues[name]=value 351 | self.dirtyRC=True 352 | 353 | def joystickLoop(self): 354 | while not self.threadInterrupted: 355 | js=self.joystick 356 | js.poll() 357 | throttle = -js.getAxis( 'y1' ) 358 | ym=500 359 | ypr=150 360 | if not self.armed: # to allow joystick commands to be sent 361 | ypr=500 362 | 363 | self.updateAxis("throttle",1000+throttle*1000); 364 | #print "throttle",1000+throttle*1000 365 | self.updateAxis("yaw", 1500+ym*js.getAxis('x1') ); 366 | self.updateAxis("pitch", 1500-ypr*js.getAxis('y2') ); 367 | self.updateAxis("roll", 1500+ypr*js.getAxis('x2') ); 368 | 369 | #print "rot",js.getAxis('rx'),js.getAxis('ry') 370 | 371 | def updateJoystickAxes(self,now): 372 | if self.dirtyRC and now-self.lastRCSend>0.05: 373 | self.dirtyRC=False 374 | self.lastRCSend=now 375 | aux1=1000 376 | if self.hover: 377 | aux1=1100 378 | self.mav.rc_channels_override_send(0,0, 379 | self.previousAxisValues['throttle'], 380 | self.previousAxisValues['yaw'], 381 | self.previousAxisValues['pitch']+self.trimPitch, 382 | self.previousAxisValues['roll']+self.trimRoll, 383 | aux1,1000,1000,1000 ) 384 | 385 | def processErrors(self,now): 386 | print "processErrors" 387 | 388 | def loop(self): 389 | 390 | now=time.time() 391 | self.se.play([[0,16,flatTone],[4,16],[7,16],[12,48]],legato=0.9,speed=1.5,pitchOffset=24) 392 | self.lastPoll=now 393 | 394 | while not self.threadInterrupted: 395 | try: 396 | t=time.time(); 397 | #print (t-now) 398 | now=t 399 | 400 | if now-self.lastHeartbeatSent>1: 401 | self.lastHeartbeatSent=now 402 | self.doHeartbeat(now) 403 | self.doArmCheck(now) 404 | 405 | msg = self.mavconnection.recv_msg() 406 | if msg!=None: 407 | self.processMessage(msg,now) 408 | 409 | self.updateJoystickAxes(now) 410 | 411 | if now-self.lastPoll>2: 412 | self.lastPoll=now 413 | self.doPoll(now) 414 | 415 | time.sleep(0.02) 416 | except Exception, err: 417 | print Exception, err 418 | time.sleep(0.1) 419 | 420 | def start(self): 421 | self.threadInterrupted=False 422 | 423 | t = Thread(target=self.loop) 424 | t.setDaemon(True) 425 | t.start() 426 | self.threadMain=t 427 | 428 | t = Thread(target=self.joystickLoop) 429 | t.setDaemon(True) 430 | t.start() 431 | self.threadJoystick=t 432 | 433 | -------------------------------------------------------------------------------- /ground/droneController/hd44780.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | #-------------------------------------- 3 | # ___ ___ _ ____ 4 | # / _ \/ _ \(_) __/__ __ __ 5 | # / , _/ ___/ /\ \/ _ \/ // / 6 | # /_/|_/_/ /_/___/ .__/\_, / 7 | # /_/ /___/ 8 | # 9 | # lcd_16x2.py 10 | # 16x2 LCD Test Script 11 | # 12 | # Author : Matt Hawkins 13 | # Date : 06/04/2015 14 | # 15 | # http://www.raspberrypi-spy.co.uk/ 16 | # 17 | #-------------------------------------- 18 | 19 | # The wiring for the LCD is as follows: 20 | # 1 : GND 21 | # 2 : 5V 22 | # 3 : Contrast (0-5V)* 23 | # 4 : RS (Register Select) 24 | # 5 : R/W (Read Write) - GROUND THIS PIN 25 | # 6 : Enable or Strobe 26 | # 7 : Data Bit 0 - NOT USED 27 | # 8 : Data Bit 1 - NOT USED 28 | # 9 : Data Bit 2 - NOT USED 29 | # 10: Data Bit 3 - NOT USED 30 | # 11: Data Bit 4 31 | # 12: Data Bit 5 32 | # 13: Data Bit 6 33 | # 14: Data Bit 7 34 | # 15: LCD Backlight +5V** 35 | # 16: LCD Backlight GND 36 | 37 | #import 38 | import RPi.GPIO as GPIO 39 | import time 40 | 41 | class hd44780: 42 | 43 | 44 | LCD_RS = 7 45 | LCD_E = 8 46 | LCD_D4 = 17 47 | LCD_D5 = 23 48 | LCD_D6 = 24 49 | LCD_D7 = 25 50 | 51 | # Define some device constants 52 | LCD_WIDTH = 16 # Maximum characters per line 53 | LCD_CHR = True 54 | LCD_CMD = False 55 | 56 | LCD_LINE_1 = 0x80 # LCD RAM address for the 1st line 57 | LCD_LINE_2 = 0xC0 # LCD RAM address for the 2nd line 58 | 59 | LCD_CLEARDISPLAY = 0x01 60 | LCD_RETURNHOME = 0x02 61 | 62 | # Timing constants 63 | E_PULSE = 0.0005 64 | E_DELAY = 0.0005 65 | 66 | # Constructor 67 | def __init__(self): 68 | 69 | GPIO.setwarnings(False) 70 | GPIO.setmode(GPIO.BCM) # Use BCM GPIO numbers 71 | GPIO.setup(self.LCD_E, GPIO.OUT) # E 72 | GPIO.setup(self.LCD_RS, GPIO.OUT) # RS 73 | GPIO.setup(self.LCD_D4, GPIO.OUT) # DB4 74 | GPIO.setup(self.LCD_D5, GPIO.OUT) # DB5 75 | GPIO.setup(self.LCD_D6, GPIO.OUT) # DB6 76 | GPIO.setup(self.LCD_D7, GPIO.OUT) # DB7 77 | 78 | # Initialise display 79 | self.lcd_byte(0x33,self.LCD_CMD) # 110011 Initialise 80 | self.lcd_byte(0x32,self.LCD_CMD) # 110010 Initialise 81 | self.lcd_byte(0x06,self.LCD_CMD) # 000110 Cursor move direction 82 | self.lcd_byte(0x0C,self.LCD_CMD) # 001100 Display On,Cursor Off, Blink Off 83 | self.lcd_byte(0x28,self.LCD_CMD) # 101000 Data length, number of lines, font size 84 | self.lcd_byte(0x01,self.LCD_CMD) # 000001 Clear display 85 | time.sleep(self.E_DELAY) 86 | 87 | def customChar(self, charCode, data): 88 | self.lcd_byte(0x40 + charCode*8, self.LCD_CMD) 89 | for i in data: 90 | self.lcd_byte(i,self.LCD_CHR) 91 | 92 | 93 | def lcd_byte(self, bits, mode): 94 | # Send byte to data pins 95 | # bits = data 96 | # mode = True for character 97 | # False for command 98 | 99 | GPIO.output(self.LCD_RS, mode) # RS 100 | 101 | # High bits 102 | GPIO.output(self.LCD_D4, False) 103 | GPIO.output(self.LCD_D5, False) 104 | GPIO.output(self.LCD_D6, False) 105 | GPIO.output(self.LCD_D7, False) 106 | if bits&0x10==0x10: 107 | GPIO.output(self.LCD_D4, True) 108 | if bits&0x20==0x20: 109 | GPIO.output(self.LCD_D5, True) 110 | if bits&0x40==0x40: 111 | GPIO.output(self.LCD_D6, True) 112 | if bits&0x80==0x80: 113 | GPIO.output(self.LCD_D7, True) 114 | 115 | # Toggle 'Enable' pin 116 | self.lcd_toggle_enable() 117 | 118 | # Low bits 119 | GPIO.output(self.LCD_D4, False) 120 | GPIO.output(self.LCD_D5, False) 121 | GPIO.output(self.LCD_D6, False) 122 | GPIO.output(self.LCD_D7, False) 123 | if bits&0x01==0x01: 124 | GPIO.output(self.LCD_D4, True) 125 | if bits&0x02==0x02: 126 | GPIO.output(self.LCD_D5, True) 127 | if bits&0x04==0x04: 128 | GPIO.output(self.LCD_D6, True) 129 | if bits&0x08==0x08: 130 | GPIO.output(self.LCD_D7, True) 131 | 132 | # Toggle 'Enable' pin 133 | self.lcd_toggle_enable() 134 | 135 | 136 | def lcd_toggle_enable(self): 137 | # Toggle enable 138 | time.sleep(self.E_DELAY) 139 | GPIO.output(self.LCD_E, True) 140 | time.sleep(self.E_PULSE) 141 | GPIO.output(self.LCD_E, False) 142 | time.sleep(self.E_DELAY) 143 | 144 | def line1(self,message): 145 | self.lcd_string(message,self.LCD_LINE_1) 146 | 147 | def line2(self,message): 148 | self.lcd_string(message,self.LCD_LINE_2) 149 | 150 | def lcd_string(self, message,line): 151 | # Send string to display 152 | message = message.ljust(self.LCD_WIDTH," ") 153 | self.lcd_byte(line, self.LCD_CMD) 154 | 155 | for i in range(self.LCD_WIDTH): 156 | self.lcd_byte(ord(message[i]),self.LCD_CHR) 157 | 158 | # clear lcd and set to home 159 | def clear(self): 160 | self.lcd_byte(self.LCD_CLEARDISPLAY,False) 161 | self.lcd_byte(self.LCD_RETURNHOME,False) 162 | 163 | def cleanup(self): 164 | self.lcd_byte(0x01, self.LCD_CMD) 165 | GPIO.cleanup() 166 | -------------------------------------------------------------------------------- /ground/droneController/joystick.py: -------------------------------------------------------------------------------- 1 | # Released by rdb under the Unlicense (unlicense.org) 2 | # Based on information from: 3 | # https://www.kernel.org/doc/Documentation/input/joystick-api.txt 4 | 5 | import os, struct, array 6 | from fcntl import ioctl 7 | 8 | 9 | class Joystick: 10 | 11 | mFile=None 12 | mOnButton=None 13 | mJSDev=None 14 | isReady=False 15 | axis_map = [] 16 | axis_states = {} 17 | button_map = [] 18 | button_states = {} 19 | 20 | def __init__(self, file, onButton): 21 | self.mFile=file 22 | self.mOnButton=onButton 23 | self.openJS() 24 | 25 | def openJS(self): 26 | self.mJSDev = open(self.mFile, 'rb') 27 | 28 | # Get the device name. 29 | #buf = bytearray(63) 30 | buf = array.array('c', ['\0'] * 64) 31 | ioctl(self.mJSDev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len) 32 | js_name = buf.tostring() 33 | print('Device name: %s' % js_name) 34 | 35 | # Get number of axes and buttons. 36 | buf = array.array('B', [0]) 37 | ioctl(self.mJSDev, 0x80016a11, buf) # JSIOCGAXES 38 | num_axes = buf[0] 39 | 40 | buf = array.array('B', [0]) 41 | ioctl(self.mJSDev, 0x80016a12, buf) # JSIOCGBUTTONS 42 | num_buttons = buf[0] 43 | 44 | # Get the axis map. 45 | buf = array.array('B', [0] * 0x40) 46 | ioctl(self.mJSDev, 0x80406a32, buf) # JSIOCGAXMAP 47 | 48 | for axis in buf[:num_axes]: 49 | axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis) 50 | self.axis_map.append(axis_name) 51 | self.axis_states[axis_name] = 0.0 52 | 53 | # Get the button map. 54 | buf = array.array('H', [0] * 200) 55 | ioctl(self.mJSDev, 0x80406a34, buf) # JSIOCGBTNMAP 56 | 57 | for btn in buf[:num_buttons]: 58 | btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn) 59 | self.button_map.append(btn_name) 60 | self.button_states[btn_name] = 0 61 | 62 | print '%d axes found: %s' % (num_axes, ', '.join(self.axis_map)) 63 | #print '%d buttons found: %s' % (num_buttons, ', '.join(self.button_map)) 64 | self.isReady=True 65 | 66 | def poll(self): 67 | try: 68 | evbuf = self.mJSDev.read(8) 69 | #print "js", len(evbuf) 70 | if evbuf: 71 | time, value, type, number = struct.unpack('IhBB', evbuf) 72 | 73 | #if type & 0x80: 74 | # print "(Press PS Button)", 75 | 76 | if type & 0x01: 77 | button = self.button_map[number] 78 | if button: 79 | self.button_states[button] = value 80 | if value: 81 | if self.mOnButton!=None: 82 | self.mOnButton(button,True) 83 | else: 84 | if self.mOnButton!=None: 85 | self.mOnButton(button,False) 86 | 87 | if type & 0x02: 88 | axis = self.axis_map[number] 89 | if axis: 90 | fvalue = value / 32767.0 91 | self.axis_states[axis] = fvalue 92 | print "%s: %.3f" % (axis, fvalue) 93 | if type>2: 94 | print "jstype:",type 95 | except IOError, e: 96 | #lost joystick 97 | self.isReady=False 98 | try: 99 | self.openJS() 100 | except: 101 | pass 102 | 103 | def getAxis(self,name): 104 | try: 105 | return self.axis_states[name] 106 | except: 107 | return 0 108 | 109 | def isReady(self): 110 | return self.isReady 111 | 112 | 113 | 114 | # These constants were borrowed from linux/input.h 115 | axis_names = { 116 | 0x00 : 'x1', 117 | 0x01 : 'y1', 118 | 0x02 : 'x2', 119 | 0x05 : 'y2', 120 | 121 | 0x3d : 'rx', 122 | 0x3c : 'ry', 123 | 124 | #0x3d : 'rx', 125 | #0x3c : 'ry', 126 | } 127 | 128 | button_names = { 129 | 0x12e : 'cross', 130 | 0x12d : 'circle', 131 | 0x12c : 'triangle', 132 | 0x12f : 'square', 133 | 134 | 0x120 : 'select', 135 | 0x2c0 : 'PS', 136 | 0x123 : 'start', 137 | 0x121 : 'thumbL', 138 | 0x122 : 'thumbR', 139 | 140 | 0x127 : 'dpad_left', 141 | 0x125 : 'dpad_right', 142 | 0x126 : 'dpad_down', 143 | 0x124 : 'dpad_up', 144 | 145 | 0x128 : 'L2', 146 | 0x129 : 'R2', 147 | 0x12a : 'L1', 148 | 0x12b : 'R1', 149 | 150 | 151 | } 152 | 153 | 154 | -------------------------------------------------------------------------------- /ground/droneController/js_linux.py: -------------------------------------------------------------------------------- 1 | # Released by rdb under the Unlicense (unlicense.org) 2 | # Based on information from: 3 | # https://www.kernel.org/doc/Documentation/input/joystick-api.txt 4 | 5 | import os, struct, array 6 | from fcntl import ioctl 7 | 8 | # Iterate over the joystick devices. 9 | print('Available devices:') 10 | 11 | for fn in os.listdir('/dev/input'): 12 | if fn.startswith('js'): 13 | print(' /dev/input/%s' % (fn)) 14 | 15 | # We'll store the states here. 16 | axis_states = {} 17 | button_states = {} 18 | 19 | # These constants were borrowed from linux/input.h 20 | axis_names = { 21 | 0x00 : 'x', 22 | 0x01 : 'y', 23 | 0x02 : 'z', 24 | 0x03 : 'rx', 25 | 0x04 : 'ry', 26 | 0x05 : 'rz', 27 | 0x06 : 'trottle', 28 | 0x07 : 'rudder', 29 | 0x08 : 'wheel', 30 | 0x09 : 'gas', 31 | 0x0a : 'brake', 32 | 0x10 : 'hat0x', 33 | 0x11 : 'hat0y', 34 | 0x12 : 'hat1x', 35 | 0x13 : 'hat1y', 36 | 0x14 : 'hat2x', 37 | 0x15 : 'hat2y', 38 | 0x16 : 'hat3x', 39 | 0x17 : 'hat3y', 40 | 0x18 : 'pressure', 41 | 0x19 : 'distance', 42 | 0x1a : 'tilt_x', 43 | 0x1b : 'tilt_y', 44 | 0x1c : 'tool_width', 45 | 0x20 : 'volume', 46 | 0x28 : 'misc', 47 | } 48 | 49 | button_names = { 50 | 0x120 : 'trigger', 51 | 0x121 : 'thumb', 52 | 0x122 : 'thumb2', 53 | 0x123 : 'top', 54 | 0x124 : 'top2', 55 | 0x125 : 'pinkie', 56 | 0x126 : 'base', 57 | 0x127 : 'base2', 58 | 0x128 : 'base3', 59 | 0x129 : 'base4', 60 | 0x12a : 'base5', 61 | 0x12b : 'base6', 62 | 0x12f : 'dead', 63 | 0x130 : 'a', 64 | 0x131 : 'b', 65 | 0x132 : 'c', 66 | 0x133 : 'x', 67 | 0x134 : 'y', 68 | 0x135 : 'z', 69 | 0x136 : 'tl', 70 | 0x137 : 'tr', 71 | 0x138 : 'tl2', 72 | 0x139 : 'tr2', 73 | 0x13a : 'select', 74 | 0x13b : 'start', 75 | 0x13c : 'mode', 76 | 0x13d : 'thumbl', 77 | 0x13e : 'thumbr', 78 | 79 | 0x220 : 'dpad_up', 80 | 0x221 : 'dpad_down', 81 | 0x222 : 'dpad_left', 82 | 0x223 : 'dpad_right', 83 | 84 | # XBox 360 controller uses these codes. 85 | 0x2c0 : 'dpad_left', 86 | 0x2c1 : 'dpad_right', 87 | 0x2c2 : 'dpad_up', 88 | 0x2c3 : 'dpad_down', 89 | } 90 | 91 | axis_map = [] 92 | button_map = [] 93 | 94 | # Open the joystick device. 95 | fn = '/dev/input/js0' 96 | print('Opening %s...' % fn) 97 | jsdev = open(fn, 'rb') 98 | 99 | # Get the device name. 100 | #buf = bytearray(63) 101 | buf = array.array('c', ['\0'] * 64) 102 | ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len) 103 | js_name = buf.tostring() 104 | print('Device name: %s' % js_name) 105 | 106 | # Get number of axes and buttons. 107 | buf = array.array('B', [0]) 108 | ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES 109 | num_axes = buf[0] 110 | 111 | buf = array.array('B', [0]) 112 | ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS 113 | num_buttons = buf[0] 114 | 115 | # Get the axis map. 116 | buf = array.array('B', [0] * 0x40) 117 | ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP 118 | 119 | for axis in buf[:num_axes]: 120 | axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis) 121 | axis_map.append(axis_name) 122 | axis_states[axis_name] = 0.0 123 | 124 | # Get the button map. 125 | buf = array.array('H', [0] * 200) 126 | ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP 127 | 128 | for btn in buf[:num_buttons]: 129 | btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn) 130 | button_map.append(btn_name) 131 | button_states[btn_name] = 0 132 | 133 | print '%d axes found: %s' % (num_axes, ', '.join(axis_map)) 134 | print '%d buttons found: %s' % (num_buttons, ', '.join(button_map)) 135 | 136 | # Main event loop 137 | while True: 138 | evbuf = jsdev.read(8) 139 | if evbuf: 140 | time, value, type, number = struct.unpack('IhBB', evbuf) 141 | 142 | if type & 0x80: 143 | print "(initial)", 144 | 145 | if type & 0x01: 146 | button = button_map[number] 147 | if button: 148 | button_states[button] = value 149 | if value: 150 | print "%s pressed" % (button) 151 | else: 152 | print "%s released" % (button) 153 | 154 | if type & 0x02: 155 | axis = axis_map[number] 156 | if axis: 157 | fvalue = value / 32767.0 158 | axis_states[axis] = fvalue 159 | print "%s: %.3f" % (axis, fvalue) 160 | -------------------------------------------------------------------------------- /ground/droneController/main copy.py: -------------------------------------------------------------------------------- 1 | from joystick import Joystick 2 | from threading import Thread 3 | import sys, time 4 | #import paho.mqtt.client as mqtt 5 | #import paho.mqtt.publish as publish 6 | from soundEnvelope import SoundEnvelope, Envelope 7 | 8 | sys.path.append('/home/pi/dev/mavlink/') 9 | from pymavlink import mavutil 10 | import pymavlink.dialects.v10.ardupilotmega as mavlink 11 | 12 | joystick=None 13 | 14 | def joystickLoop(): 15 | global joystick 16 | while True: 17 | try: 18 | if joystick==None: 19 | joystick=Joystick("/dev/input/js0",onButton) 20 | joystick.poll() 21 | 22 | except: 23 | print "Unexpected error:", sys.exc_info()[0] 24 | joystick=None 25 | time.sleep(1) 26 | 27 | def onButton(button, press): 28 | print button,press 29 | #if button=='circle': 30 | # client.publish("test","kill: 0"); 31 | 32 | previousAxisValues = { 'throttle': 1000, 'yaw': 1500 } 33 | lastRCSend=0 34 | dirtyRC=False 35 | 36 | def updateAxis(name,value): 37 | global previousAxisValues, lastRCSend, dirtyRC 38 | 39 | if not(previousAxisValues.has_key(name)) or previousAxisValues[name]!=value: 40 | previousAxisValues[name]=value 41 | dirtyRC=True 42 | #print "dirty" 43 | #s="{}: {:>6.0f}".format(name,value) 44 | #client.publish("test",s); 45 | #print s 46 | 47 | 48 | def on_connect(client, userdata, flags, rc): 49 | print("Connected with result code "+str(rc)) 50 | 51 | 52 | #client = mqtt.Client("", True, None, mqtt.MQTTv31) 53 | #client.on_connect = on_connect 54 | #client.connect("192.168.1.1", 1883, 60) 55 | #client.loop_start() 56 | 57 | mav = mavutil.mavlink_connection("/dev/ttyUSB0", baud="57600", autoreconnect = True ) 58 | se=SoundEnvelope() 59 | 60 | t = Thread(target=joystickLoop) 61 | t.setDaemon(True) 62 | t.start() 63 | 64 | lastHeartbeat=0 65 | lastRemoteHeartbeat=time.time() 66 | 67 | while True: 68 | 69 | now=time.time() 70 | if now-lastHeartbeat>1: 71 | #print "heartbeat" 72 | lastHeartbeat=now 73 | mav.mav.heartbeat_send(0,0,0,0,0) 74 | 75 | msg = mav.recv_msg() 76 | if msg!=None: 77 | msgType=msg.get_type() 78 | #print msgType 79 | if msgType=='RADIO_STATUS': 80 | localSignal=(msg.rssi/1.9-127) 81 | localNoise=(msg.noise/1.9-127) 82 | localSNR=localSignal-localNoise 83 | remoteSignal=(msg.remrssi/1.9-127) 84 | remoteNoise=(msg.remnoise/1.9-127) 85 | remoteSNR=remoteSignal-remoteNoise 86 | print localSNR, remoteSNR 87 | if now-lastRemoteHeartbeat>2: 88 | e3=Envelope(8,48,0,0,5,16,16,127,0,0,-127,127,8) 89 | se.play([[e3,0,25]],pitchOffset=0,speed=1,legato=1.0) 90 | elif msgType=='HEARTBEAT': 91 | lastRemoteHeartbeat=now 92 | elif msgType=='BATTERY_STATUS': 93 | vbat=msg.voltages[0]/1000.0 94 | print "battery",vbat 95 | 96 | time.sleep(0.02) 97 | js=joystick 98 | if js!=None: 99 | #print js.getAxis('x1'),js.getAxis('y1') 100 | throttle = -joystick.getAxis( 'y1' ) 101 | ym=0 102 | if throttle<0: 103 | throttle=0 104 | ym=500 105 | updateAxis("throttle",1000+throttle*1000); 106 | updateAxis("yaw", 1500+ym*js.getAxis('x1') ); 107 | updateAxis("pitch", 1500+100*js.getAxis('y2') ); 108 | updateAxis("roll", 1500+100*js.getAxis('x2') ); 109 | 110 | now=time.time() 111 | if dirtyRC and now-lastRCSend>0.05: 112 | #print "send" 113 | dirtyRC=False 114 | lastRCSend=now 115 | mav.mav.rc_channels_override_send(0,0,previousAxisValues['throttle'],previousAxisValues['yaw'],1003,1004,1005,1006,1007,1008) 116 | 117 | -------------------------------------------------------------------------------- /ground/droneController/main.py: -------------------------------------------------------------------------------- 1 | from GroundMainLoop import GroundMainLoop 2 | import sys, time 3 | #from setRadio import SetRadio 4 | 5 | #SetRadio("/dev/ttyUSB0") 6 | 7 | loop=GroundMainLoop( 8 | joystick="/dev/input/js0", 9 | radio="/dev/ttyUSB0" ) 10 | loop.start() 11 | 12 | while True: 13 | time.sleep(1) 14 | -------------------------------------------------------------------------------- /ground/droneController/mainMQTT.py: -------------------------------------------------------------------------------- 1 | from joystick import Joystick 2 | from threading import Thread 3 | import sys, time 4 | import paho.mqtt.client as mqtt 5 | import paho.mqtt.publish as publish 6 | 7 | joystick=None 8 | 9 | def joystickLoop(): 10 | global joystick 11 | while True: 12 | try: 13 | if joystick==None: 14 | joystick=Joystick("/dev/input/js0",onButton) 15 | joystick.poll() 16 | 17 | except: 18 | print "Unexpected error:", sys.exc_info()[0] 19 | joystick=None 20 | time.sleep(1) 21 | 22 | def onButton(button, press): 23 | print button,press 24 | if button=='circle': 25 | client.publish("test","kill: 0"); 26 | 27 | previousAxisValues = { } 28 | def updateAxis(name,value): 29 | global previousAxisValues 30 | 31 | if not(previousAxisValues.has_key(name)) or previousAxisValues[name]!=value: 32 | previousAxisValues[name]=value 33 | s="{}: {:>6.0f}".format(name,value) 34 | client.publish("test",s); 35 | print s 36 | 37 | 38 | def on_connect(client, userdata, flags, rc): 39 | print("Connected with result code "+str(rc)) 40 | 41 | 42 | client = mqtt.Client("", True, None, mqtt.MQTTv31) 43 | client.on_connect = on_connect 44 | client.connect("192.168.1.1", 1883, 60) 45 | client.loop_start() 46 | 47 | t = Thread(target=joystickLoop) 48 | t.setDaemon(True) 49 | t.start() 50 | 51 | 52 | while True: 53 | 54 | 55 | time.sleep(0.05) 56 | js=joystick 57 | if js!=None: 58 | #print js.getAxis('x1'),js.getAxis('y1') 59 | throttle = -joystick.getAxis( 'y1' ) 60 | ym=0 61 | if throttle<0: 62 | throttle=0 63 | ym=500 64 | updateAxis("throttle",1000+throttle*1000); 65 | updateAxis("yaw", 1500+ym*js.getAxis('x1') ); 66 | updateAxis("pitch", 1500+100*js.getAxis('y2') ); 67 | updateAxis("roll", 1500+100*js.getAxis('x2') ); 68 | 69 | -------------------------------------------------------------------------------- /ground/droneController/setRadio.py: -------------------------------------------------------------------------------- 1 | import serial, time,sys 2 | 3 | class SetRadio: 4 | 5 | def __init__(self,serPort): 6 | self.ser = serial.Serial() 7 | self.ser.port = serPort 8 | self.ser.baudrate = 57600 9 | self.ser.bytesize = serial.EIGHTBITS 10 | self.ser.parity = serial.PARITY_NONE 11 | self.ser.stopbits = serial.STOPBITS_ONE 12 | self.ser.timeout = 0 13 | self.ser.xonxoff = False 14 | self.ser.rtscts = False 15 | self.ser.dsrdtr = False 16 | self.ser.writeTimeout = 2 17 | self.ser.timeout=0.5 18 | 19 | self.ser.open() 20 | self.ser.flushInput() 21 | self.ser.flushOutput() 22 | self.ser.write("ATO\n\r") 23 | time.sleep(1) 24 | self.ser.write("+++") 25 | time.sleep(2) 26 | self.ser.write("ATS4=14\n\r") # 25mW 27 | time.sleep(0.1) 28 | self.ser.write("ATS11=100\n\r") # 100% duty 29 | time.sleep(0.1) 30 | self.ser.write("ATS6=1\n\r") # mavlink with RC_OVERRIDE 31 | time.sleep(0.1) 32 | self.ser.write("ATS2=64\n\r") #64kbps 33 | time.sleep(0.1) 34 | self.ser.write("ATI5\n\r") 35 | time.sleep(0.5) 36 | self.ser.write("AT&W\n\r") 37 | time.sleep(1.0) 38 | try: 39 | while True: 40 | c=self.ser.read(); 41 | if len(c)==0: 42 | break; 43 | #sys.stdout.write('.') 44 | sys.stdout.write(c) 45 | finally: 46 | 47 | self.ser.write("ATZ\n\r") 48 | time.sleep(0.5) 49 | #self.ser.write("ATO\n\r") 50 | #time.sleep(0.5) 51 | self.ser.close() 52 | 53 | sr=SetRadio("/dev/ttyUSB0") -------------------------------------------------------------------------------- /ground/droneController/soundEnvelope.py: -------------------------------------------------------------------------------- 1 | import wiringpi 2 | import time 3 | from threading import Thread 4 | 5 | twelthRootOf2=2**(1.0/12) 6 | 7 | class SoundEnvelope: 8 | 9 | thread=None; 10 | threadInterrupted=False 11 | 12 | def __init__(self,clock=76, pin=1): 13 | self.clock=clock 14 | self.pin=pin 15 | 16 | wiringpi.wiringPiSetup() 17 | wiringpi.pinMode(self.pin,wiringpi.OUTPUT) 18 | wiringpi.pinMode(self.pin,wiringpi.PWM_OUTPUT) 19 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_BAL) 20 | wiringpi.pwmSetMode(wiringpi.PWM_MODE_MS) 21 | wiringpi.pwmSetClock(clock) 22 | self.wiringpi=wiringpi 23 | 24 | def getRangeForNote2(self,note): 25 | freq=65.41*(twelthRootOf2**note) 26 | rg=int(19.2e6/self.clock/freq) 27 | return rg 28 | 29 | def getRangeForNote(self, note, subnote): 30 | scaleVal=scale[int(note%12)] 31 | if subnote!=0: 32 | scaleVal2=scale[1+int(note%12)] 33 | scaleVal=scaleVal2*subnote+scaleVal*(1-subnote) 34 | 35 | octave=1+int(note/12) 36 | freq=65.41*(2**octave)*scaleVal 37 | rg=int(19.2e6/clock/freq) 38 | #print note,freq,range, octave, int(note%12) 39 | return rg 40 | 41 | def setSound(self,note,volume): 42 | rg=self.getRangeForNote2(note) 43 | mr=int(rg*0.5*volume) 44 | self.wiringpi.pwmSetRange(rg) 45 | self.wiringpi.pwmWrite(self.pin,mr) 46 | 47 | def playRun(self,list,pitchOffset,speed,legato): 48 | env=None 49 | for sound in list: 50 | if len(sound)==3: 51 | env=sound[2] 52 | duration=int(sound[1]/speed) 53 | pitch=sound[0] 54 | if pitch==None: 55 | for i in range(0,duration): 56 | time.sleep(0.01) 57 | if self.threadInterrupted: 58 | break 59 | else: 60 | env.start((pitch+pitchOffset)*4,duration*legato) 61 | for i in range(0,duration): 62 | env.process(self) 63 | time.sleep(0.01) 64 | if self.threadInterrupted: 65 | break 66 | if self.threadInterrupted: 67 | break 68 | self.setSound(0,0) 69 | 70 | def play(self,list,pitchOffset=0,speed=1,legato=0.8): 71 | if self.thread!=None: 72 | self.threadInterrupted=True 73 | self.thread.join() 74 | self.threadInterrupted=False 75 | 76 | thread = Thread(target=self.playRun, args=(list,pitchOffset,speed,legato,)) 77 | thread.start() 78 | self.thread=thread 79 | 80 | class Envelope: 81 | 82 | def __init__(self,s,pi1,pi2,pi3,pn1,pn2,pn3,aa,ad,asus,ar,ala,ald): 83 | self.s=s 84 | self.pi=(pi1,pi2,pi3) 85 | self.pn=(pn1,pn2,pn3) 86 | self.aa=aa 87 | self.ad=ad 88 | self.asus=asus 89 | self.ar=ar 90 | self.ala=ala 91 | self.ald=ald 92 | 93 | def start(self,pitch,duration): 94 | self.pitch=pitch 95 | self.volume=0.0 96 | self.div=0 97 | self.phase=-1 98 | self.phaseStepCount=0 99 | self.ampPhase=0 100 | self.duration=duration 101 | 102 | def process(self,soundEnvelope): 103 | 104 | self.div=(self.div+1)%self.s 105 | if self.div==0: 106 | while self.phaseStepCount==0: 107 | self.phase=(self.phase+1)%3 108 | self.phaseStepCount=self.pn[self.phase] 109 | self.pitch+=self.pi[self.phase] 110 | self.phaseStepCount=self.phaseStepCount-1 111 | 112 | 113 | if self.pitch>255: 114 | self.pitch=self.pitch-256 115 | elif self.pitch<0: 116 | self.pitch=self.pitch+256 117 | 118 | if self.duration==0: 119 | self.ampPhase=3 120 | else: 121 | self.duration=self.duration-1 122 | 123 | if self.ampPhase==0: #attach 124 | self.volume+=self.aa 125 | if self.volume>=self.ala: 126 | self.ampPhase=1 127 | elif self.ampPhase==1: #decay 128 | self.volume+=self.ad 129 | if self.volume<=self.ald: 130 | self.ampPhase=2 131 | elif self.ampPhase==2: #sustain 132 | self.volume+=self.asus 133 | if self.volume<=0: 134 | self.ampPhase=4 135 | elif self.ampPhase==3: 136 | self.volume+=self.ar 137 | if self.volume<=0: 138 | self.ampPhase=4 139 | 140 | #print self.ampPhase, self.volume 141 | 142 | 143 | if self.volume<=0: 144 | soundEnvelope.setSound(12+self.pitch/4.0,0) 145 | return True 146 | else: 147 | soundEnvelope.setSound(12+self.pitch/4.0,self.volume/128.0) 148 | return False --------------------------------------------------------------------------------