Проект под кодовым названием: «Бульболёт». Часть 1. Погружение в MPU6050 (или нет)

Всем привет! Сегодня, а именно с этой статьи, я бы хотел начать свою историю разработки летательного средства на радио управление. В интернете я натыкался на множество статей где так или иначе собирали Р.У модели, и в основном это делалась на основе каких-то модулей или уже готовых плат со всей периферией. Мне не понравился такой подход к делу, и я решил начать собирать свой самолётик с нуля.

С начало я изучил основы, а именно посмотрел что там придумали китайцы, а придумали они "полётные контролеры" в основу которых входит микроконтроллер (в основном STM32), гироскоп, барометр и т.д. В принципе, подумал я, всё выглядит довольно просто, значит можно повторить.

И так мой путь начался с выбора начинки нашего "полётника". В основу я выбрал микроконтроллер STM32F103C8T6, расположенный на распаянной плате (blue pil)в периферию: микросхему MPU6050 (3 осевой гироскоп и акселерометр) разведенную на плате под кодовым названием (GY-521), BMP280 (датчик давления), HMC5883L (3-осевой цифровой компас) распаянный на плате (модуль GY-273). Для передачи и приёма я использую MRF49XA (трансивер). В последствии всё будет выпаяно и припаяно по месту назначению, а пока ограничимся макетной платой.

И так начнём, для работы с камнем я буду использовать STM32CubeMX (библиотека HAL), а для редактирования прослойки будем юзать STM32CubeIDE. Почему именно эти проги, во-первых, они официальные с поддержкой STM, во-вторых, имеют привлекательный и понятный интерфейс, а как же большое обилие примеров для изучения. Для дебагинга я использую USART, но в иделае надо бы юзать ST LINK (поэтому не экономим и берём вместе с blue pil-ом).

Приступим-с, открываем STM32CubeMX и выбираем наш МК и начинаем настраивать его, а именно для начала включим внешнее тактирование.

Переходи во вкладку Clock Configuration вписываем значение нашего кварца, в моём случае это 8, после ищем клеточку с надписью HCLK и вписываем 72, программа сама подстроить оставшиеся настройки.

Так как я буду использовать интерфейс I2C для общения, то нам надо его включить. Как вы видите я использую Fast Mode, но в принципе его можно и не использовать.

Идём дальше и забегая чуть - чуть вперёд сразу скажу, что стандартное общение по I2C довольно сильно замедляет основную программу и для решения этой проблемы у нас может быть два пути решения либо исправлять это с помощью прерываний:

• HAL_StatusTypeDef HAL_I2C_Master_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size)

• HAL_StatusTypeDef HAL_I2C_Master_Receive_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size)

Либо с помощью DMA(Direct Memory Access) или прямой доступ к памяти. Прикол этой штуки в том, чтобы распараллелить программу и облегчить жизнь CPU, то есть DMA общается по I2C, а основной процесс обрабатывает полученные данные.

И как вы поняли я выбрал 2 способ. И так приступим к настройке, так как я буду только считывать данные, то будем юзать RX, в общем смотрим на картинку.

Так же не забываем включить прерывания, чтобы знать когда DMA сделает своё дело

Включаю USART для дебага.

И так пока остановимся, ведь мне надо пояснить некоторые детали, о которых я должен был ещё сказать в начале статьи, а именно то, что сейчас мы подготавливаем наш МК для работы с MPU6050. Поэтому мы и используем I2С, однако это не все плюшки микросхемы, прошу вас обратить внимание на вывод INT. Он нужен для того чтобы подавать сигналы мастеру, к примеру, о готовности данных. Удобно! Я тоже так думаю, поэтому нам надо настроить наш МК, чтобы он захватывал эти сигналы, для этого настроим таймер.

Активируем TIM1, ибо он самый мясистый.

Теперь пробежимся по пунктам, и первым идёи режим управления таймером Master/Slave.

Суть заключается в том, чтобы при возникновении тех или иных событий, таймер мог посылать различные триггеры (сигналы) другим таймерам — режим Master.

А таймер, получающий сигнал от другого таймера, является подчинённым — режим Slave.

Следовательно, Slave Mode - этот пункт указывающий, что должен делать таймер находясь в подчинённом режиме, в нашем случае, таймер будет подчинён сам себе, то есть, совершая захват сигнала, он будет генерировать тригер, и на основании этого сигнала, обнуляет счётчик (Reset Mode — означает, что при поступлении сигнала таймер должен обнулить счётчик) - это нам и нужно. Есть также и другие функции, если кому нужно:

• Gated Mode — таймер работает пока есть сигнал высокого уровня, и останавливается, когда поступает сигнал низкого уровня.

• Trigger Mode — счётчик запускается пока есть сигнал высокого уровня, НО не сбрасывается.

• External Clock Mode 1 — таймер будет триггерится как на внешний сигнал, так и на внутренний.

Trigger Source — а этот пункт указывает что будет служить триггерным сигналом для таймера в моём случае TI1FP1, а так смотрим на картинку.

Идём дальше, и тут стоит заметить, что у каждого таймера есть четыре независимых канала, которые могут подключаться к физическим пинам микроконтроллера, а могут и не подключаться, работая как внутренние входы/выходы. Поэтому при настройке двух каналов (direct и indirect) активировался только один вход (РА8). Зачем мы это сделали? Чтобы первый канал ловил передний фронт, а второй — задний, тем самым мы и измерим длину импульса.

Чтобы не марать ручки делаем так, чтобы измерения сигнала происходили аппаратно для этого, настраиваем DMA, не забывая включить циклический режим.

Осталось дело за малым, смотрим на картинку. Тут особо менять ничего не надо, но вы проверьте чтобы всё сходилось, а я лишь остановлюсь на одном пункте: Prescaler - предделитель частоты таймера (частоты поступающей с шины APB2). И тут важно отметить! Поскольку счётчик начинает отсчёт с нуля, то предделитель должен быть на 1 меньше. В нашем случае 72мГц / 71 = 1000000 тиков в секунду.

И так с настройкой камня покончено, перейдем к самой микросхеме (MPU6050). И первым делом почитаем даташит и карту регистров. Пойдём по порядку, сначала у нас идёт SELF_TEST, мы его оперативно скипаем ибо мы и сами сможем вычислить среднее значение погрешности. Далее у нас идёт несколько пунктов (SMPLRT_DIV, CONFIG, GYRO_CONFIG, ACCEL_CONFIG), которые нам понадобится для правильной работы датчика. Реализуем это в программе для этого создадим функцию инициализации. В ней мы с помощью стандартной функции общения по I2C (HAL_I2C_Mem_Write()) будем устанавливать начальные параметры работы.

void InitMPU6050(void) {     

        uint8_t data;
        data = 0;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, PWR_MGMT_1_REG, 1, &data, 1, time);

        data = 0x07;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, SMPLRT_DIV_REG, 1, &data, 1, time);
 
        data = 0x18;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, ACCEL_CONFIG_REG, 1, &data, 1, time);

        data = 0x18;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, GYRO_CONFIG_REG, 1, &data, 1, time);

        data = 0x1;
        HAL_I2C_Mem_Write(&hi2c1, MPU6050_Address, INT_ENABLE_REG, 1, &data, 1, time);

}

• PWR_MGMT_1_REG - регистр отвечающий за питание микросхемы (0x6B).

• SMPLRT_DIV_REG - регистр устанавливает частоту работы по формуле Sample Rate = Gyroscope Output Rate(8kHz) / (1 + SMPLRT_DIV) отправляя значение 7, в итоге получаем частоту работы = 1kHz (0x19).

• GYRO_CONFIG_REG - регистр настраивающий гироскоп, в моём случае получаем 0x18 = 11000 , что соответствует ±2000°/s(0x1B).

• ACCEL_CONFIG_REG - регистр настраивающий акселерометр, в моём случае получаем 0x18 = 11000 , что соответствует ±16g(0x1C).

• INT_ENABLE_REG - регистр разрешающий подавать импульсы на вывод INT, в нашем случае по обновлению данных измерений (0x38).

Теперь научимся захватывать наши импульсы, с помощью колбека (он срабатывает при прерывании от DMA) HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim){} после мы проверяем тот ли таймер захватил сигнал, если тот то действуем.

uint32_t zntime;

/* USER CODE BEGIN 2 */
  HAL_TIM_IC_Start_DMA(&htim1, TIM_CHANNEL_2, (uint32_t*)&zntime, 1);//включаем таймер     
/* USER CODE END 2 */

/* USER CODE BEGIN 4 */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) 
{
        if(htim->Instance == TIM1)
        {
           HAL_I2C_Mem_Read_DMA(&hi2c1, MPU6050_Address, ACCEL_XOUT_H_REG, 1, data, 14);
        }
}
/* USER CODE END 4 */

Далее будем считывать готовые данные с помощью DMA, для этого нам понадобятся регистры с 3B (ACCEL_XOUT_H_REG) по 48, то есть за раз нам надо считать 14 регистров. Чтение будет выполняться с помощью функции HAL_I2C_Mem_Read_DMA() и с помощью прерывания по окончанию передачи по I2C void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)

/* USER CODE BEGIN 4 */
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
	  MPU6050Read();
}
/* USER CODE END 4 */

Также стоит отметить, что данные будут состоять из двух 8 битных частей, которые надо будет склеить в 16 битное число, а так же для наглядности высчитаем pitch.

typedef struct
{

    int16_t AccelX;
    int16_t AccelY;
    int16_t AccelZ;
    double aX;
    double aY;
    double aZ;

    int16_t GyroX;
    int16_t GyroY;
    int16_t GyroZ;
    double gX;
    double gY;
    double gZ;

    int16_t temp;
    int Temperature;

} MPU6050znach;// можно с помощью переменных или как тут


MPU6050znach z;


void MPU6050Read(void)
{
  ///////////////////////////склейка/////////////////
        z.AccelX = (int16_t)(data[0] << 8 | data[1]);
        z.AccelY = (int16_t)(data[2] << 8 | data[3]);
        z.AccelZ = (int16_t)(data[4] << 8 | data[5]);
        z.temp = (int16_t)(data[6] << 8 | data[7]);
        z.GyroX = (int16_t)(data[8] << 8 | data[9]);
        z.GyroY = (int16_t)(data[10] << 8 | data[11]);
        z.GyroZ = (int16_t)(data[12] << 8 | data[13]);

/////////////////////////////обработка////////////////////
        z.aX = z.AccelX / 16384.0;
        z.aY = z.AccelY / 16384.0;
        z.aZ = z.AccelZ / 14418.0;
        z.Temperature = (int)((int16_t)z.temp / (float)340.0 + (float)36.53);
        z.gX = z.GyroX / 131.0;
        z.gY = z.GyroY / 131.0;
        z.gZ = z.GyroZ / 131.0;

 /////////////////////////вычисление////////////////////
        int pitch;
        int pitch_sqrt = sqrt(z.aY * z.aY +  z.aZ * z.aZ);
        pitch = atan2(-z.aX, pitch_sqrt) * RAD_TO_DEG;

  /////////////////////////вывод/////////////////
         snprintf(msg, sizeof(msg), "%d", pitch);
           HAL_UART_Transmit(&huart1, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
           HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", strlen("\r\n"), HAL_MAX_DELAY);
           HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", strlen("\r\n"), HAL_MAX_DELAY);
}

И тадам получаем угол отклонения, однако значения достаточно сырые, ибо должно было получится где-то -90 градусов.

В поисках проблемы я решил посмотреть какие значения у нас на выходе, снизу представлены график значений акселерометра и гироскопа по оси X и как видим тут очень шумно.

Для борьбы с этим не дугом есть 2 пути решения использовать фильтр (к примеру Кальмана) или DMP (это небольшая програмка вшитая в MPU6050, которая сглаживает значения). И об этом мы поговорим в следующей статье, а так же подключим HMC5883L (3-осевой цифровой компас). На этом всё, до скорого!