Compiler/CC2650: Problem With LCD Driver

#include "LCD_Driver.h"
#include "stdio.h"
#include "stdint.h"
#include <ti/drivers/PIN.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/drivers/pin/PINCC26XX.h>
#include "GPTimerCC26XX.h"
#include <driverlib/ioc.h>

/*******************************************************************************
********************************************************************************
* Private Definitions 
********************************************************************************
*******************************************************************************/
GPTimerCC26XX_Params timerParams;
GPTimerCC26XX_Handle hTimer;

/*******************************************************************************
* General LCD definitions 
*******************************************************************************/

#define LCD_REFRESH_FREQ_HZ 30

#define BP_TOTAL 4   // BACK PlANES (COM signals) 
#define FP_TOTAL 10  // FRONT PLANES (LCD signals) 

// Note: our LCD is spec'd for 3V operation, however, we power the board at 
// 3.25V. We compensate for this by inserting a contrast adjustment cycle. 
// After stepping through the 4 backplanes we insert one interval (of the same
// length) where all segments are set to 0V. That is, we drive the LCD at 80% 
// duty cycle. The RMS voltages scale with the square root of the duty cycle, 
// i.e., with sqrt(0.8) ~ 0.89. Our effective drive voltage then becomes 
// 3.25 * 0.89 ~ 2.9V. This is suitable for the 3V display and should eliminate 
// any ghosting. 

/*******************************************************************************
* Timer definitions 
*******************************************************************************/

#define gLCDTimer_c gTmr2_c

// We have a 24MHz clock with a 64 prescaler giving a timer clock of 375kHz. 
// Devide that by the LCD refresh frequency to get the counter value for one 
// full refresh cycle. Within the cycle, however, we need two polarities and 
// run through all backplanes plus one contrast adjustment cycle. Hence the 
// compare value is 

#define gComp1Val_c (375000/(LCD_REFRESH_FREQ_HZ*2*(BP_TOTAL+1)))

/*******************************************************************************
* LCD GPIO assignments 
*******************************************************************************/


//static PIN_Handle hLcdPin = NULL;


typedef enum // Port 1 GPIO register bit mapping (back planes)
{
  BP0 = GPIO_DIO_22_MASK,    //((uint32_t)(1 << 1)), // COM1: GPIO 33
  BP1 = GPIO_DIO_21_MASK,    //((uint32_t)(1 << 2)), // COM2: GPIO 34
  BP2 = GPIO_DIO_20_MASK,    //((uint32_t)(1 << 3)), // COM3: GPIO 35
  BP3 = GPIO_DIO_19_MASK     //((uint32_t)(1 << 4))  // COM4: GPIO 36
} 
BP_t;

typedef enum // Port 0 GPIO register bit mapping (front planes) 
{ 
    FP0 = GPIO_DIO_18_MASK,
    FP1 = GPIO_DIO_17_MASK,
    FP2 = GPIO_DIO_15_MASK,
    FP3 = GPIO_DIO_14_MASK,
    FP4 = GPIO_DIO_13_MASK,
    FP5 = GPIO_DIO_12_MASK,
    FP6 = GPIO_DIO_11_MASK,
    FP7 = GPIO_DIO_10_MASK,
    FP8 = GPIO_DIO_9_MASK,
    FP9 = GPIO_DIO_8_MASK
} 
FP_t;

#define BP_MASK (BP0|BP1|BP2|BP3)

#define FP_MASK (FP0|FP1|FP2|FP3|FP4|FP5|FP6|FP7|FP8|FP9)

#define BP_DATA_SET_REG     //GPIO_REGS_P->DataSetHi  // BP GPIO registers
#define BP_DATA_RESET_REG   //GPIO_REGS_P->DataResetHi
#define BP_DIR_SET_REG      //GPIO_REGS_P->DirSetHi
#define BP_DIR_RESET_REG    //GPIO_REGS_P->DirResetHi

#define FP_DATA_SET_REG     //GPIO_REGS_P->DataSetLo  // FP GPIO registers
#define FP_DATA_RESET_REG   //GPIO_REGS_P->DataResetLo
#define FP_DIR_SET_REG      //GPIO_REGS_P->DirSetLo
#define FP_DIR_RESET_REG    //GPIO_REGS_P->DirResetLo


#define BP_RESET_ALL(x)           GPIO_setMultiDio(x)
#define BP_SET_REG(x,y)           GPIO_writeMultiDio(x,y)
#define BP_RESET_REG(x,y)         GPIO_writeMultiDio(x,y)

#define FP_SET_GPIO(x,y)          GPIO_writeMultiDio(x,y)
#define FP_RESET_GPIO(x,y)        GPIO_writeMultiDio(x,y)

/*******************************************************************************
* 7-Segment digit breakdown 
*******************************************************************************/

// Digits on the display are wired into adjacent front planes and accross the 
// four back planes. The following define the first front plane for each digit. 

#define gLg10_c 0  // Large number tens digit 
#define gLg01_c 2  // Large number ones digit 
#define gSm10_c 4  // Small number tens digit 
#define gSm01_c 6  // Small number ones digit 

// We map the 7 segments into a byte where bits from the LSB to the MSB are 
// associated with the (back plane, front plane offset) values (0,0), (1,0), 
// (2,0), (3,0), (0,1), (1,1), (2,1), (3,1), respectively. 

typedef enum 
{
  SS_F = (1<<0), 
  SS_G = (1<<1), 
  SS_E = (1<<2), 
  // This bit does not coorspond with the 7 segment digit
  SS_A = (1<<4), 
  SS_B = (1<<5), 
  SS_C = (1<<6), 
  SS_D = (1<<7)
} 
SevenSegMap_t; 

// 7 segment digits from 0, ..., 9 are tabulated below. We define the 
// additional blank character and the minus symbol at the end of the table, 
// with indices 

#define gBlank_c 10
#define gMinus_c 11

/*******************************************************************************
********************************************************************************
* Private memory declarations 
********************************************************************************
*******************************************************************************/

// Steps within a refresh cycle [+/-]:   BP0  BP1  BP2  BP3  CONTRAST 
static uint32_t FpOutputs[BP_TOTAL+1] = {0,   0,   0,   0,   0}; 
static uint32_t BpOutputs[BP_TOTAL+1] = {BP0, BP1, BP2, BP3, BP_MASK}; 

// Front planes by index 
const uint32_t FP[FP_TOTAL] = {FP0, FP1, FP2, FP3, FP4, FP5, FP6, FP7, FP8, FP9}; 

// Seven segment digit table (0, 1, ..., 9 digits, ' ' and '-')
const uint8_t SevenSegTbl[12] = 
{ 
  SS_A|SS_B|SS_C|SS_D|SS_E|SS_F, 
  SS_B|SS_C, 
  SS_A|SS_B|SS_G|SS_E|SS_D, 
  SS_A|SS_B|SS_C|SS_D|SS_G, 
  SS_B|SS_C|SS_F|SS_G, 
  SS_A|SS_C|SS_D|SS_F|SS_G, 
  SS_A|SS_C|SS_D|SS_E|SS_F|SS_G, 
  SS_A|SS_B|SS_C|SS_F, 
  SS_A|SS_B|SS_C|SS_D|SS_E|SS_F|SS_G, 
  SS_A|SS_B|SS_C|SS_F|SS_G, 
  0x00, 
  SS_D
};

/*******************************************************************************
********************************************************************************
* Private Prototypes 
********************************************************************************
*******************************************************************************/

void LCD_refresh(void);
void LCD_Digit(uint8_t fp, uint8_t digit);

/*******************************************************************************
********************************************************************************
* Public Functions
********************************************************************************
*******************************************************************************/

/*******************************************************************************
* Initialize the LCD driver 
*******************************************************************************/

void LCD_Init(void)
{

uint32_t load_value = 799999;    // 33 ms

GPTimerCC26XX_Params_init(&timerParams);
timerParams.mode = GPT_MODE_PERIODIC_UP;
timerParams.width = GPT_CONFIG_16BIT;;
hTimer = GPTimerCC26XX_open(GPTIMER0B, &timerParams);
if(hTimer == NULL) 
{
  Task_exit();
}
// Set timer counting period, 1 tick =1/24000000 second
// Timer starts to count at 0, and the interrupt happens when it wraps around
// so if you want 1 second period, you need to have (24000000 - 1) ticks

GPTimerCC26XX_setLoadValue(hTimer,load_value);

GPTimerCC26XX_registerInterrupt(hTimer, timerCallback, GPT_INT_TIMEOUT);

}
/*******************************************************************************
* Call-back function to refresh the LCD Display at 30Hz
*******************************************************************************/
void timerCallback(GPTimerCC26XX_Handle handle, GPTimerCC26XX_IntMask interruptMask) 
{  
  LCD_refresh();
}
/*******************************************************************************
* Turn on the display timer - callback will drive LCD 
*******************************************************************************/
void LCD_On(void)
{ 
    LCD_Init();
  // Enable LCD timer 
   GPTimerCC26XX_start(hTimer); 
}

/*******************************************************************************
* Shut down the display timer and drive all display pins low 
*******************************************************************************/

void LCD_Off(void)
{
  // Disconnect timer clock source (-> stop) 
   GPTimerCC26XX_stop(hTimer);
  
  // Turn LCD timer off 
  GPTimerCC26XX_close(hTimer);
  
  // Drive all LCD GPIOs low 
   GPIO_clearMultiDio(FP_MASK);
   GPIO_clearMultiDio(BP_MASK);
}

/*******************************************************************************
* Set all segments 
*******************************************************************************/

void LCD_SetAll(void)
{
  uint8_t i;
  
  for(i=0; i<BP_TOTAL; i++)
  {
       FpOutputs[i] = FP_MASK;
  }
}

/*******************************************************************************
* Clear all segments 
*******************************************************************************/

void LCD_ClrAll(void)
{
  uint8_t i;
  
  for(i=0; i<BP_TOTAL; i++) FpOutputs[i] = 0; 
}

/*******************************************************************************
* Set a particular segment 
*******************************************************************************/
void LCD_SetSeg(uint8_t fp,uint8_t bp)
{
  if(fp < FP_TOTAL && bp < BP_TOTAL) FpOutputs[bp] |= FP[fp]; 
}

/*******************************************************************************
* Clear a particular segment 
*******************************************************************************/

void LCD_ClrSeg(uint8_t fp,uint8_t bp)
{
  if(fp < FP_TOTAL && bp < BP_TOTAL) FpOutputs[bp] &= (~FP[fp])&FP_MASK;
}

/*******************************************************************************
* Put number num on the large number display 
*******************************************************************************/

void LCD_SetLargeNumber(uint8_t num)
{
  // Limit range 
  if(num>199) num=199; 
  
  // First digit (ones) 
  LCD_Digit(gLg01_c, num%10); 
  
  // Decimal right shift 
  num /= 10; 
  
  // Second digit (tens, suppress leading zero)
  if(num > 0) LCD_Digit(gLg10_c, num%10); 
  else        LCD_Digit(gLg10_c, gBlank_c); 
  
  // Decimal right shift 
  num /= 10; 
  
  // Third digit (hundreds) 
  if(num > 0) LCD_SetSeg(0,3); 
  else        LCD_ClrSeg(0,3); 
}

/*******************************************************************************
* Clear the large number display 
*******************************************************************************/

void LCD_ClrLargeNumber(void)
{
  LCD_ClrSeg(0,3); 
  LCD_Digit(gLg10_c, gMinus_c); 
  LCD_Digit(gLg01_c, gMinus_c); 
}

/*******************************************************************************
* Put number num on the small number display 
*******************************************************************************/

void LCD_SetSmallNumber(uint8_t num)
{
  // Limit range 
  if(num>99) num=99; 
  
  // First digit (ones) 
  LCD_Digit(gSm01_c, num%10);
  
  // Decimal right shift 
  num /= 10; 
  
  // Second digit (tens, suppress leading zero)
  if(num > 0) LCD_Digit(gSm10_c, num%10); 
  else        LCD_Digit(gSm10_c, gBlank_c); 
}

/*******************************************************************************
* Clear the small number display 
*******************************************************************************/

void LCD_ClrSmallNumber(void)
{
  LCD_Digit(gSm10_c, gBlank_c); 
  LCD_Digit(gSm01_c, gBlank_c); 
}

/*******************************************************************************
********************************************************************************
* Private Functions
********************************************************************************
*******************************************************************************/

/*******************************************************************************
* Timer interrupt service routine to drive the display 
*******************************************************************************/
void LCD_refresh(void)
{
  static uint8_t PLUS = TRUE;
  static uint8_t i = 0;
  
  // All BPs inputs (1/2 Vcc)

    //     BP_RESET_ALL(BP_MASK);
      
      GPIO_clearMultiDio(FP_MASK);
      GPIO_setMultiDio(BP_MASK);
      
     if(PLUS)
      {
        GPIO_setMultiDio(FpOutputs[i]);
        GPIO_clearMultiDio(~FpOutputs[i]&FP_MASK);
        GPIO_clearMultiDio(BpOutputs[i]);
      }
      // Negative cycle
      else
      {
        GPIO_clearMultiDio(FpOutputs[i]);
        GPIO_setMultiDio(~FpOutputs[i]&FP_MASK);
        GPIO_setMultiDio(BpOutputs[i]);
      }

      // Next plane or ...
      if(++i == BP_TOTAL + 1)
      {
        // ... start over and flip sign
        i=0; PLUS = !PLUS;
      }
}

/*******************************************************************************
* Encode the 7-segment digit value onto a diplay digit identified by fp. 
*******************************************************************************/

void LCD_Digit(uint8_t fp,uint8_t digit)
{
  uint8_t i = 0;
  
  // Bitmask for digit value 
  digit = SevenSegTbl[digit]; 
  
  // Loop through bits 
  while(i<8)
  {
    // Ignore non-7-segment bit 3 
    if(i!=3)
    {
      // Set/clear 7-segment element 
      if(digit&0x01) LCD_SetSeg(fp + i/4, i%4); 
      else           LCD_ClrSeg(fp + i/4, i%4); 
    }
    
    // Move on 
    digit>>=1; 
    i++; 
  }
}