sensor_epsonCommon.c

Go to the documentation of this file.

//==============================================================================
//
//  sensor_epsonImu.c - Epson IMU sensor protocol specific code common 
//                      for all IMU models
//
//
//  THE SOFTWARE IS RELEASED INTO THE PUBLIC DOMAIN.
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, NONINFRINGEMENT, 
//  SECURITY, SATISFACTORY QUALITY, AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT
//  SHALL EPSON BE LIABLE FOR ANY LOSS, DAMAGE OR CLAIM, ARISING FROM OR IN CONNECTION
//  WITH THE SOFTWARE OR THE USE OF THE SOFTWARE.
//
//==============================================================================
#include "sensor_epsonCommon.h"
#include "hcl.h"
#include "hcl_gpio.h"


/*****************************************************************************
** Function name:       sensorHWReset
** Description:         Toggle the RESET pin, delay, wait for NOT_READY bit=0
**                      This is only applicable on embedded platforms with 
**                      GPIO pin connected to IMU RESET#
** Parameters:          None
** Return value:        1=success, 0=fail
*****************************************************************************/
int sensorHWReset( void )
{
    unsigned int debug = FALSE;
    unsigned short rxData;
    unsigned short retryCount = 3000;

    gpioSet(EPSON_RESET);      // RESET pin HIGH
    seDelayMS(DELAY_EPSON_RESET);
    gpioClr(EPSON_RESET);      // Assert RESET (LOW)
    seDelayMS(DELAY_EPSON_RESET);
    gpioSet(EPSON_RESET);      // Deassert RESET (HIGH)
    seDelayMS(EPSON_POWER_ON_DELAY);

    // Poll NOT_READY bit every 1msec until returns 0
    // Exit after specified retries
    do
    {
        rxData = registerRead16(CMD_WINDOW1, ADDR_GLOB_CMD_LO, debug);
        seDelayMicroSecs(1000);
        retryCount--;
    } while((rxData & 0x0400) == 0x0400 && (retryCount != 0));

    if (retryCount == 0)
    {
        printf("\r\n...Error: NOT_READY stuck HIGH.");
        return FALSE;
    }
    return TRUE;
}


/*****************************************************************************
** Function name:       sensorPowerOn
** Description:         Initial startup, Goto Config Mode (sanity), 
**                      Trigger HW Reset, check for Hardware Error Flags
** Parameters:          None
** Return value:        1=success, 0=fail
*****************************************************************************/
int sensorPowerOn(void)
{
    unsigned short rxData = 0xFFFF;
    unsigned int debug = FALSE;
    unsigned short retryCount = 3000;

    // Safety Measure, Force Exit of Sampling Mode
    do
    {
        registerWriteByte(CMD_WINDOW0, ADDR_MODE_CTRL_HI, CMD_END_SAMPLING, debug);
        rxData = registerRead16(CMD_WINDOW0, ADDR_MODE_CTRL_LO, debug);
        seDelayMicroSecs(1000);
        retryCount--;
    } while((rxData & 0x0400) == 0x0000 && (retryCount != 0));

    if (retryCount == 0)
    {
        printf("\r\n...Error: Stuck in Sampling Mode.");
        return FALSE;
    }

    // Hardware Reset if connected, and check for NOT_READY flag
    if (!sensorHWReset())
        return FALSE;

    // Check for error flags
    rxData = registerRead16(CMD_WINDOW0, ADDR_DIAG_STAT, debug);
    if (rxData == 0x0000)
        return TRUE; /* success */
    else
        return FALSE; /* fail */
}



/*****************************************************************************
** Function name:       sensorStart
** Description:         Start sensor sampling (goto Sampling Mode)
** Parameters:          None
** Return value:        None
*****************************************************************************/
void sensorStart(void)
{
    unsigned int debug = FALSE;

    registerWriteByte(CMD_WINDOW0, ADDR_MODE_CTRL_HI, CMD_BEGIN_SAMPLING, debug);
    printf("\r\n...Sensor start.");
}


/*****************************************************************************
** Function name:       sensorStop
** Description:         Stop sensor sampling (goto Config Mode)
** Parameters:          None
** Return value:        None
*****************************************************************************/
void sensorStop(void)
{
    unsigned int debug = FALSE;

    registerWriteByte(CMD_WINDOW0, ADDR_MODE_CTRL_HI, CMD_END_SAMPLING, debug);
    seDelayMicroSecs(200000);   // Provide 200msec for sensor to finish sending sample
    printf("\r\n...Sensor stop.");
}


/*****************************************************************************
** Function name:       sensorReset
** Description:         Send Software Reset to Sensor + Delay 800 msec
** Parameters:          None
** Return value:        None
*****************************************************************************/
void sensorReset(void)
{
    unsigned int debug = FALSE;

    printf("\r\n...Software Reset begin.");
    registerWriteByte(CMD_WINDOW1, ADDR_GLOB_CMD_LO, CMD_SOFTRESET, debug);
    seDelayMS(EPSON_POWER_ON_DELAY);
    printf("\r\n...Software Reset complete.");
}


/*****************************************************************************
** Function name:       sensorFlashTest
** Description:         Send Flashtest command to Sensor and check status
**                      NOTE: Not supported for V340
** Parameters:          None
** Return value:        1=success, 0=fail
*****************************************************************************/
int sensorFlashTest(void)
{
    unsigned int debug = FALSE;
    unsigned short rxData;
    unsigned short retryCount = 3000;

    printf("\r\n...Flash test begin.");
    registerWriteByte(CMD_WINDOW1, ADDR_MSC_CTRL_HI, CMD_FLASHTEST, debug);
    seDelayMS(EPSON_FLASH_TEST_DELAY);
    do
    {
        rxData = registerRead16(CMD_WINDOW1, ADDR_MSC_CTRL_LO, debug);
        retryCount--;
    } while((rxData & 0x0800) == 0x0800 && (retryCount != 0));
    if (retryCount == 0)
        {
            printf("\r\n...Error: Flashtest bit did not retrun to 0b.");
            return FALSE;
        }

    rxData = registerRead16(CMD_WINDOW0, ADDR_DIAG_STAT, debug);
    printf("\r\n...Flash test complete.");

    if ((rxData & 0x0004) == 0x0000)
        return TRUE;
    else
        return FALSE;

}


/*****************************************************************************
** Function name:       sensorSelfTest
** Description:         Send SelfTest command to Sensor and check status
** Parameters:          None
** Return value:        1=success, 0=fail
*****************************************************************************/
int sensorSelfTest(void)
{
    unsigned int debug = FALSE;
    unsigned short rxData;
    unsigned short retryCount = 3000;

    printf("\r\n...Self test begin.");
    registerWriteByte(CMD_WINDOW1, ADDR_MSC_CTRL_HI, CMD_SELFTEST, debug);
    seDelayMS(EPSON_SELF_TEST_DELAY);
    do
    {
        rxData = registerRead16(CMD_WINDOW1, ADDR_MSC_CTRL_LO, debug);
        retryCount--;
    } while((rxData & 0x0400) == 0x0400 && (retryCount != 0));
    if (retryCount == 0)
    {
        printf("\r\n...Error: Self test bit did not return to 0b.");
        return FALSE;
    }

    rxData = registerRead16(CMD_WINDOW0, ADDR_DIAG_STAT, debug);
    printf("\r\n...Self test complete.");

    if ((rxData & 0x0002) == 0x0000)
        return TRUE;
    else
        return FALSE;

}


/*****************************************************************************
** Function name:       ppSensorDataRead32N
** Description:         32-bit post process for sensor data array returned 
**                      by either sensorDataReadN() or sensorDataReadBurstN()
**                      NOTE: Not supported for G350 or V340
** Parameters:          Pointer to output double float array, 
**                      pointer to input signed short array, start index
** Return value:        none
** Notes:
** 1. The input array order is assumed to be: GX GY GZ , AX AY AZ
**    The sensor output bit mode is assumed to be 32-bit.
**    Each input array element contains 16-bit data.
*****************************************************************************/
void ppSensorDataRead32N(double ppSensorReadData[], signed short sensorReadData[], unsigned char startIndex)
{
    ppSensorReadData[0]    = ((sensorReadData[startIndex] << 16) + sensorReadData[startIndex + 1]) * EPSON_GYRO_SF/65536;
    ppSensorReadData[1]    = ((sensorReadData[startIndex + 2] << 16) + sensorReadData[startIndex + 3]) * EPSON_GYRO_SF/65536;
    ppSensorReadData[2]    = ((sensorReadData[startIndex + 4] << 16) + sensorReadData[startIndex + 5]) * EPSON_GYRO_SF/65536;

    ppSensorReadData[3]    = ((sensorReadData[startIndex + 6] << 16) + sensorReadData[startIndex + 7]) * EPSON_ACCL_SF/65536;
    ppSensorReadData[4]    = ((sensorReadData[startIndex + 8] << 16) + sensorReadData[startIndex + 9]) * EPSON_ACCL_SF/65536;
    ppSensorReadData[5]    = ((sensorReadData[startIndex + 10] << 16) + sensorReadData[startIndex + 11]) * EPSON_ACCL_SF/65536;
}


/*****************************************************************************
** Function name:       ppSensorDataRead16N
** Description:         16-bit post process for  sensor data array returned 
**                      by either sensorDataReadN() or sensorDataReadBurstN()
**                      NOTE: Must be used for V340 or G350
** Parameters:          Pointer to output double float array, 
**                      pointer to input signed short array, start index
** Return value:        none
** Notes:
** 1. The input array order is assumed to be: GX GY GZ , AX AY AZ
**    The sensor output bit mode is assumed to be 16-bit.
**    Each input array element contains 16-bit data.
*****************************************************************************/
void ppSensorDataRead16N(double ppSensorReadData[], signed short sensorReadData[], unsigned char startIndex)
{
    ppSensorReadData[0]    = (sensorReadData[startIndex])     * EPSON_GYRO_SF;
    ppSensorReadData[1]    = (sensorReadData[startIndex + 1]) * EPSON_GYRO_SF;
    ppSensorReadData[2]    = (sensorReadData[startIndex + 2]) * EPSON_GYRO_SF;

    ppSensorReadData[3]    = (sensorReadData[startIndex + 3]) * EPSON_ACCL_SF;
    ppSensorReadData[4]    = (sensorReadData[startIndex + 4]) * EPSON_ACCL_SF;
    ppSensorReadData[5]    = (sensorReadData[startIndex + 5]) * EPSON_ACCL_SF;
}

/*****************************************************************************
** Function name:       calChksum16
** Description:         Calculate checksum16 from array of 16-bit words retrieved
**                      from IMU burst read (does not contain header (0x80) or 
**                      delimiter (0x0D) byte
** Parameters:          pointer to unsigned short array, end index to calculate to
** Return value:        16-bit checksum value
** Notes:
** 1. The burst packet consists of 16-bit data units.
**    Usually the last index in the array contains the IMU generated checksum16
**    to compare to
**
*****************************************************************************/
unsigned short calChecksum16(unsigned short sensorReadData[], unsigned int endOffset)
{
    int i;
    unsigned short chksum16_calc = 0;
    
    for (i = 0; i < endOffset; i++)
        chksum16_calc += sensorReadData[i];

    return chksum16_calc;
}