CANOpen Details, CANOpen for STM32 Опции

[sayberex]

Начал я для себя реализацию CANOpen под STM32 микроконтроллеры. Пытаюсь портировать CANOpenNode и попутно разобраться как же работает CANOpen. Приобрел интерфейсную плату (PISO-CAN200D, PCI to CAN, 2 channel) от ICP-DAS. Они также поставляют библиотеку CANOpen Master Library для винды. И даже шаровый OPC сервер (вдруг кому интересно). Так вот к библиотеке идут примеры. Запускаю я один из них и при инициализации он у меня спрашивает NMT состояние моего слейва ответ проходит успешно потом через SDO сервис как я понимаю он у меня запрашивает отдать ему COB ID SYNC я посылаю ответ а он мне говорит мол таймаут. Что в моем ответе не правильно?
Запрос с PC
STD ID = 0x606
RTR = NO
Length = 4
Data[0] = 0x40
Data[1] = 0x05
Data[2] = 0x10
Data[3] = 0x00

Ответ от MCU
STD ID = 0x606
RTR = NO
Length = 8
Data[0] = 0x40
Data[1] = 0x05
Data[2] = 0x10
Data[3] = 0x00
Data[4] = 0x80
Data[5] = 0x00
Data[6] = 0x00
Data[7] = 0x00

Кстати не взирая на то что утилита говорит тайм-аут ответ от меня все равно приходит(проверял в CAN Utility от ICP-DAS) просто у нее наверно тайм-аут означает как то что истекло время ожидания так и неправильный ответ.

[sayberex]

Еще немного поэкспериментировал с CANOpen зашел в очередной тупик. Проблема в алгоритме отправки сообщений. Вот мое видение алгоритма отправки.
В конце алгоритма обработки проверять нет ли новых сообщений для отправки и запускать очередь отправки сообщений если она прервалась а сообщения готовые к отправки появились.

void ProcessMain(void) {
//Heartbeat or Node Guard process
if CO_CANRX[NMT_MSG].NewMsg {
if CAN_TransmitStatus(CAN1,1) == CANTXOK {
//Transmit NMT Node Status
CO_CANTX[NMT_MSG].NewMsg = 1;
}
}

//Emergency message process
....

//SDO Service process
....

//Process User -> processing PDO and other

if (IsNewMsg()) { \\если есть сообщения для отправки
if (!IsSequenceRun) { \\Если очередь прерываний завершина то снова запустить
StartTXSequence();
}
}
}

Может кто то видит в этом алгоритме какой нибудь недостаток или предложит лучшую реализацию. И вобще правильно ли что синхронные PDO будут отправляться в ProcessUser или им нужен свой таймер чтобы они отправлялись по четко детерминированному времени тогда это вызывает много вопросов. Планируется что ProcessMain может вызываться как внутри основного цикла так и в Timer ISR.