Суть темы заключается в том, что бы разобраться с работой таких датчиков как акселерометр, гироскоп и магнитометр, работающих вместе для определения ориентации тела в пространстве.
2) Получение данных из датчика. Здесь сложности должны возникать мало у кого. Подключаем датчик к микроконтроллеру и пишем код для работы с ним (по i2c или spi в зависимости от выбранного). Спойлер
Код:
void MPU_init (void) {
uint8_t R; uint8_t c;
HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, PWR_MGMT_1, 1, 0x00, 1, 100); // Clear sleep mode bit (6), enable all sensors HAL_Delay(100); R = 0x01; HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, PWR_MGMT_1, 1, &R, 1, 100); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
// Configure Gyro and Accelerometer // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate R = 0x03; HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, CONFIG, 1, &R, 1, 100);
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) R = 0x04; HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, SMPLRT_DIV, 1, &R, 1, 100); // Use a 200 Hz rate; the same rate set in CONFIG above
// Set gyroscope full scale range // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, GYRO_CONFIG, 1, &c, 6, 100); // get current GYRO_CONFIG register value // c = c & ~0xE0; // Clear self-test bits [7:5] c = c & ~0x02; // Clear Fchoice bits [1:0] c = c & ~0x18; // Clear AFS bits [4:3] c = c | 0 << 3; // Set full scale range for the gyro - 0=+250dps // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, GYRO_CONFIG, 1, &c, 1, 100); // Write new GYRO_CONFIG value to register
// Set accelerometer full-scale range configuration HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, ACCEL_CONFIG, 1, &c, 6, 100); // get current ACCEL_CONFIG register value // c = c & ~0xE0; // Clear self-test bits [7:5] c = c & ~0x18; // Clear AFS bits [4:3] c = c | 0 << 3; // Set full scale range for the accelerometer - 0=2g HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, ACCEL_CONFIG, 1, &c, 1, 100); // Write new ACCEL_CONFIG register value
// Set accelerometer sample rate configuration // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, ACCEL_CONFIG2, 1, &c, 6, 100); // get current ACCEL_CONFIG2 register value c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, ACCEL_CONFIG2, 1, &c, 1, 100); // Write new ACCEL_CONFIG2 register value
// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
// Configure Interrupts and Bypass Enable // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips // can join the I2C bus and all can be controlled by the Arduino as master R=0x22; HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, INT_PIN_CFG, 1, &R, 1, 100); R=0x01; HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS_W, INT_ENABLE, 1, &R, 1, 100); // Enable data ready (bit 0) interrupt
} //
void init_MAGN (float * destination) {
uint8_t R; uint8_t rawData[3]; // x/y/z gyro calibration data stored here
// First extract the factory calibration for each magnetometer axis R=0x00; HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Power down magnetometer HAL_Delay(10); R=0x0F; HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Enter Fuse ROM access mode HAL_Delay(10); HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS_R, AK8963_ASAX, 1, rawData, 3, 100); // Read the x-, y-, and z-axis calibration values destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc. destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f; destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; R=0x00; HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Power down magnetometer HAL_Delay(10); // Configure the magnetometer for continuous read and highest resolution // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates R = 0 << 4 | 0x06; HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS_W, AK8963_CNTL, 1, &R, 1, 100); // Set magnetometer data resolution and sample ODR HAL_Delay(10);
uint8_t rawData[6]; HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, ACCEL_XOUT_H, 1, rawData, 6, 100); destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
} //
void MPU_get_gyro (int16_t * destination) {
uint8_t rawData[6]; // x/y/z gyro register data stored here HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, GYRO_XOUT_H, 1, rawData, 6, 100); // Read the six raw data registers sequentially into data array destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
} //
void MPU_get_magn (int16_t * destination) {
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition uint8_t c; HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS_R, AK8963_ST1, 1, &c, 1, 100); if(c >= 0x01) { // wait for magnetometer data ready bit to be set HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS_R, AK8963_XOUT_L, 1, rawData, 7, 100); // Read the six raw data and ST2 registers sequentially into data array c = rawData[6]; // End data read by reading ST2 register if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; } }
} //
int16_t MPU_get_temper (void) {
uint8_t rawData[2]; // x/y/z gyro register data stored here HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS_R, TEMP_OUT_H, 1, rawData, 2, 100); // Read the two raw data registers sequentially into data array return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
} //
3) Обработка данных. Для того чтобы подобную систему датчиков успешно использовать, необходимо обработать «сырые» данные, полученные от датчиков. Акселерометр измеряет ускорение, действующее на датчик, с помощью чего мы можем получить проекцию тела относительно земли – отклонение (вращение) по оси X и отклонение (вращение) по оси Y. Вращение по оси Z по данным акселерометра получить не удастся. Вращение по любой из осей координат можно получить при помощи гироскопа. Этот датчик измеряет угловую скорость, то есть скорость поворота вокруг оси. Показания этого датчика сильно зависят от обработки получаемых данных датчика – чем больше скорость опроса, тем более точные результаты. Каждый опрос датчика даст нам мгновенные значения угловой скорости, но между опросами угловая скорость может быть неравномерна, что не даст нам полностью достоверных данных, кроме этого необходимо знать точное время между опросами датчика для интегрирования данных по углу поворота, а раз мы многократно интегрируем данные, которые содержат ошибки, то и они будут накапливаться, что приведет к «уплыванию» показаний. Для фиксации вращения по оси Z хорошо подходит магнитометр – датчик, измеряющий магнитное поле. Как известно, земля имеет свое магнитное поле, а магнитометр как раз в роли компаса может определять направление на север. Но магнитометр также чувствителен и к другим магнитным полям, создаваемым металлическими предметами, различной электронной техникой, радиоаппаратурой и так далее, поэтому применение магнитометра часто проблематично в городских условиях. Кроме этого, измерение магнитометром можно проводить достоверно только в положении ровно перпендикулярном оси к земле. Таким образом, каждый датчик по отдельности не даст полного набора данных, по которым можно определить ориентацию тела в пространстве, но если данные от каждого датчика использовать вместе, то можно добиться очень неплохих результатов.
4) Далее вопросы, возникающие в процессе как это сделать (собственно цель создания темы). От простого к сложному. Если мы получаем данные с акселерометра и, предположим, они нас устраивают своей стабильностью. Далее минимум нам нужно получить вращение по оси Z. Используем магнитометр и компенсируем отклонение оси от перпендикулярного земле положения по данным акселерометра.
Обработка данных производится в радианах, а перевод в градусы только в самом конце. Мои результаты использования этого алгоритма: компенсация почти не работает, даже при минимальных отклонениях показания магнитометра остаются недостоверными при наклонах датчика. Почему? Вроде как метод обоснован и должен хотя бы минимально работать. Для получения положения датчиков в пространстве можно воспользоваться методом Маджвика, называемый фильтром Маджвика. Метод не прост, используем код, близкий к авторскому https://github.com/kriswiner/MPU-9250/tree/master/STM32F401. Спойлер
Судя по фотке, мне как раз такой же комплект модулей едет. В сети есть несколько открытых конструкций с разными вариантами сенсоров. Есть на Ардуне, есть на STM. Из них можно взять именно обработку результатов. Я планирую за основу взять Oculus Rift DK1 - он выполнен как раз на STM32f103, хоть и на других сенсорах. Потом посмотрю на DK2 (STM32f303) - может там обработка данных улучшена, но пока хочу просто запустить систему.
Да на самом деле не важно какие датчики используются, важно то, как данные от них обрабатываются. Интересует вопрос реально хотя бы более менее работающего алгоритма получения стабильной ориентации или может быть носом ткнуть почему эти алгоритмы не дают результата (сырые данные и то полезнее получаются)
Serj_K, а есть ссылки на эти проекты с кодом? PS нашел на гитхабе - в Oculus Rift только магнитометр используется получается?
Serj_K, а есть ссылки на эти проекты с кодом? PS нашел на гитхабе - в Oculus Rift только магнитометр используется получается?
Ссылок у меня нет - просто качал проекты. Но они на работе, могу в понедельник глянуть. В Окулусе не только магнитометр используется, но я пока детально исходники не смотрел, только описание. Модули приедут, тогда и засяду. Вот по быстрому ссылочку нашёл: https://cribstone.github.io/humblehacker/2015/08/28/A-DIY-Flight-Controller.html
Качественное и безопасное устройство, работающее от аккумулятора, должно учитывать его физические и химические свойства, профили заряда и разряда, их изменение во времени и под влиянием различных условий, таких как температура и ток нагрузки. Мы расскажем о литий-ионных аккумуляторных батареях EVE и нескольких решениях от различных китайских компаний, рекомендуемых для разработок приложений с использованием этих АКБ. Представленные в статье китайские аналоги помогут заменить продукцию западных брендов с оптимизацией цены без потери качества.
Компания EVE выпустила новый аккумулятор серии PLM, сочетающий в себе высокую безопасность, длительный срок службы, широкий температурный диапазон и высокую токоотдачу даже при отрицательной температуре.
Эти аккумуляторы поддерживают заряд при температуре от -40/-20°С (сниженным значением тока), безопасны (не воспламеняются и не взрываются) при механическом повреждении (протыкание и сдавливание), устойчивы к вибрации. Они могут применяться как для автотранспорта (трекеры, маячки, сигнализация), так и для промышленных устройств мониторинга, IoT-устройств.
у меня пока времени не хватает плотно заняться темой вообще есть 2 способа получить координаты (углы) ориентации датчика в пространстве:
1. вручную математически обработать данные гироскопа, акселерометра и магнитометра, алгоритм Маджвика или Махони - почему-то адекватных результатов не дало у меня (писал выше), однако наткнулся на редкий обзор результатов разных датчиков и то вскользь, где именно mpu9150 тоже примерно так же реагирует (упоминается вскользь) https://forum.warthunder.ru/index.php?/ ... ?p=3485026
2. использовать DMP (процессор движения), который выдает готовые данные (кватернион, что-то по отслеживанию шагов, еще что-то) (алгоритм самим производителем предоставляется), вот по этому пути попробовать получить ориентацию планирую попробовать https://github.com/kauailabs/navxmxp/bl ... roller.cpp
из всех данных критично в основном получить четкое вращение вокруг оси, направленной перпендикулярно земле (Z), т.к. ни один датчик самостоятельно этого дать не может так, чтобы это было стабильно
Перелопатил я исходники Rift DK1 под MPU9250, прошил контроллер. В прошивке контроллер общается с модулем по SPI на 20Мгц, а опрос магнетометра происходит по внутренней I2C шине MPU9250 на 400кГц, гироскоп работает как I2C мастер. Решил сделать правильно - опрашиваю регистр статуса I2C на предмет завершения передачи для последующего чтения регистров с переданными нужными lfyysvb. Но чтение по SPI "проглатывает" нужный байт, а флаги состояния после чтения регистра сбрасываются. При последующем чтении регистра там уже 0 и опрос зацикливается. Отладка в кокосе какая-то недоделанная, причину такой работы при чтении по SPI отловить пока не смог. Скорее всего переделаю так, чтобы не анализировать флаги, просто ,буду читать данные по времени.
Я переписывал весь код пошагово для себя, для понимания - естественно какие-то куски кода брал из других проектов, но старался с пониманием того, что делаю, в итоге все датчики по отдельности работают, шлют данные, вроде бы вполне правдоподобные, но вся соль оказалась после попыток объединить показания датчиков для крена, тангажа и рыска - но нет, тут я застрял
Мне кажется вполне достаточно для начала общаться с датчиком по i2c (или только по spi) - это упростит задачу в начале, а потом уже комбинировать после получения результатов обработки, пока все еще сил и времени не хватает прошерстить DMP
кстати, пишу в keil (HAL) - особо проблем с ним нет, у меня, по крайней мере
Обнаружил следующий глюк чипа MPU-9250: если частота семплирования акселерометра/гироскопа установлена 8кГц (для Rift DK1), при работе с чипом по SPI и включении в нём режима I2C Master для общения с магнетометром при чтении любого регистра магнетометра меняется частота семплирования акселерометра/гироскопа на 4кГц и на 8кГц уже не возвращается. Также при работе с чипом по SPI и включении в нём режима I2C Master наблюдается очень плохой запуск модуля. Видно образуются конфликты между внешней SPI и внутренней I2C. Переходить полностью на I2C не хочу, поэтому с магнетометром попробую общаться через внешнюю I2C AUX шину чипа, к которой он по идее подключён. При таком варианте работа с магнетометром будет независима от работы с акселерометром/гироскопом, то есть как с отдельным чипом.
Обработка данных производится в радианах, а перевод в градусы только в самом конце. Мои результаты использования этого алгоритма: компенсация почти не работает, даже при минимальных отклонениях показания магнитометра остаются недостоверными при наклонах датчика. Почему? Вроде как метод обоснован и должен хотя бы минимально работать.
Результат снова недостоверный – кажется, что показания стали еще хуже. Углы плавают. Что здесь не так?
Использую акселерометр из микросхемы MPU6050 и магнитометр HMC5883L (микросхемы эти есть в явном виде на сборках GY-86, GY-87 , на GY-91 они объеденены в один чип) , сигналы поступают на Arduino UNO. Данная формула компенсации наклона нормально работает. Но для этого магнитометр кроме калибровки нуля по осям X и Y , совершаемой оборотом в горизонтальной плоскости, нужно обязательно калибровать по оси Z (достаточно просто перевернуть вверх ногами магнитометр). Калибровки акселерометра не потребовалось.
Исходники (программа и библиотеки) взял у Корнелиуса. http://www.jarzebski.pl/arduino/rozwiaz ... owych.html Код замечательно работает, но Корнелиус по какой-то причине не ввел компенсацию нуля магнитометра по оси Z. Без нее компенсация будет однобокой.
С компенсацией по Z, получается вот такая картинка (использован код Корнелиуса для Procrssing) при качании этих датчиков (наклонно компенсированного компаса) до + - 45 градусов во всех плоскостях, и нескольких значениях азимута (в руках это сложно сделать, поэтому азимут - зеленая линия слегка загибается). Красная линия показания азимута от магнитометра без учета показаний акселерометра. https://yadi.sk/i/0vzThONlzbMKY
Вот скриншот более подробный. При превышении угла наклона больше чем на 45 градусов, компенсация перестает работать и зеленая линия (компенсированное значение) подскакивает и сливается с красной (некомпенсированное значение азимута компаса). https://yadi.sk/i/KHvW9exP327ehf
Грел датчики феном примерно до 70 градусов, дрейф азимута не превысил 2 градуса.
В дальнейшем планирую сделать калибровку средствами самого микроконтроллера (без компьютера), ну и фильтр , например, Калмана прислюнявить.
Собственно делался компас для автопилота на лодку. Тряски и сильных наклонов там нет. До этого использовался магнитометр на кардане.
Докладываю по MPU-9250. Вроде запустил я его в полном объёме - работа с МК по SPI, данные из магнетометра считываются в автоматическом режиме с нужной частотой. Единственный нюанс - плохо работает при частоте семплирования акселерометра/гироскопа 8кГц, при 1кГц всё отлично. Данные из магнетометра считываются в регистры MPU-9250 посредством I2C Master самой MPU-9250 на скорости 400кГц, можно и 500, но смысла нет. Прошивка - модифицированная Oculus Rift DK1.
Недокументироанная возможность или особенность работы MPU-9250: при возникновении прерывания по готовности данных от акселерометра/гироскопа и разрешенной работой с интерфейсом I2C_Slave_x (например I2C_SLV0_CTRL с включённым битом I2C_SLV0_EN) происходит автоматический запуск работы с I2C_Slave_x с последними состояниями настройки регистров I2C_Slave_x. То есть, если последней операцией с I2C_Slave_x было чтение регистров данных магнетометра, то эта операция будет запущена автоматически с частотой возникновения прерывания. В результате после завершения цикла передачи данных по внутренней шине I2C имеется возможность за один цикл чтения по SPI получить данные со всех сенсоров.
Применительно к Oculus Rift DK1 опрос акселерометра/гироскопа сделал с частотой 1кГц, для магнетометра частота опроса 100Гц. Максимальная частота готовности данных магнетометра не на много больше 100Гц для автоматического режима, что делает бессмысленными попытки использования режима однократных измерений с принудительным запуском. Для получения частоты опроса магнетометра 100Гц I2C_Slave включается только каждый десятый цикл измерения акселерометра/гироскопа. Теперь займусь проверкой работы сенсора собственно в качестве Oculus Rift DK1. Интересует прохождение калибровки, так как магнетометр MPU-9250 не имеет настроек своей работы в отличие от HMC5983, который применяется в Oculus Rift DK1. Возможно придётся эмулировать эти настройки программно.
Подключил к компу, установил ПО от Oculus. Сам трекер находится, но из за другого магнетометра не проходит калибровка. Нужно функцию калибровки мегнетометра эмулировать программно. Хотя можно пойти и кардинально другим путём - сделать независимый от Oculus эмулятор джойстика или мышки. Главное, что сенсор работает.
Как конечное устройство - Oculus Rift DK1 - не работает, в том числе из за отсутствия дисплейной части. Сейчас планирую переписать прошивку под обычный HID джойстик, тогда всё будет видно.
Ну что ж, выкинул из прошивки почти все Oculus Rift - специфичные вещи, переписал HID дескриптор под джойстик, вывел значения регистров сенсоров в "аналоговом" виде осей джойстика. "Шумы" есть, но явного дрифта в статическом положении сенсора нет. Магнетометр имеет маленькую чувствительность, так как имеет большой диапазон - данные нужно множить примерно на 50 для получения соизмеримых значений с остальными датчиками. Теперь нужно пропустить данные через фильтр и получить кватернион.
Практически разобрался с этим чипом. Чтобы получить кватернион пришлось разбираться с входными данными для фильтра Мэджвика - оказалось, что ему нужно данные гироскопа именно в радианах давать. Кроме того, оси магнетометра не совпадают с осями гироскопа. Когда устранил эти проблемы - получил работающий результат. Но при использовании данных магнетометра результат значительно хуже, чем без них. Пока от использования магнетометра отказался - он мне роли не играет. Пробовал делать калибровку сенсоров - результат ухудшается, углы начинают плыть.
На данный момент реализовал два варианта HID устройств - 3-х осевой джойстик и мышка. У джойстика дрифта вообще не видно, а мышка совсем немного "гуляет" (наверное в районе десятых градуса), но у неё чувствительность сделана примерно в 10 раз выше. Чувствительность мышки буду ещё подбирать.
Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 20
Вы не можете начинать темы Вы не можете отвечать на сообщения Вы не можете редактировать свои сообщения Вы не можете удалять свои сообщения Вы не можете добавлять вложения