 float rollFrequencyCutoff = 0.5;
 float pitchFrequencyCutoff = 0.5;
 float yawFrequencyCutoff = 0.5;
-FilterOnePole rollHighpass(HIGHPASS, rollFrequencyCutoff);
-FilterOnePole pitchHighpass(HIGHPASS, pitchFrequencyCutoff);
-FilterOnePole yawHighpass(HIGHPASS, yawFrequencyCutoff);
+
 
 //led output pin
 const int kLedPin =  13;

//1: serial logging,
//2: SD card logging

#define LOOP_LOGGING 1
//1: enable logging in loop
//0: disable logging in loop

#define MAX_FREQUENCY 50
const long MIN_LOOP_TIME = 1000/MAX_FREQUENCY;

}

void loop() {
  FilterOnePole rollHighpass(HIGHPASS, rollFrequencyCutoff);
  FilterOnePole pitchHighpass(HIGHPASS, pitchFrequencyCutoff);
  FilterOnePole yawHighpass(HIGHPASS, yawFrequencyCutoff);
  while(1) {
    //failover tasks
    
    //imu
    if(!updateImu()) {
      logln("Something went wrong with the IMU.");
      return; //skip the cycle if the imu doesn't update correctly
    }
    
    static long time_last = 0;
  
    //limit cycle frequency to MAX_FREQUENCY
    while(millis()-time_last < MIN_LOOP_TIME);
    long loop_time = millis() - time_last;
    time_last += loop_time;
  
    //update washout filters
    rollHighpass.input(imu.roll);
    pitchHighpass.input(imu.pitch);
    yawHighpass.input(imu.yaw);
    
    if(RUN_MOTORS) {
      //sync and update motor commands
      syncMotor(2); //roll
      syncMotor(1); //pitch
      syncMotor(3); //yaw
      
      setVal(2, ANGLE, degree2rad(imu.roll)+3.14); //roll
      setVal(1, ANGLE, degree2rad(imu.pitch)+1.5); //pitch
      setVal(3, ANGLE, degree2rad(-imu.yaw) + 1.5); //yaw
    }
  
    logln("Compiling output...");
  
    static long ct = 0;
    ct++;
    
    static char string[300];
    //compile output
    sprintf(string, "%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f",ct,loop_time,imu.roll,imu.pitch,imu.yaw,imu.ax,imu.ay,imu.az,getVal(1, ANGLE),getVal(2, ANGLE),getVal(3, ANGLE));
  
    //output
    dataln(string);
    stronglogln(string);
  }
}
