Revision 4:3618abce1646, committed 2022-04-21

Comitter:

vitpetrik

Date:

Thu Apr 21 13:19:41 2022 +0000

Parent:

3:45c2b2d465c4

Commit message:

Share for Stepan Oslejsek

Changed in this revision

diff -r 45c2b2d465c4 -r 3618abce1646 F042K6_USBDevice.lib
--- a/F042K6_USBDevice.lib	Mon Jan 18 10:51:25 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/va009039/code/F042K6_USBDevice/#0ba7c5e2b3dc

diff -r 45c2b2d465c4 -r 3618abce1646 MMC5883L.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMC5883L.lib	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,2 @@
+MMC5883L#000000000000
+

diff -r 45c2b2d465c4 -r 3618abce1646 MMC5883L/MMC5883L.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMC5883L/MMC5883L.cpp	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,248 @@
+#include "MMC5883L.h"
+#include <new>
+
+MMC5883L::MMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
+{
+    // Placement new to avoid additional heap memory allocation.
+    new(i2cRaw) I2C(sda, scl);
+
+    init();
+}
+
+MMC5883L::~MMC5883L()
+{
+    // If the I2C object is initialized in the buffer in this object, call destructor of it.
+    if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
+        reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
+}
+
+void MMC5883L::init()
+{
+    setConfiguration0(FLIP_SET);        // restore sensitivity by flip pulse
+    setConfiguration1(DATARATE_100);    // smallest rate
+    setConfiguration2(OUTPUT_RATE_OFF); // single mode
+    wait(0.01);
+}
+
+void MMC5883L::setConfiguration0(char config)
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_0; // register a address
+    cmd[1] = config;             // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void MMC5883L::startMeasurement_mag()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_0; // register a address
+    cmd[1] = MAGNETIC_MEASUREMENT_START; // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void MMC5883L::flipSet()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_0; // register a address
+    cmd[1] = FLIP_SET;          // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+void MMC5883L::flipReset()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_0; // register a address
+    cmd[1] = FLIP_RESET;         // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void MMC5883L::startMeasurement_temp()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_0; // register a address
+    cmd[1] = TEMP_MEASUREMENT_START; // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void MMC5883L::setConfiguration1(char config)
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_1; // register b address
+    cmd[1] = config;            // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void MMC5883L::setConfiguration2(char config)
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_2; // register b address
+    cmd[1] = config;            // register data
+
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+char MMC5883L::getConfiguration0()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_0; // register address
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+char MMC5883L::getConfiguration1()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_1; // register address
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+
+char MMC5883L::getConfiguration2()
+{
+    char cmd[2];
+    cmd[0] = INTERNAL_CONTROL_2; // register address
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+char MMC5883L::getStatus()
+{
+    char cmd[2];
+    cmd[0] = STATUS_REG; // status register
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+void MMC5883L::getXYZ_RAW(int16_t output[3])
+{
+    char cmd[2];
+    char data[6];
+
+    startMeasurement_mag(); // start measurement
+    while((getStatus()&STATUS_M_DONE)==0); // wait until complete
+
+    cmd[0] = OUTPUT_REG; // starting addr for reading
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, data, 6, false);
+    int32_t tmp[3];
+    for(int i = 0; i < 3; i++) { // fill the output variables, X, Y, Z, LSB first
+        tmp[i] = ((uint16_t)(data[i*2+1] << 8)) | (uint16_t)(data[i*2]);
+        tmp[i] -= 65536/2;
+        output[i] = tmp[i];
+    }
+}
+
+void MMC5883L::getXYZ_nT(int32_t output[3])
+{
+    int16_t data[3];
+    getXYZ_RAW(data);
+
+    for(int i = 0; i < 3; i++) {
+        int64_t tmp =  data[i];
+        tmp *= 800000;
+        tmp /= (65536/2); // +- 8G = 16G FS
+        output[i] = tmp;
+    }
+}
+
+void MMC5883L::getXYZ_OffsetRemoved_RAW(int16_t output[3])
+{
+    // TODO: offset reemove
+    int16_t data_set[3];
+    flipSet();
+    getXYZ_RAW(data_set);
+    int16_t data_reset[3];
+    flipReset();
+    getXYZ_RAW(data_reset);
+
+    for(int i = 0; i < 3; i++) {
+        int16_t diff = data_reset[i] - data_set[i];
+        output[i] = data_reset[i] - diff/2;
+    }
+}
+
+void MMC5883L::getXYZ_OffsetRemoved_nT(int32_t output[3])
+{
+    int16_t data[3];
+    getXYZ_OffsetRemoved_RAW(data);
+
+    for(int i = 0; i < 3; i++) {
+        int64_t tmp =  data[i];
+        tmp *= 800000;
+        tmp /= (65536/2); // +- 8G = 16G FS
+        output[i] = tmp;
+    }
+}
+
+
+double MMC5883L::getHeadingXY()
+{
+    int16_t raw_data[3];
+    getXYZ_OffsetRemoved_RAW(raw_data);
+
+    double heading = atan2(static_cast<double>(raw_data[0]), static_cast<double>(raw_data[1])); // heading = arctan(X/Y)
+
+    heading += DECLINATION_ANGLE;
+
+    if(heading < 0.0) // fix sign
+        heading += PI2;
+
+    if(heading > PI2) // fix overflow
+        heading -= PI2;
+    return heading;
+}
+double MMC5883L::getHeadingXY(int16_t output[3])
+{
+    getXYZ_OffsetRemoved_RAW(output);
+
+    double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
+
+    heading += DECLINATION_ANGLE;
+
+    if(heading < 0.0) // fix sign
+        heading += PI2;
+
+    if(heading > PI2) // fix overflow
+        heading -= PI2;
+    return heading;
+}
+double MMC5883L::getHeadingXY(int32_t output[3])
+{
+    getXYZ_OffsetRemoved_nT(output);
+
+    double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
+
+    heading += DECLINATION_ANGLE;
+
+    if(heading < 0.0) // fix sign
+        heading += PI2;
+
+    if(heading > PI2) // fix overflow
+        heading -= PI2;
+    return heading;
+}
+
+double MMC5883L::getHeadingXYDeg()
+{
+    return (getHeadingXY() * RAD_TO_DEG);
+}
+double MMC5883L::getHeadingXYDeg_RAW(int16_t output[3])
+{
+    return (getHeadingXY(output) * RAD_TO_DEG);
+}
+double MMC5883L::getHeadingXYDeg_nT(int32_t output[3])
+{
+    return (getHeadingXY(output) * RAD_TO_DEG);
+}
+
+

diff -r 45c2b2d465c4 -r 3618abce1646 MMC5883L/MMC5883L.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMC5883L/MMC5883L.h	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,152 @@
+#ifndef MMC5883L_H
+#define MMC5883L_H
+
+#include "mbed.h"
+
+/*
+* Defines
+*/
+
+//-----------
+// Registers
+//-----------
+#define INTERNAL_CONTROL_0    0x08
+#define INTERNAL_CONTROL_1    0x09
+#define INTERNAL_CONTROL_2    0x0A
+#define OUTPUT_REG      0x00
+#define STATUS_REG      0x07
+
+#define OUTPUT_RATE_OFF    0x00
+#define OUTPUT_RATE_14     0x01
+#define OUTPUT_RATE_5      0x02
+#define OUTPUT_RATE_2_2    0x03
+#define OUTPUT_RATE_1      0x04
+#define OUTPUT_RATE_0_5    0x05
+#define OUTPUT_RATE_0_25   0x06
+#define OUTPUT_RATE_0_12   0x07
+#define OUTPUT_RATE_0_06   0x08
+#define OUTPUT_RATE_0_03   0x09
+#define OUTPUT_RATE_0_015  0x0A
+#define DRDY_INTERRUPT_EN    0x40
+#define MOTION_INTERRUPT_EN  0x20
+
+#define SW_RST  0x80
+#define Z_INHIBIT       0x40
+#define Y_INHIBIT       0x20
+#define X_INHIBIT       0x10
+#define DATARATE_100       0x00
+#define DATARATE_200       0x01
+#define DATARATE_400       0x02
+#define DATARATE_600       0x03
+
+#define MAGNETIC_MEASUREMENT_START 0x01
+#define TEMP_MEASUREMENT_START 0x02
+#define MOTION_DETECT_START 0x04
+#define FLIP_SET 0x08
+#define FLIP_RESET 0x10
+
+
+// status register
+#define STATUS_M_DONE       0x01
+#define STATUS_T_DONE       0x02
+#define STATUS_MOTION_DET   0x04
+
+// Utility
+#ifndef M_PI
+#define M_PI 3.1415926535897932384626433832795
+#endif
+
+#define PI2         (2*M_PI)
+#define RAD_TO_DEG  (180.0/M_PI)
+#define DEG_TO_RAD  (M_PI/180.0)
+
+// Once you have your heading, you must then add your 'Declination Angle', 
+// which is  the 'Error' of the magnetic field in your location.
+// Find yours here: http://www.magnetic-declination.com/
+// Mine is:  -1° 13' WEST which is -1.2167 Degrees, or (which we need) 
+// 0,021234839232597676519238237683278  radians, I will use 0.02123
+// If you cannot find your Declination, put 0, your compass will be slightly off.
+
+#define  DECLINATION_ANGLE -0.02123
+//#define  DECLINATION_ANGLE 0
+
+
+/**
+ * The MMC5883L 3-Axis Digital Compass IC
+ */
+class MMC5883L
+{
+
+public:
+
+    /**
+     * The I2C address that can be passed directly to i2c object (it's already shifted 1 bit left).
+     */
+    static const int I2C_ADDRESS = 0x60;
+
+    /**
+     * Constructor.
+     *
+     * Calls init function
+     *
+     * @param sda - mbed pin to use for the SDA I2C line.
+     * @param scl - mbed pin to use for the SCL I2C line.
+     */
+    MMC5883L(PinName sda, PinName scl);
+
+    /**
+    * Constructor that accepts external i2c interface object.
+    * 
+    * Calls init function
+    *
+    * @param i2c The I2C interface object to use.
+    */
+    MMC5883L(I2C &i2c) : i2c_(i2c) {
+        init();
+    }
+
+    ~MMC5883L();
+
+    void init();
+
+    void setConfiguration0(char);
+    void setConfiguration1(char);
+    void setConfiguration2(char);
+
+    char getConfiguration0();
+    char getConfiguration1();
+    char getConfiguration2();
+
+    void flipSet();
+    void flipReset();
+
+    void startMeasurement_temp();
+    void startMeasurement_mag();
+
+    void getXYZ_RAW(int16_t raw[3]);
+    void getXYZ_OffsetRemoved_RAW(int16_t raw[3]);
+    void getXYZ_nT(int32_t raw[3]);
+    void getXYZ_OffsetRemoved_nT(int32_t raw[3]);
+
+    
+    char getStatus();
+    
+    double getHeadingXY();
+    double getHeadingXY(int16_t output[3]);    
+    double getHeadingXY(int32_t output[3]);   
+    double getHeadingXYDeg();
+    double getHeadingXYDeg_RAW(int16_t output[3]);
+    double getHeadingXYDeg_nT(int32_t output[3]);
+
+private:
+
+    I2C &i2c_;
+
+    /**
+     * The raw buffer for allocating I2C object in its own without heap memory.
+     */
+    char i2cRaw[sizeof(I2C)];
+};
+
+#endif // MMC5883L
+

diff -r 45c2b2d465c4 -r 3618abce1646 SSD1306_mini/Adafruit_GFX.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SSD1306_mini/Adafruit_GFX.cpp	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,456 @@
+/***********************************
+This is a our graphics core library, for all our displays. 
+We'll be adapting all the
+existing libaries to use this core to make updating, support 
+and upgrading easier!
+
+Adafruit invests time and resources providing this open source code, 
+please support Adafruit and open-source hardware by purchasing 
+products from Adafruit!
+
+Written by Limor Fried/Ladyada  for Adafruit Industries.  
+BSD license, check license.txt for more information
+All text above must be included in any redistribution
+****************************************/
+
+/*
+ *  Modified by Neal Horman 7/14/2012 for use in mbed
+ */
+
+#include "mbed.h"
+
+#include "Adafruit_GFX.h"
+#include "glcdfont.h"
+
+#if defined(GFX_WANT_ABSTRACTS)
+// draw a circle outline
+void Adafruit_GFX::drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color)
+{
+    int16_t f = 1 - r;
+    int16_t ddF_x = 1;
+    int16_t ddF_y = -2 * r;
+    int16_t x = 0;
+    int16_t y = r;
+    
+    drawPixel(x0, y0+r, color);
+    drawPixel(x0, y0-r, color);
+    drawPixel(x0+r, y0, color);
+    drawPixel(x0-r, y0, color);
+    
+    while (x<y)
+    {
+        if (f >= 0)
+        {
+            y--;
+            ddF_y += 2;
+            f += ddF_y;
+        }
+        x++;
+        ddF_x += 2;
+        f += ddF_x;
+        
+        drawPixel(x0 + x, y0 + y, color);
+        drawPixel(x0 - x, y0 + y, color);
+        drawPixel(x0 + x, y0 - y, color);
+        drawPixel(x0 - x, y0 - y, color);
+        drawPixel(x0 + y, y0 + x, color);
+        drawPixel(x0 - y, y0 + x, color);
+        drawPixel(x0 + y, y0 - x, color);
+        drawPixel(x0 - y, y0 - x, color);
+    }
+}
+
+void Adafruit_GFX::drawCircleHelper( int16_t x0, int16_t y0, int16_t r, uint8_t cornername, uint16_t color)
+{
+    int16_t f     = 1 - r;
+    int16_t ddF_x = 1;
+    int16_t ddF_y = -2 * r;
+    int16_t x     = 0;
+    int16_t y     = r;
+    
+    while (x<y)
+    {
+        if (f >= 0)
+        {
+            y--;
+            ddF_y += 2;
+            f += ddF_y;
+        }
+        x++;
+        ddF_x += 2;
+        f += ddF_x;
+        
+        if (cornername & 0x4)
+        {
+            drawPixel(x0 + x, y0 + y, color);
+            drawPixel(x0 + y, y0 + x, color);
+        } 
+
+        if (cornername & 0x2)
+        {
+            drawPixel(x0 + x, y0 - y, color);
+            drawPixel(x0 + y, y0 - x, color);
+        }
+
+        if (cornername & 0x8)
+        {
+            drawPixel(x0 - y, y0 + x, color);
+            drawPixel(x0 - x, y0 + y, color);
+        }
+        
+        if (cornername & 0x1)
+        {
+            drawPixel(x0 - y, y0 - x, color);
+            drawPixel(x0 - x, y0 - y, color);
+        }
+    }
+}
+
+void Adafruit_GFX::fillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color)
+{
+    drawFastVLine(x0, y0-r, 2*r+1, color);
+    fillCircleHelper(x0, y0, r, 3, 0, color);
+}
+
+// used to do circles and roundrects!
+void Adafruit_GFX::fillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, int16_t delta, uint16_t color)
+{
+    int16_t f     = 1 - r;
+    int16_t ddF_x = 1;
+    int16_t ddF_y = -2 * r;
+    int16_t x     = 0;
+    int16_t y     = r;
+    
+    while (x<y)
+    {
+        if (f >= 0)
+        {
+            y--;
+            ddF_y += 2;
+            f += ddF_y;
+        }
+        x++;
+        ddF_x += 2;
+        f += ddF_x;
+        
+        if (cornername & 0x1)
+        {
+            drawFastVLine(x0+x, y0-y, 2*y+1+delta, color);
+            drawFastVLine(x0+y, y0-x, 2*x+1+delta, color);
+        }
+
+        if (cornername & 0x2)
+        {
+            drawFastVLine(x0-x, y0-y, 2*y+1+delta, color);
+            drawFastVLine(x0-y, y0-x, 2*x+1+delta, color);
+        }
+    }
+}
+#endif
+
+#if defined(GFX_WANT_ABSTRACTS) || defined(GFX_SIZEABLE_TEXT)
+// bresenham's algorithm - thx wikpedia
+void Adafruit_GFX::drawLine(int16_t x0, int16_t y0,  int16_t x1, int16_t y1, uint16_t color)
+{
+    int16_t steep = abs(y1 - y0) > abs(x1 - x0);
+    
+    if (steep)
+    {
+        swap(x0, y0);
+        swap(x1, y1);
+    }
+    
+    if (x0 > x1)
+    {
+        swap(x0, x1);
+        swap(y0, y1);
+    }
+    
+    int16_t dx, dy;
+    dx = x1 - x0;
+    dy = abs(y1 - y0);
+    
+    int16_t err = dx / 2;
+    int16_t ystep;
+    
+    if (y0 < y1)
+        ystep = 1;
+    else
+        ystep = -1;
+    
+    for (; x0<=x1; x0++)
+    {
+        if (steep)
+            drawPixel(y0, x0, color);
+        else
+            drawPixel(x0, y0, color);
+
+        err -= dy;
+        if (err < 0)
+        {
+            y0 += ystep;
+            err += dx;
+        }
+    }
+}
+
+void Adafruit_GFX::drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color)
+{
+    // stupidest version - update in subclasses if desired!
+    drawLine(x, y, x, y+h-1, color);
+}
+
+void Adafruit_GFX::fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color)
+{
+    // stupidest version - update in subclasses if desired!
+    for (int16_t i=x; i<x+w; i++)
+        drawFastVLine(i, y, h, color); 
+}
+#endif
+
+#if defined(GFX_WANT_ABSTRACTS)
+// draw a rectangle
+void Adafruit_GFX::drawRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color)
+{
+    drawFastHLine(x, y, w, color);
+    drawFastHLine(x, y+h-1, w, color);
+    drawFastVLine(x, y, h, color);
+    drawFastVLine(x+w-1, y, h, color);
+}
+
+void Adafruit_GFX::drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color)
+{
+    // stupidest version - update in subclasses if desired!
+    drawLine(x, y, x+w-1, y, color);
+}
+
+void Adafruit_GFX::fillScreen(uint16_t color)
+{
+    fillRect(0, 0, _width, _height, color);
+}
+
+// draw a rounded rectangle!
+void Adafruit_GFX::drawRoundRect(int16_t x, int16_t y, int16_t w, int16_t h, int16_t r, uint16_t color)
+{
+    // smarter version
+    drawFastHLine(x+r  , y    , w-2*r, color); // Top
+    drawFastHLine(x+r  , y+h-1, w-2*r, color); // Bottom
+    drawFastVLine(  x    , y+r  , h-2*r, color); // Left
+    drawFastVLine(  x+w-1, y+r  , h-2*r, color); // Right
+    // draw four corners
+    drawCircleHelper(x+r    , y+r    , r, 1, color);
+    drawCircleHelper(x+w-r-1, y+r    , r, 2, color);
+    drawCircleHelper(x+w-r-1, y+h-r-1, r, 4, color);
+    drawCircleHelper(x+r    , y+h-r-1, r, 8, color);
+}
+
+// fill a rounded rectangle!
+void Adafruit_GFX::fillRoundRect(int16_t x, int16_t y, int16_t w, int16_t h, int16_t r, uint16_t color)
+{
+    // smarter version
+    fillRect(x+r, y, w-2*r, h, color);
+    
+    // draw four corners
+    fillCircleHelper(x+w-r-1, y+r, r, 1, h-2*r-1, color);
+    fillCircleHelper(x+r    , y+r, r, 2, h-2*r-1, color);
+}
+
+// draw a triangle!
+void Adafruit_GFX::drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color)
+{
+    drawLine(x0, y0, x1, y1, color);
+    drawLine(x1, y1, x2, y2, color);
+    drawLine(x2, y2, x0, y0, color);
+}
+
+// fill a triangle!
+void Adafruit_GFX::fillTriangle ( int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color)
+{
+    int16_t a, b, y, last;
+    
+    // Sort coordinates by Y order (y2 >= y1 >= y0)
+    if (y0 > y1)
+        swap(y0, y1); swap(x0, x1);
+
+    if (y1 > y2)
+        swap(y2, y1); swap(x2, x1);
+
+    if (y0 > y1)
+        swap(y0, y1); swap(x0, x1);
+
+    
+    if(y0 == y2)
+    { // Handle awkward all-on-same-line case as its own thing
+        a = b = x0;
+        if(x1 < a)
+            a = x1;
+        else if(x1 > b)
+            b = x1;
+            
+        if(x2 < a)
+            a = x2;
+        else if(x2 > b) b = x2;
+            drawFastHLine(a, y0, b-a+1, color);
+        return;
+    }
+
+    int16_t
+        dx01 = x1 - x0,
+        dy01 = y1 - y0,
+        dx02 = x2 - x0,
+        dy02 = y2 - y0,
+        dx12 = x2 - x1,
+        dy12 = y2 - y1,
+        sa   = 0,
+        sb   = 0;
+
+    // For upper part of triangle, find scanline crossings for segments
+    // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
+    // is included here (and second loop will be skipped, avoiding a /0
+    // error there), otherwise scanline y1 is skipped here and handled
+    // in the second loop...which also avoids a /0 error here if y0=y1
+    // (flat-topped triangle).
+    if(y1 == y2)
+        last = y1;   // Include y1 scanline
+    else
+        last = y1-1; // Skip it
+
+    for(y=y0; y<=last; y++)
+    {
+        a   = x0 + sa / dy01;
+        b   = x0 + sb / dy02;
+        sa += dx01;
+        sb += dx02;
+        /* longhand:
+        a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
+        b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
+        */
+        if(a > b)
+            swap(a,b);
+        drawFastHLine(a, y, b-a+1, color);
+    }
+
+    // For lower part of triangle, find scanline crossings for segments
+    // 0-2 and 1-2.  This loop is skipped if y1=y2.
+    sa = dx12 * (y - y1);
+    sb = dx02 * (y - y0);
+    for(; y<=y2; y++)
+    {
+        a   = x1 + sa / dy12;
+        b   = x0 + sb / dy02;
+        sa += dx12;
+        sb += dx02;
+        /* longhand:
+        a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
+        b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
+        */
+        if(a > b)
+            swap(a,b);
+        drawFastHLine(a, y, b-a+1, color);
+    }
+}
+
+void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, uint16_t color)
+{
+    for (int16_t j=0; j<h; j++)
+    {
+        for (int16_t i=0; i<w; i++ )
+        {
+            if (bitmap[i + (j/8)*w] & _BV(j%8))
+                drawPixel(x+i, y+j, color);
+        }
+    }
+}
+#endif
+
+size_t Adafruit_GFX::writeChar(uint8_t c)
+{
+    if (c == '\n')
+    {
+        cursor_y += textsize*8;
+        cursor_x = 0;
+    }
+    else if (c == '\r')
+        cursor_x = 0;
+    else
+    {
+        drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize);
+        cursor_x += textsize*6;
+        if (wrap && (cursor_x > (_width - textsize*6)))
+        {
+            cursor_y += textsize*8;
+            cursor_x = 0;
+        }
+    }
+    return 1;
+}
+
+// draw a character
+void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c, uint16_t color, uint16_t bg, uint8_t size)
+{
+    if(
+        (x >= _width) || // Clip right
+        (y >= _height) || // Clip bottom
+        ((x + 5 * size - 1) < 0) || // Clip left
+        ((y + 8 * size - 1) < 0) // Clip top
+        )
+    return;
+    
+    for (int8_t i=0; i<6; i++ )
+    {
+        uint8_t line = 0;
+
+        if (i == 5) 
+            line = 0x0;
+        else 
+            line = font[(c*5)+i];
+            
+        for (int8_t j = 0; j<8; j++)
+        {
+            if (line & 0x1)
+            {
+#if defined(GFX_WANT_ABSTRACTS) || defined(GFX_SIZEABLE_TEXT)
+                if (size == 1) // default size
+                    drawPixel(x+i, y+j, color);
+                else // big size
+                    fillRect(x+(i*size), y+(j*size), size, size, color);
+#else
+                drawPixel(x+i, y+j, color);
+#endif
+            }
+            else if (bg != color)
+            {
+#if defined(GFX_WANT_ABSTRACTS) || defined(GFX_SIZEABLE_TEXT)
+                if (size == 1) // default size
+                    drawPixel(x+i, y+j, bg);
+                else // big size
+                    fillRect(x+i*size, y+j*size, size, size, bg);
+#else
+                drawPixel(x+i, y+j, bg);
+#endif
+            }
+            line >>= 1;
+        }
+    }
+}
+
+void Adafruit_GFX::setRotation(uint8_t x)
+{
+    x %= 4;  // cant be higher than 3
+    rotation = x;
+    switch (x)
+    {
+        case 0:
+        case 2:
+            _width = _rawWidth;
+            _height = _rawHeight;
+            break;
+        case 1:
+        case 3:
+            _width = _rawHeight;
+            _height = _rawWidth;
+            break;
+    }
+}
+
+

diff -r 45c2b2d465c4 -r 3618abce1646 SSD1306_mini/Adafruit_GFX.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SSD1306_mini/Adafruit_GFX.h	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,185 @@
+/***********************************
+This is a our graphics core library, for all our displays. 
+We'll be adapting all the
+existing libaries to use this core to make updating, support 
+and upgrading easier!
+
+Adafruit invests time and resources providing this open source code, 
+please support Adafruit and open-source hardware by purchasing 
+products from Adafruit!
+
+Written by Limor Fried/Ladyada  for Adafruit Industries.  
+BSD license, check license.txt for more information
+All text above must be included in any redistribution
+****************************************/
+
+/*
+ *  Modified by Neal Horman 7/14/2012 for use in mbed
+ */
+
+#ifndef _ADAFRUIT_GFX_H_
+#define _ADAFRUIT_GFX_H_
+
+#include "Adafruit_GFX_Config.h"
+
+static inline void swap(int16_t &a, int16_t &b)
+{
+    int16_t t = a;
+    
+    a = b;
+    b = t;
+}
+
+#ifndef _BV
+#define _BV(bit) (1<<(bit))
+#endif
+
+#define BLACK 0
+#define WHITE 1
+
+/**
+ * This is a Text and Graphics element drawing class.
+ * These functions draw to the display buffer.
+ *
+ * Display drivers should be derived from here.
+ * The Display drivers push the display buffer to the
+ * hardware based on application control.
+ *
+ */
+class Adafruit_GFX : public Stream
+{
+ public:
+    Adafruit_GFX(int16_t w, int16_t h)
+        : _rawWidth(w)
+        , _rawHeight(h)
+        , _width(w)
+        , _height(h)
+        , cursor_x(0)
+        , cursor_y(0)
+        , textcolor(WHITE)
+        , textbgcolor(BLACK)
+        , textsize(1)
+        , rotation(0)
+        , wrap(true)
+        {};
+
+    /// Paint one BLACK or WHITE pixel in the display buffer
+    // this must be defined by the subclass
+    virtual void drawPixel(int16_t x, int16_t y, uint16_t color) = 0;
+    // this is optional
+    virtual void invertDisplay(bool i) {};
+    
+    // Stream implementation - provides printf() interface
+    // You would otherwise be forced to use writeChar()
+    virtual int _putc(int value) { return writeChar(value); };
+    virtual int _getc() { return -1; };
+
+#ifdef GFX_WANT_ABSTRACTS
+    // these are 'generic' drawing functions, so we can share them!
+    
+    /** Draw a Horizontal Line
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    virtual void drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color);
+    /** Draw a rectangle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    virtual void drawRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color);
+    /** Fill the entire display
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    virtual void fillScreen(uint16_t color);
+
+    /** Draw a circle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color);
+    void drawCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, uint16_t color);
+    
+    /** Draw and fill a circle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void fillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color);
+    void fillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, int16_t delta, uint16_t color);
+
+    /** Draw a triangle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color);
+    /** Draw and fill a triangle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color);
+    
+    /** Draw a rounded rectangle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void drawRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h, int16_t radius, uint16_t color);
+    /** Draw and fill a rounded rectangle
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void fillRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h, int16_t radius, uint16_t color);
+    /** Draw a bitmap
+     * @note GFX_WANT_ABSTRACTS must be defined in Adafruit_GFX_config.h
+     */
+    void drawBitmap(int16_t x, int16_t y, const uint8_t *bitmap, int16_t w, int16_t h, uint16_t color);
+#endif
+
+#if defined(GFX_WANT_ABSTRACTS) || defined(GFX_SIZEABLE_TEXT)
+    /** Draw a line
+     * @note GFX_WANT_ABSTRACTS or GFX_SIZEABLE_TEXT must be defined in Adafruit_GFX_config.h
+     */
+    virtual void drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t color);
+    /** Draw a vertical line
+     * @note GFX_WANT_ABSTRACTS or GFX_SIZEABLE_TEXT must be defined in Adafruit_GFX_config.h
+     */
+    virtual void drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color);
+    /** Draw and fill a rectangle
+     * @note GFX_WANT_ABSTRACTS or GFX_SIZEABLE_TEXT must be defined in Adafruit_GFX_config.h
+     */
+    virtual void fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color);
+#endif
+
+    /// Draw a text character at a specified pixel location
+    void drawChar(int16_t x, int16_t y, unsigned char c, uint16_t color, uint16_t bg, uint8_t size);
+    /// Draw a text character at the text cursor location
+    size_t writeChar(uint8_t);
+
+    /// Get the width of the display in pixels
+    inline int16_t width(void) { return _width; };
+    /// Get the height of the display in pixels
+    inline int16_t height(void) { return _height; };
+
+    /// Set the text cursor location, based on the size of the text
+    inline void setTextCursor(int16_t x, int16_t y) { cursor_x = x; cursor_y = y; };
+#if defined(GFX_WANT_ABSTRACTS) || defined(GFX_SIZEABLE_TEXT)
+    /** Set the size of the text to be drawn
+     * @note Make sure to enable either GFX_SIZEABLE_TEXT or GFX_WANT_ABSTRACTS
+     */
+    inline void setTextSize(uint8_t s) { textsize = (s > 0) ? s : 1; };
+#endif
+    /// Set the text foreground and background colors to be the same
+    inline void setTextColor(uint16_t c) { textcolor = c; textbgcolor = c; }
+    /// Set the text foreground and background colors independantly
+    inline void setTextColor(uint16_t c, uint16_t b) { textcolor = c; textbgcolor = b; };
+    /// Set text wraping mode true or false
+    inline void setTextWrap(bool w) { wrap = w; };
+
+    /// Set the display rotation, 1, 2, 3, or 4
+    void setRotation(uint8_t r);
+    /// Get the current rotation
+    inline uint8_t getRotation(void) { rotation %= 4; return rotation; };
+
+protected:
+    int16_t  _rawWidth, _rawHeight;   // this is the 'raw' display w/h - never changes
+    int16_t  _width, _height; // dependent on rotation
+    int16_t  cursor_x, cursor_y;
+    uint16_t textcolor, textbgcolor;
+    uint8_t  textsize;
+    uint8_t  rotation;
+    bool  wrap; // If set, 'wrap' text at right edge of display
+};
+
+#endif
+
+

diff -r 45c2b2d465c4 -r 3618abce1646 SSD1306_mini/Adafruit_GFX_Config.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SSD1306_mini/Adafruit_GFX_Config.h	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,15 @@
+#ifndef _ADAFRUIT_GFX_CONFIG_H_
+#define _ADAFRUIT_GFX_CONFIG_H_
+
+// Uncomment this to turn off the builtin splash
+// #define NO_SPLASH_ADAFRUIT
+
+// Uncomment this to enable all functionality
+#define GFX_WANT_ABSTRACTS
+
+// Uncomment this to enable only runtime font scaling, without all the rest of the Abstracts
+//#define GFX_SIZEABLE_TEXT
+
+
+#endif
+

diff -r 45c2b2d465c4 -r 3618abce1646 SSD1306_mini/SSD1306_mini.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SSD1306_mini/SSD1306_mini.cpp	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,135 @@
+/*********************************************************************
+This is a library for our Monochrome OLEDs based on SSD1306 drivers
+
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/category/63_98
+
+These displays use SPI to communicate, 4 or 5 pins are required to  
+interface
+
+Adafruit invests time and resources providing this open source code, 
+please support Adafruit and open-source hardware by purchasing 
+products from Adafruit!
+
+Written by Limor Fried/Ladyada  for Adafruit Industries.  
+BSD license, check license.txt for more information
+All text above, and the splash screen below must be included in any redistribution
+*********************************************************************/
+
+/*
+ *  Modified by Neal Horman 7/14/2012 for use in mbed
+ *  Modified by Jiri Stepanovsky 6/4/2018 for LPE compatibility
+ */
+
+#include "mbed.h"
+#include "SSD1306_mini.h"
+
+#define SSD1306_SETCONTRAST 0x81
+#define SSD1306_DISPLAYALLON_RESUME 0xA4
+#define SSD1306_DISPLAYALLON 0xA5
+#define SSD1306_NORMALDISPLAY 0xA6
+#define SSD1306_INVERTDISPLAY 0xA7
+#define SSD1306_DISPLAYOFF 0xAE
+#define SSD1306_DISPLAYON 0xAF
+#define SSD1306_SETDISPLAYOFFSET 0xD3
+#define SSD1306_SETCOMPINS 0xDA
+#define SSD1306_SETVCOMDETECT 0xDB
+#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
+#define SSD1306_SETPRECHARGE 0xD9
+#define SSD1306_SETMULTIPLEX 0xA8
+#define SSD1306_SETLOWCOLUMN 0x00
+#define SSD1306_SETHIGHCOLUMN 0x10
+#define SSD1306_SETSTARTLINE 0x40
+#define SSD1306_MEMORYMODE 0x20
+#define SSD1306_COMSCANINC 0xC0
+#define SSD1306_COMSCANDEC 0xC8
+#define SSD1306_SEGREMAP 0xA0
+#define SSD1306_CHARGEPUMP 0x8D
+
+void SSD1306_mini::begin(uint8_t vccstate)
+{
+    rst = 1;
+    // VDD (3.3V) goes high at start, lets just chill for a ms
+    wait_ms(1);
+    // bring reset low
+    rst = 0;
+    // wait 10ms
+    wait_ms(10);
+    // bring out of reset
+    rst = 1;
+    // turn on VCC (9V?)
+
+    command(SSD1306_DISPLAYOFF);
+    command(SSD1306_SETDISPLAYCLOCKDIV);
+    command(0x80);                                  // the suggested ratio 0x80
+
+    command(SSD1306_SETMULTIPLEX);
+    command(OLED_HEIGHT-1);
+
+    command(SSD1306_SETDISPLAYOFFSET);
+    command(0x0);                                   // no offset
+
+    command(SSD1306_SETSTARTLINE | 0x0);            // line #0
+
+    command(SSD1306_CHARGEPUMP);
+    command((vccstate == SSD1306_EXTERNALVCC) ? 0x10 : 0x14);
+
+    command(SSD1306_MEMORYMODE);
+    command(0x00);                                  // 0x0 act like ks0108
+
+    command(SSD1306_SEGREMAP | 0x1);
+
+    command(SSD1306_COMSCANDEC);
+
+    command(SSD1306_SETCOMPINS);
+    command(OLED_HEIGHT == 32 ? 0x02 : 0x12);        // TODO - calculate based on _rawHieght ?
+
+    command(SSD1306_SETCONTRAST);
+    command(OLED_HEIGHT == 32 ? 0x8F : ((vccstate == SSD1306_EXTERNALVCC) ? 0x9F : 0xCF) );
+
+    command(SSD1306_SETPRECHARGE);
+    command((vccstate == SSD1306_EXTERNALVCC) ? 0x22 : 0xF1);
+
+    command(SSD1306_SETVCOMDETECT);
+    command(0x40);
+
+    command(SSD1306_DISPLAYALLON_RESUME);
+
+    command(SSD1306_NORMALDISPLAY);
+    
+    command(SSD1306_DISPLAYON);
+}
+
+// Set a single pixel
+void SSD1306_mini::drawPixel(int16_t x, int16_t y, uint16_t color)
+{
+    if ((x < 0) || (x >= OLED_WIDTH) || (y < 0) || (y >= OLED_HEIGHT))
+        return;
+    // x is which column
+    if (color == WHITE) 
+        buffer[x+ (y/8)*OLED_WIDTH] |= 1<<(y%8);  
+    else // else black
+        buffer[x+ (y/8)*OLED_WIDTH] &= ~(1<<(y%8)); 
+}
+
+void SSD1306_mini::invertDisplay(bool i)
+{
+	command(i ? SSD1306_INVERTDISPLAY : SSD1306_NORMALDISPLAY);
+}
+
+// Send the display buffer out to the display
+void SSD1306_mini::display(void)
+{
+	command(SSD1306_SETLOWCOLUMN | 0x0);  // low col = 0
+	command(SSD1306_SETHIGHCOLUMN | 0x0);  // hi col = 0
+	command(SSD1306_SETSTARTLINE | 0x0); // line #0
+	sendDisplayBuffer();
+}
+
+// Clear the display buffer. Requires a display() call at some point afterwards
+void SSD1306_mini::clearDisplay(void)
+{
+	for(uint16_t i=0, q=OLED_BUFFER_SIZE; i<q; i++)
+		buffer[i] = 0;
+}
+

diff -r 45c2b2d465c4 -r 3618abce1646 SSD1306_mini/SSD1306_mini.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SSD1306_mini/SSD1306_mini.h	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,288 @@
+/*********************************************************************
+This is a library for our Monochrome OLEDs based on SSD1306 drivers
+
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/category/63_98
+
+These displays use SPI to communicate, 4 or 5 pins are required to  
+interface
+
+Adafruit invests time and resources providing this open source code, 
+please support Adafruit and open-source hardware by purchasing 
+products from Adafruit!
+
+Written by Limor Fried/Ladyada  for Adafruit Industries.  
+BSD license, check license.txt for more information
+All text above, and the splash screen must be included in any redistribution
+*********************************************************************/
+
+/*
+ *  Modified by Neal Horman 7/14/2012 for use in mbed
+ *  Modified by Jiri Stepanovsky 6/4/2018 for LPE compatibility
+ */
+
+#ifndef _SSD1306_MINI_H_
+#define _SSD1306_MINI_H_
+
+#include "mbed.h"
+#include "Adafruit_GFX.h"
+
+#define OLED_WIDTH 128
+#define OLED_HEIGHT 64
+#define OLED_BUFFER_SIZE ((OLED_WIDTH*OLED_HEIGHT)/8)
+
+// A DigitalOut sub-class that provides a constructed default state
+class DigitalOut2 : public DigitalOut
+{
+public:
+	DigitalOut2(PinName pin, bool active = false) : DigitalOut(pin) { write(active); };
+	DigitalOut2& operator= (int value) { write(value); return *this; };
+	DigitalOut2& operator= (DigitalOut2& rhs) { write(rhs.read()); return *this; };
+	operator int() { return read(); };
+};
+
+#define SSD1306_EXTERNALVCC 0x1
+#define SSD1306_SWITCHCAPVCC 0x2
+
+/** The pure base class for the SSD1306 display driver.
+ *
+ * You should derive from this for a new transport interface type,
+ * such as the SPI and I2C drivers.
+ */
+class SSD1306_mini : public Adafruit_GFX
+{
+public:
+	SSD1306_mini(PinName RST)
+		: Adafruit_GFX(OLED_WIDTH,OLED_HEIGHT)
+		, rst(RST,false)
+	{
+	};
+
+	void begin(uint8_t switchvcc = SSD1306_SWITCHCAPVCC);
+	
+	// These must be implemented in the derived transport driver
+	virtual void command(uint8_t c) = 0;
+	virtual void data(uint8_t c) = 0;
+	virtual void drawPixel(int16_t x, int16_t y, uint16_t color);
+
+	/// Clear the display buffer    
+	void clearDisplay(void);
+	virtual void invertDisplay(bool i);
+
+	/// Cause the display to be updated with the buffer content.
+	void display();
+    
+protected:
+	virtual void sendDisplayBuffer() = 0;
+	DigitalOut2 rst;
+
+	// the memory buffer for the LCD
+	uint8_t buffer[OLED_BUFFER_SIZE];
+};
+
+
+/** This is the SW SPI SSD1306 display driver transport class
+ *
+ */
+class SSD1306_mini_swspi : public SSD1306_mini
+{
+public:
+	/** Create a SSD1306 SW SPI transport display driver instance with the specified DC, RST, and CS pins, as well as the display dimentions
+	 *
+	 * Required parameters
+	 * @param D0 - (CLK) pin name
+	 * @param D1 - (MOSI) pin name
+	 * @param DC (Data/Command) pin name
+	 * @param RST (Reset) pin name
+	 * @param CS (Chip Select) pin name
+	 */
+	SSD1306_mini_swspi(PinName D0, PinName D1, PinName DC, PinName RST, PinName CS)
+	    : SSD1306_mini(RST)
+	    , cs(CS,true)
+	    , dc(DC,false)
+	    , clk(D0,true)
+	    , mosi(D1,false)
+	    {
+		    begin();
+		    display();
+	    };
+	    
+	void swspi_write(uint8_t v)
+	{
+		for (int bit = 7; bit >= 0; --bit) {
+        	mosi = ((v >> bit) & 0x01);
+        	clk = 0;
+        	clk = 1;
+        }
+	}
+
+	virtual void command(uint8_t c)
+	{
+	    cs = 1;
+	    dc = 0;
+	    cs = 0;
+	    swspi_write(c);
+	    cs = 1;
+	};
+
+	virtual void data(uint8_t c)
+	{
+	    cs = 1;
+	    dc = 1;
+	    cs = 0;
+	    swspi_write(c);
+	    cs = 1;
+	};
+
+protected:
+	virtual void sendDisplayBuffer()
+	{
+		cs = 1;
+		dc = 1;
+		cs = 0;
+
+		for(uint16_t i=0, q=OLED_BUFFER_SIZE; i<q; i++)
+			swspi_write(buffer[i]);
+
+		if(OLED_HEIGHT == 32)
+		{
+			for(uint16_t i=0, q=OLED_BUFFER_SIZE; i<q; i++)
+				swspi_write(0);
+		}
+
+		cs = 1;
+	};
+
+	DigitalOut2 cs, dc, clk, mosi;
+};
+
+
+/** This is the SPI SSD1306 display driver transport class
+ *
+ */
+class SSD1306_mini_spi : public SSD1306_mini
+{
+public:
+	/** Create a SSD1306 SPI transport display driver instance with the specified DC, RST, and CS pins, as well as the display dimentions
+	 *
+	 * Required parameters
+	 * @param spi - a reference to an initialized SPI object
+	 * @param DC (Data/Command) pin name
+	 * @param RST (Reset) pin name
+	 * @param CS (Chip Select) pin name
+	 */
+	SSD1306_mini_spi(SPI &spi, PinName DC, PinName RST, PinName CS)
+	    : SSD1306_mini(RST)
+	    , cs(CS,true)
+	    , dc(DC,false)
+	    , mspi(spi)
+	    {
+		    begin();
+		    display();
+	    };
+
+	virtual void command(uint8_t c)
+	{
+	    cs = 1;
+	    dc = 0;
+	    cs = 0;
+	    mspi.write(c);
+	    cs = 1;
+	};
+
+	virtual void data(uint8_t c)
+	{
+	    cs = 1;
+	    dc = 1;
+	    cs = 0;
+	    mspi.write(c);
+	    cs = 1;
+	};
+
+protected:
+	virtual void sendDisplayBuffer()
+	{
+		cs = 1;
+		dc = 1;
+		cs = 0;
+
+		for(uint16_t i=0, q=OLED_BUFFER_SIZE; i<q; i++)
+			mspi.write(buffer[i]);
+
+		if(OLED_HEIGHT == 32)
+		{
+			for(uint16_t i=0, q=OLED_BUFFER_SIZE; i<q; i++)
+				mspi.write(0);
+		}
+
+		cs = 1;
+	};
+
+	DigitalOut2 cs, dc;
+	SPI &mspi;
+};
+
+
+/** This is the I2C SSD1306 display driver transport class
+ *
+ */
+class SSD1306_mini_i2c : public SSD1306_mini
+{
+public:
+	#define SSD_I2C_ADDRESS     0x78
+	/** Create a SSD1306 I2C transport display driver instance with the specified RST pin name, the I2C address, as well as the display dimensions
+	 *
+	 * Required parameters
+	 * @param i2c - A reference to an initialized I2C object
+	 * @param RST - The Reset pin name
+	 *
+	 * Optional parameters
+	 * @param i2cAddress - The i2c address of the display
+	 */
+	SSD1306_mini_i2c(I2C &i2c, PinName RST, uint8_t i2cAddress = SSD_I2C_ADDRESS)
+	    : SSD1306_mini(RST)
+	    , mi2c(i2c)
+	    , mi2cAddress(i2cAddress)
+	    {
+		    begin();
+		    display();
+	    };
+
+	virtual void command(uint8_t c)
+	{
+		char buff[2];
+		buff[0] = 0; // Command Mode
+		buff[1] = c;
+		mi2c.write(mi2cAddress, buff, sizeof(buff));
+	}
+
+	virtual void data(uint8_t c)
+	{
+		char buff[2];
+		buff[0] = 0x40; // Data Mode
+		buff[1] = c;
+		mi2c.write(mi2cAddress, buff, sizeof(buff));
+	};
+
+protected:
+	virtual void sendDisplayBuffer()
+	{
+		char buff[17];
+		buff[0] = 0x40; // Data Mode
+
+		// send display buffer in 16 byte chunks
+		for(uint16_t i=0, q=OLED_BUFFER_SIZE; i<q; i+=16 ) 
+		{	uint8_t x ;
+
+			// TODO - this will segfault if buffer.size() % 16 != 0
+			for(x=1; x<sizeof(buff); x++) 
+				buff[x] = buffer[i+x-1];
+			mi2c.write(mi2cAddress, buff, sizeof(buff));
+		}
+	};
+
+	I2C &mi2c;
+	uint8_t mi2cAddress;
+};
+
+#endif

diff -r 45c2b2d465c4 -r 3618abce1646 SSD1306_mini/glcdfont.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SSD1306_mini/glcdfont.h	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,287 @@
+/*********************************************************************
+This is a library for our Monochrome OLEDs based on SSD1306 drivers
+
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/category/63_98
+
+These displays use SPI to communicate, 4 or 5 pins are required to  
+interface
+
+Adafruit invests time and resources providing this open source code, 
+please support Adafruit and open-source hardware by purchasing 
+products from Adafruit!
+
+Written by Limor Fried/Ladyada  for Adafruit Industries.  
+BSD license, check license.txt for more information
+All text above, and the splash screen must be included in any redistribution
+*********************************************************************/
+
+/*
+ *  Modified by Neal Horman 7/14/2012 for use in LPC1768
+ */
+ 
+#ifndef FONT5X7_H
+#define FONT5X7_H
+
+// standard ascii 5x7 font
+
+static unsigned char  font[] = {
+    0x00, 0x00, 0x00, 0x00, 0x00,   
+    0x3E, 0x5B, 0x4F, 0x5B, 0x3E,     
+    0x3E, 0x6B, 0x4F, 0x6B, 0x3E,     
+    0x1C, 0x3E, 0x7C, 0x3E, 0x1C, 
+    0x18, 0x3C, 0x7E, 0x3C, 0x18, 
+    0x1C, 0x57, 0x7D, 0x57, 0x1C, 
+    0x1C, 0x5E, 0x7F, 0x5E, 0x1C, 
+    0x00, 0x18, 0x3C, 0x18, 0x00, 
+    0xFF, 0xE7, 0xC3, 0xE7, 0xFF, 
+    0x00, 0x18, 0x24, 0x18, 0x00, 
+    0xFF, 0xE7, 0xDB, 0xE7, 0xFF, 
+    0x30, 0x48, 0x3A, 0x06, 0x0E, 
+    0x26, 0x29, 0x79, 0x29, 0x26, 
+    0x40, 0x7F, 0x05, 0x05, 0x07, 
+    0x40, 0x7F, 0x05, 0x25, 0x3F, 
+    0x5A, 0x3C, 0xE7, 0x3C, 0x5A, 
+    0x7F, 0x3E, 0x1C, 0x1C, 0x08, 
+    0x08, 0x1C, 0x1C, 0x3E, 0x7F, 
+    0x14, 0x22, 0x7F, 0x22, 0x14, 
+    0x5F, 0x5F, 0x00, 0x5F, 0x5F, 
+    0x06, 0x09, 0x7F, 0x01, 0x7F, 
+    0x00, 0x66, 0x89, 0x95, 0x6A, 
+    0x60, 0x60, 0x60, 0x60, 0x60, 
+    0x94, 0xA2, 0xFF, 0xA2, 0x94, 
+    0x08, 0x04, 0x7E, 0x04, 0x08, 
+    0x10, 0x20, 0x7E, 0x20, 0x10, 
+    0x08, 0x08, 0x2A, 0x1C, 0x08, 
+    0x08, 0x1C, 0x2A, 0x08, 0x08, 
+    0x1E, 0x10, 0x10, 0x10, 0x10, 
+    0x0C, 0x1E, 0x0C, 0x1E, 0x0C, 
+    0x30, 0x38, 0x3E, 0x38, 0x30, 
+    0x06, 0x0E, 0x3E, 0x0E, 0x06, 
+    0x00, 0x00, 0x00, 0x00, 0x00, 
+    0x00, 0x00, 0x5F, 0x00, 0x00, 
+    0x00, 0x07, 0x00, 0x07, 0x00, 
+    0x14, 0x7F, 0x14, 0x7F, 0x14, 
+    0x24, 0x2A, 0x7F, 0x2A, 0x12, 
+    0x23, 0x13, 0x08, 0x64, 0x62, 
+    0x36, 0x49, 0x56, 0x20, 0x50, 
+    0x00, 0x08, 0x07, 0x03, 0x00, 
+    0x00, 0x1C, 0x22, 0x41, 0x00, 
+    0x00, 0x41, 0x22, 0x1C, 0x00, 
+    0x2A, 0x1C, 0x7F, 0x1C, 0x2A, 
+    0x08, 0x08, 0x3E, 0x08, 0x08, 
+    0x00, 0x80, 0x70, 0x30, 0x00, 
+    0x08, 0x08, 0x08, 0x08, 0x08, 
+    0x00, 0x00, 0x60, 0x60, 0x00, 
+    0x20, 0x10, 0x08, 0x04, 0x02, 
+    0x3E, 0x51, 0x49, 0x45, 0x3E, 
+    0x00, 0x42, 0x7F, 0x40, 0x00, 
+    0x72, 0x49, 0x49, 0x49, 0x46, 
+    0x21, 0x41, 0x49, 0x4D, 0x33, 
+    0x18, 0x14, 0x12, 0x7F, 0x10, 
+    0x27, 0x45, 0x45, 0x45, 0x39, 
+    0x3C, 0x4A, 0x49, 0x49, 0x31, 
+    0x41, 0x21, 0x11, 0x09, 0x07, 
+    0x36, 0x49, 0x49, 0x49, 0x36, 
+    0x46, 0x49, 0x49, 0x29, 0x1E, 
+    0x00, 0x00, 0x14, 0x00, 0x00, 
+    0x00, 0x40, 0x34, 0x00, 0x00, 
+    0x00, 0x08, 0x14, 0x22, 0x41, 
+    0x14, 0x14, 0x14, 0x14, 0x14, 
+    0x00, 0x41, 0x22, 0x14, 0x08, 
+    0x02, 0x01, 0x59, 0x09, 0x06, 
+    0x3E, 0x41, 0x5D, 0x59, 0x4E, 
+    0x7C, 0x12, 0x11, 0x12, 0x7C, 
+    0x7F, 0x49, 0x49, 0x49, 0x36, 
+    0x3E, 0x41, 0x41, 0x41, 0x22, 
+    0x7F, 0x41, 0x41, 0x41, 0x3E, 
+    0x7F, 0x49, 0x49, 0x49, 0x41, 
+    0x7F, 0x09, 0x09, 0x09, 0x01, 
+    0x3E, 0x41, 0x41, 0x51, 0x73, 
+    0x7F, 0x08, 0x08, 0x08, 0x7F, 
+    0x00, 0x41, 0x7F, 0x41, 0x00, 
+    0x20, 0x40, 0x41, 0x3F, 0x01, 
+    0x7F, 0x08, 0x14, 0x22, 0x41, 
+    0x7F, 0x40, 0x40, 0x40, 0x40, 
+    0x7F, 0x02, 0x1C, 0x02, 0x7F, 
+    0x7F, 0x04, 0x08, 0x10, 0x7F, 
+    0x3E, 0x41, 0x41, 0x41, 0x3E, 
+    0x7F, 0x09, 0x09, 0x09, 0x06, 
+    0x3E, 0x41, 0x51, 0x21, 0x5E, 
+    0x7F, 0x09, 0x19, 0x29, 0x46, 
+    0x26, 0x49, 0x49, 0x49, 0x32, 
+    0x03, 0x01, 0x7F, 0x01, 0x03, 
+    0x3F, 0x40, 0x40, 0x40, 0x3F, 
+    0x1F, 0x20, 0x40, 0x20, 0x1F, 
+    0x3F, 0x40, 0x38, 0x40, 0x3F, 
+    0x63, 0x14, 0x08, 0x14, 0x63, 
+    0x03, 0x04, 0x78, 0x04, 0x03, 
+    0x61, 0x59, 0x49, 0x4D, 0x43, 
+    0x00, 0x7F, 0x41, 0x41, 0x41, 
+    0x02, 0x04, 0x08, 0x10, 0x20, 
+    0x00, 0x41, 0x41, 0x41, 0x7F, 
+    0x04, 0x02, 0x01, 0x02, 0x04, 
+    0x40, 0x40, 0x40, 0x40, 0x40, 
+    0x00, 0x03, 0x07, 0x08, 0x00, 
+    0x20, 0x54, 0x54, 0x78, 0x40, 
+    0x7F, 0x28, 0x44, 0x44, 0x38, 
+    0x38, 0x44, 0x44, 0x44, 0x28, 
+    0x38, 0x44, 0x44, 0x28, 0x7F, 
+    0x38, 0x54, 0x54, 0x54, 0x18, 
+    0x00, 0x08, 0x7E, 0x09, 0x02, 
+    0x18, 0xA4, 0xA4, 0x9C, 0x78, 
+    0x7F, 0x08, 0x04, 0x04, 0x78, 
+    0x00, 0x44, 0x7D, 0x40, 0x00, 
+    0x20, 0x40, 0x40, 0x3D, 0x00, 
+    0x7F, 0x10, 0x28, 0x44, 0x00, 
+    0x00, 0x41, 0x7F, 0x40, 0x00, 
+    0x7C, 0x04, 0x78, 0x04, 0x78, 
+    0x7C, 0x08, 0x04, 0x04, 0x78, 
+    0x38, 0x44, 0x44, 0x44, 0x38, 
+    0xFC, 0x18, 0x24, 0x24, 0x18, 
+    0x18, 0x24, 0x24, 0x18, 0xFC, 
+    0x7C, 0x08, 0x04, 0x04, 0x08, 
+    0x48, 0x54, 0x54, 0x54, 0x24, 
+    0x04, 0x04, 0x3F, 0x44, 0x24, 
+    0x3C, 0x40, 0x40, 0x20, 0x7C, 
+    0x1C, 0x20, 0x40, 0x20, 0x1C, 
+    0x3C, 0x40, 0x30, 0x40, 0x3C, 
+    0x44, 0x28, 0x10, 0x28, 0x44, 
+    0x4C, 0x90, 0x90, 0x90, 0x7C, 
+    0x44, 0x64, 0x54, 0x4C, 0x44, 
+    0x00, 0x08, 0x36, 0x41, 0x00, 
+    0x00, 0x00, 0x77, 0x00, 0x00, 
+    0x00, 0x41, 0x36, 0x08, 0x00, 
+    0x02, 0x01, 0x02, 0x04, 0x02, 
+    0x3C, 0x26, 0x23, 0x26, 0x3C, 
+    0x1E, 0xA1, 0xA1, 0x61, 0x12, 
+    0x3A, 0x40, 0x40, 0x20, 0x7A, 
+    0x38, 0x54, 0x54, 0x55, 0x59, 
+    0x21, 0x55, 0x55, 0x79, 0x41, 
+    0x21, 0x54, 0x54, 0x78, 0x41, 
+    0x21, 0x55, 0x54, 0x78, 0x40, 
+    0x20, 0x54, 0x55, 0x79, 0x40, 
+    0x0C, 0x1E, 0x52, 0x72, 0x12, 
+    0x39, 0x55, 0x55, 0x55, 0x59, 
+    0x39, 0x54, 0x54, 0x54, 0x59, 
+    0x39, 0x55, 0x54, 0x54, 0x58, 
+    0x00, 0x00, 0x45, 0x7C, 0x41, 
+    0x00, 0x02, 0x45, 0x7D, 0x42, 
+    0x00, 0x01, 0x45, 0x7C, 0x40, 
+    0xF0, 0x29, 0x24, 0x29, 0xF0, 
+    0xF0, 0x28, 0x25, 0x28, 0xF0, 
+    0x7C, 0x54, 0x55, 0x45, 0x00, 
+    0x20, 0x54, 0x54, 0x7C, 0x54, 
+    0x7C, 0x0A, 0x09, 0x7F, 0x49, 
+    0x32, 0x49, 0x49, 0x49, 0x32, 
+    0x32, 0x48, 0x48, 0x48, 0x32, 
+    0x32, 0x4A, 0x48, 0x48, 0x30, 
+    0x3A, 0x41, 0x41, 0x21, 0x7A, 
+    0x3A, 0x42, 0x40, 0x20, 0x78, 
+    0x00, 0x9D, 0xA0, 0xA0, 0x7D, 
+    0x39, 0x44, 0x44, 0x44, 0x39, 
+    0x3D, 0x40, 0x40, 0x40, 0x3D, 
+    0x3C, 0x24, 0xFF, 0x24, 0x24, 
+    0x48, 0x7E, 0x49, 0x43, 0x66, 
+    0x2B, 0x2F, 0xFC, 0x2F, 0x2B, 
+    0xFF, 0x09, 0x29, 0xF6, 0x20, 
+    0xC0, 0x88, 0x7E, 0x09, 0x03, 
+    0x20, 0x54, 0x54, 0x79, 0x41, 
+    0x00, 0x00, 0x44, 0x7D, 0x41, 
+    0x30, 0x48, 0x48, 0x4A, 0x32, 
+    0x38, 0x40, 0x40, 0x22, 0x7A, 
+    0x00, 0x7A, 0x0A, 0x0A, 0x72, 
+    0x7D, 0x0D, 0x19, 0x31, 0x7D, 
+    0x26, 0x29, 0x29, 0x2F, 0x28, 
+    0x26, 0x29, 0x29, 0x29, 0x26, 
+    0x30, 0x48, 0x4D, 0x40, 0x20, 
+    0x38, 0x08, 0x08, 0x08, 0x08, 
+    0x08, 0x08, 0x08, 0x08, 0x38, 
+    0x2F, 0x10, 0xC8, 0xAC, 0xBA, 
+    0x2F, 0x10, 0x28, 0x34, 0xFA, 
+    0x00, 0x00, 0x7B, 0x00, 0x00, 
+    0x08, 0x14, 0x2A, 0x14, 0x22, 
+    0x22, 0x14, 0x2A, 0x14, 0x08, 
+    0xAA, 0x00, 0x55, 0x00, 0xAA, 
+    0xAA, 0x55, 0xAA, 0x55, 0xAA, 
+    0x00, 0x00, 0x00, 0xFF, 0x00, 
+    0x10, 0x10, 0x10, 0xFF, 0x00, 
+    0x14, 0x14, 0x14, 0xFF, 0x00, 
+    0x10, 0x10, 0xFF, 0x00, 0xFF, 
+    0x10, 0x10, 0xF0, 0x10, 0xF0, 
+    0x14, 0x14, 0x14, 0xFC, 0x00, 
+    0x14, 0x14, 0xF7, 0x00, 0xFF, 
+    0x00, 0x00, 0xFF, 0x00, 0xFF, 
+    0x14, 0x14, 0xF4, 0x04, 0xFC, 
+    0x14, 0x14, 0x17, 0x10, 0x1F, 
+    0x10, 0x10, 0x1F, 0x10, 0x1F, 
+    0x14, 0x14, 0x14, 0x1F, 0x00, 
+    0x10, 0x10, 0x10, 0xF0, 0x00, 
+    0x00, 0x00, 0x00, 0x1F, 0x10, 
+    0x10, 0x10, 0x10, 0x1F, 0x10, 
+    0x10, 0x10, 0x10, 0xF0, 0x10, 
+    0x00, 0x00, 0x00, 0xFF, 0x10, 
+    0x10, 0x10, 0x10, 0x10, 0x10, 
+    0x10, 0x10, 0x10, 0xFF, 0x10, 
+    0x00, 0x00, 0x00, 0xFF, 0x14, 
+    0x00, 0x00, 0xFF, 0x00, 0xFF, 
+    0x00, 0x00, 0x1F, 0x10, 0x17, 
+    0x00, 0x00, 0xFC, 0x04, 0xF4, 
+    0x14, 0x14, 0x17, 0x10, 0x17, 
+    0x14, 0x14, 0xF4, 0x04, 0xF4, 
+    0x00, 0x00, 0xFF, 0x00, 0xF7, 
+    0x14, 0x14, 0x14, 0x14, 0x14, 
+    0x14, 0x14, 0xF7, 0x00, 0xF7, 
+    0x14, 0x14, 0x14, 0x17, 0x14, 
+    0x10, 0x10, 0x1F, 0x10, 0x1F, 
+    0x14, 0x14, 0x14, 0xF4, 0x14, 
+    0x10, 0x10, 0xF0, 0x10, 0xF0, 
+    0x00, 0x00, 0x1F, 0x10, 0x1F, 
+    0x00, 0x00, 0x00, 0x1F, 0x14, 
+    0x00, 0x00, 0x00, 0xFC, 0x14, 
+    0x00, 0x00, 0xF0, 0x10, 0xF0, 
+    0x10, 0x10, 0xFF, 0x10, 0xFF, 
+    0x14, 0x14, 0x14, 0xFF, 0x14, 
+    0x10, 0x10, 0x10, 0x1F, 0x00, 
+    0x00, 0x00, 0x00, 0xF0, 0x10, 
+    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+    0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 
+    0xFF, 0xFF, 0xFF, 0x00, 0x00, 
+    0x00, 0x00, 0x00, 0xFF, 0xFF, 
+    0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 
+    0x38, 0x44, 0x44, 0x38, 0x44, 
+    0x7C, 0x2A, 0x2A, 0x3E, 0x14, 
+    0x7E, 0x02, 0x02, 0x06, 0x06, 
+    0x02, 0x7E, 0x02, 0x7E, 0x02, 
+    0x63, 0x55, 0x49, 0x41, 0x63, 
+    0x38, 0x44, 0x44, 0x3C, 0x04, 
+    0x40, 0x7E, 0x20, 0x1E, 0x20, 
+    0x06, 0x02, 0x7E, 0x02, 0x02, 
+    0x99, 0xA5, 0xE7, 0xA5, 0x99, 
+    0x1C, 0x2A, 0x49, 0x2A, 0x1C, 
+    0x4C, 0x72, 0x01, 0x72, 0x4C, 
+    0x30, 0x4A, 0x4D, 0x4D, 0x30, 
+    0x30, 0x48, 0x78, 0x48, 0x30, 
+    0xBC, 0x62, 0x5A, 0x46, 0x3D, 
+    0x3E, 0x49, 0x49, 0x49, 0x00, 
+    0x7E, 0x01, 0x01, 0x01, 0x7E, 
+    0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 
+    0x44, 0x44, 0x5F, 0x44, 0x44, 
+    0x40, 0x51, 0x4A, 0x44, 0x40, 
+    0x40, 0x44, 0x4A, 0x51, 0x40, 
+    0x00, 0x00, 0xFF, 0x01, 0x03, 
+    0xE0, 0x80, 0xFF, 0x00, 0x00, 
+    0x08, 0x08, 0x6B, 0x6B, 0x08,
+    0x36, 0x12, 0x36, 0x24, 0x36, 
+    0x06, 0x0F, 0x09, 0x0F, 0x06, 
+    0x00, 0x00, 0x18, 0x18, 0x00, 
+    0x00, 0x00, 0x10, 0x10, 0x00, 
+    0x30, 0x40, 0xFF, 0x01, 0x01, 
+    0x00, 0x1F, 0x01, 0x01, 0x1E, 
+    0x00, 0x19, 0x1D, 0x17, 0x12, 
+    0x00, 0x3C, 0x3C, 0x3C, 0x3C, 
+    0x00, 0x00, 0x00, 0x00, 0x00,
+};
+#endif
+
+

diff -r 45c2b2d465c4 -r 3618abce1646 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 21 13:19:41 2022 +0000
@@ -0,0 +1,85 @@
+#include "mbed.h"
+#include "SSD1306_mini.h"
+#include "MMC5883L.h"
+
+SSD1306_mini_swspi gOled1(PA_5, PA_7,PA_3,PA_4,PA_2);
+// SSD1306_mini_swspi(PinName D0, PinName D1, PinName DC, PinName RST, PinName CS)
+
+MMC5883L compass(PF_0, PF_1);
+
+int main() {
+    
+    wait(0.1f);
+    compass.init();
+    wait(0.1f);
+    int16_t data[3] = {0,0,0};
+    
+    while(1) {
+        double heading = compass.getHeadingXY(data);
+        double Btot = sqrt(pow((double)data[0],2) + pow((double)data[1],2) + pow((double)data[2],2)); 
+
+        
+        int16_t compassRadius  = gOled1.height()/2-1;
+        int16_t compassCenterX = gOled1.width()/2;
+        int16_t compassCenterY = gOled1.height()/2;
+
+        gOled1.clearDisplay();
+        char str[20];
+        
+        int32_t xyz_nt[3];
+        compass.getXYZ_OffsetRemoved_nT(xyz_nt);
+        
+        gOled1.setTextCursor(0, 5);
+
+        gOled1.printf("X: %i", xyz_nt[0]);;
+        
+        gOled1.setTextCursor(0, 15);
+
+        gOled1.printf("Y: %i", xyz_nt[1]);
+        
+        gOled1.setTextCursor(0, 25);
+
+        gOled1.printf("Z: %i", xyz_nt[2]);
+        
+
+        /*
+        gOled1.printf("MMC5883        kompas\r\n");
+        
+
+            gOled1.drawCircle(compassCenterX, compassCenterY, compassRadius, WHITE );
+            gOled1.fillCircle(compassCenterX, compassCenterY, 2, WHITE );
+
+            gOled1.drawLine(compassCenterX, compassCenterY+18, compassCenterX, compassCenterY+20, WHITE);
+            gOled1.drawLine(compassCenterX, compassCenterY-18, compassCenterX, compassCenterY-20, WHITE);
+            gOled1.drawLine(compassCenterX+18, compassCenterY, compassCenterX+20, compassCenterY, WHITE);
+            gOled1.drawLine(compassCenterX-18, compassCenterY, compassCenterX-20, compassCenterY, WHITE);
+
+            double x_vect = cos(heading)* compassRadius;
+            double y_vect = sin(heading)* compassRadius;
+
+            gOled1.drawLine(compassCenterX, compassCenterY, (int16_t) (compassCenterX + x_vect), (int16_t) (compassCenterY - y_vect), WHITE);
+            
+            gOled1.setTextCursor(compassCenterX-2, compassCenterY-compassRadius);
+            gOled1.printf("S");
+            gOled1.setTextCursor(compassCenterX-2, compassCenterY+compassRadius-7);
+            gOled1.printf("J");
+            gOled1.setTextCursor(compassCenterX-compassRadius-2, compassCenterY-3);
+            gOled1.printf("Z");
+            gOled1.setTextCursor(compassCenterX+compassRadius-2, compassCenterY-3);
+            gOled1.printf("V");
+
+            gOled1.setTextCursor(0, gOled1.height()-18);
+            gOled1.printf("FEL");
+            gOled1.setTextCursor(17*6, gOled1.height()-18);
+            gOled1.printf("CVUT");
+
+            gOled1.setTextCursor(0, gOled1.height()-8);
+            gOled1.printf("LPE");
+            gOled1.setTextCursor(17*6, gOled1.height()-8);
+            gOled1.printf("2021");
+ //       }        */
+        gOled1.display();
+        wait(0.05f);
+    }
+}
+

diff -r 45c2b2d465c4 -r 3618abce1646 main_mouse.cpp
--- a/main_mouse.cpp	Mon Jan 18 10:51:25 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,24 +0,0 @@
-#if 1
-// https://developer.mbed.org/handbook/USBMouse
-#include "mbed.h"
-#include "USBMouse.h"
-
-int main() {
-    USBMouse mouse;
-
-    int16_t x = 0;
-    int16_t y = 0;
-    int32_t radius = 10;
-    int32_t angle = 0;
-
-    while (1) {
-        x = cos((double)angle*3.14/180.0)*radius;
-        y = sin((double)angle*3.14/180.0)*radius;
-        
-        mouse.move(x, y);
-        angle += 3;
-        wait(0.005);
-    }
-}
-
-#endif

diff -r 45c2b2d465c4 -r 3618abce1646 main_serial.cpp
--- a/main_serial.cpp	Mon Jan 18 10:51:25 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,15 +0,0 @@
-#if 0
-// https://developer.mbed.org/handbook/USBSerial
-#include "mbed.h"
-#include "USBSerial.h"
-
-int main() {
-    USBSerial serial;
-
-    for(int n = 0;; n++) {
-        serial.printf("I am a virtual serial port\r\n");
-        serial.printf("%d\r\n", n);
-        wait_ms(1000);
-    }
-}
-#endif