int i2c_write_reg(int addr, int reg, int val)
{
    halI2C_MSG_t        xMSG, *pMSG;
    unsigned char        msg_data[1];

    pMSG = &xMSG;
    xMSG.xTask = xTaskGetCurrentTaskHandle();
    xMSG.msg_addr = addr << 1;
    xMSG.msg_sreg = reg;
    xMSG.len_data = 1;
    xMSG.msg_data = msg_data;

    msg_data[0] = val;

    xQueueSendToBack(pI2C->xQueueReq, &pMSG, portMAX_DELAY);

    NVIC_SetPendingIRQ(I2C1_EV_IRQn);
    //ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

    return xMSG.errno;
}

xSemaphoreTake(xSem1, );

// здесь передаем данные в конечный автомат (КА) и запускаем прерывание

xSemaphoreTake(xSem2, );

// сюда попадаем когда КА сделает свои дела и вызовет xSemaphoreGive(xSem2);
// забираем результат операции

xSemaphoreGive(xSem1);

ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

#include <stddef.h>

#include "hal/hal.h"
#include "hal/i2c.h"
#include "hal/mpu6050.h"
#include "hal/timebase.h"
#include "hal/usart.h"

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

#include "fp/im.h"

#include "main.h"
#include "lib.h"
#include "sh.h"

i2c_mpu6050_t i2c_mpu6050;

fp_im_t __CCM__ fp_im;

TaskHandle_t tIM;

void timebaseIRQ()
{
BaseType_t xWoken = pdFALSE;

//xSemaphoreGiveFromISR(glob.xSemIM, &xWoken);
vTaskNotifyGiveFromISR(tIM, &xWoken);

portYIELD_FROM_ISR(xWoken);
}

void taskFP_IM(void *pvParameters)
{
int rc = 0;

rc = i2c_mpu6050_enable(&i2c_mpu6050, 1, 1);

do {
//xSemaphoreTake(glob.xSemIM, portMAX_DELAY);
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

rc = i2c_mpu6050_read(&i2c_mpu6050);

if (rc == 0) {

fp_im_main( &fp_im,
i2c_mpu6050.accel,
i2c_mpu6050.gyro,
i2c_mpu6050.temp);

if (fp_im.refresh_flag) {

fp_im.refresh_flag = 0;
xSemaphoreGive(glob.xSem100HZ);
}
}
else {
}
}
while (1);
}

void taskFP_100HZ(void *pvParameters)
{
int i = 0;

do {
xSemaphoreTake(glob.xSem100HZ, portMAX_DELAY);

i++;

if (i >= 10) {

i = 0;
xSemaphoreGive(glob.xSem10HZ);
}

}
while (1);
}

void taskFP_10HZ(void *pvParameters)
{
do {
xSemaphoreTake(glob.xSem10HZ, portMAX_DELAY);

printf("%f %f %f (%f %f %f) %f" EOL,
&fp_im.sv[0],
&fp_im.sv[1],
&fp_im.sv[2],
&fp_im.sq[1],
&fp_im.sq[2],
&fp_im.sq[3],
&fp_im.temp);

}
while (1);
}

void taskFP_INIT(void *pvParameters)
{
i2cEnable();

glob.xSemIM = xSemaphoreCreateBinary();
glob.xSem100HZ = xSemaphoreCreateBinary();
glob.xSem10HZ = xSemaphoreCreateBinary();

xTaskCreate(taskFP_IM, "tFP_IM", 1024, NULL, 4, &tIM);
xTaskCreate(taskFP_100HZ, "tFP_100HZ", 4 * 1024, NULL, 3, NULL);
xTaskCreate(taskFP_10HZ, "tFP_10HZ", 4 * 1024, NULL, 2, NULL);

timebaseEnable(IM_FREQ_HZ);

vTaskDelete(NULL);
}