ATXmega128A3U + mpu6050 scalanie żyroskopu z akcelerometrem

[tomaszrugby]

Witam

Napisałem program pod mpu6050 (w bascom), surowe dane z akcelerometru , żyroskopu i termometru są przetworzone i wyglądają na poprawne . Dane z akcelerometru mają dopisany filtr by nie reagował na gwałtowne wstrząsy. Wszystko wyświetla dobrze , oś X jak i Yy akcelerometru i żyroskopu pokazują te same odchylenie w stopniach , oś Z żyroskopu też fajnie działa . Wszystko działa dobrze kiedy przechyla się kolejno osie ale kiedy zacznę w różnych kierunkach osiami w tym samym czasie to jednak żyroskop pokazuje już inne kąty niż na początku :/ . Można powiedzieć że wygląda to tak jak na tym filie , który znalazłem na YT https://www.youtube.com/watch?v=qmd6CVrlHOM . Na filmie widać że żyroskop "odpływa" dryf , akcelerometr wraca do swojej pozycji a scalone dane na ostatnim też wyglądają ok . Pytanie , jak napisać te scalanie danych że po mimo odpływu żyra efekt końcowy jest dobry tak jak na tym filmie .Jakiś wzór .

ps.

Miałem scalanie dopisane ale dane z żyroskopu wpływały na wynik końcowy , czyli po ruchach w tym samym czasie całość pokazywała błędne położenie , a na tym filmie mimo poprzestawianego żyroskopu wszystko wygląda ok .

$REGFILE = "XM128A3UDEF.DAT"
$CRYSTAL = 60000000 '60MHz
$HWSTACK = 200
$SWSTACK = 200
$FRAMESIZE = 200
$LIB "XMEGA.LIB"
CONFIG OSC = ENABLED , PLLOSC = ENABLED , PLLMUL =  30
CONFIG SYSCLOCK = PLL
ENABLE INTERRUPTS
CONFIG PRIORITY = STATIC , VECTOR = APPLICATION , LO = ENABLED
'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------         PIN i PORT          ----------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------
   CONFIG portc.0=OUTPUT
'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------         UART          ----------------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------

   CONFIG COM6 = 115200 , MODE = ASYNCHRONEOUS , PARITY = NONE , STOPBITS = 1 , DATABITS = 8
   OPEN "COM6:" FOR BINARY AS #1
   CONFIG SERIALIN5 = BUFFERED , SIZE = 30 , BYTEMATCH = 13
   ENABLE USARTe1_RXC, lO
   '-----------------------------
   DIM M AS STRING*20
   DIM RS232 AS WORD

'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------           TCC0 OVF           ---------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------
'60MHz/8/30820 = 0.004 Hz          GYRO_KAT_X=GYRO_INT_X / 250
'60MHz/64/9612 = 0.01 Hz           GYRO_KAT_X=GYRO_INT_X / 100
'60MHz/256/9612 = 0.04 Hz          GYRO_KAT_X=GYRO_INT_X / 25
'60MHz/256/12130 = 0.1 Hz          GYRO_KAT_X=GYRO_INT_X / 20

   CONFIG TCC0 = NORMAL , PRESCALE = 256              ' - 1 , 2, 4, 8, 64, 256, 1024
   TCC0_PER = 9612
   ON TCC0_OVF CZYTAJ_MPU6050
   Enable TCC0_OVF , LO
   STOP TCC0
'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------        EVENT_SYSTEM           --------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------

'   CONFIG EVENT_SYSTEM = DUMMY , MUX0 = TCC0_OVF , DIGFLT0 = 1

'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------   32bit   TIMER OVF           --------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------

'   CONFIG TCC0 = NORMAL,  PRESCALE = 1 , RESOLUTION = 16
'   TCC0_per =60000
'   ENABLE TCC0_OVF , LO
'   STOP TCC0
'   TCC0_CNT=0
'   CONFIG TCC1 = NORMAL , PRESCALE = E0 , EVENT_SOURCE = E0 , EVENT_ACTION = CAPTURE
'   TCC1_per =65535
'   ENABLE TCC1_OVF , LO
'   START TCC1
   '-----------------------------
'   DIM T32 AS DWORD AT &H20F0
'   DIM THH AS WORD AT T32 OVERLAY
'   DIM TLL AS WORD AT T32 +2 OVERLAY
'   TLL=TCC1_CNT
'   THH=TCC0_CNT
'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------              I2C             ---------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------

   CONFIG SCL = Porte.1
   CONFIG SDA = Porte.0
   CONFIG TWIe = 400000
   OPEN "TWIe" FOR BINARY AS #4
   I2CINIT #4
   '-----------------------------
   DIM TWI_START AS BYTE

'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------         LCD 240x320          ---------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------

'Designate the connection port of LCD. Cs1 = [CS] , Rst = [RESET] , A0 = [DC] , Si = [SDI] , Sclk = [SCK]
   CONFIG Vport0 = f 'wybierz sobie port
   CONFIG Graphlcd = Color , Cs1 = Port0.7 , Rst = Port0.6 , A0 = Port0.5 , Si = Port0.4 , Sclk = Port0.3
   CONST Portrait = 1
   CONST Rotate_180 = 0
   DIM Offset256 As Byte
   $LIB "glcd-ILI9341_2R8_240x320.lib"
   INITLCD
   CLS
   SETFONT Color8x8
   CONST Black = &B0000_0000
   CONST Red = &B1110_0000
   CONST Green = &B0001_1100
   CONST Blue = &B0000_0011
   CONST Magenta = &B1110_0011
   CONST Yellow = &B1111_1100
   CONST Cyan = &B0001_1111
   CONST White = &B1111_1111
   CONST Orange = &B1111_0100
   CONST Brightgreen = &B0011_1110
   CONST Brightblue = &B0001_1111
   CONST Darkred = &B1100_0000
   CONST Darkgreen = &B0001_0100
   CONST Darkblue = &B0000_0010
   OffSET256 = 1
   BOXFILL(0 , 0) -(64 , 240) , Black
   OFFSET256 = 0
   BOXFILL (0 , 0) -(255 , 240) , Black

'----------------------------------------------------------------------------------------------------------------------------
'-------------------------------------------------           ZMIENNE            ---------------------------------------------
'----------------------------------------------------------------------------------------------------------------------------
'
'ACC
'
   DIM ACCX(2) AS BYTE,ACCY(2) AS BYTE , ACCZ(2) AS BYTE
   DIM AKCCEL_X AS INTEGER , AKCCEL_Y AS INTEGER , AKCCEL_Z AS INTEGER
   DIM AKCCEL_SING_X AS SINGLE , AKCCEL_SING_Y , AKCCEL_SING_Z AS SINGLE
   DIM AKCC_KAT_X AS SINGLE , AKCC_KAT_Y AS SINGLE , AKCC_KAT_Z AS SINGLE
   DIM AKCC_KATX AS SINGLE , AKCC_KATY AS SINGLE , AKCC_KATZ AS SINGLE
'
'TEMP
'
   DIM TEMP(2) AS BYTE
   DIM TEMP_INT AS INTEGER
   DIM TEMPERATURA AS SINGLE
   DIM TEMP_INTC AS INTEGER
'GYRO
'
   DIM GYRX(2) AS BYTE , GYRY(2) AS BYTE , GYRZ(2) AS BYTE
   DIM GYRO_X AS INTEGER , GYRO_Y AS INTEGER , GYRO_Z AS INTEGER
   DIM GYRO_INT_X AS INTEGER , GYRO_INT_Y AS INTEGER, GYRO_INT_Z AS INTEGER
   DIM GYRO_KAT_X AS INTEGER , GYRO_KAT_Y AS INTEGER , GYRO_KAT_Z AS INTEGER
'  KALIBRACJA
'
   DIM KAL_X AS DWORD , KAL_Y AS DWORD , KAL_Z AS DWORD
   DIM PETLA AS BYTE ,AX AS WORD ,AY AS WORD , AZ AS WORD
'FILTR


   DIM XAn  AS SINGLE ,YAN AS SINGLE
   DIM XMn1  AS SINGLE ,YMN1 AS SINGLE
   DIM XMn  AS SINGLE , YMN AS SINGLE
   DIM XMN2 AS single , YMN2 AS single

'SCALANIE
'
   DIM XG AS SINGLE
   DIM XA AS SINGLE
   DIM X_WY AS SINGLE

   DIM YG AS SINGLE
   DIM YA AS SINGLE
   DIM Y_WY AS SINGLE

'INNE

   DIM GYRXX AS INTEGER , GYRYY AS INTEGER , GYRZZ AS INTEGER
   DIM LICZNIK AS WORD
   DIM XACC AS INTEGER , YACC AS INTEGER

'----------------------------------------------------------------------------------------------------------------------------

   GOSUB KONFIGURACJA_MPU6050
   GOSUB KALIBRACJA
   TCC0_CNT=0
   START TCC0
'----------------------------------------------------------------------------------------------------------------------------
'PITCH-Y / ROLL-X / YAW-Z / TILT-NACHYLENIE
Do

  INCR LICZNIK

  lcdat  0   , 64 , LICZNIK;"       " ,white ,black
  lcdat  20  , 64 , GYRO_KAT_X ;"       " ,white ,black
  lcdat  30  , 64 , GYRO_KAT_Y ;"       " ,white ,black
  lcdat  40  , 64 , GYRO_KAT_Z ;"       " ,white ,black
  lcdat  50  , 64 , XACC;"       " ,white ,black
  lcdat  60  , 64 , YACC;"       " ,white ,black
  lcdat  80  , 64 , TEMP_INTC;"       " ,white ,black
  lcdat  100  , 0 , "FILTR X ";XMN2;"       " ,white ,black
  lcdat  110  , 0 , "FILTR Y ";YMN2;"       " ,white ,black

  'lcdat  120  , 0 ,"X  ";GYRXX;"        ",white ,black
  'lcdat  130  , 0 ,"Y  ";GYRYY;"        ",white ,black
  'lcdat  140  , 0 ,"Z  ";GYRZZ;"        ",white ,black
  lcdat  200  , 0 ," X_WY "; X_WY;"        ",white ,black
  lcdat  210  , 0 ," Y_WY "; Y_WY;"        ",white ,black

  PRINT #1 , XACC;",";XMN2;",";YACC;",";YMN2;",";GYRO_KAT_Z

Loop
End
'----------------------------------------------------------------------------------------------------------------------------

CZYTAJ_MPU6050:

'-----------------------------
   TOGGLE PORTC.0
'-----------------------------
'   ODCZYT
'-----------------------------
   I2CSTART                   #4                                 'START ODCZYTU OD REJESTRU 59
   I2cwbyte &HD0            , #4
   I2CWBYTE 59              , #4
   I2CSTART                   #4
   I2CWBYTE &HD1            , #4

   I2CRBYTE ACCX(2) , Ack   , #4
   I2CRBYTE ACCX(1) , Ack   , #4
   I2CRBYTE ACCY(2) , Ack   , #4
   I2CRBYTE ACCY(1) , Ack   , #4
   I2CRBYTE ACCZ(2) , Ack   , #4
   I2CRBYTE ACCZ(1) , Ack   , #4

   I2CRBYTE TEMP(2) , Ack   , #4
   I2CRBYTE TEMP(1) , Ack   , #4

   I2CRBYTE GYRX(2) , Ack   , #4
   I2CRBYTE GYRX(1) , Ack   , #4
   I2CRBYTE GYRY(2) , Ack   , #4
   I2CRBYTE GYRY(1) , Ack   , #4
   I2CRBYTE GYRZ(2) , Ack   , #4
   I2CRBYTE GYRZ(1) , Nack  , #4
   I2CSTOP                     #4

'-----------------------------
'   AKCELEROMETR
'-----------------------------

   AKCCEL_X=makeint(ACCX(1),ACCX(2))
   AKCCEL_Y=makeint(ACCY(1),ACCY(2))
   AKCCEL_Z=makeint(ACCZ(1),ACCZ(2))

   AKCCEL_X=AKCCEL_X-500
   AKCCEL_Y=AKCCEL_Y+480
   AKCCEL_Z=AKCCEL_Z+1111

   AKCCEL_SING_X=AKCCEL_X/16384
   AKCCEL_SING_Y=AKCCEL_Y/16384
   AKCCEL_SING_Z=AKCCEL_Z/16384

   AKCC_KAT_X=atn2(AKCCEL_SING_Y,AKCCEL_SING_Z)
   AKCC_KAT_Y=atn2(AKCCEL_SING_X,AKCCEL_SING_Z)

   AKCC_KAT_X=AKCC_KAT_X * -1

   AKCC_KATX=RAD2DEG(AKCC_KAT_X)
   AKCC_KATY=RAD2DEG(AKCC_KAT_Y)

   XACC=AKCC_KATX
   YACC=AKCC_KATY

'  FILTR

   XAN=XACC
   XMN = 0.1 * XAN
   XAN = 1 - 0.1
   XMN1 = XMN1 * XAN
   XMN = XMN + XMN1
   XMN1 = XMN
   XMN2=XMN1

   YAN=YACC
   YMN = 0.1 * YAN
   YAN = 1 - 0.1
   YMN1 = YMN1 * YAN
   YMN = YMN + YMN1
   YMN1 = YMN
   YMN2=YMN1
'-----------------------------
'  TEMPERATURA
'-----------------------------

   TEMP_INT=MAKEINT(TEMP(1),TEMP(2))
   TEMPERATURA=TEMP_INT/340
   TEMPERATURA=TEMPERATURA+36.53
   TEMP_INTC=TEMPERATURA

'-----------------------------
'  ZYROSKOP
'-----------------------------

   GYRO_X=makeint(GYRX(1),GYRX(2))
   GYRO_Y=makeint(GYRY(1),GYRY(2))
   GYRO_Z=makeint(GYRZ(1),GYRZ(2))

   GYRXX=GYRO_X
   GYRYY=GYRO_Y
   GYRZZ=GYRO_Z

   GYRO_X=GYRO_X + 150
   GYRO_Y=GYRO_Y + 170
   GYRO_Z=GYRO_Z

   GYRO_X=GYRO_X / 131
   GYRO_Y=GYRO_Y / 131
   GYRO_Z=GYRO_Z / 131

   GYRO_INT_X=GYRO_INT_X - GYRO_X
   GYRO_INT_Y=GYRO_INT_Y - GYRO_Y
   GYRO_INT_Z=GYRO_INT_Z - GYRO_Z

   GYRO_KAT_X=GYRO_INT_X / 25
   GYRO_KAT_Y=GYRO_INT_Y / 25
   GYRO_KAT_Z=GYRO_INT_Z / 25

'-----------------------------
'  SCALANIE
'-----------------------------
   XG=GYRO_KAT_X*0.9
   XA=XMN2*0.1
   X_WY=XG+XMN2
   'X_WY=X_WY/2
'
   YG=GYRO_KAT_Y*0.9
   YA=YMN2*0.1
   Y_WY=YG+YMN2
   'Y_WY=Y_WY/2

RETURN

'----------------------------------------------------------------------------------------------------------------------------
   $INCLUDE "color8x8.font"
'----------------------------------------------------------------------------------------------------------------------------
SERIAL5CHARMATCH:

   PUSHALL
   INPUT #1, M NOECHO
   RS232=VAL(M)
   POPALL

RETURN
'----------------------------------------------------------------------------------------------------------------------------
KONFIGURACJA_MPU6050:

   '---     25   SAMPL    ---
   I2CSTART               #4                                     'REJESTR - 25 Sample Rate Divider
   I2CWBYTE &HD0        , #4
   I2CWBYTE 25          , #4
   I2CWBYTE &B00000000  , #4
   I2CSTOP                #4
   '---     26   DLPF     ---
   I2CSTART               #4                                      'REJESTR - 26 DLPF_CFG lowpass filter
   I2CWBYTE &HD0        , #4
   I2CWBYTE 26          , #4
   I2CWBYTE &B11010000  , #4
   I2CSTOP                #4
   '---     27   GYRO     ---
   I2CSTART               #4                                      'REJESTR - 27 Gyro Configuration
   I2CWBYTE &HD0        , #4
   I2CWBYTE 27          , #4
   I2CWBYTE &B00000000  , #4
   I2CSTOP                #4
   '---     28   ACC      ---
   I2CSTART               #4                                      'REJESTR - 28 ACC Configuration
   I2CWBYTE &HD0        , #4
   I2CWBYTE 28          , #4
   I2CWBYTE &B00000000  , #4
   I2CSTOP                #4
   '---     107   POWER   ---                                      'REJESTR - 107 Power Management
   I2CSTART               #4
   I2CWBYTE &HD0        , #4
   I2CWBYTE 107         , #4
   I2CWBYTE &B00000000  , #4
   I2CSTOP                #4

  lcdat   0  , 0 , "LICZNIK ",white ,black
  lcdat  20  , 0 , "GYRO-X ",white ,black
  lcdat  30  , 0 , "GYRO-Y ",white ,black
  lcdat  40  , 0 , "GYRO-Z ",white ,black
  lcdat  50  , 0 , "ACC-X  ",white ,black
  lcdat  60  , 0 , "ACC-Y  ",white ,black
  lcdat  80  , 0 , "TEMP   ",white ,black


RETURN
'----------------------------------------------------------------------------------------------------------------------------

KALIBRACJA:
'WAIT 1
'-----------------------------
'FOR PETLA=1 TO 10
   WAITMS 100
   I2CSTART                   #4                                 'START ODCZYTU OD REJESTRU 59
   I2cwbyte &HD0            , #4
   I2CWBYTE 59              , #4
   I2CSTART                   #4
   I2CWBYTE &HD1            , #4

   I2CRBYTE ACCX(2) , Ack   , #4
   I2CRBYTE ACCX(1) , Ack   , #4
   I2CRBYTE ACCY(2) , Ack   , #4
   I2CRBYTE ACCY(1) , Ack   , #4
   I2CRBYTE ACCZ(2) , Ack   , #4
   I2CRBYTE ACCZ(1) , NAck  , #4
   I2CSTOP


   AKCCEL_X=makeint(ACCX(1),ACCX(2))
   AKCCEL_Y=makeint(ACCY(1),ACCY(2))
   AKCCEL_Z=makeint(ACCZ(1),ACCZ(2))



   KAL_X=KAL_X+AKCCEL_X
   KAL_Y=KAL_Y+AKCCEL_Y
   KAL_Z=KAL_Z+AKCCEL_Z

'   NEXT

   AX=KAL_X
   AY=KAL_Y
   AZ=KAL_Z


RETURN

 

[tomaszrugby]

Noo udało mi się znaleźć wzór i zaimplementować - roll=[roll+omega_y*DT]*0.95 + roll_acc*0.05 i to samo dla pitch zamieniająć roll . Wszystko śmiga jak trzeba . Muszę jeszcze dodać kompas i scalić z YAW żyroskopu

[tomaszrugby]

Kupiłem moduł mpu9250 bo w jednej kostce będzie też magnetometr . Próbowałem dostać się do magnetometru ale bez powodzenia , zresztą wielu widze ma ten problem . Próbowałem na podstawie tego opisu z tej strony https://longnight975551865.wordpress.com/2018/02/11/how-to-read-data-from-mpu9250/ . Może ktoś bawił się mpu9250 i wie jakie rejestry skonfigurować by dostać się do kompasu .... ja już nie mam pomysłu a nie chcę używać mpu6050 + zewnętrzny kompas np; hmc5883 ....nie trzeba mi kodu a rejestry , kolejność i konfiguracja tych rejestrów

[Gość]

Właśnie zacząłem przygodę z MPU6050 ale ma przyjść MPU9250 i jestem zainteresowany pracą MPU9250.Dryf na żyroskopie w MPU6050 jest i to duży a to spowodowane jest błędami przesunięcia.Wygląda na to,że kalibracja jest najważniejsza.Na razie uczę się obsługiwać to urządzenie.Literatura w necie jest i jeśli mogę w czymś pomóc to przeczytaj następujące artykuły AN3916 ,AN3447 ,AN3916.Co prawda te artykuły nie odnoszą się bezpośrednio do MPU6050 i MPU9250 ale dają wiedzę jak postępować z akcelerometrem.Mój MPU6050 obsługiwany jest przez adruino nano a w nim jest program FILE: GY521_pitch_roll_yaw.ino

// AUTHOR: Rob Tillaar

// VERSION: 0.1.0

// PURPOSE: demo PRY
// DATE: 2020-08-06
Ściągniesz z Git Huba      https://github.com/RobTillaart/GY521 lub ze strony    https://platformio.org/lib/show/10801/GY521
W tym programie Gy521 należy zmienić w linii
GY521 sensor(0x69);
na
GY521 sensor(0x68);
Warto też zajrzeć na youtube na stronę       https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/
Mile widziane spostrzeżenia.

[Harnas]

26 minut temu, szymonw napisał:

Dryf na żyroskopie w MPU6050 jest i to duży

Nie wiem dokładnie do czego to potrzebujesz. Dobre 8 lat temu, popularne były sterowniki do dronów oparte na MultiWii, gdzie MPU6050 bez problemu dawał radę.

[ethanak]

1 godzinę temu, szymonw napisał:

Dryf na żyroskopie w MPU6050 jest i to duży

i w związku z tym

1 godzinę temu, szymonw napisał:

Wygląda na to,że kalibracja jest najważniejsza

Pytanie zasadnicze: korzystasz z DMP na pokładzie MPU6050 czy liczysz sobie stopnie i milisekundy?