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