| 16,7 → 16,6 |
|---|
| uint8_t volatile pwm_position[6]; |
| uint16_t volatile pwm_limit[6]; |
| uint8_t pwm_active; |
| uint8_t pwm_status = 1; |
| #if DEBUG_SIGNAL |
| uint8_t volatile display_values = 0; |
| #endif |
| 35,17 → 34,19 |
|---|
| void set_pwm() { |
| |
| for(uint8_t n = 0; n < 6; n++) { |
| pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n]; |
| if (pwm_limit[n] != 0) { |
| uint8_t l, h; |
| l = pwm_limit[n] & 0xff; |
| h = pwm_limit[n] >> 8; |
| if (pwm_signal[n] < l) { |
| pwm_signal[n] = l; |
| if (pwm_limit[0] != 0xffff) { |
| pwm_signal[n] = pwm_position[n] - 127 + pwm_neutral_position[n]; |
| if (pwm_limit[n] != 0) { |
| uint8_t l, h; |
| l = pwm_limit[n] & 0xff; |
| h = pwm_limit[n] >> 8; |
| if (pwm_signal[n] < l) { |
| pwm_signal[n] = l; |
| } |
| if (pwm_signal[n] > h) { |
| pwm_signal[n] = h; |
| } |
| } |
| if (pwm_signal[n] > h) { |
| pwm_signal[n] = h; |
| } |
| } |
| } |
| |
| 60,28 → 61,27 |
|---|
| |
| } |
| |
| void pwm_check_active() { |
| |
| // check if pwm is configured |
| pwm_active = (pwm_limit[0] != 0xffff) ? (1<<SERVO1) : 0; |
| pwm_active |= (pwm_limit[1] != 0xffff) ? (1<<SERVO2) : 0; |
| pwm_active |= (pwm_limit[2] != 0xffff) ? (1<<SERVO3) : 0; |
| pwm_active |= (pwm_limit[3] != 0xffff) ? (1<<SERVO4) : 0; |
| pwm_active |= (pwm_limit[4] != 0xffff) ? (1<<SERVO5) : 0; |
| pwm_active |= (pwm_limit[5] != 0xffff) ? (1<<SERVO6) : 0; |
| |
| } |
| |
| |
| void eeprom_init() { |
| |
| eeprom_busy_wait(); |
| eeprom_read_block(&pwm_neutral_position[0], &eeprom_neutral_pos, sizeof(pwm_neutral_position)); |
| eeprom_read_block(&pwm_limit[0], &eeprom_pwm_limit, sizeof(pwm_limit)); |
| pwm_check_active(); |
| |
| set_pwm_active(); |
| } |
| |
| void set_pwm_active() { |
| |
| // check if pwm is configured |
| pwm_active = (pwm_limit[0] != 0xffff) ? (1 << SERVO1) : 0; |
| pwm_active |= (pwm_limit[1] != 0xffff) ? (1 << SERVO2) : 0; |
| pwm_active |= (pwm_limit[2] != 0xffff) ? (1 << SERVO3) : 0; |
| pwm_active |= (pwm_limit[3] != 0xffff) ? (1 << SERVO4) : 0; |
| pwm_active |= (pwm_limit[4] != 0xffff) ? (1 << SERVO5) : 0; |
| pwm_active |= (pwm_limit[5] != 0xffff) ? (1 << SERVO6) : 0; |
| |
| } |
| |
| void eeprom_write() { |
| |
| cli(); |
| 91,7 → 91,6 |
|---|
| |
| } |
| |
| /* |
| void delay(int ms) { |
| |
| while(ms--) { |
| 99,7 → 98,6 |
|---|
| } |
| |
| } |
| */ |
| |
| int main(void) { |
| |
| 110,6 → 108,8 |
|---|
| DDRD = (1<<PD7); |
| PORTD = (1<<PD7); |
| |
| delay(1000); |
| |
| cli(); |
| uart_init(); |
| eeprom_init(); |
| 131,25 → 131,21 |
|---|
| // show status |
| blink_counter++; |
| |
| if (pwm_active == 0) { // not configured |
| |
| if (pwm_active == 0) { |
| blink = 50; |
| pwm_status = 1; |
| |
| } else { |
| |
| if (I2C_timeout) { |
| I2C_timeout--; |
| if (I2C_timeout == 0) { // no i2c signal |
| blink = 0; |
| pwm_status = 2; |
| set_pwm_neutral(); |
| set_pwm(); |
| } else { |
| blink = 5; |
| } |
| blink = 5; |
| pwm_status = 0; |
| } else { |
| blink = 0; |
| } |
| |
| } |
| |
| if (blink == 0) { |