Создание собственной системы стабилизации
Там читается хитрО не как в остальных датчиках там к адресу первого(с которого прочитать надо) регистра чёт прибавить надо - блин уже и не помню 😦
Если читать несколько регистров подряд то в адресе нужно определенный битик поднять.
во как в 330-й было
LSM330_OUT_X_L_G +0x80, 6, buf// к адресу регистра цыфирка прибавляеццо
6-й - автоинкремен
Там читается хитрО не как в остальных датчиках там к адресу первого(с которого прочитать надо) регистра чёт прибавить надо
то в адресе нужно определенный битик поднять.
я про это ж и написал…
кароч придется вначале проверить через и2с а потом уже дальше экспериментировать
ну или попробовать каждый регистр отдельно почитать…
Вот как я с MPU делал…
static void mpu6000AccRead(int16_t *accData)
{
uint8_t buf[6];
uint8_t i;
MPU_ON;// прижал
spi_writeByte(MPU_RA_ACCEL_XOUT_H | 0x80);
for (i = 0; i < 6; i++)
buf[i] = spi_readByte();
MPU_OFF;//отпустил
accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
}
///////////////////////////////////////////////////////////////////////////
uint8_t spi_writeByte(uint8_t Data)
{
/* Wait until the transmit buffer is empty */
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_SendData(SPI1, Data);
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
return SPI_I2S_ReceiveData(SPI1);
}
uint8_t spi_readByte(void)
{
volatile uint8_t data = 0;
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
SPI_I2S_SendData(SPI1,0xFF); // Dummy Byte
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) == SET);
data = SPI_I2S_ReceiveData(SPI1);
/* Return the shifted data */
return data;
}
спи у вас на прерывании или тупо отослал/принял? лично мне первый вариантом не совсем нравится (может и зависнуть…)
У меня с прерываниями, в ПХ4 без. Зависнуть может, но для этого должен быть серьёзный сбой проца и датчика, что врятли они после этого будут правильно работать.
Грубо говоря, он 200 раз в секунду, раскладывает сигнал на частоты и отсеивает все что больше
Как это он “сигнал раскладывает” ??
Он просто может делать выборки АЦП не каждый такт, а через такт, через два и т.д…
(в зависимости от настройки DLPF, этож железка…), выдавать он будет со своей частотой опроса, но фактически пойдут повторы данных ниже частоты среза…
А про какой то встроенный алгоритм фильтрации по “раскладыванию” я так ничего в документах и не нашел, Вы где то читали (?) поделитесь плиз…
блин и спутники даже на кухне находит, а позиционирование никакое может метров на 40 прыгнуть…
А математика какая по расчету позиции/расстояния ?? или это “черный ящик” ?
Я себе сделал на гаверсинусах… вроде похоже на правду выдает, но окончательно сказать пока трудно, надо погулять с железом по улице (пока только дома смотрю).
Может внутри какой-нибудь БИХ фильтр, т.к. от частоты среза меняется задержка.
Кстати, какие в PX4 для USB VID и PID, определяется в системе как HID устройство или как-то подругому?
хмм… ошибку как бы нашел, читаются все 6 регистров последовательно нормально… но мало помогло:
если читать редко - то все ок. (с выводом на уарт через 50-200мс),
как запускаешь код вия - херня всякая вместо данных.
пробовал даже считывать и проверять регистр статуса на наличие новых данных - все равно такая же канитель…
Как это он “сигнал раскладывает” ??
Например FFT(БПФ) - это не суть важно, да и не знаю, какие алгоритмы зашиты в MPU. Кстати, MPU - это не тупой аналоговый датчик с АЦП преобразователем. Там стоит процессор, который обрабатывает поступающие сигналы и выдает отфильтрованные. И он даже умеет делать Sensor Fusion (и на выходе уже выдавать углы Эйлера).
Но что бы вы разобрались, вот картинка данных, полученных с гироскопа:
Синий график - это данные с самого сенсора, как видите, в нем присутствуют высокочастотные шумы. Что бы от них избавится, MPU использует цифровой фильтр (DLPF) который пропускает только низкочастотный сигнал (синусоида на графике). Что бы MPU понять, с какой частотой изменяется сигнал, ему надо записать в память несколько сэмплов сигнала и подсчитать частоту - именно поэтому появляется задержка сигнала на выходе (показано красным графиком). В зависимости от того, какую частоту надо отфильтровать, меняется и задержка.
какие в PX4 для USB VID и PID, определяется в системе как HID устройство или как-то подругому?
определяется как com-порт id не помню - в виндовом драйвере посмотреть можно, они в самом низу драйвера прописаны…
определяется как com-порт id не помню - в виндовом драйвере посмотреть можно, они в самом низу драйвера прописаны…
видно используют библиотеку от ST, было интересно как с лицензией на usb поступили))
Например FFT
FFT он вряд ли делает… ресурсов не хватит… скорее всего там обычный цифровой НЧ фильтр (DLPF), который как раз и дает смещение/задержку фазы сигнала как на картинке…
вот пример простого в понимании дискретного LPF с заданием нужной частоты среза:
/// Low pass filter cut frequency
float filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )";
// Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3
// discrete low pass filter, cuts out the high frequency noise, where dt in seconds
value = value + (dt / ( filter + dt)) * (value - lastValue);
lastValue = value;
Т.е. чем ниже частота сигнала на входе (чем выше dt), тем большая часть приращения (разность между текущим и пред-м значениями) идет в результат…
п.с. Редко появляюсь тут сейчас. Звиняйте если шо пропустил 😃
upd: ошибочка
...
value = lastValue + (dt / ( filter + dt)) * (inputValue - lastValue);
...
если читать редко - то все ок. (с выводом на уарт через 50-200мс),
как запускаешь код вия - херня всякая вместо данных.
Пробовал считывать все регистры данных по и2ц 250 раз в сек, никаких сбоев не было. Смотри код, прежде всего работу с и2ц. По СТМ32 могу помочь, если код покажешь.
видно используют библиотеку от ST, было интересно как с лицензией на usb поступили))
Нет, не использует. В загрузчике свой код, в нутексе драйвер нутекса.
что бы вы разобрались
Я Вас прекрасно понимаю, тут все верно, моя цель добраться до истины, а это предположения (с этого и начал)…
В “интеллектуальной” обработке данных, еще раз скажу, -сомневаюсь… , просто иначе в документации это было бы отражено, еще бы ! , такая “фишка” - тема для пиара…
Т.е. чем ниже частота сигнала на входе (чем выше dt), тем большая часть приращения
Вот и я к тому же… (просто разный дискрет выборок), дешево, надежно, и примитивно… очень похоже на правду.
мдя… подключил по и2с - все шикарно… вернул на спи - та же хрень…
Сергей, покажи драйвер, а то непонятно о чём разговор 😦
Вот и я к тому же… (просто разный дискрет выборок), дешево, надежно, и примитивно… очень похоже на правду.
При влючении фильтра в MPU частота дискритизации действительно уменьшается (без фильтра 8 кГц, а с фильтром 1 кГц)
INTERNAL CLOCK SOURCE CLK_SEL=0,1,2,3
Gyroscope Sample Rate, Fast DLPFCFG=0
SAMPLERATEDIV = 0
8 kHz
Gyroscope Sample Rate, Slow DLPFCFG=1,2,3,4,5, or 6
SAMPLERATEDIV = 0
1 kHz
Accelerometer Sample Rate 1 kHz
фильтр все равно там есть и используется скорее всего БИХ фильтр.
Почему уменьшается частота дискретизации: видно процессор в MPU не справляется при общете фильтра с большим потоком данных вот и уменьшили Sample Rate.
да тут наврятли в самом драйвере дело… там все просто и понятно
* \param spi The SPI_Master_t struct instance.
* \param TXdata Data to transmit to slave.
*
* \return Data received from slave.
*/
uint8_t SPI_MasterTransceiveByte(SPI_Master_t *spi, uint8_t TXdata)
{
/* Send pattern. */
spi->module->DATA = TXdata;
/* Wait for transmission complete. */
while(!(spi->module->STATUS & SPI_IF_bm)) {
}
/* Read received data. */
uint8_t result = spi->module->DATA;
return(result);
}
вызов
spi_getSixRawADC(GYRO_PIN, 0x80 + 0x28);
void spi_getSixRawADC(uint8_t pinMask, uint8_t reg) {
SPI_MasterSSLow(&PORTE, pinMask);
SPI_MasterTransceiveByte(&spiMasterE, 0x40 | reg);
for (uint8_t i=0; i<6; i++) {
rawADC[i]=SPI_MasterTransceiveByte(&spiMasterE, 0xFF);
}
SPI_MasterSSHigh(&PORTE, pinMask);
}
самое обидное именно в том что если вывожу в уарт так
if (GYRO) Gyro_init();
if (ACC) {ACC_init();acc_25deg = acc_1G * 0.423;}
while (1) {
SerialPrint(2, "GYRO X = ");
spi_getSixRawADC(GYRO_PIN, 0x80 | 0x28);
put_hex_int((uint16_t)(rawADC[1]<<8)|rawADC[0]);
delay(10);
SerialPrint(2, "; Y = ");
put_hex_int((uint16_t)(rawADC[3]<<8)|rawADC[2]);
delay(10);
SerialPrint(2, "; Z = ");
put_hex_int((uint16_t)(rawADC[5]<<8)|rawADC[4]);
SerialPrint(2, ";\n");
delay(100);
}
то все отлично… все данные корректны
стоит закомментировать и получаем вот что
на и2с с теми же настройками все отлично
кстати есть тут кто кто делал из 4-й дискавери логический анализатор по статье на хабре?
просто как раз пробовал им посмотреть что творится - выдает тоже фиг знает что
да тут наврятли в самом драйвере дело… там все просто и понятно
Код не смотрел (пока не силен в STM32), но по описанию, очевидно что у вас неправильно идет работа с SPI.