Помогите разобраться с iarduino_multiservo

[Skull]

Доброго времени суток!
Есть большой проект с 27ю платами pca9685, управляющими сервоприводами. Столкнулся с очень низкой скоростью работы приводов.
Связано это с тем, что библиотека блокирует работу проца до отработки всех передаваемых пакетов I2C, в связи с чем возникают задержки до 1с (разница запрос- ответ ModBus TCP).
Программы на С я в принципе читаю, но понять библиотеку для меня оказалось очень сложно.
Из того,что я понял, выражение TWBR = ((F_CPU/(I2C_mass_STATUS[0]*1000))-16)/2 и есть установка регистров скорости TWI, где оная скорость
равняется (I2C_mass_STATUS[0]*1000).
Подскажите пожалуйста , как изменить эту часть библиотеки, чтобы поднять скорость TWI до 400 кГц и 1 мГц ?

Нужная часть библиотеки под спойлером

СпойлерПоказать

/** внутренние функции для работы с шиной I2C **/
void iarduino_MultiServo::I2C_func_begin () { I2C_mass_STATUS[2]=1; TWBR = ((F_CPU/(I2C_mass_STATUS[0]*1000))-16)/2; if(TWBR<10){TWBR=10;} TWSR&=(~(_BV(TWPS1)|_BV(TWPS0)));}
void iarduino_MultiServo::I2C_func_START () {int I2C_var_I=0; I2C_mass_STATUS[2]=1; TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTA); while(!(TWCR & _BV(TWINT))){I2C_var_I++; if(I2C_var_I>I2C_mass_STATUS[1]){I2C_mass_STATUS[2]=0; break;}} I2C_mass_STATUS[3] = TWSR & 0xF8; if(I2C_mass_STATUS[3]!=0x08){I2C_mass_STATUS[2]=0;}}
void iarduino_MultiServo::I2C_func_RESTART () {int I2C_var_I=0; if(I2C_mass_STATUS[2]){ TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTA); while(!(TWCR & _BV(TWINT))){I2C_var_I++; if(I2C_var_I>I2C_mass_STATUS[1]){I2C_mass_STATUS[2]=0; break;}} I2C_mass_STATUS[3] = TWSR & 0xF8; if(I2C_mass_STATUS[3]!=0x10){I2C_mass_STATUS[2]=0;}}}
void iarduino_MultiServo::I2C_func_SEND_ID (uint8_t I2C_byte_ID, bool I2C_bit_RW) {int I2C_var_I=0; if(I2C_mass_STATUS[2]){ TWDR = (I2C_byte_ID<<1)+I2C_bit_RW; TWCR = _BV(TWINT) | _BV(TWEN); while(!(TWCR & _BV(TWINT))){I2C_var_I++; if(I2C_var_I>I2C_mass_STATUS[1]){I2C_mass_STATUS[2]=0; break;}} I2C_mass_STATUS[3] = TWSR & 0xF8; if(I2C_bit_RW) {if(I2C_mass_STATUS[3]!=0x40){I2C_mass_STATUS[2]=0;}}else{if(I2C_mass_STATUS[3]!=0x18){I2C_mass_STATUS[2]=0;}}}}
void iarduino_MultiServo::I2C_func_WRITE_BYTE (uint8_t I2C_byte_DATA) {int I2C_var_I=0; if(I2C_mass_STATUS[2]){ TWDR = I2C_byte_DATA; TWCR = _BV(TWINT) | _BV(TWEN); while(!(TWCR & _BV(TWINT))){I2C_var_I++; if(I2C_var_I>I2C_mass_STATUS[1]){I2C_mass_STATUS[2]=0; break;}} I2C_mass_STATUS[3] = TWSR & 0xF8; if(I2C_mass_STATUS[3]!=0x28){I2C_mass_STATUS[2]=0;}}}
uint8_t iarduino_MultiServo::I2C_func_READ_BYTE (bool I2C_bit_ACK) {int I2C_var_I=0; if(I2C_mass_STATUS[2]){ TWCR = _BV(TWINT) | _BV(TWEN) | I2C_bit_ACK<<TWEA; while(!(TWCR & _BV(TWINT))){I2C_var_I++; if(I2C_var_I>I2C_mass_STATUS[1]){I2C_mass_STATUS[2]=0; break;}} I2C_mass_STATUS[3] = TWSR & 0xF8; if(I2C_bit_ACK){if(I2C_mass_STATUS[3]!=0x50){I2C_mass_STATUS[2]=0;}}else{if(I2C_mass_STATUS[3]!=0x58){I2C_mass_STATUS[2]=0;}} return TWDR;}else{return 0xFF;}}
void iarduino_MultiServo::I2C_func_STOP () {int I2C_var_I=0; TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTO); while(!(TWCR & _BV(TWSTO))){I2C_var_I++; if(I2C_var_I>I2C_mass_STATUS[1]){I2C_mass_STATUS[2]=0; break;}} delayMicroseconds(20);}
void iarduino_MultiServo::I2C_func_WRITE_WORD (uint8_t I2C_byte_SUM) {int I2C_var_I=0; BUS_buff_DATA[0x20]=0; for(I2C_var_I=0; I2C_var_I<I2C_byte_SUM; I2C_var_I++){I2C_func_WRITE_BYTE(BUS_buff_DATA[I2C_var_I]); if(I2C_mass_STATUS[2]){BUS_buff_DATA[0x20]++;}}} // выполнение функции I2C_func_WRITE_BYTE I2C_byte_SUM раз, с передачей байтов из массива BUS_buff_DATA
void iarduino_MultiServo::I2C_func_READ_WORD (uint8_t I2C_byte_SUM) {int I2C_var_I=0; BUS_buff_DATA[0x20]=0; for(I2C_var_I=0; I2C_var_I<I2C_byte_SUM; I2C_var_I++){BUS_buff_DATA[I2C_var_I]=I2C_func_READ_BYTE(I2C_var_I==I2C_byte_SUM-1?0:1);if(I2C_mass_STATUS[2]){BUS_buff_DATA[0x20]++;}}} // выполнение функции I2C_func_READ_BYTE I2C_byte_SUM раз, при этом последний раз с отправкой NACK, а все предыдущие с отправкой ACK. Результат записываем в массив BUS_buff_DATA

[Sancho]

Читать тут.
TWBR = (F_CPU / I2C_FREQ - 16) / 2;
Соответственно, нужно в Вашем массиве изменить I2C_mass_STATUS[0], увеличив в 4 раза.

Отправлено спустя 32 секунды:
В 4 для 400кГц.
Или множитель, 1000 -> 4000
Следующий кусок кода:

Код: Выделить всё

if(TWBR<10){TWBR=10;}

не даст выставить более 444кГц.
Поэтому можете просто написать:
TWBR=12; // для 400к и 16МГц вместо TWBR = ((F_CPU/(I2C_mass_STATUS[0]*1000))-16)/2;

[Skull]

Спасибо!!!!!