/**
 * Simple ADC loop for a 6 DOF IMU
 * for ATMega168
 * author: Achim Walther, www.voidpointer.de
 * version: 0.12, 28.02.2010
 *
 * output of rotomotion IMU is
 * $GPADC,<n.a.>,<n.a.>,<roll>,<pitch>,<accz>,<accy>,<accx>,<yaw>
 * the new board output:
 * $IMADC,<time_hundredths>,<pitch>,<roll>,<yaw>,<accy>,<accx>,<accz>
 *
 * With a 7.3728 Mhz Clock and 8 ADC samples per channel the IMU has a update rate of 100 Hz.
 * This rate can be influenced via UART, simply by sending a division quotient.
 * Operattion starts with a division quotient = 0, i.e. output is disabled.
 * Sending a "d1" will start the output at 100 Hz, "dA" will set the output rate to 10 Hz
 * and "d0" will disable the output again.
 */

#define VERSION "0.12"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#ifndef F_CPU
#define F_CPU 7372800UL     // Clock 7.3728 Mhz
#endif

#define BAUD 115200L          // Baudrate
#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1)

#define UPDATE_RATE 100
#define COMPARE F_CPU / 64 / UPDATE_RATE

volatile uint8_t isrCounter;
volatile uint8_t adcCounter;
volatile uint8_t adcDiv;
volatile uint8_t adcFlag;

volatile uint8_t comd = 0;

int uart_putc(unsigned char c)
{
    while (!(UCSR0A & (1<<UDRE0)));  // wait for ready to send
 
    UDR0 = c;                        // send char
    return 0;
}
 
 
/** uart_puts implemented independent from controller type */
void uart_puts (char *s)
{
    while (*s) {
        // as long as *s != '\0' end of string
        uart_putc(*s);
        s++;
    }
}


ISR(TIMER1_COMPA_vect)
{
    // if adcDiv == 0 there will be no ADC
    if (adcDiv) {
        adcCounter++;
        if (adcCounter == adcDiv) {
            adcFlag = 1;            // start another ADC in the main loop
            adcCounter = 0;         // reset adcCounter
        }

        // if adcDiv is set the LED flashes, otherwise it toggles after 1 second
        if (isrCounter == 0) PORTD |= ( 1 << PD7 ); 
    }
    isrCounter++;
    
    if (isrCounter == UPDATE_RATE)  // LED flash every full second
    {
        PORTD ^= ( 1 << PD7 ); 
        isrCounter = 0;
    }
}

ISR(USART_RX_vect)
{
    // Get and return received data from buffer
    unsigned char c = UDR0;
    //uart_putc(c);        // DEBUG

    if (comd == 0 && c == 'd') {
        // received command to set adcDiv -> the next character is the div value
        comd = 1;
    } else if (comd) {
        adcDiv = (c < 'A' ? (c - '0') : (c - 'A' + 10));
        adcCounter = 0;
        isrCounter = 0;
        comd = 0;
    }

}

/** ADC function */
uint16_t readChannel(uint8_t mux)
{
  uint8_t i;
  uint16_t result;
 
  //ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // Division Factor 8 
  //ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS0);    // Division Factor 32 
  ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1);    // Division Factor 64 
  //ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);    // Division Factor 128 
 
  ADMUX = mux;                      // chose ADC channel
  ADMUX |= (1<<REFS0);              // AVCC with external capacitor at AREF pin
 
  // after activating the ADC a dummy readout is recommended
  // the first value is read out and discarded
  ADCSRA |= (1<<ADSC);              // ADC Start Conversion 
  while ( ADCSRA & (1<<ADSC) ) ;    // wait for the conversion to complete

  result = ADCW;  // dummy readout of ADCW 

  // real measurement - compute the average of 8 conversions (oversampling)
  result = 0; 
  for (i=0; i<8; i++) {
    ADCSRA |= (1<<ADSC);            // single conversion
    while ( ADCSRA & (1<<ADSC) ) ;  // wait for the conversion to complete

    result += ADCW;		            // add result
  }
  ADCSRA &= ~(1<<ADEN);             // deactivate ADC
 
  return (result >> 1);             // compute the average by dividing by 2
}
 
 
int main(void)
{
    char out[] = "$IMADC,00,000,000,000,000,000,000\r\n\0";
    uint16_t adcval;
    uint8_t ch;
    char b2, b1, b0;
    char * p;

    adcDiv = 0;     // no ADC, no serial output
    adcCounter = 0;

    //sprintf(s, "6DOF IMU firmware version %s\r\n", VERSION);

	DDRD |= (1 << DDD7);	// configure PinD7 as output
    PORTD = 0;

    // Power and noise reduction
    PRR = (1<<PRTWI)|(1<<PRTIM2)|(1<<PRTIM0)|(1<<PRSPI);

    // USART-Init for ATmega168
 
    UBRR0H = UBRR_VAL >> 8;
    UBRR0L = UBRR_VAL & 0xFF;

    // Enable receiver and transmitter
    UCSR0B = (1<<RXCIE0)|(1<<RXEN0)|(1<<TXEN0);  // Interrupt on receive
    UCSR0C = (1<<USBS0)|(3<<UCSZ00);    // Asynchron 8N2 
 
    // initialize 16-bit timer
    TCCR1A = 0;
    TCCR1B = (1<<WGM12)|(1<<CS11)|(1<<CS10);     // CTC mode, prescaler 64
    OCR1A = COMPARE;
    TIMSK1 = (1<<OCIE1A);    // interrupt on Output Compare A

    sei();

    // main loop
	while (1) {

        // wait for exact timing
        while (!adcFlag);
        adcFlag = 0;

        p = (char *) out + 7;      // position in output string
        
        // timing output
        char hundrets = isrCounter * 100 / UPDATE_RATE;
        *p++ = '0' + (hundrets / 10);
        *p++ = '0' + (hundrets % 10);

        for (ch=2; ch<8; ch++) {
            adcval = readChannel(ch);

            p++;
            adcval &= 0x0FFF;
            b2 = '0' + (adcval >> 8);
            if (b2 > '9') b2 += 'A'-'0'-10;
            *p++ = b2;
            adcval &= 0x00FF;
            b1 = '0' + (adcval >> 4);
            if (b1 > '9') b1 += 'A'-'0'-10;
            *p++ = b1;
            adcval &= 0x000F;
            b0 = '0' + adcval;
            if (b0 > '9') b0 += 'A'-'0'-10;
            *p++ = b0;
        }
        uart_puts(out);

	}

}