FilesExpand file tree

 master

/

SonarSensor.py

Copy path

Blame

More file actions

Blame

More file actions

Latest commit

 

History

History

History

50 lines (38 loc) · 1.19 KB

 master

/

SonarSensor.py

Copy path

Top

File metadata and controls

• Code
• Blame

50 lines (38 loc) · 1.19 KB

Raw

Copy raw file

Download raw file

Open symbols panel

Edit and raw actions

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

import math

class SonarAltimeter:

def __init__(self):

self.pitch = 0.0

self.roll = 0.0

def set_attitude(self,attitude):

self.pitch, self.roll = attitude

self.pitch = math.radians(self.pitch)

self.roll = math.radians(self.roll)

def print_attitude(self):

print (math.degrees(self.pitch),math.degrees(self.roll))

def set_range(self,sonar_range):

self.range = sonar_range

def get_altitude(self):

return math.cos(self.pitch)*math.cos(self.roll)*self.range

if __name__ == "__main__":

import pylab

from numpy import *

altitude1 = []

altitude2 = []

a = SonarAltimeter()

a.set_range(10.0)

for angle in range(0,90):

a.set_attitude((angle,0.0))

#a.print_attitude()

altitude1.append(a.get_altitude())

a.set_range(11.0)

for angle in range(0,90):

a.set_attitude((angle,0.0))

#a.print_attitude()

altitude2.append(a.get_altitude())

alt = array(altitude1)

alt2 = array(altitude2)

diff = alt2 - alt

pylab.plot(alt,'b*-')

pylab.plot(alt2,'g*-')

pylab.plot(diff,'r-')

pylab.show()