#define F_CPU 32000000UL
#include <inttypes.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>
#include <stdint.h>
#define TwiStart TWIC_MASTER_ADDR
#define TwiStop TWIC_MASTER_CTRLC = TWI_MASTER_CMD_STOP_gc
#define TwiData TWIC_MASTER_DATA
#define TwiStatusW while(!(TWIC.MASTER.STATUS & TWI_MASTER_WIF_bm))
#define TwiStatusR while(!(TWIC.MASTER.STATUS & TWI_MASTER_RIF_bm))
void TWI_MasterInit()
{
TWIC.CTRL = 0x00; //Normal TWI mode
TWIC.MASTER.BAUD = 75;//155--> 100 KHz at 32MHz clock; 75--> 200 KHz at 32MHz clock
TWIC.MASTER.CTRLA = TWI_MASTER_ENABLE_bm;
TWI_MASTER_INTLVL_HI_gc;
TWI_MASTER_RIEN_bm;
TWI_MASTER_WIEN_bm;
TWIC.MASTER.CTRLB = TWI_MASTER_TIMEOUT_DISABLED_gc; //Timeout disabled
TWIC.MASTER.STATUS = TWI_MASTER_BUSSTATE_IDLE_gc; //Set bus state idle
}
void WMP_INIT()
{
TwiStart = 0xA6;
TwiStatusW;
TwiData = 0xFE;
TwiStatusW;
TwiData = 0x04;
TwiStatusW;
TwiStop;
}
void WMP_LESEN() //aktuelle Messwerte auslesen
{
uint8_t wmpbyte[6];
TwiStart = 0xA4;
TwiStatusW;
TwiData = 0x00;
TwiStatusW;
TwiStop;
TwiStart = 0xA5;
TwiStatusR;
// Read data + send ACK
for(uint8_t i = 0; i < 6 ; i++)
{
while (!(TWIC_MASTER_STATUS & TWI_MASTER_RIF_bm));
wmpbyte[i] = TwiData;
TWIC_MASTER_CTRLC = TWI_MASTER_CMD_RECVTRANS_gc;
}
TWIC_MASTER_CTRLC = 0x06;//NACK, indicating that we are done reading
YawSpeed = wmpbyte[3] << 6; YawSpeed = YawSpeed >> 7; //Slow/Fast Bit
PitchSpeed = wmpbyte[3] << 7; PitchSpeed = PitchSpeed >> 7; //Slow/Fast Bit
RollSpeed = wmpbyte[4] << 6; RollSpeed = RollSpeed >> 7; //Slow/Fast Bit
for (uint8_t i = 3; i <= 5; i++) //Byte 3-5 die Bits 0 und 1 entfernen
{
wmpbyte[i] = wmpbyte[i] >> 2;
}
Yaw = ((wmpbyte[3] << 8 | wmpbyte[0]); //Zusammenfügen --> 14Bit
Roll = ((wmpbyte[4] << 8 | wmpbyte[1]); //Zusammenfügen --> 14Bit
Pitch = ((wmpbyte[5] << 8 | wmpbyte[2]); //Zusammenfügen --> 14Bit
}
void WMP_Kalibrierung() //60 Messungen durchführen
{
for (uint8_t i = 0; i < 10; i++) //10 Messungen durchführen da die ersten Fehlerhaft sind
{
WMP_LESEN();
}
for (uint8_t i = 1; i <= 50; i++) //Kalibrierungsmessungen
{
WMP_LESEN();
YawOffset = YawOffset + Yaw;
RollOffset = RollOffset + Roll;
PitchOffset = PitchOffset + Pitch;
}
YawOffset = YawOffset / 50;
RollOffset = RollOffset / 50;
PitchOffset = PitchOffset / 50;
}
Zitieren.
.
) und acc_winkel ist der Winkel berechnet aus den Daten des ACCs. Bis ca. 30° kannst du aber auch eine Kleinwinkelnäherung verwenden. dt ist die Zeit, die seit der letzten Berechnung vergangen ist, also 1 / f, wobei f die Frequenz deines Programms ist.
Lesezeichen