/***************************************************************************/
/* kalmanTest.c                                                            */
/* Testing the Kalman Filter algorithm                                     */
/* Author: Rich Chi Ooi                                                    */
/* Version: 1.0                                                            */
/* Date:30.05.2003                                                         */ 
/* -------------------------------                                         */
/* Compilation  procedure:                                                 */
/*       Linux                                                             */
/*      gcc68 -c  XXXXXX.c (to create object file)                         */
/*      gcc68 -o  XXXXXX.hex XXXXXX.o ppwa.o                               */
/***************************************************************************/
/* In this version:                                                        */
/***************************************************************************/
/*0.002798*/
#include <math.h>
#include <stdlib.h>
#include "eyebot.h"
#include "ppwa.h"
#include "kalman.h"
#include "sensors.h"
#include "sendData.h"

#define MAX 4000

static const float GY_SCALE =0.002798;

int counter = 0;
float GYANGLE[MAX];
float IANGLE[MAX];
float ANGLE[MAX];
float QBIAS[MAX];
float RATE[MAX];
float IRATE[MAX];
float inc_previous =0;
float inc_rate =0;

/*
 *Initialize the filter
 */
void stateInit(){

  int gyRaw = 0;
  
  gyRaw = gyroRaw();
  q_bias =gyRaw*GY_SCALE;
  angle = incAngle(); 
}

/*
 *Executes the filter code
 */
void runFilter(){
  
  float gy_angle;
  float in_angle;
  
  gy_angle = gyroRaw()*GY_SCALE;
  in_angle = incAngle();
  
  stateUpdate(gy_angle);
  kalmanUpdate(in_angle);
  
  inc_rate = (in_angle - inc_previous)/0.02;
  inc_previous = in_angle;
 
  /*Records the data*/
  GYANGLE[counter] = gy_angle;
  IANGLE[counter] = in_angle;
  ANGLE[counter] = angle;
  QBIAS[counter] = q_bias;
  RATE[counter] = rate;
  IRATE[counter] = inc_rate;
  counter++;
}

/*
 *Uploading data to PC
 */
static void uploadData(){
  int i;
  int iKey;

  LCDPrintf("Upload Data?\n");
  OSWait(50);
  LCDMenu(" "," ","YES","END");
  iKey=KEYGet();

  if(iKey==KEY3){
    OSInitRS232(SER115200,NONE,SERIAL1);
    
    for(i=0;i<=MAX;i++){
      /*LCDPrintf("V: %d D: %d\n",values[i],diff[i]);*/
      sendInteger(GYANGLE[i]*1000);
      sendCharacter(',');
      sendInteger(IANGLE[i]*1000);
      sendCharacter(',');
      sendInteger(ANGLE[i]*1000);
      sendCharacter(',');
      sendInteger(QBIAS[i]*1000);
      sendCharacter(',');
      sendInteger(RATE[i]*1000);
      sendCharacter(',');
      sendInteger(IRATE[i]*1000);
      sendCharacter('\n');
    }
    LCDPrintf("data sent");
    AUBeep();
    /*terminate transmission*/
    /*OSSendRS232(&term,SERIAL1);*/
  }
}

int main(void)
{
  int i;
  ServoHandle gyro;
  ServoHandle servo;
  TimerHandle runFilterTimer;

 /*initialize ports and sensors*/
  tpuInit();
  gyroInit(gyro);
  inclinoInit();
  servo = SERVOInit(SERVO10);

  OSWait(100);
  LCDPrintf("Kalman Filter Test\n");

  stateInit();
  /*Run the filter every 20ms*/
  runFilterTimer = OSAttachTimer(2,(TimerFnc)runFilter);

  /*rotate gyro 5 times +30 to -30 degrees*/
   for(i=0;i<15;i++){
    OSWait(200);	
    }
  
  SERVOSet(servo,128);
  OSWait(100);
  
  /* Release all handles*/
  SERVORelease(servo);
  SERVORelease(gyro);
  OSDetachTimer(runFilterTimer);
    
  LCDPrintf("reading ends !\n");
  OSWait(100);

  /*upload data*/
  uploadData();
}