1 | /* Calculation of acceleration in koordinates (6 degrees of freedom)
|
2 | * 10.12.15
|
3 | *
|
4 | * used prefixes:
|
5 | * s_ struct definitions
|
6 | * g_ global variables
|
7 | * d_ preprocessor definitions
|
8 | */
|
9 |
|
10 | //includes
|
11 | #include <stdio.h>
|
12 | #include <avr/io.h>
|
13 | #include <avr/interrupt.h>
|
14 | //definitions
|
15 | #define d_filterlength 10
|
16 | //custom datatypes
|
17 | struct s_cinematic{
|
18 | int16_t translation_x1;
|
19 | int16_t translation_x2;
|
20 | int16_t translation_x3;
|
21 | int16_t rotation_x1;
|
22 | int16_t rotation_x2;
|
23 | int16_t rotation_x3;
|
24 | };
|
25 | //global variables
|
26 | //contains calculated values of position
|
27 | struct s_cinematic g_position;
|
28 | //contains calculated values of velocity
|
29 | struct s_cinematic g_velocity;
|
30 | //contains acceleration values, read from gyrosensor
|
31 | volatile struct s_cinematic g_acceleration[d_filterlength];
|
32 | volatile int8_t g_acceleration_pointer;
|
33 | //function prototypes
|
34 | void readgyro();
|
35 | struct s_cinematic kalman();
|
36 | void integrate_cinematic(struct s_cinematic input,struct s_cinematic *output, int8_t mul_translation, int8_t mul_rotation);
|
37 |
|
38 | void setup() {
|
39 | ///configure Timer2 (8-bit) system clock
|
40 | //TOV on OC2A >> WGM22 WGM21 WGM20
|
41 | TCCR2A = (1<<WGM20)|(1<<WGM21);
|
42 | //system tick = 100u >> 2000clk/8=250 >> CS21
|
43 | TCCR2B = (1<<WGM22)|(1<<CS21);
|
44 | OCR2A = 249;
|
45 | //TOV enable
|
46 | TIMSK2 = (1<<TOIE2);
|
47 | sei();
|
48 | }
|
49 |
|
50 | //interrupt 100us
|
51 | ISR(TIMER2_OVF_vect){
|
52 | //read new values
|
53 | readgyro();
|
54 | //filter values
|
55 | struct s_cinematic temp;
|
56 | temp = kalman();
|
57 | //integrate acceleration >> velocity
|
58 | integrate_cinematic(temp, &g_velocity, 1, 1);
|
59 | //integrate velocity >> position
|
60 | integrate_cinematic(g_velocity, &g_position, 1, 1);
|
61 | return;
|
62 | }
|
63 |
|
64 | void loop() {
|
65 |
|
66 | }
|
67 |
|
68 | //functions
|
69 | void readgyro(){
|
70 | //read from gyro
|
71 | struct s_cinematic temp;
|
72 | //...
|
73 | //overtake new values
|
74 | ++g_acceleration_pointer;
|
75 | if(g_acceleration_pointer>=d_filterlength)
|
76 | g_acceleration_pointer=0;
|
77 | g_acceleration[g_acceleration_pointer] = (volatile struct s_cinematic)temp; //############
|
78 | return;
|
79 | }
|
80 | struct s_cinematic kalman(){
|
81 | struct s_cinematic temp;
|
82 | //...
|
83 | return temp;
|
84 | }
|
85 | void integrate_cinematic(struct s_cinematic input, struct s_cinematic *output, int8_t mul_translation, int8_t mul_rotation){
|
86 | output->translation_x1 += input.translation_x1;
|
87 | output->translation_x2 += input.translation_x2;
|
88 | output->translation_x3 += input.translation_x3;
|
89 | output->rotation_x1 += input.rotation_x1;
|
90 | output->rotation_x2 += input.rotation_x2;
|
91 | output->rotation_x3 += input.rotation_x3;
|
92 | return;
|
93 | }
|
Thread beobachten
Seitenaufteilung abschalten