#include <dsp.h>
#include "config.h"
#include "can.h"
#include "types.h"
#include "imu.h"
#include "nortek.h"

volatile float time;


volatile int outSamples[FILTERSAMPLES];

volatile int temp_inSamples[FILTERSAMPLES];
volatile int temp_outSamples[FILTERSAMPLES];

int raw = 0;  //indicates if output should be raw counts or engineering units

volatile unsigned int mode = DEFAULT_POWERUP_MODE;

volatile signed long steps = 0;
volatile signed long step_target = 0;
volatile int direction = 0;

volatile signed int CurrentHeading = 0;
volatile signed int DefaultHeading;
volatile signed int HeadingTarget;
volatile unsigned int HeadingTargetTimeout;

volatile signed int SpecifiedRudderCmd = 0;

volatile unsigned char HeadingFilterTimeConstant;
volatile unsigned int HeadingFilterDivisorCounter;

volatile unsigned char HeadingControlUpdatePeriod;
volatile unsigned int HeadingControlUpdatePeriodCounter;

volatile int f_RudderPosition;

tPID Heading_PID;
volatile fractional kCoeffs_Heading[3];

volatile int WingPos_ADC[FILTERSAMPLES];
volatile int Pressure_ADC[FILTERSAMPLES];

volatile int WingPos_Buf;
volatile int Pressure_Buf;

volatile int MeasuredHeading_Buf;

volatile float MaxRudderPosition = 1.0;

volatile int BangBang_Deadband;



unsigned int BufferA[FILTERSAMPLES*NUMAIO] __attribute__((space(dma),aligned(64)));
unsigned int BufferB[FILTERSAMPLES*NUMAIO] __attribute__((space(dma),aligned(64)));


unsigned int ECAN1MSGBUF[ECAN1_MSG_BUF_LENGTH][8] __attribute__((space(dma),aligned(ECAN1_MSG_BUF_LENGTH*16)));
//unsigned int BufferCAN1TX[CAN1TXBUFFSIZE] __attribute__((space(dma)));


CANmsg message[ECAN1_NUM_TX_BUFFERS-1];  //One message structure for each buffer


volatile STATUS Status;

IMU_Packet IMU_DataA,IMU_DataB;
volatile int IMU_Buff;
volatile Nortek_Packet Nortek_DataA;
volatile Nortek_Packet Nortek_DataB;
volatile int Nortek_Buff;

#ifdef DEBUG
volatile char test_str[255];
unsigned int uiTemp1;

int iTemp1;
int iTemp2;
int iTemp3;
int iTemp4;

float  fTemp1;
float  fTemp2;

long lTemp1;


#endif


unsigned int Speeds_PR2[8] = {8000, 4000, 2000};  //Timer values that coorespond to four speeds
unsigned int PosCtrIncr[8] = {10, 20, 40};                //Timer values that coorespond to four speeds
unsigned int speed = 2;