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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
|
/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
* ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
* ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
* OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
Config cfg;
Gyroscope gyroscope;
Accelerometer accelerometer;
Compass compass;
Barometer barometer;
MotorController motorCtrl;
Receiver receiver;
CopterModel model;
FlightControl flightCtrl;
Kinematics kinematics;
bool loop() {
try {
/* get sensor data */
double gx, gy, gz, ax, ay, az;
int16 mx, my, mz;
gyroscope.get_data(out gx, out gy, out gz);
accelerometer.get_data(out ax, out ay, out az);
compass.get_data(out mx, out my, out mz);
/* calculate kinematics (TODO: calculate time diff) */
kinematics.update(gx, gy, gz, ax, ay, az, mx, my, mz, 0.1);
/* update motors */
flightCtrl.process();
} catch(Error e) {
stderr.printf("%s\n", e.message);
}
return true;
}
int main(string[] args) {
Environment.set_prgname("microcopterd");
try {
cfg = new Config();
Log.init();
gyroscope = cfg.get_gyroscope();
accelerometer = cfg.get_accelerometer();
compass = cfg.get_compass();
barometer = cfg.get_barometer();
motorCtrl = cfg.get_motor_controller();
model = cfg.get_model();
receiver = cfg.get_receiver();
motorCtrl.enable();
receiver.set_speed.connect((id, speed) => {
motorCtrl.set_single(id, speed);
});
kinematics = new Kinematics(0, 0); // TODO: Compass
flightCtrl = new FlightControl();
Timeout.add(10, loop, Priority.HIGH);
} catch(Error e) {
stderr.printf("%s\n", e.message);
return 1;
}
new MainLoop(null, false).run();
return 0;
}
|