Akcelerometr i Żyroskop + Filtr

Tu możesz pisać o swoich problemach z pisaniem programów w języku C dla AVR.
ps19
Newb
Newb
Posty: 56
Rejestracja: poniedziałek 05 paź 2015, 22:27
Lokalizacja: Opole
Kontaktowanie:

Akcelerometr i Żyroskop + Filtr

Postautor: ps19 » sobota 02 sty 2016, 13:12

Witajcie

Mam problem z działanie filtru Madgwicka - korzystałem z kodu z tej strony http://www.x-io.co.uk/open-source-imu-a ... lgorithms/ i przeliczenia wartości wychodzących z filtra z http://www.x-io.co.uk/quaternions/ .Moduł akcelerometru i żyroskopu to MPU6050 GY521. Procesor to Atmega644PA (8 Mhz wewnętrzny kwarc) (W przyszłości jakiś STM z obsługą liczb zmiennoprzecinkowych).

Problem polega na tym, że o ile dane z żyroskopu i akcelerometru odczytuję i zmieniają się wraz z obrotami, to wartość z filtra wyświetla się poprawna tylko jak obracam/ruszam modułem, a jak moduł leży bez ruchu to wartość jak jest ujemna to rośnie jak dodatnia to maleje.

Kod: Zaznacz cały

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>
#include <math.h> // Math library required for `sqrt'
#include <stdio.h>

#include "I2C/i2c_lib.h"
#include "MKUART/mkuart.h"
#include "TERMINAL_VT100/term_vt100.h"

#include "mpu6050.h"
#include "MadgwickAHRS/MadgwickAHRS.h"

/******************************UWAGA !!!***********************************
*
* Jeżeli występuje błąd collect2.exe: error: ld returned 1 exit status
* Project-->Properties-->C/C++ Build-->Settings-->AVR C Linker -->Libraries
* w Libraries (-l) wpisac litere "m" (bez "")
*
***************************************************************************/

inline void acc_read_values(void);
inline void gyro_read_values(void);
inline void temperature_read(void);

inline void rs232_send_values(void);

volatile int temp;
volatile int8_t temp_l, temp_h;

char str1 [15];
char str2 [15];
char str3 [15];

typedef struct {
   volatile float raw_x_axis, raw_y_axis, raw_z_axis;
   volatile int16_t x_axis_l, x_axis_h, y_axis_l, y_axis_h, z_axis_l, z_axis_h;
} AXES;

AXES gyro;
AXES acc;

int main(void)
{
   USART_Init(__UBRR);

   sei();

   i2cSetBitrate(100); //Bitrate 400 kHz

   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_2, MPU6050_WAKE_FREQ_5); //Sets clock source to gyro reference w/ PLL
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, MPU6050_CLOCK_DIV_500); //Sets sample rate to 1000/1+9 = 100Hz
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, MPU6050_DLPF_BW_5); //Disable FSync, 5Hz DLPF
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0x18); //Disable gyro self tests, scale of 2000 degrees/s
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x18); //Disable accel self tests, scale of +-16g, no DHPF
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FF_THR, 0x00); //Freefall threshold of <|0mg|
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FF_DUR, 0x00); //Freefall duration limit of 0
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_MOT_THR, 0x00); //Motion threshold of >0mg
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_MOT_DUR, 0x00); //Motion duration of >0s
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00); //Zero motion threshold
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00); //Zero motion duration threshold
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); //Disable sensor output to FIFO buffer
   //
   ////AUX I2C setup
   ////Sets AUX I2C to single master control, plus other config
   
   //Setup AUX I2C slaves
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);

   //Setup INT pin and AUX I2C pass through
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
   //Enable data ready interrupt
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);

   //More slave config
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);

   //Reset sensor signal paths
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);

   //Motion detection control
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);

   //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);

   //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);

   //Data transfer to and from the FIFO buffer
   i2c_write_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);

   term_cls(1);
   _delay_ms(1000);

   while(1)
   {
      acc_read_values();
      gyro_read_values();
      temperature_read();
      MadgwickAHRSupdateIMU(gyro.raw_x_axis, gyro.raw_y_axis, gyro.raw_z_axis, acc.raw_x_axis,acc.raw_y_axis,acc.raw_z_axis);
      rs232_send_values();
      _delay_ms(20);
      term_cls(1);
      //skorzystać z poziomnica w Pobrane !!!
   }
}

inline void acc_read_values(void)
{
   acc.x_axis_h = i2c_read_byte(MPU6050_RA_ACCEL_XOUT_H,MPU6050_DEFAULT_ADDRESS);
   acc.x_axis_l = i2c_read_byte(MPU6050_RA_ACCEL_XOUT_L,MPU6050_DEFAULT_ADDRESS);

   acc.y_axis_h = i2c_read_byte(MPU6050_RA_ACCEL_YOUT_H,MPU6050_DEFAULT_ADDRESS);
   acc.y_axis_l = i2c_read_byte(MPU6050_RA_ACCEL_YOUT_L,MPU6050_DEFAULT_ADDRESS);

   acc.z_axis_h = i2c_read_byte(MPU6050_RA_ACCEL_ZOUT_H,MPU6050_DEFAULT_ADDRESS);
   acc.z_axis_l = i2c_read_byte(MPU6050_RA_ACCEL_ZOUT_L,MPU6050_DEFAULT_ADDRESS);

   acc.raw_x_axis = ((acc.x_axis_h<<8) | acc.x_axis_l);
   acc.raw_y_axis = ((acc.y_axis_h<<8) | acc.y_axis_l);
   acc.raw_z_axis = ((acc.z_axis_h<<8) | acc.z_axis_l);
}

inline void gyro_read_values(void)
{
   gyro.x_axis_h = i2c_read_byte(MPU6050_RA_GYRO_XOUT_H,MPU6050_DEFAULT_ADDRESS);
   gyro.x_axis_l = i2c_read_byte(MPU6050_RA_GYRO_XOUT_L,MPU6050_DEFAULT_ADDRESS);
   gyro.y_axis_h = i2c_read_byte(MPU6050_RA_GYRO_YOUT_H,MPU6050_DEFAULT_ADDRESS);
   gyro.y_axis_l = i2c_read_byte(MPU6050_RA_GYRO_YOUT_L,MPU6050_DEFAULT_ADDRESS);
   gyro.z_axis_h = i2c_read_byte(MPU6050_RA_GYRO_ZOUT_H,MPU6050_DEFAULT_ADDRESS);
   gyro.z_axis_l = i2c_read_byte(MPU6050_RA_GYRO_ZOUT_L,MPU6050_DEFAULT_ADDRESS);

   gyro.raw_y_axis = ((gyro.y_axis_h<<8) | gyro.y_axis_l);
   gyro.raw_x_axis = ((gyro.x_axis_h<<8) | gyro.x_axis_l);
   gyro.raw_z_axis = ((gyro.z_axis_h<<8) | gyro.z_axis_l);
}

inline void temperature_read(void)
{
   temp_h = i2c_read_byte(MPU6050_RA_TEMP_OUT_H,MPU6050_DEFAULT_ADDRESS);
   temp_l = i2c_read_byte(MPU6050_RA_TEMP_OUT_L,MPU6050_DEFAULT_ADDRESS);
   temp = ( ((temp_h<<8) + temp_l)/340 )+36.56;
}

inline void rs232_send_values(void)
{
   term_locate(2,6);
   uart_puts("X:");
   term_locate(6,6);
   uart_putint32(angle_x,10);

   term_locate(22,6);
   uart_puts("Y:");
   term_locate(27,6);
   uart_putint32(angle_y,10);

   term_locate(42,6);
   uart_puts("Z:");
   term_locate(47,6);
   uart_putint32(angle_z,10);
   
   term_locate(10,3);
   uart_puts("Akcelerometr: ");

   term_locate(2,5);
   uart_puts("Raw X: ");
   term_locate(9,5);
   uart_putint32(acc.raw_x_axis,10);

   term_locate(15,5);
   uart_puts("Raw Y: ");
   term_locate(22,5);
   uart_putint32(acc.raw_y_axis,10);

   term_locate(28,5);
   uart_puts("Raw Z: ");
   term_locate(35,5);
   uart_putint32(acc.raw_z_axis,10);

   term_locate(13,8);
   uart_puts("Zyroskop: ");

   term_locate(2,10);
   uart_puts("Raw X: ");
   term_locate(9,10);
   uart_putint32(gyro.raw_x_axis,10);

   term_locate(15,10);
   uart_puts("Raw Y: ");
   term_locate(22,10);
   uart_putint32(gyro.raw_y_axis,10);

   term_locate(28,10);
   uart_puts("Raw Z: ");
   term_locate(35,10);
   uart_putint32(gyro.raw_z_axis,10);

   term_locate(8,12);
   uart_puts("Temperatura: ");
   uart_putint32(temp,10);
}


MadgwickAHRS.c (przeliczenie wartości z macierzy na kąt, funkcje dopisane prze zemnie, reszta nie zmieniona )

Kod: Zaznacz cały


volatile float angle_z, angle_y, angle_x;

void ComputeEulerAngles(float qa0,float qa1,float qa2,float qa3)
{
   //Computes XYZ Euler angles
   
   angle_z = atan2( 2.0f*(qa2*q3 - qa0*qa1), 2.0f*pow(qa0,2) - 1 + 2.0f*pow(qa3,2) ); //angle around Z axis
   angle_z = angle_z*180/M_PI; //rad to deg
   
   angle_y = (-1)*atan( ( 2.0f*(qa1*qa3 + qa0*qa2) )/sqrt(1.0f-pow(2.0f*q1*q3 + 2.0f*qa0*qa2, 2)) ); //angle around Y axis
   angle_y = angle_y*180/M_PI; //rad to deg
   
   angle_x = atan2(2.0f*(qa1*qa2 - qa0*qa3), 2.0f*pow(qa0,2) - 1 + 2.0f*pow(qa1,2) ); //angle around X axis
   angle_x = angle_x*180/M_PI; //rad to deg
}

float invSqrt(float x) {
   float y = 1.0f/sqrt(x);
   return y;
}

Awatar użytkownika
Antystatyczny
Geek
Geek
Posty: 1123
Rejestracja: czwartek 03 wrz 2015, 22:02

Re: Akcelerometr i Żyroskop + Filtr

Postautor: Antystatyczny » sobota 02 sty 2016, 13:18

Hmm, a masz dołączoną bibliotekę math do projektu? Z tego co mi wiadomo bez niej obliczenia na zmiennoprzecinkowych nie sa takie, jakie być powinny. Rozumiem, że z wyjścia filtra Kalmana wartości samoistnie narastają/opadają mimo braku ruchu, tak?
"The true sign of intelligence is not knowledge but imagination" Albert Einstein.

ps19
Newb
Newb
Posty: 56
Rejestracja: poniedziałek 05 paź 2015, 22:27
Lokalizacja: Opole
Kontaktowanie:

Re: Akcelerometr i Żyroskop + Filtr

Postautor: ps19 » sobota 02 sty 2016, 13:22

1.Tak, math.h jest dołączona.
2.Tak, ale tylko w bezruchu. Jak ruszę modułem to wartość wyświetla się poprawna ale potem zaczyna opadać albo narastać w zależności od "biegunowości zmiennej"
3. To nie filtr kalmana tylko Madgwicka.

Awatar użytkownika
Antystatyczny
Geek
Geek
Posty: 1123
Rejestracja: czwartek 03 wrz 2015, 22:02

Re: Akcelerometr i Żyroskop + Filtr

Postautor: Antystatyczny » sobota 02 sty 2016, 13:25

Zasadniczo dane z żyroskopu są tymi, które powodują zmiany, jak to nazywam, długofalowe. Zaś dane z akcelerometru traktowane są jako chwilowe. Moduł MPU jeszcze nie lezy u mnie na biurku, wiec z własnego doświadczenia niewiele Ci powiem. Mogę jednak polecić rozmowę z Acidem, on maglował stosunkowo niedawno MPU6050 i sprawiał wrażenie ogarniającego ten temat :)

PS. Faktycznie, przeoczyłem, że to nie filtr Kalmana.
"The true sign of intelligence is not knowledge but imagination" Albert Einstein.

Awatar użytkownika
mokrowski
User
User
Posty: 189
Rejestracja: czwartek 08 paź 2015, 20:50
Lokalizacja: Tam gdzie Centymetro

Re: Akcelerometr i Żyroskop + Filtr

Postautor: mokrowski » sobota 02 sty 2016, 16:09

Niewiele mogę pomóc, ale może zasugeruję. Niestety nie mam tego modułu na biurku. Któryś z członów przekształceń "buduje Ci błąd". Jeśli masz możliwość, wyrzuć np. na RS232 wyniki pośrednie przekształceń (podziel je na kilka), a znajdziesz winowajcę. Wtedy najprawdopodobniej znajdziesz błąd. Na mój "nos" to będzie coś z zaokrąglaniem floata. Ale spekuluję ;-).

W jednym zdaniu, od siebie, pytam bo nie wiem. Zastosowałeś Madgwick'a bo?

Trochę tracisz pamięci i mocy obliczeniowej na przesyłaniu danych przez kopię (np .ComputeEulerAngles(...)). No ale nie tego dotyczyło pytanie :-)
,,Myślenie nie jest łatwe, ale można się do niego przyzwyczaić" - Alan Alexander Milne: Kubuś Puchatek

ps19
Newb
Newb
Posty: 56
Rejestracja: poniedziałek 05 paź 2015, 22:27
Lokalizacja: Opole
Kontaktowanie:

Re: Akcelerometr i Żyroskop + Filtr

Postautor: ps19 » sobota 02 sty 2016, 22:05

1. Zauważyłem coś ciekawego - jak za komentuję odczytywanie wartości z żyroskopu to z wartość wychodząca z filtra jest równa zero albo podskakuj co najwyżej do 0,01 i zmienna nie zmienia sama wartości. Także chyba mam coś z inicjalizacją żyroskopu/akcelerometru.
2. Na razie tylko testuję.

ps19
Newb
Newb
Posty: 56
Rejestracja: poniedziałek 05 paź 2015, 22:27
Lokalizacja: Opole
Kontaktowanie:

Re: Akcelerometr i Żyroskop + Filtr

Postautor: ps19 » niedziela 03 sty 2016, 21:14

Więc tak uprościłem całość do maksimum i wrzuciłem filtr komplementarny i o ile zmienię float/double na int`a to wynik wychodzi - niedokładny ale jest. Jak tylko zmienię int na float/double to wartości znowu zaczynają samoczynną inkrementacje/dekrementację.

Kod: Zaznacz cały

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>
#include <math.h> // Math library required for `sqrt'
#include <stdio.h>

#include "I2C/I2cbase.h"
#include "MKUART/mkuart.h"
#include "TERMINAL_VT100/term_vt100.h"

#include "mpu6050.h"

/******************************UWAGA !!!***********************************
*
* Jeżeli występuje błąd collect2.exe: error: ld returned 1 exit status
* Project-->Properties-->C/C++ Build-->Settings-->AVR C Linker -->Libraries
* w Libraries (-l) wpisac litere "m" (bez "")
*
***************************************************************************/

inline void acc_read_values(void);
inline void gyro_read_values(void);
inline void temperature_read(void);
void ComplementaryFilter(uint16_t gx, uint16_t gy, uint16_t gz, uint16_t ax, uint16_t ay, uint16_t az);
inline void rs232_send_values(void);

volatile int temp, flaga;
volatile int8_t temp_l, temp_h;

volatile float roll, pitch;

char str1 [15];
char str2 [15];
char str3 [15];

typedef struct {
   volatile uint16_t raw_x_axis, raw_y_axis, raw_z_axis;
   volatile uint16_t x_axis_l, x_axis_h, y_axis_l, y_axis_h, z_axis_l, z_axis_h;
} AXES;

AXES gyro;
AXES acc;

ISR(TIMER1_COMPA_vect, ISR_NOBLOCK)
{
   flaga = 1;
}

int main(void)
{
   USART_Init(__UBRR);
   
   TCCR1A = _BV(WGM12);
   TCCR1B = _BV(CS10) | _BV(CS11); //preskler 1024
   OCR1A = 1249; //100 Hz
   TIMSK1 = _BV(OCIE1A);

   sei();

   I2C_Init();

   I2C_SendByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, MPU6050_DLPF_BW_5); //DLPF 5 Hz
   I2C_SendByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000); //Gyroscope Range
   I2C_SendByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_16); //Accelerometer Range
   I2C_SendByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_MST_CTRL, MPU6050_CLOCK_DIV_400); //I2C Master Clock Speed
   I2C_SendByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT); // cycle between sleep mode and waking up
   I2C_SendByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_2, MPU6050_WAKE_FREQ_10); //Wake up frequency 40 Hz

   term_cls(1);
   _delay_ms(1000);
   
   while(1)
   {
      if(flaga == 1)
      {         
         ComplementaryFilter(gyro.raw_x_axis,gyro.raw_y_axis,gyro.raw_z_axis,acc.raw_x_axis,acc.raw_y_axis,acc.raw_z_axis);
         flaga = 0;
      }
      else
      {
         acc_read_values();
         gyro_read_values();
         rs232_send_values();
      }
   }
}

void ComplementaryFilter(uint16_t gx, uint16_t gy, uint16_t gz, uint16_t ax, uint16_t ay, uint16_t az)
{
   //tau = (alpha*dt)/(1.0f-alpha);
   float const dt = 0.01; //10 ms
   float const alpha = 0.98f;
   
   pitch += gx*dt;
   roll -= gy*dt;
   
   volatile float acc_pitch = atan2(ax,sqrt( (ay*ay)+(az*az) ))*180.0f/M_PI;
   pitch = (alpha*pitch) + ((1.0f-alpha)*acc_pitch); //pitch
   
   volatile float acc_roll = atan2(ay, sqrt( (ax*ax)+(az*az) ))*180.0f/M_PI;
   roll = (alpha*roll) + ((1.0f-alpha)*acc_roll); //roll
}

inline void acc_read_values(void)
{
   acc.x_axis_h = I2C_ReadByte(MPU6050_RA_ACCEL_XOUT_H,MPU6050_DEFAULT_ADDRESS);
   acc.x_axis_l = I2C_ReadByte(MPU6050_RA_ACCEL_XOUT_L,MPU6050_DEFAULT_ADDRESS);

   acc.y_axis_h = I2C_ReadByte(MPU6050_RA_ACCEL_YOUT_H,MPU6050_DEFAULT_ADDRESS);
   acc.y_axis_l = I2C_ReadByte(MPU6050_RA_ACCEL_YOUT_L,MPU6050_DEFAULT_ADDRESS);

   acc.z_axis_h = I2C_ReadByte(MPU6050_RA_ACCEL_ZOUT_H,MPU6050_DEFAULT_ADDRESS);
   acc.z_axis_l = I2C_ReadByte(MPU6050_RA_ACCEL_ZOUT_L,MPU6050_DEFAULT_ADDRESS);

   acc.raw_x_axis = ((acc.x_axis_h<<8) | acc.x_axis_l);
   acc.raw_y_axis = ((acc.y_axis_h<<8) | acc.y_axis_l);
   acc.raw_z_axis = ((acc.z_axis_h<<8) | acc.z_axis_l);
}

inline void gyro_read_values(void)
{
   gyro.x_axis_h = I2C_ReadByte(MPU6050_RA_GYRO_XOUT_H,MPU6050_DEFAULT_ADDRESS);
   gyro.x_axis_l = I2C_ReadByte(MPU6050_RA_GYRO_XOUT_L,MPU6050_DEFAULT_ADDRESS);
   gyro.y_axis_h = I2C_ReadByte(MPU6050_RA_GYRO_YOUT_H,MPU6050_DEFAULT_ADDRESS);
   gyro.y_axis_l = I2C_ReadByte(MPU6050_RA_GYRO_YOUT_L,MPU6050_DEFAULT_ADDRESS);
   gyro.z_axis_h = I2C_ReadByte(MPU6050_RA_GYRO_ZOUT_H,MPU6050_DEFAULT_ADDRESS);
   gyro.z_axis_l = I2C_ReadByte(MPU6050_RA_GYRO_ZOUT_L,MPU6050_DEFAULT_ADDRESS);

   gyro.raw_y_axis = ((gyro.y_axis_h<<8) | gyro.y_axis_l);
   gyro.raw_x_axis = ((gyro.x_axis_h<<8) | gyro.x_axis_l);
   gyro.raw_z_axis = ((gyro.z_axis_h<<8) | gyro.z_axis_l);
}

inline void temperature_read(void)
{
   temp_h = I2C_ReadByte(MPU6050_RA_TEMP_OUT_H,MPU6050_DEFAULT_ADDRESS);
   temp_l = I2C_ReadByte(MPU6050_RA_TEMP_OUT_L,MPU6050_DEFAULT_ADDRESS);
   temp = ( ((temp_h<<8) + temp_l)/340 )+36.56;
}

inline void rs232_send_values(void)
{
   term_locate(2,6);
   uart_puts("X:");
   term_locate(6,6);
   sprintf(str1,"%1.3f", pitch);
   uart_puts(str1);

   term_locate(22,6);
   uart_puts("Y:");
   term_locate(27,6);
   sprintf(str2,"%1.3f", roll);
   uart_puts(str2);

   //term_locate(42,6);
   //uart_puts("Z:");
   //term_locate(47,6);
   //sprintf(str3,"%1.3f", angle_z);
   //uart_puts(str3);
   
   term_locate(10,3);
   uart_puts("Akcelerometr: ");

   term_locate(2,5);
   uart_puts("Raw X: ");
   term_locate(9,5);
   uart_putint32(acc.raw_x_axis,10);

   term_locate(15,5);
   uart_puts("Raw Y: ");
   term_locate(22,5);
   uart_putint32(acc.raw_y_axis,10);

   term_locate(28,5);
   uart_puts("Raw Z: ");
   term_locate(35,5);
   uart_putint32(acc.raw_z_axis,10);

   term_locate(13,8);
   uart_puts("Zyroskop: ");

   term_locate(2,10);
   uart_puts("Raw X: ");
   term_locate(9,10);
   uart_putint32(gyro.raw_x_axis,10);

   term_locate(15,10);
   uart_puts("Raw Y: ");
   term_locate(22,10);
   uart_putint32(gyro.raw_y_axis,10);

   term_locate(28,10);
   uart_puts("Raw Z: ");
   term_locate(35,10);
   uart_putint32(gyro.raw_z_axis,10);

   term_locate(8,12);
   uart_puts("Temperatura: ");
   uart_putint32(temp,10);
}

Drauggul
Posty: 1
Rejestracja: piątek 23 gru 2016, 00:16

Re: Akcelerometr i Żyroskop + Filtr

Postautor: Drauggul » piątek 23 gru 2016, 00:27

Witam wszystkich serdecznie!

Ja ostatnio mialem ciekawe przygody z biblioteką math.h

operacje typu: sqrt(3)

wynik: 3

Zupelnie inna "nowa" matematyka.

Problem polegal na tym, że procesor nie obslugiwal sprzetowo liczb zmiennoprzecinkowych, a kompilator mial flagę do obslugi sprzętowej.
Zmiana na soft pomogla.

Warto sprawdzić parametry kompilacji ;)


Wróć do „Programowanie AVR w C”

Kto jest online

Użytkownicy przeglądający to forum: Obecnie na forum nie ma żadnego zarejestrowanego użytkownika i 1 gość