Создание собственной системы стабилизации
а вот с аркосинусами и вектором беда - в какойто момент времени просто отказывается считать углы уже через альфа бета фильтр, и непойму почему, вектор отказывается считать хотя расчёты ускорений идут…
вертикаль
всмысле по курсу? пока никак нечем отследить кроме гироскопа - только если по угловой скорости пока как в КУКе, а так магнитометр нужен
а если хе крен и тангаж то atan2 возврашает значение угла между горизонтом и осью по которой считаеи X или Y
Не раньше было как: выставка горизонта на земле т.е. ноль(сейчас инициализация), потом от этого ноля и плясали. Вопрос, если случайным образом подбросить объект с гироскопами и акселерометрами (не делая перед этим инициализацию), мы будем знать где низ?
не так колибровка то зачем, я ж с ней столько времени угрохал на третьей странице как раз на земле нулевые показания от них и пляшу
А за горизонт гироскопы отвечают? То есть мы запоминаем их значение когда объект на земле выставлен в горизонт?
да
А можно качественно сравнить точность акселерометров и точность гироскопов, может вместо гироскопов использовать шесть акселерометров? Это позволит избавиться от дрейфа гироскопа, но будет накапливаться ошибка интегрирования.
интегрируем как раз угловую скорость от гир дабы получить угол, а с акселерометрами тоже не всё так просто, хотя есть проект квадрик с магнетометром трёхосевым и акселерометром
#include <Wire.h> //Library for I2C implementation using analog pins 4 (SDA) and 5
#include <MegaServo.h>
#define NBR_SERVOS 4 // the maximum number of servos (up to 12 on Arduino Diecimil
#define FIRST_SERVO_PIN 6 // define motor pins on the Arduino board
#define SECOND_SERVO_PIN 9
#define THIRD_SERVO_PIN 10
#define FOURTH_SERVO_PIN 11
MegaServo Servos[NBR_SERVOS] ; // max servos
//Accelerometer variables
float ax,ay,az; //Acceleration values in m/s^2 for all three axis
const float as=0.00488; //Arduino analog resolution = 5Volts/1024 (10bits)
const float zg=1.63; //Sensor zero g bias = Supply Voltage/2 =3.3/2 (V)
const float sR=0.3; //Sensor resolution (V/g)
//Compass variables
int compassAddress = 0x42 >*> 1; // compass I2C slave adress: 0x42
// because the wire.h library only uses the 7 bits most significant bits
// a shift necessary is to get the most significant bits.
int psi = 0; // Compass heading = yaw (direct readings from the compass in degrees)
float psirad; // Compass heading = yaw in radians
//Euler angles Roll and pitch
float phi; // (rad)
float theta; // (rad)
//Kalman filter data
const float A[6][6] = {
{1.0000,0.0000,0.0000,-0.5985,-0.0000,0.0000},
{0.0000,1.0000,-0.0000,-0.0000,-0.5985,0.0000},
{-0.0000,-0.0000,1.0000,-0.0000,-0.0000,-0.0479},
{0.0500,0.0000,0.0000,0.3384,-0.0000,-0.0000},
{0.0000,0.0500,-0.0000,-0.0000,0.3384,0.0000},
{-0.0000,0.0000,0.0500,0.0000,0.0000,0.9147}
};//(Kalman state-space matrix Ak)
const float B[6][10] = {
{-0.0000,-0.0188,-0.0000,0.0188,0.0000,0.0302,-0.0000,0.0031,-0.0000,-0.0000},
{0.0194,0.0006,-0.0194,-0.0006,-0.0302,-0.0000,-0.0000,-0.0000,0.0031,-0.0000},
{0.0009,-0.0009,0.0009,-0.0009,-0.0000,0.0000,0.0000,0.0000,0.0000,0.0479},
{-0.0000,0.0002,-0.0000,-0.0002,0.0000,0.0334,-0.0000,0.0034,-0.0000,0.0000},
{0.0005,0.0006,-0.0005,-0.0006,-0.0334,-0.0000,-0.0000,-0.0000,0.0034,-0.0000},
{0.0000,-0.0000,0.0000,-0.0000,0.0000,0.0000,0.0000,0.0000,-0.0000,0.0853}
}; //(Kalman state-space matrix Bk)
float x[6] = {0,0,0,0,0,0}; //state vector: Roll speed, Pitch Speed, Yaw speed, Roll angle, Pitch angle, Yaw angle
float u[10] = {0,0,0,0,0,0,0,0,0,0}; //input vector: w1 (motor 1 speed),w2,w3,w4,ax,ay,az,phi,theta,yaw
float At[6] = {0,0,0,0,0,0}; //temp variable
float Bt[6] = {0,0,0,-0.01,-0.04,0}; //temp variable
//LQR gain matrix
const float K[4][6] = {
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{-7.1030,0.0000,-20.4163,-18.2839,0.0000,-14.4659},
{0.0000,-7.1030,20.4163,0.0000,-18.2839,14.4659},
{7.1030,0.0000,-20.4163,18.2839,0.0000,-14.4659}
};
float w1,w2,w3,w4; //Motor inputs from LQR controller (rad.s^-1)
float w0; //Throttle to the motors (from joystick)
//Motor inputs for PWM signal
float wc1=0;
float wc2=0;
float wc3=0;
float wc4=0;
//PWM signals
int p1 = 200;
int p2 = 200;
int p3 = 200;
int p4 = 200;
//Reference
float yr[6]={0,0,0,0,0,0};
void setup()
{
Wire.begin();
Serial.begin(9600); // Initialize serial communications with setup
Servos[0].attach( FIRST_SERVO_PIN, 800, 2200); //Motor 1 - North - Clockwise rotation
Servos[1].attach( SECOND_SERVO_PIN, 800, 2200); //Motor 2 - East - Counterclockwise rotation
Servos[2].attach( THIRD_SERVO_PIN, 800, 2200); //Motor 3 - South - Clockwise rotation
Servos[3].attach( FOURTH_SERVO_PIN, 800, 2200); // Motor 4 - West - Counterclockwise rotation
//Put the motors in full stop
Servos[0].write(0);
Servos[1].write(0);
Servos[2].write(0);
Servos[3].write(0);
}
void loop()
{
// get the most recent acceleration values for all three axis
ax = ((analogRead(0)*as-zg)/sR)*9.81; //acceleration x-axis (m.s^-2)
ay = ((analogRead(1)*as-zg)/sR)*9.81; //acceleration y-axis (m.s^-2)
az = ((analogRead(2)*as-zg)/sR)*9.81; //acceleration z-axis (m.s^-2)
ax=-ax; //This correction allows the accelerometer to provide positive acceleration in the x-axis direction
// Compass task 1: connect to the HMC6352 sensor
Wire.beginTransmission(compassAddress);
Wire.send(’A’); // The ascii character "A" tells the compass sensor to send data
Wire.endTransmission();
// Compass task 2: wait for data processing
delay(50); // Compass datasheet says we need to wait at least 6000 microsegundos (0.006s) for data processing in the sensor
//We can also take this opotunity to implement the sampling time (20Hz)
// Compass task 3: Request heading
Wire.requestFrom(compassAddress, 2); // request 2 bytes of data
// Compass task 4: Get heading data
if(2 <= Wire.available()) // if 2 bytes are available
{
//16bit numbers can be broken in two 8 bit chunks. The first is called high byte and the second low byte
psi = Wire.receive(); // get high byte
psi = psi <*< 8; // shift high byte
psi += Wire.receive(); // get low byte
psi /= 10; // comment this line if you wish to get the heading in decidegrees instead of degrees
}
psirad = deg2rad((float)psi); // Map the compass reading from 0 to 359 degrees to -pi radians to pi radians
phi=atan(ay/az); //Calculate roll angle from the accelerations provided by the accelerometer (rad)
theta=-atan(ax/az); //Calculate pitch angle from the accelerations provided by the accelerometer (rad)
//Perform Kalman filtering
At[0]=A[0][0]*x[0]+A[0][1]*x[1]+A[0][2]*x[2]+A[0][3]*x[3]+A[0][4]*x[4]+A[0][5]*x[5];
At[1]=A[1][0]*x[0]+A[1][1]*x[1]+A[1][2]*x[2]+A[1][3]*x[3]+A[1][4]*x[4]+A[1][5]*x[5];
At[2]=A[2][0]*x[0]+A[2][1]*x[1]+A[2][2]*x[2]+A[2][3]*x[3]+A[2][4]*x[4]+A[2][5]*x[5];
At[3]=A[3][0]*x[0]+A[3][1]*x[1]+A[3][2]*x[2]+A[3][3]*x[3]+A[3][4]*x[4]+A[3][5]*x[5];
At[4]=A[4][0]*x[0]+A[4][1]*x[1]+A[4][2]*x[2]+A[4][3]*x[3]+A[4][4]*x[4]+A[4][5]*x[5];
At[5]=A[5][0]*x[0]+A[5][1]*x[1]+A[5][2]*x[2]+A[5][3]*x[3]+A[5][4]*x[4]+A[5][5]*x[5];
//Get throttle from the joystick
if (Serial.available() > 0)
{
// read the incoming byte:
w0 = map((float)Serial.read(), 0, 100, 0, 500);
w0=constrain(w0,0,500); //Limit maximum velocity to 600 rad/s
Serial.flush();
}
else
{
w0 = 0;
}
//place known inputs and measurements in the input vector u
u[0]=wc1-w0;
u[1]=wc2-w0;
u[2]=wc3-w0;
u[3]=wc4-w0;
u[4]=ax-yr[0];
u[5]=ay-yr[1];
u[6]=az-yr[2];
u[7]=phi-yr[3];
u[8]=theta-yr[4];
u[9]=psirad-yr[5];
Bt[0]=B[0][0]*u[0]+B[0][1]*u[1]+B[0][2]*u[2]+B[0][3]*u[3]+B[0][4]*u[4];
Bt[0]=Bt[0]+B[0][5]*u[5]+B[0][6]*u[6]+B[0][7]*u[7]+B[0][8]*u[8]+B[0][9]*u[9];
Bt[1]=B[1][0]*u[0]+B[1][1]*u[1]+B[1][2]*u[2]+B[1][3]*u[3]+B[1][4]*u[4]
Bt[1]=Bt[1]+B[1][5]*u[5]+B[1][6]*u[6]+B[1][7]*u[7]+B[1][8]*u[8]+B[1][9]*u[9];
Bt[2]=B[2][0]*u[0]+B[2][1]*u[1]+B[2][2]*u[2]+B[2][3]*u[3]+B[2][4]*u[4]
Bt[2]=Bt[2]+B[2][5]*u[5]+B[2][6]*u[6]+B[2][7]*u[7]+B[2][8]*u[8]+B[2][9]*u[9];
Bt[3]=B[3][0]*u[0]+B[3][1]*u[1]+B[3][2]*u[2]+B[3][3]*u[3]+B[3][4]*u[4]
Bt[3]=Bt[3]+B[3][5]*u[5]+B[3][6]*u[6]+B[3][7]*u[7]+B[3][8]*u[8]+B[3][9]*u[9];
Bt[4]=B[4][0]*u[0]+B[4][1]*u[1]+B[4][2]*u[2]+B[4][3]*u[3]+B[4][4]*u[4]
Bt[4]=Bt[4]+B[4][5]*u[5]+B[4][6]*u[6]+B[4][7]*u[7]+B[4][8]*u[8]+B[4][9]*u[9];
Bt[5]=B[5][0]*u[0]+B[5][1]*u[1]+B[5][2]*u[2]+B[5][3]*u[3]+B[5][4]*u[4]
Bt[5]=Bt[5]+B[5][5]*u[5]+B[5][6]*u[6]+B[5][7]*u[7]+B[5][8]*u[8]+B[5][9]*u[9];
//Get estimated states x(k+1)=A.x(k)+B.u(k)
x[0]=At[0]+Bt[0];
x[1]=At[1]+Bt[1];
x[2]=At[2]+Bt[2];
x[3]=At[3]+Bt[3];
x[4]=At[4]+Bt[4];
x[5]=At[5]+Bt[5];
//Controlo LQR
w1=K[0][0]*x[0]+K[0][1]*x[1]+K[0][2]*x[2]+K[0][3]*x[3]+K[0][4]*x[4]+K[0][5]*x[5];
w2=K[1][0]*x[0]+K[1][1]*x[1]+K[1][2]*x[2]+K[1][3]*x[3]+K[1][4]*x[4]+K[1][5]*x[5];
w3=K[2][0]*x[0]+K[2][1]*x[1]+K[2][2]*x[2]+K[2][3]*x[3]+K[2][4]*x[4]+K[2][5]*x[5];
w4=K[3][0]*x[0]+K[3][1]*x[1]+K[3][2]*x[2]+K[3][3]*x[3]+K[3][4]*x[4]+K[3][5]*x[5];
//Adjust speeds with the throttle
wc1=w0-w1;
wc2=w0-w2;
wc3=w0-w3;
wc4=w0-w4;
//Calculate and send pulse width to the motors
p1=(int)((wc1/2.0276)+1252); // (speed of motor 1 (rad/s) / transfer function gain) + Dead-zone pulse width
p2=(int)((wc2/1.8693)+1262);
p3=(int)((wc3/2.0018)+1255);
p4=(int)((wc4/1.9986)+1255);
if(w0==0) //Forçar paragem
{
p1=400;
p2=400;
p3=400;
p4=400;
}
Servos[0].write(p1);
Servos[1].write(p2);
Servos[2].write(p3);
Servos[3].write(p4);
// output states
Serial.print(x[0]);
Serial.print(" ");
Serial.print(x[1]);
Serial.print(" ");
Serial.print(x[2]);
Serial.print(" ");
Serial.print(x[3]);
Serial.print(" ");
Serial.print(x[4]);
Serial.print(" ");
Serial.print(x[5]);
Serial.println("");
}
float deg2rad (float x) //function that receives an angle between 0 and 359 degrees and resturns the same angle between -pi
and pi radians
{
if(x>=0 && x<=180)
{
x=-x*3.14/180;
}
else
{
x = (360-x)*3.14/180;
}
return x;
}
непомню уже где нашол…
compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle)); здесь хочу какимто образом сделать коэффициент(бета) изменяющийся в зависимости от показаний акселерометра, а то при уменьшении угловой скорости угол тоже уменьшается, где-то до 70 гр нормально а если платку поставить вертикально тоеть 90 гр угол покажет где-то 60гр…
Ну вот, добрался всётаки до камеры, фотика нет так что качество не ахти
Подцепил гиры в мультивийную прошивку
Вот что вышло
Это нормально в режиме покоя? -2 +2 если да то тогда нифига непонимаю почему в моей прошивке большой разброс параметров, обижусь сам на себя и уйду под Wii, если разберусь как мой аксель к нему подрубить:)
я уже и гироскопы с акселем разделил по питанию, одна фиг:)
поганял я виййную прошивку, больше промучался с портами, ониже у меня не совпадают с вием, если кому интересно входы RX в трёх местах перепрограммировать надо, сначала задефайнить сами порты, потом этимже портам разрешить прерывания, и на последок указать на какие прерывания реагировать (по тылу или по фронту) и опять же на каких портах, вобщем пришол к выводу что управление лучше взять из КУКа, а вот IMU пока ещё не решил. Кто на пальцах объяснит как в Вие читаются акселерометры, а то и датащеты перевернул на них, ничё не могу понять, шесть показаний а откуда???
Ну вот, добрался всётаки до камеры, фотика нет так что качество не ахти
а где купить murata enc-03?
У корейца были (не реклама)
но они меньше размерами
У корейца были (не реклама)
но они меньше размерами
10 баксов за штуку…
Где-то видел у японцев 75 центов за штуку но партия от 1000 штук, дешевле китайский кук купить
Собираю инфу хочу поставить все датчики аналоговые, народ спорит, что лучше на аналоговых нет задержек при чтении, ненадо программировать, да и разрешение сигнала выше 10-бит а не 8 по i2c присматриваюсь также к Atmega 1280, по крайней мере сейчас могу себе её позволить, ARM становится несбыточной мечтой пока…
Вот попросил привезти мне STM32F103VET6 онже SAM3S Cortex™ M3, если конечно привезут попробую эту штучку…
Кстати атмегу 168 доканал не мычит ни телется только греется, ха-ха уложил неубиваемый контроллер😁
Примерный список:
Контроллер - STM32F103VET6
Гироскопы - ISZ500 + 2 IDG500 либо оставлю murata enc-03
Акселерометр - ADXL335 либо LIS352AX это уж чё достану
Магнитометр - НМС2003 аналоговый трёхосевой
Барометр - BMP085 ну без i2c никак
GPS - EM406 наверно…
На счёт программного, поддержка: Arducopter , Ardupilot , Multiwii , Aeroquad,
естественно не моё изобретение, но попробовать стоит😁
Украдено тут: code.google.com/p/multipilot32/
Я вот думаю что нибудь собрать из вот этой платки c AllInOne датчиками, единственно, не совсем догоняю, обязательно ли покупать программатор JTAG или там уже есть бутлоадер и можно обойтись USB… (Как у Arduino)
собрать из вот этой платки
Игрался я именно с такой платкой - довольно занятная вещица!
По умолчанию, умеет через ком-порт прошиваться (бутлоадер прошит в чип на заводе, затереть его нельзя), после доработок прошивки - и по юсб через dfuse умеет
10 баксов за штуку…
здесь 5 eur. где то видел даже за 3 USD, но немогу найти линк.
хммм… отладка на стмке 30 баксов + плата датчиков 100 баксов. или FY20 купить за 100. но в ней нет барометра и магнитного компаса. и вроде акселерометр там двух осевой.