Создание собственной системы стабилизации
Может у вас I2C в контроллере виснет? Или другие датчики нормально читаются?
Ошибок по I2C нет, аксель с магнитометром работают и гиры работают но криво…
Сергей,похоже нашёл косяк в твоей схеме. Те выводы ЛСМ, которые у тебя обозначены как НС, насамом деле
“Res Reserved connect to GND”, насколько я знаком с англосаксонским, то их нужно со схемной землей соеденить. Иначе у тебя лихой дисбаланс в питании, на 5 вводов питания и подтяжку линий всего 1 один стекающий вывод!
У меня так:
У меня они на земле все и проблем с датчиком нет. Может в этом и проблема.
SDO G
Это серьезно, при нуле там режим СПИ включается!
Кстати, ты же ж вроде говорил что у тебя СПИ интерфейс с ЛСМ?
Это серьезно, при нуле там режим СПИ включается!
Нет меняется адрес I2C
Кстати, ты же ж вроде говорил что у тебя СПИ интерфейс с ЛСМ?
я хотел но не сделал эта плата вообще как-то спонтанно получилась - вот захотелось маленькую и SPI я бы на ней не развёл…
Те выводы ЛСМ, которые у тебя обозначены как НС, насамом деле
“Res Reserved connect to GND”, насколько я знаком с англосаксонским, то их нужно со схемной землей соеденить. Иначе у тебя лихой дисбаланс в питании, на 5 вводов питания и подтяжку линий всего 1 один стекающий вывод!
токи там маленькие, ну если глюк не прйдёт закоротить не проблема 😃
Вобщем глюка пока нет, я уже даже таскаю его с “места проживания” на кухню к ноуту - это чтоб соблюсти все выкрутасы для появления глюка 😃
О, надо ещё магнитометр перекалибровать - так на всякий случай…
Сообщение от rual Это серьезно, при нуле там режим СПИ включается! Нет меняется адрес I2C
Да, точно! Но всё равно, этож читать он не понятно что будет…
Вот с какими показаниями сейчас живём, на баро не смотреть он дохлый:
Но всё равно, этож читать он не понятно что будет…
уже исправил вот и тестю 😃
Ну что, подлетнул я 😃 чувства немного смешанные, видео не получилось - пригласил друга поснимать, а он чёт в собственном фотике не разобрался 😦 сказал что снял, а видео мы так и не нашли 😦 есть пару фоток, и так:
- Летать я разучился, в особенности работать газом из пяти раз один только нормально посадил (остальные с подпрыгиванием)
- На удивление аппарат ведёт себя хорошо не смотря на порывы ветра - пару раз отпускал полностью стики давольно на длительное врея (летал в левеле но без баро)
- Надо покурить пиды
- с сервой проблемы - надо сравнить мультивии 1.9 и 2.0, общался с Глебом SovGVD, у него на трёшке таже беда была на 2.0, а на 1.9 всё нормально, так что прежде чем менять серву лучше поэкспериментирую
- купить новый барометр и посмотреть
что ж за монстры такие кабаки да девкикак работает AltHold - Самое приятное ГЛЮКА НЕТ!!!
В общем доволен…
Кривую газа поправь, чтобы в точке висения она была максимально плавной и навык сразу вернется -)
Информация к размышлению
По поводу MPX баро зарубежные коллеги с ними экспериментировали и выяснили, что очень критичны они к шумам по питания, проблема решается отдельной шиной питания с собственным регулятором, утверждают, что не стоит его использование затраченных усилий.
MPX баро зарубежные коллеги с ними экспериментировали и выяснили
в smaltime вроде такие датчики… Есть ссылка на картинки шумов?
с сервой проблемы
может ШИМ на неё с мусором идёт, попробуй поставить ПИД в нули и посмотри.
В общем доволен…
Это главное!
Сергей (SergDoc), дай, пож, ссылку на исходники или сам файл с процедурой связи с Вийным Гуем и ссылку на Гуй, который с этим кодом работает. А то, что то код коммуникатора вия, взятый с оф.сайта вия, никак не хочет вязаться с гуем…
Исходники но там надо смотреть ТС сломал в последних работу с гуи
MultiWii_2_1 сам мультивий и гуи рабочее
Mw-WinGUI-2.1 гуи которым пользуюсь
вот сам serial.c
вот cli.c для настроек через терминал
У меня кстати серву не показывает…
У меня кстати серву не показывает…
Где не показывает? В гуе?
Гироскоп исправился? Прошивка для проверки нужна?
да в гуи - потом как нибудь поковыряю, гира да, больше не беспокоит 😃 всё остальное допилю по тихоньку, если на выходные погода не подгадит то выберусь на природу 😃 кстати я ещё не включал Head Free
p.s.
Что-то мне подсказывает что контроллеру на F407 и MPU6000 суждено быть 😃
Отчитаюсь: обновился до крайней паршивки ТС:
Из хорошего серва появилась в GUI, так же серву уже не так колбасит 😃 P=8 😃
нашёл причину дёргания - вибрации от самой сервы передаются по её же шнурку к контроллеру (я так предполагаю - экспериментировал) плюс лапы слишком пружинистые, надо попробовать шнурок у контроллера на карандаш намотать чтоб спираль получилась
Из плохого - погода портится, вечером ещё ничего но кругом куча детей, а когда их загоняют то и темно (не особо страшно - коптер как новогодняя ёлка светится) и ветер сильный - буду ждать погоды.
Надо дорисовать большую плату, чтоб до зимы изготовить, а там уже можно и программное прикручивать по тихоньку - чёт я планов настроил как Наполеон, надо ж ещё и с маленькой с микшером разобраться - хочу и сервы на подвес запустить, а то по ходу в трикоптере и при стандартном приёмнике они не работают…
“Выкинул” я серву 😦 зря потраченные деньги, хотя может при других обстоятельствах (другом поворотном механизме) она и будет работать, но прицепленная прямо на луч - никогда в жизни.
Причина:
Отклеил наклейки бумажные, а там движок за подлицо с корпусом, он мне и мосх сума и сводил.
Поставил старую серву - небо и земля, никаких дёрганий, подлетнул сегодня даже в сильный ветер (порывами) на прошлом контроллере я себе такого не позволял (ну очень сегодня хотелось) аппарат без повреждений вернулся домой, умудряется висеть даже в акро, ну только плывёт по ветру, в левеле пытается сопротивлятся ветру - красота.
С приёмником всёже есть проблемы, секунд 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 */
Всего ИМУ то
у меня и того меньше, правда там пока баро нет.