/****************************************************************************/
/* Copyright 2012-2018 MBARI												*/
/****************************************************************************/
/* Summary	: Routines to set up IMU for BEDS2 on PIC32MX470				*/
/* Filename : imu.c															*/
/* Author	: Robert Herlien (rah)											*/
/* Project	: BEDS (Benthic Event Detection System)							*/	
/* Revision : 1.0															*/
/* Created	: 10/15/2012													*/
/*																			*/
/* MBARI provides this documentation and code "as is", with no warranty,	*/
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium	*/
/* Research Institute to assist in its use, correction, modification, or	*/
/* enhancement. This information should not be published or distributed to	*/
/* third parties without specific written permission from MBARI.			*/
/*																			*/
/****************************************************************************/
/* Modification History:													*/
/* 15oct2012 rah - created													*/
/* 23feb2018 rah - ported to imu.c for BEDS2 on PIC32MX470					*/
/****************************************************************************/

#include <mbariTypes.h>			/* MBARI type definitions				*/
#include <beds.h>				/* BEDS general definitions				*/		
#include <clock.h>				/* BEDS Clock module					*/
#include <imu.h>				/* BEDS IMU setup routines				*/
#include <syslog.h>				/* System logger						*/

#include <stdio.h>


/********************************/
/*		External Data			*/
/********************************/

Extern Int32	accelRange;				/* Accelerometer range (g)			*/
Extern Int32	gyroRange;				/* Gyro range (deg/sec)				*/
Extern Int32	filterBW;				/* Digital filter bandwidth (Hz)	*/
Extern Flt32	noMotionThresh;			/* MPU no motion threshold			*/
Extern Flt32	noMotionTime;			/* MPU no motion time				*/


/********************************/
/*		Module Local Data		*/
/********************************/

MLocal Int16	curFilterBW = 42;
MLocal Int32	dmpRate = 200;
MLocal Int32	curFIFOrate = 50;
MLocal Int32	curAccelRange = 16;
MLocal Int32	curGyroRange = 2000;
MLocal Flt32	curNoMotionThresh = -1.0f;		//Force it to be set
MLocal Flt32	curNoMotionTime = -1.0f;		//Ditto


/************************************************************************/
/* Function	   : imuInit												*/
/* Purpose	   : Check filterBW for validity							*/
/* Inputs	   : None													*/
/* Outputs	   : NULL for OK, else error string							*/
/************************************************************************/
Errno	imuInit(void)
{
	return(OK);

} /* imuInit() */


/************************************************************************/
/* Function	   : imuSetup												*/
/* Purpose	   : Set up ranges and rates for IMU						*/
/* Inputs	   : FIFO rate												*/
/* Outputs	   : None													*/
/************************************************************************/
Errno	imuSetup(Int32 rate)
{
	return(OK);

} /* setDMP() */


/************************************************************************/
/* Function	   : imuReadFifo											*/
/* Purpose	   : Read one IMU FIFO data point, store results locally	*/
/* Inputs	   : None													*/
/* Outputs	   : Bytes read												*/
/************************************************************************/
int		imuReadFifo(void)
{
	return(0);
}


/************************************************************************/
/* Function	   : imuGetAccel											*/
/* Purpose	   : Fill in acceleration vector from most recent FIFO read	*/
/* Inputs	   : Ptr to accel array, number of axes in array			*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Errno	imuGetAccel(Nat32 *accel, Nat32 axes)
{
	Nat32		i;

	for (i = 0; i < axes; i++)
		accel[i] = 0;

	return(OK);
}

/************************************************************************/
/* Function	   : imuGetQuat												*/
/* Purpose	   : Fill in quaternion vector from most recent FIFO read	*/
/* Inputs	   : Ptr to quaternion array, number of axes in array		*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Errno	imuGetQuat(Nat32 *quat, Nat32 axes)
{
	Nat32		i;

	for (i = 0; i < axes; i++)
		quat[i] = 0;

	return(OK);
}


/************************************************************************/
/* Function	   : imuGetMotionState										*/
/* Purpose	   : Return Motion State									*/
/* Inputs	   : None													*/
/* Outputs	   : IMUMotionState											*/
/************************************************************************/
ImuMotionState imuGetMotionState(void)
{
	return(IMU_NO_MOTION);
}


/************************************************************************/
/* Function	   : imuLoopDelay											*/
/* Purpose	   : Calculate loop delay for given FIFO rate				*/
/* Inputs	   : FIFO rate												*/
/* Outputs	   : Delay, in ticks, to run the loop						*/
/************************************************************************/
Nat32	loopDelay(Int32 fifoRate)
{
	Nat32 delay = (FIFO_DEPTH * TICKS_PER_SECOND) / fifoRate;

	if (delay > TICKS_PER_SECOND)
		delay = TICKS_PER_SECOND;

	return(delay);

} /* loopDelay() */