void LLQuaternion::quantize8(F32 lower, F32 upper)
{
mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper);
mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, upper);
mQ[VZ] = U8_to_F32(F32_to_U8_ROUND(mQ[VZ], lower, upper), lower, upper);
mQ[VS] = U8_to_F32(F32_to_U8_ROUND(mQ[VS], lower, upper), lower, upper);
normQuat();
}
// Quatizations
void LLQuaternion::quantize16(F32 lower, F32 upper)
{
F32 x = mQ[VX];
F32 y = mQ[VY];
F32 z = mQ[VZ];
F32 s = mQ[VS];
x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper);
y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper);
z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper);
s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper);
mQ[VX] = x;
mQ[VY] = y;
mQ[VZ] = z;
mQ[VS] = s;
normQuat();
}
int main(void)
{
//-------------------------USART INTERRUPT REGISTRATION.------------//
// Set Clock: Oscillator needs to initialized once: First
pcl_switch_to_osc(PCL_OSC0, FOSC0, OSC0_STARTUP);
// -------------- USART INIT -----------------------------------------------
static const gpio_map_t USART_GPIO_MAP =
{
{AVR32_USART0_RXD_0_0_PIN, AVR32_USART0_RXD_0_0_FUNCTION},
{AVR32_USART0_TXD_0_0_PIN, AVR32_USART0_TXD_0_0_FUNCTION}
};
// USART options.
static const usart_options_t USART_OPTIONS =
{
.baudrate = USART_BAUDRATE,
.charlength = 8,
.paritytype = USART_NO_PARITY,
.stopbits = USART_1_STOPBIT,
.channelmode = USART_NORMAL_CHMODE
};
// Assign GPIO to USART
gpio_enable_module(USART_GPIO_MAP, sizeof(USART_GPIO_MAP) / sizeof(USART_GPIO_MAP[0]));
// Init USART
usart_init_rs232(USART_0, &USART_OPTIONS, FOSC0);
Disable_global_interrupt();
INTC_init_interrupts(); // Init Interrupt Table: Once at first
// Register USART Interrupt (hinzufügen)
INTC_register_interrupt(&usart_int_handler, AVR32_USART0_IRQ, AVR32_INTC_INT0);
USART_0->ier = AVR32_USART_IER_RXRDY_MASK; // Activate ISR on RX Line
Enable_global_interrupt();
// -----------------------------------------------------------------------------------
// -------------------------- Display INIT ----------------------------------
// Map SPI Pins
static const gpio_map_t DIP204_SPI_GPIO_MAP =
{
{DIP204_SPI_SCK_PIN, DIP204_SPI_SCK_FUNCTION }, // SPI Clock.
{DIP204_SPI_MISO_PIN, DIP204_SPI_MISO_FUNCTION}, // MISO.
{DIP204_SPI_MOSI_PIN, DIP204_SPI_MOSI_FUNCTION}, // MOSI.
{DIP204_SPI_NPCS_PIN, DIP204_SPI_NPCS_FUNCTION} // Chip Select NPCS.
};
// add the spi options driver structure for the LCD DIP204
spi_options_t spiOptions =
{
.reg = DIP204_SPI_NPCS,
.baudrate = 1000000,
.bits = 8,
.spck_delay = 0,
.trans_delay = 0,
.stay_act = 1,
.spi_mode = 0,
.modfdis = 1
};
// SPI Inits: Assign I/Os to SPI
gpio_enable_module(DIP204_SPI_GPIO_MAP,
sizeof(DIP204_SPI_GPIO_MAP) / sizeof(DIP204_SPI_GPIO_MAP[0]));
// Initialize as master
spi_initMaster(DIP204_SPI, &spiOptions);
// Set selection mode: variable_ps, pcs_decode, delay
spi_selectionMode(DIP204_SPI, 0, 0, 0);
// Enable SPI
spi_enable(DIP204_SPI);
// setup chip registers
spi_setupChipReg(DIP204_SPI, &spiOptions, FOSC0);
// initialize delay driver: Muss vor dip204_init() ausgeführt werden
delay_init( FOSC0 );
// initialize LCD
dip204_init(backlight_PWM, TRUE);
// ---------------------------------------------------------------------------------------
// ----------------- Timer Counter Init ---------------------------------
// Timer Configs: Options for waveform generation.
static const tc_waveform_opt_t WAVEFORM_OPT =
{
.channel = TC_CHANNEL, // Channel selection.
.bswtrg = TC_EVT_EFFECT_NOOP, // Software trigger effect on TIOB.
.beevt = TC_EVT_EFFECT_NOOP, // External event effect on TIOB.
.bcpc = TC_EVT_EFFECT_NOOP, // RC compare effect on TIOB.
.bcpb = TC_EVT_EFFECT_NOOP, // RB compare effect on TIOB.
.aswtrg = TC_EVT_EFFECT_NOOP, // Software trigger effect on TIOA.
.aeevt = TC_EVT_EFFECT_NOOP, // External event effect on TIOA.
.acpc = TC_EVT_EFFECT_NOOP, // RC compare effect on TIOA: toggle.
.acpa = TC_EVT_EFFECT_NOOP, // RA compare effect on TIOA: toggle
.wavsel = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER,// Count till RC and reset (S. 649): Waveform selection
.enetrg = FALSE, // External event trigger enable.
.eevt = 0, // External event selection.
.eevtedg = TC_SEL_NO_EDGE, // External event edge selection.
.cpcdis = FALSE, // Counter disable when RC compare.
.cpcstop = FALSE, // Counter clock stopped with RC compare.
.burst = FALSE, // Burst signal selection.
.clki = FALSE, // Clock inversion.
.tcclks = TC_CLOCK_SOURCE_TC3 // Internal source clock 3, connected to fPBA / 8.
};
// TC Interrupt Enable Register
static const tc_interrupt_t TC_INTERRUPT =
{ .etrgs = 0, .ldrbs = 0, .ldras = 0, .cpcs = 1, .cpbs = 0, .cpas = 0, .lovrs = 0, .covfs = 0
};
// 0 = No Effect | 1 = Enable ( CPCS = 1 enables the RC Compare Interrupt )
// ***************** Timer Setup ***********************************************
// Initialize the timer/counter.
tc_init_waveform(tc, &WAVEFORM_OPT); // Initialize the timer/counter waveform.
// Set the compare triggers.
tc_write_rc(tc, TC_CHANNEL, RC); // Set RC value.
tc_configure_interrupts(tc, TC_CHANNEL, &TC_INTERRUPT);
// Start the timer/counter.
tc_start(tc, TC_CHANNEL); // And start the timer/counter.
// *******************************************************************************
Disable_global_interrupt();
// Register TC Interrupt
INTC_register_interrupt(&tc_irq, AVR32_TC_IRQ0, AVR32_INTC_INT3);
Enable_global_interrupt();
// ---------------------------------------------------------------------------------------
imu_init();
//-------------------------------TWI R/W ---------------------------------------------------
sensorDaten imu_data = {0};
char disp1[30], disp2[30], disp3[30], disp4[30];
short GX,GY,GZ, AX, AY, AZ; //shifted comlete Data
RPY currMoveRPY;
Quaternion currQuat;
currQuat.q0 = 1.0;
currQuat.q1 = 0;
currQuat.q2 = 0;
currQuat.q3 = 0;
Quaternion deltaQuat;
RotMatrix rot = {0};
RPY reconverted;
calibrate_all(&imu_data);
while(1){
if(exe){
exe = false;
read_sensor(&imu_data);
AX = imu_data.acc_x + imu_data.acc_x_bias;
AY = imu_data.acc_y + imu_data.acc_y_bias;
AZ = imu_data.acc_z + imu_data.acc_z_bias;
GX = imu_data.gyro_x + imu_data.gyro_x_bias;
GY = imu_data.gyro_y + imu_data.gyro_y_bias;
GZ = imu_data.gyro_z + imu_data.gyro_z_bias;
//convert to 1G
float ax = (float)AX * (-4.0);
float ay = (float)AY * (-4.0); //wegen 2^11= 2048, /2 = 1024 entspricht 4G -> 1G = (1024/4)
float az = (float)AZ * (-4.0);
//convert to 1°/s
gx = ((float)GX/ 14.375); // in °/s
gy = ((float)GY/ 14.375);
gz = ((float)GZ/ 14.375);
//Integration over time
dGx = (gx*0.03);
dGy = (gy*0.03);
dGz = (gz*0.03);
currMoveRPY.pitch = -dGx;
currMoveRPY.roll = dGy;
currMoveRPY.yaw = dGz;
//aufaddieren auf den aktuellen Winkel IN GRAD
gxDeg += dGx;
gyDeg += dGy;
gzDeg += dGz;
//RPY in Quaternion umwandeln
RPYtoQuat(&deltaQuat, &currMoveRPY);
//normieren
normQuat(&deltaQuat);
//aufmultiplizeiren
quatMultiplication(&deltaQuat, &currQuat, &currQuat);
//nochmal normieren
normQuat(&currQuat);
//rücktransformation nicht nötig!!
char send[80];
sprintf(send,"$,%f,%f,%f,%f,#", currQuat.q0, currQuat.q1, currQuat.q2, currQuat.q3);
usart_write_line(USART_0,send);
sprintf(disp1,"q0:%.3f, GX:%3.0f",currQuat.q0,gxDeg);
sprintf(disp2,"q1:%.3f, GY:%3.0f",currQuat.q1, gyDeg);
sprintf(disp3,"q2:%.3f, GZ:%3.0f",currQuat.q2, gzDeg);
sprintf(disp4,"q3:%.3f",currQuat.q3);
dip204_clear_display();
dip204_set_cursor_position(1,1);
dip204_write_string(disp1);
dip204_set_cursor_position(1,2);
dip204_write_string(disp2);
dip204_set_cursor_position(1,3);
dip204_write_string(disp3);
dip204_set_cursor_position(1,4);
dip204_write_string(disp4);
//sprintf(data,"TEST:%s",high);
//print_dbg(data);
}
}
}
