ArduCopter Mega: порт на обычную Arduino (тестим)
Александр, но почему тогда система быстро не “доплывает” в горизонт за счет акселей? “Поколбасил” плату и замер, держу ровно и в покое: Гиры = 0, но аксели сигналят, что наклон 90-120-180 градусов!
С этим согласен, влияние акселей видимо значительно задушено в IMU…
Гиры явно работают, потому что отлично ловят быстрые короткие наклоны (такие в полете происходят)
У меня “доплывание” на 20-30гр. вопроизводилось и при разовых оч. резких наклонах (гира 3205->3200), потому и решил что не хватает пирату скорости ловить быстрые ускорения, т.к. в мультивии поблема с доплыванием на разовые резкие повороты вопроизводилась всего 3-5гр.
з.ы. хотя вот вспомнил, что тогда я не учел что НЧ фильтр в вии был 256гц а в пирате 20гц… может в этом дело, т.к. если я правильно понимаю то чем ниже значения фильтра, тем ниже способность гиры измерять высокие/быстрые угловые ускорения…
влияние акселей видимо значительно задушено в IMU
не хватает пирату скорости ловить быстрые ускорения
Поэтому смысл покупать крутые датчики (?). Данные с них быстро идут, а обработка тормозит. И я не могу сказать, что AllinOne - “шумная”, очень даже нормальная.
Возможно, что система не рассчитана на работу в предельных условиях. Небольшие наклоны до 30 градусов она отлично отслеживает. Наверное, усложнение кода, чтобы он мог вернуть коптер из кувырков, вызовет рост времени цикла и задержки.
—
Нашел _adc_constraint = мое значение 12000; // 360degree было 5100
AP_IMU_Oilpan::_sensor_in(uint8_t channel, int adc_value)
{
adc_in = adc_value;
// constrain sensor readings to the sensible range
if (fabs(adc_in) > _adc_constraint) {
adc_constraints++; // We keep track of the number of times
adc_in = constrain(adc_in, -_adc_constraint, _adc_constraint); // Throw out nonsensical values
}
return adc_in;
}
никто OSD от МегаПиратоСамолета не ковыряет щас? а то занятся, а друг уже есть =)
Увы нет… А хотелось бы, квадрик все же это только низко и близко, а иногда хотца далеко и высоко 😃 Поэтому самолетный вариант обновленный бы точно не помешал 😃
я чето застрял с прошивкой e-osd, avrdude умеет шить atmega88, на e-osd atmega88p - вот думаю рассказть ему чтоб забил на различия? или 88 и 88P отличаются принципиально?
avrdude: Device signature = 0x1e930f
avrdude: Expected signature for ATMEGA88 is 1E 93 0A
Double check chip, or use -F to override this check.
udp: обновил avrdude, теперь он знает про 88p =)
upd2: прошился, работает, показывает много инфы… осталось код в пирата вкрутить
Проблема медленного доплывания горизонта решена! Вес акселя в этом процессе был зажат. Я расширил границы “ошибок” определения положения от 1.17 до 30 единиц по модулю. И повысил вес акселя от (0…1) до 10 единиц.
Положение коптера в Планнере мгновенно определяется. Как летает - не знаю.
AP_DCM::drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
//float mag_heading_x;
//float mag_heading_y;
float error_course;
float accel_magnitude;
float accel_weight;
float integrator_magnitude;
//static float scaled_omega_P[3];
//static float scaled_omega_I[3];
static bool in_motion = false;
Matrix3f rot_mat;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
accel_magnitude = _accel_vector.length() / 9.80665f;
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
//accel_weight = constrain(1 - 3 * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0)
accel_weight = 10.0; //повышаем вес акселя, чтобы быстрее доплывал горизонт
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
_health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1); //вроде нигде не используется. сэкономить время и убрать???
// adjust the ground of reference
_error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
// error_roll_pitch are in Accel m/s^2 units
// Limit max error_roll_pitch to limit max omega_P and omega_I
_error_roll_pitch.x = constrain(_error_roll_pitch.x, -30.00f, 30.00f); //было от -1.17 до +1.17 зачем?
_error_roll_pitch.y = constrain(_error_roll_pitch.y, -30.00f, 30.00f); //в raw приходит от -25 до +25, не будем сужать диапазон
_error_roll_pitch.z = constrain(_error_roll_pitch.z, -30.00f, 30.00f);
_omega_P = _error_roll_pitch * (_kp_roll_pitch * accel_weight); //теперь горизонт быстро доплывет!
_omega_I += _error_roll_pitch * (_ki_roll_pitch * accel_weight);
//*****YAW***************
if (_compass) {
// We make the gyro YAW drift correction based on compass magnetic heading
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error
} else {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) {
_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
_course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
if(in_motion) {
error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x); // Equation 23, Calculating YAW error
} else {
float cos_psi_err, sin_psi_err;
// This is the case for when we first start moving and reset the DCM so that yaw matches the gps ground course
// This is just to get a reasonable estimate faster
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
// Ryx = sin psi err, Ryy = cos psi err, Ryz = 0
// Rzx = Rzy = 0, Rzz = 1
rot_mat.a.x = cos_psi_err;
rot_mat.a.y = -sin_psi_err;
rot_mat.b.x = sin_psi_err;
rot_mat.b.y = cos_psi_err;
rot_mat.a.z = 0;
rot_mat.b.z = 0;
rot_mat.c.x = 0;
rot_mat.c.y = 0;
rot_mat.c.z = 1.0;
_dcm_matrix = rot_mat * _dcm_matrix;
in_motion = true;
error_course = 0;
}
} else {
error_course = 0;
in_motion = false;
}
}
_error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
_omega_P += _error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector.
_omega_I += _error_yaw * _ki_yaw; // adding yaw correction to integrator correction vector.
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
integrator_magnitude = _omega_I.length();
if (integrator_magnitude > radians(300)) {
//_omega_I *= (0.5f * radians(300) / integrator_magnitude); // Why do we have this discontinuous? EG, why the 0.5? Я тоже не знаю, убрал
}
}
конец.
--------------------
Другая идея. Показания акселя делятся на 4 (adc_value[4] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])) >> 2; ).
То есть, разрешение снижается искуственно. Если в AP_ADC_ADS7844.cpp убрать деление (adc_value[4] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])); ), то диапазон значений возрастает.
Но это влечет за собой:
AP_IMU_Oilpan.h _gravity = 11200, а не 2730; ///< 1G (For BMA180) in the raw data coming from the accelerometer
_accel_scale = 9.80665 / 11200, а не 2730;
_adc_constraint = ? станет больше ? 5100; // 360degree
_accel_total_cal_change = ? больше? 4.0;
_accel_max_cal_offset = Больше? 250.0;
Возможно, где-то еще будут переполнения. Если купить серьезные аксели и гиры, то код придется адаптировать(?)
Другая идея. Показания акселя делятся на 4 (adc_value[4] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])) >> 2; ). То есть, разрешение снижается искуственно. Если в AP_ADC_ADS7844.cpp убрать деление (adc_value[4] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])); ), то диапазон значений возрастает. Но это влечет за собой: AP_IMU_Oilpan.h _gravity = 11200, а не 2730; ///< 1G (For BMA180) in the raw data coming from the accelerometer _accel_scale = 9.80665 / 11200, а не 2730; _adc_constraint = ? станет больше ? 5100; // 360degree _accel_total_cal_change = ? больше? 4.0; _accel_max_cal_offset = Больше? 250.0; Возможно, где-то еще будут переполнения. Если купить серьезные аксели и гиры, то код придется адаптировать(?)
Это не так. BMA180 имеет 14 бит разрешение, вот на эти 2 бита и смещается значение. Так что все там правильно.
По гире, я думаю проблема в том, что она работает на частоте 250Гц, что не достаточно для отработки быстрых перемещений. Надо повышать частоту, например до 500Гц или 1КГц. Предполагаю, что надо задействовать INT Gyro, что бы успевать считывать с нее данные.
Проблема медленного доплывания горизонта решена!
rcopen.com/forum/f123/topic232641/2963
Вот что бывает после болтанки в руке:
www.youtube.com/watch?v=ELLuJsubUr4
Это не так. BMA180 имеет 14 бит разрешение, вот на эти 2 бита и смещается значение. Так что все там правильно.
В мультивие BMA180 смещается >>5. Странно, но ладно.
По гире, я думаю проблема в том, что она работает на частоте 250Гц, что не достаточно для отработки быстрых перемещений. Надо повышать частоту, например до 500Гц или 1КГц. Предполагаю, что надо задействовать INT Gyro, что бы успевать считывать с нее данные.
Это было бы отлично! По гире видно, что очень быстрые наклоны она не отрабатывает, кратковременные скачки не успевают захватиться и интегрироваться.
Только что отлетал два пака на 44альфа2. Летает стабильно. Один раз был глюк. Летал на алт-холде в полутора метрах от земли. На стоковых пидах колебания порядка 50 см. При переключении в стабиле коптер стал на бок и вошел в планету. После пробывал много много раз менять режимы в руках - такого не было. РТЛ работает, но скорость возврата гораздо больше чем на 40 прошивке
итак, OSD, который писал Олег, но так и не добавил в коптерную версию прошивки
пока что работает только авиагоризонт (остальное не тестилось), остальное вернуть не проблема - надо только значения с нужных параметров отсылать
прошивал через ардуинку (скетч ArduinoISP) + avrdude:
/usr/local/bin/avrdude -p m88p -P /dev/ttyUSB0 -c avrisp -b 19200 -e -U flash:w:mpo.hex
Запаять провод на 30 ногу (на фотке тут 29, 30 и 31 почти слились, припаивать надо к 3 ноге, считая сверху слева, крайне не удобно паять!), а другой конец на TX1, в прошивке (SVN) раскоментить //#define OSD ENABLED
распиновка E-OSD (стоит перепроверить)
судя по коду он должне корректно показывать высоту (в метрах), вместо компаса (то что сверху над горизонтом) - направление домой, скорость берется от GPS, дистанция до дома тоже
а еще у меня не влезает строчка со статусами аппы/гпс и собственно сами гпс координаты =) строк чтоли не хватает в камере
некорректно показывает режим полета (расчитано на самолет, надо перешивать сам e-osd с новыми значениями)
данные о батарекий берет не с самой OSDшки =( так что придется (даже надо, особенно для автополета) допаивать делитель
зы: думаю заказать еще одну OSDшку, малоли спалю =)
Проблема медленного доплывания горизонта решена! Вес акселя в этом процессе был зажат.
Видимо в этом и было дело. Ща вот вспомнил, что пробовал на днях в DCM мультивии уменьшать вес акселя и получал тотже эффект доплывания, НО судя по всему это сделано в АС не просто так. Из личных наблюдений, аксель более чувствителен к вибрациям (нунчак по аналогу или bma020 в сравнении с гирой 3205). И видимо если уменьшить его вес в DCM, то углы будут высчитываться в основном на показаниях гиры, которая менее чувствительна к вибрациям.
В мультивии стабмоде так и не полетел в итоге (сегодня тестил на bma020), т.к. только дашь газку аксель сходит с ума и никакими предварительными калибровками не равняется, хотя в ГУИ при выключенных моторах красота!
Надо будет существенно увеличить GYR_CMPF_FACTOR и попробовать подлетнуть…
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#define GYR_CMPF_FACTOR 310.0f
аксель более чувствителен к вибрациям
Да, он гораздо более шумный.
углы будут высчитываться в основном на показаниях гиры, которая менее чувствительна к вибрациям.
- Ровный полет: вес акселя =1
- полет с перегрузками (или вибрация): вес акселя -> 0 accel_weight = constrain(1 - 3 * fabs(1 - accel_magnitude), 0, 1);
Испытывал на оригинальной прошивке. Горизонт доплывает очень медленно (минуты) после кувыркания, если плата вибрирует.
Попробую вечером сохранить большой вес акселя, но жестко фильтровать его (accel_filtered). Еще в IMU надо покопать, там тоже какие-то ограничения есть.
kp_roll_pitch(0.05967) ki_roll_pitch(0.00001278) и error_roll_pitch тоже можно потестировать.
никто OSD от МегаПиратоСамолета не ковыряет щас? а то занятся, а друг уже есть =)
Глеб, а в чем заключается суть портирования? Изменения портов ввода вывода, или все гораздо сложней?
Потестил сегодня режим авто… эх нравится мне 40 портированная прошивка (тест8), РТЛ работает, полет по точкам работает, высоту держит! Вот ее бы портировать на самули а? Есть умельцы в русских селеньях 😃?
Кто нибудь пробовал использовать habrahabr.ru/blogs/DIY/125214/ (не реклама) заместо APC220? Сделал по инструкции к APC220 (поменял скорость) все подключилось но в планере показывает что плата повернута влево градусов на 190, хотя через юсб кабель нормально показывает. Прошивка родная (black vortex).
Подскажите что нужно изменить в MegaPirateNG_2.0.40_Test8 чтобы работал на black vortex?
Спасибо!
Посмотрите надцать страниц ранее. Или спросите Хатуля
Глеб, а в чем заключается суть портирования?
портирование OSD? или самого ArduCopter?
в OSD надо тупо поменять значения режимов (и для моей камерки с 420 линиями еще и сдвиги для блока с тестом и графикой, а то за пределы всё уходит), в ардукоптере сложнее: code.google.com/p/megapirateng/wiki/portmanual (кто-то пополет по мере пиления?)
Кто нибудь пробовал использовать habrahabr.ru/blogs/DIY/125214/ (не реклама) заместо APC220?
Я использовал, тока не помню, что бы у меня была разница с USB.
Это было бы отлично! По гире видно, что очень быстрые наклоны она не отрабатывает, кратковременные скачки не успевают захватиться и интегрироваться.
Вчера думал, куда же прикрутить INT Gyro… так и не придумал, т.к. походу все выводы, на которые можно повесить прерывание - заняты.
Еще можно задействовать INT Baro (как в оригинальном AC), для этого можно задйствовать любой цифровой вывод (которых полно свободных).
И еще, если тут есть специалисты в микроконтроллерах. Допустим мы подключим INT Gyro к одному из выводов и повесим на него прерывание. Как лучше организовать чтение значений гиры? Прямо в коде прерывания наверное нельзя… Видимо надо выставлять какой то флаг готовности и в основном цикле программы, вызывать чтение гиры как можно быстрее…? Но во втором случае, есть вероятность пропустить одну из итераций, т.к. у нас есть “медленный” код, который работает на частоте 50Hz…
UPD: Походу придумал - можно TX Sonar перенести на обычные цифровые выходы, а его пин задействовать для INT Gyro… сегодня попробую вечером 😃
Вчера думал, куда же прикрутить INT Gyro… так и не придумал, т.к. походу все выводы, на которые можно повесить прерывание - заняты.
Я думал сделать супер_быстрый цикл 1 кГц, в нем проверять флаг готовности. Gyro INT подать на d**. Если d** = 1, то считаем с гиры.
Нужно узнать время на считывания гиры и интегрирование, от этого задавать время сверх_быстрого цикла.
Как потом отдавать интегрированное значение в DCM ?
------------
Или в супер_быстром цикле 1 кГц проверять флаг готовности. Gyro INT подать на d**. Если d** = 1, то считаем с гиры, значение акселя берем старое, все это подаем в DCM.fast_update();
можно TX Sonar перенести на обычные цифровые выходы, а его пин задействовать для INT Gyro
А что вызывать в коде прерывания? readAHRS() ? Или считывать гиро и копить, потом отдавать в IMU ?
Или в супер_быстром цикле 1 кГц проверять флаг готовности. Gyro INT подать на d**. Если d** = 1, то считаем с гиры, значение акселя берем старое, все это подаем в DCM.fast_update()
DCM.fast_update можно не вызывать. Надо просто суммировать показания гиры, до следующего считывания DCM’ом (а он обновляется в fast_loop). Т.е. идея такая - в супер быстром цикле, проверяем флаг гиры, если установлен - считываем показания и суммируем с предыдущим (при этом запоминаем количество считываний). Далее, раз в 4ms (250Hz) идет fast_loop который в свою очередь обновляет IMU - а тот уже читает значения гиры. Отдаем просуммированные показания и Dt (количество циклов гиры (считанных) * период одного цикла) (сейчас Dt = 4000мкс).
Т.е. идея такая - в супер быстром цикле, проверяем флаг гиры, если установлен - считываем показания
Можно не трогать сонар, а просто повесить gyro_INT на цифровой вход. Только определиться с частотой super_fast_loop, чтобы гира гарантировано успела дать INT=1 и хватило времени проинтегрировать.
Можно не трогать сонар, а просто повесить gyro_INT на цифровой вход
Ну это смотря как его использовать. Если просто как флаг готовности, то да - этого достаточно, а если как прерывание, то его можно повестить только вместо сонара.
если как прерывание, то его можно повестить только вместо сонара.
жалко сонар трогать, придется потом не забывать править его код при портировании новой версии.
Кстати, гира якобы работает на 400кГц, она часто будет вызывать прерывания.