ArduCopter Mega: порт на обычную Arduino (тестим)
имхо аксель задушен сильно, когда игрался с кастомными датчиками такой же косяк был - сильновато поделил аксель и в итоге гира сразу показывала, а потом корретировалась задушенным акселем (уезжал обратно)
Если я правильно понимаю процесс, то с точки зрения DCM ничего не должно было изменится. Я считываю показания гиры в 4 раза быстрее, сам усредняю и отдаю в DCM как будто гира работает на 250Гц…
Кстати, посмотрите кто нибудь код инициализации акселя, я правильно понимаю что он на 10Гц работает???
а где код?
Горизонт теперь уплывает быстро! Это говорит о том, что идет частый опрос гиры (и ошибка интегрирования растет быстрее). Аксель отключил. IMU точно следует за быстрыми движениями руки только за счет гиры.
Выкладываю то, что пока есть:
ArduCopter.pde
static unsigned long super_fast_loopTimer; //my new timer
...........
///раздел с LOOP-ами
//мой цикл 500Гц
if ((timer - super_fast_loopTimer) >= 2000) {
super_fast_loopTimer = timer;
super_fast_loop();
}
//конец мой цикл 500Гц
if ((timer - fast_loopTimer) >= 4000) { //250ГЦ, ничего не менял тут
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f;
..........
float r,p,y; //глобальные переменные: по гире roll, pitch, yaw
int adc_val[6];
uint8_t sen[6] = { 0, 1, 2, 4, 5, 6};
static void super_fast_loop()
{
adc.Ch6(sen, adc_val);
y += adc_val[0];
r += adc_val[1];
p += adc_val[2]; //в супер_быстром_цикле 500Гц будем опрашивать Гиру и суммировать значения по осям
}
.......
///обычный цикл 250Гц, ничего в нем не менял.
static void fast_loop()
{
// try to send any deferred messages if the serial port now has
// some space available
gcs.send_message(MSG_RETRY_DEFERRED);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_RETRY_DEFERRED);
#endif
// Read radio
// ----------
read_radio();
// IMU DCM Algorithm
read_AHRS(); //вызывается функция! ДЕЛЬТА= 4000 мс, потому что цикл 250Гц
// Look for slow loop times
// ------------------------
//if (delta_ms_fast_loop > G_Dt_max)
// G_Dt_max = delta_ms_fast_loop;
// custom code/exceptions for flight modes
// ---------------------------------------
update_yaw_mode();
update_roll_pitch_mode();
// write out the servo PWM values
// ------------------------------
set_servos_4();
}
static void read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
hil.update();
#endif
dcm.update_DCM_fast(y,r,p); //отдадим готовые суммированные значения ГИРЫ в эту функцию. А в ней гиру уже не будем считывать!!
y=0; r=0; p=0; // обнулим суммированные значения гиры по осям.
//omega = dcm.get_gyro(); // закомментировал! и не ругается!
}
APM_DCM.h
void update_DCM(void);
float r,p,y; //суммарные значения ГИРЫ, которые мы передаем раз в 4000мс и обнуляем. А суммы накапливаем в супер_быстром_цикле
void update_DCM_fast(float r, float p, float y); //эту функцию вызываем раз в 4000мс из read_AHRS(void) в фаст_лооп
.......
AP_DCM.cpp
AP_DCM::update_DCM_fast(float y,float r,float p)
{
float delta_t;
//мы копили ГИРЫ в течении 4000мс, передали их в эту функцию.
//умножаем на gyro_gain, как это происходит в ap_adc_ads7844.срр
_gyro_vector.x = 0.0012141421*r; //gyro_gain* axis from ap_adc_ads7844.h
_gyro_vector.y = - 0.0012141421*p; //gyro_gain* axis from ap_adc_ads7844.h
_gyro_vector.z = -0.0012141421*y; //gyro_gain* axis from ap_adc_ads7844.h
_imu->update();
//_gyro_vector = _imu->get_gyro(); //У НАС УЖЕ ЕСТЬ ГИРА! НЕ будем вызывать. Get current values for IMU sensors
_accel_vector = _imu->get_accel(); // Get current values for IMU sensors
delta_t = _imu->get_delta_time(); //4000мс - столько мы копили ГИРЫ
.........
-----------------
Гиру запустил на 500Гц i2c_write(0x15); // register Sample Rate Divider
i2c_write(0x1); // 7: 1000Hz/(1+1) = 500Hz
------------------
static void read_AHRS(void)
{
//omega = dcm.get_gyro(); // закомментировал! и не ругается! Но точно не знаю. У меня только плата. Без моторов и полетов!
--------------
Сделал функцию adc.Ch3(sen, adc_val) - возвращает только Гиру. sen[3] = {0,1,2}; adc_val[3] = {y,r,p};
--------------
Pitch & Yaw надо отдавать со знаком минус. Тогда при вращении Yaw быстро достигается нужное положение за счет согласования Гиры и компаса. Питч - видно по наклонам. Если Yaw отдавать с “+”, то идет рывок в противоположную сторону за счет гиры, а потом доводка компасом в обратную.
Выберите “Help” в планере и поставьте в самом низу экрана “Show console” и перезапустите планер. Теперь у вас кроме основного окна, будет еще консоль. Так вот нажимая Ресет в момент подключения, вы там должны увидеть, что ваша Ардуинка пошла на перезагрузку. У меня почему то ресет срабатывает не с первого раза (именно когда MAVLink пытается подцепится, в других случаях ресет отрабатывает нормально)!
Слов нет! Спасибо. Помогло.
я правильно понимаю что он на 10Гц работает???
если 0<<4 это 0000xxxx то на 10гц можно до 1200 Гц поднять =)
Сократил super_fast_loop до 960 мкс, задал ему частоту 1 кГц. Гира тоже на 1кГц.
Спец функция опрашивает только гиру и суммирует с прошлыми значениями, пока fast_loop не отправит в dcm.fast_update(y,r,p) и обнулит.
Вся фишка в том, что в Планнере угол не доворачивается, все равно. Крен на 90 - поворот картинки на 50-60 градусов. Остальное доводят аксели.
Похоже, гнаться за частотой опроса бесполезно. Или надо что-то придумать, не знаю, что.
—
Зато с барометром точно разобрался. Сделал рассчет высоты по общепринятой формуле и повысил разрядность переменной, чтобы прыгающий последний разряд был не 1Па, а 0.01 Па. Теперь надо ждать MS5611 =)
--------
Понял, зачем нужно accel_weight = constrain(1 - 3 * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0)!!!
Когда коптер делает вираж или сильно вибрирует, аксель врет. Он учитывает центробежную силу (и Кориолисову?) или перегрузки от сильной вибрации. Поэтому в этот момент его весовой коэффициент => 0. А когда все спокойно - происходит коррекция горизонта по акселю.
Тут надо экспериментально подобрать ограничения по перегрузкам, чтобы все-таки происходила коррекция по акселю.
Решил попробовать MegaPirateNG.2.0.44
При настройке режимов полета в GUI выбираю ALT_HOLD и STABILIZE.
При этом при переключении в консоли вижу AUTO и STABILIZE. Теперь настраиваю через консоль ALT_HOLD и STABILIZE.
В GUI получаю SIMPLE и STABILIZE. Чему верить… В полете еще не проверял.
Продолжаю с Planer 1.0.76. В GUI 1 - STABILIZE, 6 - ALT_HOLD.
При переключении в терминале меняется STABILIZE и SIMPLE.
По просьбе Александра (ака CSG_EU) выкладываю версию 2.0.42, адаптированную для платы Black Vortex.
Кстати он там представил еще платки для дальнобойной телеметрии (не реклама) www.rcgroups.com/forums/showpost.php?p=19436306&po…
Кстати он там представил еще платки для дальнобойной телеметрии (не реклама) www.rcgroups.com/forums/showp...&postcount=460
120 ойро за комплект… ммм… дороговато однако. Коптероводам наверное не нужна такая дальность (5-10км).
Вот что более интересно для нас, так это AllInOne и FreeIMU с новым барометром 😃
По просьбе Александра (ака CSG_EU) выкладываю версию 2.0.42
а чего он сам не выложит? у его теперь же есть доступ к проеут MegaPirateNG на гуглокоде
а чего он сам не выложит? у его теперь же есть доступ к проеут MegaPirateNG на гуглокоде
Что то он так и не добавил информацию в вики по BV…
Кстати, было бы неплохо, встроить поддержку BV в текущие наши прошивки… вроде там ничего особого не надо менять.
вроде там ничего особого не надо менять.
там имхо вообще ничего не надо менять, только вот не факт что всякие int gyro|baro подведены
только вот не факт что всякие int gyro|baro подведены
Посмотрел код, опрос датчиков не изменился, как и у нас. Только в dcm.cpp delta_time передается внутри каждой функции в виде dcm.function (float _G_dt). Время G_dt = время быстрого цикла. А раньше время интегрирования запрашивалось из IMU при вызове массива датчиков и не зависело от времени основного цикла. Было неудобно менять.
А раньше время интегрирования запрашивалось из IMU при вызове массива датчиков и не зависело от времени основного цикла. Было неудобно менять.
Наоборот, G_dt было в предыдущих прошивках, а начиная с 2.0.43 убрали и время выдается теперь в adc.Ch6. Так что, у него самая обычная прошивка, только параметры заданы по умолчанию, под BV.
А раньше время интегрирования запрашивалось из IMU при вызове массива датчиков и не зависело от времени основного цикла
Кстати, я так и не понял, какое время должна возвращать Ch6? Время за которое у нас есть показания с датчиков (Частота опроса гиры * количество опросов) или все же время между вызовами этой функции?
А когда уже альфа3 выйдет для аллиноне, с новым кодом для барометра и акселя, чтоб висел коптер как на новых китайских мозгах 😃 . Хотя в принципе чего-то особенного при полетах на них я не увидел. Все полеты без ветра. Я думаю, что если пирата нормально отстроить, отбалансировать моторы-винты летать будет не хуже. А на видео, где он по корридору летает, думаю мой квадр на 40 прошивке с отстроеными пидами по сонару пролетел бы так же. При полете над грунтовой дорогой в ветер колебения по высоте были не более 10 см. Высота над дорогой примерно 1м
Кстати, я так и не понял, какое время должна возвращать Ch6? Время за которое у нас есть показания с датчиков (Частота опроса гиры * количество опросов) или все же время между вызовами этой функции?
_цепочка = fast_loop -> readAHRS() -> dcm.fast_update() -> imu.update(): sample_time = _adc->Ch6 чтобы шпионы не догадались)
Время накопления данных с датчиков = 4000 мкс (fast_loop) = время между двумя вызовами Ch6 из fast_loop.
Поэтому я стал отдавать в dcm.fast_update(yaw, pitch, roll, G_dt);
G_dt вычисляется как разность в fast_loop и равно 4000мкс.
yaw, pitch, roll = град\с, которые успеваю накопить за 4000мкс и отдать, потом они обнуляются, и все начинается сначала.
adc.Ch3(sen, adc_val); //моя функция, которая возврщает только ГИРЫ
y += adc_val[0];
r += adc_val[1];
p += adc_val[2];
ADC_ADS7844.cpp
//вспомогательная функция, которая вызывается из моей AP_ADC_ADS7844::Ch3 и возвращает ТОЛЬКО ГИРЫ
void i2c_ACC_getGR() { // ITG3200 read data
static uint8_t i;
i2c_rep_start(0XD0); // I2C write direction ITG3200
i2c_write(0X1D); // Start multiple read
i2c_rep_start(0XD0 +1); // I2C read direction => 1
for(i = 0; i< 5; i++) {
rawADC_ITG3200[i]= i2c_readAck();}
rawADC_ITG3200[5]= i2c_readNak();
#ifdef ALLINONE
adc_value[0] = ((rawADC_ITG3200[4]<<8) | rawADC_ITG3200[5]); //g yaw
adc_value[1] = ((rawADC_ITG3200[2]<<8) | rawADC_ITG3200[3]); //g roll
adc_value[2] =- ((rawADC_ITG3200[0]<<8) | rawADC_ITG3200[1]); //g pitch
#endif
#ifdef FFIMU
adc_value[0] = ((rawADC_ITG3200[4]<<8) | rawADC_ITG3200[5]); //g yaw
adc_value[2] = ((rawADC_ITG3200[2]<<8) | rawADC_ITG3200[3]); //g roll
adc_value[1] = ((rawADC_ITG3200[0]<<8) | rawADC_ITG3200[1]); //g pitch
#endif
}
// возвращает 3 оси по ГИРЕ
uint32_t AP_ADC_ADS7844::Ch3(const uint8_t *channel_numbers, int *result)
{
i2c_ACC_getGR();
for (uint8_t i=0; i<3; i++) {
result[i] = adc_value[channel_numbers[i]];
}
return 0;
}
Так же можно и с акселями. Избавиться от массивной Ch6, отдельно запрашивать из IMU или ADC гиры, отдельно (реже) аксели.
там имхо вообще ничего не надо менять, только вот не факт что всякие int gyro|baro подведены
Я заливал и alpha2 себе на BV. Работало все, кроме GPS. Причем все, что говорил Александр перепробовал - не помогло. Надо будет разобраться что тут такого в этой версии, потому что в ней GPS работает.
GPS работает.
выставить UBLOX (или nmea) в качестве протокола и скорость 9600 вроде, хотя еще есть вариант что там в гпс коде чето хитрое отсылается
косяк с гпс должен быть такой же как у владельцев allinone2 с GPS подключенным через uart(tx rx)
Хотя в принципе чего-то особенного при полетах на них я не увидел. Все полеты без ветра.
[ИМХО] главное не как работает, а то что цена большая - простая логика обычного народа: если дороже, значит лучше
продавали бы пирата за 2-3k$ - про китайца и не заговорили бы =)[/ИМХО]
Время накопления данных с датчиков = 4000 мкс (fast_loop) = время между двумя вызовами Ch6 из fast_loop.
Далеко не факт, может оказаться и 5 и 6 и 7мс, скажем в момент, когда выполняется медленный код на 50Гц.
выставить UBLOX (или nmea) в качестве протокола и скорость 9600 вроде, хотя еще есть вариант что там в гпс коде чето хитрое отсылается
косяк с гпс должен быть такой же как у владельцев allinone2 с GPS подключенным через uart(tx rx)
Ставил и UBLOX, и NMEA, и скорость 9600 ставил - без толку.
продавали бы пирата за 2-3k$ - про китайца и не заговорили бы
+100 😃
скорость 9600 ставил
У меня с хобей только не 4800 работает
Сократил super_fast_loop до 960 мкс, задал ему частоту 1 кГц. Гира тоже на 1кГц.
Только ДУС ITG3200. Видно, что система распознает быстрый поворот (крутил, как только позволяла скорость руки). Ближе к концу ролика совершал плавные круговые движения. Медленное уплывание горизонта - естественное явление для ДУС + интегратор.
www.youtube.com/watch?v=YiD1gTeZbiI
MegaPirateNG 1kHz ITG3200, 250Hz BMA180 (accel_weight = 3*constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0))
www.youtube.com/watch?v=s4V7dDGB5yU
Далеко не факт, может оказаться и 5 и 6 и 7мс, скажем в момент, когда выполняется медленный код на 50Гц.
Значит, нельзя брать константу 4000мкс из Ch6, это значение никогда не меняется! Я сейчас везде использую время интегрирования G_Dt = (float)(timer - fast_loopTimer) / 1000000.f;
------
Кстати, в прошивке для BV используются мс millis(), а у нас - мкс micros():
if ((timer - fast_loopTimer) >= 4) //BV
{
G_Dt = (float)(timer - fast_loopTimer) / 1000.f;