Talk:Navigation Module - Mathematical Background

Наконец-то нашел то, что мне нужно =) --Swinemaker 23:00, 14 November 2011 (EET)

Отлично, если будут вопросы по разделу - пиши сюда. Хорошо что ты зарегистрировался. АЦП еще не купил (AD7328). Во вторник 22/11/11 займусь этим делом. -- Executor 06:47, 21 November 2011 (EET)

ОУ или не ОУ?

Хмм... Вот возникли сомнения по поводу необходимости ОУ в этой системке.. Сие замечательное устройство (MMA7260) выдает довольно-таки пристойный сигнал (порядка от 1 до 3V) [1],верх. граница сигнала - 5V собственно и усиливать там нечего.. вот-с.

З.Ы. как там насчет АЦП?

UPD: К тому же, походу, сам акселерометр пошумливает (см. рисунокъ [2] - до половины чувствительность 800 mV/g, далее - 200 mV/g). И еще его надо как-то калибровать. Блин. Swinemaker 20:33, 23 November 2011 (EET)

UPD2: какой целевой битрейт юарта? Swinemaker 21:11, 23 November 2011 (EET)

Привет.

• ОУ - как хочешь, согласен что не обязательно (тем более ему желательно еще и отрицательное напряжение для улучшения линейности, которого пока нету), плюс еще и на печатной плате его разводить - согласен, что лучше всего первую версию сделать как можно проще.
• АЦП - заказываю, еще 1 день и будет готово, модель AD7328 (или AD7327), 8 каналов, интерфейс SPI.
• по поводу шумов - это был неподвижный акселерометр? если да - предлагаю поставить хороший конденсатор танталовый и параллельно обычный, емкость пока не знаю - можно поставить еще небольшую индуктивность - LC фильтр нижних частот (срезать верхние) на частоту среза около 10 КГц. Тут есть програмная фильтрация: http://www.mdpi.com/1424-8220/8/4/2886/pdf. Взято из коментов к статье: http://habrahabr.ru/blogs/DIY/131502/, там же про фильтры Калмана: http://habrahabr.ru/search/?q=%D1%84%D0%B8%D0%BB%D1%8C%D1%82%D1%80%D1%8B+%D0%9A%D0%B0%D0%BB%D0%BC%D0%B0%D0%BD%D0%B0 - не уверен что поможет, но попробовать стоит. Вот тоже небольшая дискусия: http://roboforum.ru/forum86/topic9320.html
• Смещение нуля акселерометра - можем по началу вообще не сражаться с ним - просто запомни что является нулем и сделай вокруг этого значения 1-3% (можно даже 5%) зону нечувствительности, все что выходит за эту зону - линейно влияет на выходное ускорение. (то есть игнорировать ускорения до 0.01 g, например).
• Калибровать акселерометр не просто. Можно калибровать поворачивая акселерометр на разные стороны, по очереди каждую ось вертикально подставляя под вектор ускорения свободного падения - он должен ровно засвечиваться в показаниях оси как 1g, но поставить акселерометр на ребро так ровно - не знаю как. Постоянную скорость поддерживать сложно, но можно определить уровень ошибки акселерометра (а это покажет какая из осей говорит неправду), допустим пройтись с акселерометром по комнате, и затем вернуть его назад на то же место (с точностью до миллиметра). Если интегральная величина - путь - равна нулю - то есть перемещения небыло как фактически, так и по показаниям, - то все хорошо. Если присутствовала ошибка, то её будет четко видно и ты будешь знать в какую сторону плыли значения. Нам акселерометр может быть полезен для определения пройденного пути, думаю в первую очередь, во вторую - для определения текущей скорости. В последнюю - чтобы знать текущее ускорение и направление земли.
• Целевая скорость UART - 57600 (вообще говоря можно такие скорости: 9600, 19200, 38400, 57600, 115200 - все эти скорости поддерживаются в LINUX/POSIX/Windows). Биты данных - от 6 до 8 (возможно проще всего 8), четность - нет/чет/нечет, стоповые биты: 1 или 2 (1.5 не поддерживается в Linux). Управление потоком отсутствует (хотя можно аппаратное или XON/XOFF - не пробовал). Давай, может, условимся пока-что baud rate = 57600, data bits = 8, parity = none, stop bits = 1 - если такие подойдут - напиши. Нам еще нужно не просто передавать данные будет, а сделать так чтобы ошибки передачи небыло:) Во первых нужен точный кварц, а во вторых - помехоустойчивое кодирование - можно ввести контрольную сумму в пакет данных или кодом Хэмминга кодировать - не сложно - могу написать функцию кодирования (на C). Кодирования (предмета) у вас еще не было и тебе может быть с ним сложно, так что я тебе с этим буду помогать, если надумаешь сделать передачу надежной настолько - можно во второй версии такое сделать, а сейчас просто контрольную сумму бит.
• Формулу для восстановления вектора направления земли (вектор 1g) из трех выходных координат (от результата акселерометра) - я напишу тебе в ближайшее время.

С уважением, Executor 20:15, 24 November 2011 (EET)

UPD Есть еще один способ борьбы с шумами - поставить еще один акселерометр параллельно, такой-же точно, а значения усреднять. Если интересно попробовать - пиши - я его закажу. Время доставки его около 2 недель, так что говори заранее:) С уважением, Executor 20:18, 24 November 2011 (EET)

• Ага, я такой алгоритм калибровки примерно и реализовываю, причем значения при +-1g записываются при калибровке в ПЗУ, а при перезагрузке оттуда вынимаются. Управлять калибровкой будем, естественно, через UART.
• Про интерфейс. ИМХО, 57600 кагбе многоватенько, да и с моим "стендом для изучения сигналов на длинных линиях (тм)" в роли удлинителя для ком-порта всяко проглючивает, поэтому стартовый (при инициализации) битрейт будет 38400 (пока, может потом еще меньше сделаем). А уже дальше можно будет с помощью интерфейса настроить скорость, какая душе угодна. Биты - 8 (не издеваемся над бедными программерами=) ), на всю остальную фигню можно забить; обычно кроме того, что ты написал ничего не применяется (по крайней мере, я на практике не видел). Управление потоком - нах, так как у нас вылезет необходимость отфильтровывать некотроые символы, а это не есть гут. На счет кодирования и проч. - а что џрџ не пойдеть? -_-
• по поводу шумов Да, он был неподвижный, причем с уменьшением чувствительности до 600мВ/g они пропали. Что очень странно, это то, что меговская АЦП с предделителем 128 шумит больше чем с без него 0_о хотя, возможно, это был мой косяк или что-то еще. Но в принципе, это не так критично, все равно двигатели быудуть жжжжужжжать и платформу будеть трясти )) C мух. Swinemaker 22:29, 24 November 2011 (EET)
• Допољњењїе. Я пока решил оставить всю математику на float'ах, так как зарылся в реализацию синусов, деления и ко. и немного напугался =). Но оно, походу, и так достаточно быстро соображает. Так вот. В каком виде принято float'ы передавать через UART? Swinemaker 22:35, 24 November 2011 (EET)

• 38400 - норм. CRC как в качестве контрольной суммы (проверки битов посылки) подходит. В качестве излишка может добавим код Хэмминга (помехоустойчивый) паралельно CRC, чтобы восстанавливать искаженные биты (например два искаженных на 8 бит посылку).
• шумы - Ок))
• float - Ок. Можешь передавать их в чистом виде. Как принято - не знаю - не пробовал флоты передавать)). Там 32 битный float?

	// разбираем
	float my_float = 999.99f;
	char f0 = ((char*)&my_float)[0];
	char f1 = ((char*)&my_float)[1];
	char f2 = ((char*)&my_float)[2];
	char f3 = ((char*)&my_float)[3];
	send(f0);
	send(f1);
	send(f2);
	send(f3);
 
	// --------- передача по проводам
 
	// собираем
	char r0,r1,r2,r3;
	receive(r0);
	receive(r1);
	receive(r2);
	receive(r3);
 
	float my_value;
	((char*)&my_value)[0] = r0;
	((char*)&my_value)[1] = r1;
	((char*)&my_value)[2] = r2;
	((char*)&my_value)[3] = r3;

С уважением, Executor 04:43, 25 November 2011 (EET)

Ну да, float обычный, 4-х байтный. Я это к чему: есть стандартная функция преобразования их в текст. строку - но думаю, это немного "неприлично" =) ([3]). Просто передача с таким разбором переает данные эээ.. как бы без смыслы что ли (?). Хотя, в принципе, они так в памяти и хранятся, чего уж тут. С пятницой, Swinemaker 16:45, 25 November 2011 (EET)

Да, показания в строку превращать - вычислительно накладно. Лучше всего их поместить в структуру полей

// эту структуру неплохо-бы расширить так чтобы её потом не менять,
// например добавив пару резервных 
typedef struct ACCELEROMETER_DATA
{
  byte request_id; // номер ответа равен номеру запроса или уникальный
  //номер сообщения (можно до 32 байт)
  float ax,ay,az; // ускорения
  float vx,vy,vz; // текущая скорость
  float sx,sy,sz; // текущий путь от времени запуска или момента сброса пути
 
  unsigned char crc8; 
  // CRC8 http://www.rajivchakravorty.com/source-code/uncertainty/multimedia-sim/html/crc8_8c-source.html
} TAG_ACCELEROMETER_DATA;
 
struct ACCELEROMETER_DATA my_data;
 
// заполняем важными данными
my_data.ax = 999.0f;
 
// функция передачи массива данных через UART
// bool send_uart(void *data_ptr, unsigned char number_of_bytes_to_send);
 
send_uart(&my_data, sizeof(ACCELEROMETER_DATA));

• Нужно обсудить формат запроса-ответа. Например мы можем ввести режимы когда:

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

2) решить проблему можно начав передачу только после подачи сигнала компом, о том, что теперь можно начать передавать данные, и контроллер каждые 0.05-0.1 секунды будет выдавать пакет с текущими показаниями скорости и ускорения

3) нужно, чтобы в этом режиме (free-mode) комп говорил с какой частотой выдавать данные твоему контроллеру, если он хочет их получать периодически.

4) лучше всего ввести еще один режим, при котором комп сначала делает запрос, в виде пакета. В пакете содержится уникальный номер запроса - например от 0 до 65535. Ты копируешь этот номер в ответ, и говоришь - вот на твой запрос номер 999, я даю тебе ответ, и отправляешь все данные структуры. А комп будет их готов получать. Наподобие режима клиент-сервер, где твое устройство выступает в режиме сервера, а комп - в виде клиента, периодически запрашивая обслуживание.

С уважением, Executor 12:39, 26 November 2011 (EET)

Показания гироскопа (!)

Тут еще такой момент: по расчету, везде показания гироскопа в радианах. Однако, он же возвращает нам величину изменения угла (рад/сек). То есть, нужно сначала преобразовать это изменение в величину отклонения от нормального, которое предварительно задать с помощью указания с контроллера о том, что сейчас позиция модуля горизонтальная, а в самой процедуре readGyro(Vec3 *retVecAccel) мы читаем не сами показания акселерометра, а это предварительно обсчитанное отклонение, или как? Swinemaker 18:32, 25 November 2011 (EET)

Все уже ясно. ТруЪ, Swinemaker 23:43, 26 November 2011 (EET)

Да, измерения гироскопа идут в величинах омега - угловая скорость (смотри ниже, readGyro в коде возвращает сами углы, пока-что). Нам да, вообще надо сначала чтобы комп сказал - вот сейчас "НОЛЬ по всем осям", или "запиши текущий угол осей как такой Фи_X = 0.1 радиана, Фи_Y = 0.02 радиана, Фи_Z = 0.3 радиана". Это можно организовать в виде считывания структуры от компа:

typedef struct GYRO_CALIBRATION
{
  float Phi_X; // на момент приема, считать текущий угол отсчета таким (в радианах)
  float Phi_Y;
  float Phi_Z;
};

readGyro - возвращает нам текущие углы. (в исходнике расписано не удобно для тебя - MY FAIL. Я должен поправить, поскольку чистые значения показаний гироскопа - текущие угловые ускорения. Поэтому туда добавятся функции преобразования текущий данных датчиков, в искомые данные (углы)).

readAccelerometer - чистые показания акселерометра в м/с², без поворота на угол гироскопа.

Правильно: readGyro(Vec3 *retVecGyro) - то есть угол поворота робота. Я сейчас все распишу в исходном коде на странице статьи.

P.S.>И еще одно, я сам не любитель радианов, если честно. Но мы должны их использовать, поскольку это стандарт СИ, и поскольку все функции математики принимают углы именно в радианах.

С уважением, Executor 06:13, 27 November 2011 (EET)

Эх, пафос-пафос) ... Мы должны их использовать потому, что стандартные функции работают с радианами =). Сейчас вот новая проблема появилась - постоянная аддитивная ошибка... ррррръъъ

Вот возвращаем мы из гироскопа угол:

void QuickConvert(void) {
	// radian
	 //USART_Send_byte(TCNT2);
	CurrentAngle_UnitSystem.x += ( ( (float)Advanced_run_ADC(ADC_SRC_C) - (float)RG30_AxisCalibrationC )/128 ) * (1/(float)GYRO_UPDATE_FREQUENCY);
	CurrentAngle_UnitSystem.y += ( ( (float)Advanced_run_ADC(ADC_SRC_B) - (float)RG30_AxisCalibrationB )/128 ) * (1/(float)GYRO_UPDATE_FREQUENCY);
	CurrentAngle_UnitSystem.z += ( ( (float)Advanced_run_ADC(ADC_SRC_A) - (float)RG30_AxisCalibrationA )/128 ) * (1/(float)GYRO_UPDATE_FREQUENCY);
 
	#ifdef SW_ALPHA_VERSION
	 //USART_Send_Pstring(PSTR("\r\nOK"));
	 //USART_Send_byte(TCNT2);
	#endif
}

А калибровку я уже давно написал, как же без нее, но

Аддитивная ошибка

Во-первых мне нужны кварцЫ. То есть, завтра поеду за ними - 16 000 000 и 32 768. (я как-то думал что Мега 16-я может работать от встроенного генератора на 16МГц =)) Во-вторых вылезла ошибка, нужно ее как-то фильтровать будет... (может, таки нужен внешний АЦП) Вот, что получилось: LOGFILE (это угол) Бррр.. я уже умал, что это модуль поз воздействием какой-то силы немного вращается, но по идее не должен - всю ночь ведь он в состоянии спокойствия лежал, или это у меня дом крутится? 0_о

Это за всю ночь? Не уверен что у гироскопа достаточно точности, но, если это вся ночь - то да, это вращение планеты (ну и шум тоже). Нужно отделить "мухи от котлет". Нужно немного подумать еще (мне). Я завтра буду, кварц точный принесу, и все остальное что давно обещал, включая припой. С уважением, Executor 17:38, 27 November 2011 (EET)

Ыыыыыы... Не, это не вся ночь! это 10 минут... =(

Кварџ в каком корпусе? Swinemaker 17:40, 27 November 2011 (EET)

Кварц стандартный двухвыводный, высокий. http://electrcomponent.90mb.ru/pict/1299318393399632.jpg.

10 минут - мало слишком для планеты, согласен:) Сделай 1% корридор нечувствительности пока-что. С уважением, Executor 17:44, 27 November 2011 (EET)