void mag_test(void)
{
mag_read();
Mtestx = Mresultx;
Mtesty = Mresulty;
Mtestz = Mresultz;
printf("\n\r Magenotometer Values");
printf("\n\rx = %d \t",Mtestx);
printf("y = %d \t",Mtesty);
printf("z = %d \n\r",Mtestz);
}
int main(void)
{
//Set data direction, set mux pin to low and turn off LEDs.
DDRD |= _BV(SERIAL_MUX_PIN) | _BV(RED_LED_PIN) | _BV(BLUE_LED_PIN);
PORTD &= ~(_BV(SERIAL_MUX_PIN) | _BV(BLUE_LED_PIN) | _BV(RED_LED_PIN));
//Enable USART and connect to scanf/printf.
USART_init();
USART_to_stdio();
//Initialize sensors and FPGA.
mpu_init();
fpga_init(&PORTB, &DDRB, 2); //Needs to be after mpu_init().
mag_init();
milli_timer_init();
//This is pretty important.
sei();
//Get some initial readings
mpu_scale();
mag_read();
attitude();
mag_calculate(roll, pitch);
//Use those to initialize PID state
pid_init(&pitch_pid, P_CONST, I_CONST, D_CONST, pitch + PITCH_CORRECT);
pid_init(&roll_pid, P_CONST, I_CONST, D_CONST, roll + ROLL_CORRECT);
pid_init(&heading_pid, P_CONST, I_CONST, D_CONST, heading);
//We want to be level and retain heading (for now)
target_pitch = 0;
target_roll = 0;
target_heading = heading;
//Calibrate and arm the motors.
printf("Enter y to calibrate and arm motors.\n");
scanf("%c", &cmd);
if (cmd == 'y')
{
calibrate_motors();
printf("Motors calibrated and armed.\n");
}
else
{
printf("Calibration skipped.\n");
do
{
printf("Enter 'a' to arm motors.\n");
scanf("%c", &cmd);
} while (cmd != 'a');
arm_motors();
PORTD |= _BV(BLUE_LED_PIN);
printf("Motors armed.\n");
}
uint16_t loop_duration;
unsigned long last_loop_time = 0;
uint8_t count = 0;
while(1)
{
//unsigned pt = PID_PERIOD;
//unsigned pf = PID_FREQ;
/*
printf("Period: %u; frequency: %u\n", pt, pf);
//Wait for pid flag to be true.
while (!pid_flag);
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) //Reset it atomically as soon as we start.
{
pid_flag = 0;
}
*/
loop_duration = current_time - last_loop_time;
last_loop_time = current_time;
mpu_scale();
mag_read();
attitude();
//mag_calculate(roll, pitch);
pitch_correct = CORRECTION_GAIN * pid_update(&pitch_pid, target_pitch, pitch + PITCH_CORRECT);
roll_correct = CORRECTION_GAIN * pid_update(&roll_pid, target_roll, roll + ROLL_CORRECT);
//heading_correct = CORRECTION_GAIN * pid_update(&heading_pid, target_heading, heading);
heading_correct = 0;
//Safety: if throttle=0, stop motors.
if (throttle)
{
PORTD &= ~_BV(RED_LED_PIN);
//adjust_attitude(pitch_correct, roll_correct, heading_correct);
pcms[0] = throttle; pcms[1] = throttle; pcms[2] = throttle; pcms[3] = throttle;
set_pcm(pcms);
}
else
{
PORTD |= _BV(RED_LED_PIN);
pcms[0] = 0; pcms[1] = 0; pcms[2] = 0; pcms[3] = 0;
set_pcm(pcms);
//prevent integration windup when stationary
roll_pid.integrated_error = 0;
pitch_pid.integrated_error = 0;
}
++count;
#ifdef PRINTING
if (count % 100 == 0)
{
#ifdef PRINT_CSV
printf("%i,%i,%i,", accel_x, accel_y, accel_z);
printf("%f,%f,%f,", gyro_x, gyro_y, gyro_z);
printf("%i,%i,%i,", mag_x, mag_y, mag_z);
printf("%f,%f,%f,", pitch, roll, heading);
printf("%f,%f,%f,", pitch_correct, roll_correct, heading_correct);
printf("%u,%u,%u,%u,%u", (0x00FF & (unsigned)pcms[0]), (0x00FF & (unsigned)pcms[1]), (0x00FF & (unsigned)pcms[2]), (0x00FF & (unsigned)pcms[3]), (0x00FF & (unsigned)throttle));
printf("%u\n", loop_duration);
#endif
#ifdef PRETTY_PRINT
printf("ax: %i; ay: %i; az: %i; ", accel_x, accel_y, accel_z);
printf("gx: %f; gy: %f; gz: %f; ", gyro_x, gyro_y, gyro_z);
printf("mx: %i; my: %i; mz: %i; ", mag_x, mag_y, mag_z);
printf("p: %f; r: %f; h: %f; ", pitch + PITCH_CORRECT, roll + ROLL_CORRECT, heading_deg);
printf("PC: %f; RC: %f; HC: %f; ", pitch_correct, roll_correct, heading_correct);
printf("fl: %u; fr: %u; rl: %u; rr: %u; ", (0x00FF & (unsigned)pcms[FRONT_LEFT]), (0x00FF & (unsigned)pcms[FRONT_RIGHT]), (0x00FF & (unsigned)pcms[REAR_LEFT]), (0x00FF & (unsigned)pcms[REAR_RIGHT]));
printf("thr: %u; ", (0x00FF & (unsigned)throttle));
printf("t: %u\n", loop_duration);
#endif
}
#endif
//don't send anything other than numbers, dumbass
if (!(count % 64) && (rx_bytes >= 2))
{
if (scanf("%u", &num) != 0x00)
{
if (num > 800)
throttle = 800;
else
throttle = num;
#ifndef PRINTING
printf("Throttle: %u\n", throttle);
#endif
}
}
}
}
