Создание собственной системы стабилизации
“Выкинул” я серву 😦 зря потраченные деньги, хотя может при других обстоятельствах (другом поворотном механизме) она и будет работать, но прицепленная прямо на луч - никогда в жизни.
Причина:
Отклеил наклейки бумажные, а там движок за подлицо с корпусом, он мне и мосх сума и сводил.
Поставил старую серву - небо и земля, никаких дёрганий, подлетнул сегодня даже в сильный ветер (порывами) на прошлом контроллере я себе такого не позволял (ну очень сегодня хотелось) аппарат без повреждений вернулся домой, умудряется висеть даже в акро, ну только плывёт по ветру, в левеле пытается сопротивлятся ветру - красота.
С приёмником всёже есть проблемы, секунд 15-20 аппарат жил своей жизнью, но всё обошлось, надо копить денюжку и срочно менять…
О кстати, до замены сервы, я увидел сценарий того как умер прежний контроллер. При неудачной посадке хвостовым винтом зацепил землю, а так как задний мотор крепится у меня только стяжками - его срывает и он направляется прямо в центр рамы:( , с нынешним контроллером всё обошлось 😃
в левеле пытается сопротивлятся ветру - красота.
это интересно, алгоритм компенсирует линейные ускорения ?
Нет скорей всего это побочный эффект от парусности и ещё не настроенных толком ПИДов - переруливание, но факт остаётся фактом, в акро летит по ветру, а в левеле нет и если порыв стихает то некоторое время летит против ветра пока не выравняется, акро я настроил - держится как хорошо настроенный КУК доволен, а вот с левелом пока ещё борюсь, в основном мне мешала серва, ночью попробую запустить НЛО 😃 , о пока светло посмотреть провода между домами чтоб не вписатся в них…
Всего ИМУ то
#include "board.h"
#include "mw.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int16_t sonarAlt; //to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
// **************
// gyro+acc IMU
// **************
int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
static void getEstimatedAttitude(void);
void imuInit(void)
{
acc_25deg = acc_1G * 0.423f;
#ifdef MAG
// if mag sensor is enabled, use it
if (sensors(SENSOR_MAG))
Mag_init();
#endif
}
void computeIMU(void)
{
uint32_t axis;
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
#define GYRO_INTERLEAVE
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
#ifdef GYRO_INTERLEAVE
gyroADCp[axis] = gyroADC[axis];
#else
gyroData[axis] = gyroADC[axis];
#endif
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
}
timeInterleave = micros();
annexCode();
#ifdef GYRO_INTERLEAVE
if ((micros() - timeInterleave) > 650) {
annex650_overrun_count++;
} else {
while ((micros() - timeInterleave) < 650); // empirical, interleaving delay between 2 consecutive reads
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
}
#endif
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
static int16_t gyroSmooth[3] = { 0, 0, 0 };
if (Smoothing[0] == 0) {
// initialize
Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
}
}
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by
//
// adapted by ziss_dm :
//
// The following ideas was used in this project:
// 1) Rotation matrix:
// 2) Small-angle approximation:
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks:
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Modified: 19/04/2011 by ziss_dm
// Version: V1.1
//
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
// **************************************************
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 4
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#define GYR_CMPFM_FACTOR 200.0f
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)cfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
// #define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result (ITG-3200)
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
struct fp_vector v_tmp = *v;
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
}
static int16_t _atan2f(float y, float x)
{
// no need for aidsy inaccurate shortcuts on a proper platform
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
static void getEstimatedAttitude(void)
{
uint32_t axis;
int32_t accMag = 0;
static t_fp_vector EstG;
static t_fp_vector EstM;
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale;
if (cfg.acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
accSmooth[axis] = accLPF[axis];
} else {
accSmooth[axis] = accADC[axis];
}
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
if (sensors(SENSOR_MAG)) {
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
}
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle);
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
f.SMALL_ANGLES_25 = 1;
else
f.SMALL_ANGLES_25 = 0;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)cfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
if (sensors(SENSOR_MAG)) {
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
}
// Attitude of the estimated vector
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
#ifdef MAG
if (sensors(SENSOR_MAG)) {
// Attitude of the cross product vector GxM
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);
heading = heading + magneticDeclination;
heading = heading / 10;
if (heading > 180)
heading = heading - 360;
else if (heading < -180)
heading = heading + 360;
}
#endif
}
#ifdef BARO
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
void getEstimatedAltitude(void)
{
uint32_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static uint32_t BaroHistIdx;
static int32_t BaroHigh = 0;
static int32_t BaroLow = 0;
int32_t temp32;
int16_t last;
if ((int32_t)(currentTime - deadLine) < 0)
return;
deadLine = currentTime + UPDATE_INTERVAL;
//**** Alt. Set Point stabilization PID ****
//calculate speed for D calculation
last = BaroHistTab[BaroHistIdx];
BaroHistTab[BaroHistIdx] = BaroAlt / 10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
BaroLow += BaroHistTab[index];
BaroLow -= last;
BaroHistIdx++;
if (BaroHistIdx >= BARO_TAB_SIZE)
BaroHistIdx = 0;
BaroPID = 0;
//D
temp32 = cfg.D8[PIDALT] * (BaroHigh - BaroLow) / 40;
BaroPID -= temp32;
EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && abs(BaroPID) < 10)
BaroPID = 0; // remove small D parameter to reduce noise near zero position
// P
BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150
// I
errorAltitudeI += temp32 * cfg.I8[PIDALT] / 50;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
temp32 = errorAltitudeI / 500; // I in range +/-60
BaroPID += temp32;
}
#endif /* BARO */
Всего ИМУ то
у меня и того меньше, правда там пока баро нет.
Эх, гулять так гулять, купил я F407-й, впринципе скоро будет полный комплект, вопрос с барометром пока открытый (на первых порах BMP085, ну а там уж, что нибудь надо думать, схемно заложить - выпадет оказия допаять)…
С маленькой продолжаю войну с ПИДами - плаваел немного по сторонам…
Какой титанический труд с написанием программ.Ждем результата с нетерпением и лучше бы заложить сразу хороший баро,5611 например.
Я б с удовольствием, но где его взять чтоб доставка не была в три-четыре раза дороже 😦
на гудлаке есть такой goodluckbuy.com/ms5611-01ba03-ms5611-pressure-mete… и такой goodluckbuy.com/ms5607-02ba01-ms5611-digital-24bit… , брать?
Чёт с портами запутался, зачем - если не сказать хуже, два ресета с JTAG - бррр, пытаюсь как-нибудь упорядочит входы выходы, а то Multipilot32 извините бред какой-то… я про 407 если что…
Всего ИМУ то
ыть узнаю вий…
блин у меня сейчас большое желание переписать его на FreeIMU (там калман и вроде как шустрый) т.к. хмега позволяет его засунуть… если получиться могу потом поделится
Хочу выходы сделать так:
Motor1 - PA1 t2cn2
Motor2 - PA2 t2cn3
Motor3 - PA3 t2cn4
Motor4 - PB0 t8cn2
Motor5 - PB1 t8cn3
Motor6 - PB6 t4cn2
Motor7 - PB8 t4cn3 /SCL
Motor8 - PB9 t4cn4 /SDA - это если регули I2C
ну если что, то порты местами поменяю - для того чтоб легче плату разводить, надеюсь подводные камни меня не ждут, а то помню в мультивиие долго потом ковырял переназначивая порты и прерывания…
А входы я с гордостью сопру у Олега (Siberian) надеюсь не обидится 😃
Что-то мне подсказывает что контроллеру на F407 и MPU6000 суждено быть
Почему не на 405м? Датчики СПИ? Линии готовности данных ДУС и акселя выведены на прерывания? ЮСБ с поддержкой штатного загрузчика? Свободный СПИ и АЦП будут? СДИО? Полностью аппартный ШИМ вход/выход?
Если вспомню ещё допишу)))
Я б с удовольствием, но где его взять чтоб доставка не была в три-четыре раза дороже на гудлаке есть такой www.goodluckbuy.com/ms5611-01...er-module.html и такой www.goodluckbuy.com/ms5607-02...or-module.html , брать?
бери здесь, www.aliexpress.com/item/…/557622969.html
я с китайцем договаривался, он за несколько деталек с меня одну доставку взял.
Сергей, ты наверное интересовался, насколько хорош аксель МПУ6000, например, по сравнению с ЛСМ330?
ЗЫ если на верхние вопросы ещё нет ответов, можешь считать предложением )
Начну с конца, аксель однозначно лучше, меньше боится вибраций и не дёргается как у ЛСМ сам по себе, есть готовые драйвера, одна шина и я его купил 😃 , естественно на этот раз датчики СПИ 😃
На алиекспресс доставка в Беларусь самая дешовая 45, а так как мне осталось прикупить только баро (его тоже можно по СПИ пользовать) то уж лучше на гудлаке - осталось то 10 рублей американских найти…
на счёт свободного СПИ - врятли, завязана MicroCD, флешка маленькая (закос под ОР), МПУ и возможно баро. Отдельные UART на GPS, телеметрию, и возможно программирования на первых порах. Вообще планируется USB - но так всё туманно расписано, либо же брать другой бутлоадер, либо же извращатся, ну до ПО ещё далеко ДУС с МПУ планирую, ШИМ надежда на аппаратный ещё есть 😃 , АЦП свободные будут, но вопрос, а буду ли я их разводить - посмотрим, по крайней мере батарейка мне понравилась в назе32 - так что вход для неё обязательно будет, хочу и не хочу управление подсветкой - делать, не делать - всё будет зависеть от загруженности платы (нижней), а ещё же сонар куда-то всунуть надо 😃 вобщем копать не перекопать - но интересно ж зараза 😃
Блиин - думал же что что-то забыл, надо ж было вместе с процем FT232 заказать - JTAG сделать хррр…
а ещё же сонар куда-то всунуть надо
для сонара делал выход с конвертером на 2 резисторах и полевике - им нужен сигнал не меньше 4.3в иначе нестабильно работают(SR04), вход - чисто резистивный делитель, итого 2 ноги проца. кстати с норм кодом обработки сонара замучался (корректное определение норм данных/ошибки), как исходники использовал код от alexmos, в итоге вроде норм получилось - могу поделиться
для сонара делал выход с конвертером на 2 резисторах и полевике - им нужен сигнал не меньше 4.3в иначе нестабильно работают(SR04), вход - чисто резистивный делитель, итого 2 ноги проца. кстати с норм кодом обработки сонара замучался
Для СТМа бесполезное шаманство, входы примут+5В, выход с открытым стоком и резистор на +5В.
Сергей, я вот все читаю вашу тему и никак не могу вкурить, какой смысл пытаться повторить чужие разработки? ИМХО, если немного почитать про плюсы и минусы существующих контроллеров, можно вполне наваять свой - который возможно пойдет в массы. Например взять CRIUS AIOP - все в нем хорошо, еще немного и будет совсем хитовым контроллером. Если немного посмотреть вперед, пора делать похожий но на базе ARM. А прошивки постепенно подтянутся 😃
Тот же ArduCopter компилится под F4 при большом желании (Проект Multipilot32)
Алексей, я тоже что-то не вкурил 😃
какой смысл пытаться повторить чужие разработки?
это вы у Таймкопа спросите 😃
и
Если немного посмотреть вперед, пора делать похожий но на базе ARM.
в чём вопрос, я на данный момент, как раз ухожу в сторону (точнее собираю солянку из того что мне нравится), оно уже не ArduCopter, не Multipilot32, не разработки OP и не Multiwii, хотя близко, я не сделаю свою наземную станцию, так что всё равно придётся к чему-то привязатся. Я в первую очередь делаю железяку для себя и никакой коммерческой составляющей в этом нет (и если кто-то захочет - может повторить, но сам), получается контроллер с разными плюшками, но дешевле чем продающиеся в той же комплектации, ну и как следствие - я его знаю и знаю что могу ожидать от него. Я делаю это для души - мне это нравится! 😃
Для СТМа бесполезное шаманство, входы примут+5В, выход с открытым стоком и резистор на +5В.
у ТС он вообще на голые лапы без всяких шаманств работает 😃
скорей всего барометр по SPI будет, но пока только предположение…
А ещё я очень сильно тоскую по крутилкам в КУКе 😃
Куда лучше воткнуть барометр (MS5611) на нижнюю или верхнюю плату? - сам склоняюсь к нижней…
Его надо защищать от прямых потоков воздух и солнечного света, отсюда и исходите
на нижнюю или верхнюю плату?
подальше от солнца - лучше нижняя
О какая штучка появилась goodluckbuy.com/stm32f407-417zg-module-hy-stm32f4x…
Вобщем, если кто-то захочет поучавствовать в созидании новой платы, прошу вносить предложения пока я не начал разводку (нижняя почти готова)
на нижней плате будут распологаться: STM32F407VET6, MPU6000(SPI), MS5611(SPI), а так же, MicroSD(SPI), W25X40A (SPI), AT24C512BN (I2C), разъёмы.
На верхней: MT3329 (UART), HMC5883 (I2C)
За одно переехать сюда github.com/SergDoc/Nev_MultirotorControl - думаю будет удоней, т.к. я не особо программер 😃
DCMI будет разведен?
А надо?
Не знаю, интересуюсь 😃