void imu_run()
{
while(1)
{
last = CNT;
waitcnt(CNT + CLKFREQ/1000*IMU_UPDATE_DELAY);
imu_update();
}
}
void tasks_run_imu_update(void* arg)
{
if (central_data->state.mav_mode.HIL == HIL_ON)
{
simulation_update(¢ral_data->sim_model);
}
else
{
lsm330dlc_gyro_update(&(central_data->imu.raw_gyro));
lsm330dlc_acc_update(&(central_data->imu.raw_accelero));
hmc5883l_update(&(central_data->imu.raw_compass));
}
imu_update( ¢ral_data->imu);
qfilter_update(¢ral_data->attitude_filter);
if (central_data->imu.calibration_level == OFF)
{
position_estimation_update(¢ral_data->position_estimation);
}
}
int main (void)
{
unsigned char value;
//Configure all ports as inputs
TRISA = 0xFFFF; TRISB = 0xFFFF;// TRISC = 0xFFFF;
//---------------------------------------------------------------------
// OSCILATOR
//---------------------------------------------------------------------
oscilator_init();
//config_load();
//---------------------------------------------------------------------
// UART
//---------------------------------------------------------------------
uart_init();
printf("Uart Init Ok\n");
//---------------------------------------------------------------------
// STATUS (LED/SOUND)
//---------------------------------------------------------------------
status_init();
// STATUS(BLINK_SLOW,_ON,_ON);
// __delay_ms(10);
// STATUS_INIT;
//---------------------------------------------------------------------
// ADC
//---------------------------------------------------------------------
// adc_init();
// timer_init_ms();
//---------------------------------------------------------------------
// I2C
//---------------------------------------------------------------------
i2c_init();
printf("i2c Init Ok\n");
ITG3200_begin();
printf("ITG3200 Ok\n");
ADXL345_begin();
printf("ADXL Init Ok\n");
//STATUS(BLINK_FAST,_ON,_ON);
printf("Init Gyro\n");
// STATUS_NORMAL;
//__delay_ms(100);
// Acceleromter calibration
int AcclCalibration;
AcclCalibration =1;
//#define AccCali
#ifdef AccCali
while(AcclCalibration){
int i;
double adjAccl;
for(i=0;i<10;i++){
ADXL345_update();
}
float Acclx=getAcclXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gravity=sqrt(Acclx*Acclx+Accly*Accly+Acclz*Acclz);
adjAccl= 1/Gravity;
ADXL345_Scaler =adjAccl * ADXL345_Scaler;
printf("%1.3f %1.3f %1.3f Gravity %1.9f y %1.9f %1.3f\n",Acclx,Accly,Acclz,Gravity,ADXL345_Scaler,adjAccl);
if (Gravity == 1.0000) AcclCalibration=0;
}
printf("Accelerometer Calibrated \n");
#endif
//#define GyroTest
#ifdef GyroTest
while(1){
ITG3200_update();
float GyroX=getGyroXOutput();
float GyroY=getGyroYOutput();
float GyroZ=getGyroZOutput();
int ID=getGyroIDOutput();
float Temp= getGyroTempOutput();
printf("%1.3f %1.3f %1.3f %1.3f %i \n",GyroX,GyroY,GyroZ,Temp,ID);
}
#endif
//---------------------------------------------------------------------
// MOTOR
//---------------------------------------------------------------------
motor_init();
printf("motor init ok\n");
__delay_ms(100);
//#define motor_debug
#ifdef motor_debug
//while(1){
// _LAT(pinMotor0) = 1;
// _LAT(pinMotor1) = 1;
// _LAT(pinMotor2) = 1;
// _LAT(pinMotor3) = 1;
//
// _TRIS(pinMotor0) = 0;
// _TRIS(pinMotor1) = 0;
// _TRIS(pinMotor2) = 0;
// _TRIS(pinMotor3)= 0;
//
//
//}
int i=0;
while(i<50){
_LAT(pinLed1) =! _LAT(pinLed1);
motor_set_duty(0,i);
motor_set_duty(1,i);
motor_set_duty(2,i);
motor_set_duty(3,i);
motor_apply_duty();
__delay_ms(50);
i = i + 1;
// if(i> 90) i = 0;
printf("Motor %x \n");
}
__delay_ms(250);
i=0;
motor_set_duty(0,i);
motor_apply_duty();
motor_set_duty(1,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(2,i);
__delay_ms(250);
motor_apply_duty();
motor_set_duty(3,i);
__delay_ms(250);
motor_apply_duty();
__delay_ms(250);
#endif
//---------------------------------------------------------------------
// IMU
//---------------------------------------------------------------------
imu_init();
//#define imu_debug
#ifdef imu_debug
//IMU debug comment out for production
float interval_ms=0;
while (1){
ITG3200_update();
ADXL345_update();
//interval_ms = timer_dt();
// if(interval_ms > 0 ){
//we have fresh adc samples
// printf("Time %lu ",interval_us);
imu_update(interval_ms);
float* K = dcmEst[2]; //K(body) vector from DCM matrix
float pitch_roll = acos(K[2]); //total pitch and roll, angle necessary to bring K to [0,0,1]
//cos(K,K0) = [Kx,Ky,Kz].[0,0,1] = Kz
float Kxy = sqrt(K[0]*K[0] + K[1]*K[1]);
float anglePitch = - (pitch_roll * asin(K[0]/Kxy))*50;//*180/PI ;
float angleRoll = (pitch_roll * asin(K[1]/Kxy))*50;//*180/PI;
//float angleRoll = (atan2(dcmEst[2][2],dcmEst[2][1]))*180/3.14;
//float anglePitch = -(asin(dcmEst[2][0]))*180/3.14;
// float angleYaw=(-atan2(dcmGyro[1][0],dcmGyro[0][0]))*180/3.14;
float driftX=(sin(anglePitch*(3.14/180))*(sin(angleRoll*(3.14/180))));
float CalcDriftX=getAcclXOutput()-driftX;
float Acclx=getAcclXOutput();
float Gyrox=getGyroXOutput();
float Accly=getAcclYOutput();
float Acclz=getAcclZOutput();
float Gyroy=getGyroYOutput();
//printf("\n");
//if(0 == imu_sequence % 4){
// hdlc_send_byte(float_to_int(anglePitch));
// hdlc_send_byte(float_to_int(angleRoll));
// hdlc_send_byte(float_to_int(Acclx*100));
// hdlc_send_byte(float_to_int(Gyrox*1000));
// hdlc_send_byte(float_to_int(Accly*100));
// hdlc_send_byte(float_to_int(Gyroy*1000));
// //hdlc_send_byte(float_to_int(Acclz*100));
// //hdlc_send_byte(float_to_int(Kxy*100));
// //hdlc_send_byte(float_to_int(K[0]*100));
// //hdlc_send_byte(float_to_int(K[1]*100));
// //hdlc_send_byte(float_to_int(K[2]*100));
//
//// float GyroYpitch = GyroYpitch+ (getGyroYOutput()*(interval_us/1000));
//// printf("pitch %1.3f roll %1.3f Gyro ",pitch.value,roll.value,GyroYpitch);
//// printf(" Angle P%1.3f R%1.3f Drift=%1.3f Acclx=%1.3f Calc=%1.3f GyroX=%1.3f\n",anglePitch,angleRoll,driftX,Acclx,CalcDriftX,GyroX);
// hdlc_send_sep();
//}
}
}
