sensor_epsonG364.c

Go to the documentation of this file.

//==============================================================================
//
//  sensor_epsonG364.c - Epson IMU sensor protocol specific code for 
//                                 G364 
//
//
//  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"

int sensorInitOptions(struct EpsonOptions options){
  unsigned short rxData;
  unsigned int debug = FALSE;
  unsigned short retryCount = 3000;
  
  // Safety Measure, Incase Device is in Sampling Mode, goto Config 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;
      }*/
  
  // SIG_CTRL
  int SIG_CTRL_LO = (options.enable_zgyro_delta & 0x01) << 2 |
    (options.enable_ygyro_delta & 0x01) << 3 |
    (options.enable_xgyro_delta & 0x01) << 4 |
    (options.enable_zaccel_delta & 0x01) << 5 |
    (options.enable_yaccel_delta & 0x01) << 6 |
    (options.enable_xaccel_delta & 0x01) << 7;
  
  int SIG_CTRL_HI = (options.enable_zaccel & 0x01) << 1 |
    (options.enable_yaccel & 0x01) << 2 |
    (options.enable_xaccel & 0x01) << 3 |
    (options.enable_zgyro & 0x01) << 4 |
    (options.enable_ygyro & 0x01) << 5 |
    (options.enable_xgyro & 0x01) << 6 |
    (options.enable_temp & 0x01) << 7;
  
  // MSC_CTRL
  int MSC_CTRL_LO = (options.drdy_pol & 0x01) << 1 |
    (options.drdy_on & 0x01) << 2 |
    (options.ext_pol & 0x01) << 5 |
    (options.ext_sel & 0x03) << 6;
  
  int MSC_CTRL_HI = (options.self_test & 0x01) << 2 |
    (options.flash_test & 0x01) << 3;
  
  // SMPL_CTRL
  int SMPL_CTRL_HI = options.dout_rate & 0xFF;
  
  // FILTER_CTRL
  int FILTER_CTRL_LO = (options.filter_sel & 0x1F);
  
  // UART_CTRL
  int UART_CTRL_LO = (options.uart_auto & 0x01) |
    (options.auto_start & 0x01) << 1;

  int UART_CTRL_HI = options.baud_rate & 0x01;
  
  // BURST_CTRL1
  int BURST_CTRL1_LO = (options.checksum_out & 0x1) |
    (options.count_out & 0x1) << 1 |
    (options.gpio_out & 0x01) << 2;

  int BURST_CTRL1_HI = (options.gyro_delta_out & 0x1) << 2 |
    (options.accel_delta_out & 0x01) << 3 |
    (options.accel_out & 0x01) << 4 |
    (options.gyro_out & 0x01) << 5 |
    (options.temp_out & 0x01) << 6 |
    (options.flag_out & 0x01) << 7;
  
  // BURST_CTRL2
  int BURST_CTRL2_HI = (options.gyro_delta_bit & 0x01) << 2 |
    (options.accel_delta_bit & 0x01) << 3 |
    (options.accel_bit & 0x01) << 4 |
    (options.gyro_bit & 0x01) << 5 |
    (options.temp_bit & 0x01) << 6;
  
  // POL_CTRL
  int POL_CTRL_LO = (options.invert_zaccel & 0x01) << 1 |
    (options.invert_yaccel & 0x01) << 2 |
    (options.invert_xaccel & 0x01) << 3 |
    (options.invert_zgyro & 0x01) << 4 |
    (options.invert_ygyro & 0x01) << 5 |
    (options.invert_xgyro & 0x01) << 6;

  // DLT_CTRL
  int DLT_CTRL_HI = (options.dlt_ovf_en & 0x01) << 1;
  int DLT_CTRL_LO = (options.dlt_range_ctrl & 0x0F); 

    printf("%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n",
         ADDR_SIG_CTRL_HI, SIG_CTRL_HI,
         ADDR_MSC_CTRL_LO, MSC_CTRL_LO,
         ADDR_SMPL_CTRL_HI, SMPL_CTRL_HI,
         ADDR_POL_CTRL_LO, POL_CTRL_LO,
         ADDR_FILTER_CTRL_LO, FILTER_CTRL_LO,
         ADDR_UART_CTRL_LO, UART_CTRL_LO,
         ADDR_BURST_CTRL1_LO, BURST_CTRL1_LO,
         ADDR_BURST_CTRL1_HI, BURST_CTRL1_HI,
         ADDR_BURST_CTRL2_HI, BURST_CTRL2_HI);
  
  registerWriteByte(CMD_WINDOW1, ADDR_SIG_CTRL_HI, SIG_CTRL_HI, debug);
  
  registerWriteByte(CMD_WINDOW1, ADDR_MSC_CTRL_LO, MSC_CTRL_LO, debug);
  
  registerWriteByte(CMD_WINDOW1, ADDR_SMPL_CTRL_HI, SMPL_CTRL_HI, debug);

  registerWriteByte(CMD_WINDOW1, ADDR_POL_CTRL_LO, POL_CTRL_LO, debug);
  
  registerWriteByte(CMD_WINDOW1, ADDR_FILTER_CTRL_LO, FILTER_CTRL_LO, debug);
  
  // Delay for filter config
  seDelayMS(EPSON_FILTER_DELAY);
  
  retryCount = 3000;
  do{
    rxData = registerRead16(CMD_WINDOW1, ADDR_FILTER_CTRL_LO, debug);
    retryCount--;
  } while((rxData & 0x0020) == 0x0020 && (retryCount != 0));
  
  if (retryCount == 0){
    printf("\r\n...Error: Filter busy bit did not return to 0b.");
    return FALSE;
  }
  
  registerWriteByte(CMD_WINDOW1, ADDR_UART_CTRL_LO, UART_CTRL_LO, debug);
  
  registerWriteByte(CMD_WINDOW1, ADDR_BURST_CTRL1_LO, BURST_CTRL1_LO, debug);
  
  registerWriteByte(CMD_WINDOW1, ADDR_BURST_CTRL1_HI, BURST_CTRL1_HI, debug);
  
  registerWriteByte(CMD_WINDOW1, ADDR_BURST_CTRL2_HI, BURST_CTRL2_HI, debug);

  registerWriteByte(CMD_WINDOW1, ADDR_DLT_CTRL_HI, DLT_CTRL_HI, debug);

  registerWriteByte(CMD_WINDOW1, ADDR_DLT_CTRL_LO, DLT_CTRL_LO, debug);
  
  return TRUE; /* success */
}

/*****************************************************************************
** Function name:       sensorInit
** Description:         Initialize the sensor hardware to desired settings
**                      Data Output Rate, Filter Setting, Burst Read Sensor Data
** Parameters:          None
** Return value:        1=success, 0=fail
*****************************************************************************/
int sensorInit(void)
{
    unsigned short rxData;
    unsigned int debug = FALSE;
    unsigned short retryCount = 3000;

    printf("%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n%d: %d\n",
         ADDR_SIG_CTRL_HI, CMD_EN_NDFLAGS,
         ADDR_MSC_CTRL_LO, CMD_RSTCNTR_DRDY,
         ADDR_SMPL_CTRL_HI, CMD_RATE125,
         ADDR_POL_CTRL_LO, 0x00,
         ADDR_FILTER_CTRL_LO, CMD_FLTAP16,
         ADDR_UART_CTRL_LO, 0x01,
         ADDR_BURST_CTRL1_LO, CMD_EN_BRSTDATA_LO,
         ADDR_BURST_CTRL1_HI, CMD_EN_BRSTDATA_HI,
         ADDR_BURST_CTRL2_HI, CMD_32BIT);
    return FALSE;
    // Safety Measure, Incase Device is in Sampling Mode, goto Config 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;
    }
    
    // POL_CTRL = Not Inverted
    registerWriteByte(CMD_WINDOW1, ADDR_POL_CTRL_LO, 0x00, debug);
    
    // Enable new data flags for gyros and accls
    registerWriteByte(CMD_WINDOW1, ADDR_SIG_CTRL_HI, CMD_EN_NDFLAGS, debug);
    
    // EXT=Reset Counter, EXT_POL=Positive Logic, DRDY_ON=Data_Ready, DRDY_POL = Active Low
    registerWriteByte(CMD_WINDOW1, ADDR_MSC_CTRL_LO, CMD_RSTCNTR_DRDY, debug);
    
    // Set samples per second to 125
    registerWriteByte(CMD_WINDOW1, ADDR_SMPL_CTRL_HI, CMD_RATE125, debug);
    
    // Filter TAP = 16
    registerWriteByte(CMD_WINDOW1, ADDR_FILTER_CTRL_LO, CMD_FLTAP16, debug);
    
    // Delay for filter config
    seDelayMS(EPSON_FILTER_DELAY);
    
    retryCount = 3000;
    do{
      rxData = registerRead16(CMD_WINDOW1, ADDR_FILTER_CTRL_LO, debug);
      retryCount--;
    } while((rxData & 0x0020) == 0x0020 && (retryCount != 0));
    
    if (retryCount == 0){
      printf("\r\n...Error: Filter busy bit did not return to 0b.");
      return FALSE;
    }
    
    // AUTO_START=Disable, UART_AUTO=Enable (For UART Interface)
    registerWriteByte(CMD_WINDOW1, ADDR_UART_CTRL_LO, 0x01, debug);
    
    // Enable COUNT and CHKSM data for burst mode
    registerWriteByte(CMD_WINDOW1, ADDR_BURST_CTRL1_LO, CMD_EN_BRSTDATA_LO, debug);
    
    // Enable GYRO and ACCL data for burst mode
    registerWriteByte(CMD_WINDOW1, ADDR_BURST_CTRL1_HI, CMD_EN_BRSTDATA_HI, debug);
    
    // Enable 32-bit Output for Gyro & Accel
    registerWriteByte(CMD_WINDOW1, ADDR_BURST_CTRL2_HI, CMD_32BIT, debug);
    
    return TRUE; /* success */
}


/*****************************************************************************
** Function name:       registerDump
** Description:         Read all registers for debug purpose
** Parameters:          None
** Return value:        None
*****************************************************************************/
void registerDump(void)
{
    unsigned int debug = TRUE;
    printf("\r\nRegister Dump:\r\n");
    registerRead16(0x00, 0x02, debug);
    registerRead16(0x00, 0x04, debug);
    registerRead16(0x00, 0x06, debug);
    printf("\r\n");
    registerRead16(0x00, 0x08, debug);
    registerRead16(0x00, 0x0A, debug);
    registerRead16(0x00, 0x0E, debug);
    printf("\r\n");
    registerRead16(0x00, 0x10, debug);
    registerRead16(0x00, 0x12, debug);
    registerRead16(0x00, 0x14, debug);
    printf("\r\n");
    registerRead16(0x00, 0x16, debug);
    registerRead16(0x00, 0x18, debug);
    registerRead16(0x00, 0x1A, debug);
    printf("\r\n");
    registerRead16(0x00, 0x1C, debug);
    registerRead16(0x00, 0x1E, debug);
    registerRead16(0x00, 0x20, debug);
    printf("\r\n");
    registerRead16(0x00, 0x22, debug);
    registerRead16(0x00, 0x24, debug);
    registerRead16(0x00, 0x26, debug);
    printf("\r\n");
    registerRead16(0x00, 0x28, debug);
    registerRead16(0x00, 0x64, debug);
    registerRead16(0x00, 0x68, debug);
    printf("\r\n");
    registerRead16(0x00, 0x6C, debug);
    registerRead16(0x00, 0x6E, debug);
    registerRead16(0x00, 0x70, debug);
    printf("\r\n");
    registerRead16(0x00, 0x72, debug);
    registerRead16(0x00, 0x74, debug);
    registerRead16(0x00, 0x76, debug);
    printf("\r\n");
    registerRead16(0x00, 0x78, debug);
    registerRead16(0x00, 0x7A, debug);
    printf("\r\n");
    registerRead16(0x01, 0x00, debug);
    registerRead16(0x01, 0x02, debug);
    printf("\r\n");
    registerRead16(0x01, 0x04, debug);
    registerRead16(0x01, 0x06, debug);
    registerRead16(0x01, 0x08, debug);
    printf("\r\n");
    registerRead16(0x01, 0x0A, debug);
    registerRead16(0x01, 0x0C, debug);
    registerRead16(0x01, 0x0E, debug);
    printf("\r\n");
    registerRead16(0x01, 0x6A, debug);
    registerRead16(0x01, 0x6C, debug);
    registerRead16(0x01, 0x6E, debug);
    printf("\r\n");
    registerRead16(0x01, 0x70, debug);
    registerRead16(0x01, 0x72, debug);
    registerRead16(0x01, 0x74, debug);
    printf("\r\n");
    registerRead16(0x01, 0x76, debug);
    registerRead16(0x01, 0x78, debug);
    registerRead16(0x01, 0x7A, debug);
    printf("\r\n");
    registerRead16(0x01, 0x7E, debug);
    printf("\r\n");
}