int main()
{
size_t nb_block = 1000;
static const size_t BlockSize = 6;
size_t rows = nb_block*BlockSize, cols = nb_block*BlockSize;
Dense dense(rows,cols,BlockSize);
Sparse sparse(dense);
Blocks<BlockSize> blocks(dense);
utils::Tic<true> ticd("dense");
compute(dense);
ticd.disp();
utils::Tic<true> tics("sparse");
compute(sparse);
tics.disp();
utils::Tic<true> ticb("blocks");
compute(blocks);
ticb.disp();
// std::cout << m << std::endl;
// std::cout << std::endl;
// std::cout << v.transpose() << std::endl;
//r = s * v;
return 0;
}
int main(void)
{
//timer0_init();
timer1_init();
uart_init(115200);
init_printf((void*)0,putc);
CPU_PRESCALE(0); // set for 16 MHz clock
DDRB = 0xFF;
PORTB = 0x00;
DDRD = 0xFF;
//usb_init();
uint16_t last_print = 0;
uint16_t last_led = 0;
while (1) {
_delay_ms(100);
uint16_t c = tics() >> 14;
}
}
int main()
{
SerialDebug(250000); //PrintString() and PrintFloat() using UART
BeginBasics();
Blink();
Enable_PeriphClock();
/*Roll-0 Pitch-1 Yaw-2..Roll rotation around X axis.Pitch rotation around Y axis and Yaw rotation around Z axis
note: It does not mean rotation along the X Y or Z axis*/
float RPY_c[3],RPY_k[3]; //RPY_c and RPY_k..Roll pitch and yaw obtained from complemntary filter and kalman filter
float Accel[3],Gyro[3],Tempreature; //raw values
float Accel_RealWorld[3]; //Real World Acceleration
while(Init_I2C(400)){PrintString("\nI2C Connection Error");}
while(MPU6050_Init()){PrintString("MPU6050 Initialization Error");}
MPU6050_UpdateOffsets(&MPU6050_Offsets[0]);
//MPU6050_ConfirmOffsets(&MPU6050_Offsets[0]);
while(0) //to play around with quaternions and vector rotation
{
float vector[3]= {1,0,0};
float axis_vector[3]= {0,1,0}; //rotate around Y axis
float rot_angle=90; //with 90 degrees
Quaternion q;
q=RotateVectorY(vector,rot_angle); //Rotates vector around Y axis with rot_angle
PrintString("\nRotated Vector's Quaternion\t");
DisplayQ(q);
PrintString("\n");
}
while(1)
{
MPU6050_GetRaw(&Accel[0],&Gyro[0],&Tempreature); //Reads MPU6050 Raw Data Buffer..i.e Accel Gyro and Tempreature values
if(Gyro[2]<0.3 && Gyro[2]>-0.3) Gyro[2]=0; //this actually reduces Yaw drift..will add magnetometer soon
spudnut=tics(); //tics() return current timing info..using SysTick running at CPU_Core_Frequency/8..Counter Runs from 0xFFFFFF to 0 therefore overflows every 1.864135 secs
delt=spudnut-donut; //small time dt
donut=spudnut;
//Display_Raw(Accel,Gyro,Tempreature);
Attitude_k(Accel,Gyro,RPY_k,delt); //Estimates YPR using Kalman
PrintString("\nYPR\t");
PrintFloat(RPY_k[2]);
PrintString("\t");
PrintFloat(RPY_k[1]);
PrintString("\t");
PrintFloat(RPY_k[0]);
RemoveGravity(RPY_k,Accel,Accel_RealWorld);
PrintString("\tReal World Accel with Gravity\t"); //still glitchish..working on it
DisplayVector(Accel_RealWorld);
PrintString("\t");
PrintFloat(1/delt);
}
}
