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 | ||