// main.c - measure tilt and heading with two MLX90609 gyro's

// connections:
//   PORTB 00 trigger out
//         02 distance in

// hardware:
//   The MLX90609 is either a SPI-based or an analog gyro
//   This application uses the SPI interface.
//   
// logic:
//   initialize

// $Id$

////////////////////////////////////////////////////////////////
// includes
#include <avr/io.h>            // device definitions
#include <inttypes.h>          // integer types
#include <avr/sleep.h>         // sleep mode
#include <avr/interrupt.h>
#include <string.h>
#define F_CPU 16000000UL
#define BAUD 38400
#include <util/setbaud.h>

#include "../include/ports.h"  // ports definitions
#include "../include/gyro.h"   // mlx90609 specific definitions
#include "../include/timer.h"  // timer definitions
#include "../include/bin_ascii.h" // conversion routines

#include "../include/trace.h"  // TRACE
#include "../include/spi.h"

#include "../include/datalog.h"

#include <avr/sleep.h>       /* sleep_mode() */

////////////////////////////////////////////////////////////////
// defines
#define SPI_PORT PORTB
#define ACCEL_PORT PORTB
#define ACCEL_CS 2
#define ACCEL_READ    _BV(7)
#define ACCEL_WRITE   0
#define ACCEL_SINGLE  0
#define ACCEL_MULT    _BV(6)
#define ACCEL_WHOAMI  0x0f
#define ACCEL_CTRL_REG1 0x20
#define ACCEL_CTRL_REG2 0x21
#define ACCEL_CTRL_REG3 0x22
#define ACCEL_STATUS 0x27
#define ACCEL_OUTX_L 0x28
#define ACCEL_OUTX_H 0x29
#define ACCEL_OUTY_L 0x2A
#define ACCEL_OUTY_H 0x2B
#define ACCEL_OUTZ_L 0x2C
#define ACCEL_OUTZ_H 0x2D
#define ACCEL_SPI_MODE 3

#define GYRO_HEADING 1
#define GYRO_TILT 2
#define G_ADCR_CMD 0x80
#define G_SATR_CMD 0x84
#define G_RATE_CMD 0x94
#define G_TEMP_CMD 0x9c
#define G_HEADING_PORT PORTB
#define G_HEADING_CS 3
#define G_TILT_PORT PORTB
#define G_TILT_CS 4
#define GYRO_ILLEGAL_CS 0xffff
#define GYRO_SPI_MODE  0

////////////////////////////////////////////////////////////////
// types
typedef struct spi_struct_t {
  uint8_t count;
  uint8_t spi_mode;  // set the spi mode in the spi_cmd() function
  uint8_t data[1]; // declare one, but, this will be overlaid on a buffer
} spi_struct_t ;

////////////////////////////////////////////////////////////////
// prototypes
void bin2bcd(uint16_t, char *);
void bin2ascii(uint16_t , char *);
char *bin_to_hex_ascii(uint8_t, char *);
void serial_init(void);
void adc_init(void);
uint16_t gyro(uint8_t, uint8_t);
void spi_init(void);
void spi_cmd(volatile uint8_t *, uint8_t, spi_struct_t *);
void spi_mode(uint8_t);
void gyro_start(void);
void accel_init(void);

////////////////////////////////////////////////////////////////
// global variables
int32_t loop_count;
uint16_t g_data;
extern uint8_t g_hi, g_lo;

uint8_t *dl_end;
uint8_t *dl_ptr;
uint8_t dl_buf[60];
dl_t *dl_struct;

spi_struct_t *spi_struct = (spi_struct_t *)&dl_buf[30];

////////////////////////////////////////////////////////////////
// serial init
void serial_init(void) {
  UBRRH = UBRRH_VALUE; // defined by setbaud.h values above
  UBRRL = UBRRL_VALUE; // defined by setbaud.h values above
  /* enable tx and rx */
  UCSRB = _BV(RXEN) | _BV(TXEN);
  // set URSEL to write ucsrc
  // 8N1 is the reset default
  UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);
  return;
}


//////////////////////////////////////////////////////////////////////
// uart
ISR(SIG_UART_DATA) {
  if (dl_ptr > dl_end) {
        UCSRB &= ~_BV(UDRIE);
  } else {
        UDR = *dl_ptr++;
  }
}

volatile uint8_t spi_count;
uint8_t *spi_ptr;
volatile uint8_t spi_done;

///////////////////////////////////////////////////////////////
// initialize the SPI port
void spi_init() {
  // set spi bits output
  DDRB |= _BV(SS) | _BV(SCK) | _BV(MOSI);
  // LCD Slave Select bit
  DDRB |= _BV(03);
  // set spi bits high and lcd ss high
  PORTB |= _BV(SS) | _BV(SCK) | _BV(MOSI) | _BV(03);
  // enable spi interrupts, enable spi, master, F_CPU/16
  SPCR  = _BV(SPIE) | _BV(SPE) | _BV(MSTR) | _BV(SPR0);
  // since we have not done anything, we are done with the last command
  spi_done = TRUE;
  sei();
}

///////////////////////////////////////////////////////////////
// change the spi mode
// set spi mode, spcr[3:2]
void spi_mode(uint8_t mode) {
  volatile uint8_t tmp = SPCR & ~(_BV(CPOL) | _BV(CPHA));
  switch(mode) {
  default:
  case 0:
	break;
  case 1:
	tmp |= _BV(CPHA);
	break;
  case 2:
	tmp |= _BV(CPOL);
	break;
  case 3:
	tmp |= _BV(CPOL) | _BV(CPHA);
	break;
  }
  SPCR = tmp;
}

///////////////////////////////////////////////////////////////
// spi command
void spi_cmd(volatile uint8_t *port, uint8_t bit, spi_struct_t *cmd) {
  // make this an avrx wait semaphore
  // This function is blocking.
  // make sure the spi is done
  while (!spi_done) ;
  // select the device
  *port &= ~_BV(bit);
  // set up for the next buffer
  spi_done = FALSE;
  spi_ptr = (uint8_t *)&cmd->data[0];
  spi_count = cmd->count; // decremented in ISR
  // send the first byte
  SPDR = *spi_ptr;        // incremented in the ISR
  // wait until the spi is done
  while (!spi_done) ;
  *port |= _BV(bit);
  // de-select the device
}


///////////////////////////////////////////////////////////////
// SPI interrupt handler
ISR(SIG_SPI) {
  TRACE_ON(TRACE6);
  // put the clocked in data into the command structure
  *spi_ptr++ = SPDR;
  // decrement the byte count
  spi_count--;
  if (spi_count == 0) {
	// we are done
	spi_done = TRUE;
  } else {
	// not done, send the next byte
	SPDR = *spi_ptr;
  }
  TRACE_OFF(TRACE6);
}

//////////////////////////////////////////////////////////////////////
// gyro init
void gyro_start() {
  spi_struct->spi_mode = GYRO_SPI_MODE;
  spi_mode(0);
  //
  // first heading
  spi_struct->data[ 0] = G_RATE_CMD;
  spi_struct->data[ 1] = 0; // gyro_x hi
  spi_struct->data[ 2] = 0; // gyro_x lo
  spi_struct->count = 3;   // one for cmd, two data
  spi_cmd(&G_HEADING_PORT, G_HEADING_CS, spi_struct);
  //
  // now, tilt
  spi_struct->data[ 0] = G_RATE_CMD;
  spi_struct->data[ 1] = 0; // gyro_x hi
  spi_struct->data[ 2] = 0; // gyro_x lo
  spi_struct->count = 3;   // one for cmd, two data
  spi_cmd(&G_TILT_PORT, G_TILT_CS, spi_struct);  
}

////////////////////////////////////////////////////////////////
// accel init
void accel_init() {
  spi_mode(ACCEL_SPI_MODE);
  // write the setup data
  spi_struct->data[0] = ACCEL_WRITE | ACCEL_SINGLE | ACCEL_CTRL_REG1;
  spi_struct->data[1] = 0xC7;  // enable all 3 axis
  spi_struct->count = 2; // 8 bytes, write 1 register
  spi_cmd(&ACCEL_PORT, ACCEL_CS, spi_struct);
}  

//////////////////////////////////////////////////////////////////////
// adc init
void adc_init() {
  DDRA=0x00;  /* all input */
  PORTA = 0x00;  /* all zero */
  // we will use the full 10 bits, so do not set ADLAR
  ADMUX = _BV(REFS0) | 0x07; // adc channel 7
  // adc enable, auto trigger, interrupt, F_CPU/128
  ADCSRA = _BV(ADEN) | _BV(ADATE) | _BV(ADIE)
	| _BV(ADPS2)| _BV(ADPS1) | _BV(ADPS0);
  // and, start conversion
  ADCSRA |= _BV( ADSC);
}

//////////////////////////////////////////////////////////////////////
// adc
ISR(SIG_ADC) {
  // first low then high
  dl_struct->pot = ADCL;
  dl_struct->pot |= (ADCH << 8);
  PORTC = ~ADCH;
}

////////////////////////////////////////////////////////////////
// main
int main() {
  
  timer_init();
  spi_init(); // does sei()
  TRACE_INIT;
  adc_init();
  serial_init();
  DDRB |=  _BV(4) | _BV(3) | _BV(2);  // chip selects as output
  PORTB |= _BV(4) | _BV(3) | _BV(2);  // clear select

  dl_struct = (dl_t *)&dl_buf[0];

  DDRC = 0xff;
  DDRD |= 0xf0; // bits 7:4 output

  timer_wait_ms(200);
  accel_init();
  // After init, it takes 5/decimation to settle
  // default decimation is 40, so this is 125 msec
  // This is the minimum decimation.
  timer_wait_ms(200);
  while(1) {
	spi_mode(GYRO_SPI_MODE);
	 spi_struct->spi_mode = GYRO_SPI_MODE;
	// start both gyros converting
	gyro_start();  // after spi_init
	// heading
	do {
	  spi_struct->data[ 0] = G_ADCR_CMD;
	  spi_struct->data[ 1] = 0; // gyro_x hi
	  spi_struct->data[ 2] = 0; // gyro_x lo
	  spi_struct->count = 3;   // one for cmd, two data
	  // do the command
	  spi_cmd(&G_HEADING_PORT, G_HEADING_CS, spi_struct);
	} while ((spi_struct->data[1] & _BV(5)) == 0);
	dl_struct->hdg =
	  (uint16_t) (spi_struct->data[1] << 8) | spi_struct->data[2];
	// tilt
	do {
	  spi_struct->data[ 0] = G_ADCR_CMD;
	  spi_struct->data[ 1] = 0; // gyro_x lo
	  spi_struct->data[ 2] = 0; // gyro_x hi
	  spi_struct->count = 3;   // one for cmd, two data
	  // do the command
	  spi_cmd(&G_TILT_PORT, G_TILT_CS, spi_struct);
	} while ((spi_struct->data[1] & _BV(5)) == 0);
	// put the data into datalog
	dl_struct->tilt =
	  (uint16_t) (spi_struct->data[1] << 8) | spi_struct->data[2];
	// accel
	// set up command structure - this reads three accelerations
	// in the order of lo-byte, hi-byte
	spi_mode(ACCEL_SPI_MODE);
	spi_struct->spi_mode = ACCEL_SPI_MODE;
	spi_struct->data[ 0] = ACCEL_READ | ACCEL_MULT | ACCEL_OUTX_L;
	spi_struct->data[ 1] = 0; // accel_x lo
	spi_struct->data[ 2] = 0; // accel_x hi
	spi_struct->data[ 3] = 0; // accel_y lo
	spi_struct->data[ 4] = 0; // accel_y hi
	spi_struct->data[ 5] = 0; // accel_z lo
	spi_struct->data[ 6] = 0; // accel_z hi
	spi_struct->count = 7 ;   // one for cmd, six data
	// do the command
	spi_cmd(&ACCEL_PORT, ACCEL_CS, spi_struct);
	// put the data into datalog
	dl_struct->accel_x = *(uint16_t *)&spi_struct->data[1];
	dl_struct->accel_y = *(uint16_t *)&spi_struct->data[3];
	dl_struct->accel_z = *(uint16_t *)&spi_struct->data[5];
	// setup and send the dl 
	dl_struct->dl_count = loop_count;
	dl_struct->time = timer_current();
	dl_ptr = &dl_buf[0];
	dl_end = &dl_buf[23];
#define DLE 0x10
	UDR = DLE;
	UCSRB |= _BV(UDRIE); // enable interrupts
	loop_count++;
	timer_wait_ms(19);
  }
  return 0;
}