Создание собственной системы стабилизации

mataor

хмм… ошибку как бы нашел, читаются все 6 регистров последовательно нормально… но мало помогло:
если читать редко - то все ок. (с выводом на уарт через 50-200мс),
как запускаешь код вия - херня всякая вместо данных.
пробовал даже считывать и проверять регистр статуса на наличие новых данных - все равно такая же канитель…

Sir_Alex
oleg70:

Как это он “сигнал раскладывает” ??

Например FFT(БПФ) - это не суть важно, да и не знаю, какие алгоритмы зашиты в MPU. Кстати, MPU - это не тупой аналоговый датчик с АЦП преобразователем. Там стоит процессор, который обрабатывает поступающие сигналы и выдает отфильтрованные. И он даже умеет делать Sensor Fusion (и на выходе уже выдавать углы Эйлера).

Но что бы вы разобрались, вот картинка данных, полученных с гироскопа:

Синий график - это данные с самого сенсора, как видите, в нем присутствуют высокочастотные шумы. Что бы от них избавится, MPU использует цифровой фильтр (DLPF) который пропускает только низкочастотный сигнал (синусоида на графике). Что бы MPU понять, с какой частотой изменяется сигнал, ему надо записать в память несколько сэмплов сигнала и подсчитать частоту - именно поэтому появляется задержка сигнала на выходе (показано красным графиком). В зависимости от того, какую частоту надо отфильтровать, меняется и задержка.

SergDoc
djdron:

какие в PX4 для USB VID и PID, определяется в системе как HID устройство или как-то подругому?

определяется как com-порт id не помню - в виндовом драйвере посмотреть можно, они в самом низу драйвера прописаны…

djdron
SergDoc:

определяется как com-порт id не помню - в виндовом драйвере посмотреть можно, они в самом низу драйвера прописаны…

видно используют библиотеку от ST, было интересно как с лицензией на usb поступили))

mahowik
Sir_Alex:

Например 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), тем большая часть приращения (разность между текущим и пред-м значениями) идет в результат…

п.с. Редко появляюсь тут сейчас. Звиняйте если шо пропустил 😃

mahowik

upd: ошибочка

...
value = lastValue + (dt / ( filter + dt)) * (inputValue - lastValue);
...
rual
mataor:

если читать редко - то все ок. (с выводом на уарт через 50-200мс),
как запускаешь код вия - херня всякая вместо данных.

Пробовал считывать все регистры данных по и2ц 250 раз в сек, никаких сбоев не было. Смотри код, прежде всего работу с и2ц. По СТМ32 могу помочь, если код покажешь.

djdron:

видно используют библиотеку от ST, было интересно как с лицензией на usb поступили))

Нет, не использует. В загрузчике свой код, в нутексе драйвер нутекса.

oleg70
Sir_Alex:

что бы вы разобрались

Я Вас прекрасно понимаю, тут все верно, моя цель добраться до истины, а это предположения (с этого и начал)…
В “интеллектуальной” обработке данных, еще раз скажу, -сомневаюсь… , просто иначе в документации это было бы отражено, еще бы ! , такая “фишка” - тема для пиара…

mahowik:

Т.е. чем ниже частота сигнала на входе (чем выше dt), тем большая часть приращения

Вот и я к тому же… (просто разный дискрет выборок), дешево, надежно, и примитивно… очень похоже на правду.

mataor

мдя… подключил по и2с - все шикарно… вернул на спи - та же хрень…

SergDoc

Сергей, покажи драйвер, а то непонятно о чём разговор 😦

djdron
oleg70:

Вот и я к тому же… (просто разный дискрет выборок), дешево, надежно, и примитивно… очень похоже на правду.

При влючении фильтра в 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.

mataor

да тут наврятли в самом драйвере дело… там все просто и понятно

*  \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-й дискавери логический анализатор по статье на хабре?
просто как раз пробовал им посмотреть что творится - выдает тоже фиг знает что

Sir_Alex
mataor:

да тут наврятли в самом драйвере дело… там все просто и понятно

Код не смотрел (пока не силен в STM32), но по описанию, очевидно что у вас неправильно идет работа с SPI.

mataor

это не СТМ а atxmega
если неправильно то почему тогда в одном случае все отлично, а в другом - фиг пойми что/

наверное попробую снова все это дело на прерываниях сделать

SergDoc

пробуй через счётчик все разом как я показывал с MPU:

static void mpu6000AccRead(int16_t *accData) {
 uint8_t buf[6];
     uint8_t i;
       MPU_ON;// прижал
spi_writeByte(MPU_RA_ACCEL_XOUT_H | 0x80); // говорит что читать отсюда и до забора (6-в данном случае)
            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; }

MPU_RA_ACCEL_XOUT_H - адрес первого регистра
0x80 - цыфирка что читать надо несколько…

djdron

В atxmega CS аппаратный? какая частота SPI? Вообще такие вещи надо бы осцилом смотреть что на ногах творится

mataor
SergDoc:

пробуй через счётчик все разом

у меня вообще то именно так все и сделано
0х80 у ст-шных - это чтение, а все разом - это 0х40

djdron:

В atxmega CS аппаратный?

если 1 слейв - то да, если несколько - то сам указываеш/управляеш

осцилл к сожалению под рукой только портативный ЮСБ-шный одноканальный… им все что больше 50-100кГц бесполезно смотреть

частоту пробовал и 8МГц, и 2… надо попробовать будет 500кГц

SergDoc
mataor:

spi_getSixRawADC(GYRO_PIN, 0x80 | 0x28);

здесь должно происходить :
прижал лапу
write_bute - забор|обед
счётчик шагов от забора до обеда
read_bute (все шаги) складываем в rawADC
отпустил лапу

mataor
mataor:

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);

SergDoc:

здесь должно происходить :
прижал лапу
write_bute - забор-обед
счётчик шагов от забора до обеда
read_bute (все шаги)
отпустил лапу

блин… я ж это все привел.

SergDoc

uint8_t result = spi->module->DATA; // result - правильная переменная?

Sir_Alex

>SPI_MasterTransceiveByte(&spiMasterE, 0xFF);

Не бейте ногами, а зачем присваивать в начале 0xFF?