Создание собственной системы стабилизации
при приеме данных сонара кто-нибудь учитывает крен/тангаж?
при приеме данных сонара кто-нибудь учитывает крен/тангаж?
А смысл? Сонар не имеет четкой направленности в пределах рабочего сектора. Если только сфокусировать его…
и что он покажет при наклоне аппарата больше “рабочего сектора”? дунул ветер при посадке (или висении), аппарат наклонился для компенсации, высота резко “увеличилась”, софт обязан сбросить газ.
в симуляторе (эмуляция сонара с 10° сектором и идеально ровной земли) это приводит к такой колбасне, что после 2-3 раза обязательно последует падение.
кто-нибудь учитывает крен/тангаж?
Что-то, в свое время на мегапирате пробовали
и что он покажет при наклоне аппарата больше “рабочего сектора”?
Реально поможет только повесить сонар на подвес, смотрящий всегда вниз. При большем угле чем 10-15гр - показания будут равны нулю.
В Ардукоптере (и в мегапирате) есть такая коррекция, но отключена всегда:
#if SONAR_TILT_CORRECTION == 1
// correct alt for angle of the sonar
float temp = cos_pitch_x * cos_roll_x;
temp = max(temp, 0.707);
sonar_alt = (float)sonar_alt * temp;
#endif
Я использую MB1000 у него хорошая площадь пятна наклоны до 15 градусов нормально отрабатываются при половине дальности сонара, больше наклоны не проверял, думаю больше и не нужно. Над ковром работает отлично, над травой фигово.
Ну вот, что-то похожее на иму 😃
#include "board.h"
#include "mw.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
float accLPFVel[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int16_t sonarAlt;//to think about the unit
int32_t EstSAlt;
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
float accVelScale; //Dobavil
// **************
// 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;
accVelScale = 9.80665f / acc_1G / 10000.0f; //Dobabil
#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;
t_fp_vector EstG;
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
// 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 float accLPFVel[3]; // Dobavil
//static t_fp_vector EstG; // Ubral
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];
}
accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f / cfg.acc_lpf_for_velocity)) + accADC[axis] * (1.0f / cfg.acc_lpf_for_velocity); // Dobavil
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 //*BARO+Sonar*//
// #define NEW_ACCZ_HOLD // Dobavil ne aktiviroval да потому что Альт Нолд только Mahovik, а остальное пристрелил :)
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
//#define BARO_TAB_SIZE 40
//#define BARO_TAB_SIZE_MAX 48
//#define ACC_Z_DEADBAND (acc_1G / 50)
/////////////////////////////////////////////////////////////////
//#ifdef NEW_ACCZ_HOLD
int16_t applyDeadband16(int16_t value, int16_t deadband)
{
if (abs(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;
} else if (value < 0) {
value += deadband;
}
return value;
}
float applyDeadbandFloat(float value, int16_t deadband)
{
if (abs(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;
} else if (value < 0) {
value += deadband;
}
return value;
}
float InvSqrt(float x)
{
union {
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x)
{
return x * x;
}
/////////////////////////////////////////////////////////////////////////////////
void getEstimatedAltitude(void)
{
//uint8_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE_MAX];
static int8_t baroHistIdx;
static int32_t baroHigh;
uint32_t dTime;
int16_t error;
float invG;
int16_t accZ;
static float vel = 0.0f;
static int32_t lastBaroAlt;
float baroVel;
static int16_t SonarHistTab[SONAR_TAB_SIZE_MAX];
static int8_t SonarHistIdx;
static int32_t SonarHigh;
static int32_t lastSonarAlt;
float SonarVel;
uint8_t limit = 0.0f;
static uint8_t sonarerrors = 0;
if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL)
return;
dTime = currentTime - deadLine;
deadLine = currentTime;
//**** Alt. Set Point stabilization PID ****
baroHistTab[baroHistIdx] = BaroAlt / 10;
baroHigh += baroHistTab[baroHistIdx];
baroHigh -= baroHistTab[(baroHistIdx + 1) % cfg.baro_tab_size];
baroHistIdx++;
if (baroHistIdx == cfg.baro_tab_size)
baroHistIdx = 0;
// EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
EstAlt = EstAlt * cfg.baro_noise_lpf + (baroHigh * 10.0f / (cfg.baro_tab_size - 1)) * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// P
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband16(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 100), -150, +150);
// I
errorAltitudeI += error * cfg.I8[PIDALT] / 50;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += (errorAltitudeI / 500); // I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
// int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
// int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
accZ = applyDeadband16(accZ, acc_1G / cfg.accz_deadband);
debug[0] = accZ;
// Integrator - velocity, cm/sec
vel += accZ * accVelScale * dTime;
//lastBaroAlt = EstSAlt;
baroVel = (EstAlt - lastBaroAlt) / (dTime / 1000000.0f);
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero
lastBaroAlt = EstAlt;
debug[1] = baroVel;
//#ifdef SONAR
if ((sonarAlt < SONAR_MAX_DISTANCE)&(sonarAlt > SONAR_MIN_DISTANCE)) {
if(sonarerrors > limit) sonarerrors--;
else sonarerrors = limit;
}
else {
if(sonarerrors < SONAR_ERROR_MAX)
sonarerrors++;;
}
SonarHistTab[SonarHistIdx] = sonarAlt;// / 10;
SonarHigh += SonarHistTab[SonarHistIdx];
SonarHigh -= SonarHistTab[(SonarHistIdx + 1) % cfg.sonar_tab_size];
SonarHistIdx++;
if (SonarHistIdx == cfg.sonar_tab_size){
SonarHistIdx = 0;
}
// EstAlt
EstSAlt = SonarHigh; //EstSAlt * cfg.sonar_noise_lpf + (SonarHigh * 10.0f / (cfg.sonar_tab_size - 1)) * (1.0f - cfg.sonar_noise_lpf); // additional LPF
error = constrain(AltHold - EstSAlt, -300, 300);
SonarVel = (EstSAlt - lastSonarAlt) / (dTime / 1000000.0f);
SonarVel = constrain(SonarVel, -300, 300); // constrain baro velocity +/- 300cm/s
SonarVel = applyDeadbandFloat(SonarVel, 2); // to reduce noise near zero
lastSonarAlt = EstSAlt;
debug[3] = SonarVel;
if(sonarerrors == 0) {
vel = vel * cfg.sonar_cf + SonarVel * (1.0f - cfg.sonar_cf);
}
else if(sonarerrors> 0 & sonarerrors < SONAR_ERROR_MAX) {
vel = (vel * (SONAR_ERROR_MAX - sonarerrors) + SonarVel * sonarerrors)/SONAR_ERROR_MAX;
}
else if (sonarerrors >=SONAR_ERROR_MAX) {
// Sonar is crasy,
vel = vel * cfg.baro_cf + baroVel * (1.0f - cfg.baro_cf);
}
//#endif
// apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity
// vel = vel * cfg.baro_cf + baroVel * (1.0f - cfg.baro_cf);//vel = vel * 0.985f + baroVel * 0.015f;
// vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
// debug[3] = applyDeadbandFloat(vel, 5);
// D
BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
//debug[3] = BaroPID;
}
#endif /* BARO+Sonar */
сильно не пинайте…
Рано выложил - нашел ещё косяков, исправил, теперь по моему будет без разницы - включен сонар, не включен, окончательную версию выложу после испытаний…
Барометр в Россию уехал 😦
Ну вот добился чего-то похожего на правду, сонар висит на 2-м(триггер) и 6-м(эхо) моторах имеется ввиду NAZE32, в моей платке расположение моторов другое, код портирован из мультивия www.multiwii.com/forum/viewtopic.php?f=7&t=1033&st…, естественно с AltHold от Mahowik.
Паршивка подходит к NAZE32 без MPU3050 можно использовать только на квадрике без подвеса и трикоптере, ну Y4 должен работать, для подключения квадрика пропускать второй мотор, трикоптер без изменений.
через Cli включить сонар…
Парочка функций для реализации задержки и отсчета времени в микросекундах. Может пригодится.
Паршивка подходит к NAZE32 без MPU3050
Перед прошивкой ОБЯЗАТЕЛЬНО произвести полное стирание!!!
Видео полетов где можно посмотреть?
Надеюсь в воскресенье камера придёт 😃
Эх, мне бы хоть на десять минут спортзал, стоит коптер пылится, ни камеры ни погоды блин 😦 на лестничной площадке боюсь выше экрана подымать…
Ну что, отрицательный результат - тоже результат, не учёл что коптер стал тяжелее, и точка висения сместилась, и получилась бяка при работе с сонаром коптер сбрасывает обороты 😦 на предыдущую точку висения - пока дёрался с высотой и выключением Альтхолда вписался в забор - минус проп и надо проверить батарейку - вылезла из своей “кожуры” - поправлю и опять на испытания, видео к сожалению отменяется 😦, пойдём мы наверно фотик купим…
Починился, поднастроился, подлетнул, правда уже в ветер, картина такая: при переходе с сонара, на баро+сонар происходит прыжок сантиметров 20-30 вверх - потом меленно обратно, выше (с баро+сонар на баро) ещё не пробовал, что делать?
Разозлился и купил фотик, теперь осталось жену научит видима снимать, сыну пока не доверяю…
при переходе с сонара, на баро+сонар происходит прыжок сантиметров 20-30 вверх
Думаю, это нормально, датчики зиначально дают разные показания, при переходе их значения усредняются ступенькой. Потом баро потихоньку подтягивается к сонару и всё… При отключении датчика ступеньки не должно быть.
Немного с бубном потанцевал ещё, буду проверять…
А есть какое-нибудь логирование? надо логи глянуть какие там данные дает сонар и какие баро и сразу все станет ясно