CC1310: Interrupt issue when switching from SDK 2_10_00_36 to 3_20_00_23

/*
 * lsm6dsm.c
 *
 *  Created on: Jul 26, 2018
 *      Author: Victor Valencia
 *   Copyright: Willowview Consulting 2018
 */
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <ti/drivers/GPIO.h>
#include <ti/drivers/SPI.h>
#include <ti/drivers/rf/RF.h>
#include <ti/drivers/PIN.h>
#include <ti/drivers/pin/PINCC26XX.h>
#include <ti/sysbios/knl/task.h>
#include <ti/sysbios/knl/event.h>
#include "board.h"
#include "swdefs.h"
#include "lsm6dsm_reg.h"
#include "lsm6dsm.h"
#include "bolusProto.h"
#include "wvcSpi.h"
#include "timeFuncs.h"
#include "trace.h"
#include "stats.h"

#define ACCEL_DEBUG(fmt, ...) \
            do { if (_ACCEL_DEBUG == 1) printf(fmt, ##__VA_ARGS__); \
                 if ((_ACCEL_DEBUG == 1) && (_TRACE_ENABLED == 1)) {snprintf(tracePrintBuf, sizeof(tracePrintBuf), fmt, ##__VA_ARGS__); \
                                           trace(tracePrintBuf); } \
            } while (0)
axis3bit16_t  data_raw_acceleration;
axis1bit16_t  data_raw_temperature;
uint8_t       whoamI;
uint8_t       rst;
lsm6dsm_ctx_t accel_ctx;

accelConfig   lsm6dsmDefaultConfiguration = {
                                             .accelDataRate      = 1,  // 12.5 hz
                                             .gyroDataRate       = 0,
                                             .accelScale         = LSM6DSM_2g,
                                             .gyroScale          = 0,
                                             .accelFilter        = LSM6DSM_XL_ANA_BW_400Hz,
                                             .gyroFilteringChain = 0,
                                             .fifoThreshold      = FIFO_THRESHOLD,
                                             .accelDecimation    = LSM6DSM_FIFO_XL_NO_DEC,
};

uint16_t getWordsInFifo(void);
int16_t accelClearFifo(void);

/*
 * For debug...
 */
void printAccelConfig(void)
{
    uint8_t idx;
    uint8_t regVal;

    for (idx = 0; idx < 0x75; idx++)
    {
        lsm6dsm_read_reg(&accel_ctx, idx, &regVal, 1);
        printf("[0x%02.02x] = 0x%02.02x\n", idx, regVal);
    }
}

RET_CODE accelLoadConfig(accelConfig *cfg)
{
    /*
     *  Check device ID.
     */
    whoamI = 0;
    lsm6dsm_device_id_get(&accel_ctx, &whoamI);
    if ( whoamI != LSM6DSM_ID )
      return(RET_CODE_ERROR);      /* device not found */

    /*
     *  Restore default configuration.
     */
    lsm6dsm_reset_set(&accel_ctx, PROPERTY_ENABLE);
    do {
      lsm6dsm_reset_get(&accel_ctx, &rst);
    } while (rst);

    /*
     * id samples that are taken prior to settling time
     */
    lsm6dsm_filter_settling_mask_set(&accel_ctx, 1);

    /*
     * Set Output Data Rate.
     */
    lsm6dsm_xl_data_rate_set(&accel_ctx, (lsm6dsm_odr_xl_t)cfg->accelDataRate);

    /*
     * Set low-power mode
     */
    lsm6dsm_xl_power_mode_set(&accel_ctx, (lsm6dsm_xl_hm_mode_t)LSM6DSM_XL_NORMAL);

    /*
     * Power down gyro
     */
    lsm6dsm_gy_data_rate_set(&accel_ctx, LSM6DSM_GY_ODR_OFF);

    /*
     * Set fifo interrupt threshold
     */
    lsm6dsm_fifo_watermark_set(&accel_ctx, cfg->fifoThreshold);

    /*
     * Set decimation
     */
    lsm6dsm_fifo_xl_batch_set(&accel_ctx, (lsm6dsm_dec_fifo_xl_t)cfg->accelDecimation);

    /*
     * Set fifo mode
     */
    lsm6dsm_fifo_mode_set(&accel_ctx, LSM6DSM_FIFO_MODE);

    /*
     * Set to stop on watermark
     */
    lsm6dsm_fifo_stop_on_wtm_set(&accel_ctx, PROPERTY_ENABLE);

    /*
     * Set fifo data rate
     */
    lsm6dsm_fifo_data_rate_set(&accel_ctx, LSM6DSM_FIFO_12Hz5);

    /*
     * Enable Block Data Update.  Required since we're using
     * the FIFO.
     */
    lsm6dsm_block_data_update_set(&accel_ctx, PROPERTY_ENABLE);

    /*
     * Enable auto-increment.  Required since we're using
     * the FIFO.
     */
    lsm6dsm_auto_increment_set(&accel_ctx, PROPERTY_ENABLE);

    /*
     * We are not providing an external sampling trigger.
     */
    lsm6dsm_den_mode_set(&accel_ctx, LSM6DSM_DEN_DISABLE);

    /*
     * Set interrupt output to active low
     */
    lsm6dsm_pin_polarity_set(&accel_ctx, LSM6DSM_ACTIVE_LOW);

    /*
     * Set interrupt routing
     */
    lsm6dsm_int1_route_t intRouting = {
                                       .int1_full_flag     = 1,
    };

    lsm6dsm_pin_int1_route_set(&accel_ctx, (lsm6dsm_int1_route_t)intRouting);

    /*
     * Set the interrupt mode
     */
    lsm6dsm_data_ready_mode_set(&accel_ctx, LSM6DSM_DRDY_LATCHED);

    lsm6dsm_xl_data_rate_set(&accel_ctx, (lsm6dsm_odr_xl_t)cfg->accelDataRate);

    /*
     * Set full scale.
     */
    lsm6dsm_xl_full_scale_set(&accel_ctx, (lsm6dsm_fs_xl_t)cfg->accelScale);

    /*
     * Configure filtering chain(No aux interface).
     */
    /*
     * Accelerometer - analog filter.
     */
    lsm6dsm_xl_filter_analog_set(&accel_ctx, (lsm6dsm_bw0_xl_t)cfg->accelFilter);

    /*
     * Accelerometer - LPF1 path ( LPF2 not used ).
     */
    //lsm6dsm_xl_lp1_bandwidth_set(&accel_ctx, LSM6DSM_XL_LP1_ODR_DIV_4);

    /*
     * Accelerometer - LPF1 + LPF2 path.
     */
    lsm6dsm_xl_lp2_bandwidth_set(&accel_ctx, (lsm6dsm_input_composite_t)cfg->accelBandwidth);

    /*
     * Accelerometer - High Pass / Slope path.
     */
    //lsm6dsm_xl_reference_mode_set(&accel_ctx, PROPERTY_DISABLE);
    //lsm6dsm_xl_hp_bandwidth_set(&accel_ctx, LSM6DSM_XL_HP_ODR_DIV_100);

    return(RET_CODE_OK);
}

/*
 * Get number of 16-bit words remaining in the fifo
 */
uint16_t getWordsInFifo(void)
{
    uint8_t tmp[2];
    lsm6dsm_read_reg(&accel_ctx, LSM6DSM_FIFO_STATUS1, tmp, 2);
    return(tmp[0] | ((tmp[1] & 0x7) << 8));
}

/*
 * In order to restart fifo after a read we must
 * change modes.
 */
void accelFifoRestart(void)
{
    lsm6dsm_fifo_mode_set(&accel_ctx, LSM6DSM_BYPASS_MODE);
    lsm6dsm_fifo_mode_set(&accel_ctx, LSM6DSM_FIFO_MODE);
}

/*
 * Read in a loop until the fifo is empty
 */
int16_t accelClearFifo(void)
{
    int16_t accelData[3];
    int16_t count = 0;

    while(getWordsInFifo() != 0)
    {
        accelReadFifo(accelData);
        count++;
    }

    return(count);
}

/*
 * Interrupt handler for accelerometer fifo full.
 */
void accelInterrupt(uint8_t index)
{
    updateStatsIncCounter(&(getStats()->accelInterrupts));
    Event_post(mainThreadEvent, EVENT_ACCEL_DATA_READY);
    return;
}

/*
 * Kick off accelerometer interrupts
 */
void accelStart(void)
{
    /* Enable interrupts */
    GPIO_enableInt(4);
    accelFifoRestart();

    return;
}

void accelStop(void)
{
    /* Disable interrupts */
    GPIO_disableInt(4);

    return;
}


void accelInit(void)
{
    accel_ctx.write_reg = (lsm6dsm_write_ptr)spi_write;
    accel_ctx.read_reg  = (lsm6dsm_read_ptr)spi_read;
    accel_ctx.handle    = 0;

    /*
     * configure the device registers
     */
    if (accelLoadConfig(&lsm6dsmDefaultConfiguration) != RET_CODE_OK)
    {
        ACCEL_DEBUG("Error initializing accelerometer\n");
        return;
    }
    else
    {
        ACCEL_DEBUG("Accelerometer initialized.\n");
    }

    /*
     * Clear fifo so that we
     * catch the first falling edge.
     */
    int16_t numReads = accelClearFifo();

    int16_t temperature = accelGetTemp();

    lsm6dsm_reg_t reg;
    lsm6dsm_status_reg_get(&accel_ctx, &reg.status_reg);

    /* config interrupt pin for falling-edge */
    GPIO_setConfig(4, GPIO_CFG_IN_PU | GPIO_CFG_IN_INT_FALLING);

    /* install interrupt callback */
    GPIO_setCallback(4, accelInterrupt);

}

/*
 * Read a single-byte register
 */
RET_CODE accelReadReg(uint8_t regIdx, uint8_t *value)
{
    return(lsm6dsm_read_reg(&accel_ctx, regIdx, value, 1));
}

/*
 * Snag the current accelerometer values
 */
RET_CODE accelReadAccelXYZ(int16_t *accelX, int16_t *accelY, int16_t *accelZ)
{
    lsm6dsm_acceleration_raw_get(&accel_ctx, data_raw_acceleration.u8bit);
    *accelX =
            LSM6DSM_FROM_FS_2g_TO_mg(data_raw_acceleration.i16bit[0]);
    *accelY =
            LSM6DSM_FROM_FS_2g_TO_mg(data_raw_acceleration.i16bit[1]);
    *accelZ =
            LSM6DSM_FROM_FS_2g_TO_mg(data_raw_acceleration.i16bit[2]);

    return(RET_CODE_OK);
}

/*
 * Read the current accelerometer values from the fifo
 */
RET_CODE accelReadFifo(int16_t *buf)
{
    int16_t tmpBuf[3];

    lsm6dsm_fifo_raw_data_get(&accel_ctx, (uint8_t*)&tmpBuf[0], 2);
    buf[0] = LSM6DSM_FROM_FS_2g_TO_mg(tmpBuf[0]);

    lsm6dsm_fifo_raw_data_get(&accel_ctx, (uint8_t*)&tmpBuf[1], 2);
    buf[1] = LSM6DSM_FROM_FS_2g_TO_mg(tmpBuf[1]);

    lsm6dsm_fifo_raw_data_get(&accel_ctx, (uint8_t*)&tmpBuf[2], 2);
    buf[2] = LSM6DSM_FROM_FS_2g_TO_mg(tmpBuf[2]);

    return(RET_CODE_OK);
}

/*
 * Snag the temperature.  It is not stored in the fifo.
 */
int16_t accelGetTemp(void)
{
    lsm6dsm_temperature_raw_get(&accel_ctx, data_raw_temperature.u8bit);
    return(LSM6DSM_FROM_LSB_TO_degC( data_raw_temperature.i16bit ));
}