GPS с гироскопом и одометром.

Ну rdtsc это не совсем время, это количество тактов, а тактовая частота процессоров может быть непостоянной… впрочем, что я википедию цитирую. Одним словом, с этим проблем будет больше, чем пользы :slight_smile: Я трек к GPS притянуть не могу, а придётся ещё и rdtsc к часам притягивать :smiley:

Лучше бы поискать функцию доступа к системным часам. Вроде линукс как-то умеет более точно время брать…
На винде сейчас использую GetSystemTimeAsFileTime, получается где-то 15-16мс разрешение.

Хотя QueryPerformanceCounter можно попробовать, он вроде должен учитывать и смену частоты процессора.

Насколько я вижу, $WDST считается исключительно из целого количества импульсов. ИМХО, можно считать точнее если импульсов 2 и больше.
Раз есть “время, прошедшее с момента последнего импульса”, его можно поделить на “длительность последнего импульса” и получить накопленную “дробную часть” следующего импульса. Ну, прогноз, конечно.

Можно. Только, придётся запоминать, сколько “одолжил”, и не забыть вычесть из следующего раза :slight_smile:
Ещё можно сюда акселерометр приплести (имея информацию о разгоне\торможении, прогноз можно делать точнее).
Главное, ничего не прогнозировать на скоростях менее 5км\ч, ибо машина может и остановиться, и прогноз не сбудется :slight_smile:
Хотя, как мне кажется, не принципиально это всё.

Ещё есть вариант вместо фиксированных 100гц считать, передавать и логировать время каждого импульса индивидуально. Правда, как мне показалось, это будет неоправдано неудобно для обработки. Да и на высоких скоростях пропускная способность шины будет впритык, если это всё на компьютер через i2c адаптер отправлять…

Трэк 17 только по одометру
http://ge.tt/8zXzcRS/v/41?c
Тупенько в Excel-е. Из условия, что колёса задние, не поворотные; задано, что первое (левое) колесо выдаёт 50.4 импульса на метр, а второе (правое) - 50.17. Колея автомашины - 1.41 метра.
Судя по вики, стандартная колея задних колёс 1424 мм, то есть очень близко.

Маркер перескакивает на моем CAN адаптере с 0xae60 на 0x0000:


AE5F
AE60
0000
0001

такт для сообщения 4A0 = 20 ms в ~90% случаев, но получается целая гистограмма
возможных значений задержки между “реальными” сообщениями с CAN шины.
Завтра попробую поставить “железный” CAN фильтр на 4A0, посмотрим связано ли это
с самой шиной, или с CAN приемником.
0C2 (положение руля + скорость изменения положения руля) имеет такт ~10 ms.
Как его пересчитать в угол поворота автомобиля (по гироскопу)?

Пробовал решить эту задачу, вышло что-то вот такое :smiley:

Правда, я колёсную базу брал ровно 1424, и диаметр колёс тоже предположил одинаковый :slight_smile: Но похоже, там и сами расчёты неправильные.

Колёсная база и диаметр колёс подбирались вручную, или вывелись автоматически?

Подбирались. Диаметры явно разные (кол-во импульсов на метр с датчика), иначе график разности хода колёс имеет явный уклон, а трек становится вот таким, состоящим из дуг.

Вместо HMC5883L заказал более продвинутый компас HMC5983. С температурной коррекцией и автоматической компенсации оффсета. Посмотрим, будет ли толк.

Наткнулся на интересную идею:

  1. Интегрированный массив МЭМС гироскопов с повышенной стабильностью показаний
  2. Signal Processing of MEMS Gyroscope Arrays to Improve Accuracy Using a 1st Order Markov for Rate Signal Modeling (pdf версия, вроде более полная).

Для тех, кому лень читать, сразу результаты для 6 гироскопов:

Результат позитивный, т.к. даже Simple average дал улучшение более чем в 2 раза (а это именно тот алгоритм, который я в состоянии сделать самостоятельно :D).

Достоинства подхода:

  • Можно недорого улучшить точность всех сенсоров (гироскопы, акселерометры). MPU-6050 стоит менее 4$, а в нём сразу и то и другое в одном чипе.
  • Способ выглядит простым (для Simple average), с возможностью дальнейших улучшений алгоритма (если повторить статью в полном объёме).

Недостатки:

  • Придётся отказаться от MotionFusion алгоритма, зашитого в MPU-6050, и считать всё вручную.

Касательно последнего пункта - в теории, алгоритмы можно взять из arducopter или ardu-imu, но там всё настолько заточено под конкретную задачу, что выдрать что-то оттуда и заставить работать отдельно для меня выглядит сложным.

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

Теперь вопрос: есть желающие? :slight_smile:
Попробовать матан с DCM можно уже сейчас (в логах есть сырые данные акселерометра, гироскопа, компаса, и текущая скорость для компенсации центробежной силы). Позже останется только заменить гироскоп на виртуальный, более точный. Кстати, с акселерометром и компасом можно проделать тоже самое.

Я уже рублю ручной рассчёт постепенно.
Вот скажи, сколько были AFS_SEL и FS_SEL?
А то акселерометр в покое (начало и конец) показывает силу по модулю около 6916 отсчётов, тогда как g должно быть 8192 или 16384 или 4096 или 2048. Если AFS_SEL=1, то акселерометр показывает что g было ~ 8.27 м/с^2. Это меньше, чем на Венере и примерно как на высоте 600 км от Земли :slight_smile:

Тяжело сказать. При использовании MotionFusion я брал значения, которые выдавал он сам, обещая их сырыми. Что там на деле - я не знаю, код закрытый…
Тут либо попробовать принять 1g за 6916 и посмотреть что выйдет, либо план Б.

План Б: Если надо, могу записать трек без MotionFusion. Тогда, могу выдать действительно сырые данные с частотой 1кгц. Но в логе уже не будет кватернионов, курса, крена, тангажа и т.п. - ручной рассчёт станет обязательным условием :slight_smile:

План Б, для ручного рассчёта:

http://svimik.com/imuraw_1.rar (571кб) - Уровень сложности: простой, аналогичен треку 17.
http://svimik.com/imuraw_2.rar (894кб) - Уровень сложности: сложный, езда в помещении как трек 18.

Частота: гироскоп\акселерометр: 200Гц, компас и колёса: 100Гц.
Порядок чтения: $IMU, $MAGR, $IMU, $WRAW.
$IMU может представлять несколько пакетов подряд из-за наличия буффера у MPU-6050, который я читаю целиком за раз.
Настройки: AFS_SEL=0 (2g), FS_SEL=0 (250°/s). Компас: -2048…2047 соответствует ±0.88 Ga.
В начале и конце немного постоял для статистики.
Начало и конец обоих треков - одно и то же парковочное место, т.е. должны совпасть.

Кто все эти числа? :slight_smile:

Если в $IMU сначала идёт 3 оси акселерометра, то по показаниям получается ~0.86 g.

$IMU - данные из FIFO буффера MPU-6050. Первые 3 похожи на акселерометр, и следующие - на гироскоп :slight_smile:
Предположительно ACCEL_X, ACCEL_Y, ACCEL_Z, GYRO_X, GYRO_Y, GYRO_Z.

$MAGR - магнетометр. X, Z, Y.

$WRAW - с ним читатель уже знаком :slight_smile:

Ничего не знаю, что выдало - то и записал :slight_smile: Возможно, нуждается в калибровке. Возможно чип бракованный.

Исходный кот:


short imu[]={0,0,0,0,0,0};
short magn[]={0,0,0};

while(app_running){
	/* MPU6050 */

	// check FIFO overflow
	if(mpu6050_fifo_overflow()){
		printf("FIFO overflow!\r\n");
		fprintf(log, "$ERR overflow\r\n");
		mpu6050_fifo_reset();
		continue;
	}

	// get FIFO count
	int fifo_count=getFIFOCount();
	if(fifo_count>1024){
		printf("Wrong FIFO count (%i)!\r\n", fifo_count);
		mpu6050_fifo_reset();
		continue;
	}
	int frames=fifo_count/12;
	if(frames==0){continue;}
	
	// get timestamp
	getdate(&date);
	if(date.wMilliseconds!=prevdate.wMilliseconds || date.wSecond!=prevdate.wSecond){
		fprintf(log, "[%02i:%02i:%02i.%03i]\r\n", date.wHour, date.wMinute, date.wSecond, date.wMilliseconds);
	}
	prevdate=date;

	// read FIFO
	unsigned char tmp[1024];
	read(GYRO_ADDR, MPU6050_FIFO_R_W, tmp, frames*12);

	// parse data
	for(int i=0;i<frames;i++){
		unsigned char * rbuf=tmp+(i*12);
		imu[0]=(rbuf[0]<<8)|(rbuf[1]&0xFF);
		imu[1]=(rbuf[2]<<8)|(rbuf[3]&0xFF);
		imu[2]=(rbuf[4]<<8)|(rbuf[5]&0xFF);
		imu[3]=(rbuf[6]<<8)|(rbuf[7]&0xFF);
		imu[4]=(rbuf[8]<<8)|(rbuf[9]&0xFF);
		imu[5]=(rbuf[10]<<8)|(rbuf[11]&0xFF);
		fprintf(log, "$IMU %i,%i,%i,%i,%i,%i\r\n", imu[0],imu[1],imu[2],imu[3],imu[4],imu[5]);
	}

	if(++tick2%2==0){
		/* wheel sensors */
		unsigned char ss_cnt[2];
		unsigned int ss_interval[2], ss_delay[2];
		if(!speedsensor_get(ss_cnt, ss_interval, ss_delay)){printf("Bad packet\r\n");}
		fprintf(log, "$WRAW %i,%i,%i,%i,%i,%i\r\n", ss_cnt[0], ss_cnt[1], ss_interval[0], ss_interval[1], ss_delay[0], ss_delay[1]);
	}else{
		/* compass */
		compass_read(&magn[0], &magn[1], &magn[2]);
		fprintf(log, "$MAGR %i,%i,%i\r\n", magn[0], magn[1], magn[2]);
	}
}

Есть ещё вариант попробовать ITG-3205 + ADXL345 вместо MPU-6050.
Минусы:

  • В MPU-6050 гироскоп и акселерометр синхронизированы (оцифровываются одновременно), пара раздельных сенсоров будут работать асинхронно.
  • В MPU-6050 встроенный таймер и свой буффер, дающий фиксированную частоту данных (независимо от лагов принимающей стороны). У других сенсоров будет просто чтение регистров в цикле, и QueryPerformanceCounter на каждое сообщение.

Плюсы:

  • Можем получить адекватное 1g :slight_smile:
  • Шанс сравнить два сенсора с разными подходами.

Ну я ещё не добрался до ручного интегрирования гироскопа.
Но экспериментируй, если хочешь :slight_smile:

Вариант с ITG-3205 + ADXL345: http://svimik.com/accelraw2_1.rar (6.71мб)
Формат:
$A %f,%i,%i,%i - time interval (us), accel x, y, z
$G %f,%i,%i,%i - time interval (us), gyro x, y, z
$W %f,%i,%i,%i,%i,%i,%i - time interval (us), wheel sensor data
$M %f,%i,%i,%i - time interval (us), magnetometer x,z,y

Порядок следования: фиксированный, AG, AG, W, AG, AG, M
Частота: плавающая, порядка ~500Гц.
Временные метки: на каждой строке, прошедшее время в мкс относительно предыдущей строки.

В общем, что-то странное с этими данными.
GYRO_Z явно имеет постоянную составляющую в покое - ~ -173, GYRO_X - ~39.
Интегрировать показания гироскопа получается, но только если умножать на коэффициент. Для imudata17.rar это 1.44 (если считать что показания в градусах в секунду), для imuraw_1.rar - 1.05 (если считать, что показания - 131 LSB / градус в секунду). Причём результат интегрирования всех трёх осей хуже, чем только Z. Коэффициент нужен помимо преобразования градусов в радианы, времени в секунды и т.п.
ACCEL_X, ACCEL_Y тоже имеют постоянную составляющую, но это может быть обусловлено ещё и наклоном чипа относительно горизонтали. Тут я не понимаю как выделить разложение g на 3 оси и как - постоянные составлющие показаний чипа. В результате не могу даже уверенно определить, куда машина разгоняется - вперёд или назад.
По спецификации не могу понять, допустимы ли эти постоянные составляющие и коэффициенты или нет.

Буду пробовать новые данные.
В крайнем случае можно будет оставить только GPS и одометр.

Дрейф гироскопа никто не отменял. С компенсацией постоянной составляющей борятся в первую очередь, вычитая её.

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

Ясен пень, наш мир не идеален :slight_smile:

http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf
Страницы 12 и 13.
Если правильно понимаю, то коэфициенты это Sensitivity Scale Factor Tolerance, домустимо ±3%, что впринципе близко к 1.05.
Есть ещё Initial ZRO Tolerance. Не знаю, что это, но возможно постоянная составляющая. Там аж ±20º/s.

А какой магнетометр?