root / quad1 / Documentation / Architecture / pseudoCode / pseudoCode.pde @ 9240aaa3
History | View | Annotate | Download (3.41 KB)
1 |
void setup() { |
---|---|
2 |
flightCommmand.init(type, frequency); //type = PWM, serial |
3 |
motorCommand.init(config, type, frequency); //config = +,X,octo type = PWM,I2C |
4 |
attitudeSensor.init(frequency); |
5 |
cameraStabilization.init(frequency); |
6 |
GPS.init(frequency); |
7 |
videoDisplay.init(frequency); |
8 |
lightPattern.init(frequency); |
9 |
batteryMonitor.init(frequency); |
10 |
remoteCommand.init(frequency); |
11 |
telemetry.init(frequency); |
12 |
|
13 |
filter.init(param); |
14 |
} |
15 |
|
16 |
void loop() { |
17 |
if (flightCommand.process()) { |
18 |
rawCommand.throttle = flightCommand.read(THROTTLE); |
19 |
rawCommand.roll = flightCommand.read(ROLL); |
20 |
rawCommand.pitch = flightCommand.read(PITCH); |
21 |
rawCommand.yaw = flightCommand.read(YAW); |
22 |
command = filter(rawCommand); |
23 |
switch(flightCommand.check()) { |
24 |
case CALIBRATE: |
25 |
angleSensor.calibrate(); |
26 |
rateSensor.calibrate(); |
27 |
altitudeSensor.calibrate(); |
28 |
break; |
29 |
case ARM: |
30 |
motor.arm(); |
31 |
break; |
32 |
case DISARM: |
33 |
motor.disarm(); |
34 |
break; |
35 |
default: |
36 |
break; |
37 |
} |
38 |
} |
39 |
|
40 |
if (GPS.process()) { |
41 |
attitude.longitude = GPS.read(LONGITUDE); |
42 |
attitude.latitude = GPS.read(LATITUDE); |
43 |
attitude.altitudeGPS = GPS.read(ALTITUDE); |
44 |
} |
45 |
|
46 |
if (attitudeSensor.process()) { |
47 |
rawAttitude.rate = attitudeSensor.read(RATE); |
48 |
rawAattitude.angle = attitudeSensor.read(ANGLE); |
49 |
rawAattitude.altitudeSensor = attitudeSensor.read(ALTITUDE); |
50 |
rawAattitude.heading = attitudeSensor.read(HEADING); |
51 |
|
52 |
attitude = filter(rawAttitude); |
53 |
direction = flightControl.calculateAttitude(command, attitude); |
54 |
} |
55 |
|
56 |
if (motorCommand.process()) { |
57 |
motorCommand.throttle(direction); |
58 |
switch(motorCommand.type()) { |
59 |
case: PLUS |
60 |
motorCommand.write(FRONT, direction); |
61 |
motorCommand.write(REAR, direction); |
62 |
motorCommand.write(LEFT, direction); |
63 |
motorCommand.write(RIGHT, direction); |
64 |
break; |
65 |
case: X |
66 |
motorCommand.write(FRONTLEFT, direction); |
67 |
motorCommand.write(FRONTRIGHT, direction); |
68 |
motorCommand.write(REARLEFT, direction); |
69 |
motorCommand.write(REARRIGHT, direction); |
70 |
break; |
71 |
case: OCTO |
72 |
motorCommand.write(FRONT, direction); |
73 |
motorCommand.write(REAR, direction); |
74 |
motorCommand.write(LEFT, direction); |
75 |
motorCommand.write(RIGHT, direction); |
76 |
motorCommand.write(FRONTLEFT, direction); |
77 |
motorCommand.write(FRONTRIGHT, direction); |
78 |
motorCommand.write(REARLEFT, direction); |
79 |
motorCommand.write(REARRIGHT, direction); |
80 |
break; |
81 |
case: HEXA |
82 |
motorCommand.write(FRONTLEFT, direction); |
83 |
motorCommand.write(LEFT, direction); |
84 |
motorCommand.write(REARLEFT, direction); |
85 |
motorCommand.write(FRONTRIGHT, direction); |
86 |
motorCommand.write(RIGHT, direction); |
87 |
motorCommand.write(REARRIGHT, direction); |
88 |
default: |
89 |
break; |
90 |
} |
91 |
} |
92 |
|
93 |
if (cameraStabilization.process()) { |
94 |
cameraStabilization.write(ROLL, attitude.roll); |
95 |
cameraStabilization.write(PITCH, attitude.pitch); |
96 |
} |
97 |
|
98 |
if (batteryMonitor.process()) { |
99 |
batteryState = batteryMonitor.read(); |
100 |
batteryMonitor.alarm(batteryState); |
101 |
} |
102 |
|
103 |
if (videoDisplay.process()) { |
104 |
videoDisplay.write(attitude, batteryState); |
105 |
} |
106 |
|
107 |
if (lightPattern.process()) { |
108 |
lightPattern.write(); |
109 |
} |
110 |
|
111 |
if (remoteCommand.process()) { |
112 |
cmd = remoteCommand.read(); |
113 |
remoteCommand.execute(cmd); |
114 |
} |
115 |
|
116 |
if (telemetry.process()) { |
117 |
telemetry.write(); |
118 |
} |
119 |
} |