Diff: LCD_ST7735/LCD_ST7735.cpp

Revision:

0:d85c449aca6d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LCD_ST7735/LCD_ST7735.cpp	Wed Jan 28 03:26:07 2015 +0000
@@ -0,0 +1,864 @@
+#include "mbed.h"
+#include "LCD_ST7735.h"
+
+const uint16_t LCD_ST7735::DefaultPalette[] = {
+    0x0000, // 0  - Black
+    0x0019, // 1  - Blue
+    0xc800, // 2  - Red
+    0xc819, // 3  - Magenta
+    0x0660, // 4  - Green
+    0x0679, // 5  - Cyan
+    0xce60, // 6  - Yellow
+    0xce79, // 7  - White        
+    0x001f, // 8  - Bright Blue
+    0xf800, // 9  - Bright Red
+    0xf81f, // 10 - Bright Magenta
+    0x07e0, // 11 - Bright Green
+    0x07ff, // 12 - Bright Cyan
+    0xffe0, // 13 - Bright Yellow
+    0xffff, // 14 - Bright White
+};
+
+LCD_ST7735::LCD_ST7735(
+    PinName backlightPin,
+    PinName resetPin,
+    PinName dsPin,
+    PinName mosiPin,
+    PinName misoPin,
+    PinName clkPin,
+    PinName csPin,
+    PanelColorFilter colorFilter
+    ) :
+        _colorFilter(colorFilter),
+        _backlight(backlightPin, 0),
+        _reset(resetPin, 1),
+        _ds(dsPin, 0),
+        _cs(csPin, 1),
+        _spi(mosiPin, misoPin, clkPin)        
+{        
+    _spi.format(8, 3);
+    _spi.frequency(18000000);
+    
+    initDisplay();
+    clearScreen();
+    setForegroundColor(0xffff);
+    setBackgroundColor(0x0000);    
+    
+    _palette = (uint16_t*)DefaultPalette;    
+}
+
+void LCD_ST7735::setOrientation(Orientation orientation, bool flip)
+{
+    const static uint8_t my = 0x80; 
+    const static uint8_t mx = 0x40;
+    const static uint8_t mv = 0x20;
+    
+    uint8_t madctlData = _colorFilter;
+    switch(orientation)
+    {
+        case Rotate0:
+            _width = 128;
+            _height = 160;
+            madctlData |= flip ? mx : 0;
+            break;
+            
+        case Rotate90:
+            _width = 160;
+            _height = 128;
+            madctlData |= flip ? my | mv | mx : mv | mx;
+            break;
+            
+        case Rotate180:
+            _width = 128;
+            _height = 160;
+            madctlData |= flip ? my : mx | my;
+            break;
+            
+        case Rotate270:
+            _width = 160;
+            _height = 128;
+            madctlData |= flip ? mv : mv | my;
+            break;
+    }
+    write(CMD_MADCTL, (uint8_t[]){madctlData}, 1);
+}
+
+int LCD_ST7735::getWidth()
+{
+    return _width;
+}
+        
+int LCD_ST7735::getHeight()
+{
+    return _height;
+}
+
+void LCD_ST7735::setBacklight(bool state)
+{
+    _backlight = state ? 1 : 0;
+}
+
+void LCD_ST7735::setPalette(uint16_t *palette)
+{
+    _palette = palette;
+}
+
+void LCD_ST7735::clearScreen(uint16_t color)
+{
+    clipRect(0, 0, _width - 1, _height - 1);
+    beginBatchCommand(CMD_RAMWR);
+    uint8_t colorHigh = color >> 8;
+    uint8_t colorLow = color;
+    for(int i = 0; i < 128 * 160 * 2; ++i)
+    {
+        writeBatchData(colorHigh, colorLow);
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::setPixel(int x, int y, uint16_t color)
+{
+    write(CMD_CASET, (uint8_t[]){0, x, 0, x}, 4);    
+    write(CMD_RASET, (uint8_t[]){0, y, 0, y}, 4);    
+    write(CMD_RAMWR, color);
+}
+
+void LCD_ST7735::drawLine(int x1, int y1, int x2, int y2, uint16_t color)
+{
+    int dx = abs(x2 - x1);
+    int dy = abs(y2 - y1);
+    
+    if (dx == 0) 
+    {
+        if (y1 > y2) swap(y1, y2);
+        drawVertLine(x1, y1, y2, color);
+        return;
+    }
+    else if(dy == 0)
+    {
+        if (x1 > x2) swap(x1, x2);
+        drawHorizLine(x1, y1, x2, color);
+        return;
+    }
+   
+    int sx = (x1 < x2) ? 1 : -1;
+    int sy = (y1 < y2) ? 1 : -1;
+    int err = dx - dy;
+    while(x1 != x2 || y1 != y2)
+    {
+        setPixel(x1, y1, color);
+        int e2 = err << 1;
+        if (e2 > -dy)
+        {
+            err -= dy;
+            x1 += sx;            
+        }
+        if (e2 < dx)
+        {
+            err += dx;
+            y1 += sy;
+        }
+    }
+    setPixel(x2, y2, color);
+}
+
+void LCD_ST7735::swap(int &a, int &b)
+{
+    int t = a;
+    a = b;
+    b = t;
+}
+
+void LCD_ST7735::drawRect(int x1, int y1, int x2, int y2, uint16_t color)
+{
+    if (x1 > x2) swap(x1, x2);
+    if (y1 > y2) swap(y1, y2);
+    
+    drawHorizLine(x1, y1, x2, color);
+    drawHorizLine(x1, y2, x2, color);
+    drawVertLine(x1, y1, y2, color);
+    drawVertLine(x2, y1, y2, color);
+}
+
+void LCD_ST7735::drawCircle(int x, int y, int r, uint16_t color)
+{
+    int ix = r;
+    int iy = 0;
+    int err = 1 - r;
+    
+    while(ix >= iy)
+    {
+        setPixel(x + ix, y + iy, color);
+        setPixel(x + iy, y + ix, color);
+        setPixel(x - ix, y + iy, color);
+        setPixel(x - iy, y + ix, color);
+        setPixel(x - ix, y - iy, color);
+        setPixel(x - iy, y - ix, color);
+        setPixel(x + ix, y - iy, color);
+        setPixel(x + iy, y - ix, color);
+        iy++;
+        if (err < 0)
+        {
+            err += 2 * iy + 1;
+        }
+        else
+        {
+            ix--;
+            err += 2 * (iy - ix + 1);
+        }
+    }
+}
+
+void LCD_ST7735::drawEllipse(int x, int y, int rx, int ry, uint16_t color)
+{
+    int a2 = rx * rx;
+    int b2 = ry * ry;
+    int fa2 = 4 * a2;
+    int fb2 = 4 * b2;
+    
+    int ix, iy, sigma;    
+    for (ix = 0, iy = ry, sigma = 2 * b2 + a2 * (1 - 2 * ry); b2 * ix <= a2 * iy; ix++)
+    {
+        setPixel(x + ix, y + iy, color);
+        setPixel(x - ix, y + iy, color);
+        setPixel(x + ix, y - iy, color);
+        setPixel(x - ix, y - iy, color);
+        if (sigma >= 0)
+        {
+            sigma+= fa2 * (1 - iy);
+            iy--;
+        }
+        sigma += b2 * ((4 * ix) + 6);
+    }
+    
+    for (ix = rx, iy = 0, sigma = 2 * a2 + b2 * (1 - 2 * rx); a2 * iy <= b2 * ix; iy++)
+    {
+        setPixel(x + ix, y + iy, color);
+        setPixel(x - ix, y + iy, color);
+        setPixel(x + ix, y - iy, color);
+        setPixel(x - ix, y - iy, color);
+        if (sigma >= 0)
+        {
+            sigma+= fb2 * (1 - ix);
+            ix--;
+        }
+        sigma += a2 * ((4 * iy) + 6);
+    }
+}
+void LCD_ST7735::fillRect(int x1, int y1, int x2, int y2, uint16_t fillColor)
+{
+    clipRect(x1, y1, x2, y2);
+    int c = ((x2-x1) * (y2-y1)) << 1;
+    uint8_t colorHigh = fillColor >> 8;
+    uint8_t colorLow = fillColor;
+    beginBatchCommand(CMD_RAMWR);
+    while(c--)
+    {
+        writeBatchData(colorHigh, colorLow);
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::fillRect(int x1, int y1, int x2, int y2, uint16_t borderColor, uint16_t fillColor)
+{
+    if (x1 > x2) swap(x1, x2);
+    if (y1 > y2) swap(y1, y2);
+    
+    drawRect(x1, y1, x2, y2, borderColor);
+    ++x1; ++y1; --x2; --y2;
+    if (x2 >= x1 && y2 >= y1)
+    {
+        clipRect(x1, y1, x2, y2);
+        int c = ((x2-x1-2) * (y2-y1-2)) << 1;
+        uint8_t colorHigh = fillColor >> 8;
+        uint8_t colorLow = fillColor;
+        beginBatchCommand(CMD_RAMWR);
+        while(c--)
+        {
+            writeBatchData(colorHigh, colorLow);
+        }
+        endBatchCommand();
+    }
+}
+
+void LCD_ST7735::fillCircle(int x, int y, int r, uint16_t borderColor, uint16_t fillColor)
+{
+    int ix = r;
+    int iy = 0;
+    int err = 1 - r;
+    
+    while(ix >= iy)
+    {
+        setPixel(x - ix, y + iy, borderColor);
+        setPixel(x + ix, y + iy, borderColor);        
+        drawHorizLine(x - ix + 1, y + iy, x + ix - 1, fillColor);
+        
+        setPixel(x - iy, y + ix, borderColor);
+        setPixel(x + iy, y + ix, borderColor);                
+        drawHorizLine(x - iy + 1, y + ix, x + iy - 1, fillColor);
+                
+        setPixel(x - ix, y - iy, borderColor);
+        setPixel(x + ix, y - iy, borderColor);
+        drawHorizLine(x - ix + 1, y - iy, x + ix - 1, fillColor);
+        
+        setPixel(x - iy, y - ix, borderColor);        
+        setPixel(x + iy, y - ix, borderColor);
+        drawHorizLine(x - iy + 1, y - ix, x + iy - 1, fillColor);
+        iy++;
+        if (err < 0)
+        {
+            err += 2 * iy + 1;
+        }
+        else
+        {
+            ix--;
+            err += 2 * (iy - ix + 1);
+        }
+    }
+}
+
+void LCD_ST7735::fillEllipse(int x, int y, int rx, int ry, uint16_t borderColor, uint16_t fillColor)
+{
+    int a2 = rx * rx;
+    int b2 = ry * ry;
+    int fa2 = 4 * a2;
+    int fb2 = 4 * b2;
+    
+    int ix, iy, sigma;    
+    for (ix = 0, iy = ry, sigma = 2 * b2 + a2 * (1 - 2 * ry); b2 * ix <= a2 * iy; ix++)
+    {
+        setPixel(x + ix, y + iy, borderColor);
+        setPixel(x - ix, y + iy, borderColor);
+        drawHorizLine(x - ix + 1, y + iy, x + ix - 1, fillColor);
+        
+        setPixel(x + ix, y - iy, borderColor);
+        setPixel(x - ix, y - iy, borderColor);
+        drawHorizLine(x - ix + 1, y - iy, x + ix - 1, fillColor);
+        
+        if (sigma >= 0)
+        {
+            sigma+= fa2 * (1 - iy);
+            iy--;
+        }
+        sigma += b2 * ((4 * ix) + 6);
+    }
+    
+    for (ix = rx, iy = 0, sigma = 2 * a2 + b2 * (1 - 2 * rx); a2 * iy <= b2 * ix; iy++)
+    {
+        setPixel(x + ix, y + iy, borderColor);
+        setPixel(x - ix, y + iy, borderColor);
+        drawHorizLine(x - ix + 1, y + iy, x + ix - 1, fillColor);
+        
+        setPixel(x + ix, y - iy, borderColor);
+        setPixel(x - ix, y - iy, borderColor);
+        drawHorizLine(x - ix + 1, y - iy, x + ix - 1, fillColor);
+        if (sigma >= 0)
+        {
+            sigma+= fb2 * (1 - ix);
+            ix--;
+        }
+        sigma += a2 * ((4 * iy) + 6);
+    }
+}
+
+void LCD_ST7735::drawBitmap(int x, int y, const uint16_t *pbmp)
+{
+    int w = *pbmp++;
+    int h = *pbmp++;
+    
+    drawBitmap(x, y, pbmp, 0, 0, w, h);
+}
+
+void LCD_ST7735::drawBitmap(int x, int y, const uint16_t *pbmp, int srcX, int srcY, int srcWidth, int srcHeight)
+{
+    // Clip if out of screen    
+    if ((x >= _width) || (x + srcWidth < 0) || 
+        (y >= _height) || (y + srcHeight < 0))
+    {
+        return;
+    }
+    
+    // Clip X
+    if (x < 0) { srcX += -x; srcWidth += x; x = 0; }
+    if (x + srcWidth >= _width) { srcWidth += _width - (x + srcWidth); }
+    
+    // Clip Y
+    if (y  < 0) {srcY += -y; srcHeight += y; y = 0; }  
+    if (y + srcHeight >= _height) { srcHeight += _height - (y + srcHeight); }
+    
+    int w = *pbmp++;
+    int h = *pbmp++;
+    
+    clip(x, y, srcWidth, srcHeight);
+    beginBatchCommand(CMD_RAMWR);    
+    const uint16_t *p = pbmp + srcX + (srcY * w);
+    for(int iy = 0; iy < srcHeight; ++iy)
+    {
+        for(int ix = 0; ix < srcWidth; ++ix)
+        {
+            writeBatchData(*(p + ix));
+        }
+        p += w;
+    } 
+    endBatchCommand();
+}
+
+void LCD_ST7735::drawBitmap(int x, int y, Bitmap2bpp &bmp, int srcX, int srcY, int srcWidth, int srcHeight)
+{
+    // Clip if out of screen    
+    if ((x >= _width) || (x + srcWidth < 0) || 
+        (y >= _height) || (y + srcHeight < 0))
+    {
+        return;
+    }
+    
+    // Clip X
+    if (x < 0) { srcX += -x; srcWidth += x; x = 0; }
+    if (x + srcWidth >= _width) { srcWidth += _width - (x + srcWidth); }
+    
+    // Clip Y
+    if (y  < 0) {srcY += -y; srcHeight += y; y = 0; }  
+    if (y + srcHeight >= _height) { srcHeight += _height - (y + srcHeight); }
+    
+    int offset = (bmp.getStride() * srcY) + (srcX / 4);
+    int startbits = 6 - ((srcX % 4) << 1);
+    
+    clip(x, y, srcWidth, srcHeight);
+    beginBatchCommand(CMD_RAMWR);
+    for(int r = 0; r < srcHeight; ++r)
+    {
+        const uint8_t *p = bmp.getBitmapData() + offset;
+        
+        uint8_t b = *p;
+        for (int c = 0, shift = startbits; c < srcWidth; ++c, shift -= 2)
+        {
+            if (shift < 0) 
+            {
+                shift = 6;
+                b = *++p;
+            }
+            
+            uint8_t colorIndex = (b >> shift) & 0x03;
+            writeBatchData(_palette[colorIndex]);            
+        }   
+        offset += bmp.getStride();             
+    }
+    endBatchCommand();
+}
+
+
+void LCD_ST7735::drawBitmap(int x, int y, Bitmap4bpp &bmp, int srcX, int srcY, int srcWidth, int srcHeight)
+{
+    // Clip if out of screen    
+    if ((x >= _width) || (x + srcWidth < 0) || 
+        (y >= _height) || (y + srcHeight < 0))
+    {
+        return;
+    }
+    
+    // Clip X
+    if (x < 0) { srcX += -x; srcWidth += x; x = 0; }
+    if (x + srcWidth >= _width) { srcWidth += _width - (x + srcWidth); }
+    
+    // Clip Y
+    if (y  < 0) {srcY += -y; srcHeight += y; y = 0; }  
+    if (y + srcHeight >= _height) { srcHeight += _height - (y + srcHeight); }
+    
+    int stride = bmp.getStride();
+    
+    bool oddStart = srcX & 0x01;
+    bool oddWidth = srcWidth & 0x01;    
+    bool oddEnd = oddStart ^ oddWidth;
+    
+    int startX = oddStart ? 1 : 0;
+    int endX = (oddEnd ? srcWidth : srcWidth + 1) >> 1;    
+    
+    const uint8_t *p = bmp.getBitmapData() + (srcX >> 1) + (srcY * stride);    
+
+    clip(x, y, srcWidth, srcHeight);
+    beginBatchCommand(CMD_RAMWR);        
+    for(int iy = 0; iy < srcHeight; ++iy, p += stride)
+    {
+        if (oddStart) writeBatchData(_palette[*p & 0x0f]);
+        for(int ix = startX; ix < endX; ++ix)
+        {
+            uint8_t c = *(p + ix);
+            writeBatchData(_palette[(c >> 4) & 0x0f]);
+            writeBatchData(_palette[c & 0x0f]);
+        }        
+        if (oddEnd) writeBatchData(_palette[(*(p + endX) >> 4) & 0x0f]);
+    } 
+    endBatchCommand();
+}
+
+void LCD_ST7735::drawBitmap(int x, int y, Bitmap1bpp &bmp, int srcX, int srcY, int srcWidth, int srcHeight, uint16_t foregroundColor, uint16_t backgroundColor)
+{
+    // Clip if out of screen    
+    if ((x >= _width) || (x + srcWidth < 0) || 
+        (y >= _height) || (y + srcHeight < 0))
+    {
+        return;
+    }
+    
+    // Clip X
+    if (x < 0) { srcX += -x; srcWidth += x; x = 0; }
+    if (x + srcWidth >= _width) { srcWidth += _width - (x + srcWidth); }
+    
+    // Clip Y
+    if (y  < 0) {srcY += -y; srcHeight += y; y = 0; }  
+    if (y + srcHeight >= _height) { srcHeight += _height - (y + srcHeight); }
+    
+    uint8_t fch = foregroundColor >> 8;
+    uint8_t fcl = foregroundColor;
+    uint8_t bch = backgroundColor >> 8;
+    uint8_t bcl = backgroundColor;
+    
+    clip(x, y, srcWidth, srcHeight);
+    
+    int offset = (bmp.getStride() * srcY) + (srcX / 8);
+    int startbits = srcX % 8;
+    
+    beginBatchCommand(CMD_RAMWR);
+    for(int r = 0; r < srcHeight; ++r)
+    {
+        const uint8_t *p = bmp.getBitmapData() + offset;
+        
+        uint8_t b = *p;
+        for (int c = 0, shift = startbits; c < srcWidth; ++c, ++shift)
+        {
+            if (shift == 8) 
+            {
+                shift = 0;
+                b = *++p;
+            }
+            
+            if ((b << shift) & 0x80)                            
+            {
+                writeBatchData(fch, fcl);                
+            }
+            else
+            {
+                writeBatchData(bch, bcl);                
+            }
+        }   
+        offset += bmp.getStride();             
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::drawBitmap(int x, int y, const uint8_t *pbmp, int srcX, int srcY, int srcWidth, int srcHeight, uint16_t foregroundColor, uint16_t backgroundColor)
+{
+    // Clip if out of screen    
+    if ((x >= _width) || (x + srcWidth < 0) || 
+        (y >= _height) || (y + srcHeight < 0))
+    {
+        return;
+    }
+    
+    // Clip X
+    if (x < 0) { srcX += -x; srcWidth += x; x = 0; }
+    if (x + srcWidth >= _width) { srcWidth += _width - (x + srcWidth); }
+    
+    // Clip Y
+    if (y  < 0) {srcY += -y; srcHeight += y; y = 0; }  
+    if (y + srcHeight >= _height) { srcHeight += _height - (y + srcHeight); }
+    
+    uint8_t fch = foregroundColor >> 8;
+    uint8_t fcl = foregroundColor;
+    uint8_t bch = backgroundColor >> 8;
+    uint8_t bcl = backgroundColor;
+    
+    uint16_t w = (*(pbmp + 1) << 8) | (*(pbmp + 0));    
+    pbmp += 4;
+    
+    int stride = w / 8;
+    clip(x, y, srcWidth, srcHeight);
+    
+    int offset = (stride * srcY) + (srcX / 8);
+    int startbits = srcX % 8;
+    
+    beginBatchCommand(CMD_RAMWR);
+    for(int r = 0; r < srcHeight; ++r)
+    {
+        const uint8_t *p = pbmp + offset;
+        
+        uint8_t b = *p;
+        for (int c = 0, shift = startbits; c < srcWidth; ++c, ++shift)
+        {
+            if (shift == 8) 
+            {
+                shift = 0;
+                b = *++p;
+            }
+            
+            if ((b << shift) & 0x80)                            
+            {
+                writeBatchData(fch, fcl);                
+            }
+            else
+            {
+                writeBatchData(bch, bcl);                
+            }
+        }   
+        offset += stride;             
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::setForegroundColor(uint16_t color)
+{
+    _foregroundColorHigh = color >> 8;
+    _foregroundColorLow = color;
+}
+
+void LCD_ST7735::setBackgroundColor(uint16_t color)
+{
+    _backgroundColorHigh = color >> 8;
+    _backgroundColorLow = color;
+}
+        
+void LCD_ST7735::drawString(const uint8_t *pFont, int x, int y, const char *pString)
+{
+    char *p = (char*)pString;
+    while(*p != 0)
+    {
+        drawChar(pFont, x, y, *p++);
+        x += 8;
+    }
+}
+
+void LCD_ST7735::selectDevice()
+{
+    _spi.prepareFastSPI();
+}
+
+void LCD_ST7735::drawVertLine(int x1, int y1, int y2, uint16_t color)
+{
+    clipRect(x1, y1, x1, y2);
+    beginBatchCommand(CMD_RAMWR);
+    int c = (y2 - y1) << 1;
+    uint8_t colorHigh = color >> 8;
+    uint8_t colorLow = color;
+    for (int i = 0; i < c; ++i)
+    {
+        writeBatchData(colorHigh, colorLow);        
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::drawHorizLine(int x1, int y1, int x2, uint16_t color)
+{    
+    clipRect(x1, y1, x2, y1);
+    beginBatchCommand(CMD_RAMWR);
+    int c = (x2 - x1) << 1;
+    uint8_t colorHigh = color >> 8;
+    uint8_t colorLow = color;
+    for (int i = 0; i < c; ++i)
+    {
+        writeBatchData(colorHigh, colorLow);
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::drawChar(const uint8_t *pFont, int x, int y, char c)
+{
+    const uint8_t *pChar = pFont + (c * 8);
+    
+    clip(x, y, 8, 8);
+    beginBatchCommand(CMD_RAMWR);
+    for(int r = 0; r < 8; ++r)
+    {
+        uint8_t b = pChar[r];
+        for(int c = 0; c < 8; ++c)
+        {
+            if (b & 0x80)
+            {
+                writeBatchData(_foregroundColorHigh);
+                writeBatchData(_foregroundColorLow);
+            }
+            else
+            {
+                writeBatchData(_backgroundColorHigh);
+                writeBatchData(_backgroundColorLow);
+            }
+                
+            b <<= 1;
+        }
+    }
+    endBatchCommand();
+}
+
+void LCD_ST7735::initDisplay()
+{
+    selectDevice();
+    reset();
+    
+    writeCommand(CMD_SLPOUT);
+    
+    write(CMD_FRMCTR1, (uint8_t[]){0x01, 0x2c, 0x2d}, 3);
+    write(CMD_FRMCTR2, (uint8_t[]){0x01, 0x2c, 0x2d}, 3);
+    write(CMD_FRMCTR3, (uint8_t[]){0x01, 0x2c, 0x2d, 0x01, 0x2c, 0x2d}, 6);
+    
+    write(CMD_INVCTR, (uint8_t[]){0x07}, 1);
+    
+    write(CMD_PWCTR1, (uint8_t[]){0xa2, 0x02, 0x84}, 3);
+    write(CMD_PWCTR2, (uint8_t[]){0xc5}, 1);
+    write(CMD_PWCTR3, (uint8_t[]){0x0a, 0x00}, 2);
+    write(CMD_PWCTR4, (uint8_t[]){0x8a, 0x2a}, 2);
+    write(CMD_PWCTR5, (uint8_t[]){0x8a, 0xee}, 2);
+    
+    write(CMD_VMCTR1, (uint8_t[]){0x0e}, 1);
+    
+    write(CMD_MADCTL, (uint8_t[]){0xc0 | _colorFilter}, 1);
+    
+    // Gama sequence
+    write(CMD_GAMCTRP1, (uint8_t[])
+        {
+            0x0f, 0x1a,
+            0x0f, 0x18,
+            0x2f, 0x28,
+            0x20, 0x22,
+            0x1f, 0x1b,
+            0x23, 0x37,
+            0x00, 0x07,
+            0x02, 0x10
+        }, 16);
+        
+    write(CMD_GAMCTRN1, (uint8_t[])
+        {
+            0x0f, 0x1b,
+            0x0f, 0x17,
+            0x33, 0x2c,
+            0x29, 0x2e,
+            0x30, 0x30,
+            0x39, 0x3f,
+            0x00, 0x07,
+            0x03, 0x10
+        }, 16);
+        
+    write(CMD_CASET, (uint8_t[]){0x00, 0x00, 0x00, 0x7f}, 4);
+    write(CMD_RASET, (uint8_t[]){0x00, 0x00, 0x00, 0x9f}, 4);
+    
+    write(CMD_EXTCTRL, (uint8_t[]){0x01}, 1);            
+    
+    // Disable RAM power save
+    write(0xf6, (uint8_t[]){0x00}, 1);                    
+    
+    // 65k color mode
+    write(CMD_COLMOD, (uint8_t[]){0x05}, 1);            
+    
+    // Enable display
+    writeCommand(CMD_DISPON);            
+    
+    setBacklight(true);
+}
+
+void LCD_ST7735::reset()
+{
+    _reset = 0;
+    wait_us(100);
+    _reset = 1;
+    wait_us(100);
+}
+
+void LCD_ST7735::clip(int x, int y, int w, int h)
+{
+    clipRect(x, y, (x + w) - 1, (y + h) - 1);
+}
+
+void LCD_ST7735::clipRect(int x1, int y1, int x2, int y2)
+{
+    uint8_t x1l = (uint8_t)x1;
+    //uint8_t x1h = (uint8_t)(x1 >> 8);
+    uint8_t x2l = (uint8_t)x2;
+    //uint8_t x2h = (uint8_t)(x2 >> 8);
+    write(CMD_CASET, (uint8_t[]){0, x1l, 0, x2l}, 4);    
+    
+    uint8_t y1l = (uint8_t)y1;
+    //uint8_t y1h = (uint8_t)(y1 >> 8);
+    uint8_t y2l = (uint8_t)y2;
+    //uint8_t y2h = (uint8_t)(y2 >> 8);
+    write(CMD_RASET, (uint8_t[]){0, y1l, 0, y2l}, 4);    
+}
+        
+void LCD_ST7735::writeCommand(uint8_t cmd)
+{
+    _cs = 0;
+    _ds = 0;    
+    _spi.fastWrite(cmd);
+    _spi.waitWhileBusy();
+    _spi.clearRx();
+    _cs = 1;
+}
+
+void LCD_ST7735::write(uint8_t cmd, uint8_t data[], int dataLen)
+{
+    _cs = 0;
+    _ds = 0;    
+    _spi.fastWrite(cmd);
+    _spi.waitWhileBusy();
+    if (data != NULL & dataLen > 0)
+    {
+        _ds = 1;        
+        for(int i = 0; i < dataLen; ++i)
+        {            
+            _spi.fastWrite(data[i]); 
+        }        
+        _spi.waitWhileBusy();
+        _ds = 0; 
+    }    
+    _spi.clearRx();
+    _cs = 1;
+}
+
+void LCD_ST7735::write(uint8_t cmd, uint16_t data)
+{
+    _cs = 0; 
+    _ds = 0;    
+    _spi.fastWrite(cmd);       
+    _spi.waitWhileBusy();
+    _ds = 1;            
+    _spi.fastWrite(data >> 8);
+    _spi.fastWrite(data);
+    _spi.waitWhileBusy();
+    _spi.clearRx();
+    _ds = 0;     
+    _cs = 1;
+}
+
+void LCD_ST7735::beginBatchCommand(uint8_t cmd)
+{
+    _cs = 0;
+    _ds = 0;        
+    _spi.fastWrite(cmd);   
+    _spi.waitWhileBusy();
+    _ds = 1;
+}
+
+void LCD_ST7735::writeBatchData(uint8_t data)
+{
+    _spi.fastWrite(data);
+}
+
+void LCD_ST7735::writeBatchData(uint8_t dataHigh, uint8_t dataLow)
+{
+    _spi.fastWrite(dataHigh);
+    _spi.fastWrite(dataLow);
+}
+
+
+void LCD_ST7735::writeBatchData(uint16_t data)
+{
+    _spi.fastWrite(data >> 8);
+    _spi.fastWrite(data); 
+}
+
+void LCD_ST7735::endBatchCommand()
+{
+    _spi.waitWhileBusy();
+    _spi.clearRx();
+    _ds = 0; 
+    _cs = 1; 
+}
+
+