Всем привет! Сегодня, а именно с этой статьи, я бы хотел начать свою историю разработки летательного средства на радио управлении. В интернете я натыкался на множество статей, где так или иначе собирали р. у. модели, и в основном это делалась на основе каких-то модулей или уже готовых плат со всей периферией. Мне не понравился такой подход к делу, и я решил начать собирать свой самолётик с нуля.
С начала я изучил основы, а именно - посмотрел что там придумали китайцы, а придумали они "полётные контроллеры", в основу которых входит микроконтроллер (в основном STM32), гироскоп, барометр и т.д. В принципе, подумал я, всё выглядит довольно просто, значит, можно повторить.
Итак, мой путь начался с выбора начинки нашего "полётника". Я взял за основу микроконтроллер STM32F103C8T6, расположенный на распаянной плате (blue pil). В периферию: микросхему MPU6050 (3 осевой гироскоп и акселерометр) разведенную на плате под кодовым названием (GY-521), BMP280 (датчик давления), HMC5883L (3-осевой цифровой компас) распаянный на плате (модуль GY-273). Для передачи и приёма я использую MRF49XA (трансивер). В последствии всё будет выпаяно и припаяно по месту назначению, а пока ограничимся макетной платой.
И так начнём, для работы с камнем я буду использовать STM32CubeMX (библиотека HAL), а для редактирования прослойки будем юзать STM32CubeIDE. Почему именно эти проги, во-первых, они официальные с поддержкой STM, во-вторых, имеют привлекательный и понятный интерфейс, а как же большое обилие примеров для изучения. Для дебагинга я использую USART, но в иделае надо бы юзать ST LINK (поэтому не экономим и берём вместе с blue pil-ом).
Приступим-с! Открываем STM32CubeMX и выбираем наш МК. Начинаем настраивать его, а именно, для начала включим внешнее тактирование (ведь чем больше частота - тем быстрее работает МК).
![На плате blue pil кварц уже припаян к нужным ножкам, его надо только подключить. На плате blue pil кварц уже припаян к нужным ножкам, его надо только подключить.](https://habrastorage.org/getpro/habr/upload_files/de6/e93/7a3/de6e937a3b641bdce3bbd5edcc7a8539.png)
Переходи во вкладку Clock Configuration, вписываем значение нашего кварца, в моём случае это 8. После ищем клеточку с надписью HCLK и вписываем 72 (72 MHz - максимальная частота при кварце на 8 MHz). Программа сама подстроит оставшиеся настройки.
![Достаточно удобно. Достаточно удобно.](https://habrastorage.org/getpro/habr/upload_files/af3/ddf/3c4/af3ddf3c4bbdd1d0bccbf46c7b081c02.png)
Так как я буду использовать интерфейс I2C для общения, то нам надо его включить. Как вы видите, я использую Fast Mode, но в принципе его можно и не использовать. (влияет только на скорость общения)
![клацк - клацк клацк - клацк](https://habrastorage.org/getpro/habr/upload_files/d29/b3a/ec1/d29b3aec1aaf6922436afe974b2cdaf6.png)
Идём дальше и забегая чуть - чуть вперёд сразу скажу, что стандартное общение по 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. Все настройке на картинке.
![](https://habrastorage.org/getpro/habr/upload_files/5ff/f96/403/5fff964035f62acaf411f4fffd7b92dd.png)
Так же не забываем включить прерывания, чтобы знать, когда DMA сделает своё дело
![клацк - клацк клацк - клацк](https://habrastorage.org/getpro/habr/upload_files/94e/133/fec/94e133fec412cc04e5d54b802fc1e01b.png)
Включаю USART для дебага.
![Не обязательно Не обязательно](https://habrastorage.org/getpro/habr/upload_files/e03/774/03d/e0377403df419447742c4e991398d609.png)
И так пока остановимся, ведь мне надо пояснить некоторые детали, о которых я должен был ещё сказать в начале статьи, а именно то, что сейчас мы подготавливаем наш МК для работы с MPU6050. Поэтому мы и используем I2С, однако это не все плюшки микросхемы, прошу вас обратить внимание на вывод INT. Он нужен для того, чтобы подавать сигналы мастеру, к примеру, о готовности данных. Удобно! Я тоже так думаю, поэтому нам надо настроить наш МК, чтобы он захватывал эти сигналы, для этого настроим таймер.
![not клацк - клацк not клацк - клацк](https://habrastorage.org/getpro/habr/upload_files/888/fc2/5f4/888fc25f4a88718590ddddf93e34a521.png)
Активируем TIM1, ибо он самый мясистый.
![](https://habrastorage.org/getpro/habr/upload_files/4ad/edb/978/4adedb978250be0980ee1f7653843077.png)
Теперь пробежимся по пунктам, и первым идёт режим управления таймером Master/Slave.
Суть заключается в том, чтобы при возникновении тех или иных событий таймер мог посылать различные триггеры (сигналы) другим таймерам — режим Master.
А таймер, получающий сигнал от другого таймера, является подчинённым — режим Slave.
Следовательно, Slave Mode - этот пункт, указывающий, что должен делать таймер, находясь в подчинённом режиме, в нашем случае, таймер будет подчинён сам себе, то есть, совершая захват сигнала, он будет генерировать тригер, и на основании этого сигнала, обнуляет счётчик (Reset Mode — означает, что при поступлении сигнала таймер должен обнулить счётчик) - это нам и нужно. Есть также и другие функции, если кому нужно:
Gated Mode — таймер работает пока есть сигнал высокого уровня, и останавливается, когда поступает сигнал низкого уровня.
Trigger Mode — счётчик запускается пока есть сигнал высокого уровня, НО не сбрасывается.
External Clock Mode 1 — таймер будет триггерится как на внешний сигнал, так и на внутренний.
Trigger Source — а этот пункт указывает что будет служить триггерным сигналом для таймера в моём случае TI1FP1, а так смотрим на картинку.
![](https://habrastorage.org/getpro/habr/upload_files/494/434/ace/494434ace82a10ec4589e136f8cfd6e7.png)
Идём дальше, и тут стоит заметить, что у каждого таймера есть четыре независимых канала, которые могут подключаться к физическим пинам микроконтроллера, а могут и не подключаться, работая как внутренние входы/выходы. Поэтому при настройке двух каналов (direct и indirect) активировался только один вход (РА8). Зачем мы это сделали? Чтобы первый канал ловил передний фронт, а второй — задний, тем самым мы и измерим длину импульса.
Чтобы не марать ручки, делаем так, чтобы измерения сигнала происходили аппаратно. Для этого, настраиваем DMA, не забывая включить циклический режим.
![клацк - клацк клацк - клацк](https://habrastorage.org/getpro/habr/upload_files/a1b/9d4/532/a1b9d45325d0e392e6d2d874dca997b9.png)
Осталось дело за малым, смотрим на картинку. Тут особо менять ничего не надо, но вы проверьте чтобы всё сходилось, а я лишь остановлюсь на одном пункте: Prescaler - предделитель частоты таймера (частоты поступающей с шины APB2). И тут важно отметить! Поскольку счётчик начинает отсчёт с нуля, то предделитель должен быть на 1 меньше. В нашем случае 72мГц / 71 = 1000000 тиков в секунду.
![важно важно](https://habrastorage.org/getpro/habr/upload_files/219/ef3/770/219ef37702e655e7021ca08c5b7c589c.png)
И так с настройкой камня покончено, перейдем к самой микросхеме (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.
![pitch - тангаж pitch - тангаж](https://habrastorage.org/getpro/habr/upload_files/0bc/21e/b3c/0bc21eb3c4da657f7ce29e4f93eae622.png)
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 / 2048.0;
z.aY = z.AccelY / 2048.0;
z.aZ = z.AccelZ / 2048.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 градусов.
![](https://habrastorage.org/getpro/habr/upload_files/db7/85a/42f/db785a42f3a48cd813cf33b9eaaa0ad3.png)
В поисках проблемы я решил посмотреть какие значения у нас на выходе, снизу представлены график значений акселерометра и гироскопа по оси X и как видим тут очень шумно.
![закройте окно - шумно! закройте окно - шумно!](https://habrastorage.org/getpro/habr/upload_files/e2e/ada/574/e2eada5747b255690514fc9808a96255.png)
![](https://habrastorage.org/getpro/habr/upload_files/309/f66/678/309f666780467f3207e28e4a259b3210.png)
Для борьбы с этим недугом есть 2 пути решения использовать фильтр (к примеру Кальмана) или DMP (это небольшая програмка вшитая в MPU6050, которая сглаживает значения). И об этом мы поговорим в следующей статье, а так же подключим HMC5883L (3-осевой цифровой компас). На этом всё, до скорого!
Комментарии (32)
OvO
03.11.2021 12:23+1STM32F103C8T6 трудно найти в дикой природе за разумные деньги, в магазинах водится только STM32x0 серии. На смену MPU6050 пришел mpu9250 со встроенным компасом, а для новых разработок рекомендуется использовать icm20948.
zzomda Автор
03.11.2021 12:27Насчёт STM32 не согласен взял её рублей за 200 на алике, а так в принципе интересное предложение, возьму на заметку.
DanilinS
03.11.2021 14:22+1Сейчас вообще очень сложно найти что-то из STM32 за разумные деньги. Все как метлой вымели ...
OvO
03.11.2021 16:21+1Сейчас STM32[G,L]0 можно отхватить за ~2$ за штуку, но это без USB. STM32G4 LQFP месяц назад были за ~4$, а сейчас только остатки в *BGA корпусах. Или китайские аналоги.
DanilinS
03.11.2021 17:11+1Все, что сейчас можно отхватить на али - очень часто отбраковка производства.
Заказывал 2 платы на али на STM32G431 у разных продавцов. Первая имела не рабочий DAC, на второй плата чип вообще через ST-Link не виделся.
OvO
04.11.2021 12:21Я говорил про евроамериканские магазины:mouser, arrow, digikey, rs и др. В начале октября забрал десяток STM32G431 в Mouser по 4$ - их с начала года много было по всем магазинам, а сейчас почти нет. STM32[G,L]0 пока в ассортименте от 2$.
На aliexpress покупать под пайку компоненты - рисковое дело. Привезут дрянь или пустышки и ничего не докажешь. Где-то видел статью по разновидности обмана на aliexpress и отбраковка еще не самый плохой вариант.
FGV
03.11.2021 14:02int pitch;
int pitch_sqrt = sqrt(z.aY * z.aY + z.aZ * z.aZ);
pitch = atan2(-z.aX, pitch_sqrt) * RAD_TO_DEG;
Странно у Вас тангаж считается.
1) Почему инты? что 89.9 градусов или .99g не может быть?
2) Крен/тангаж обычно считаются как atan2(Ax,Ay)/atan2(Az,Zy), откуда такая волшебная формула приведенная выше? В чем ее математический/физический смысл? Может чертежик нарисовать с вектором тяжести и как он по осям аксика раскладывается написать?
Shinsei75
05.11.2021 12:54Про MPU6050_Address и time можно по подробнее?
zzomda Автор
05.11.2021 12:55MPU6050_Address - адресс устройства прописан в даташите.
time - время ожидания ответа, может быт любым, на ваше усмотрение. У меня 500.
Shinsei75
05.11.2021 18:50И еще один вопрос. Я наверное глуп, но как data у Вас превратилась в массив, если изначально она объявлена как простая переменная uint8_t?
zzomda Автор
05.11.2021 19:02Одна переменная объявлена внутри функции, то есть она распространяется только в ней. А другая уже объявлена глобально, то есть распространяется на весь код.
Userpc0101
05.11.2021 23:31Почему MPU6050? Этот IMU довольно редко встречается в дронах чаще MPU6000. Обычно для борьбы с шумом в датчике применяется ещё DLPF (ФНЧ). Ну и т.к. у вас летающий аппарат вы уже пробовали делать алгоритм слияния данных с GNSS?
zzomda Автор
07.11.2021 11:44Что было под рукой, как говорится! Насчёт DLPF, интересная тема, обязательно учту
Userpc0101
07.11.2021 12:50Да, вероятно вам придётся бороться с вибрациями на аппарате и если они будут достаточно сильными придётся снижать частоту опроса и увеличивать размер окна фильтра таким образом, что-бы амплитуда колебаний от вибрации полностью поместилась в размерность окна фильтра, тогда даже при сильных вибрациях датчик будет показывать правильный результат, это конечно увеличит задержку и возможно значительно на 20-30мс но это единственный выход в такой ситуации как мне кажется.
FGV
у канала z другой масштабный коэф.?
zzomda Автор
Да, в даташите табличка есть для всех значений.
FGV
И в каком месте? Для "MPU-6000 and MPU-6050 Product Specification Revision 3.4" у всех каналов аксика коэффициенты одинаковы... по 13-й листу цены мл. разряда в зависимости от AFS_SEL:
AFS_SEL=0: k=1.0/16384.0 (диапазон +/-2g)
AFS_SEL=1: k=1.0/8192.0 (диапазон +/-4g)
AFS_SEL=2: k=1.0/4096.0 (диапазон +/-8g)
AFS_SEL=3: k=1.0/2048.0 (диапазон +/-16g)
zzomda Автор
Извиняюсь, перепутал.