   } else {
     mag_err = true;
   }
-
-  mag.enableDefault();
     
   // Start serial port to TRex
   trex.begin(19200);
+
+  next_blink_time = millis();
+  blink_state = LOW;
+  pinMode(BLINK_PIN, OUTPUT);
 
   // Initialize ros node
   ros_nh.initNode();

  // Send all the ROS messages
  ros_nh.spinOnce();

  // TODO: control the rate that messages are published?
}

bool imu_err = false;
bool mag_err = false;

#define BLINK_DURATION 2000
#define BLINK_PIN 13 // Teensy digital pin 13 for LED
int blink_state;
unsigned long next_blink_time;

char debug_str[80];

std_msgs::String ros_debug_msg;
