Sérgio Natan
teste
Revision 0:cf17b1f16335, committed 2017-10-24
- Comitter:
- sergionatan
- Date:
- Tue Oct 24 20:12:54 2017 +0000
- Commit message:
- Initial commit
Changed in this revision
diff -r 000000000000 -r cf17b1f16335 BurstSPI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BurstSPI.lib Tue Oct 24 20:12:54 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Sissors/code/BurstSPI/#bc069279eb37
diff -r 000000000000 -r cf17b1f16335 Fonts.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Fonts.lib Tue Oct 24 20:12:54 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/star297/code/Fonts/#b7905075b31a
diff -r 000000000000 -r cf17b1f16335 GraphicsDisplay.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GraphicsDisplay.cpp Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,173 @@
+ // ST7735 8 Bit SPI Library
+
+#include "GraphicsDisplay.h"
+
+const unsigned char FONT8x8[97][8] = {
+0x08,0x08,0x08,0x00,0x00,0x00,0x00,0x00, // columns, rows, num_bytes_per_char
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // space 0x20
+0x30,0x78,0x78,0x30,0x30,0x00,0x30,0x00, // !
+0x6C,0x6C,0x6C,0x00,0x00,0x00,0x00,0x00, // "
+0x6C,0x6C,0xFE,0x6C,0xFE,0x6C,0x6C,0x00, // #
+0x18,0x3E,0x60,0x3C,0x06,0x7C,0x18,0x00, // $
+0x00,0x63,0x66,0x0C,0x18,0x33,0x63,0x00, // %
+0x1C,0x36,0x1C,0x3B,0x6E,0x66,0x3B,0x00, // &
+0x30,0x30,0x60,0x00,0x00,0x00,0x00,0x00, // '
+0x0C,0x18,0x30,0x30,0x30,0x18,0x0C,0x00, // (
+0x30,0x18,0x0C,0x0C,0x0C,0x18,0x30,0x00, // )
+0x00,0x66,0x3C,0xFF,0x3C,0x66,0x00,0x00, // *
+0x00,0x30,0x30,0xFC,0x30,0x30,0x00,0x00, // +
+0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x30, // ,
+0x00,0x00,0x00,0x7E,0x00,0x00,0x00,0x00, // -
+0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00, // .
+0x03,0x06,0x0C,0x18,0x30,0x60,0x40,0x00, // / (forward slash)
+0x3E,0x63,0x63,0x6B,0x63,0x63,0x3E,0x00, // 0 0x30
+0x18,0x38,0x58,0x18,0x18,0x18,0x7E,0x00, // 1
+0x3C,0x66,0x06,0x1C,0x30,0x66,0x7E,0x00, // 2
+0x3C,0x66,0x06,0x1C,0x06,0x66,0x3C,0x00, // 3
+0x0E,0x1E,0x36,0x66,0x7F,0x06,0x0F,0x00, // 4
+0x7E,0x60,0x7C,0x06,0x06,0x66,0x3C,0x00, // 5
+0x1C,0x30,0x60,0x7C,0x66,0x66,0x3C,0x00, // 6
+0x7E,0x66,0x06,0x0C,0x18,0x18,0x18,0x00, // 7
+0x3C,0x66,0x66,0x3C,0x66,0x66,0x3C,0x00, // 8
+0x3C,0x66,0x66,0x3E,0x06,0x0C,0x38,0x00, // 9
+0x00,0x18,0x18,0x00,0x00,0x18,0x18,0x00, // :
+0x00,0x18,0x18,0x00,0x00,0x18,0x18,0x30, // ;
+0x0C,0x18,0x30,0x60,0x30,0x18,0x0C,0x00, // <
+0x00,0x00,0x7E,0x00,0x00,0x7E,0x00,0x00, // =
+0x30,0x18,0x0C,0x06,0x0C,0x18,0x30,0x00, // >
+0x3C,0x66,0x06,0x0C,0x18,0x00,0x18,0x00, // ?
+0x3E,0x63,0x6F,0x69,0x6F,0x60,0x3E,0x00, // @ 0x40
+0x18,0x3C,0x66,0x66,0x7E,0x66,0x66,0x00, // A
+0x7E,0x33,0x33,0x3E,0x33,0x33,0x7E,0x00, // B
+0x1E,0x33,0x60,0x60,0x60,0x33,0x1E,0x00, // C
+0x7C,0x36,0x33,0x33,0x33,0x36,0x7C,0x00, // D
+0x7F,0x31,0x34,0x3C,0x34,0x31,0x7F,0x00, // E
+0x7F,0x31,0x34,0x3C,0x34,0x30,0x78,0x00, // F
+0x1E,0x33,0x60,0x60,0x67,0x33,0x1F,0x00, // G
+0x66,0x66,0x66,0x7E,0x66,0x66,0x66,0x00, // H
+0x3C,0x18,0x18,0x18,0x18,0x18,0x3C,0x00, // I
+0x0F,0x06,0x06,0x06,0x66,0x66,0x3C,0x00, // J
+0x73,0x33,0x36,0x3C,0x36,0x33,0x73,0x00, // K
+0x78,0x30,0x30,0x30,0x31,0x33,0x7F,0x00, // L
+0x63,0x77,0x7F,0x7F,0x6B,0x63,0x63,0x00, // M
+0x63,0x73,0x7B,0x6F,0x67,0x63,0x63,0x00, // N
+0x3E,0x63,0x63,0x63,0x63,0x63,0x3E,0x00, // O
+0x7E,0x33,0x33,0x3E,0x30,0x30,0x78,0x00, // P 0x50
+0x3C,0x66,0x66,0x66,0x6E,0x3C,0x0E,0x00, // Q
+0x7E,0x33,0x33,0x3E,0x36,0x33,0x73,0x00, // R
+0x3C,0x66,0x30,0x18,0x0C,0x66,0x3C,0x00, // S
+0x7E,0x5A,0x18,0x18,0x18,0x18,0x3C,0x00, // T
+0x66,0x66,0x66,0x66,0x66,0x66,0x7E,0x00, // U
+0x66,0x66,0x66,0x66,0x66,0x3C,0x18,0x00, // V
+0x63,0x63,0x63,0x6B,0x7F,0x77,0x63,0x00, // W
+0x63,0x63,0x36,0x1C,0x1C,0x36,0x63,0x00, // X
+0x66,0x66,0x66,0x3C,0x18,0x18,0x3C,0x00, // Y
+0x7F,0x63,0x46,0x0C,0x19,0x33,0x7F,0x00, // Z
+0x3C,0x30,0x30,0x30,0x30,0x30,0x3C,0x00, // [
+0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00, // \ (back slash)
+0x3C,0x0C,0x0C,0x0C,0x0C,0x0C,0x3C,0x00, // ]
+0x08,0x1C,0x36,0x63,0x00,0x00,0x00,0x00, // ^
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF, // _
+0x18,0x18,0x0C,0x00,0x00,0x00,0x00,0x00, // ` 0x60
+0x00,0x00,0x3C,0x06,0x3E,0x66,0x3B,0x00, // a
+0x70,0x30,0x3E,0x33,0x33,0x33,0x6E,0x00, // b
+0x00,0x00,0x3C,0x66,0x60,0x66,0x3C,0x00, // c
+0x0E,0x06,0x3E,0x66,0x66,0x66,0x3B,0x00, // d
+0x00,0x00,0x3C,0x66,0x7E,0x60,0x3C,0x00, // e
+0x1C,0x36,0x30,0x78,0x30,0x30,0x78,0x00, // f
+0x00,0x00,0x3B,0x66,0x66,0x3E,0x06,0x7C, // g
+0x70,0x30,0x36,0x3B,0x33,0x33,0x73,0x00, // h
+0x18,0x00,0x38,0x18,0x18,0x18,0x3C,0x00, // i
+0x06,0x00,0x06,0x06,0x06,0x66,0x66,0x3C, // j
+0x70,0x30,0x33,0x36,0x3C,0x36,0x73,0x00, // k
+0x38,0x18,0x18,0x18,0x18,0x18,0x3C,0x00, // l
+0x00,0x00,0x66,0x7F,0x7F,0x6B,0x63,0x00, // m
+0x00,0x00,0x7C,0x66,0x66,0x66,0x66,0x00, // n
+0x00,0x00,0x3C,0x66,0x66,0x66,0x3C,0x00, // o
+0x00,0x00,0x6E,0x33,0x33,0x3E,0x30,0x78, // p
+0x00,0x00,0x3B,0x66,0x66,0x3E,0x06,0x0F, // q
+0x00,0x00,0x6E,0x3B,0x33,0x30,0x78,0x00, // r
+0x00,0x00,0x3E,0x60,0x3C,0x06,0x7C,0x00, // s
+0x08,0x18,0x3E,0x18,0x18,0x1A,0x0C,0x00, // t
+0x00,0x00,0x66,0x66,0x66,0x66,0x3B,0x00, // u
+0x00,0x00,0x66,0x66,0x66,0x3C,0x18,0x00, // v
+0x00,0x00,0x63,0x6B,0x7F,0x7F,0x36,0x00, // w
+0x00,0x00,0x63,0x36,0x1C,0x36,0x63,0x00, // x
+0x00,0x00,0x66,0x66,0x66,0x3E,0x06,0x7C, // y
+0x00,0x00,0x7E,0x4C,0x18,0x32,0x7E,0x00, // z
+0x0E,0x18,0x18,0x70,0x18,0x18,0x0E,0x00, // {
+0x0C,0x0C,0x0C,0x00,0x0C,0x0C,0x0C,0x00, // |
+0x70,0x18,0x18,0x0E,0x18,0x18,0x70,0x00, // }
+0x3B,0x6E,0x00,0x00,0x00,0x00,0x00,0x00, // ~
+0x1C,0x36,0x36,0x1C,0x00,0x00,0x00,0x00}; // DEL
+
+GraphicsDisplay::GraphicsDisplay(const char *name):TextDisplay(name) {
+ foreground(0xFFFF);
+ background(0x0000);
+}
+
+void GraphicsDisplay::character(int column, int row, int value) {
+ blitbit(column * 8, row * 8, 8, 8, (char*)&(FONT8x8[value - 0x1F][0]));
+}
+
+void GraphicsDisplay::gwindow(int x, int y, int w, int h) {
+ // current pixel location
+ _x = x;
+ _y = y;
+ // window settings
+ _x1 = x;
+ _x2 = x + w - 1;
+ _y1 = y;
+ _y2 = y + h - 1;
+}
+
+void GraphicsDisplay::putp(int colour) {
+ // put pixel at current pixel location
+ pixel(_x, _y, colour);
+ // update pixel location based on window settings
+ _x++;
+ if(_x > _x2) {
+ _x = _x1;
+ _y++;
+ if(_y > _y2) {
+ _y = _y1;
+ }
+ }
+}
+
+void GraphicsDisplay::fill(int x, int y, int w, int h, int colour) {
+ gwindow(x, y, w, h);
+ for(int i=0; i<w*h; i++) {
+ putp(colour);
+ }
+}
+
+void GraphicsDisplay::cls() {
+ fill(0, 0, width(), height(), _background);
+}
+
+void GraphicsDisplay::blit(int x, int y, int w, int h, const int *colour) {
+ gwindow(x, y, w, h);
+ for(int i=0; i<w*h; i++) {
+ putp(colour[i]);
+ }
+}
+
+void GraphicsDisplay::blitbit(int x, int y, int w, int h, const char* colour) {
+ gwindow(x, y, w, h);
+ for(int i = 0; i < w*h; i++) {
+ char byte = colour[i >> 3];
+ int offset = i & 0x7;
+ int c = ((byte << offset) & 0x80) ? _foreground : _background;
+ putp(c);
+ }
+}
+
+int GraphicsDisplay::columns() {
+ return width() / 8;
+}
+
+int GraphicsDisplay::rows() {
+ return height() / 8;
+}
+
diff -r 000000000000 -r cf17b1f16335 GraphicsDisplay.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GraphicsDisplay.h Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,44 @@
+ // ST7735 8 Bit SPI Library
+
+#ifndef MBED_GRAPHICSDISPLAY_H
+#define MBED_GRAPHICSDISPLAY_H
+
+#include "TextDisplay.h"
+
+class GraphicsDisplay : public TextDisplay {
+
+public:
+
+ GraphicsDisplay(const char* name);
+
+ virtual void pixel(int x, int y, int colour) = 0;
+ virtual int width() = 0;
+ virtual int height() = 0;
+
+ virtual void gwindow(int x, int y, int w, int h);
+ virtual void putp(int colour);
+
+ virtual void cls();
+ virtual void fill(int x, int y, int w, int h, int colour);
+ virtual void blit(int x, int y, int w, int h, const int *colour);
+ virtual void blitbit(int x, int y, int w, int h, const char* colour);
+
+ virtual void character(int column, int row, int value);
+ virtual int columns();
+ virtual int rows();
+
+protected:
+
+ // pixel location
+ short _x;
+ short _y;
+
+ // window location
+ short _x1;
+ short _x2;
+ short _y1;
+ short _y2;
+
+};
+
+#endif
diff -r 000000000000 -r cf17b1f16335 MMA8451Q.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8451Q.cpp Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,81 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "MMA8451Q.h"
+
+#define REG_WHO_AM_I 0x0D
+#define REG_CTRL_REG_1 0x2A
+#define REG_OUT_X_MSB 0x01
+#define REG_OUT_Y_MSB 0x03
+#define REG_OUT_Z_MSB 0x05
+
+#define UINT14_MAX 16383
+
+MMA8451Q::MMA8451Q(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+ // activate the peripheral
+ uint8_t data[2] = {REG_CTRL_REG_1, 0x01};
+ writeRegs(data, 2);
+}
+
+MMA8451Q::~MMA8451Q() { }
+
+uint8_t MMA8451Q::getWhoAmI() {
+ uint8_t who_am_i = 0;
+ readRegs(REG_WHO_AM_I, &who_am_i, 1);
+ return who_am_i;
+}
+
+float MMA8451Q::getAccX() {
+ return (float(getAccAxis(REG_OUT_X_MSB))/4096.0);
+}
+
+float MMA8451Q::getAccY() {
+ return (float(getAccAxis(REG_OUT_Y_MSB))/4096.0);
+}
+
+float MMA8451Q::getAccZ() {
+ return (float(getAccAxis(REG_OUT_Z_MSB))/4096.0);
+}
+
+void MMA8451Q::getAccAllAxis(float * res) {
+ res[0] = getAccX();
+ res[1] = getAccY();
+ res[2] = getAccZ();
+}
+
+int16_t MMA8451Q::getAccAxis(uint8_t addr) {
+ int16_t acc;
+ uint8_t res[2];
+ readRegs(addr, res, 2);
+
+ acc = (res[0] << 6) | (res[1] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+
+ return acc;
+}
+
+void MMA8451Q::readRegs(int addr, uint8_t * data, int len) {
+ char t[1] = {addr};
+ m_i2c.write(m_addr, t, 1, true);
+ m_i2c.read(m_addr, (char *)data, len);
+}
+
+void MMA8451Q::writeRegs(uint8_t * data, int len) {
+ m_i2c.write(m_addr, (char *)data, len);
+}
diff -r 000000000000 -r cf17b1f16335 MMA8451Q.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8451Q.h Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,110 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef MMA8451Q_H
+#define MMA8451Q_H
+
+#include "mbed.h"
+
+/**
+* MMA8451Q accelerometer example
+*
+* @code
+* #include "mbed.h"
+* #include "MMA8451Q.h"
+*
+* #define MMA8451_I2C_ADDRESS (0x1d<<1)
+*
+* int main(void) {
+*
+* MMA8451Q acc(P_E25, P_E24, MMA8451_I2C_ADDRESS);
+* PwmOut rled(LED_RED);
+* PwmOut gled(LED_GREEN);
+* PwmOut bled(LED_BLUE);
+*
+* while (true) {
+* rled = 1.0 - abs(acc.getAccX());
+* gled = 1.0 - abs(acc.getAccY());
+* bled = 1.0 - abs(acc.getAccZ());
+* wait(0.1);
+* }
+* }
+* @endcode
+*/
+class MMA8451Q
+{
+public:
+ /**
+ * MMA8451Q constructor
+ *
+ * @param sda SDA pin
+ * @param sdl SCL pin
+ * @param addr addr of the I2C peripheral
+ */
+ MMA8451Q(PinName sda, PinName scl, int addr);
+
+ /**
+ * MMA8451Q destructor
+ */
+ ~MMA8451Q();
+
+ /**
+ * Get the value of the WHO_AM_I register
+ *
+ * @returns WHO_AM_I value
+ */
+ uint8_t getWhoAmI();
+
+ /**
+ * Get X axis acceleration
+ *
+ * @returns X axis acceleration
+ */
+ float getAccX();
+
+ /**
+ * Get Y axis acceleration
+ *
+ * @returns Y axis acceleration
+ */
+ float getAccY();
+
+ /**
+ * Get Z axis acceleration
+ *
+ * @returns Z axis acceleration
+ */
+ float getAccZ();
+
+ /**
+ * Get XYZ axis acceleration
+ *
+ * @param res array where acceleration data will be stored
+ */
+ void getAccAllAxis(float * res);
+
+private:
+ I2C m_i2c;
+ int m_addr;
+ void readRegs(int addr, uint8_t * data, int len);
+ void writeRegs(uint8_t * data, int len);
+ int16_t getAccAxis(uint8_t addr);
+
+};
+
+#endif
diff -r 000000000000 -r cf17b1f16335 MPU6050.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.cpp Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,386 @@
+/* MPU6050 Library
+*
+* @author: Baser Kandehir
+* @date: July 16, 2015
+* @license: MIT license
+*
+* Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to deal
+* in the Software without restriction, including without limitation the rights
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+* copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in
+* all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+* THE SOFTWARE.
+*
+*/
+
+// Most of the code is adapted from Kris Winer's MPU6050 library
+
+#include "MPU6050.h"
+
+/* For LPC1768 board */
+//I2C i2c(p9,p10); // setup i2c (SDA,SCL)
+
+/* For NUCLEO-F411RE board */
+//static I2C i2c(D14,D15); // setup i2c (SDA,SCL)
+
+I2C i2c(PTE0,PTE1); // setup i2c (SDA,SCL)
+
+/* Set initial input parameters */
+
+// Acc Full Scale Range +-2G 4G 8G 16G
+enum Ascale
+{
+ AFS_2G=0,
+ AFS_4G,
+ AFS_8G,
+ AFS_16G
+};
+
+// Gyro Full Scale Range +-250 500 1000 2000 Degrees per second
+enum Gscale
+{
+ GFS_250DPS=0,
+ GFS_500DPS,
+ GFS_1000DPS,
+ GFS_2000DPS
+};
+
+// Sensor datas
+float ax,ay,az;
+float gx,gy,gz;
+int16_t accelData[3],gyroData[3],tempData;
+float accelBias[3] = {0, 0, 0}; // Bias corrections for acc
+float gyroBias[3] = {0, 0, 0}; // Bias corrections for gyro
+
+// Specify sensor full scale range
+int Ascale = AFS_2G;
+int Gscale = GFS_250DPS;
+
+// Scale resolutions per LSB for the sensors
+float aRes, gRes;
+
+// Calculates Acc resolution
+void MPU6050::getAres()
+{
+ switch(Ascale)
+ {
+ case AFS_2G:
+ aRes = 2.0/32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0/32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0/32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0/32768.0;
+ break;
+ }
+}
+
+// Calculates Gyro resolution
+void MPU6050::getGres()
+{
+ switch(Gscale)
+ {
+ case GFS_250DPS:
+ gRes = 250.0/32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0/32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0/32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0/32768.0;
+ break;
+ }
+}
+
+void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{
+ char data_write[2];
+ data_write[0]=subAddress; // I2C sends MSB first. Namely >>|subAddress|>>|data|
+ data_write[1]=data;
+ i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false);
+}
+
+char MPU6050::readByte(uint8_t address, uint8_t subAddress)
+{
+ char data_read[1]; // will store the register data
+ char data_write[1];
+ data_write[0]=subAddress;
+ i2c.write(address,data_write,1,1); // have not stopped yet
+ i2c.read(address,data_read,1,0); // read the data and stop
+ return data_read[0];
+}
+
+void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest)
+{
+ char data[14],data_write[1];
+ data_write[0]=subAddress;
+ i2c.write(address,data_write,1,1);
+ i2c.read(address,data,byteNum,0);
+ for(int i=0;i<byteNum;i++) // equate the addresses
+ dest[i]=data[i];
+}
+
+// Communication test: WHO_AM_I register reading
+void MPU6050::whoAmI()
+{
+ uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68
+ pc.printf("I AM 0x%x \r\n",whoAmI);
+
+ if(whoAmI==0x68)
+ {
+ pc.printf("MPU6050 is online... \r\n");
+ //led2=1;
+ }
+ else
+ {
+ pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");
+ //toggler1.attach(&toggle_led1,0.1); // toggles led1 every 100 ms
+ }
+}
+
+// Initializes MPU6050 with the following config:
+// PLL with X axis gyroscope reference
+// Sample rate: 200Hz for gyro and acc
+// Interrupts are disabled
+void MPU6050::init()
+{
+ i2c.frequency(400000); // fast i2c: 400 kHz
+
+ /* Wake up the device */
+ writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6)
+ wait_ms(100); // wait 100 ms to stabilize
+
+ /* Get stable time source */
+ // PLL with X axis gyroscope reference is used to improve stability
+ writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
+
+ /* Configure Gyroscope and Accelerometer */
+ // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz
+ // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate)
+ writeByte(MPU6050_ADDRESS, CONFIG, 0x03);
+
+ /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */
+ // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above)
+ writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
+
+ /* Accelerometer configuration */
+ uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
+ writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5]
+ writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3]
+ writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range
+
+ /* Gyroscope configuration */
+ temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG);
+ writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5]
+ writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3]
+ writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3); // Set full scale range
+}
+
+// Resets the device
+void MPU6050::reset()
+{
+ writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // set bit7 to reset the device
+ wait_ms(100); // wait 100 ms to stabilize
+}
+
+void MPU6050::readAccelData(int16_t* dest)
+{
+ uint8_t rawData[6]; // x,y,z acc data
+ readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array
+
+ /* Turn the MSB LSB into signed 16-bit value */
+ dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT
+ dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT
+ dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT
+}
+
+void MPU6050::readGyroData(int16_t* dest)
+{
+ uint8_t rawData[6]; // x,y,z gyro data
+ readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // read the six raw data registers sequentially and write them into data array
+
+ /* Turn the MSB LSB into signed 16-bit value */
+ dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT
+ dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT
+ dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT
+}
+
+int16_t MPU6050::readTempData()
+{
+ uint8_t rawData[2]; // temperature data
+ readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // read the two raw data registers sequentially and write them into data array
+ return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // turn the MSB LSB into signed 16-bit value
+}
+
+/* Function which accumulates gyro and accelerometer data after device initialization.
+ It calculates the average of the at-rest readings and
+ then loads the resulting offsets into accelerometer and gyro bias registers. */
+/*
+ IMPORTANT NOTE: In this function;
+ Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program.
+ Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program.
+ Resulting data seems satisfactory.
+*/
+// dest1: accelBias dest2: gyroBias
+void MPU6050::calibrate(float* dest1, float* dest2)
+{
+ uint8_t data[12]; // data array to hold acc and gyro x,y,z data
+ uint16_t fifo_count, packet_count, count;
+ int32_t accel_bias[3] = {0,0,0};
+ int32_t gyro_bias[3] = {0,0,0};
+ float aRes = 2.0/32768.0;
+ float gRes = 250.0/32768.0;
+ uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g
+ //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps
+
+ reset(); // Reset device
+
+ /* Get stable time source */
+ writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability
+ writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); // Disable accel only low power mode
+ wait(0.2);
+
+ /* Configure device for bias calculation */
+ writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
+ writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
+ writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
+ writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
+ writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
+ writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04); // Reset FIFO
+ wait(0.015);
+
+ /* Configure accel and gyro for bias calculation */
+ writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
+ writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
+ writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+ writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+
+ /* Configure FIFO to capture accelerometer and gyro data for bias calculation */
+ writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
+ writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050)
+ wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds.
+ // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte. 12*80=960 byte < 1024 byte
+
+ /* At end of sample accumulation, turn off FIFO sensor read */
+ writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
+ readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count
+ fifo_count = ((uint16_t)data[0] << 8) | data[1];
+ packet_count = fifo_count/12; // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case
+
+ for(count=0; count<packet_count; count++)
+ {
+ int16_t accel_temp[3]={0,0,0};
+ int16_t gyro_temp[3]={0,0,0};
+ readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
+
+ /* Form signed 16-bit integer for each sample in FIFO */
+ accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ;
+ accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
+ accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
+ gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
+ gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
+ gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
+
+ /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */
+ accel_bias[0] += (int32_t) accel_temp[0];
+ accel_bias[1] += (int32_t) accel_temp[1];
+ accel_bias[2] += (int32_t) accel_temp[2];
+ gyro_bias[0] += (int32_t) gyro_temp[0];
+ gyro_bias[1] += (int32_t) gyro_temp[1];
+ gyro_bias[2] += (int32_t) gyro_temp[2];
+ }
+
+ /* Normalize sums to get average count biases */
+ accel_bias[0] /= (int32_t) packet_count;
+ accel_bias[1] /= (int32_t) packet_count;
+ accel_bias[2] /= (int32_t) packet_count;
+ gyro_bias[0] /= (int32_t) packet_count;
+ gyro_bias[1] /= (int32_t) packet_count;
+ gyro_bias[2] /= (int32_t) packet_count;
+
+ /* Remove gravity from the z-axis accelerometer bias calculation */
+ if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;}
+ else {accel_bias[2] += (int32_t) accelsensitivity;}
+
+ /* Output scaled accelerometer biases for manual subtraction in the main program */
+ dest1[0] = accel_bias[0]*aRes;
+ dest1[1] = accel_bias[1]*aRes;
+ dest1[2] = accel_bias[2]*aRes;
+
+ /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */
+ data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+ data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
+ data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
+ data[3] = (-gyro_bias[1]/4) & 0xFF;
+ data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
+ data[5] = (-gyro_bias[2]/4) & 0xFF;
+
+ /* Push gyro biases to hardware registers */
+ writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]);
+ writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]);
+ writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]);
+ writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]);
+ writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]);
+ writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]);
+
+ /* Construct gyro bias in deg/s for later manual subtraction */
+ dest2[0] = gyro_bias[0]*gRes;
+ dest2[1] = gyro_bias[1]*gRes;
+ dest2[2] = gyro_bias[2]*gRes;
+}
+
+void MPU6050::complementaryFilter(float* pitch, float* roll)
+{
+ /* Get actual acc value */
+ readAccelData(accelData);
+ getAres();
+ ax = accelData[0]*aRes - accelBias[0];
+ ay = accelData[1]*aRes - accelBias[1];
+ az = accelData[2]*aRes - accelBias[2];
+
+ /* Get actual gyro value */
+ readGyroData(gyroData);
+ getGres();
+ gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i]
+ gy = gyroData[1]*gRes; // - gyroBias[1];
+ gz = gyroData[2]*gRes; // - gyroBias[2];
+
+ float pitchAcc, rollAcc,roll1, pitch1;
+
+ /* Integrate the gyro data(deg/s) over time to get angle */
+ *pitch += gx * dt; // Angle around the X-axis
+ *roll -= gy * dt; // Angle around the Y-axis
+ float pt1 = (accelData[0]*accelData[0]) + (accelData[2]*accelData[2]);
+ float pt2 = (accelData[1]*accelData[1]) + (accelData[2]*accelData[2]);
+ roll1 = atan2f(accelData[0], sqrt(pt1));
+ pitch1 = atan2f(accelData[1], sqrt(pt2));
+
+ /* Turning around the X-axis results in a vector on the Y-axis
+ whereas turning around the Y-axis results in a vector on the X-axis. */
+ pitchAcc = atan2f(accelData[1], accelData[2])*180/PI;
+ rollAcc = atan2f(accelData[0], accelData[2])*180/PI;
+
+ /* Apply Complementary Filter */
+ *pitch = *pitch * 0.98 + pitchAcc * 0.02;
+ *roll = *roll * 0.98 + rollAcc * 0.02;
+}
diff -r 000000000000 -r cf17b1f16335 MPU6050.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.h Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,70 @@
+/*
+* Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to deal
+* in the Software without restriction, including without limitation the rights
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+* copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in
+* all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+* THE SOFTWARE.
+*
+*/
+
+// Most of the code is adapted from Kris Winer's MPU6050 library
+
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "mbed.h"
+#include "math.h"
+#include "MPU6050RegDef.h"
+
+//#define PI 3.14159265359 // This value will be used when calculating angles
+#define dt 0.005 // 200 Hz sampling period
+
+extern float aRes, gRes;
+
+/* whoAmI func uses this func, variables etc */
+//extern Ticker toggler1;
+extern Serial pc;
+extern DigitalOut led2;
+//extern void toggle_led1();
+
+/* Sensor datas to be used in program */
+extern float ax,ay,az;
+extern float gx,gy,gz;
+extern int16_t accelData[3],gyroData[3],tempData;
+extern float accelBias[3], gyroBias[3];
+
+/* Function Prototypes */
+class MPU6050
+{
+ protected:
+ public:
+ void getAres();
+ void getGres();
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+ char readByte(uint8_t address, uint8_t subAddress);
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest);
+ void whoAmI();
+ void init();
+ void reset();
+ void readAccelData(int16_t* dest);
+ void readGyroData(int16_t* dest);
+ int16_t readTempData();
+ void calibrate(float* dest1, float* dest2);
+ void complementaryFilter(float* pitch, float* roll);
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r cf17b1f16335 MPU6050RegDef.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050RegDef.h Tue Oct 24 20:12:54 2017 +0000 @@ -0,0 +1,153 @@ +/* +* Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in +* all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +* THE SOFTWARE. +* +*/ + +// Taken from Kris Winer's MPU6050 library + +// Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device +// Invensense Inc., www.invensense.com +// See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in +// above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor +// +#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD +#define YGOFFS_TC 0x01 +#define ZGOFFS_TC 0x02 +#define X_FINE_GAIN 0x03 // [7:0] fine gain +#define Y_FINE_GAIN 0x04 +#define Z_FINE_GAIN 0x05 +#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +#define XA_OFFSET_L_TC 0x07 +#define YA_OFFSET_H 0x08 +#define YA_OFFSET_L_TC 0x09 +#define ZA_OFFSET_H 0x0A +#define ZA_OFFSET_L_TC 0x0B +#define SELF_TEST_X 0x0D +#define SELF_TEST_Y 0x0E +#define SELF_TEST_Z 0x0F +#define SELF_TEST_A 0x10 +#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050? +#define XG_OFFS_USRL 0x14 +#define YG_OFFS_USRH 0x15 +#define YG_OFFS_USRL 0x16 +#define ZG_OFFS_USRH 0x17 +#define ZG_OFFS_USRL 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define FF_THR 0x1D // Free-fall +#define FF_DUR 0x1E // Free-fall +#define MOT_THR 0x1F // Motion detection threshold bits [7:0] +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU6050 0x75 // Should return 0x68 + +// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor +// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 +#define ADO 0 +#if ADO +#define MPU6050_ADDRESS 0x69<<1 // Device address when ADO = 1 +#else +#define MPU6050_ADDRESS 0x68<<1 // Device address when ADO = 0 +#endif \ No newline at end of file
diff -r 000000000000 -r cf17b1f16335 ST7735_TFT.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ST7735_TFT.cpp Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,698 @@
+ // ST7735 8 Bit SPI Library
+
+
+#include "ST7735_TFT.h"
+#include "mbed.h"
+
+#define BPP 16 // Bits per pixel
+
+ST7735_TFT::ST7735_TFT(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName rs, PinName reset, const char *name)
+ : GraphicsDisplay(name),_spi(mosi, miso, sclk), _cs(cs), _rs(rs), _reset(reset)
+ {
+ tft_reset();
+ orientation = 2;
+ char_x = 0;
+}
+int ST7735_TFT::width() {
+ if (orientation == 0 || orientation == 2) return 128;
+ else return 160;
+}
+int ST7735_TFT::height() {
+ if (orientation == 0 || orientation == 2) return 160;
+ else return 128;
+}
+void ST7735_TFT::set_orientation(unsigned int o) {
+ orientation = o;
+ switch (orientation) {
+ case 0:
+ wr_reg(ST7735_MADCTL, 0x0008);
+ break;
+ case 1:
+ wr_reg(ST7735_MADCTL, 0x0068);
+ break;
+ case 2:
+ wr_reg(ST7735_MADCTL, 0x00C8);
+ break;
+ case 3:
+ wr_reg(ST7735_MADCTL, 0x00A8);
+ break;
+ }
+}
+void ST7735_TFT::wr_cmd(int cmd) {
+ _rs = 0; // rs low, cs low for transmitting command
+ _cs = 0;
+ _spi.write(cmd);
+ _cs = 1;
+}
+void ST7735_TFT::wr_dat(int dat) {
+ _rs = 1; // rs high, cs low for transmitting data
+ _cs = 0;
+ _spi.write(dat);
+ _cs = 1;
+}
+void ST7735_TFT::wr_dat_start(void) {
+ _rs = 1; // rs high, cs low for transmitting data
+ _cs = 0;
+}
+void ST7735_TFT::wr_dat_stop (void) {
+ _cs = 1;
+}
+void ST7735_TFT::wr_dat_only (unsigned short dat) {
+ _spi.write(dat);
+}
+unsigned short ST7735_TFT::rd_dat (void) {
+ unsigned short val = 0;
+ _cs = 0;
+ _spi.write(0); /* Dummy read 1 */
+ val = _spi.write(0); /* Read D8..D15 */
+ val <<= 8;
+ val |= _spi.write(0); /* Read D0..D7 */
+ _cs = 1;
+ return (val);
+}
+void ST7735_TFT::wr_reg (unsigned char reg, unsigned short val) {
+
+ wr_cmd(reg);
+ wr_dat(val);
+}
+unsigned short ST7735_TFT::rd_reg (unsigned char reg) {
+
+ wr_cmd(reg);
+ return(rd_dat());
+}
+void ST7735_TFT::read_area(unsigned int x, unsigned int y, unsigned int w, unsigned int h,unsigned char *buffer) {
+ // BEWARE !
+ // DOES NOT WORK CORRECTLY YET !!!
+ int val;
+ window(x,y,w,h);
+ wr_cmd(ST7735_RAMRD); // write to RAM
+ _cs = 0;
+ _rs = 1;
+ _spi.write(0); /* Dummy read 1 */
+
+ val = _spi.write(0); /* Read D8..D15 */
+ val <<= 8;
+ val |= _spi.write(0); /* Read D0..D7 */
+ _cs = 1;
+}
+int ST7735_TFT::getpixel(unsigned int x, unsigned int y) {
+ // BEWARE !
+ // DOES NOT WORK CORRECTLY YET !!!
+ int val;
+ // _spi.format(8,3);
+ wr_cmd(ST7735_CASET); // column addr set
+ wr_dat(0x00);
+ wr_dat(x+2); // XSTART
+ wr_dat(0x00);
+ wr_dat(x+2+2); // XEND
+
+ wr_cmd(ST7735_RASET); // row addr set
+ wr_dat(0x00);
+ wr_dat(y+1); // YSTART
+ wr_dat(0x00);
+ wr_dat(y+1+1); // YEND
+
+ _rs = 0; // rs low, cs low for transmitting command
+ _cs = 0;
+ _spi.write(0x2E);
+ _rs = 1;
+ _spi.write(0x00); /* Dummy read 1 */
+
+ val = _spi.write(0x00); /* Read D8..D15 */
+ val <<= 8;
+ val |= _spi.write(0x00); /* Read D0..D7 */
+
+ _cs = 1;
+ return val;
+}
+void ST7735_TFT::tft_reset() {
+ // init SPI
+ _spi.format(8,3); // 8 bit spi mode 3
+ _spi.frequency(15000000); // 16Mhz SPI clock ... 15Mhz is maximum for display, but it seems to work
+ _cs = 0;
+ _reset = 1; // reset
+ wait_ms(10);
+ _reset = 0; // reset
+ wait_ms(10);
+ _reset = 1; // reset
+ wait_ms(10);
+
+ /* Start Initial Sequence ----------------------------------------------------*/
+ wr_cmd(ST7735_SWRESET); /* SW Reset */
+ wait_ms(150);
+ wr_cmd(ST7735_SLPOUT); /* Out of sleepmode */
+ wait_ms(500);
+
+ wr_cmd(ST7735_FRMCTR1); /* Frame rate in normal mode */
+ wr_dat(0x01);
+ wr_dat(0x2C);
+ wr_dat(0x2D);
+
+ wr_cmd(ST7735_FRMCTR2); /* Frame rate in idle mode */
+ wr_dat(0x01);
+ wr_dat(0x2C);
+ wr_dat(0x2D);
+
+ wr_cmd(ST7735_FRMCTR3); /* Frame rate in partial mode */
+ wr_dat(0x01);
+ wr_dat(0x2C);
+ wr_dat(0x2D);
+ wr_dat(0x01); // inversion mode settings
+ wr_dat(0x2C);
+ wr_dat(0x2D);
+
+ wr_cmd(ST7735_INVCTR); // Inverted mode off
+ wr_dat(0x07);
+
+ wr_cmd(ST7735_PWCTR1); // POWER CONTROL 1
+ wr_dat(0xA2);
+ wr_dat(0x02); // -4.6V
+ wr_dat(0x84); // AUTO mode
+
+ wr_cmd(ST7735_PWCTR2); // POWER CONTROL 2
+ wr_dat(0xC5); // VGH25 = 2.4C VGSEL =-10 VGH = 3*AVDD
+
+ wr_cmd(ST7735_PWCTR3); // POWER CONTROL 3
+ wr_dat(0x0A); // Opamp current small
+ wr_dat(0x00); // Boost freq
+
+ wr_cmd(ST7735_PWCTR4); // POWER CONTROL 4
+ wr_dat(0x8A); // BCLK/2, Opamp current small / medium low
+ wr_dat(0x2A); //
+
+ wr_cmd(ST7735_PWCTR5); // POWER CONTROL 5
+ wr_dat(0x8A); // BCLK/2, Opamp current small / medium low
+ wr_dat(0xEE); //
+
+ wr_cmd(ST7735_VMCTR1); // POWER CONTROL 6
+ wr_dat(0x0E); //
+
+ wr_cmd(ST7735_INVOFF); // INVOFF
+
+ wr_cmd(ST7735_MADCTL); // ORIENTATION
+ wr_dat(0xC8); //
+
+ wr_cmd(ST7735_COLMOD); // COLOR MODE
+ wr_dat(0x05); //
+
+ wr_cmd(ST7735_CASET); // COLUMN ADDR SET
+ wr_dat(0x00); //
+ wr_dat(0x00); // xstart = 0
+ wr_dat(0x00); //
+ wr_dat(0x7F); // xend = 127
+
+ wr_cmd(ST7735_RASET); // ROW ADDR SET
+ wr_dat(0x00); //
+ wr_dat(0x00); // ystart = 0
+ wr_dat(0x00); //
+ wr_dat(0x9F); // yend = 159
+
+ /* Gamma settings -----------------------------------------------------------*/
+
+ wr_cmd(0xE0); // GMCTRP1
+ wr_dat(0x02);
+ wr_dat(0x1c);
+ wr_dat(0x07);
+ wr_dat(0x12);
+ wr_dat(0x37);
+ wr_dat(0x32);
+ wr_dat(0x29);
+ wr_dat(0x2d);
+ wr_dat(0x29);
+ wr_dat(0x25);
+ wr_dat(0x2B);
+ wr_dat(0x39);
+ wr_dat(0x00);
+ wr_dat(0x01);
+ wr_dat(0x03);
+ wr_dat(0x10);
+ wr_cmd(0xE1); // GMCTRN1
+ wr_dat(0x03);
+ wr_dat(0x1d);
+ wr_dat(0x07);
+ wr_dat(0x06);
+ wr_dat(0x2E);
+ wr_dat(0x2C);
+ wr_dat(0x29);
+ wr_dat(0x2D);
+ wr_dat(0x2E);
+ wr_dat(0x2E);
+ wr_dat(0x37);
+ wr_dat(0x3F);
+ wr_dat(0x00);
+ wr_dat(0x00);
+ wr_dat(0x02);
+ wr_dat(0x10);
+
+ wr_cmd(ST7735_DISPON); // display ON
+ wait_ms(100);
+
+ wr_cmd(ST7735_NORON); // normal display on
+ wait_ms(10);
+
+ switch (orientation) {
+ case 0:
+ wr_reg(0xC8, 0x0008);
+ break;
+ case 1:
+ wr_reg(0xC8, 0x0068);
+ break;
+ case 2:
+ wr_reg(0xC8, 0x00C8);
+ break;
+ case 3:
+ wr_reg(0xC8, 0x00A8);
+ break;
+ }
+ WindowMax ();
+}
+void ST7735_TFT::pixel(int x, int y, int color) {
+ if ((x >= width()) || (y >= height())) return;
+ window(x,y,x+1,y+1);
+ _rs = 1;
+ _cs = 0;
+ _spi.write(color);
+ _spi.write(color>>8);
+ _cs = 1;
+}
+void ST7735_TFT::window (unsigned int x, unsigned int y, unsigned int w, unsigned int h) {
+ wr_cmd(ST7735_CASET); // column addr set
+ wr_dat(0x00);
+ wr_dat(x); // XSTART
+ wr_dat(0x00);
+ wr_dat(x+w-1); // XEND
+ wr_cmd(ST7735_RASET); // row addr set
+ wr_dat(0x00);
+ wr_dat(y); // YSTART
+ wr_dat(0x00);
+ wr_dat(y+h-1); // YEND
+ wr_cmd(ST7735_RAMWR); // write to RAM
+}
+void ST7735_TFT::WindowMax (void) {
+ window (0, 0, width(), height());
+}
+
+void ST7735_TFT::cls (void) {
+ unsigned int i;
+ WindowMax();
+ wr_dat_start();
+ for (i = 0; i < ( (width()) * (height())); i++) {
+
+ //_spi.write(_background>>8);
+ //_spi.write(_background);
+
+ _spi.fastWrite(_background>>8);
+ _spi.fastWrite(_background);
+
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+}
+void ST7735_TFT::circle(int x0, int y0, int r, int color) {
+ int draw_x0, draw_y0;
+ int draw_x1, draw_y1;
+ int draw_x2, draw_y2;
+ int draw_x3, draw_y3;
+ int draw_x4, draw_y4;
+ int draw_x5, draw_y5;
+ int draw_x6, draw_y6;
+ int draw_x7, draw_y7;
+ int xx, yy;
+ int di;
+ WindowMax();
+ if (r == 0) { /* no radius */
+ return;
+ }
+ draw_x0 = draw_x1 = x0;
+ draw_y0 = draw_y1 = y0 + r;
+ if (draw_y0 < height()) {
+ pixel(draw_x0, draw_y0, color); /* 90 degree */
+ }
+ draw_x2 = draw_x3 = x0;
+ draw_y2 = draw_y3 = y0 - r;
+ if (draw_y2 >= 0) {
+ pixel(draw_x2, draw_y2, color); /* 270 degree */
+ }
+ draw_x4 = draw_x6 = x0 + r;
+ draw_y4 = draw_y6 = y0;
+ if (draw_x4 < width()) {
+ pixel(draw_x4, draw_y4, color); /* 0 degree */
+ }
+ draw_x5 = draw_x7 = x0 - r;
+ draw_y5 = draw_y7 = y0;
+ if (draw_x5>=0) {
+ pixel(draw_x5, draw_y5, color); /* 180 degree */
+ }
+ if (r == 1) {
+ return;
+ }
+ di = 3 - 2*r;
+ xx = 0;
+ yy = r;
+ while (xx < yy) {
+
+ if (di < 0) {
+ di += 4*xx + 6;
+ } else {
+ di += 4*(xx - yy) + 10;
+ yy--;
+ draw_y0--;
+ draw_y1--;
+ draw_y2++;
+ draw_y3++;
+ draw_x4--;
+ draw_x5++;
+ draw_x6--;
+ draw_x7++;
+ }
+ xx++;
+ draw_x0++;
+ draw_x1--;
+ draw_x2++;
+ draw_x3--;
+ draw_y4++;
+ draw_y5++;
+ draw_y6--;
+ draw_y7--;
+
+ if ( (draw_x0 <= width()) && (draw_y0>=0) ) {
+ pixel(draw_x0, draw_y0, color);
+ }
+ if ( (draw_x1 >= 0) && (draw_y1 >= 0) ) {
+ pixel(draw_x1, draw_y1, color);
+ }
+ if ( (draw_x2 <= width()) && (draw_y2 <= height()) ) {
+ pixel(draw_x2, draw_y2, color);
+ }
+ if ( (draw_x3 >=0 ) && (draw_y3 <= height()) ) {
+ pixel(draw_x3, draw_y3, color);
+ }
+ if ( (draw_x4 <= width()) && (draw_y4 >= 0) ) {
+ pixel(draw_x4, draw_y4, color);
+ }
+ if ( (draw_x5 >= 0) && (draw_y5 >= 0) ) {
+ pixel(draw_x5, draw_y5, color);
+ }
+ if ( (draw_x6 <=width()) && (draw_y6 <= height()) ) {
+ pixel(draw_x6, draw_y6, color);
+ }
+ if ( (draw_x7 >= 0) && (draw_y7 <= height()) ) {
+ pixel(draw_x7, draw_y7, color);
+ }
+ }
+ return;
+}
+void ST7735_TFT::fillcircle(int x, int y, int r, int color) {
+ int i;
+ for (i = 0; i <= r; i++)
+ circle(x,y,i,color);
+}
+void ST7735_TFT::hline(int x0, int x1, int y, int color) {
+ int w;
+ w = x1 - x0 + 1;
+ window(x0,y,w,1);
+ wr_cmd(0x2C);
+ wr_dat_start();
+ for (int x=0; x<w; x++) {
+ //_spi.write(color);
+ //_spi.write(color >> 8);
+ _spi.fastWrite(color>>8);
+ _spi.fastWrite(color);
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+ return;
+}
+void ST7735_TFT::vline(int x, int y0, int y1, int color) {
+ int h;
+ h = y1 - y0 + 1;
+ window(x,y0,1,h);
+ wr_cmd(0x2C);
+ wr_dat_start();
+ for (int y=0; y<h; y++) {
+ //_spi.write(color);
+ //_spi.write(color >> 8);
+ _spi.fastWrite(color>>8);
+ _spi.fastWrite(color);
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+ return;
+}
+void ST7735_TFT::line(int x0, int y0, int x1, int y1, int color) {
+ WindowMax();
+ int dx = 0, dy = 0;
+ int dx_sym = 0, dy_sym = 0;
+ int dx_x2 = 0, dy_x2 = 0;
+ int di = 0;
+ dx = x1-x0;
+ dy = y1-y0;
+
+ if (dx == 0) { /* vertical line */
+ if (y1 > y0) vline(x0,y0,y1,color);
+ else vline(x0,y1,y0,color);
+ return;
+ }
+ if (dx > 0) {
+ dx_sym = 1;
+ } else {
+ dx_sym = -1;
+ }
+ if (dy == 0) { /* horizontal line */
+ if (x1 > x0) hline(x0,x1,y0,color);
+ else hline(x1,x0,y0,color);
+ return;
+ }
+ if (dy > 0) {
+ dy_sym = 1;
+ } else {
+ dy_sym = -1;
+ }
+ dx = dx_sym*dx;
+ dy = dy_sym*dy;
+ dx_x2 = dx*2;
+ dy_x2 = dy*2;
+ if (dx >= dy) {
+ di = dy_x2 - dx;
+ while (x0 != x1) {
+
+ pixel(x0, y0, color);
+ x0 += dx_sym;
+ if (di<0) {
+ di += dy_x2;
+ } else {
+ di += dy_x2 - dx_x2;
+ y0 += dy_sym;
+ }
+ }
+ pixel(x0, y0, color);
+ } else {
+ di = dx_x2 - dy;
+ while (y0 != y1) {
+ pixel(x0, y0, color);
+ y0 += dy_sym;
+ if (di < 0) {
+ di += dx_x2;
+ } else {
+ di += dx_x2 - dy_x2;
+ x0 += dx_sym;
+ }
+ }
+ pixel(x0, y0, color);
+ }
+ return;
+}
+void ST7735_TFT::rect(int x0, int y0, int x1, int y1, int color) {
+ if (x1 > x0) hline(x0,x1,y0,color);
+ else hline(x1,x0,y0,color);
+ if (y1 > y0) vline(x0,y0,y1,color);
+ else vline(x0,y1,y0,color);
+ if (x1 > x0) hline(x0,x1,y1,color);
+ else hline(x1,x0,y1,color);
+ if (y1 > y0) vline(x1,y0,y1,color);
+ else vline(x1,y1,y0,color);
+ return;
+}
+void ST7735_TFT::fillrect(int x0, int y0, int x1, int y1, int color) {
+ int h = y1 - y0 ;
+ int w = x1 - x0 ;
+ int pixel = h * w;
+ window(x0,y0,w,h);
+ wr_cmd(0x2C);
+ wr_dat_start();
+ for (int p=0; p<pixel; p++) {
+ //_spi.write(color);
+ //_spi.write(color >> 8);
+ _spi.fastWrite(color>>8);
+ _spi.fastWrite(color);
+
+
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+ return;
+}
+void ST7735_TFT::locate(int x, int y) {
+ char_x = x;
+ char_y = y;
+}
+int ST7735_TFT::columns() {
+ return width() / font[1];
+}
+int ST7735_TFT::rows() {
+ return height() / font[2];
+}
+int ST7735_TFT::_putc(int value) {
+ if (value == '\n') { // new line
+ char_x = 0;
+ char_y = char_y + font[2];
+ if (char_y >= height() - font[2]) {
+ char_y = 0;
+ }
+ } else {
+ character(char_x, char_y, value);
+ }
+ return value;
+}
+void ST7735_TFT::character(int x, int y, int c) {
+ unsigned int hor,vert,offset,bpl,j,i,b;
+ unsigned char* sign;
+ unsigned char z,w;
+ if ((c < 31) || (c > 127)) return; // test char range
+ // read font parameter from start of array
+ offset = font[0]; // bytes / char
+ hor = font[1]; // get hor size of font
+ vert = font[2]; // get vert size of font
+ bpl = font[3]; // bytes per line
+ if (char_x + hor > width()) {
+ char_x = 0;
+ char_y = char_y + vert;
+ if (char_y >= height() - font[2]) {
+ char_y = 0;
+ }
+ }
+ window(char_x, char_y,hor,vert); // char box
+ wr_cmd(0x2C);
+ wr_dat_start();
+ sign = &font[((c -32) * offset) + 4]; // start of char bitmap
+ w = sign[0]; // width of actual char
+ for (j=0; j<vert; j++) { // vert line
+ for (i=0; i<hor; i++) { // horz line
+ z = sign[bpl * i + ((j & 0xF8) >> 3)+1];
+ b = 1 << (j & 0x07);
+ if (( z & b ) == 0x00) {
+ //_spi.write(_background>>8);
+ //_spi.write(_background);
+ _spi.fastWrite(_background>>8);
+ _spi.fastWrite(_background);
+
+ } else {
+ // _spi.write(_foreground>>8);
+ // _spi.write(_foreground);
+ _spi.fastWrite(_foreground>>8);
+ _spi.fastWrite(_foreground);
+
+ }
+ }
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+ if ((w + 2) < hor) { // x offset to next char
+ char_x += w + 2;
+ } else char_x += hor;
+}
+void ST7735_TFT::set_font(unsigned char* f) {
+ font = f;
+}
+void ST7735_TFT::Bitmap(unsigned int x, unsigned int y, unsigned int w, unsigned int h,unsigned char *bitmap) {
+ unsigned int i,j; //value;
+ unsigned short *bitmap_ptr = (unsigned short *)bitmap;
+ window(x, y, w, h);
+ wr_cmd(0x2C);
+ wr_dat_start();
+ for (j = 0; j < h; j++) { //Lines
+ for (i = 0; i < w; i++) { // copy pixel data to TFT
+ //_spi.write(*bitmap_ptr); // one line
+ //_spi.write(*bitmap_ptr >> 8);
+ _spi.fastWrite(*bitmap_ptr);
+ _spi.fastWrite(*bitmap_ptr >> 8);
+ bitmap_ptr++;
+ }
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+}
+int ST7735_TFT::BMP_16(unsigned int x, unsigned int y, const char *Name_BMP) {
+// BEWARE !
+// NOT TESTED
+#define OffsetPixelWidth 18
+#define OffsetPixelHeigh 22
+#define OffsetFileSize 34
+#define OffsetPixData 10
+#define OffsetBPP 28
+ char filename[50];
+ unsigned char BMP_Header[54];
+ unsigned short BPP_t;
+ unsigned int PixelWidth,PixelHeigh,start_data;
+ unsigned int i,off;
+ int padd,j;
+ unsigned short *line;
+ // get the filename
+ //LocalFileSystem local("local"); not working on FRDM-KL25Z
+ sprintf(&filename[0],"/local/");
+ i=7;
+ while (*Name_BMP!='\0') {
+ filename[i++]=*Name_BMP++;
+ }
+ FILE *Image = fopen((const char *)&filename[0], "r"); // open the bmp file
+ if (!Image) {
+ return(0); // error file not found !
+ }
+ fread(&BMP_Header[0],1,54,Image); // get the BMP Header
+
+ if (BMP_Header[0] != 0x42 || BMP_Header[1] != 0x4D) { // check magic byte
+ fclose(Image);
+ return(-1); // error no BMP file
+ }
+ BPP_t = BMP_Header[OffsetBPP] + (BMP_Header[OffsetBPP + 1] << 8);
+ if (BPP_t != 0x0010) {
+ fclose(Image);
+ return(-2); // error no 16 bit BMP
+ }
+ PixelHeigh = BMP_Header[OffsetPixelHeigh] + (BMP_Header[OffsetPixelHeigh + 1] << 8) + (BMP_Header[OffsetPixelHeigh + 2] << 16) + (BMP_Header[OffsetPixelHeigh + 3] << 24);
+ PixelWidth = BMP_Header[OffsetPixelWidth] + (BMP_Header[OffsetPixelWidth + 1] << 8) + (BMP_Header[OffsetPixelWidth + 2] << 16) + (BMP_Header[OffsetPixelWidth + 3] << 24);
+ if (PixelHeigh > height() + y || PixelWidth > width() + x) {
+ fclose(Image);
+ return(-3); // to big
+ }
+ start_data = BMP_Header[OffsetPixData] + (BMP_Header[OffsetPixData + 1] << 8) + (BMP_Header[OffsetPixData + 2] << 16) + (BMP_Header[OffsetPixData + 3] << 24);
+
+ line = (unsigned short *) malloc (PixelWidth); // we need a buffer for a line
+ if (line == NULL) {
+ return(-4); // error no memory
+ }
+ // the lines are padded to multiple of 4 bytes
+ padd = -1;
+ do {
+ padd ++;
+ } while ((PixelWidth * 2 + padd)%4 != 0);
+ window(x, y,PixelWidth,PixelHeigh);
+ wr_cmd(0x2C);
+ wr_dat_start();
+ for (j = PixelHeigh - 1; j >= 0; j--) { //Lines bottom up
+ off = j * (PixelWidth * 2 + padd) + start_data; // start of line
+ fseek(Image, off ,SEEK_SET);
+ fread(line,1,PixelWidth * 2,Image); // read a line - slow !
+ for (i = 0; i < PixelWidth; i++) { // copy pixel data to TFT
+ //_spi.write(line[i]); // 2 x 8 bit pixel
+ //_spi.write(line[i]>>8);
+ _spi.fastWrite(line[i]);
+ _spi.fastWrite(line[i]>>8);
+ }
+ }
+ _spi.clearRX();
+ wr_dat_stop();
+ free (line);
+ fclose(Image);
+ return(1);
+}
\ No newline at end of file
diff -r 000000000000 -r cf17b1f16335 ST7735_TFT.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ST7735_TFT.h Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,403 @@
+ // ST7735 8 Bit SPI Library
+
+#ifndef MBED_ST7735_TFT_H
+#define MBED_ST7735_TFT_H
+
+#include "mbed.h"
+#include "GraphicsDisplay.h"
+#include "BurstSPI.h"
+
+#define RGB(r,g,b) (((r&0xF8)<<8)|((g&0xFC)<<3)|((b&0xF8)>>3)) //5 red | 6 green | 5 blue
+
+/*define ST7735 Commands */
+
+#define ST7735_NOP 0x0
+#define ST7735_SWRESET 0x01
+#define ST7735_RDDID 0x04
+#define ST7735_RDDST 0x09
+
+#define ST7735_SLPIN 0x10
+#define ST7735_SLPOUT 0x11
+#define ST7735_PTLON 0x12
+#define ST7735_NORON 0x13
+
+#define ST7735_INVOFF 0x20
+#define ST7735_INVON 0x21
+#define ST7735_DISPOFF 0x28
+#define ST7735_DISPON 0x29
+#define ST7735_CASET 0x2A
+#define ST7735_RASET 0x2B
+#define ST7735_RAMWR 0x2C
+#define ST7735_RAMRD 0x2E
+
+#define ST7735_COLMOD 0x3A
+#define ST7735_MADCTL 0x36
+
+
+#define ST7735_FRMCTR1 0xB1
+#define ST7735_FRMCTR2 0xB2
+#define ST7735_FRMCTR3 0xB3
+#define ST7735_INVCTR 0xB4
+#define ST7735_DISSET5 0xB6
+
+#define ST7735_PWCTR1 0xC0
+#define ST7735_PWCTR2 0xC1
+#define ST7735_PWCTR3 0xC2
+#define ST7735_PWCTR4 0xC3
+#define ST7735_PWCTR5 0xC4
+#define ST7735_VMCTR1 0xC5
+
+#define ST7735_RDID1 0xDA
+#define ST7735_RDID2 0xDB
+#define ST7735_RDID3 0xDC
+#define ST7735_RDID4 0xDD
+
+#define ST7735_PWCTR6 0xFC
+
+#define ST7735_GMCTRP1 0xE0
+#define ST7735_GMCTRN1 0xE1
+
+// some RGB color definitions RED GREEN BLUE
+
+#define Black 0x0000 // 0, 0, 0
+#define Navy 0x8000 // 0, 0, 128
+#define DarkGreen 0x0400 // 0, 128, 0
+#define DarkCyan 0x8400 // 0, 128, 128
+#define Maroon 0x0010 // 128, 0, 0
+#define Purple 0x8010 // 128, 0, 128
+#define Olive 0x0410 // 128, 128, 0
+#define LightGrey 0xC618 // 192, 192, 192
+#define DarkGrey 0x7BEF // 128, 128, 128
+#define Red 0x001F // 255, 0, 0
+#define Green 0x07E0 // 0, 255, 0
+#define Yellow 0x07FF // 255, 255, 0
+#define Blue 0xF800 // 0, 0, 255
+#define Magenta 0xF81F // 255, 0, 255
+#define Cyan 0xFFE0 // 0, 255, 255
+#define White 0xFFFF // 255, 255, 255
+#define Orange 0x053F // 255, 165, 0
+#define GreenYellow 0x2FF5 // 173, 255, 47
+
+ /* Use this to Calculate and define any colour
+
+ #define RGB565CONVERT(red, green, blue) (uint16_t)( (( blue >> 3 ) << 11 ) | (( green >> 2 ) << 5 ) | ( red >> 3 ))
+ lcd.printf("%4X ",RGB565CONVERT(red value 0-255,green value 0-255,blue value 0-255));
+ */
+
+class ST7735_TFT : public GraphicsDisplay {
+ public:
+
+ /** Create a ST7735_TFT object connected to SPI and three pins. ST7735 requires rs pin to toggle between data/command
+ *
+ * @param mosi(SDA),miso(NC),sclk(SCK) SPI
+ * @param cs pin connected to CS of display (called SS for 'Slave Select' in ST7735 datasheet)
+ * @param rs pin connected to RS (A0)of display (called D/CX in ST7735 datasheet)
+ * @param reset pin connected to RESET of display
+ *
+ */
+ ST7735_TFT(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName rs, PinName reset,const char* name ="TFT");
+
+ /** Get the width of the screen in pixel
+ *
+ * @param
+ * @returns width of screen in pixel
+ *
+ */
+ virtual int width();
+
+ /** Get the height of the screen in pixel
+ *
+ * @returns height of screen in pixel
+ *
+ */
+ virtual int height();
+
+ /** Draw a pixel at x,y with color
+ *
+ * @param x horizontal position
+ * @param y vertical position
+ * @param color 16 bit pixel color
+ */
+ virtual void pixel(int x, int y, int colour);
+
+ /** Get colour of pixel at x,y
+ *
+ * @param x horizontal position
+ * @param y vertical position
+ */
+
+ int getpixel(unsigned int x, unsigned int y);
+
+ /** draw a circle
+ *
+ * @param x0,y0 center
+ * @param r radius
+ * @param color 16 bit color *
+ *
+ */
+
+ void circle(int x, int y, int r, int colour);
+
+ /** draw a filled circle
+ *
+ * @param x0,y0 center
+ * @param r radius
+ * @param color 16 bit color *
+ *
+ * use circle with different radius,
+ * can miss some pixel
+ */
+ void fillcircle(int x, int y, int r, int colour);
+
+ /** draw a 1 pixel line
+ *
+ * @param x0,y0 start point
+ * @param x1,y1 stop point
+ * @param color 16 bit color
+ *
+ */
+ void line(int x0, int y0, int x1, int y1, int colour);
+
+ /** draw a rect
+ *
+ * @param x0,y0 top left corner
+ * @param x1,y1 down right corner
+ * @param color 16 bit color
+ * *
+ */
+ void rect(int x0, int y0, int x1, int y1, int colour);
+
+ /** draw a filled rect
+ *
+ * @param x0,y0 top left corner
+ * @param x1,y1 down right corner
+ * @param color 16 bit color
+ *
+ */
+ void fillrect(int x0, int y0, int x1, int y1, int colour);
+
+ /** setup cursor position
+ *
+ * @param x x-position (top left)
+ * @param y y-position
+ */
+ virtual void locate(int x, int y);
+
+ /** Fill the screen with _backgroun color
+ *
+ */
+ virtual void cls (void);
+
+
+ /** calculate the max number of char in a line
+ *
+ * @returns max columns
+ * depends on actual font size
+ *
+ */
+ virtual int columns(void);
+
+ /** calculate the max number of columns
+ *
+ * @returns max column
+ * depends on actual font size
+ *
+ */
+ virtual int rows(void);
+
+ /** put a char on the screen
+ *
+ * @param value char to print
+ * @returns printed char
+ *
+ */
+ virtual int _putc(int value);
+
+ /** draw a character on given position out of the active font to the TFT
+ *
+ * @param x x-position of char (top left)
+ * @param y y-position
+ * @param c char to print
+ *
+ */
+ virtual void character(int x, int y, int c);
+
+ /** paint a bitmap on the TFT
+ *
+ * @param x,y : upper left corner
+ * @param w width of bitmap
+ * @param h high of bitmap
+ * @param *bitmap pointer to the bitmap data
+ *
+ * bitmap format: 16 bit R5 G6 B5
+ *
+ * use Gimp to create / load , save as BMP, option 16 bit R5 G6 B5
+ * use winhex to load this file and mark data stating at offset 0x46 to end
+ * use edit -> copy block -> C Source to export C array
+ * paste this array into your program
+ *
+ * define the array as static const unsigned char to put it into flash memory
+ * cast the pointer to (unsigned char *) :
+ * tft.Bitmap(10,40,309,50,(unsigned char *)scala);
+ */
+ void Bitmap(unsigned int x, unsigned int y, unsigned int w, unsigned int h,unsigned char *bitmap);
+
+
+ /** paint a 16 bit BMP from local filesytem on the TFT (slow)
+ *
+ * @param x,y : upper left corner
+ * @param *Name_BMP name of the BMP file
+ * @returns 1 if bmp file was found and painted
+ * @returns -1 if bmp file was found not found
+ * @returns -2 if bmp file is not 16bit
+ * @returns -3 if bmp file is to big for screen
+ * @returns -4 if buffer malloc go wrong
+ *
+ * bitmap format: 16 bit R5 G6 B5
+ *
+ * use Gimp to create / load , save as BMP, option 16 bit R5 G6 B5
+ * copy to internal file system
+ *
+ */
+
+ int BMP_16(unsigned int x, unsigned int y, const char *Name_BMP);
+
+ /** Read an area from the LCD RAM to MCU RAM
+ *
+ * @param x,y : upper left corner
+ * @param w width of bitmap
+ * @param h high of bitmap
+ * @param *buffer pointer to the buffer
+ */
+
+ void read_area(unsigned int x, unsigned int y, unsigned int w, unsigned int h,unsigned char *buffer);
+
+ /** select the font to use
+ *
+ * @param f pointer to font array
+ *
+ * font array can created with GLCD Font Creator from http://www.mikroe.com
+ * you have to add 4 parameter at the beginning of the font array to use:
+ * - the number of byte / char
+ * - the vertial size in pixel
+ * - the horizontal size in pixel
+ * - the number of byte per vertical line
+ * you also have to change the array to char[]
+ *
+ */
+ void set_font(unsigned char* f);
+
+ /** Set the orientation of the screen
+ * x,y: 0,0 is always top left
+ *
+ * @param o direction to use the screen (0-3) 90� Steps
+ *
+ */
+ void set_orientation(unsigned int o);
+
+ //SPI _spi;
+ BurstSPI _spi;
+ DigitalOut _cs;
+ DigitalOut _rs;
+ DigitalOut _reset;
+ unsigned char* font;
+
+//protected:
+
+ /** draw a horizontal line
+ *
+ * @param x0 horizontal start
+ * @param x1 horizontal stop
+ * @param y vertical position
+ * @param color 16 bit color
+ *
+ */
+ void hline(int x0, int x1, int y, int colour);
+
+ /** draw a vertical line
+ *
+ * @param x horizontal position
+ * @param y0 vertical start
+ * @param y1 vertical stop
+ * @param color 16 bit color
+ */
+ void vline(int y0, int y1, int x, int colour);
+
+ /** Set draw window region
+ *
+ * @param x horizontal position
+ * @param y vertical position
+ * @param w window width in pixel
+ * @param h window height in pixels
+ */
+protected:
+ void window (unsigned int x, unsigned int y, unsigned int w, unsigned int h);
+
+ /** Set draw window region to whole screen
+ *
+ */
+ void WindowMax (void);
+
+ /** Init the ST7735 controller
+ *
+ */
+
+ void tft_reset();
+
+ /** Write data to the LCD controller
+ *
+ * @param dat data written to LCD controller
+ *
+ */
+ void wr_dat(int value);
+
+ /** Write a command the LCD controller
+ *
+ * @param cmd: command to be written
+ *
+ */
+ void wr_cmd(int value);
+
+ /** Start data sequence to the LCD controller
+ *
+ */
+ void wr_dat_start();
+
+ /** Stop of data writing to the LCD controller
+ *
+ */
+ void wr_dat_stop();
+
+ /** write data to the LCD controller
+ *
+ * @param data to be written
+ * *
+ */
+ void wr_dat_only(unsigned short dat);
+
+ /** Read data from the LCD controller
+ *
+ * @returns data from LCD controller
+ *
+ */
+ unsigned short rd_dat(void);
+
+ /** Write a value to the to a LCD register
+ *
+ * @param reg register to be written
+ * @param val data to be written
+ */
+ void wr_reg (unsigned char reg, unsigned short val);
+
+ /** Read a LCD register
+ *
+ * @param reg register to be read
+ * @returns value of the register
+ */
+ unsigned short rd_reg (unsigned char reg);
+
+ unsigned int orientation;
+ unsigned int char_x;
+ unsigned int char_y;
+};
+#endif
diff -r 000000000000 -r cf17b1f16335 TextDisplay.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TextDisplay.cpp Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,67 @@
+ // ST7735 8 Bit SPI Library
+
+#include "TextDisplay.h"
+
+TextDisplay::TextDisplay(const char *name) : Stream(name){
+ _row = 0;
+ _column = 0;
+ if (name == NULL) {
+ _path = NULL;
+ } else {
+ _path = new char[strlen(name) + 2];
+ sprintf(_path, "/%s", name);
+ }
+}
+
+int TextDisplay::_putc(int value) {
+ if(value == '\n') {
+ _column = 0;
+ _row++;
+ if(_row >= rows()) {
+ _row = 0;
+ }
+ } else {
+ character(_column, _row, value);
+ _column++;
+ if(_column >= columns()) {
+ _column = 0;
+ _row++;
+ if(_row >= rows()) {
+ _row = 0;
+ }
+ }
+ }
+ return value;
+}
+void TextDisplay::cls() {
+ locate(0, 0);
+ for(int i=0; i<columns()*rows(); i++) {
+ putc(' ');
+ }
+}
+void TextDisplay::locate(int column, int row) {
+ _column = column;
+ _row = row;
+}
+int TextDisplay::_getc() {
+ return -1;
+}
+void TextDisplay::foreground(uint16_t colour) {
+ _foreground = colour;
+}
+void TextDisplay::background(uint16_t colour) {
+ _background = colour;
+}
+bool TextDisplay::claim (FILE *stream) {
+ if ( _path == NULL) {
+ fprintf(stderr, "claim requires a name to be given in the instantioator of the TextDisplay instance!\r\n");
+ return false;
+ }
+ if (freopen(_path, "w", stream) == NULL) {
+ // Failed, should not happen
+ return false;
+ }
+ // make sure we use line buffering
+ setvbuf(stdout, NULL, _IOLBF, columns());
+ return true;
+}
diff -r 000000000000 -r cf17b1f16335 TextDisplay.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TextDisplay.h Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,67 @@
+ // ST7735 8 Bit SPI Library
+
+#ifndef MBED_TEXTDISPLAY_H
+#define MBED_TEXTDISPLAY_H
+
+#include "mbed.h"
+
+class TextDisplay : public Stream {
+public:
+
+ // functions needing implementation in derived implementation class
+ /** Create a TextDisplay interface
+ *
+ * @param name The name used in the path to access the strean through the filesystem
+ */
+ TextDisplay(const char *name = NULL);
+
+ /** output a character at the given position
+ *
+ * @param column column where charater must be written
+ * @param row where character must be written
+ * @param c the character to be written to the TextDisplay
+ */
+ virtual void character(int column, int row, int c) = 0;
+
+ /** return number if rows on TextDisplay
+ * @result number of rows
+ */
+ virtual int rows() = 0;
+
+ /** return number if columns on TextDisplay
+ * @result number of rows
+ */
+ virtual int columns() = 0;
+
+ // functions that come for free, but can be overwritten
+
+ /** redirect output from a stream (stoud, sterr) to display
+ * @param stream stream that shall be redirected to the TextDisplay
+ */
+ virtual bool claim (FILE *stream);
+
+ /** clear screen
+ */
+ virtual void cls();
+ virtual void locate(int column, int row);
+ virtual void foreground(uint16_t colour);
+ virtual void background(uint16_t colour);
+ // putc (from Stream)
+ // printf (from Stream)
+
+protected:
+
+ virtual int _putc(int value);
+ virtual int _getc();
+
+ // character location
+ uint16_t _column;
+ uint16_t _row;
+
+ // colours
+ uint16_t _foreground;
+ uint16_t _background;
+ char *_path;
+};
+
+#endif
diff -r 000000000000 -r cf17b1f16335 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,54 @@
+#include "mbed.h"
+#include "ST7735_TFT.h"
+#include "Arial24x23i.h"
+#include "Arial11x11.h"
+#include "Arial9x9.h"
+#include "MPU6050.h"
+#include "MMA8451Q.h"
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+Serial pc(USBTX, USBRX); // tx, rx default baud rate: 9600
+
+void compFilter();
+void preparePeriferals();
+
+MPU6050 mpu6050; // class: MPU6050, object: mpu6050
+PinName const SDA = PTE25;
+PinName const SCL = PTE24;
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); //on-board accelerometer
+//ST7735_TFT lcd(PTD6, NC, PTD5, PTA13, PTD2, PTD4, "TFT"); // TFT; sda, miso (not connected), sck, cs, AO(rs), reset
+Ticker systick;
+
+float pitchAngle = 0;
+float rollAngle = 0;
+float rollX = 0;
+float pitchY = 0;
+int main()
+{
+
+ pc.baud(9600); // baud rate: 9600
+ mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
+ wait(1);
+ mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
+ pc.printf("Calibration is completed. \r\n");
+ wait(0.5);
+ mpu6050.init(); // Initialize the sensor
+ wait(1);
+ pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+ wait_ms(500);
+ systick.attach(&compFilter, 0.005); // calls the complementaryFilter func. every 5 ms (200 Hz sampling period)
+ while (true)
+ {
+ atan (ax);
+ //pc.printf("Accelerometer (onboard) X = %1.2f, Y = %1.2f, Z = %1.2f\r\n", acc.getAccX(), acc.getAccY(), acc.getAccZ());
+ pc.printf("Accelerometer MPU6050(g) X = %.3f, Y = %.3f, Z = %.3f\r\n", ax, ay, az);
+ pc.printf("Gyroscope MPU6050(deg/s) gx = %.3f, gy = %.3f, gz = %.3f\r\n", gx, gy, gz);
+ pc.printf("Gyroscope MPU6050(deg/s) roll = %.3f, pitch = %.3f\r\n",rollAngle, pitchAngle);
+ wait(1.0f);
+
+ }
+}
+void compFilter() {
+ mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
+}
diff -r 000000000000 -r cf17b1f16335 mbed-os.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Tue Oct 24 20:12:54 2017 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#6e0d01cd13e8aca7bf4d697c3699ec9225386881
diff -r 000000000000 -r cf17b1f16335 readme.txt
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/readme.txt Tue Oct 24 20:12:54 2017 +0000
@@ -0,0 +1,11 @@
+In this project I attempted to create a backup avionics device ("Artificial horizon") based on a microcontroller (Freedom board FRDM-KL46Z), a Motion Trackig device that combines a 3-axis gyroscope, 3-axis accelerometer, and a Digital Motion Processor (InvenSense MPU-6050), and a simple 1.8-inches LCD monitor with SPI interface.
+The code is written in C.
+The raw data has been read from MPU-6050 through I2C interface, processed with complementary filter, and then displayed on an LCD through SPI interface. The GUI of the project imitates the behavior of a real "Artificial horizon" device with respect to the roll and pitch angles of an aircraft.
+
+Used libraries and brief description:
+MMA8451Q - used for communication with the on-board accelerometer (on FRDM-KL46Z)
+MPU6050, MPU6050RegDef - used for communication with the MPU-6050 board
+ST7735, GraphicsDisplay, TextDisplay - used for comminication with the LCD
+
+More information on libraries can be found on mbed.org. I'm very grateful to all people who contributed to the development of these libraries and shared them to the public.
+