int readNumSamplesFifo(void){
unsigned char fifo_num_l = ReadAcc(XL362_FIFO_ENTRIES_L);
unsigned char fifo_num_h = ReadAcc(XL362_FIFO_ENTRIES_H);
fifo_num_h &= 0x03; /*only the last 2 bits of the upper register matter*/
int fifo_num_samples = (int)(((int)fifo_num_h<<8) | fifo_num_l);
return fifo_num_samples;
}
//will only set low register values high; may need to add functionality later to set values to 0
unsigned char ConfigureAcc(unsigned char reg, unsigned char value) {
unsigned char current_reg_val = ReadAcc(reg);
unsigned char new_val = current_reg_val |= value;
WriteAcc(reg, new_val);
rprintf("AccConfig: ");
rprintf("Reg %x, Val: %x\n\r", reg, ReadAcc(reg));
return new_val;
}
void Hardware_::GetAccelerometer(signed char* xyz)
{
SPI_Enable();
SPCR = 0x50;
AACCS_SS_LOW();
for (u8 i = 0; i < 3; i++)
xyz[i] = ReadAcc(i+6);
AACCS_SS_HIGH();
SPI_Disable();
}
/**
* @brief Thread that read the accelerometer
* @param argument:
* @retval None
*/
void Thread_Acc (void const *argument) {
while(1) {
//Wait for the interrupt to issue a flag before reading the value
//0 for the timeout value so that it never starts before the flag
osSignalWait(ACC_INT_FLAG, osWaitForever);
ReadAcc();
//printf("%f\n",accValue[0]);
osSignalClear(tid_Thread_Acc,ACC_INT_FLAG);
}
}
void Display_sensors(void)
{
while(BUTTON1 != 0)
{
if (BUTTON4 == 0)
{
_delay_ms(500);
CalibrateAcc();
CalibrateGyros();
}
if (BUTTON3 == 0)
{
_delay_ms(500);
CalibrateInvAcc();
}
ReadGyros();
ReadAcc();
LCD_Display_Text(26,(prog_uchar*)Verdana8,0,0); // Gyro
LCD_Display_Text(30,(prog_uchar*)Verdana8,70,0); // Acc
LCD_Display_Text(27,(prog_uchar*)Verdana8,10,15); // X
LCD_Display_Text(28,(prog_uchar*)Verdana8,10,25); // Y
LCD_Display_Text(29,(prog_uchar*)Verdana8,10,35); // Z
mugui_lcd_puts(itoa(gyroADC[PITCH],pBuffer,10),(prog_uchar*)Verdana8,30,15);
mugui_lcd_puts(itoa(gyroADC[ROLL],pBuffer,10),(prog_uchar*)Verdana8,30,25);
mugui_lcd_puts(itoa(gyroADC[YAW],pBuffer,10),(prog_uchar*)Verdana8,30,35);
mugui_lcd_puts(itoa(accADC[PITCH],pBuffer,10),(prog_uchar*)Verdana8,80,15);
mugui_lcd_puts(itoa(accADC[ROLL],pBuffer,10),(prog_uchar*)Verdana8,80,25);
mugui_lcd_puts(itoa(accADC[YAW],pBuffer,10),(prog_uchar*)Verdana8,80,35);
// Print bottom markers
LCD_Display_Text(12, (prog_uchar*)Wingdings, 0, 57); // Left
LCD_Display_Text(157, (prog_uchar*)Verdana8, 75, 55); // Inverted Calibrate
LCD_Display_Text(60, (prog_uchar*)Verdana8, 108, 55); // Calibrate
// Update buffer
write_buffer(buffer,1);
clear_buffer(buffer);
_delay_ms(100);
}
}
void Display_balance(void)
{
int16_t x_pos, y_pos;
int8_t roll_axis, pitch_axis;
while(BUTTON1 != 0)
{
ReadAcc();
// Refresh accSmooth values
// Note that because it takes 4.096ms to refresh the whole GLCD this loop cannot run
// faster than 244Hz, but that's close enough to the actual loop time so that the
// actual Acc LPF effect is closely mirrored on the balance meter.
getEstimatedAttitude();
// HORIZONTAL: Pitch = X, Roll = Y
// UPSIDEDOWN: Pitch = X, Roll = Y
// AFT: Pitch = X, Roll = Y
// VERTICAL: Pitch = Y, Roll = X
// SIDEWAYS: Pitch = Y, Roll = X
if ((Config.Orientation == VERTICAL) || (Config.Orientation == SIDEWAYS))
{
roll_axis = PITCH;
pitch_axis = ROLL;
}
else
{
roll_axis = ROLL;
pitch_axis = PITCH;
}
// We need to reverse the polarity reversal so that the meter is once again
// related to the KK2.0, not the model.
// For some reason, pitch has to be reversed on he KK2.1
#ifdef KK21
x_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][pitch_axis]) * -accSmooth[pitch_axis]) + 32;
#else
x_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][pitch_axis]) * accSmooth[pitch_axis]) + 32;
#endif
y_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][roll_axis]) * accSmooth[roll_axis]) + 64;
if (x_pos < 0) x_pos = 0;
if (x_pos > 64) x_pos = 64;
if (y_pos < 0) y_pos = 0;
if (y_pos > 128) y_pos = 128;
// Print bottom markers
LCD_Display_Text(12, (prog_uchar*)Wingdings, 2, 55); // Left
// Draw balance meter
drawrect(buffer, 0, 0, 128, 64, 1); // Border
drawrect(buffer, 54, 22, 21, 21, 1); // Target
drawline(buffer, 64, 8, 64, 56, 1); // Crosshairs
drawline(buffer, 32, 32, 96, 32, 1);
fillcircle(buffer, y_pos, x_pos, 8, 1); // Bubble
// Refresh GLCD
write_buffer(buffer,1);
clear_buffer(buffer);
}
}
unsigned int ADXLDeviceIDCheck(void) {
unsigned char id = ReadAcc(XL362_DEVID_AD);
return (id == 0xAD);
}
void Display_sensors(void)
{
bool first_time = true;
clear_buffer(buffer);
// While BACK not pressed
while(BUTTON1 != 0)
{
ReadGyros();
ReadAcc();
LCD_Display_Text(26,(const unsigned char*)Verdana8,37,0); // Gyro
LCD_Display_Text(30,(const unsigned char*)Verdana8,77,0); // Acc
//
LCD_Display_Text(27,(const unsigned char*)Verdana8,5,13); // Roll
LCD_Display_Text(28,(const unsigned char*)Verdana8,5,23); // Pitch
LCD_Display_Text(29,(const unsigned char*)Verdana8,5,33); // Yaw/Z
//
mugui_lcd_puts(itoa(gyroADC[ROLL],pBuffer,10),(const unsigned char*)Verdana8,40,13);
mugui_lcd_puts(itoa(gyroADC[PITCH],pBuffer,10),(const unsigned char*)Verdana8,40,23);
mugui_lcd_puts(itoa(gyroADC[YAW],pBuffer,10),(const unsigned char*)Verdana8,40,33);
mugui_lcd_puts(itoa(accADC[ROLL],pBuffer,10),(const unsigned char*)Verdana8,80,13);
mugui_lcd_puts(itoa(accADC[PITCH],pBuffer,10),(const unsigned char*)Verdana8,80,23);
mugui_lcd_puts(itoa(accADC[YAW],pBuffer,10),(const unsigned char*)Verdana8,80,33);
// Print bottom markers
LCD_Display_Text(12, (const unsigned char*)Wingdings, 0, 57); // Left
LCD_Display_Text(37, (const unsigned char*)Verdana8, 75, 55); // Inverted Calibrate
LCD_Display_Text(60, (const unsigned char*)Verdana8, 108, 55); // Calibrate
// Update buffer
write_buffer(buffer);
clear_buffer(buffer);
if (first_time)
{
// Wait until finger off button
Wait_BUTTON4();
first_time = false;
}
// Normal calibrate button pressed
if (BUTTON4 == 0)
{
// Wait until finger off button
Wait_BUTTON4();
// Pause until steady
_delay_ms(250);
// Calibrate sensors
CalibrateGyrosFast();
CalibrateAcc(NORMAL);
}
// Inverted calibrate button pressed
if (BUTTON3 == 0)
{
// Wait until button snap dissipated
_delay_ms(250);
CalibrateAcc(REVERSED);
}
}
}