int main(void)
{
setup();
uart_init();
TWI_Init();
input = getchar();
while((input != 's'))//wait for user input to begin program
{
input = getchar();
}
sei();//global interrupts on
IMU_setup();
printf("\nLet's Begin\n\nChoose an option:\n\n space bar - PID (loops forever)\n 'm' - manual control (loops forever)\n 'i' - check IMU\n 'x' - get x acceleration\n 'y' - get y acceleration\n 'z' - get z acceleration\n 'f' - bluetooth fast mode\n 't' - test IMU write\n 'a' - enter acceleration mode\n");
while((1))
{
input = getchar();
if ((input == ' '))//PID algorithm
{
output_timer_on();
while((1))
{
PID();
}
}
else if ((input == 'm'))//manual control
{
printf("\nManual Mode:\n\nInstructions:\n\n 'n' - forwards\n 'v' - reverse\n 'b' - stop\n '1-9' - use the number keys to select the power level\n");
while((1))
{
manual();
}
}
else if ((input == 'i'))//check WHO_AM_I register
{
check_imu();
}
else if ((input == 'x'))//get x direction acceleration
{
acceleration = get_accel('x');
print_float(acceleration);
printf("\n");
}
else if ((input == 'y'))//get y direction acceleration
{
acceleration = get_accel('y');
print_float(acceleration);
printf("\n");
}
else if ((input == 'z'))//get z direction acceleration
{
acceleration = get_accel('z');
print_float(acceleration);
printf("\n");
}
else if ((input == 'f'))//place RN-42 HID into fast mode
{
fast_mode();
}
else if ((input == 't'))//write a regsiter of the IMU and check it worked
{
TWI_WRITEBYTE(MPU6050_PWR_MGMT_1, 0x02);
if ((TWI_READBYTE(MPU6050_PWR_MGMT_1) == 0x02))
printf("\nSuccess\n");
else
printf("\nFailure\n");
}
else if ((input == 'a'))//acceleration mode for acquiring data
{
acceleration_mode();
}
}
}
void vIMU_tasks()
{
uint8_t time_count;
portTickType ticks_now,ticks_old = 0;
uint8_t IMU_update_count = 0;
IMU_setup();
Baro_init();
if(RCC_GetFlagStatus(RCC_FLAG_SFTRST) == RESET) //if it was a hardware reset
{
NVIC_SystemReset(); //force software reset so that MPU sensitivity returns to normal range
}
NVIC_Configuration();
SD_Init();
//////////////////////////****************************//////////////////////////////
// create_log_file(); //Uncomment to enable log on start, else log will start only when the button on GCS is pressed
//////////////////////////****************************//////////////////////////////
for(int y=0; y<=5; y++) // Read first initial ADC values for offset.
AN_OFFSET[y] = 0;
for(;;)
{
WDT_status |= 1; //update status for IMU tasks
if(log_init_flag)
{
log_data(); //append new data to buffer
}
ticks_now = xTaskGetTickCount()*2;
if((ticks_now - ticks_old) >= 17) //do the following every 17ms
{
G_Dt = (ticks_now - ticks_old)/1000.0;
ticks_old = ticks_now;
if((!calibrating_IMU) && (!calibrating_mag)) //update IMU only if not calibrating IMU or MAG
{
IMU_Update_ARDU();
HMC5883_calculate(roll,pitch);
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
update_variables();
}
else if(calibrating_mag) //if calibrating MAG then do not process IMU, only get raw values and obtain MAG offsets
{
if(IMU_update_count >= 5)
{
IMU_Update_ARDU();
IMU_update_count = 0;
}
else
IMU_update_count++;
}
altitude = ((Baro_update(G_Dt*1000)) - ground_alt_offset); //time in ms
innerloop(); //set servo output
if(time_count == 5) //do this every 5*17ms ~= 100ms
{
send_attitude();
send_attitude_RAW();
time_count = 0;
}
else
time_count++;
}
vTaskDelay( 17/ portTICK_RATE_MS ); //3ms less to compensate for IMU update time
}
}