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;
}