/****************************************************************************/
/* Copyright 2003-2012 MBARI						    */
/****************************************************************************/
/* Summary  : Routines to set up DMP (MPU6050) for BEDS on Persistor CF2    */
/* Filename : dmpSetup.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						    */
/* $Log$
 */
/****************************************************************************/

#include <mbariTypes.h>		/* MBARI type definitions		*/
#include <mbariConst.h>		/* MBARI constants			*/
#include <cfxpico.h>		/* Persistor PicoDOS Definitions	*/
#include <beds.h>		/* BEDS general definitions		*/	
#include <io.h>			/* BEDS I/O definitions			*/
#include <dmpSetup.h>		/* BEDS DMP setup routines		*/
#include <syslog.h>		/* System logger			*/

#include <stdio.h>

/* From Invensense MotionApps Stack */
#include "log.h"
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "uMPL-main"

// MPL interface
#include "ml.h"
#include "mldl.h"
#include "mlFIFO.h"
#include "umpl-states.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 uchar	filterLPF = MPU_FILTER_42HZ;

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    : checkBW						*/
/* Purpose     : Check filterBW for validity				*/
/* Inputs      : None							*/
/* Outputs     : NULL for OK, else error string				*/
/************************************************************************/
char	*checkBW(Void)
{
    if (filterBW < 7) {
	filterBW = 5;
	filterLPF = MPU_FILTER_5HZ;
    }
    else if (filterBW < 12) {
	filterBW = 10;
	filterLPF = MPU_FILTER_10HZ;
    }
    else if (filterBW < 25) {
	filterBW = 20;
	filterLPF = MPU_FILTER_20HZ;
    }
    else if (filterBW < 50) {
	filterBW = 42;
	filterLPF = MPU_FILTER_42HZ;
    }
    else if (filterBW <= 100) {
	filterBW = 98;
	filterLPF = MPU_FILTER_98HZ;
    }
    else if (filterBW < 200) {
	filterBW = 188;
	filterLPF = MPU_FILTER_188HZ;
    }
    else {
	filterBW = 256;
	filterLPF = MPU_FILTER_256HZ_NOLPF2;
    }

    return(NULL);

} /* checkBW() */


/************************************************************************/
/* Function    : checkAccelRange					*/
/* Purpose     : Check accelRange for validity				*/
/* Inputs      : None							*/
/* Outputs     : NULL for OK, else error string				*/
/************************************************************************/
char	*checkAccelRange(Void)
{
    if ((accelRange != 2) && (accelRange != 4) && 
	(accelRange != 8) && (accelRange != 16))
	return("Invalid range for acceleration (2/4/8/16)");
    return(NULL);

} /* checkAccelRange() */


/************************************************************************/
/* Function    : checkGyroRange						*/
/* Purpose     : Check gyroRange for validity				*/
/* Inputs      : None							*/
/* Outputs     : NULL for OK, else error string				*/
/************************************************************************/
char	*checkGyroRange(Void)
{
    if ((gyroRange != 250) && (gyroRange != 500) && 
	(gyroRange != 1000) && (gyroRange != 2000))
	return("Invalid range for gyro (250/500/1000/2000)");
    return(NULL);

} /* checkGyroRange() */


/************************************************************************/
/* Function    : setAccelRange						*/
/* Purpose     : Set the full-scale range of the accelerometers		*/
/* Inputs      : FS Range as a float					*/
/* Outputs     : None							*/
/* Comments    : Written by Bob Herlien (rah), MBARI, 13Apr2012		*/
/************************************************************************/
Void setAccelRange(float range)
{
    int		i;
    long	calMatrix[9];
    signed char orientation[9];

    inv_get_accel_cal_matrix(calMatrix);
    for (i = 0; i < 9; i++)
	orientation[i] = (signed char)(calMatrix[i] & 0xff);
    inv_set_accel_calibration(range, orientation);
}


/************************************************************************/
/* Function    : setDMP							*/
/* Purpose     : Set up ranges and rates for MPU6050			*/
/* Inputs      : FIFO rate						*/
/* Outputs     : None							*/
/************************************************************************/
Void setDMP(Int32 fifoRate)
{
    Nat16	fifoDivisor;

    if ((fifoRate != curFIFOrate) || (filterBW != curFilterBW))
    {
	fifoDivisor = dmpRate/fifoRate;
	if (fifoDivisor < 1)
	    fifoDivisor = 1;

	if (filterBW != curFilterBW)
	    inv_dl_cfg_sampling(filterLPF, (unsigned char)(fifoDivisor-1));
	
	inv_set_fifo_rate((ushort)(fifoDivisor-1));

	curFilterBW = filterBW;
	curFIFOrate = dmpRate/fifoDivisor;
    }

    if (accelRange != curAccelRange)
    {
	setAccelRange((float)accelRange);
	curAccelRange = accelRange;
    }

    if (gyroRange != curGyroRange)
    {
	inv_set_full_scale((float)gyroRange);
	curGyroRange = gyroRange;
    }

    if (noMotionThresh != curNoMotionThresh)
    {
	inv_set_no_motion_thresh(noMotionThresh);
	curNoMotionThresh = noMotionThresh;
    }

    if (noMotionTime != curNoMotionTime)
    {
	inv_set_no_motion_time(noMotionTime);
	curNoMotionTime = noMotionTime;
    }

} /* setDMP() */


/************************************************************************/
/* Function    : loopDelay						*/
/* 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/2)
	delay = TICKS_PER_SECOND/2;

    return(delay);

} /* loopDelay() */