Subversion Repositories FlightCtrl

Rev

Rev 1475 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1475 Rev 1477
Line 14... Line 14...
14
uint8_t pwm_signal[6];
14
uint8_t pwm_signal[6];
15
uint8_t volatile pwm_neutral_position[6];
15
uint8_t volatile pwm_neutral_position[6];
16
uint8_t volatile pwm_position[6];
16
uint8_t volatile pwm_position[6];
17
uint16_t volatile pwm_limit[6];
17
uint16_t volatile pwm_limit[6];
18
uint8_t pwm_active;
18
uint8_t pwm_active;
19
uint8_t pwm_status = 1;
-
 
20
#if DEBUG_SIGNAL
19
#if DEBUG_SIGNAL
21
uint8_t volatile display_values = 0;
20
uint8_t volatile display_values = 0;
22
#endif
21
#endif
Line 23... Line 22...
23
 
22
 
Line 33... Line 32...
33
}
32
}
Line 34... Line 33...
34
 
33
 
Line 35... Line 34...
35
void set_pwm() {
34
void set_pwm() {
-
 
35
 
36
 
36
        for(uint8_t n = 0; n < 6; n++) {
37
        for(uint8_t n = 0; n < 6; n++) {
37
                if (pwm_limit[0] != 0xffff) {
38
                pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];
38
                        pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n];
39
                if (pwm_limit[n] != 0) {
39
                        if (pwm_limit[n] != 0) {
40
                        uint8_t l, h;
40
                                uint8_t l, h;
41
                        l = pwm_limit[n] & 0xff;
41
                                l = pwm_limit[n] & 0xff;
42
                        h = pwm_limit[n] >> 8;
42
                                h = pwm_limit[n] >> 8;
43
                        if (pwm_signal[n] < l) {
43
                                if (pwm_signal[n] < l) {
44
                                pwm_signal[n] = l;
44
                                        pwm_signal[n] = l;
45
                        }
45
                                }
-
 
46
                                if (pwm_signal[n] > h) {
46
                        if (pwm_signal[n] > h) {
47
                                        pwm_signal[n] = h;
47
                                pwm_signal[n] = h;
48
                                }
48
                        }
49
                        }
Line 49... Line 50...
49
                }
50
                }
Line 58... Line 59...
58
        }
59
        }
59
        set_pwm();
60
        set_pwm();
Line 60... Line 61...
60
 
61
 
Line 61... Line -...
61
}
-
 
62
 
-
 
63
void pwm_check_active() {
-
 
64
 
-
 
65
        // check if pwm is configured
-
 
66
        pwm_active  = (pwm_limit[0] != 0xffff) ? (1<<SERVO1) : 0;
-
 
67
        pwm_active |= (pwm_limit[1] != 0xffff) ? (1<<SERVO2) : 0;
-
 
68
        pwm_active |= (pwm_limit[2] != 0xffff) ? (1<<SERVO3) : 0;
-
 
69
        pwm_active |= (pwm_limit[3] != 0xffff) ? (1<<SERVO4) : 0;
-
 
70
        pwm_active |= (pwm_limit[4] != 0xffff) ? (1<<SERVO5) : 0;
-
 
71
        pwm_active |= (pwm_limit[5] != 0xffff) ? (1<<SERVO6) : 0;
-
 
72
 
-
 
73
}
-
 
74
 
62
}
Line 75... Line 63...
75
 
63
 
76
void eeprom_init() {
64
void eeprom_init() {
77
 
65
 
-
 
66
        eeprom_busy_wait();
78
        eeprom_busy_wait();
67
        eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
-
 
68
        eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
-
 
69
 
-
 
70
        set_pwm_active();
-
 
71
}
-
 
72
 
-
 
73
void set_pwm_active() {
-
 
74
 
-
 
75
        // check if pwm is configured
-
 
76
        pwm_active  = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0;
-
 
77
        pwm_active |= (pwm_limit[1] != 0xffff) ? (1 << SERVO2) : 0;
-
 
78
        pwm_active |= (pwm_limit[2] != 0xffff) ? (1 << SERVO3) : 0;
Line 79... Line 79...
79
        eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position));
79
        pwm_active |= (pwm_limit[3] != 0xffff) ? (1 << SERVO4) : 0;
Line 80... Line 80...
80
        eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
80
        pwm_active |= (pwm_limit[4] != 0xffff) ? (1 << SERVO5) : 0;
Line 89... Line 89...
89
        eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
89
        eeprom_write_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit));
90
        sei();
90
        sei();
Line 91... Line 91...
91
 
91
 
Line 92... Line -...
92
}
-
 
93
 
92
}
Line 94... Line 93...
94
/*
93
 
95
void delay(int ms) {
94
void delay(int ms) {
96
 
95
 
Line 97... Line 96...
97
        while(ms--) {
96
        while(ms--) {
98
                _delay_ms(1);
-
 
Line 99... Line 97...
99
        }
97
                _delay_ms(1);
Line 100... Line 98...
100
 
98
        }
101
}
99
 
Line 102... Line 100...
102
*/
100
}
103
 
101
 
104
int main(void) {
102
int main(void) {
Line -... Line 103...
-
 
103
 
-
 
104
        uint8_t blink_counter = 0;
105
 
105
        uint8_t blink = 0;
106
        uint8_t blink_counter = 0;
106
 
107
        uint8_t blink = 0;
107
        // intialize
108
 
108
        DDRD = (1<<PD7);
109
        // intialize
109
        PORTD = (1<<PD7);
Line 129... Line 129...
129
                set_pwm();
129
                set_pwm();
Line 130... Line 130...
130
 
130
 
131
                // show status
131
                // show status
Line 132... Line 132...
132
                blink_counter++;
132
                blink_counter++;
133
 
-
 
134
                if (pwm_active == 0) { // not configured
133
 
135
 
-
 
136
                        blink = 50;
-
 
137
                        pwm_status = 1;
134
                if (pwm_active == 0) {
138
 
-
 
139
                } else {
135
                        blink = 50;
140
 
136
                } else {
141
                        if (I2C_timeout) {
137
                        if (I2C_timeout) {
142
                                I2C_timeout--;
138
                                I2C_timeout--;
143
                                if (I2C_timeout == 0) { // no i2c signal
-
 
144
                                        blink = 0;
139
                                if (I2C_timeout == 0) { // no i2c signal
145
                                        pwm_status = 2;
140
                                        blink = 0;
-
 
141
                                        set_pwm_neutral();
-
 
142
                                        set_pwm();
146
                                        set_pwm_neutral();
143
                                } else {
147
                                        set_pwm();
144
                                        blink = 5;
148
                                }
145
                                }
149
                                blink = 5;
146
                        } else {
150
                                pwm_status = 0;
-
 
151
                        }
147
                                blink = 0;
Line 152... Line 148...
152
 
148
                        }
153
                }
149
                }
154
 
150