gistfile1.hpp

#include <avr/interrupt.h>

union TimerCounter {
  struct {
    unsigned int interval_count_lo, interval_count_hi;
    unsigned char overflow_countdown;
  };
  unsigned long interval_count;
};

volatile union TimerCounter timer1_counter;

ISR(TIMER1_OVF_vect) {
  if (--timer1_counter.overflow_countdown == 0) {
    timer1_counter.overflow_countdown = 250;
    if (++timer1_counter.interval_count_lo == 0)
      ++timer1_counter.interval_count_hi == 0;
  }
}

unsigned long new_millis(void) {
  TimerCounter counter;

  uint8_t oldSREG = SREG;
  cli();
  counter.overflow_countdown = timer1_counter.overflow_countdown;
  counter.interval_count     = timer1_counter.interval_count;
  SREG = oldSREG;
  
  unsigned int extra_ticks = (unsigned int)(250 - counter.overflow_countdown) << 8; // Add TCNT1 for more accuracy.
  unsigned int extra_millis = extra_ticks/250;
  
  return (counter.interval_count << 8) | extra_millis;
}

void setup() {
  timer1_counter.interval_count = 0;
  timer1_counter.overflow_countdown = 250;
  
  bitSet  (TCCR1B, WGM12); // Put timer 1 in Fast PWM, 8-bit mode
  bitClear(TCCR1A, WGM11);
  bitSet  (TCCR1A, WGM10);
 
  bitSet  (TIMSK1, TOIE1); // Enable timer 1 overflow interrupts
  
  Serial.begin(9600);
}

void loop() {
  unsigned long x = new_millis();
  unsigned long y = millis();
  
  Serial.print(x);
  Serial.print(' ');
  Serial.print(y);
  Serial.print(' ');
  Serial.println((long)x - (long)y);
  delay(100);
}