diff --git a/esp32/libraries/FreematicsOLED/FreematicsOLED.cpp b/esp32/libraries/FreematicsOLED/FreematicsOLED.cpp new file mode 100644 index 0000000..7593799 --- /dev/null +++ b/esp32/libraries/FreematicsOLED/FreematicsOLED.cpp @@ -0,0 +1,787 @@ +/************************************************************************* +* Simple OLED Text & Bitmap Display Library for Arduino +* Distributed under BSD +* (C)2013-2018 Stanley Huang +*************************************************************************/ + +#include +#include +#include "FreematicsOLED.h" + +#define I2C_ADDR 0x78 >> 1 + +// fonts data +const unsigned char digits16x24[][48] = { +{0x00,0x00,0x00,0xF0,0xFF,0x0F,0xFC,0xFF,0x3F,0xFE,0xFF,0x7F,0xFE,0xFF,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x07,0x00,0xE0,0x07,0x00,0xE0,0x07,0x00,0xE0,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFF,0x7F,0xFE,0xFF,0x7F,0xFC,0xFF,0x3F,0xF0,0xFF,0x0F},/*"0",0*/ +{0x00,0x00,0x00,0x70,0x00,0x00,0x70,0x00,0x00,0x70,0x00,0x00,0x78,0x00,0x00,0xF8,0x00,0x00,0xFC,0xFF,0xFF,0xFE,0xFF,0xFF,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"1",0*/ +{0x00,0x00,0x00,0xF8,0x00,0xE0,0xFC,0x00,0xF8,0xFE,0x00,0xFE,0xFE,0x80,0xFF,0xFF,0xC0,0xFF,0x07,0xF0,0xFF,0x07,0xFC,0xFF,0x07,0xFF,0xEF,0xFF,0xFF,0xE3,0xFF,0xFF,0xE1,0xFE,0x7F,0xE0,0xFE,0x3F,0xE0,0xFC,0x0F,0xE0,0xF0,0x03,0x00,0x00,0x00,0x00},/*"2",2*/ +{0x00,0x00,0x00,0xF8,0x80,0x1F,0xFE,0x80,0x3F,0xFE,0x80,0x7F,0xFF,0x80,0x7F,0xFF,0x80,0xFF,0xFF,0x9C,0xFF,0xFF,0x9C,0xFF,0x07,0x1C,0xE0,0x07,0x3E,0xE0,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFF,0x7F,0xFE,0xF7,0x7F,0xFC,0xF7,0x3F,0xF0,0xE3,0x1F},/*"3",3*/ +{0x00,0xF0,0x0F,0x00,0xFE,0x0F,0x80,0xFF,0x0F,0xE0,0xFF,0x0F,0xFC,0xBF,0x0F,0xFF,0x87,0x0F,0xFF,0x81,0x0F,0x3F,0x80,0x0F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x80,0x0F,0x00,0x80,0x0F},/*"4",4*/ +{0x00,0x00,0x00,0xFF,0xC7,0x0F,0xFF,0xC7,0x3F,0xFF,0xC7,0x7F,0xFF,0xC7,0x7F,0xFF,0xC7,0xFF,0xFF,0xC7,0xFF,0x87,0x01,0xE0,0xC7,0x01,0xE0,0xC7,0x01,0xE0,0xC7,0xFF,0xFF,0xC7,0xFF,0xFF,0xC7,0xFF,0x7F,0x87,0xFF,0x7F,0x87,0xFF,0x3F,0x07,0xFE,0x1F},/*"5",5*/ +{0x00,0x00,0x00,0xF0,0xFF,0x0F,0xFC,0xFF,0x3F,0xFE,0xFF,0x7F,0xFE,0xFF,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x07,0x06,0xE0,0x07,0x07,0xE0,0x07,0x07,0xE0,0x3F,0xFF,0xFF,0x3F,0xFF,0xFF,0x3E,0xFF,0x7F,0x3E,0xFE,0x7F,0x3C,0xFE,0x3F,0x38,0xF8,0x1F},/*"6",6*/ +{0x00,0x00,0x00,0x07,0x00,0x00,0x07,0x00,0x00,0x07,0x00,0xC0,0x07,0x00,0xF8,0x07,0x00,0xFF,0x07,0xE0,0xFF,0x07,0xFE,0xFF,0xC7,0xFF,0xFF,0xFF,0xFF,0x3F,0xFF,0xFF,0x07,0xFF,0xFF,0x00,0xFF,0x0F,0x00,0xFF,0x01,0x00,0x1F,0x00,0x00,0x00,0x00,0x00},/*"7",1*/ +{0x00,0x00,0x00,0xF0,0xE3,0x1F,0xFC,0xF7,0x3F,0xFE,0xFF,0x7F,0xFE,0xFF,0x7F,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x07,0x1C,0xE0,0x07,0x1C,0xE0,0x07,0x1C,0xE0,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFF,0x7F,0xFE,0xF7,0x7F,0xFC,0xF7,0x3F,0xF0,0xE3,0x1F},/*"8",8*/ +{0x00,0x00,0x00,0xF8,0x1F,0x1C,0xFC,0x7F,0x3C,0xFE,0x7F,0x7C,0xFE,0xFF,0x7C,0xFF,0xFF,0xFC,0xFF,0xFF,0xFC,0x07,0xE0,0xE0,0x07,0xE0,0xE0,0x07,0x60,0xE0,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xFF,0x7F,0xFE,0xFF,0x7F,0xFC,0xFF,0x3F,0xF0,0xFF,0x0F},/*"9",9*/ +}; + +const unsigned char digits8x8[][8] = { +{0x3C,0x7E,0x83,0x81,0x81,0x7E,0x3C,0x00},/*0*/ +{0x84,0x84,0x82,0xFF,0xFF,0x80,0x80,0x00},/*1*/ +{0x84,0xC6,0xE1,0xA1,0xB1,0x9F,0x8E,0x00},/*2*/ +{0x42,0xC3,0x81,0x89,0x89,0xFF,0x76,0x00},/*3*/ +{0x20,0x38,0x24,0x22,0xFF,0xFF,0x20,0x00},/*4*/ +{0x5F,0xDF,0x99,0x89,0x89,0xF9,0x70,0x00},/*5*/ +{0x3C,0x7E,0x89,0x89,0x89,0xFB,0x72,0x00},/*6*/ +{0x01,0x01,0xE1,0xF9,0x1D,0x07,0x01,0x00},/*7*/ +{0x6E,0xFF,0x89,0x89,0x99,0xFF,0x76,0x00},/*8*/ +{0x4E,0xDF,0x91,0x91,0x91,0x7F,0x3E,0x00},/*9*/ +}; + +// The 7-bit ASCII character set... +const unsigned char font5x8[][5] = { + { 0x00, 0x00, 0x5f, 0x00, 0x00 }, // 21 ! + { 0x00, 0x07, 0x00, 0x07, 0x00 }, // 22 " + { 0x14, 0x7f, 0x14, 0x7f, 0x14 }, // 23 # + { 0x24, 0x2a, 0x7f, 0x2a, 0x12 }, // 24 $ + { 0x23, 0x13, 0x08, 0x64, 0x62 }, // 25 % + { 0x36, 0x49, 0x55, 0x22, 0x50 }, // 26 & + { 0x00, 0x05, 0x03, 0x00, 0x00 }, // 27 ' + { 0x00, 0x1c, 0x22, 0x41, 0x00 }, // 28 ( + { 0x00, 0x41, 0x22, 0x1c, 0x00 }, // 29 ) + { 0x14, 0x08, 0x3e, 0x08, 0x14 }, // 2a * + { 0x08, 0x08, 0x3e, 0x08, 0x08 }, // 2b + + { 0x00, 0x50, 0x30, 0x00, 0x00 }, // 2c , + { 0x08, 0x08, 0x08, 0x08, 0x08 }, // 2d - + { 0x00, 0x60, 0x60, 0x00, 0x00 }, // 2e . + { 0x20, 0x10, 0x08, 0x04, 0x02 }, // 2f / + { 0x3e, 0x51, 0x49, 0x45, 0x3e }, // 30 0 + { 0x00, 0x42, 0x7f, 0x40, 0x00 }, // 31 1 + { 0x42, 0x61, 0x51, 0x49, 0x46 }, // 32 2 + { 0x21, 0x41, 0x45, 0x4b, 0x31 }, // 33 3 + { 0x18, 0x14, 0x12, 0x7f, 0x10 }, // 34 4 + { 0x27, 0x45, 0x45, 0x45, 0x39 }, // 35 5 + { 0x3c, 0x4a, 0x49, 0x49, 0x30 }, // 36 6 + { 0x01, 0x71, 0x09, 0x05, 0x03 }, // 37 7 + { 0x36, 0x49, 0x49, 0x49, 0x36 }, // 38 8 + { 0x06, 0x49, 0x49, 0x29, 0x1e }, // 39 9 + { 0x00, 0x36, 0x36, 0x00, 0x00 }, // 3a : + { 0x00, 0x56, 0x36, 0x00, 0x00 }, // 3b ; + { 0x08, 0x14, 0x22, 0x41, 0x00 }, // 3c < + { 0x14, 0x14, 0x14, 0x14, 0x14 }, // 3d = + { 0x00, 0x41, 0x22, 0x14, 0x08 }, // 3e > + { 0x02, 0x01, 0x51, 0x09, 0x06 }, // 3f ? + { 0x32, 0x49, 0x79, 0x41, 0x3e }, // 40 @ + { 0x7e, 0x11, 0x11, 0x11, 0x7e }, // 41 A + { 0x7f, 0x49, 0x49, 0x49, 0x36 }, // 42 B + { 0x3e, 0x41, 0x41, 0x41, 0x22 }, // 43 C + { 0x7f, 0x41, 0x41, 0x22, 0x1c }, // 44 D + { 0x7f, 0x49, 0x49, 0x49, 0x41 }, // 45 E + { 0x7f, 0x09, 0x09, 0x09, 0x01 }, // 46 F + { 0x3e, 0x41, 0x49, 0x49, 0x7a }, // 47 G + { 0x7f, 0x08, 0x08, 0x08, 0x7f }, // 48 H + { 0x00, 0x41, 0x7f, 0x41, 0x00 }, // 49 I + { 0x20, 0x40, 0x41, 0x3f, 0x01 }, // 4a J + { 0x7f, 0x08, 0x14, 0x22, 0x41 }, // 4b K + { 0x7f, 0x40, 0x40, 0x40, 0x40 }, // 4c L + { 0x7f, 0x02, 0x0c, 0x02, 0x7f }, // 4d M + { 0x7f, 0x04, 0x08, 0x10, 0x7f }, // 4e N + { 0x3e, 0x41, 0x41, 0x41, 0x3e }, // 4f O + { 0x7f, 0x09, 0x09, 0x09, 0x06 }, // 50 P + { 0x3e, 0x41, 0x51, 0x21, 0x5e }, // 51 Q + { 0x7f, 0x09, 0x19, 0x29, 0x46 }, // 52 R + { 0x46, 0x49, 0x49, 0x49, 0x31 }, // 53 S + { 0x01, 0x01, 0x7f, 0x01, 0x01 }, // 54 T + { 0x3f, 0x40, 0x40, 0x40, 0x3f }, // 55 U + { 0x1f, 0x20, 0x40, 0x20, 0x1f }, // 56 V + { 0x3f, 0x40, 0x38, 0x40, 0x3f }, // 57 W + { 0x63, 0x14, 0x08, 0x14, 0x63 }, // 58 X + { 0x07, 0x08, 0x70, 0x08, 0x07 }, // 59 Y + { 0x61, 0x51, 0x49, 0x45, 0x43 }, // 5a Z + { 0x00, 0x7f, 0x41, 0x41, 0x00 }, // 5b [ + { 0x02, 0x04, 0x08, 0x10, 0x20 }, // 5c backslash + { 0x00, 0x41, 0x41, 0x7f, 0x00 }, // 5d ] + { 0x04, 0x02, 0x01, 0x02, 0x04 }, // 5e ^ + { 0x40, 0x40, 0x40, 0x40, 0x40 }, // 5f _ + { 0x00, 0x01, 0x02, 0x04, 0x00 }, // 60 ` + { 0x20, 0x54, 0x54, 0x54, 0x78 }, // 61 a + { 0x7f, 0x48, 0x44, 0x44, 0x38 }, // 62 b + { 0x38, 0x44, 0x44, 0x44, 0x20 }, // 63 c + { 0x38, 0x44, 0x44, 0x48, 0x7f }, // 64 d + { 0x38, 0x54, 0x54, 0x54, 0x18 }, // 65 e + { 0x08, 0x7e, 0x09, 0x01, 0x02 }, // 66 f + { 0x0c, 0x52, 0x52, 0x52, 0x3e }, // 67 g + { 0x7f, 0x08, 0x04, 0x04, 0x78 }, // 68 h + { 0x00, 0x44, 0x7d, 0x40, 0x00 }, // 69 i + { 0x20, 0x40, 0x44, 0x3d, 0x00 }, // 6a j + { 0x7f, 0x10, 0x28, 0x44, 0x00 }, // 6b k + { 0x00, 0x41, 0x7f, 0x40, 0x00 }, // 6c l + { 0x7c, 0x04, 0x18, 0x04, 0x78 }, // 6d m + { 0x7c, 0x08, 0x04, 0x04, 0x78 }, // 6e n + { 0x38, 0x44, 0x44, 0x44, 0x38 }, // 6f o + { 0x7c, 0x14, 0x14, 0x14, 0x08 }, // 70 p + { 0x08, 0x14, 0x14, 0x18, 0x7c }, // 71 q + { 0x7c, 0x08, 0x04, 0x04, 0x08 }, // 72 r + { 0x48, 0x54, 0x54, 0x54, 0x20 }, // 73 s + { 0x04, 0x3f, 0x44, 0x40, 0x20 }, // 74 t + { 0x3c, 0x40, 0x40, 0x20, 0x7c }, // 75 u + { 0x1c, 0x20, 0x40, 0x20, 0x1c }, // 76 v + { 0x3c, 0x40, 0x30, 0x40, 0x3c }, // 77 w + { 0x44, 0x28, 0x10, 0x28, 0x44 }, // 78 x + { 0x0c, 0x50, 0x50, 0x50, 0x3c }, // 79 y + { 0x44, 0x64, 0x54, 0x4c, 0x44 }, // 7a z + { 0x00, 0x08, 0x36, 0x41, 0x00 }, // 7b { + { 0x00, 0x00, 0x7f, 0x00, 0x00 }, // 7c | + { 0x00, 0x41, 0x36, 0x08, 0x00 }, // 7d } + { 0x10, 0x08, 0x08, 0x10, 0x08 }, // 7e ~ +}; + +#ifndef MEMORY_SAVING +const unsigned char digits16x16[][32] = { +{0x00,0xE0,0xF8,0xFC,0xFE,0x1E,0x07,0x07,0x07,0x07,0x1E,0xFE,0xFC,0xF8,0xF0,0x00,0x00,0x07,0x0F,0x3F,0x3F,0x7C,0x70,0x70,0x70,0x70,0x7C,0x3F,0x1F,0x1F,0x07,0x00},/*0*/ +{0x00,0x00,0x00,0x06,0x07,0x07,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0x7F,0x7F,0x7F,0x00,0x00,0x00,0x00,0x00,0x00},/*1*/ +{0x00,0x38,0x3C,0x3E,0x3E,0x0F,0x07,0x07,0x07,0xCF,0xFF,0xFE,0xFE,0x38,0x00,0x00,0x00,0x40,0x40,0x60,0x70,0x78,0x7C,0x7E,0x7F,0x77,0x73,0x71,0x70,0x70,0x00,0x00},/*2*/ +{0x00,0x18,0x1C,0x1E,0x1E,0x0F,0xC7,0xC7,0xE7,0xFF,0xFE,0xBE,0x9C,0x00,0x00,0x00,0x00,0x0C,0x1C,0x3C,0x3C,0x78,0x70,0x70,0x70,0x79,0x7F,0x3F,0x1F,0x0F,0x00,0x00},/*3*/ +{0x00,0x00,0x80,0xC0,0xE0,0x70,0x38,0x1C,0x1E,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x06,0x07,0x07,0x07,0x06,0x06,0x06,0x06,0x06,0x7F,0x7F,0x7F,0x7F,0x06,0x06,0x00},/*4*/ +{0x00,0x00,0x00,0x00,0xF0,0xFF,0xFF,0xFF,0xE7,0xE7,0xE7,0xE7,0xC7,0x87,0x00,0x00,0x00,0x00,0x38,0x78,0x71,0x70,0x70,0x70,0x70,0x70,0x39,0x3F,0x3F,0x1F,0x0F,0x00},/*5*/ +{0x00,0x80,0xE0,0xF0,0xF8,0xFC,0x7F,0x7F,0x6F,0x67,0xE1,0xE1,0xC0,0x80,0x00,0x00,0x00,0x0F,0x1F,0x3F,0x3F,0x78,0x70,0x70,0x70,0x70,0x78,0x3F,0x3F,0x1F,0x0F,0x00},/*6*/ +{0x00,0x07,0x07,0x07,0x07,0x07,0xC7,0xE7,0xF7,0xFF,0x7F,0x3F,0x1F,0x07,0x03,0x01,0x00,0x20,0x38,0x7C,0x7E,0x3F,0x0F,0x07,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*7*/ +{0x00,0x00,0x00,0x1C,0xBE,0xFE,0xFF,0xE7,0xC3,0xC3,0xE7,0xFF,0xFE,0xBE,0x1C,0x00,0x00,0x00,0x0E,0x3F,0x3F,0x7F,0x71,0x60,0x60,0x60,0x71,0x7F,0x3F,0x3F,0x0F,0x00},/*8*/ +{0x00,0x78,0xFC,0xFE,0xFE,0x8F,0x07,0x07,0x07,0x07,0x8F,0xFE,0xFE,0xFC,0xF8,0x00,0x00,0x00,0x00,0x01,0x43,0x43,0x73,0x7B,0x7F,0x7F,0x1F,0x0F,0x07,0x03,0x00,0x00},/*9*/ +}; + +const unsigned char font8x16_doslike[][16] = { +{0x00,0x00,0x00,0x00,0x78,0x00,0xFC,0x09,0xFC,0x09,0x78,0x00,0x00,0x00,0x00,0x00},/*"!*/ +{0x00,0x00,0x0E,0x00,0x1E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1E,0x00,0x0E,0x00},/*""*/ +{0x20,0x02,0xF8,0x1F,0xF8,0x1F,0x20,0x02,0x20,0x02,0xF8,0x1F,0xF8,0x1F,0x20,0x02},/*"#*/ +{0x00,0x00,0x38,0x06,0x7C,0x0C,0x44,0x08,0xFF,0x3F,0x47,0x38,0xCC,0x0F,0x98,0x07},/*"$*/ +{0x10,0x08,0x38,0x0C,0x28,0x06,0x38,0x03,0x90,0x05,0xC0,0x0E,0x60,0x0A,0x30,0x0E},/*"%*/ +{0x80,0x07,0xD8,0x0F,0x7C,0x08,0x64,0x08,0xE4,0x08,0xBC,0x07,0x18,0x0F,0x80,0x09},/*"&*/ +{0x00,0x00,0x10,0x00,0x1C,0x00,0x0C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"'*/ +{0x00,0x00,0x00,0x00,0xF0,0x03,0xF8,0x07,0x0C,0x0C,0x04,0x08,0x00,0x00,0x00,0x00},/*"(*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x04,0x08,0x0C,0x0C,0xF8,0x07,0xF0,0x03,0x00,0x00},/*")*/ +{0x80,0x00,0xA0,0x02,0xE0,0x03,0xC0,0x01,0xC0,0x01,0xE0,0x03,0xA0,0x02,0x80,0x00},/*"**/ +{0x80,0x00,0x80,0x00,0x80,0x00,0xE0,0x03,0xE0,0x03,0x80,0x00,0x80,0x00,0x80,0x00},/*"+0*/ +{0x00,0x00,0x00,0x00,0x00,0x20,0x00,0x3C,0x00,0x1C,0x00,0x00,0x00,0x00,0x00,0x00},/*",*/ +{0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00},/*"-*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x0C,0x00,0x0C,0x00,0x00,0x00,0x00,0x00,0x00},/*".*/ +{0x00,0x08,0x00,0x0C,0x00,0x06,0x00,0x03,0x80,0x01,0xC0,0x00,0x60,0x00,0x30,0x00},/*"/*/ +{0xF0,0x03,0xF8,0x07,0x0C,0x0C,0xC4,0x08,0xC4,0x08,0x0C,0x0C,0xF8,0x07,0xF0,0x03},/*"0*/ +{0x00,0x00,0x10,0x08,0x18,0x08,0xFC,0x0F,0xFC,0x0F,0x00,0x08,0x00,0x08,0x00,0x00},/*"1*/ +{0x08,0x0E,0x0C,0x0F,0x84,0x09,0xC4,0x08,0x64,0x08,0x3C,0x0C,0x18,0x0C,0x00,0x00},/*"2*/ +{0x08,0x04,0x0C,0x0C,0x04,0x08,0x44,0x08,0x44,0x08,0xFC,0x0F,0xB8,0x07,0x00,0x00},/*"3*/ +{0xC0,0x01,0xE0,0x01,0x30,0x01,0x18,0x09,0xFC,0x0F,0xFC,0x0F,0x00,0x09,0x00,0x00},/*"4*/ +{0x7C,0x04,0x7C,0x0C,0x44,0x08,0x44,0x08,0x44,0x08,0xC4,0x0F,0x84,0x07,0x00,0x00},/*"5*/ +{0xF0,0x07,0xF8,0x0F,0x4C,0x08,0x44,0x08,0x44,0x08,0xC4,0x0F,0x80,0x07,0x00,0x00},/*"6*/ +{0x0C,0x00,0x0C,0x00,0x84,0x0F,0xC4,0x0F,0x64,0x00,0x3C,0x00,0x1C,0x00,0x00,0x00},/*"7*/ +{0xB8,0x07,0xFC,0x0F,0x44,0x08,0x44,0x08,0x44,0x08,0xFC,0x0F,0xB8,0x07,0x00,0x00},/*"8*/ +{0x38,0x00,0x7C,0x08,0x44,0x08,0x44,0x08,0x44,0x0C,0xFC,0x07,0xF8,0x03,0x00,0x00},/*"9*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x06,0x30,0x06,0x00,0x00,0x00,0x00,0x00,0x00},/*":*/ +{0x00,0x00,0x00,0x00,0x00,0x10,0x60,0x1C,0x60,0x0C,0x00,0x00,0x00,0x00,0x00,0x00},/*";",27*/ +{0x00,0x00,0x80,0x00,0xC0,0x01,0x60,0x03,0x30,0x06,0x18,0x0C,0x08,0x08,0x00,0x00},/*"<",28*/ +{0x20,0x01,0x20,0x01,0x20,0x01,0x20,0x01,0x20,0x01,0x20,0x01,0x20,0x01,0x00,0x00},/*"=",29*/ +{0x00,0x00,0x00,0x00,0x08,0x08,0x18,0x0C,0x30,0x06,0x60,0x03,0xC0,0x01,0x80,0x00},/*">",30*/ +{0x08,0x00,0x0C,0x00,0x04,0x00,0x84,0x0D,0xC4,0x0D,0x7C,0x00,0x38,0x00,0x00,0x00},/*"?",31*/ +{0xF8,0x07,0xFC,0x0F,0x04,0x08,0x84,0x09,0xC4,0x09,0xFC,0x09,0xF8,0x00,0x00,0x00},/*"@",32*/ +{0xE0,0x0F,0xF0,0x0F,0x98,0x00,0x8C,0x00,0x98,0x00,0xF0,0x0F,0xE0,0x0F,0x00,0x00},/*"A",33*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x44,0x08,0x44,0x08,0xFC,0x0F,0xB8,0x07,0x00,0x00},/*"B",34*/ +{0xF0,0x03,0xF8,0x07,0x0C,0x0C,0x04,0x08,0x04,0x08,0x0C,0x0C,0x18,0x06,0x00,0x00},/*"C",35*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x04,0x08,0x0C,0x0C,0xF8,0x07,0xF0,0x03,0x00,0x00},/*"D",36*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x44,0x08,0xE4,0x08,0x0C,0x0C,0x0C,0x0C,0x00,0x00},/*"E",37*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x44,0x08,0xE4,0x00,0x0C,0x00,0x0C,0x00,0x00,0x00},/*"F",38*/ +{0xF0,0x03,0xF8,0x07,0x0C,0x0C,0x04,0x08,0x84,0x08,0x8C,0x07,0x98,0x0F,0x00,0x00},/*"G",39*/ +{0xFC,0x0F,0xFC,0x0F,0x40,0x00,0x40,0x00,0x40,0x00,0xFC,0x0F,0xFC,0x0F,0x00,0x00},/*"H",40*/ +{0x00,0x00,0x04,0x08,0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x04,0x08,0x04,0x08,0x00,0x00},/*"I",41*/ +{0x00,0x06,0x00,0x0E,0x00,0x08,0x04,0x08,0xFC,0x0F,0xFC,0x07,0x04,0x00,0x00,0x00},/*"J",42*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0xE0,0x00,0xB0,0x01,0x1C,0x0F,0x0C,0x0E,0x00,0x00},/*"K",43*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x04,0x08,0x00,0x08,0x00,0x0C,0x00,0x0C,0x00,0x00},/*"L",44*/ +{0xFC,0x0F,0xFC,0x0F,0x30,0x00,0xE0,0x00,0xE0,0x00,0x30,0x00,0xFC,0x0F,0xFC,0x0F},/*"M",45*/ +{0xFC,0x0F,0xFC,0x0F,0x30,0x00,0x60,0x00,0xC0,0x00,0xFC,0x0F,0xFC,0x0F,0x00,0x00},/*"N",46*/ +{0xF8,0x07,0xFC,0x0F,0x04,0x08,0x04,0x08,0x04,0x08,0xFC,0x0F,0xF8,0x07,0x00,0x00},/*"O",47*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x84,0x08,0x84,0x00,0xFC,0x00,0x78,0x00,0x00,0x00},/*"P",48*/ +{0xF8,0x07,0xFC,0x0F,0x04,0x08,0x04,0x0C,0x04,0x18,0xFC,0x3F,0xF8,0x27,0x00,0x00},/*"Q",49*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x44,0x00,0xC4,0x00,0xFC,0x0F,0x38,0x0F,0x00,0x00},/*"R",50*/ +{0x18,0x04,0x3C,0x0C,0x64,0x08,0x44,0x08,0xC4,0x08,0x8C,0x0F,0x08,0x07,0x00,0x00},/*"S",51*/ +{0x0C,0x00,0x0C,0x00,0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x04,0x08,0x0C,0x00,0x0C,0x00},/*"T",52*/ +{0xFC,0x07,0xFC,0x0F,0x00,0x08,0x00,0x08,0x00,0x08,0xFC,0x0F,0xFC,0x07,0x00,0x00},/*"U",53*/ +{0xFC,0x01,0xFC,0x03,0x00,0x06,0x00,0x0C,0x00,0x0C,0x00,0x06,0xFC,0x03,0xFC,0x01},/*"V",54*/ +{0xFC,0x03,0xFC,0x0F,0x00,0x0E,0x80,0x03,0x80,0x03,0x00,0x0E,0xFC,0x0F,0xFC,0x03},/*"W",55*/ +{0x0C,0x0C,0x1C,0x0E,0x30,0x03,0xE0,0x01,0xE0,0x01,0x30,0x03,0x1C,0x0E,0x0C,0x0C},/*"X",56*/ +{0x1C,0x00,0x3C,0x00,0x60,0x08,0xC0,0x0F,0xC0,0x0F,0x60,0x08,0x3C,0x00,0x1C,0x00},/*"Y",57*/ +{0x0C,0x0E,0x0C,0x0F,0x84,0x09,0xC4,0x08,0x64,0x08,0x34,0x08,0x1C,0x0C,0x0C,0x0C},/*"Z",58*/ +{0x00,0x00,0x00,0x00,0xFC,0x0F,0xFC,0x0F,0x04,0x08,0x04,0x08,0x00,0x00,0x00,0x00},/*"[",59*/ +{0x00,0x08,0x00,0x0C,0x00,0x06,0x00,0x03,0x80,0x01,0xC0,0x00,0x60,0x00,0x30,0x00},/*"/",60*/ +{0x00,0x00,0x00,0x00,0x04,0x08,0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x00,0x00,0x00,0x00},/*"]",61*/ +{0x00,0x00,0x10,0x00,0x18,0x00,0x0C,0x00,0x06,0x00,0x0C,0x00,0x18,0x00,0x10,0x00},/*"^",62*/ +{0x00,0x20,0x00,0x20,0x00,0x20,0x00,0x20,0x00,0x20,0x00,0x20,0x00,0x20,0x00,0x20},/*"_",63*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x06,0x00,0x0E,0x00,0x08,0x00,0x00,0x00,0x00,0x00},/*"`",64*/ +{0x00,0x07,0xA0,0x0F,0xA0,0x08,0xA0,0x08,0xE0,0x07,0xC0,0x0F,0x00,0x08,0x00,0x00},/*"a",65*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x07,0x20,0x08,0x60,0x08,0xC0,0x0F,0x80,0x07,0x00,0x00},/*"b",66*/ +{0xC0,0x07,0xE0,0x0F,0x20,0x08,0x20,0x08,0x20,0x08,0x60,0x0C,0x40,0x04,0x00,0x00},/*"c",67*/ +{0x80,0x07,0xC0,0x0F,0x60,0x08,0x24,0x08,0xFC,0x07,0xFC,0x0F,0x00,0x08,0x00,0x00},/*"d",68*/ +{0xC0,0x07,0xE0,0x0F,0x20,0x09,0x20,0x09,0x20,0x09,0xE0,0x0D,0xC0,0x05,0x00,0x00},/*"e",69*/ +{0x00,0x00,0x40,0x08,0xF8,0x0F,0xFC,0x0F,0x44,0x08,0x4C,0x00,0x08,0x00,0x00,0x00},/*"f",70*/ +{0xC0,0x27,0xE0,0x6F,0x20,0x48,0x20,0x48,0xC0,0x7F,0xE0,0x3F,0x20,0x00,0x00,0x00},/*"g",71*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x40,0x00,0x20,0x00,0xE0,0x0F,0xC0,0x0F,0x00,0x00},/*"h",72*/ +{0x00,0x00,0x00,0x00,0x20,0x08,0xEC,0x0F,0xEC,0x0F,0x00,0x08,0x00,0x00,0x00,0x00},/*"i",73*/ +{0x00,0x00,0x00,0x20,0x00,0x60,0x00,0x40,0x20,0x40,0xEC,0x7F,0xEC,0x3F,0x00,0x00},/*"j",74*/ +{0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x80,0x01,0xC0,0x03,0x60,0x0E,0x20,0x0C,0x00,0x00},/*"k",75*/ +{0x00,0x00,0x00,0x00,0x04,0x08,0xFC,0x0F,0xFC,0x0F,0x00,0x08,0x00,0x00,0x00,0x00},/*"l",76*/ +{0xE0,0x0F,0xE0,0x0F,0x60,0x00,0xC0,0x07,0xC0,0x07,0x60,0x00,0xE0,0x0F,0xC0,0x0F},/*"m",77*/ +{0x20,0x00,0xE0,0x0F,0xC0,0x0F,0x20,0x00,0x20,0x00,0xE0,0x0F,0xC0,0x0F,0x00,0x00},/*"n",78*/ +{0xC0,0x07,0xE0,0x0F,0x20,0x08,0x20,0x08,0x20,0x08,0xE0,0x0F,0xC0,0x07,0x00,0x00},/*"o",79*/ +{0x20,0x40,0xE0,0x7F,0xC0,0x7F,0x20,0x48,0x20,0x08,0xE0,0x0F,0xC0,0x07,0x00,0x00},/*"p",80*/ +{0xC0,0x07,0xE0,0x0F,0x20,0x08,0x20,0x48,0xC0,0x7F,0xE0,0x7F,0x20,0x40,0x00,0x00},/*"q",81*/ +{0x20,0x08,0xE0,0x0F,0xC0,0x0F,0x60,0x08,0x20,0x00,0x60,0x00,0x40,0x00,0x00,0x00},/*"r",82*/ +{0xC0,0x04,0xE0,0x0D,0x20,0x09,0x20,0x09,0x20,0x09,0x60,0x0F,0x40,0x06,0x00,0x00},/*"s",83*/ +{0x20,0x00,0x20,0x00,0xF8,0x07,0xFC,0x0F,0x20,0x08,0x20,0x0C,0x00,0x04,0x00,0x00},/*"t",84*/ +{0xE0,0x07,0xE0,0x0F,0x00,0x08,0x00,0x08,0xE0,0x07,0xE0,0x0F,0x00,0x08,0x00,0x00},/*"u",85*/ +{0xE0,0x01,0xE0,0x03,0x00,0x06,0x00,0x0C,0x00,0x0C,0x00,0x06,0xE0,0x03,0xE0,0x01},/*"v",86*/ +{0xE0,0x07,0xE0,0x0F,0x00,0x0C,0x00,0x07,0x00,0x07,0x00,0x0C,0xE0,0x0F,0xE0,0x07},/*"w",87*/ +{0x20,0x08,0x60,0x0C,0xC0,0x06,0x80,0x03,0x80,0x03,0xC0,0x06,0x60,0x0C,0x20,0x08},/*"x",88*/ +{0xE0,0x47,0xE0,0x4F,0x00,0x48,0x00,0x48,0x00,0x68,0xE0,0x3F,0xE0,0x1F,0x00,0x00},/*"y",89*/ +{0x60,0x0C,0x60,0x0E,0x20,0x0B,0xA0,0x09,0xE0,0x08,0x60,0x0C,0x20,0x0C,0x00,0x00},/*"z",90*/ +{0x00,0x00,0x40,0x00,0x40,0x00,0xF8,0x07,0xBC,0x0F,0x04,0x08,0x04,0x08,0x00,0x00},/*"{",91*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0x1F,0x7C,0x1F,0x00,0x00,0x00,0x00,0x00,0x00},/*"|",92*/ +{0x00,0x00,0x04,0x08,0x04,0x08,0xBC,0x0F,0xF8,0x07,0x40,0x00,0x40,0x00,0x00,0x00},/*"}",93*/ +}; + +const unsigned char font8x16_terminal[][16] = { +{0x00,0x00,0x00,0x00,0x7C,0x00,0xFE,0x1B,0xFE,0x1B,0x7C,0x00,0x00,0x00,0x00,0x00},/*"!",0*/ +{0x00,0x00,0x0E,0x00,0x1E,0x00,0x00,0x00,0x00,0x00,0x1E,0x00,0x0E,0x00,0x00,0x00},/*""",1*/ +{0x20,0x01,0xFC,0x0F,0xFC,0x0F,0x20,0x01,0x20,0x01,0xFC,0x0F,0xFC,0x0F,0x20,0x01},/*"#",2*/ +{0x38,0x06,0x7C,0x0C,0x44,0x08,0xFF,0x3F,0xFF,0x3F,0x84,0x08,0x8C,0x0F,0x18,0x07},/*"$",3*/ +{0x1C,0x18,0x14,0x1E,0x9C,0x07,0xE0,0x01,0x78,0x1C,0x1E,0x14,0x06,0x1C,0x00,0x00},/*"%",4*/ +{0xBC,0x1F,0xFE,0x10,0x42,0x10,0xC2,0x10,0xFE,0x1F,0x3C,0x0F,0x80,0x19,0x80,0x10},/*"&",5*/ +{0x00,0x00,0x00,0x00,0x10,0x00,0x1E,0x00,0x0E,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"'",6*/ +{0x00,0x00,0x00,0x00,0xF0,0x07,0xFC,0x1F,0x0E,0x38,0x02,0x20,0x00,0x00,0x00,0x00},/*"(",7*/ +{0x00,0x00,0x00,0x00,0x02,0x20,0x0E,0x38,0xFC,0x1F,0xF0,0x07,0x00,0x00,0x00,0x00},/*")",8*/ +{0x80,0x00,0xA0,0x02,0xE0,0x03,0xC0,0x01,0xC0,0x01,0xE0,0x03,0xA0,0x02,0x80,0x00},/*"*",9*/ +{0x80,0x00,0x80,0x00,0x80,0x00,0xE0,0x03,0xE0,0x03,0x80,0x00,0x80,0x00,0x80,0x00},/*"+",10*/ +{0x00,0x00,0x00,0x00,0x00,0x40,0x00,0x78,0x00,0x38,0x00,0x00,0x00,0x00,0x00,0x00},/*",",11*/ +{0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00},/*"-",12*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x00,0x18,0x00,0x00,0x00,0x00,0x00,0x00},/*".",13*/ +{0x00,0x18,0x00,0x1E,0x80,0x07,0xE0,0x01,0x78,0x00,0x1E,0x00,0x06,0x00,0x00,0x00},/*"/",14*/ +{0xF8,0x07,0xFC,0x0F,0x06,0x18,0xC2,0x10,0xC2,0x10,0x06,0x18,0xFC,0x0F,0xF8,0x07},/*"0",15*/ +{0x00,0x00,0x08,0x10,0x0C,0x10,0xFE,0x1F,0xFE,0x1F,0x00,0x10,0x00,0x10,0x00,0x00},/*"1",16*/ +{0x04,0x1C,0x06,0x1E,0x02,0x13,0x82,0x11,0xC2,0x10,0x62,0x10,0x3E,0x18,0x1C,0x18},/*"2",17*/ +{0x04,0x08,0x06,0x18,0x02,0x10,0x42,0x10,0x42,0x10,0x42,0x10,0xFE,0x1F,0xBC,0x0F},/*"3",18*/ +{0xC0,0x01,0xE0,0x01,0x30,0x01,0x18,0x01,0x0C,0x11,0xFE,0x1F,0xFE,0x1F,0x00,0x11},/*"4",19*/ +{0x7E,0x08,0x7E,0x18,0x42,0x10,0x42,0x10,0x42,0x10,0x42,0x10,0xC2,0x1F,0x82,0x0F},/*"5",20*/ +{0xF8,0x0F,0xFC,0x1F,0x46,0x10,0x42,0x10,0x42,0x10,0x42,0x10,0xC0,0x1F,0x80,0x0F},/*"6",21*/ +{0x06,0x00,0x06,0x00,0x02,0x00,0x02,0x1F,0xC2,0x1F,0xF2,0x00,0x3E,0x00,0x0E,0x00},/*"7",22*/ +{0xBC,0x0F,0xFE,0x1F,0x42,0x10,0x42,0x10,0x42,0x10,0x42,0x10,0xFE,0x1F,0xBC,0x0F},/*"8",23*/ +{0x3C,0x00,0x7E,0x10,0x42,0x10,0x42,0x10,0x42,0x10,0x42,0x18,0xFE,0x0F,0xFC,0x07},/*"9",24*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x0C,0x30,0x0C,0x00,0x00,0x00,0x00,0x00,0x00},/*":",26*/ +{0x00,0x00,0x00,0x00,0x00,0x20,0x60,0x3C,0x60,0x1C,0x00,0x00,0x00,0x00,0x00,0x00},/*";",27*/ +{0x80,0x00,0xC0,0x01,0x60,0x03,0x30,0x06,0x18,0x0C,0x0C,0x18,0x04,0x10,0x00,0x00},/*"<",28*/ +{0x40,0x02,0x40,0x02,0x40,0x02,0x40,0x02,0x40,0x02,0x40,0x02,0x40,0x02,0x40,0x02},/*"=",29*/ +{0x04,0x10,0x0C,0x18,0x18,0x0C,0x30,0x06,0x60,0x03,0xC0,0x01,0x80,0x00,0x00,0x00},/*">",30*/ +{0x04,0x00,0x06,0x00,0x02,0x00,0x82,0x1B,0xC2,0x1B,0x62,0x00,0x3E,0x00,0x1C,0x00},/*"?",31*/ +{0xFC,0x0F,0xFE,0x1F,0x02,0x10,0x82,0x11,0xC2,0x13,0xE2,0x13,0xFE,0x13,0xFC,0x03},/*"@",32*/ +{0xF0,0x1F,0xF8,0x1F,0x0C,0x01,0x06,0x01,0x06,0x01,0x0C,0x01,0xF8,0x1F,0xF0,0x1F},/*"A",33*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x42,0x10,0x42,0x10,0x42,0x10,0xFE,0x1F,0xBC,0x0F},/*"B",34*/ +{0xF8,0x07,0xFC,0x0F,0x06,0x18,0x02,0x10,0x02,0x10,0x02,0x10,0x06,0x18,0x0C,0x0C},/*"C",35*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x02,0x10,0x02,0x10,0x06,0x18,0xFC,0x0F,0xF8,0x07},/*"D",36*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x42,0x10,0x42,0x10,0xE2,0x10,0x06,0x18,0x06,0x18},/*"E",37*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x42,0x10,0x42,0x00,0xE2,0x00,0x06,0x00,0x06,0x00},/*"F",38*/ +{0xF8,0x07,0xFC,0x0F,0x06,0x18,0x02,0x10,0x82,0x10,0x82,0x10,0x86,0x0F,0x8C,0x1F},/*"G",39*/ +{0xFE,0x1F,0xFE,0x1F,0x40,0x00,0x40,0x00,0x40,0x00,0x40,0x00,0xFE,0x1F,0xFE,0x1F},/*"H",40*/ +{0x00,0x00,0x02,0x10,0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x02,0x10,0x02,0x10,0x00,0x00},/*"I",41*/ +{0x00,0x0C,0x00,0x1C,0x00,0x10,0x00,0x10,0x02,0x10,0xFE,0x1F,0xFE,0x0F,0x02,0x00},/*"J",42*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0xE0,0x00,0xB0,0x01,0x18,0x03,0x0E,0x1E,0x06,0x1C},/*"K",43*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x02,0x10,0x00,0x10,0x00,0x10,0x00,0x18,0x00,0x18},/*"L",44*/ +{0xFE,0x1F,0xFE,0x1F,0x18,0x00,0xF0,0x00,0xF0,0x00,0x18,0x00,0xFE,0x1F,0xFE,0x1F},/*"M",45*/ +{0xFE,0x1F,0xFE,0x1F,0x38,0x00,0x70,0x00,0xE0,0x00,0xC0,0x01,0xFE,0x1F,0xFE,0x1F},/*"N",46*/ +{0xFC,0x0F,0xFE,0x1F,0x02,0x10,0x02,0x10,0x02,0x10,0x02,0x10,0xFE,0x1F,0xFC,0x0F},/*"O",47*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x42,0x10,0x42,0x00,0x42,0x00,0x7E,0x00,0x3C,0x00},/*"P",48*/ +{0xFC,0x0F,0xFE,0x1F,0x02,0x10,0x02,0x1C,0x02,0x38,0x02,0x70,0xFE,0x5F,0xFC,0x0F},/*"Q",49*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x42,0x00,0x42,0x00,0xC2,0x00,0xFE,0x1F,0x3C,0x1F},/*"R",50*/ +{0x1C,0x0C,0x3E,0x1C,0x62,0x10,0x42,0x10,0x42,0x10,0xC2,0x10,0x8E,0x1F,0x0C,0x0F},/*"S",51*/ +{0x06,0x00,0x06,0x00,0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x02,0x10,0x06,0x00,0x06,0x00},/*"T",52*/ +{0xFE,0x0F,0xFE,0x1F,0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10,0xFE,0x1F,0xFE,0x0F},/*"U",53*/ +{0xFE,0x03,0xFE,0x07,0x00,0x0C,0x00,0x18,0x00,0x18,0x00,0x0C,0xFE,0x07,0xFE,0x03},/*"V",54*/ +{0xFE,0x07,0xFE,0x1F,0x00,0x1C,0xC0,0x07,0xC0,0x07,0x00,0x1C,0xFE,0x1F,0xFE,0x07},/*"W",55*/ +{0x0E,0x1C,0x1E,0x1E,0x30,0x03,0xE0,0x01,0xE0,0x01,0x30,0x03,0x1E,0x1E,0x0E,0x1C},/*"X",56*/ +{0x1E,0x00,0x3E,0x00,0x60,0x10,0xC0,0x1F,0xC0,0x1F,0x60,0x10,0x3E,0x00,0x1E,0x00},/*"Y",57*/ +{0x06,0x1E,0x06,0x1F,0x82,0x11,0xC2,0x10,0x62,0x10,0x32,0x10,0x1E,0x18,0x0E,0x18},/*"Z",58*/ +{0x00,0x00,0x00,0x00,0xFE,0x1F,0xFE,0x1F,0x02,0x10,0x02,0x10,0x00,0x00,0x00,0x00},/*"[",59*/ +{0x00,0x18,0x00,0x1E,0x80,0x07,0xE0,0x01,0x78,0x00,0x1E,0x00,0x06,0x00,0x00,0x00},/*"/",60*/ +{0x00,0x00,0x00,0x00,0x02,0x10,0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x00,0x00,0x00,0x00},/*"]",61*/ +{0x20,0x00,0x30,0x00,0x18,0x00,0x0C,0x00,0x18,0x00,0x30,0x00,0x20,0x00,0x00,0x00},/*"^",62*/ +{0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80},/*"_",63*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x38,0x00,0x78,0x00,0x40,0x00,0x00,0x00,0x00,0x00},/*"`",64*/ +{0x00,0x0E,0x20,0x1F,0x20,0x11,0x20,0x11,0x20,0x11,0xE0,0x0F,0xC0,0x1F,0x00,0x10},/*"a",65*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x0F,0x20,0x10,0x20,0x10,0x60,0x10,0xC0,0x1F,0x80,0x0F},/*"b",66*/ +{0xC0,0x0F,0xE0,0x1F,0x20,0x10,0x20,0x10,0x20,0x10,0x20,0x10,0x60,0x18,0x40,0x08},/*"c",67*/ +{0x80,0x0F,0xC0,0x1F,0x60,0x10,0x20,0x10,0x22,0x10,0xFE,0x0F,0xFE,0x1F,0x00,0x10},/*"d",68*/ +{0xC0,0x0F,0xE0,0x1F,0x20,0x11,0x20,0x11,0x20,0x11,0x20,0x11,0xE0,0x19,0xC0,0x09},/*"e",69*/ +{0x00,0x00,0x20,0x10,0xFC,0x1F,0xFE,0x1F,0x22,0x10,0x22,0x00,0x06,0x00,0x04,0x00},/*"f",70*/ +{0xC0,0x4F,0xE0,0xDF,0x20,0x90,0x20,0x90,0x20,0x90,0xC0,0xFF,0xE0,0x7F,0x20,0x00},/*"g",71*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x40,0x00,0x20,0x00,0x20,0x00,0xE0,0x1F,0xC0,0x1F},/*"h",72*/ +{0x00,0x00,0x20,0x10,0x20,0x10,0xEC,0x1F,0xEC,0x1F,0x00,0x10,0x00,0x10,0x00,0x00},/*"i",73*/ +{0x00,0x60,0x00,0xC0,0x20,0x80,0x20,0x80,0xEC,0xFF,0xEC,0x7F,0x00,0x00,0x00,0x00},/*"j",74*/ +{0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x80,0x01,0x80,0x03,0xC0,0x06,0x60,0x1C,0x20,0x18},/*"k",75*/ +{0x00,0x00,0x02,0x10,0x02,0x10,0xFE,0x1F,0xFE,0x1F,0x00,0x10,0x00,0x10,0x00,0x00},/*"l",76*/ +{0xE0,0x1F,0xE0,0x1F,0x60,0x00,0xC0,0x0F,0xC0,0x0F,0x60,0x00,0xE0,0x1F,0xC0,0x1F},/*"m",77*/ +{0x20,0x00,0xE0,0x1F,0xC0,0x1F,0x20,0x00,0x20,0x00,0x20,0x00,0xE0,0x1F,0xC0,0x1F},/*"n",78*/ +{0xC0,0x0F,0xE0,0x1F,0x20,0x10,0x20,0x10,0x20,0x10,0x20,0x10,0xE0,0x1F,0xC0,0x0F},/*"o",79*/ +{0x20,0x80,0xE0,0xFF,0xC0,0xFF,0x20,0x90,0x20,0x10,0x20,0x10,0xE0,0x1F,0xC0,0x0F},/*"p",80*/ +{0xC0,0x0F,0xE0,0x1F,0x20,0x10,0x20,0x10,0x20,0x90,0xC0,0xFF,0xE0,0xFF,0x20,0x80},/*"q",81*/ +{0x20,0x10,0xE0,0x1F,0xC0,0x1F,0x60,0x10,0x20,0x00,0x20,0x00,0x60,0x00,0x40,0x00},/*"r",82*/ +{0xC0,0x08,0xE0,0x19,0x20,0x11,0x20,0x11,0x20,0x13,0x20,0x12,0x60,0x1E,0x40,0x0C},/*"s",83*/ +{0x20,0x00,0x20,0x00,0xFC,0x0F,0xFE,0x1F,0x20,0x10,0x20,0x18,0x00,0x08,0x00,0x00},/*"t",84*/ +{0xE0,0x0F,0xE0,0x1F,0x00,0x10,0x00,0x10,0x00,0x10,0xE0,0x0F,0xE0,0x1F,0x00,0x10},/*"u",85*/ +{0xE0,0x03,0xE0,0x07,0x00,0x0C,0x00,0x18,0x00,0x18,0x00,0x0C,0xE0,0x07,0xE0,0x03},/*"v",86*/ +{0xE0,0x0F,0xE0,0x1F,0x00,0x18,0x00,0x0F,0x00,0x0F,0x00,0x18,0xE0,0x1F,0xE0,0x0F},/*"w",87*/ +{0x20,0x10,0x60,0x18,0xC0,0x0C,0x80,0x07,0x80,0x07,0xC0,0x0C,0x60,0x18,0x20,0x10},/*"x",88*/ +{0xE0,0x8F,0xE0,0x9F,0x00,0x90,0x00,0x90,0x00,0x90,0x00,0xD0,0xE0,0x7F,0xE0,0x3F},/*"y",89*/ +{0x60,0x18,0x60,0x1C,0x20,0x16,0x20,0x13,0xA0,0x11,0xE0,0x10,0x60,0x18,0x20,0x18},/*"z",90*/ +{0x00,0x00,0x00,0x00,0x80,0x00,0xFC,0x1F,0x7E,0x3F,0x02,0x20,0x02,0x20,0x00,0x00},/*"{",91*/ +{0x00,0x00,0x00,0x00,0x00,0x00,0x7C,0x3E,0x7C,0x3E,0x00,0x00,0x00,0x00,0x00,0x00},/*"|",92*/ +{0x00,0x00,0x02,0x20,0x02,0x20,0x7E,0x3F,0xFC,0x1F,0x80,0x00,0x00,0x00,0x00,0x00},/*"}",93*/ +}; +#endif + +const uint8_t bitmap_tick[16 *16 / 8] = +{0x00,0x80,0xC0,0xE0,0xC0,0x80,0x00,0x80,0xC0,0xE0,0xF0,0xF8,0xFC,0x78,0x30,0x00,0x00,0x01,0x03,0x07,0x0F,0x1F,0x1F,0x1F,0x0F,0x07,0x03,0x01,0x00,0x00,0x00,0x00}; + +const uint8_t bitmap_cross[16 *16 / 8] = +{0x00,0x0C,0x1C,0x3C,0x78,0xF0,0xE0,0xC0,0xE0,0xF0,0x78,0x3C,0x1C,0x0C,0x00,0x00,0x00,0x30,0x38,0x3C,0x1E,0x0F,0x07,0x03,0x07,0x0F,0x1E,0x3C,0x38,0x30,0x00,0x00}; + +void LCD_Common::printInt(uint16_t value, int8_t padding) +{ + uint16_t den = 10000; + for (int8_t i = 5; i > 0; i--) { + byte v = (byte)(value / den); + value -= v * den; + den /= 10; + if (v == 0 && padding && den) { + if (padding >= i) { + writeDigit((m_flags & FLAG_PAD_ZERO) ? 0 : -1); + } + continue; + } + padding = 0; + writeDigit(v); + } +} + +void LCD_Common::printLong(uint32_t value, int8_t padding) +{ + uint32_t den = 1000000000; + for (int8_t i = 10; i > 0; i--) { + byte v = (byte)(value / den); + value -= v * den; + den /= 10; + if (v == 0 && padding && den) { + if (padding >= i) { + writeDigit((m_flags & FLAG_PAD_ZERO) ? 0 : -1); + } + continue; + } + padding = 0; + writeDigit(v); + } +} + + +void OLED_SH1106::WriteCommand(unsigned char ins) +{ + Wire.beginTransmission(I2C_ADDR);//0x78 >> 1 + Wire.write(0x00);//0x00 + Wire.write(ins); + Wire.endTransmission(); +} + +void OLED_SH1106::WriteData(unsigned char dat) +{ + Wire.beginTransmission(I2C_ADDR);//0x78 >> 1 + Wire.write(0x40);//0x40 + Wire.write(dat); + Wire.endTransmission(); +} + +void OLED_SH1106::setCursor(unsigned char x, unsigned char y) +{ + m_col = x + 2; + m_row = y; + WriteCommand(0xb0 + m_row); + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address +} + +void OLED_SH1106::clear(byte x, byte y, byte width, byte height) +{ + height >>= 3; + width >>= 3; + y >>= 3; +#ifdef TWBR + uint8_t twbrbackup = TWBR; + TWBR = 18; // upgrade to 400KHz! +#endif + for (byte i = 0; i < height; i++) { + // send a bunch of data in one xmission + WriteCommand(0xB0 + i + y);//set page address + WriteCommand((x + 2) & 0xf);//set lower column address + WriteCommand(0x10 | (x >> 4));//set higher column address + + for(byte j = 0; j < 8; j++){ + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte k = 0; k < width; k++) { + Wire.write(0); + } + Wire.endTransmission(); + } + } +#ifdef TWBR + TWBR = twbrbackup; +#endif + setCursor(0, 0); +} + +size_t OLED_SH1106::write(uint8_t c) +{ + if (c == '\n') { + setCursor(0, m_row + ((m_font == FONT_SIZE_SMALL) ? 1 : 2)); + return 1; + } else if (c == '\r') { + m_col = 0; + return 1; + } + +#ifdef TWBR + uint8_t twbrbackup = TWBR; + TWBR = 18; // upgrade to 400KHz! +#endif +#ifndef MEMORY_SAVING + if (m_font == FONT_SIZE_SMALL) { +#endif + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + if (c > 0x20 && c < 0x7f) { + c -= 0x21; + for (byte i = 0; i < 5; i++) { + byte d = font5x8[c][i]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.write(0); + } else { + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 11 : 6; i > 0; i--) { + Wire.write(0); + } + } + Wire.endTransmission(); + m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 11 : 6; + if (m_col >= 128) { + m_col = 0; + m_row ++; + } +#ifndef MEMORY_SAVING + } else { + if (c > 0x20 && c < 0x7f) { + c -= 0x21; + + WriteCommand(0xB0 + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = 0; i <= 14; i += 2) { + byte d = font8x16_terminal[c][i]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 1);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = 1; i <= 15; i += 2) { + byte d = font8x16_terminal[c][i]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + } else { + WriteCommand(0xB0 + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 16 : 8; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 1);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 16 : 8; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + } + m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 17 : 9; + if (m_col >= 128) { + m_col = 0; + m_row += 2; + } + } +#endif +#ifdef TWBR + TWBR = twbrbackup; +#endif + return 1; +} + +void OLED_SH1106::writeDigit(byte n) +{ +#ifdef TWBR + uint8_t twbrbackup = TWBR; + TWBR = 18; // upgrade to 400KHz! +#endif + if (m_font == FONT_SIZE_SMALL) { + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + if (n <= 9) { + n += '0' - 0x21; + for (byte i = 0; i < 5; i++) { + Wire.write(font5x8[n][i]); + } + Wire.write(0); + } else { + for (byte i = 0; i < 6; i++) { + Wire.write(0); + } + } + Wire.endTransmission(); + m_col += 6; + } else if (m_font == FONT_SIZE_MEDIUM) { + write(n <= 9 ? ('0' + n) : ' '); +#ifndef MEMORY_SAVING + } else if (m_font == FONT_SIZE_LARGE) { + if (n <= 9) { + byte i; + WriteCommand(0xB0 + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (i = 0; i < 16; i ++) { + byte d = digits16x16[n][i]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 1);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (; i < 32; i ++) { + byte d = digits16x16[n][i]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + } else { + WriteCommand(0xB0 + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 1);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + } + m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 30 : 16; +#endif + } else { + if (n <= 9) { + byte i; + WriteCommand(0xB0 + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (i = 0; i < 16; i ++) { + byte d = digits16x24[n][i * 3]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 1);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (i = 0; i < 16; i ++) { + byte d = digits16x24[n][i * 3 + 1]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 2);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (i = 0; i < 16; i ++) { + byte d = digits16x24[n][i * 3 + 2]; + Wire.write(d); + if (m_flags & FLAG_PIXEL_DOUBLE_H) Wire.write(d); + } + Wire.endTransmission(); + } else { + WriteCommand(0xB0 + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 1);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + + WriteCommand(0xB0 + m_row + 2);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte i = (m_flags & FLAG_PIXEL_DOUBLE_H) ? 32 : 16; i > 0; i--) { + Wire.write(0); + } + Wire.endTransmission(); + } + m_col += (m_flags & FLAG_PIXEL_DOUBLE_H) ? 30 : 16; + } +#ifdef TWBR + TWBR = twbrbackup; +#endif +} + +void OLED_SH1106::draw(const byte* buffer, byte width, byte height) +{ +#ifdef TWBR + uint8_t twbrbackup = TWBR; + TWBR = 18; // upgrade to 400KHz! +#endif + + const byte *p = buffer; + height >>= 3; + width >>= 3; + for (byte i = 0; i < height; i++) { + // send a bunch of data in one xmission + WriteCommand(0xB0 + i + m_row);//set page address + WriteCommand(m_col & 0xf);//set lower column address + WriteCommand(0x10 | (m_col >> 4));//set higher column address + + for(byte j = 0; j < 8; j++){ + Wire.beginTransmission(I2C_ADDR); + Wire.write(0x40); + for (byte k = 0; k < width; k++, p++) { + Wire.write(*p); + } + Wire.endTransmission(); + } + } +#ifdef TWBR + TWBR = twbrbackup; +#endif + m_col += width; +} + +void OLED_SH1106::begin() +{ + Wire.begin(); + Wire.setClock(400000); + + WriteCommand(0xAE); /*display off*/ + + WriteCommand(0x02); /*set lower column address*/ + WriteCommand(0x10); /*set higher column address*/ + + WriteCommand(0x40); /*set display start line*/ + + WriteCommand(0xB0); /*set page address*/ + + WriteCommand(0x81); /*contract control*/ + WriteCommand(0x80); /*128*/ + + WriteCommand(0xA1); /*set segment remap*/ + + WriteCommand(0xA6); /*normal / reverse*/ + + WriteCommand(0xA8); /*multiplex ratio*/ + WriteCommand(0x3F); /*duty = 1/32*/ + + WriteCommand(0xad); /*set charge pump enable*/ + WriteCommand(0x8b); /*external VCC */ + + WriteCommand(0x30); /*0X30---0X33 set VPP 9V liangdu!!!!*/ + + WriteCommand(0xC8); /*Com scan direction*/ + + WriteCommand(0xD3); /*set display offset*/ + WriteCommand(0x00); /* 0x20 */ + + WriteCommand(0xD5); /*set osc division*/ + WriteCommand(0x80); + + WriteCommand(0xD9); /*set pre-charge period*/ + WriteCommand(0x1f); /*0x22*/ + + WriteCommand(0xDA); /*set COM pins*/ + WriteCommand(0x12); + + WriteCommand(0xdb); /*set vcomh*/ + WriteCommand(0x40); + + WriteCommand(0xAF); /*display ON*/ + + clear(); +} diff --git a/esp32/libraries/FreematicsOLED/FreematicsOLED.h b/esp32/libraries/FreematicsOLED/FreematicsOLED.h new file mode 100644 index 0000000..81f0342 --- /dev/null +++ b/esp32/libraries/FreematicsOLED/FreematicsOLED.h @@ -0,0 +1,63 @@ +/************************************************************************* +* Simple OLED Text & Bitmap Display Library for Arduino +* Distributed under BSD +* (C)2013-2018 Stanley Huang +*************************************************************************/ + +#include + +typedef enum { + FONT_SIZE_SMALL = 0, + FONT_SIZE_MEDIUM, + FONT_SIZE_LARGE, + FONT_SIZE_XLARGE +} FONT_SIZE; + +#define FLAG_PAD_ZERO 1 +#define FLAG_PIXEL_DOUBLE_H 2 +#define FLAG_PIXEL_DOUBLE_V 4 +#define FLAG_PIXEL_DOUBLE (FLAG_PIXEL_DOUBLE_H | FLAG_PIXEL_DOUBLE_V) + +extern const unsigned char font5x8[][5]; +extern const unsigned char digits8x8[][8] ; +extern const unsigned char digits16x16[][32]; +extern const unsigned char digits16x24[][48]; +extern const unsigned char font8x16_doslike[][16]; +extern const unsigned char font8x16_terminal[][16]; +extern const uint8_t bitmap_tick[]; +extern const uint8_t bitmap_cross[]; + +class LCD_Common +{ +public: + void setFontSize(FONT_SIZE size) { m_font = size; } + void setFlags(byte flags) { m_flags = flags; } + virtual void backlight(bool on) {} + virtual void draw(const byte* buffer, byte width, byte height) {} + void printInt(uint16_t value, int8_t padding = -1); + void printLong(uint32_t value, int8_t padding = -1); +protected: + virtual void writeDigit(byte n) {} + byte m_font = FONT_SIZE_SMALL; + byte m_flags = 0; +}; + +class OLED_SH1106 : public LCD_Common, public Print +{ +public: + void begin(); + void setCursor(byte column, byte line); + void draw(const byte* buffer, byte width, byte height); + size_t write(uint8_t c); + void clear(byte x = 0, byte y = 0, byte width = 128, byte height = 64); + void clearLine(byte line); + byte getLines() { return 21; } + byte getCols() { return 8; } +private: + void WriteCommand(unsigned char ins); + void WriteData(unsigned char dat); + void writeDigit(byte n); + byte m_col = 0; + byte m_row = 0; +}; + diff --git a/esp32/libraries/FreematicsONE/FreematicsBase.h b/esp32/libraries/FreematicsONE/FreematicsBase.h new file mode 100644 index 0000000..8f45041 --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsBase.h @@ -0,0 +1,65 @@ +/************************************************************************* +* Base class for Freematics telematics products +* Distributed under BSD license +* Visit http://freematics.com/products/freematics-one for more information +* (C)2017-18 Stanley Huang +*************************************************************************/ + +#ifndef FREEMATICS_BASE +#define FREEMATICS_BASE + +#include + +// non-OBD/custom PIDs (no mode number) +#define PID_GPS_LATITUDE 0xA +#define PID_GPS_LONGITUDE 0xB +#define PID_GPS_ALTITUDE 0xC +#define PID_GPS_SPEED 0xD +#define PID_GPS_HEADING 0xE +#define PID_GPS_SAT_COUNT 0xF +#define PID_GPS_TIME 0x10 +#define PID_GPS_DATE 0x11 +#define PID_ACC 0x20 +#define PID_GYRO 0x21 +#define PID_COMPASS 0x22 +#define PID_MEMS_TEMP 0x23 +#define PID_BATTERY_VOLTAGE 0x24 +#define PID_ORIENTATION 0x25 + +// custom PIDs for calculated data +#define PID_TRIP_DISTANCE 0x30 +#define PID_DATA_SIZE 0x80 +#define PID_CSQ 0x81 +#define PID_DEVICE_TEMP 0x82 +#define PID_DEVICE_HALL 0x83 + +typedef struct { + uint32_t date; + uint32_t time; + int32_t lat; + int32_t lng; + int16_t alt; /* meter */ + uint16_t speed; /* 1/100 Knot */ + int16_t heading; + uint8_t sat; +} GPS_DATA; + +class CFreematics +{ +public: + virtual byte begin() { return 0; } + // start xBee UART communication + virtual bool xbBegin(unsigned long baudrate = 115200L) = 0; + // read data to xBee UART + virtual int xbRead(char* buffer, int bufsize) = 0; + // send data to xBee UART + virtual void xbWrite(const char* cmd) = 0; + // receive data from xBee UART (returns 0/1/2) + virtual int xbReceive(char* buffer, int bufsize, unsigned int timeout = 1000, const char** expected = 0, byte expectedCount = 0) = 0; + // purge xBee UART buffer + virtual void xbPurge() = 0; + // toggle xBee module power + virtual void xbTogglePower() = 0; +}; + +#endif diff --git a/esp32/libraries/FreematicsONE/FreematicsMEMS.cpp b/esp32/libraries/FreematicsONE/FreematicsMEMS.cpp new file mode 100644 index 0000000..b87ed27 --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsMEMS.cpp @@ -0,0 +1,643 @@ +/************************************************************************* +* Freematics MPU6050 helper class +* Distributed under BSD license +* Visit http://freematics.com for more information +* (C)2016-2018 Stanley Huang +*************************************************************************/ + +#include +#include +#include "FreematicsBase.h" +#include "FreematicsMEMS.h" + + +// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" +// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) +// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute +// device orientation +void CQuaterion::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) +{ + uint32_t now = millis(); + deltat = ((float)(now - lastUpdate)/1000.0f); // set integration time by time elapsed since last filter update + lastUpdate = now; + + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability + float norm; + float hx, hy, _2bx, _2bz; + float s1, s2, s3, s4; + float qDot1, qDot2, qDot3, qDot4; + + // Auxiliary variables to avoid repeated arithmetic + float _2q1mx; + float _2q1my; + float _2q1mz; + float _2q2mx; + float _4bx; + float _4bz; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; + float _2q1q3 = 2.0f * q1 * q3; + float _2q3q4 = 2.0f * q3 * q4; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = sqrtf(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrtf(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + _2q1mx = 2.0f * q1 * mx; + _2q1my = 2.0f * q1 * my; + _2q1mz = 2.0f * q1 * mz; + _2q2mx = 2.0f * q2 * mx; + hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; + hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude + norm = 1.0f/norm; + s1 *= norm; + s2 *= norm; + s3 *= norm; + s4 *= norm; + + // Compute rate of change of quaternion + qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; + qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; + qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; + qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; + + // Integrate to yield quaternion + q1 += qDot1 * deltat; + q2 += qDot2 * deltat; + q3 += qDot3 * deltat; + q4 += qDot4 * deltat; + norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion + norm = 1.0f/norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + +void CQuaterion::getOrientation(ORIENTATION* ori) +{ + ori->yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) * 180.0f / PI; + ori->pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])) * 180.0f / PI; + ori->roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]) * 180.0f / PI; +} + +//============================================================================== +//====== Set of useful function to access acceleration. gyroscope, magnetometer, +//====== and temperature data +//============================================================================== +void MPU9250_ACC::readAccelData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + + +void MPU9250_9DOF::readGyroData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + +void MPU9250_9DOF::readMagData(int16_t * destination) +{ + if(readByteAK(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set + uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition + readBytesAK(AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array + uint8_t c = rawData[6]; // End data read by reading ST2 register + if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; + } + } +} + +int16_t MPU9250_ACC::readTempData() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + readBytes(TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value +} + +bool MPU9250_9DOF::initAK8963(float * destination) +{ + if (readByteAK(WHO_AM_I_AK8963) != 0x48) { + return false; + } + + // First extract the factory calibration for each magnetometer axis + uint8_t rawData[3]; // x/y/z gyro calibration data stored here + writeByteAK(AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + writeByteAK(AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); + // Read the x-, y-, and z-axis calibration values + /* + if (!readBytesAK(AK8963_ASAX, 3, &rawData[0], 3000)) { + return false; + } + */ + rawData[0] = readByteAK(AK8963_ASAX); + rawData[1] = readByteAK(AK8963_ASAX); + rawData[2] = readByteAK(AK8963_ASAX); + destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. + destination[1] = (float)(rawData[1] - 128)/256. + 1.; + destination[2] = (float)(rawData[2] - 128)/256. + 1.; + writeByte(AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + // Configure the magnetometer for continuous read and highest resolution + // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, + // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates + writeByte(AK8963_CNTL, MFS_16BITS << 4 | Mmode); // Set magnetometer data resolution and sample ODR + delay(10); + return true; +} + +// 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. +void MPU9250_9DOF::calibrateMPU9250(float * gyroBias, float * accelBias) +{ + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t ii, packet_count, fifo_count; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + + // reset device + // Write a one to bit 7 reset bit; toggle reset device + writeByte(PWR_MGMT_1, 0x80); + delay(100); + + // get stable time source; Auto select clock source to be PLL gyroscope + // reference if ready else use the internal oscillator, bits 2:0 = 001 + writeByte(PWR_MGMT_1, 0x01); + writeByte(PWR_MGMT_2, 0x00); + delay(200); + + // Configure device for bias calculation + writeByte(INT_ENABLE, 0x00); // Disable all interrupts + writeByte(FIFO_EN, 0x00); // Disable FIFO + writeByte(PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(I2C_MST_CTRL, 0x00); // Disable master + writeByte(USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(USER_CTRL, 0x2C); // Reset FIFO and DMP + delay(15); + +// Configure MPU6050 gyro and accelerometer for bias calculation + writeByte(CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(USER_CTRL, 0x40); // Enable FIFO + writeByte(FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) + delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + +// At end of sample accumulation, turn off FIFO sensor read + writeByte(FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readBytes(FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging + + for (ii = 0; ii < packet_count; ii++) + { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + readBytes(FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO + 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]); + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + 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]; + } + accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + 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; + + if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation + else {accel_bias[2] += (int32_t) accelsensitivity;} + +// 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(XG_OFFSET_H, data[0]); + writeByte(XG_OFFSET_L, data[1]); + writeByte(YG_OFFSET_H, data[2]); + writeByte(YG_OFFSET_L, data[3]); + writeByte(ZG_OFFSET_H, data[4]); + writeByte(ZG_OFFSET_L, data[5]); + +// Output scaled gyro biases for display in the main program + gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity; + +// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain +// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold +// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature +// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that +// the accelerometer biases calculated above must be divided by 8. + + int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + readBytes(XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis + + for(ii = 0; ii < 3; ii++) { + if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accel_bias[1]/8); + accel_bias_reg[2] -= (accel_bias[2]/8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + +// Apparently this is not working for the acceleration biases in the MPU-9250 +// Are we handling the temperature correction bit properly? +// Push accelerometer biases to hardware registers + writeByte(XA_OFFSET_H, data[0]); + writeByte(XA_OFFSET_L, data[1]); + writeByte(YA_OFFSET_H, data[2]); + writeByte(YA_OFFSET_L, data[3]); + writeByte(ZA_OFFSET_H, data[4]); + writeByte(ZA_OFFSET_L, data[5]); + +// Output scaled accelerometer biases for display in the main program + accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; + accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; + accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; +} + + +// Accelerometer and gyroscope self test; check calibration wrt factory settings +void MPU9250_9DOF::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +{ + uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; + uint8_t selfTest[6]; + int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; + float factoryTrim[6]; + uint8_t FS = 0; + + writeByte(SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(GYRO_CONFIG, 1<MadgwickQuaternionUpdate(acc[0], acc[1], acc[2], gyr[0]*PI/180.0f, gyr[1]*PI/180.0f, gyr[2]*PI/180.0f, mag[0], mag[1], mag[2]); + quaterion->getOrientation(ori); + } + return true; +} diff --git a/esp32/libraries/FreematicsONE/FreematicsMEMS.h b/esp32/libraries/FreematicsONE/FreematicsMEMS.h new file mode 100644 index 0000000..beaabe3 --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsMEMS.h @@ -0,0 +1,285 @@ +/************************************************************************* +* Freematics MPU6050 helper class +* Distributed under BSD license +* Visit http://freematics.com for more information +* (C)2016-2018 Stanley Huang +*************************************************************************/ + +#ifndef _MPU9250_H +#define _MPU9250_H + +// See also MPU-9250 Register Map and Descriptions, Revision 4.0, +// RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in above +// document; the MPU9250 and MPU9150 are virtually identical but the latter has +// a different register map + +//Magnetometer Registers +#define AK8963_ADDRESS 0x0C +#define WHO_AM_I_AK8963 0x00 // should return 0x48 +#define INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_CNTL2 0x0B +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 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_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F + +#define SELF_TEST_A 0x10 + +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F + +// Duration counter threshold for motion interrupt generation, 1 kHz rate, +// LSB = 1 ms +#define MOT_DUR 0x20 +// Zero-motion detection threshold bits [7:0] +#define ZMOT_THR 0x21 +// Duration counter threshold for zero motion interrupt generation, 16 Hz rate, +// LSB = 64 ms +#define ZRMOT_DUR 0x22 + +#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_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E + +#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 +#define AK8963_ADDRESS 0x0C // Address of magnetometer + +enum { + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_16G +}; + +enum { + GFS_250DPS = 0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS +}; + +enum { + MFS_14BITS = 0, // 0.6 mG per LSB + MFS_16BITS // 0.15 mG per LSB +}; + +// Specify sensor full scale +#define Ascale AFS_2G +#define Gscale GFS_250DPS + +#define mRes (10.*4912./32760.0) + +#if Ascale == AFS_2G + #define aRes (2.0/32768.0) +#elif Ascale == AFS_4G + #define aRes (4.0/32768.0) +#elif Ascale == AFS_8G + #define aRes (8.0/32768.0) +#elif Ascale == AFS_16G + #define aRes (16.0/32768.0) +#endif + +#if Gscale == GFS_250DPS + #define gRes (250.0/32768.0) +#elif Gscale == GFS_500DPS + #define gRes (500.0/32768.0) +#elif Gscale == GFS_1000DPS + #define gRes (1000.0/32768.0) +#elif Gscale == GFS_2000DPS + #define gRes (2000.0/32768.0) +#endif + +// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read +#define Mmode 0x02 + +#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral +#define Ki 0.0f + +typedef struct { + float pitch; + float yaw; + float roll; +} ORIENTATION; + +class CQuaterion +{ +public: + void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); + void getOrientation(ORIENTATION* ori); +private: + float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion + // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) + float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) + float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) + float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta + float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value + uint32_t firstUpdate = 0; // used to calculate integration interval + uint32_t lastUpdate = 0; + float deltat = 0.0f; +}; + +class MPU9250_ACC +{ +public: + virtual byte begin(bool fusion = false); + virtual bool read(float* acc, float* gyr = 0, float* mag = 0, int16_t* temp = 0, ORIENTATION* ori = 0); +protected: + void getAres(); + void readAccelData(int16_t *); + int16_t readTempData(); + void initMPU9250(); + void writeByte(uint8_t, uint8_t); + uint8_t readByte(uint8_t); + void readBytes(uint8_t, uint8_t, uint8_t *); + int16_t accelCount[3] = {0}; +}; + +class MPU9250_9DOF : public MPU9250_ACC +{ +public: + byte begin(bool fusion = false); + bool read(float* acc, float* gyr = 0, float* mag = 0, int16_t* temp = 0, ORIENTATION* ori = 0); +private: + void getMres(); + void getGres(); + void readGyroData(int16_t *); + void readMagData(int16_t *); + bool initAK8963(float *); + void calibrateMPU9250(float * gyroBias, float * accelBias); + void MPU9250SelfTest(float * destination); + void writeByteAK(uint8_t, uint8_t); + uint8_t readByteAK(uint8_t); + void readBytesAK(uint8_t, uint8_t, uint8_t *); + float gyroBias[3] = {0}; + float accelBias[3] = {0}; // Bias corrections for gyro and accelerometer + float magCalibration[3] = {0}; + int16_t gyroCount[3] = {0}; + int16_t magCount[3] = {0}; // Stores the 16-bit signed magnetometer sensor output + CQuaterion* quaterion = 0; +}; + +#endif diff --git a/esp32/libraries/FreematicsONE/FreematicsNetwork.cpp b/esp32/libraries/FreematicsONE/FreematicsNetwork.cpp new file mode 100644 index 0000000..ee2ba18 --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsNetwork.cpp @@ -0,0 +1,713 @@ +/************************************************************************* +* Freematics Hub Client implementations for ESP8266-AT, SIM800, SIM5360 +* Distributed under BSD license +* Visit http://freematics.com/products/freematics-one for more information +* (C)2017-2019 Stanley Huang +#include "FreematicsBase.h" +#include "FreematicsNetwork.h" + +#define XBEE_BAUDRATE 115200 + +/******************************************************************************* + Implementation for ESP8266 WIFI (ESP8266 AT command-set) +*******************************************************************************/ +bool UDPClientESP8266AT::begin(CFreematics* device, bool nocheck) +{ + m_device = device; + m_device->xbBegin(XBEE_BAUDRATE); + for (byte n = 0; n < 5; n++) { + if (sendCommand("AT\r\n", 200)) { + return true; + } + } + return false; +} + +void UDPClientESP8266AT::end() +{ + //sendCommand("AT+CWQAP\r\n"); +} + +bool UDPClientESP8266AT::setup(const char* ssid, const char* password, unsigned int timeout) +{ + bool gotIP = false; + // test the module by issuing ATE0 command and confirming response of "OK" + const char* cmds[] = {"ATE0\r\n", "AT+CWMODE=1\r\n", "AT+CIPMUX=0\r\n", "AT+CWDHCP=1,1\r\n"}; + for (byte n = 0; n < sizeof(cmds) / sizeof(cmds[0]); n++) { + delay(100); + if (!sendCommand(cmds[n], 100)) { + delay(100); + sendCommand(cmds[n], 100); + } + if (rxLen) { + if (strstr_P(rxBuf, PSTR("WIFI GOT IP"))) { + // WIFI automatically connected + gotIP = true; + } + } + } + // generate and send AT command for joining AP + if (!gotIP) { + sprintf_P(buffer, PSTR("AT+CWJAP=\"%s\",\"%s\"\r\n"), ssid, password); + if (!sendCommand(buffer, 7000)) return false; + } + return true; +} + +String UDPClientESP8266AT::getIP() +{ + // get IP address + for (uint32_t t = millis(); millis() - t < 10000; ) { + if (sendCommand("AT+CIFSR\r\n") && !strstr_P(buffer, PSTR("0.0.0.0"))) { + char *p = strchr(buffer, '\"'); + char *ip = p ? p + 1 : buffer; + if ((p = strchr(ip, '\"')) || (p = strchr(ip, '\r'))) *p = 0; + // output IP address + return ip; + } + delay(500); + } + return ""; +} + +bool UDPClientESP8266AT::open(const char* host, uint16_t port) +{ + for (byte n = 0; n < 3; n++) { + close(); + sprintf_P(buffer, PSTR("AT+CIPSTART=\"UDP\",\"%s\",%u,8000,0\r\n"), host, port); + if (sendCommand(buffer, 3000)) { + return true; + } else { + // check if already connected + if (strstr_P(buffer, PSTR("CONN"))) return true; + } + Serial.println(buffer); + } + close(); + return false; +} + +void UDPClientESP8266AT::close() +{ + sendCommand("AT+CIPCLOSE\r\n", 500); +} + +bool UDPClientESP8266AT::send(const char* data, unsigned int len) +{ + sprintf_P(buffer, PSTR("AT+CIPSEND=%u\r\n"), len); + if (sendCommand(buffer, 100, ">") && sendCommand(data, 1000, "OK")) { + return true; + } else { + Serial.println(buffer); + return false; + } +} + +char* UDPClientESP8266AT::receive(int* pbytes, unsigned int timeout) +{ + if (!rxLen && !sendCommand(0, timeout, "+IPD,")) { + return 0; + } + if (rxLen) { + if (pbytes) *pbytes = rxLen; + rxLen = 0; + return rxBuf; + } + return 0; +} + +bool UDPClientESP8266AT::sendCommand(const char* cmd, unsigned int timeout, const char* expected) +{ + if (cmd) { + m_device->xbWrite(cmd); + } + buffer[0] = 0; + byte ret = m_device->xbReceive(buffer, sizeof(buffer), timeout, &expected, 1); + // reception + char *p = strstr_P(buffer, PSTR("+IPD,")); + if (p) { + p += 5; + rxLen = atoi(p); + char *q = strchr(p, ':'); + if (q++) { + int maxLen = buffer + sizeof(buffer) - q; + if (rxLen > maxLen) rxLen = maxLen; + if (rxBuf) free(rxBuf); + rxBuf = (char*)malloc(rxLen + 1); + memcpy(rxBuf, q, rxLen); + rxBuf[rxLen] = 0; + } else { + rxLen = 0; + } + } + return ret != 0; +} + +/******************************************************************************* + Implementation for SIM800 (SIM800 AT command-set) +*******************************************************************************/ + +bool UDPClientSIM800::begin(CFreematics* device, bool nocheck) +{ + m_device = device; + if (m_stage == 0) { + device->xbBegin(XBEE_BAUDRATE); + m_stage = 1; + } + if (nocheck) { + device->xbTogglePower(); + return true; + } + unsigned long t = millis(); + for (;;) { + for (byte m = 0; m < 3; m++) { + sendCommand("AT\r"); + if (sendCommand("ATE0\r") && sendCommand("ATI\r")) { + m_stage = 2; + return true; + } + } + if (millis() - t >= 30000) break; + device->xbTogglePower(); + delay(2000); + } + return false; +} + +void UDPClientSIM800::end() +{ + sendCommand("AT+CPOWD=1\r"); + m_stage = 1; +} + +bool UDPClientSIM800::setup(const char* apn, unsigned int timeout, bool gps, const char* pin) +{ + uint32_t t = millis(); + bool success = false; + if (pin) { + sprintf_P(m_buffer, PSTR("AT+CPIN=\"%s\"\r"), pin); + sendCommand(m_buffer); + } + do { + sendCommand("AT+CREG?\r"); + success = strstr_P(m_buffer, PSTR("+CREG: 0,1")) || strstr_P(m_buffer, PSTR("+CREG: 0,5")); + Serial.print('.'); + if (success) break; + delay(500); + } while (millis() - t < timeout); + if (!success) return false; + do { + success = sendCommand("AT+CGATT?\r", 3000, "+CGATT: 1"); + } while (!success && millis() - t < timeout); + if (*apn) { + sprintf_P(m_buffer, PSTR("AT+CSTT=\"%s\"\r"), apn); + sendCommand(m_buffer); + } + sendCommand("AT+CIICR\r"); + return success; +} + +String UDPClientSIM800::getIP() +{ + for (uint32_t t = millis(); millis() - t < 60000; ) { + if (sendCommand("AT+CIFSR\r", 3000, ".")) { + char *p; + for (p = m_buffer; *p && !isdigit(*p); p++); + char *q = strchr(p, '\r'); + if (q) *q = 0; + return p; + } + } + return ""; +} + +int UDPClientSIM800::getSignal() +{ + if (sendCommand("AT+CSQ\r", 500)) { + char *p = strchr(m_buffer, ':'); + if (p) { + int csq = atoi(p + 2); + if (csq == 0) + return -115; + else if (csq == 1) + return -111; + else if (csq != 99) + return csq * 2 - 114; + } + } + return 0; +} + +String UDPClientSIM800::getOperatorName() +{ + // display operator name + if (sendCommand("AT+COPS?\r") == 1) { + char *p = strstr(m_buffer, ",\""); + if (p) { + p += 2; + char *s = strchr(p, '\"'); + if (s) *s = 0; + return p; + } + } + return ""; +} + +bool UDPClientSIM800::checkSIM() +{ + return (sendCommand("AT+CPIN?\r") && strstr(m_buffer, "READY")); +} + +bool UDPClientSIM800::open(const char* host, uint16_t port) +{ +#if 0 + if (host) { + if (!isdigit(host[0])) { + String ip = queryIP(host); + if (ip.length()) { + strncpy(udpIP, ip.c_str(), sizeof(udpIP) - 1); + } + } + } +#endif + //sendCommand("AT+CLPORT=\"UDP\",8000\r"); + sendCommand("AT+CIPSRIP=1\r"); + //sendCommand("AT+CIPUDPMODE=1\r"); + sprintf_P(m_buffer, PSTR("AT+CIPSTART=\"UDP\",\"%s\",\"%u\"\r"), host, port); + if (sendCommand(m_buffer, 3000)) return true; + close(); + return false; +} + +void UDPClientSIM800::close() +{ + sendCommand("AT+CIPCLOSE\r"); +} + +bool UDPClientSIM800::send(const char* data, unsigned int len) +{ + sprintf_P(m_buffer, PSTR("AT+CIPSEND=%u\r"), len); + if (sendCommand(m_buffer, 200, ">")) { + m_device->xbWrite(data); + m_device->xbWrite("\r"); + if (sendCommand(0, 5000, "\r\nSEND OK")) { + return true; + } + } + return false; +} + +char* UDPClientSIM800::receive(int* pbytes, unsigned int timeout) +{ + char *data = checkIncoming(pbytes); + if (data) return data; + if (sendCommand(0, timeout, "RECV FROM:")) { + return checkIncoming(pbytes); + } + return 0; +} + +char* UDPClientSIM800::checkIncoming(int* pbytes) +{ + char *p = strstr(m_buffer, "RECV FROM:"); + if (p) p = strchr(p, '\r'); + if (!p) return 0; + while (*(++p) == '\r' || *p =='\n'); + int len = strlen(p); + if (len > 2) { + if (pbytes) *pbytes = len; + return p; + } else { + if (sendCommand("AT\r", 1000, "OK\r\n", true)) { + p = m_buffer; + while (*p && (*p == '\r' || *p == '\n')) p++; + if (pbytes) *pbytes = strlen(p); + return p; + } + } + return 0; +} + +String UDPClientSIM800::queryIP(const char* host) +{ + sprintf_P(m_buffer, PSTR("AT+CDNSGIP=\"%s\"\r"), host); + if (sendCommand(m_buffer, 10000)) { + char *p = strstr(m_buffer, host); + if (p) { + p = strstr(p, ",\""); + if (p) { + char *ip = p + 2; + p = strchr(ip, '\"'); + if (p) *p = 0; + return ip; + } + } + } + return ""; +} + +bool UDPClientSIM800::sendCommand(const char* cmd, unsigned int timeout, const char* expected, bool terminated) +{ + if (cmd) { + m_device->xbWrite(cmd); + } + m_buffer[0] = 0; + byte ret = m_device->xbReceive(m_buffer, sizeof(m_buffer), timeout, &expected, 1); + if (ret) { + if (terminated) { + char *p = strstr(m_buffer, expected); + if (p) *p = 0; + } + return true; + } else { + return false; + } +} + +GPS_DATA* UDPClientSIM800::getLocation() +{ + if (sendCommand("AT+CIPGSMLOC=1,1\r", 3000)) do { + if (!m_gps) m_gps = new GPS_DATA; + char *p; + if (!(p = strchr(m_buffer, ':'))) break; + if (!(p = strchr(p, ','))) break; + float lng = atof(++p); + if (!(p = strchr(p, ','))) break; + float lat = atof(++p); + if (!(p = strchr(p, ','))) break; + int year = atoi(++p); + if (year < 2019) break; + if (!(p = strchr(p, '/'))) break; + int month = atoi(++p); + if (!(p = strchr(p, '/'))) break; + int day = atoi(++p); + if (!m_gps) m_gps = new GPS_DATA; + m_gps->lng = (int32_t)(lng * 1000000); + m_gps->lat = (int32_t)(lat * 1000000); + m_gps->date = (uint32_t)day * 10000 + month * 100 + (year - 2000); + if (!(p = strchr(p, ','))) break; + int hour = atoi(++p); + if (!(p = strchr(p, ':'))) break; + int minute = atoi(++p); + if (!(p = strchr(p, ':'))) break; + int second = atoi(++p); + m_gps->time = (uint32_t)hour * 10000 + minute * 100 + second; + m_gps->speed = 0; + m_gps->alt = 0; + m_gps->heading = 0; + return m_gps; + } while(0); + return 0; +} + +/******************************************************************************* + Implementation for SIM5360 +*******************************************************************************/ + +bool UDPClientSIM5360::begin(CFreematics* device, bool nocheck) +{ + m_device = device; + if (m_stage == 0) { + device->xbBegin(XBEE_BAUDRATE); + m_stage = 1; + } + if (nocheck) { + device->xbTogglePower(); + return true; + } + unsigned long t = millis(); + for (;;) { + if (!sendCommand("AT\r")) sendCommand(0, 5000, "START"); + if (sendCommand("ATE0\r") && sendCommand("ATI\r")) { + m_stage = 2; + return true; + } + if (millis() - t >= 30000) break; + device->xbTogglePower(); + } + return false; +} + +void UDPClientSIM5360::end() +{ + sendCommand("AT+CPOF\r"); + m_stage = 1; +} + +bool UDPClientSIM5360::setup(const char* apn, unsigned int timeout, const char* pin) +{ + uint32_t t = millis(); + bool success = false; + //sendCommand("AT+CNMP=14\r"); // use WCDMA only + if (pin) { + sprintf_P(m_buffer, PSTR("AT+CPIN=\"%s\"\r"), pin); + sendCommand(m_buffer); + } + do { + do { + Serial.print('.'); + delay(1000); + success = sendCommand("AT+CPSI?\r", 1000, "Online"); + if (success) { + if (!strstr_P(m_buffer, PSTR("NO SERVICE"))) + break; + success = false; + if (strstr_P(m_buffer, PSTR("ERROR"))) break; + } else { + if (strstr_P(m_buffer, PSTR("Off"))) break; + } + } while (millis() - t < timeout); + if (!success) break; + + success = false; + do { + if (sendCommand("AT+CREG?\r", 1000, "+CREG: 0,")) { + char *p = strstr(m_buffer, "+CREG: 0,"); + success = (p && (*(p + 9) == '1' || *(p + 9) == '5')); + } + } while (!success && millis() - t < timeout); + if (!success) break; + + success = false; + do { + if (sendCommand("AT+CGREG?\r",1000, "+CGREG: 0,")) { + char *p = strstr(m_buffer, "+CGREG: 0,"); + success = (p && (*(p + 10) == '1' || *(p + 10) == '5')); + } + } while (!success && millis() - t < timeout); + if (!success) break; + + if (*apn) { + sprintf_P(m_buffer, PSTR("AT+CGSOCKCONT=1,\"IP\",\"%s\"\r"), apn); + sendCommand(m_buffer); + } + + sendCommand("AT+CSOCKSETPN=1\r"); + //sendCommand("AT+CSOCKAUTH=,,\"password\",\"password\"\r"); + sendCommand("AT+CIPMODE=0\r"); + sendCommand("AT+NETOPEN\r"); + } while(0); + if (!success) Serial.println(m_buffer); + return success; +} + +bool UDPClientSIM5360::startGPS() +{ + // set GPS antenna voltage + sendCommand("AT+CVAUXV=61\r"); + sendCommand("AT+CVAUXS=1\r"); + if (sendCommand("AT+CGPS=1\r") || sendCommand("AT+CGPS?\r", 100, "+CGPS: 1")) { + if (!m_gps) m_gps = new GPS_DATA; + memset(m_gps, 0, sizeof(GPS_DATA)); + return true; + } + return false; +} + +void UDPClientSIM5360::stopGPS() +{ + sendCommand("AT+CGPS=0\r"); + if (m_gps) { + delete m_gps; + m_gps = 0; + } +} + +String UDPClientSIM5360::getIP() +{ + uint32_t t = millis(); + do { + if (sendCommand("AT+IPADDR\r", 2000)) { + char *p = strstr_P(m_buffer, PSTR("+IPADDR:")); + if (p) { + char *ip = p + 9; + if (*ip != '0') { + char *q = strchr(ip, '\r'); + if (q) *q = 0; + return ip; + } + } + } + delay(500); + } while (millis() - t < 10000); + return ""; +} + +int UDPClientSIM5360::getSignal() +{ + if (sendCommand("AT+CSQ\r", 500)) { + char *p = strchr(m_buffer, ':'); + if (p) { + int csq = atoi(p + 2); + if (csq != 99) { + return csq * 2 - 113; + } + } + } + return 0; +} + +String UDPClientSIM5360::getOperatorName() +{ + // display operator name + if (sendCommand("AT+COPS?\r") == 1) { + char *p = strstr(m_buffer, ",\""); + if (p) { + p += 2; + char *s = strchr(p, '\"'); + if (s) *s = 0; + return p; + } + } + return ""; +} + +bool UDPClientSIM5360::checkSIM() +{ + bool success; + for (byte n = 0; n < 10 && !(success = sendCommand("AT+CPIN?\r", 500, ": READY")); n++); + return success; +} + +bool UDPClientSIM5360::open(const char* host, uint16_t port) +{ + if (host) { + sprintf_P(m_buffer, PSTR("AT+CDNSGIP=\"%s\"\r"), host); + if (sendCommand(m_buffer, 10000, "+CDNSGIP:")) { + char *p = strstr(m_buffer, host); + if (p) { + if ((p = strchr(p, ','))) { + p += 2; + do { + udpIP[0] = atoi(p); + if (!(p = strchr(p, '.'))) break; + udpIP[1] = atoi(++p); + if (!(p = strchr(p, '.'))) break; + udpIP[2] = atoi(++p); + if (!(p = strchr(p, '.'))) break; + udpIP[3] = atoi(++p); + } while(0); + } + } + } + if (!udpIP[0]) { + return false; + } + udpPort = port; + } + sprintf_P(m_buffer, PSTR("AT+CIPOPEN=0,\"UDP\",\"%u.%u.%u.%u\",%u,8000\r"), + udpIP[0], udpIP[1], udpIP[2], udpIP[3], udpPort); + if (sendCommand(m_buffer, 3000)) return true; + close(); + return false; +} + +void UDPClientSIM5360::close() +{ + sendCommand("AT+CIPCLOSE=0\r"); +} + +bool UDPClientSIM5360::send(const char* data, unsigned int len) +{ + sprintf_P(m_buffer, PSTR("AT+CIPSEND=0,%u,\"%u.%u.%u.%u\",%u\r"), + len, udpIP[0], udpIP[1], udpIP[2], udpIP[3], udpPort); + m_device->xbWrite(m_buffer); + delay(10); + return sendCommand(data, 1000); +} + +char* UDPClientSIM5360::receive(int* pbytes, unsigned int timeout) +{ + for (;;) { + checkGPS(); + char *ipd = strstr(m_buffer, "+IPD"); + if (ipd) { + char *payload = checkIncoming(ipd, pbytes); + if (payload) { + *ipd = '-'; + return payload; + } + } + if (!sendCommand("AT+CGPSINFO\r", timeout, "+IPD")) { + checkGPS(); + break; + } + } + return 0; +} + +char* UDPClientSIM5360::checkIncoming(char* ipd, int* pbytes) +{ + unsigned int len = atoi(ipd + 4); + if (len == 0) return 0; + if (pbytes) *pbytes = len; + char *p = strchr(ipd, '\n'); + if (p) { + if (strlen(++p) > len) *(p + len) = 0; + return p; + } + return 0; +} + +long UDPClientSIM5360::parseDegree(const char* s) +{ + char *p; + unsigned long left = atol(s); + unsigned long tenk_minutes = (left % 100UL) * 100000UL; + if ((p = strchr(s, '.'))) + { + unsigned long mult = 10000; + while (isdigit(*++p)) + { + tenk_minutes += mult * (*p - '0'); + mult /= 10; + } + } + return (left / 100) * 1000000 + tenk_minutes / 6; +} + +void UDPClientSIM5360::checkGPS() +{ + char *p; + if (m_gps && (p = strstr_P(m_buffer, PSTR("+CGPSINFO:")))) do { + GPS_DATA gl; + if (!(p = strchr(p, ':'))) break; + if (*(++p) == ',') break; + gl.lat = parseDegree(p); + if (!(p = strchr(p, ','))) break; + if (*(++p) == 'S') gl.lat = -gl.lat; + if (!(p = strchr(p, ','))) break; + gl.lng = parseDegree(++p); + if (!(p = strchr(p, ','))) break; + if (*(++p) == 'W') gl.lng = -gl.lng; + if (!(p = strchr(p, ','))) break; + gl.date = atol(++p); + if (!(p = strchr(p, ','))) break; + gl.time = atol(++p) * 100; + if (!(p = strchr(p, ','))) break; + gl.alt = atoi(++p); + if (!(p = strchr(p, ','))) break; + gl.speed = atof(++p) * 100; + if (!(p = strchr(p, ','))) break; + gl.heading = atoi(++p); + memcpy(m_gps, &gl, sizeof(gl)); + } while (0); +} + +bool UDPClientSIM5360::sendCommand(const char* cmd, unsigned int timeout, const char* expected) +{ + if (cmd) { + m_device->xbWrite(cmd); + } + memset(m_buffer, 0, sizeof(m_buffer)); + byte ret = m_device->xbReceive(m_buffer, sizeof(m_buffer), timeout, &expected, 1); + if (ret) { + return true; + } else { + return false; + } +} + diff --git a/esp32/libraries/FreematicsONE/FreematicsNetwork.h b/esp32/libraries/FreematicsONE/FreematicsNetwork.h new file mode 100644 index 0000000..c08e5fa --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsNetwork.h @@ -0,0 +1,97 @@ +/************************************************************************* +* Freematics Hub Client implementations for ESP8266-AT, SIM800, SIM5360 +* Distributed under BSD license +* Visit http://freematics.com/products/freematics-one for more information +* (C)2017-2019 Stanley Huang +#include "FreematicsBase.h" + +class NullClient +{ +public: + virtual GPS_DATA* getLocation() { return m_gps; } + virtual bool startGPS() { return false; } + virtual void stopGPS() {} +protected: + GPS_DATA* m_gps = 0; +}; + +class UDPClientESP8266AT : public NullClient +{ +public: + bool begin(CFreematics* device, bool nocheck = false); + void end(); + bool setup(const char* ssid, const char* password, unsigned int timeout = 15000); + String getIP(); + float getSignal() { return 0; } + bool open(const char* host, uint16_t port); + void close(); + bool send(const char* data, unsigned int len); + char* receive(int* pbytes = 0, unsigned int timeout = 5000); + char* getBuffer() { return buffer; } +private: + bool sendCommand(const char* cmd, unsigned int timeout = 2000, const char* expected = "OK"); + char* rxBuf = 0; + int rxLen = 0; + CFreematics* m_device = 0; + char buffer[128]; +}; + +class UDPClientSIM800 : public NullClient +{ +public: + bool begin(CFreematics* device, bool nocheck = false); + void end(); + bool setup(const char* apn, unsigned int timeout = 30000, bool gps = false, const char* pin = 0); + String getIP(); + int getSignal(); + String getOperatorName(); + bool checkSIM(); + bool open(const char* host, uint16_t port); + bool send(const char* data, unsigned int len); + void close(); + char* receive(int* pbytes = 0, unsigned int timeout = 5000); + String queryIP(const char* host); + GPS_DATA* getLocation(); + char* getBuffer() { return m_buffer; } +private: + bool sendCommand(const char* cmd, unsigned int timeout = 1000, const char* expected = "\r\nOK", bool terminated = false); + char* checkIncoming(int* pbytes); + char m_buffer[80] = {0}; + uint8_t m_stage = 0; + CFreematics* m_device = 0; +}; + +class UDPClientSIM5360 : public NullClient +{ +public: + bool begin(CFreematics* device, bool nocheck = false); + void end(); + bool setup(const char* apn, unsigned int timeout = 30000, const char* pin = 0); + String getIP(); + int getSignal(); + String getOperatorName(); + bool checkSIM(); + bool startGPS(); + void stopGPS(); + bool open(const char* host, uint16_t port); + void close(); + bool send(const char* data, unsigned int len); + char* receive(int* pbytes = 0, unsigned int timeout = 5000); + char* getBuffer() { return m_buffer; } +private: + // send command and check for expected response + bool sendCommand(const char* cmd, unsigned int timeout = 1000, const char* expected = "\r\nOK"); + char* checkIncoming(char* ipd, int* pbytes); + long parseDegree(const char* s); + void checkGPS(); + char m_buffer[160] = {0}; + byte udpIP[4] = {0}; + uint16_t udpPort = 0; + uint8_t m_stage = 0; + CFreematics* m_device = 0; +}; diff --git a/esp32/libraries/FreematicsONE/FreematicsONE.cpp b/esp32/libraries/FreematicsONE/FreematicsONE.cpp new file mode 100644 index 0000000..8ed5748 --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsONE.cpp @@ -0,0 +1,737 @@ +/************************************************************************* +* Arduino Library for Freematics ONE +* Distributed under BSD license +* Visit http://freematics.com/products/freematics-one for more information +* (C)2012-2018 Stanley Huang +*************************************************************************/ + +#include +#include +#include "FreematicsONE.h" + +//#define XBEE_DEBUG +//#define DEBUG Serial + +static const uint8_t targets[][4] = { + {0x24, 0x4f, 0x42, 0x44}, + {0x24, 0x47, 0x50, 0x53}, + {0x24, 0x47, 0x53, 0x4D} +}; + +static SPISettings settings = SPISettings(250000, MSBFIRST, SPI_MODE0); + +uint16_t hex2uint16(const char *p) +{ + char c = *p; + uint16_t i = 0; + for (uint8_t n = 0; c && n < 4; c = *(++p)) { + if (c >= 'A' && c <= 'F') { + c -= 7; + } else if (c>='a' && c<='f') { + c -= 39; + } else if (c == ' ' && n == 2) { + continue; + } else if (c < '0' || c > '9') { + break; + } + i = (i << 4) | (c & 0xF); + n++; + } + return i; +} + +byte hex2uint8(const char *p) +{ + byte c1 = *p; + byte c2 = *(p + 1); + if (c1 >= 'A' && c1 <= 'F') + c1 -= 7; + else if (c1 >='a' && c1 <= 'f') + c1 -= 39; + else if (c1 < '0' || c1 > '9') + return 0; + + if (c2 >= 'A' && c2 <= 'F') + c2 -= 7; + else if (c2 >= 'a' && c2 <= 'f') + c2 -= 39; + else if (c2 < '0' || c2 > '9') + return 0; + + return c1 << 4 | (c2 & 0xf); +} + +int dumpLine(char* buffer, int len) +{ + int bytesToDump = len >> 1; + for (int i = 0; i < len; i++) { + // find out first line end and discard the first line + if (buffer[i] == '\r' || buffer[i] == '\n') { + // go through all following \r or \n if any + while (++i < len && (buffer[i] == '\r' || buffer[i] == '\n')); + bytesToDump = i; + break; + } + } + memmove(buffer, buffer + bytesToDump, len - bytesToDump); + return bytesToDump; +} + +byte COBDSPI::readDTC(uint16_t codes[], byte maxCodes) +{ + /* + Response example: + 0: 43 04 01 08 01 09 + 1: 01 11 01 15 00 00 00 + */ + byte codesRead = 0; + for (byte n = 0; n < 6; n++) { + char buffer[128]; + sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n); + write(buffer); + if (receive(buffer, sizeof(buffer)) > 0) { + if (!strstr_P(buffer, PSTR("NO DATA"))) { + char *p = strstr_P(buffer, PSTR("43")); + if (p) { + while (codesRead < maxCodes && *p) { + p += 6; + if (*p == '\r') { + p = strchr(p, ':'); + if (!p) break; + p += 2; + } + uint16_t code = hex2uint16(p); + if (code == 0) break; + codes[codesRead++] = code; + } + } + break; + } + } + } + return codesRead; +} + +void COBDSPI::clearDTC() +{ + char buffer[32]; + write("04\r"); + receive(buffer, sizeof(buffer)); +} + +void COBDSPI::lowPowerMode() +{ + char buf[16]; + sendCommand("ATLP\r", buf, sizeof(buf)); +} + +bool COBDSPI::testPID(byte pid) +{ + char buffer[32]; + sprintf_P(buffer, PSTR("%02X%02X\r"), dataMode, pid); + write(buffer); + idleTasks(); + if (receive(buffer, sizeof(buffer)) <= 0 || checkErrorMessage(buffer)) { + return false; + } + return true; +} + +bool COBDSPI::readPID(byte pid, int& result) +{ + char buffer[32]; + // send a single query command + sprintf_P(buffer, PSTR("%02X%02X\r"), dataMode, pid); + write(buffer); + // receive and parse the response + idleTasks(); + char* data = 0; + if (receive(buffer, sizeof(buffer)) > 0 && !checkErrorMessage(buffer)) { + char *p = buffer; + while ((p = strstr(p, "41 "))) { + p += 3; + byte curpid = hex2uint8(p); + if (curpid == pid) { + errors = 0; + p += 2; + if (*p == ' ') { + data = p + 1; + break; + } + } + } + } + if (!data) { + errors++; + return false; + } + result = normalizeData(pid, data); + return true; +} + +int COBDSPI::normalizeData(byte pid, char* data) +{ + int result; + switch (pid) { + case PID_RPM: + case PID_EVAP_SYS_VAPOR_PRESSURE: + result = getLargeValue(data) >> 2; + break; + case PID_FUEL_PRESSURE: + result = getSmallValue(data) * 3; + break; + case PID_COOLANT_TEMP: + case PID_INTAKE_TEMP: + case PID_AMBIENT_TEMP: + case PID_ENGINE_OIL_TEMP: + result = getTemperatureValue(data); + break; + case PID_THROTTLE: + case PID_COMMANDED_EGR: + case PID_COMMANDED_EVAPORATIVE_PURGE: + case PID_FUEL_LEVEL: + case PID_RELATIVE_THROTTLE_POS: + case PID_ABSOLUTE_THROTTLE_POS_B: + case PID_ABSOLUTE_THROTTLE_POS_C: + case PID_ACC_PEDAL_POS_D: + case PID_ACC_PEDAL_POS_E: + case PID_ACC_PEDAL_POS_F: + case PID_COMMANDED_THROTTLE_ACTUATOR: + case PID_ENGINE_LOAD: + case PID_ABSOLUTE_ENGINE_LOAD: + case PID_ETHANOL_FUEL: + case PID_HYBRID_BATTERY_PERCENTAGE: + result = getPercentageValue(data); + break; + case PID_MAF_FLOW: + result = getLargeValue(data) / 100; + break; + case PID_TIMING_ADVANCE: + result = (int)(getSmallValue(data) / 2) - 64; + break; + case PID_DISTANCE: // km + case PID_DISTANCE_WITH_MIL: // km + case PID_TIME_WITH_MIL: // minute + case PID_TIME_SINCE_CODES_CLEARED: // minute + case PID_RUNTIME: // second + case PID_FUEL_RAIL_PRESSURE: // kPa + case PID_ENGINE_REF_TORQUE: // Nm + result = getLargeValue(data); + break; + case PID_CONTROL_MODULE_VOLTAGE: // V + result = getLargeValue(data) / 1000; + break; + case PID_ENGINE_FUEL_RATE: // L/h + result = getLargeValue(data) / 20; + break; + case PID_ENGINE_TORQUE_DEMANDED: // % + case PID_ENGINE_TORQUE_PERCENTAGE: // % + result = (int)getSmallValue(data) - 125; + break; + case PID_SHORT_TERM_FUEL_TRIM_1: + case PID_LONG_TERM_FUEL_TRIM_1: + case PID_SHORT_TERM_FUEL_TRIM_2: + case PID_LONG_TERM_FUEL_TRIM_2: + case PID_EGR_ERROR: + result = ((int)getSmallValue(data) - 128) * 100 / 128; + break; + case PID_FUEL_INJECTION_TIMING: + result = ((int32_t)getLargeValue(data) - 26880) / 128; + break; + case PID_CATALYST_TEMP_B1S1: + case PID_CATALYST_TEMP_B2S1: + case PID_CATALYST_TEMP_B1S2: + case PID_CATALYST_TEMP_B2S2: + result = getLargeValue(data) / 10 - 40; + break; + case PID_AIR_FUEL_EQUIV_RATIO: // 0~200 + result = (long)getLargeValue(data) * 200 / 65536; + break; + default: + result = getSmallValue(data); + } + return result; +} + +float COBDSPI::getVoltage() +{ + char buf[32]; + if (sendCommand("ATRV\r", buf, sizeof(buf)) > 0) { + return atof(buf + 4); + } + return 0; +} + +bool COBDSPI::getVIN(char* buffer, int bufsize) +{ + for (byte n = 0; n < 5; n++) { + if (sendCommand("0902\r", buffer, bufsize)) { + int len = hex2uint16(buffer + sizeof(targets[0])); + char *p = strstr_P(buffer + 4, PSTR("0: 49 02 01")); + if (p) { + char *q = buffer; + p += 11; // skip the header + do { + while (*(++p) == ' '); + for (;;) { + *(q++) = hex2uint8(p); + while (*p && *p != ' ') p++; + while (*p == ' ') p++; + if (!*p || *p == '\r') break; + } + p = strchr(p, ':'); + } while(p); + *q = 0; + if (q - buffer == len - 3) { + return true; + } + } + } + delay(200); + } + return false; +} + +bool COBDSPI::isValidPID(byte pid) +{ + if (pid >= 0x7f) + return true; + pid--; + byte i = pid >> 3; + byte b = 0x80 >> (pid & 0x7); + return pidmap[i] & b; +} + +void COBDSPI::reset() +{ + char buf[32]; + sendCommand("ATR\r", buf, sizeof(buf)); + delay(2000); + errors = 0; +} + +void COBDSPI::end() +{ + char buf[32]; + sendCommand("ATPC\r", buf, sizeof(buf)); + SPI.end(); +} + +byte COBDSPI::checkErrorMessage(const char* buffer) +{ + const char *errmsg[] = {"UNABLE", "ERROR", "TIMEOUT", "NO DATA"}; + for (byte i = 0; i < sizeof(errmsg) / sizeof(errmsg[0]); i++) { + if (strstr(buffer, errmsg[i])) return i + 1; + } + return 0; +} + +bool COBDSPI::init(OBD_PROTOCOLS protocol) +{ + const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"}; + char buffer[64]; + m_state = OBD_DISCONNECTED; + for (byte i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { + if (!sendCommand(initcmd[i], buffer, sizeof(buffer), OBD_TIMEOUT_SHORT)) { + continue; + } + } + if (protocol != PROTO_AUTO) { + sprintf_P(buffer, PSTR("ATSP%u\r"), protocol); + sendCommand(buffer, buffer, sizeof(buffer), OBD_TIMEOUT_SHORT); + } + bool success = sendCommand("010D\r", buffer, sizeof(buffer), OBD_TIMEOUT_SHORT) && !checkErrorMessage(buffer); + // load pid map + memset(pidmap, 0xff, sizeof(pidmap)); + for (byte i = 0; i < 4; i++) { + byte pid = i * 0x20; + sprintf_P(buffer, PSTR("%02X%02X\r"), dataMode, pid); + write(buffer); + delay(10); + if (!receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) || checkErrorMessage(buffer)) break; + for (char *p = buffer; (p = strstr_P(p, PSTR("41 "))); ) { + p += 3; + if (hex2uint8(p) == pid) { + p += 2; + for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) { + pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1); + } + success = true; + } + } + } + if (success) { + m_state = OBD_CONNECTED; + errors = 0; + } + return true; +} + +#ifdef DEBUG +void COBDSPI::debugOutput(const char *s) +{ + DEBUG.print('['); + DEBUG.print(millis()); + DEBUG.print(']'); + DEBUG.println(s); +} +#endif + +byte COBDSPI::begin() +{ + m_target = TARGET_OBD; + pinMode(SPI_PIN_READY, INPUT); + pinMode(SPI_PIN_CS, OUTPUT); + digitalWrite(SPI_PIN_CS, HIGH); + SPI.begin(); + //SPI.setClockDivider(SPI_CLOCK_DIV64); + return getVersion(); +} + +byte COBDSPI::getVersion() +{ + byte version = 0; + for (byte n = 0; n < 30; n++) { + char buffer[32]; + if (sendCommand("ATI\r", buffer, sizeof(buffer), 1000)) { + char *p = strstr_P(buffer, PSTR("OBDUART")); + if (p) { + p += 9; + version = (*p - '0') * 10 + (*(p + 2) - '0'); + if (version) break; + } + } + } + return version; +} + +int COBDSPI::receive(char* buffer, int bufsize, unsigned int timeout) +{ + int n = 0; + bool eos = false; + bool matched = false; + uint32_t t = millis(); + do { + while (digitalRead(SPI_PIN_READY) == HIGH) { + if (millis() - t > timeout) return -1; + } + SPI.beginTransaction(settings); + digitalWrite(SPI_PIN_CS, LOW); + while (digitalRead(SPI_PIN_READY) == LOW && millis() - t < timeout) { + char c = SPI.transfer(' '); + if (eos) continue; + if (!matched) { + if (c == '$') { + buffer[0] = c; + n = 1; + matched = true; + } else if (c == 0x9) { + eos = true; + break; + } + continue; + } + if (n > 6 && c == '.' && buffer[n - 1] == '.' && buffer[n - 2] == '.') { + // SEARCHING... + n = 4; + timeout += OBD_TIMEOUT_LONG; + } else if (c == 0x9) { + eos = true; + break; + } else if (c != 0 && c != 0xff && n < bufsize - 1) { + buffer[n++] = c; + } + } + delay(1); + digitalWrite(SPI_PIN_CS, HIGH); + SPI.endTransaction(); + } while (!eos && millis() - t < timeout); +#ifdef DEBUG + if (!eos && millis() - t >= timeout) { + // timed out + debugOutput("RECV TIMEOUT"); + } +#endif + if (n >= 2 && buffer[n - 1] == '>' && buffer[n - 2] == '\r') n -= 2; + buffer[n] = 0; +#ifdef DEBUG + if (m_target == TARGET_OBD) { + debugOutput(buffer); + } +#endif + // wait for READY pin to restore high level so SPI bus is released + while (digitalRead(SPI_PIN_READY) == LOW); + return n; +} + +void COBDSPI::write(const char* s) +{ +#ifdef DEBUG + debugOutput(s); +#endif + //delay(1); + SPI.beginTransaction(settings); + digitalWrite(SPI_PIN_CS, LOW); + delay(1); + if (*s != '$') { + for (byte i = 0; i < sizeof(targets[0]); i++) { + SPI.transfer(targets[m_target][i]); + } + } + byte c = 0; + for (; *s ;s++) { + c = *s; + SPI.transfer((byte)c); + } + if (c != '\r') SPI.transfer('\r'); + // send terminating byte + SPI.transfer(0x1B); + delay(1); + digitalWrite(SPI_PIN_CS, HIGH); + SPI.endTransaction(); +} + +byte COBDSPI::sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout) +{ + uint32_t t = millis(); + int n = 0; + for (byte i = 0; i < 30 && millis() - t < timeout; i++) { + write(cmd); + n = receive(buf, bufsize, timeout); + if (n == -1) { + t = millis(); + Serial.print('_'); + continue; + } + if (n == 0 || (buf[1] != 'O' && !memcmp_P(buf + 5, PSTR("NO DATA"), 7))) { + // data not ready + delay(50); + i = 0; + } else { + break; + } + } + return n; +} + +bool COBDSPI::ioConfig(byte pin, IO_PIN_MODE mode) +{ + // mode: 0: digital input, 1: digial output + char buf[32]; + sprintf_P(buf, PSTR("ATMXCFG %u,%u\r"), pin, mode); + return sendCommand(buf, buf, sizeof(buf), 100) && strstr_P(buf, PSTR("OK")); +} + +bool COBDSPI::ioWrite(byte pin, byte level) +{ + char buf[32]; + sprintf_P(buf, PSTR("ATMXSET %u,%u\r"), pin, level); + return sendCommand(buf, buf, sizeof(buf), 100) && strstr_P(buf, PSTR("OK")); +} + +byte COBDSPI::ioRead() +{ + char buf[32]; + sprintf_P(buf, PSTR("ATMXGET\r")); + sendCommand(buf, buf, sizeof(buf), 50); + char *p = strstr_P(buf, PSTR("MX:")); + if (!p || *(p + 4) != ',') return -1; + return (*(p + 3) - '0') | (*(p + 5) - '0') << 1; +} + +bool COBDSPI::gpsInit(unsigned long baudrate) +{ + bool success = false; + char buf[64]; + if (baudrate) { + if (sendCommand("ATGPSON\r", buf, sizeof(buf))) { + sprintf_P(buf, PSTR("ATBR2%lu\r"), baudrate); + sendCommand(buf, buf, sizeof(buf)); + uint32_t t = millis(); + delay(100); + do { + if (gpsGetRawData(buf, sizeof(buf)) && strstr_P(buf, PSTR("S$G"))) { + return true; + } + } while (millis() - t < GPS_INIT_TIMEOUT); + } + } else { + success = true; + } + //turn off GPS power + sendCommand("ATGPSOFF\r", buf, sizeof(buf)); + return success; +} + +bool COBDSPI::gpsGetData(GPS_DATA* gdata) +{ + char buf[128]; + if (sendCommand("ATGPS\r", buf, sizeof(buf), GPS_READ_TIMEOUT) == 0) { + return false; + } + if (memcmp_P(buf, PSTR("$GPS,"), 5)) { + return false; + } + + byte index = 0; + bool valid = true; + char *s = buf + 5; + for (char* p = s; *p && valid; p++) { + char c = *p; + if (c == ',' || c == '>' || c <= 0x0d) { + long value = atol(s); + switch (index) { + case 0: + if (value == 0) + valid = false; + else + gdata->date = (uint32_t)value; + break; + case 1: + gdata->time = (uint32_t)value; + break; + case 2: + gdata->lat = value; + break; + case 3: + gdata->lng = value; + break; + case 4: + gdata->alt = value / 100; + break; + case 5: + gdata->speed = value * 100000 / 1852; + break; + case 6: + gdata->heading = value; + break; + case 7: + gdata->sat = value; + break; + default: + valid = false; + } + index++; + if (c != ',') break; + s = p + 1; + } + } + return valid && index > 7; +} + +int COBDSPI::gpsGetRawData(char* buf, int bufsize) +{ + int n = sendCommand("ATGRR\r", buf, bufsize, GPS_READ_TIMEOUT); + if (n > 2) { + n -= 2; + buf[n] = 0; + } + return n; +} + +void COBDSPI::gpsSendCommand(const char* cmd) +{ + m_target = TARGET_GPS; + write(cmd); + m_target = TARGET_OBD; +} + +bool COBDSPI::xbBegin(unsigned long baudrate) +{ + char buf[16]; + sprintf_P(buf, PSTR("ATBR1%lu\r"), baudrate); + if (sendCommand(buf, buf, sizeof(buf))) { + xbPurge(); + return true; + } else { + return false; + } +} + +void COBDSPI::xbWrite(const char* cmd) +{ +#ifdef XBEE_DEBUG + Serial.print("[SEND]"); + Serial.println(cmd); + Serial.println("[/SEND]"); +#endif + m_target = TARGET_BEE; + write(cmd); + m_target = TARGET_OBD; +} + +int COBDSPI::xbRead(char* buffer, int bufsize) +{ + write("ATGRD\r"); + delay(10); + int bytes = receive(buffer, bufsize, 500); + if (bytes < 4 || memcmp(buffer, targets[TARGET_BEE], 4)) { + bytes = -1; + } else if (bytes >= 11 && !memcmp_P(buffer + 4, PSTR("NO DATA"), 7)) { + buffer[0] = 0; + bytes = 0; + } + return bytes; +} + +int COBDSPI::xbReceive(char* buffer, int bufsize, unsigned int timeout, const char** expected, byte expectedCount) +{ + int bytesRecv = 0; + uint32_t t = millis(); + buffer[0] = 0; + do { + while (bytesRecv >= bufsize / 2) { + bytesRecv -= dumpLine(buffer, bytesRecv); + } + int n = xbRead(buffer + bytesRecv, bufsize - bytesRecv); + if (n > 0) { + buffer[bytesRecv + n] = 0; + // skip 4-byte header + char *p = buffer + bytesRecv + 4; + n -= 4; + if (bytesRecv == 0 && *p == '\n') { + // strip first \n + p++; + n--; + } + memmove(buffer + bytesRecv, p, n); + bytesRecv += n; + buffer[bytesRecv] = 0; +#ifdef XBEE_DEBUG + if (n > 0) { + Serial.print("[RECV]"); + Serial.print(buffer + bytesRecv - n); + Serial.println("[/RECV]"); + } +#endif + if (expectedCount == 0 && bytesRecv > 0) { + return bytesRecv; + } + for (byte i = 0; i < expectedCount; i++) { + // match expected string(s) + if (expected[i] && strstr(buffer, expected[i])) return i + 1; + } + delay(50); + } if (n < 0) { + break; + } else { + if (millis() - t + 200 < timeout) + delay(200); + else + break; + } + } while (millis() - t < timeout); + return 0; +} + +void COBDSPI::xbPurge() +{ + char buf[32]; + sendCommand("ATCLRGSM\r", buf, sizeof(buf)); +} + +void COBDSPI::xbTogglePower() +{ + char buf[32]; + sendCommand("ATGSMPWR\r", buf, sizeof(buf)); +} diff --git a/esp32/libraries/FreematicsONE/FreematicsONE.h b/esp32/libraries/FreematicsONE/FreematicsONE.h new file mode 100644 index 0000000..1176369 --- /dev/null +++ b/esp32/libraries/FreematicsONE/FreematicsONE.h @@ -0,0 +1,200 @@ +/************************************************************************* +* Arduino Library for Freematics ONE +* Distributed under BSD license +* Visit https://freematics.com/products/freematics-one for more information +* (C)2012-2018 Stanley Huang +*************************************************************************/ + +#pragma once + +#include "FreematicsBase.h" +#include "FreematicsMEMS.h" + +#define OBD_TIMEOUT_SHORT 1000 /* ms */ +#define OBD_TIMEOUT_LONG 10000 /* ms */ +#define OBD_SERIAL_BAUDRATE 38400 + +#define GPS_READ_TIMEOUT 200 /* ms */ +#define GPS_INIT_TIMEOUT 1000 /* ms */ + +// Mode 1 PIDs +#define PID_ENGINE_LOAD 0x04 +#define PID_COOLANT_TEMP 0x05 +#define PID_SHORT_TERM_FUEL_TRIM_1 0x06 +#define PID_LONG_TERM_FUEL_TRIM_1 0x07 +#define PID_SHORT_TERM_FUEL_TRIM_2 0x08 +#define PID_LONG_TERM_FUEL_TRIM_2 0x09 +#define PID_FUEL_PRESSURE 0x0A +#define PID_INTAKE_MAP 0x0B +#define PID_RPM 0x0C +#define PID_SPEED 0x0D +#define PID_TIMING_ADVANCE 0x0E +#define PID_INTAKE_TEMP 0x0F +#define PID_MAF_FLOW 0x10 +#define PID_THROTTLE 0x11 +#define PID_AUX_INPUT 0x1E +#define PID_RUNTIME 0x1F +#define PID_DISTANCE_WITH_MIL 0x21 +#define PID_COMMANDED_EGR 0x2C +#define PID_EGR_ERROR 0x2D +#define PID_COMMANDED_EVAPORATIVE_PURGE 0x2E +#define PID_FUEL_LEVEL 0x2F +#define PID_WARMS_UPS 0x30 +#define PID_DISTANCE 0x31 +#define PID_EVAP_SYS_VAPOR_PRESSURE 0x32 +#define PID_BAROMETRIC 0x33 +#define PID_CATALYST_TEMP_B1S1 0x3C +#define PID_CATALYST_TEMP_B2S1 0x3D +#define PID_CATALYST_TEMP_B1S2 0x3E +#define PID_CATALYST_TEMP_B2S2 0x3F +#define PID_CONTROL_MODULE_VOLTAGE 0x42 +#define PID_ABSOLUTE_ENGINE_LOAD 0x43 +#define PID_AIR_FUEL_EQUIV_RATIO 0x44 +#define PID_RELATIVE_THROTTLE_POS 0x45 +#define PID_AMBIENT_TEMP 0x46 +#define PID_ABSOLUTE_THROTTLE_POS_B 0x47 +#define PID_ABSOLUTE_THROTTLE_POS_C 0x48 +#define PID_ACC_PEDAL_POS_D 0x49 +#define PID_ACC_PEDAL_POS_E 0x4A +#define PID_ACC_PEDAL_POS_F 0x4B +#define PID_COMMANDED_THROTTLE_ACTUATOR 0x4C +#define PID_TIME_WITH_MIL 0x4D +#define PID_TIME_SINCE_CODES_CLEARED 0x4E +#define PID_ETHANOL_FUEL 0x52 +#define PID_FUEL_RAIL_PRESSURE 0x59 +#define PID_HYBRID_BATTERY_PERCENTAGE 0x5B +#define PID_ENGINE_OIL_TEMP 0x5C +#define PID_FUEL_INJECTION_TIMING 0x5D +#define PID_ENGINE_FUEL_RATE 0x5E +#define PID_ENGINE_TORQUE_DEMANDED 0x61 +#define PID_ENGINE_TORQUE_PERCENTAGE 0x62 +#define PID_ENGINE_REF_TORQUE 0x63 + +typedef enum { + PROTO_AUTO = 0, + PROTO_ISO_9141_2 = 3, + PROTO_KWP2000_5KBPS = 4, + PROTO_KWP2000_FAST = 5, + PROTO_CAN_11B_500K = 6, + PROTO_CAN_29B_500K = 7, + PROTO_CAN_29B_250K = 8, + PROTO_CAN_11B_250K = 9, +} OBD_PROTOCOLS; + +// states +typedef enum { + OBD_DISCONNECTED = 0, + OBD_CONNECTING = 1, + OBD_CONNECTED = 2, + OBD_FAILED = 3 +} OBD_STATES; + +// IO PIN modes +typedef enum { + IO_PIN_INPUT = 0, + IO_PIN_OUTPUT = 1 +} IO_PIN_MODE; + +uint16_t hex2uint16(const char *p); +uint8_t hex2uint8(const char *p); + +#define SPI_PIN_CS 7 +#define SPI_PIN_READY 6 + +#define TARGET_OBD 0 +#define TARGET_GPS 1 +#define TARGET_BEE 2 +#define TARGET_RAW 3 + +class COBDSPI : public CFreematics { +public: + byte begin(); + void end(); + // initialize OBD-II connection + bool init(OBD_PROTOCOLS protocol = PROTO_AUTO); + // reset OBD + void reset(); + // read specified OBD-II PID value + bool readPID(byte pid, int& result); + // test PID reading + bool testPID(byte pid); + // send AT command and receive response + byte sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout = OBD_TIMEOUT_LONG); + // initialize GPS (set baudrate to 0 to power off GPS) + bool gpsInit(unsigned long baudrate = 115200L); + // get parsed GPS data + bool gpsGetData(GPS_DATA* gdata); + // get GPS NMEA data + int gpsGetRawData(char* buf, int bufsize); + // send command string to GPS + void gpsSendCommand(const char* cmd); + // start xBee UART communication + bool xbBegin(unsigned long baudrate = 115200L); + // read data to xBee UART + int xbRead(char* buffer, int bufsize); + // send data to xBee UART + void xbWrite(const char* cmd); + // receive data from xBee UART + int xbReceive(char* buffer, int bufsize, unsigned int timeout, const char** expected = 0, byte expectedCount = 0); + // purge xBee UART buffer + void xbPurge(); + // toggle xBee module power + void xbTogglePower(); + // set I/O pin mode (pin: 1/2, mode: input/output) + bool ioConfig(byte pin, IO_PIN_MODE mode); + // set I/O pin level (pin: 1/2, level: 0/1) + bool ioWrite(byte pin, byte level); + // get I/O pin level (bit 0 for pin 1, bit 1 for pin 2) + byte ioRead(); + // get connection state + OBD_STATES getState() { return m_state; } + // read diagnostic trouble codes (return number of DTCs read) + byte readDTC(uint16_t codes[], byte maxCodes = 1); + // clear diagnostic trouble code + void clearDTC(); + // get battery voltage (works without ECU) + float getVoltage(); + // get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE + bool getVIN(char* buffer, int bufsize); + // determine if the PID is supported + bool isValidPID(byte pid); + // enter low power mode + void lowPowerMode(); + // get firmware version + byte getVersion(); + // set current PID mode + byte dataMode = 1; + // occurrence of errors + byte errors = 0; + // bit map of supported PIDs + byte pidmap[4 * 4] = {0}; +protected: + // write data to SPI bus + void write(const char* s); + // receive data from SPI bus + int receive(char* buffer, int bufsize, unsigned int timeout = OBD_TIMEOUT_SHORT); + // set SPI data target + void debugOutput(const char* s); + int normalizeData(byte pid, char* data); + virtual void idleTasks() { delay(1); } + OBD_STATES m_state = OBD_DISCONNECTED; + byte m_target = TARGET_OBD; +private: + uint8_t getPercentageValue(char* data) + { + return (uint16_t)hex2uint8(data) * 100 / 255; + } + uint16_t getLargeValue(char* data) + { + return hex2uint16(data); + } + uint8_t getSmallValue(char* data) + { + return hex2uint8(data); + } + int16_t getTemperatureValue(char* data) + { + return (int)hex2uint8(data) - 40; + } + byte checkErrorMessage(const char* buffer); +}; diff --git a/esp32/libraries/FreematicsONE/keywords.txt b/esp32/libraries/FreematicsONE/keywords.txt new file mode 100644 index 0000000..feed9b5 --- /dev/null +++ b/esp32/libraries/FreematicsONE/keywords.txt @@ -0,0 +1,58 @@ +################################################################################ +# Datatypes (KEYWORD1) +################################################################################ +COBDSPI KEYWORD1 +MPU9250_ACC KEYWORD1 +MPU9250_9DOF KEYWORD1 +MPU9250_DMP KEYWORD1 +UDPClientESP8266AT KEYWORD1 +UDPClientSIM800 KEYWORD1 +UDPClientSIM5360 KEYWORD1 + +################################################################################ +# Methods and Functions (KEYWORD2) +################################################################################ +begin KEYWORD2 +end KEYWORD2 +init KEYWORD2 +uninit KEYWORD2 +reset KEYWORD2 +sleep KEYWORD2 +enterLowPowerMode KEYWORD2 +leaveLowPowerMode KEYWORD2 +readPID KEYWORD2 +readDTC KEYWORD2 +clearDTC KEYWORD2 +getVoltage KEYWORD2 +getVIN KEYWORD2 + +gpsInit KEYWORD2 +gpsGetData KEYWORD2 +gpsGetRawData KEYWORD2 +gpsSendCommand KEYWORD2 +internalGPS KEYWORD2 + +xbBegin KEYWORD2 +xbRead KEYWORD2 +xbWrite KEYWORD2 +xbReceive KEYWORD2 +xbPurge KEYWORD2 +xbTogglePower KEYWORD2 + +################################################################################ +# Constants (LITERAL1) +################################################################################ +OBD_PROTOCOLS LITERAL1 +OBD_STATES LITERAL1 +GPS_DATA LITERAL1 +NET_LOCATION LITERAL1 +INV_X_GYRO LITERAL1 +INV_Y_GYRO LITERAL1 +INV_Z_GYRO LITERAL1 +ORIENTATION LITERAL1 +EVENT_LOGIN LITERAL1 +EVENT_LOGOUT LITERAL1 +EVENT_SYNC LITERAL1 +EVENT_RECONNECT LITERAL1 +EVENT_COMMAND LITERAL1 +EVENT_ACK LITERAL1 diff --git a/esp32/libraries/FreematicsONE/library.json b/esp32/libraries/FreematicsONE/library.json new file mode 100644 index 0000000..fdc3767 --- /dev/null +++ b/esp32/libraries/FreematicsONE/library.json @@ -0,0 +1,18 @@ +{ + "name": "FreematicsONE", + "description": "A cost-effective Arduino programmable vehicle telemetry device in form of OBD dongle with optional BLE or cellular network support.", + "keywords": "obd, freematics, telematics", + "authors": + { + "name": "Stanley Huang", + "email": "stanleyhuangyc@gmail.com", + "url": "https://freematics.com" + }, + "repository": { + "type": "git", + "url": "https://github.com/stanleyhuangyc/Freematics.git" + }, + "include": "firmware_v4/libraries/FreematicsONE", + "frameworks": "arduino", + "platforms": "*" +} diff --git a/esp32/libraries/FreematicsONE/library.properties b/esp32/libraries/FreematicsONE/library.properties new file mode 100644 index 0000000..62e4367 --- /dev/null +++ b/esp32/libraries/FreematicsONE/library.properties @@ -0,0 +1,10 @@ +name=FreematicsONE +version=1.0 +author=Stanley Huang +maintainer=Stanley Huang +sentence=Arduino library for Freematics ONE +paragraph=Arduino library for Freematics ONE +category=Communication +url=http://freematics.com +architectures=avr +includes=FreematicsONE.h \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/FreematicsBase.h b/esp32/libraries/FreematicsPlus/FreematicsBase.h new file mode 100644 index 0000000..052ec11 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsBase.h @@ -0,0 +1,96 @@ +/************************************************************************* +* Base class for Freematics telematics products +* Distributed under BSD license +* Visit https://freematics.com for more information +* (C)2017-2018 Stanley Huang + +// non-OBD/custom PIDs (no mode number) +#define PID_GPS_LATITUDE 0xA +#define PID_GPS_LONGITUDE 0xB +#define PID_GPS_ALTITUDE 0xC +#define PID_GPS_SPEED 0xD +#define PID_GPS_HEADING 0xE +#define PID_GPS_SAT_COUNT 0xF +#define PID_GPS_TIME 0x10 +#define PID_GPS_DATE 0x11 +#define PID_GPS_HDOP 0x12 +#define PID_ACC 0x20 +#define PID_GYRO 0x21 +#define PID_COMPASS 0x22 +#define PID_BATTERY_VOLTAGE 0x24 +#define PID_ORIENTATION 0x25 + +// custom PIDs +#define PID_TIMESTAMP 0 +#define PID_TRIP_DISTANCE 0x30 +#define PID_DATA_SIZE 0x80 +#define PID_CSQ 0x81 +#define PID_DEVICE_TEMP 0x82 +#define PID_DEVICE_HALL 0x83 +#define PID_EXT_SENSOR1 0x90 +#define PID_EXT_SENSOR2 0x91 + +typedef struct { + float pitch; + float yaw; + float roll; +} ORIENTATION; + +typedef struct { + uint32_t ts; + uint32_t date; + uint32_t time; + float lat; + float lng; + float alt; /* meter */ + float speed; /* knot */ + uint16_t heading; /* degree */ + uint8_t hdop; + uint8_t sat; + uint16_t sentences; + uint16_t errors; +} GPS_DATA; + +class CLink +{ +public: + virtual ~CLink() {} + virtual bool begin(unsigned int baudrate = 0, int rxPin = 0, int txPin = 0) { return true; } + virtual void end() {} + // send command and receive response + virtual int sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout) { return 0; } + // receive data from SPI + virtual int receive(char* buffer, int bufsize, unsigned int timeout) { return 0; } + // write data to SPI + virtual bool send(const char* str) { return false; } + virtual int read() { return -1; } +}; + +class CFreematics +{ +public: + virtual void begin() {} + // start xBee UART communication + virtual bool xbBegin(unsigned long baudrate = 115200L, int pinRx = 0, int pinTx = 0) = 0; + virtual void xbEnd() {} + // read data to xBee UART + virtual int xbRead(char* buffer, int bufsize, unsigned int timeout = 1000) = 0; + // send data to xBee UART + virtual void xbWrite(const char* cmd) = 0; + // send data to xBee UART + virtual void xbWrite(const char* data, int len) = 0; + // receive data from xBee UART (returns 0/1/2) + virtual int xbReceive(char* buffer, int bufsize, unsigned int timeout = 1000, const char** expected = 0, byte expectedCount = 0) = 0; + // purge xBee UART buffer + virtual void xbPurge() = 0; + // toggle xBee module power + virtual void xbTogglePower(unsigned int duration = 500) = 0; +}; + +#endif diff --git a/esp32/libraries/FreematicsPlus/FreematicsGPS.cpp b/esp32/libraries/FreematicsPlus/FreematicsGPS.cpp new file mode 100644 index 0000000..bded445 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsGPS.cpp @@ -0,0 +1,450 @@ +/* +TinyGPS - a small GPS library for Arduino providing basic NMEA parsing +Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers. +Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson. +Copyright (C) 2008-2012 Mikal Hart +All rights reserved. + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#include "FreematicsGPS.h" + +#define _GPRMC_TERM "GPRMC" +#define _GPGGA_TERM "GPGGA" + +TinyGPS::TinyGPS() + : _time(GPS_INVALID_TIME) + , _date(GPS_INVALID_DATE) + , _latitude(GPS_INVALID_ANGLE) + , _longitude(GPS_INVALID_ANGLE) + , _altitude(GPS_INVALID_ALTITUDE) + , _speed(GPS_INVALID_SPEED) + , _course(GPS_INVALID_ANGLE) + , _hdop(GPS_INVALID_HDOP) + , _numsats(GPS_INVALID_SATELLITES) + , _last_time_fix(GPS_INVALID_FIX_TIME) + , _last_position_fix(GPS_INVALID_FIX_TIME) + , _parity(0) + , _is_checksum_term(false) + , _sentence_type(_GPS_SENTENCE_OTHER) + , _term_number(0) + , _term_offset(0) + , _gps_data_good(false) +#ifndef _GPS_NO_STATS + , _good_sentences(0) + , _failed_checksum(0) +#endif +{ + _term[0] = '\0'; +} + +// +// public methods +// + +bool TinyGPS::encode(char c) +{ + bool valid_sentence = false; + + switch(c) + { + case ',': // term terminators + _parity ^= ','; + case '\r': + case '\n': + case '*': + if (_term_offset < sizeof(_term)) + { + _term[_term_offset] = 0; + valid_sentence = term_complete(); + } + ++_term_number; + _term_offset = 0; + _is_checksum_term = c == '*'; + return valid_sentence; + + case '$': // sentence begin + _term_number = _term_offset = 0; + _parity = 0; + _sentence_type = _GPS_SENTENCE_OTHER; + _is_checksum_term = false; + _gps_data_good = false; + return valid_sentence; + } + + // ordinary characters + if (_term_offset < sizeof(_term) - 1) + _term[_term_offset++] = c; + if (!_is_checksum_term) + _parity ^= (byte)c; + + return valid_sentence; +} + +#ifndef _GPS_NO_STATS +void TinyGPS::stats(unsigned short *sentences, unsigned short *failed_cs) +{ + if (sentences) *sentences = _good_sentences; + if (failed_cs) *failed_cs = _failed_checksum; +} +#endif + +// +// internal utilities +// +byte TinyGPS::from_hex(char a) +{ + if (a >= 'A' && a <= 'F') + return a - 'A' + 10; + else if (a >= 'a' && a <= 'f') + return a - 'a' + 10; + else + return a - '0'; +} + +byte TinyGPS::hex2uint8(const char *p) +{ + byte c1 = *p; + byte c2 = *(p + 1); + if (c1 >= 'A' && c1 <= 'F') + c1 -= 7; + else if (c1 >= 'a' && c1 <= 'f') + c1 -= 39; + else if (c1 < '0' || c1 > '9') + return 0; + + if (c2 == 0) + return (c1 & 0xf); + else if (c2 >= 'A' && c2 <= 'F') + c2 -= 7; + else if (c2 >= 'a' && c2 <= 'f') + c2 -= 39; + else if (c2 < '0' || c2 > '9') + return 0; + + return c1 << 4 | (c2 & 0xf); +} + +unsigned long TinyGPS::parse_decimal() +{ + char *p = _term; + bool isneg = *p == '-'; + if (isneg) ++p; + unsigned long ret = 100UL * atol(p); + while (isdigit(*p)) ++p; + if (*p == '.') + { + if (isdigit(p[1])) + { + ret += 10 * (p[1] - '0'); + if (isdigit(p[2])) + ret += p[2] - '0'; + } + } + return isneg ? -ret : ret; +} + +unsigned long TinyGPS::parse_degrees() +{ + char *p; + unsigned long left = atol(_term); + unsigned long tenk_minutes = (left % 100UL) * 100000UL; + for (p=_term; isdigit(*p); ++p); + if (*p == '.') + { + unsigned long mult = 10000; + while (isdigit(*++p)) + { + tenk_minutes += mult * (*p - '0'); + mult /= 10; + } + } + return (left / 100) * 1000000 + tenk_minutes / 6; +} + +#define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number) + +// Processes a just-completed term +// Returns true if new sentence has just passed checksum test and is validated +bool TinyGPS::term_complete() +{ + if (_is_checksum_term) + { + byte checksum = hex2uint8(_term); + if (checksum == _parity) + { +#ifndef _GPS_NO_STATS + ++_good_sentences; +#endif + if (_gps_data_good) + { + _last_time_fix = _new_time_fix; + _last_position_fix = _new_position_fix; + + switch(_sentence_type) + { + case _GPS_SENTENCE_GPRMC: + _time = _new_time; + _date = _new_date; + _latitude = _new_latitude; + _longitude = _new_longitude; + _speed = _new_speed; + _course = _new_course; + break; + case _GPS_SENTENCE_GPGGA: + _altitude = _new_altitude; + _time = _new_time; + _latitude = _new_latitude; + _longitude = _new_longitude; + _numsats = _new_numsats; + _hdop = _new_hdop; + break; + } + + return true; + } + } + +#ifndef _GPS_NO_STATS + else + ++_failed_checksum; +#endif + return false; + } + + // the first term determines the sentence type + if (_term_number == 0) + { + if (!gpsstrcmp(_term, _GPRMC_TERM)) + _sentence_type = _GPS_SENTENCE_GPRMC; + else if (!gpsstrcmp(_term, _GPGGA_TERM)) + _sentence_type = _GPS_SENTENCE_GPGGA; + else + _sentence_type = _GPS_SENTENCE_OTHER; + return false; + } + + if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) + switch(COMBINE(_sentence_type, _term_number)) + { + case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences + case COMBINE(_GPS_SENTENCE_GPGGA, 1): + _new_time = parse_decimal(); + _new_time_fix = millis(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity + _gps_data_good = _term[0] == 'A'; + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude + case COMBINE(_GPS_SENTENCE_GPGGA, 2): + _new_latitude = parse_degrees(); + _new_position_fix = millis(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S + case COMBINE(_GPS_SENTENCE_GPGGA, 3): + if (_term[0] == 'S') + _new_latitude = -_new_latitude; + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude + case COMBINE(_GPS_SENTENCE_GPGGA, 4): + _new_longitude = parse_degrees(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W + case COMBINE(_GPS_SENTENCE_GPGGA, 5): + if (_term[0] == 'W') + _new_longitude = -_new_longitude; + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC) + _new_speed = parse_decimal(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC) + _new_course = parse_decimal(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC) + _new_date = atol(_term); + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA) + _gps_data_good = _term[0] > '0'; + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA) + _new_numsats = (unsigned char)atoi(_term); + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP + _new_hdop = parse_decimal(); + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA) + _new_altitude = parse_decimal(); + break; + } + + return false; +} + +int TinyGPS::gpsstrcmp(const char *str1, const char *str2) +{ + if (*str1 == *str2) { + str1 += 2; + str2 += 2; + while (*str1 && *str1 == *str2) { + ++str1; + ++str2; + } + } + return *str1; +} + +/* static */ +float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) +{ + // returns distance in meters between two positions, both specified + // as signed decimal-degrees latitude and longitude. Uses great-circle + // distance computation for hypothetical sphere of radius 6372795 meters. + // Because Earth is no exact sphere, rounding errors may be up to 0.5%. + // Courtesy of Maarten Lamers + float delta = radians(long1-long2); + float sdlong = sin(delta); + float cdlong = cos(delta); + lat1 = radians(lat1); + lat2 = radians(lat2); + float slat1 = sin(lat1); + float clat1 = cos(lat1); + float slat2 = sin(lat2); + float clat2 = cos(lat2); + delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); + delta = sq(delta); + delta += sq(clat2 * sdlong); + delta = sqrt(delta); + float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); + delta = atan2(delta, denom); + return delta * 6372795; +} + +float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) +{ + // returns course in degrees (North=0, West=270) from position 1 to position 2, + // both specified as signed decimal-degrees latitude and longitude. + // Because Earth is no exact sphere, calculated course may be off by a tiny fraction. + // Courtesy of Maarten Lamers + float dlon = radians(long2-long1); + lat1 = radians(lat1); + lat2 = radians(lat2); + float a1 = sin(dlon) * cos(lat2); + float a2 = sin(lat1) * cos(lat2) * cos(dlon); + a2 = cos(lat1) * sin(lat2) - a2; + a2 = atan2(a1, a2); + if (a2 < 0.0) + { + a2 += TWO_PI; + } + return degrees(a2); +} + +const char *TinyGPS::cardinal (float course) +{ + static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"}; + + int direction = (int)((course + 11.25f) / 22.5f); + return directions[direction % 16]; +} + +// lat/long in hundred thousandths of a degree and age of fix in milliseconds +void TinyGPS::get_position(long *latitude, long *longitude, unsigned long *fix_age) +{ + if (latitude) *latitude = _latitude; + if (longitude) *longitude = _longitude; + if (fix_age) { + if (_last_position_fix == GPS_INVALID_FIX_TIME) + *fix_age = GPS_INVALID_AGE; + else + *fix_age = millis() - _last_position_fix; + } +} + +// date as ddmmyy, time as hhmmsscc, and age in milliseconds +void TinyGPS::get_datetime(unsigned long *date, unsigned long *time, unsigned long *age) +{ + if (date) *date = _date; + if (time) *time = _time; + if (age) { + if (_last_time_fix == GPS_INVALID_FIX_TIME) + *age = GPS_INVALID_AGE; + else + *age = millis() - _last_time_fix; + } +} + +void TinyGPS::f_get_position(float *latitude, float *longitude, unsigned long *fix_age) +{ + long lat, lon; + get_position(&lat, &lon, fix_age); + *latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 100000.0); + *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 100000.0); +} + +void TinyGPS::crack_datetime(int *year, byte *month, byte *day, + byte *hour, byte *minute, byte *second, byte *hundredths, unsigned long *age) +{ + unsigned long date, time; + get_datetime(&date, &time, age); + if (year) + { + *year = date % 100; + *year += *year > 80 ? 1900 : 2000; + } + if (month) *month = (date / 100) % 100; + if (day) *day = date / 10000; + if (hour) *hour = time / 1000000; + if (minute) *minute = (time / 10000) % 100; + if (second) *second = (time / 100) % 100; + if (hundredths) *hundredths = time % 100; +} + +float TinyGPS::f_altitude() +{ + return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0; +} + +float TinyGPS::f_course() +{ + return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0; +} + +float TinyGPS::f_speed_knots() +{ + return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0; +} + +float TinyGPS::f_speed_mph() +{ + float sk = f_speed_knots(); + return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * f_speed_knots(); +} + +float TinyGPS::f_speed_mps() +{ + float sk = f_speed_knots(); + return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * f_speed_knots(); +} + +float TinyGPS::f_speed_kmph() +{ + float sk = f_speed_knots(); + return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * f_speed_knots(); +} + +const float TinyGPS::GPS_INVALID_F_ANGLE = 1000.0; +const float TinyGPS::GPS_INVALID_F_ALTITUDE = 1000000.0; +const float TinyGPS::GPS_INVALID_F_SPEED = -1.0; diff --git a/esp32/libraries/FreematicsPlus/FreematicsGPS.h b/esp32/libraries/FreematicsPlus/FreematicsGPS.h new file mode 100644 index 0000000..85cf612 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsGPS.h @@ -0,0 +1,145 @@ +/* +TinyGPS - a small GPS library for Arduino providing basic NMEA parsing +Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers. +Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson. +Copyright (C) 2008-2012 Mikal Hart +All rights reserved. + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef TinyGPS_h +#define TinyGPS_h + +#include "Arduino.h" + +#define _GPS_VERSION 12 // software version of this library +#define _GPS_MPH_PER_KNOT 1.15077945 +#define _GPS_MPS_PER_KNOT 0.51444444 +#define _GPS_KMPH_PER_KNOT 1.852 +#define _GPS_MILES_PER_METER 0.00062137112 +#define _GPS_KM_PER_METER 0.001 + +class TinyGPS +{ +public: + enum { + GPS_INVALID_AGE = 0xFFFFFFFF, GPS_INVALID_ANGLE = 999999999, + GPS_INVALID_ALTITUDE = 999999999, GPS_INVALID_DATE = 0, + GPS_INVALID_TIME = 0xFFFFFFFF, GPS_INVALID_SPEED = 999999999, + GPS_INVALID_FIX_TIME = 0xFFFFFFFF, GPS_INVALID_SATELLITES = 0xFF, + GPS_INVALID_HDOP = 0xFFFFFFFF + }; + + static const float GPS_INVALID_F_ANGLE, GPS_INVALID_F_ALTITUDE, GPS_INVALID_F_SPEED; + + TinyGPS(); + bool encode(char c); // process one character received from GPS + TinyGPS &operator << (char c) {encode(c); return *this;} + + // lat/long in hundred thousandths of a degree and age of fix in milliseconds + void get_position(long *latitude, long *longitude, unsigned long *fix_age = 0); + + // date as ddmmyy, time as hhmmsscc, and age in milliseconds + void get_datetime(unsigned long *date, unsigned long *time, unsigned long *age = 0); + + // signed altitude in centimeters (from GPGGA sentence) + inline long altitude() { return _altitude; } + + // course in last full GPRMC sentence in 100th of a degree + inline unsigned long course() { return _course; } + + // speed in last full GPRMC sentence in 100ths of a knot + inline unsigned long speed() { return _speed; } + + // satellites used in last full GPGGA sentence + inline unsigned short satellites() { return _numsats; } + + // horizontal dilution of precision in 100ths + inline unsigned long hdop() { return _hdop; } + + void f_get_position(float *latitude, float *longitude, unsigned long *fix_age = 0); + void crack_datetime(int *year, byte *month, byte *day, + byte *hour, byte *minute, byte *second, byte *hundredths = 0, unsigned long *fix_age = 0); + float f_altitude(); + float f_course(); + float f_speed_knots(); + float f_speed_mph(); + float f_speed_mps(); + float f_speed_kmph(); + + static int library_version() { return _GPS_VERSION; } + + static float distance_between (float lat1, float long1, float lat2, float long2); + static float course_to (float lat1, float long1, float lat2, float long2); + static const char *cardinal(float course); + +#ifndef _GPS_NO_STATS + void stats(unsigned short *good_sentences, unsigned short *failed_cs); +#endif + +private: + enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER}; + + // properties + unsigned long _time, _new_time; + unsigned long _date, _new_date; + long _latitude, _new_latitude; + long _longitude, _new_longitude; + long _altitude, _new_altitude; + unsigned long _speed, _new_speed; + unsigned long _course, _new_course; + unsigned long _hdop, _new_hdop; + unsigned short _numsats, _new_numsats; + + unsigned long _last_time_fix, _new_time_fix; + unsigned long _last_position_fix, _new_position_fix; + + // parsing state variables + byte _parity; + bool _is_checksum_term; + char _term[15]; + byte _sentence_type; + byte _term_number; + byte _term_offset; + bool _gps_data_good; + + // statistics +#ifndef _GPS_NO_STATS + unsigned short _good_sentences; + unsigned short _failed_checksum; +#endif + + // internal utilities + byte from_hex(char a); + byte hex2uint8(const char* p); + unsigned long parse_decimal(); + unsigned long parse_degrees(); + bool term_complete(); + int gpsstrcmp(const char *str1, const char *str2); +}; + +#if !defined(ARDUINO) +// Arduino 0012 workaround +#undef int +#undef char +#undef long +#undef byte +#undef float +#undef abs +#undef round +#endif + +#endif diff --git a/esp32/libraries/FreematicsPlus/FreematicsMEMS.cpp b/esp32/libraries/FreematicsPlus/FreematicsMEMS.cpp new file mode 100644 index 0000000..495f90e --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsMEMS.cpp @@ -0,0 +1,1563 @@ +/************************************************************************* +* Freematics MEMS motion sensor helper classes +* Distributed under BSD license +* Visit https://freematics.com for more information +* (C)2016-2020 Stanley Huang +*************************************************************************/ + +#include "FreematicsMEMS.h" +#include +#include "utility/ICM_20948_REGISTERS.h" +#include "utility/AK09916_REGISTERS.h" + +#define WRITE_BIT I2C_MASTER_WRITE /*!< I2C master write */ +#define READ_BIT I2C_MASTER_READ /*!< I2C master read */ +#define ACK_CHECK_EN 0x1 /*!< I2C master will check ack from slave*/ +#define ACK_CHECK_DIS 0x0 /*!< I2C master will not check ack from slave */ +#define ACK_VAL (i2c_ack_type_t)0x0 /*!< I2C ack value */ +#define NACK_VAL (i2c_ack_type_t)0x1 /*!< I2C nack value */ + +// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" +// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) +// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute +// device orientation +void CQuaterion::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) +{ + uint32_t now = millis(); + deltat = ((float)(now - lastUpdate)/1000.0f); // set integration time by time elapsed since last filter update + lastUpdate = now; + + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability + float norm; + float hx, hy, _2bx, _2bz; + float s1, s2, s3, s4; + float qDot1, qDot2, qDot3, qDot4; + + // Auxiliary variables to avoid repeated arithmetic + float _2q1mx; + float _2q1my; + float _2q1mz; + float _2q2mx; + float _4bx; + float _4bz; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; + float _2q1q3 = 2.0f * q1 * q3; + float _2q3q4 = 2.0f * q3 * q4; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = sqrtf(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrtf(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + _2q1mx = 2.0f * q1 * mx; + _2q1my = 2.0f * q1 * my; + _2q1mz = 2.0f * q1 * mz; + _2q2mx = 2.0f * q2 * mx; + hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; + hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude + norm = 1.0f/norm; + s1 *= norm; + s2 *= norm; + s3 *= norm; + s4 *= norm; + + // Compute rate of change of quaternion + qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; + qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; + qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; + qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; + + // Integrate to yield quaternion + q1 += qDot1 * deltat; + q2 += qDot2 * deltat; + q3 += qDot3 * deltat; + q4 += qDot4 * deltat; + norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion + norm = 1.0f/norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + +void CQuaterion::getOrientation(ORIENTATION* ori) +{ + ori->yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) * 180.0f / PI; + ori->pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])) * 180.0f / PI; + ori->roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]) * 180.0f / PI; +} + +/******************************************************************************* + Base I2C MEMS class +*******************************************************************************/ + +#define WRITE_BIT I2C_MASTER_WRITE /*!< I2C master write */ +#define READ_BIT I2C_MASTER_READ /*!< I2C master read */ +#define ACK_CHECK_EN 0x1 /*!< I2C master will check ack from slave*/ +#define ACK_CHECK_DIS 0x0 /*!< I2C master will not check ack from slave */ +#define ACK_VAL (i2c_ack_type_t)0x0 /*!< I2C ack value */ +#define NACK_VAL (i2c_ack_type_t)0x1 /*!< I2C nack value */ + +bool MEMS_I2C::initI2C(unsigned long clock) +{ + i2c_port_t i2c_master_port = I2C_NUM_0; + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, +#ifdef ARDUINO_ESP32C3_DEV + .sda_io_num = 4, + .scl_io_num = 5, +#else + .sda_io_num = 21, + .scl_io_num = 22, +#endif + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_pullup_en = GPIO_PULLUP_ENABLE, + }; + conf.master.clk_speed = clock; + return i2c_param_config(i2c_master_port, &conf) == ESP_OK && + i2c_driver_install(i2c_master_port, conf.mode, 0, 0, 0) == ESP_OK; +} + +void MEMS_I2C::uninitI2C() +{ + i2c_driver_delete((i2c_port_t)I2C_NUM_0); +} +/******************************************************************************* + MPU-9250 class functions +*******************************************************************************/ + +//============================================================================== +//====== Set of useful function to access acceleration. gyroscope, magnetometer, +//====== and temperature data +//============================================================================== +void MPU9250::readAccelData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + + +void MPU9250::readGyroData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + +void MPU9250::readMagData(int16_t * destination) +{ + if(readByteAK(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set + uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition + readBytesAK(AK8963_XOUT_L, 7, rawData); // Read the six raw data and ST2 registers sequentially into data array + uint8_t c = rawData[6]; // End data read by reading ST2 register + if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; + } + } +} + +int16_t MPU9250::readTempData() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + readBytes(TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value +} + +bool MPU9250::initAK8963(float * destination) +{ + if (readByteAK(WHO_AM_I_AK8963) != 0x48) { + return false; + } + + // First extract the factory calibration for each magnetometer axis + uint8_t rawData[3]; // x/y/z gyro calibration data stored here + writeByteAK(AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + writeByteAK(AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); + // Read the x-, y-, and z-axis calibration values + /* + if (!readBytesAK(AK8963_ASAX, 3, &rawData[0], 3000)) { + return false; + } + */ + rawData[0] = readByteAK(AK8963_ASAX); + rawData[1] = readByteAK(AK8963_ASAY); + rawData[2] = readByteAK(AK8963_ASAZ); + destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. + destination[1] = (float)(rawData[1] - 128)/256. + 1.; + destination[2] = (float)(rawData[2] - 128)/256. + 1.; + writeByteAK(AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + // Configure the magnetometer for continuous read and highest resolution + // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, + // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates + writeByteAK(AK8963_CNTL, MFS_16BITS << 4 | Mmode); // Set magnetometer data resolution and sample ODR + delay(10); + return true; +} + +// 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. +void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) +{ + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t ii, packet_count, fifo_count; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + + // reset device + // Write a one to bit 7 reset bit; toggle reset device + writeByte(PWR_MGMT_1, 0x80); + delay(100); + + // get stable time source; Auto select clock source to be PLL gyroscope + // reference if ready else use the internal oscillator, bits 2:0 = 001 + writeByte(PWR_MGMT_1, 0x01); + writeByte(PWR_MGMT_2, 0x00); + delay(200); + + // Configure device for bias calculation + writeByte(INT_ENABLE, 0x00); // Disable all interrupts + writeByte(FIFO_EN, 0x00); // Disable FIFO + writeByte(PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(I2C_MST_CTRL, 0x00); // Disable master + writeByte(USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(USER_CTRL, 0x2C); // Reset FIFO and DMP + delay(15); + +// Configure MPU6050 gyro and accelerometer for bias calculation + writeByte(CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(USER_CTRL, 0x40); // Enable FIFO + writeByte(FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) + delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + +// At end of sample accumulation, turn off FIFO sensor read + writeByte(FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readBytes(FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging + + for (ii = 0; ii < packet_count; ii++) + { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + readBytes(FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO + 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]); + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + 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]; + } + accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + 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; + + if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation + else {accel_bias[2] += (int32_t) accelsensitivity;} + +// 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(XG_OFFSET_H, data[0]); + writeByte(XG_OFFSET_L, data[1]); + writeByte(YG_OFFSET_H, data[2]); + writeByte(YG_OFFSET_L, data[3]); + writeByte(ZG_OFFSET_H, data[4]); + writeByte(ZG_OFFSET_L, data[5]); + +// Output scaled gyro biases for display in the main program + gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity; + +// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain +// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold +// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature +// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that +// the accelerometer biases calculated above must be divided by 8. + + int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + readBytes(XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis + + for(ii = 0; ii < 3; ii++) { + if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accel_bias[1]/8); + accel_bias_reg[2] -= (accel_bias[2]/8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + +// Apparently this is not working for the acceleration biases in the MPU-9250 +// Are we handling the temperature correction bit properly? +// Push accelerometer biases to hardware registers + writeByte(XA_OFFSET_H, data[0]); + writeByte(XA_OFFSET_L, data[1]); + writeByte(YA_OFFSET_H, data[2]); + writeByte(YA_OFFSET_L, data[3]); + writeByte(ZA_OFFSET_H, data[4]); + writeByte(ZA_OFFSET_L, data[5]); + +// Output scaled accelerometer biases for display in the main program + accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; + accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; + accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; +} + + +// Accelerometer and gyroscope self test; check calibration wrt factory settings +void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +{ + uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; + uint8_t selfTest[6]; + int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; + float factoryTrim[6]; + uint8_t FS = 0; + + writeByte(SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(GYRO_CONFIG, 1< 1) { + i2c_master_read(cmd, dest, count - 1, ACK_VAL); + } + i2c_master_read_byte(cmd, dest + count - 1, NACK_VAL); + i2c_master_stop(cmd); + ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK; +} + +void MPU9250::writeByteAK(uint8_t subAddress, uint8_t data) +{ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( AK8963_ADDRESS << 1 ) | WRITE_BIT, ACK_CHECK_EN); + // write sub-address and data + uint8_t buf[2] = {subAddress, data}; + i2c_master_write(cmd, buf, sizeof(buf), ACK_CHECK_EN); + i2c_master_stop(cmd); + i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); +} + +uint8_t MPU9250::readByteAK(uint8_t subAddress) +{ + // write sub-address + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( AK8963_ADDRESS << 1 ) | WRITE_BIT, ACK_CHECK_DIS); + i2c_master_write_byte(cmd, subAddress, ACK_CHECK_EN); + i2c_master_stop(cmd); + i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + // read data + uint8_t data = 0; + cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( AK8963_ADDRESS << 1 ) | READ_BIT, ACK_CHECK_EN); + i2c_master_read_byte(cmd, &data, NACK_VAL); + i2c_master_stop(cmd); + i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return data; +} + +bool MPU9250::readBytesAK(uint8_t subAddress, uint8_t count, uint8_t * dest) +{ + // write sub-address + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( AK8963_ADDRESS << 1 ) | WRITE_BIT, ACK_CHECK_DIS); + i2c_master_write_byte(cmd, subAddress, ACK_CHECK_EN); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + if (ret != ESP_OK) return false; + // read data + cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( AK8963_ADDRESS << 1 ) | READ_BIT, ACK_CHECK_EN); + if (count > 1) { + i2c_master_read(cmd, dest, count - 1, ACK_VAL); + } + i2c_master_read_byte(cmd, dest + count - 1, NACK_VAL); + i2c_master_stop(cmd); + ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK; +} + +void MPU9250::init() +{ + // wake up device + writeByte(PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors + delay(100); // Wait for all registers to reset + + // get stable time source + writeByte(PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else + delay(200); + + // Configure Gyro and Thermometer + // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; + // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot + // be higher than 1 / 0.0059 = 170 Hz + // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both + // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz + writeByte(CONFIG, 0x03); + + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + writeByte(SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate + // determined inset in CONFIG above + + // Set gyroscope full scale range + // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 + uint8_t c = readByte(GYRO_CONFIG); // get current GYRO_CONFIG register value + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x02; // Clear Fchoice bits [1:0] + c = c & ~0x18; // Clear AFS bits [4:3] + c = c | Gscale << 3; // Set full scale range for the gyro + // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG + writeByte(GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register + + // Set accelerometer full-scale range configuration + c = readByte(ACCEL_CONFIG); // get current ACCEL_CONFIG register value + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x18; // Clear AFS bits [4:3] + c = c | Ascale << 3; // Set full scale range for the accelerometer + writeByte(ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value + + // Set accelerometer sample rate configuration + // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for + // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz + c = readByte(ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value + c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) + c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz + writeByte(ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value + // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, + // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting + + // Configure Interrupts and Bypass Enable + // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, + // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips + // can join the I2C bus and all can be controlled by the Arduino as master + writeByte(INT_PIN_CFG, 0x22); + writeByte(INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt + delay(100); + +} + +byte MPU9250::begin(bool fusion) +{ + if (!initI2C(100000)) return 0; + byte ret = 0; + for (byte attempt = 0; attempt < 2; attempt++) { + //float SelfTest[6]; + //MPU9250SelfTest(SelfTest); + byte c = readByte(WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 + if (c != 0x68 && c != 0x71) continue; + calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + init(); + if (c == 0x71 && initAK8963(magCalibration)) + ret = 2; + else + ret = 1; + break; + } + if (ret && fusion && !quaterion) { + quaterion = new CQuaterion; + } + return ret; +} + +bool MPU9250::read(float* acc, float* gyr, float* mag, float* temp, ORIENTATION* ori) +{ + if (acc) { + readAccelData(accelCount); + acc[0] = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set + acc[1] = (float)accelCount[1]*aRes; // - accelBias[1]; + acc[2] = (float)accelCount[2]*aRes; // - accelBias[2]; + } + if (gyr) { + readGyroData(gyroCount); + gyr[0] = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set + gyr[1] = (float)gyroCount[1]*gRes; + gyr[2] = (float)gyroCount[2]*gRes; + } + if (mag) { + float magbias[3]; + magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated + magbias[1] = +120.; // User environmental x-axis correction in milliGauss + magbias[2] = +125.; // User environmental x-axis correction in milliGauss + + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental corrections + readMagData(magCount); + mag[0] = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set + mag[1] = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; + mag[2] = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; + } + if (temp) { + int t = readTempData(); + *temp = (float)t / 333.87 + 21; + } + + if (quaterion && acc && gyr && mag) { + quaterion->MadgwickQuaternionUpdate(acc[0], acc[1], acc[2], gyr[0]*PI/180.0f, gyr[1]*PI/180.0f, gyr[2]*PI/180.0f, mag[0], mag[1], mag[2]); + quaterion->getOrientation(ori); + } + return true; +} + +/******************************************************************************* + ICM-42627 class functions +*******************************************************************************/ +byte ICM_42627::begin(bool fusion) +{ + if (!initI2C(100000) || readByte(WHO_AM_I_ICM42627) != ICM_42627_DeviceID) return 0; + init(); + return 1; +} + +void ICM_42627::init() +{ + writeByte(PWR_MGMT0_REG, TEMP_DIS_ON | IDLE_ON | ACCEL_MODE_LN | GYRO_MODE_LN ); + delay(100); + + writeByte(ACCEL_CONFIG0_REG, ACCEL_ODR_1KHZ | ACCEL_FS_SEL_2G); // Auto select clock source to be PLL gyroscope reference if ready else + delay(100); + + writeByte(GYRO_CONFIG0_REG, GYRO_ODR_1KHZ | GYRO_FS_SEL_250dps); + delay(100); + + writeByte(SELF_TEST_CONFIG_REG, 0x07); + delay(100); +} + +void ICM_42627::writeByte(uint8_t subAddress, uint8_t data) +{ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( ICM_42627_ADDRESS << 1 ) | WRITE_BIT, ACK_CHECK_EN); + // write sub-address and data + uint8_t buf[2] = {subAddress, data}; + i2c_master_write(cmd, buf, sizeof(buf), ACK_CHECK_EN); + i2c_master_stop(cmd); + i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); +} + +uint8_t ICM_42627::readByte(uint8_t subAddress) +{ + // write sub-address + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( ICM_42627_ADDRESS << 1 ) | WRITE_BIT, ACK_CHECK_DIS); + i2c_master_write_byte(cmd, subAddress, ACK_CHECK_EN); + i2c_master_stop(cmd); + i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + // read data + uint8_t data = 0; + cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( ICM_42627_ADDRESS << 1 ) | READ_BIT, ACK_CHECK_EN); + i2c_master_read_byte(cmd, &data, NACK_VAL); + i2c_master_stop(cmd); + i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return data; +} + +bool ICM_42627::readBytes(uint8_t subAddress, uint8_t count, uint8_t * dest) +{ + // write sub-address + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( ICM_42627_ADDRESS << 1 ) | WRITE_BIT, ACK_CHECK_DIS); + i2c_master_write_byte(cmd, subAddress, ACK_CHECK_EN); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + if (ret != ESP_OK) return false; + // read data + cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( ICM_42627_ADDRESS << 1 ) | READ_BIT, ACK_CHECK_EN); + if (count > 1) { + i2c_master_read(cmd, dest, count - 1, ACK_VAL); + } + i2c_master_read_byte(cmd, dest + count - 1, NACK_VAL); + i2c_master_stop(cmd); + ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK; +} + +void ICM_42627::readAccelData(int16_t data[]) +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(ACCEL_XOUT_H_REG, 6, &rawData[0]); // Read the six raw data registers into data array + data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + +int16_t ICM_42627::readTempData() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + readBytes(TEMP_OUT_H_REG, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value +} + +void ICM_42627::readGyroData(int16_t data[]) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(GYRO_XOUT_H_REG, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + +bool ICM_42627::read(float* acc, float* gyr, float* mag, float* temp, ORIENTATION* ori) +{ + if (acc) { + int16_t accelCount[3] = {0}; + readAccelData(accelCount); + acc[0] = (float)accelCount[0]*aRes; // - accelBias[0]; // get actual g value, this depends on scale being set + acc[1] = (float)accelCount[1]*aRes; // - accelBias[1]; + acc[2] = (float)accelCount[2]*aRes; // - accelBias[2]; + } + if (gyr) { + int16_t gyroCount[3] = {0}; + readGyroData(gyroCount); + gyr[0] = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set + gyr[1] = (float)gyroCount[1]*gRes; + gyr[2] = (float)gyroCount[2]*gRes; + } + if (temp) { + *temp = (float)readTempData() / 132.48 + 25.0; + } + return true; +} + +/******************************************************************************* + ICM-20948 class functions +*******************************************************************************/ + +// serif functions for the I2C and SPI classes +ICM_20948_Status_e ICM_20948_write_I2C(uint8_t reg, uint8_t* data, uint32_t len, void* user){ + if(user == NULL){ return ICM_20948_Stat_ParamErr; } + uint8_t addr = ((ICM_20948_I2C*)user)->_addr; + + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( addr << 1 ) | WRITE_BIT, ACK_CHECK_EN); + i2c_master_write_byte(cmd, reg, ACK_CHECK_EN); + i2c_master_write(cmd, data, len, ACK_CHECK_DIS); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK ? ICM_20948_Stat_Ok : ICM_20948_Stat_Err; +} + +ICM_20948_Status_e ICM_20948_read_I2C(uint8_t reg, uint8_t* buff, uint32_t len, void* user){ + if(user == NULL){ return ICM_20948_Stat_ParamErr; } + uint8_t addr = ((ICM_20948_I2C*)user)->_addr; + + // write sub-address + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( addr << 1 ) | WRITE_BIT, ACK_CHECK_EN); + i2c_master_write_byte(cmd, reg, ACK_CHECK_EN); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + if (ret != ESP_OK) return ICM_20948_Stat_Err; + // read data + cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( addr << 1 ) | READ_BIT, ACK_CHECK_EN); + if (len > 1) { + i2c_master_read(cmd, buff, len - 1, ACK_VAL); + } + i2c_master_read_byte(cmd, buff + len - 1, NACK_VAL); + i2c_master_stop(cmd); + ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK ? ICM_20948_Stat_Ok : ICM_20948_Stat_NoData; +} + +ICM_20948_AGMT_t ICM_20948::getAGMT ( void ){ + status = ICM_20948_get_agmt( &_device, &agmt ); + + if( _has_magnetometer ){ + getMagnetometerData( &agmt ); + } + + return agmt; +} + +float ICM_20948::magX ( void ){ + return getMagUT(agmt.mag.axes.x); +} + +float ICM_20948::magY ( void ){ + return getMagUT(agmt.mag.axes.y); +} + +float ICM_20948::magZ ( void ){ + return getMagUT(agmt.mag.axes.z); +} + +float ICM_20948::getMagUT ( int16_t axis_val ){ + return (((float)axis_val)*0.15); +} + +float ICM_20948::accX ( void ){ + return getAccMG(agmt.acc.axes.x); +} + +float ICM_20948::accY ( void ){ + return getAccMG(agmt.acc.axes.y); +} + +float ICM_20948::accZ ( void ){ + return getAccMG(agmt.acc.axes.z); +} + +float ICM_20948::getAccMG ( int16_t axis_val ){ + switch(agmt.fss.a){ + case 0 : return (((float)axis_val)/16.384); break; + case 1 : return (((float)axis_val)/8.192); break; + case 2 : return (((float)axis_val)/4.096); break; + case 3 : return (((float)axis_val)/2.048); break; + default : return 0; break; + } +} + +float ICM_20948::gyrX ( void ){ + return getGyrDPS(agmt.gyr.axes.x); +} + +float ICM_20948::gyrY ( void ){ + return getGyrDPS(agmt.gyr.axes.y); +} + +float ICM_20948::gyrZ ( void ){ + return getGyrDPS(agmt.gyr.axes.z); +} + +float ICM_20948::getGyrDPS ( int16_t axis_val ){ + switch(agmt.fss.g){ + case 0 : return (((float)axis_val)/131); break; + case 1 : return (((float)axis_val)/65.5); break; + case 2 : return (((float)axis_val)/32.8); break; + case 3 : return (((float)axis_val)/16.4); break; + default : return 0; break; + } +} + +float ICM_20948::temp ( void ){ + return getTempC(agmt.tmp.val); +} + +float ICM_20948::getTempC ( int16_t val ){ + return (((float)val)/333.87) + 21; +} + + + +const char* ICM_20948::statusString ( ICM_20948_Status_e stat ){ + ICM_20948_Status_e val; + if( stat == ICM_20948_Stat_NUM){ + val = status; + }else{ + val = stat; + } + + switch(val){ + case ICM_20948_Stat_Ok : return "All is well."; break; + case ICM_20948_Stat_Err : return "General Error"; break; + case ICM_20948_Stat_NotImpl : return "Not Implemented"; break; + case ICM_20948_Stat_ParamErr : return "Parameter Error"; break; + case ICM_20948_Stat_WrongID : return "Wrong ID"; break; + case ICM_20948_Stat_InvalSensor : return "Invalid Sensor"; break; + case ICM_20948_Stat_NoData : return "Data Underflow"; break; + case ICM_20948_Stat_SensorNotSupported : return "Sensor Not Supported"; break; + default : + return "Unknown Status"; break; + + } + return "None"; +} + + + +// Device Level +ICM_20948_Status_e ICM_20948::setBank ( uint8_t bank ){ + status = ICM_20948_set_bank( &_device, bank ); + return status; +} + +ICM_20948_Status_e ICM_20948::swReset ( void ){ + status = ICM_20948_sw_reset( &_device ); + return status; +} + +ICM_20948_Status_e ICM_20948::sleep ( bool on ){ + status = ICM_20948_sleep( &_device, on ); + return status; +} + +ICM_20948_Status_e ICM_20948::lowPower ( bool on ){ + status = ICM_20948_low_power( &_device, on ); + return status; +} + +ICM_20948_Status_e ICM_20948::setClockSource ( ICM_20948_PWR_MGMT_1_CLKSEL_e source ){ + status = ICM_20948_set_clock_source( &_device, source ); + return status; +} + +ICM_20948_Status_e ICM_20948::checkID ( void ){ + status = ICM_20948_check_id( &_device ); + return status; +} + +bool ICM_20948::dataReady ( void ){ + status = ICM_20948_data_ready( &_device ); + if( status == ICM_20948_Stat_Ok ){ return true; } + return false; +} + +uint8_t ICM_20948::getWhoAmI ( void ){ + uint8_t retval = 0x00; + status = ICM_20948_get_who_am_i( &_device, &retval ); + return retval; +} + +bool ICM_20948::isConnected ( void ){ + status = checkID(); + if( status == ICM_20948_Stat_Ok ){ return true; } + return false; +} + + +// Internal Sensor Options +ICM_20948_Status_e ICM_20948::setSampleMode ( uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode ){ + status = ICM_20948_set_sample_mode( &_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, (ICM_20948_LP_CONFIG_CYCLE_e)lp_config_cycle_mode ); + return status; +} + +ICM_20948_Status_e ICM_20948::setFullScale ( uint8_t sensor_id_bm, ICM_20948_fss_t fss ){ + status = ICM_20948_set_full_scale( &_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, fss ); + return status; +} + +ICM_20948_Status_e ICM_20948::setDLPFcfg ( uint8_t sensor_id_bm, ICM_20948_dlpcfg_t cfg ){ + status = ICM_20948_set_dlpf_cfg( &_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, cfg ); + return status; +} + +ICM_20948_Status_e ICM_20948::enableDLPF ( uint8_t sensor_id_bm, bool enable ){ + status = ICM_20948_enable_dlpf( &_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, enable ); + return status; +} + +ICM_20948_Status_e ICM_20948::setSampleRate ( uint8_t sensor_id_bm, ICM_20948_smplrt_t smplrt ){ + status = ICM_20948_set_sample_rate( &_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, smplrt ); + return status; +} + + + + + +// Interrupts on INT Pin +ICM_20948_Status_e ICM_20948::clearInterrupts ( void ){ + ICM_20948_INT_STATUS_t int_stat; + ICM_20948_INT_STATUS_1_t int_stat_1; + + // read to clear interrupts + status = ICM_20948_set_bank( &_device, 0 ); if( status != ICM_20948_Stat_Ok ){ return status; } + status = ICM_20948_execute_r( &_device, AGB0_REG_INT_STATUS, (uint8_t*)&int_stat, sizeof(ICM_20948_INT_STATUS_t) ); if( status != ICM_20948_Stat_Ok ){ return status; } + status = ICM_20948_execute_r( &_device, AGB0_REG_INT_STATUS_1, (uint8_t*)&int_stat_1, sizeof(ICM_20948_INT_STATUS_1_t) ); if( status != ICM_20948_Stat_Ok ){ return status; } + + // todo: there may be additional interrupts that need to be cleared, like FIFO overflow/watermark + + return status; +} + + +ICM_20948_Status_e ICM_20948::cfgIntActiveLow ( bool active_low ){ + ICM_20948_INT_PIN_CFG_t reg; + status = ICM_20948_int_pin_cfg ( &_device, NULL, ® ); // read phase + if(status != ICM_20948_Stat_Ok){ return status; } + reg.INT1_ACTL = active_low; // set the setting + status = ICM_20948_int_pin_cfg ( &_device, ®, NULL ); // write phase + if(status != ICM_20948_Stat_Ok){ return status; } + return status; +} + +ICM_20948_Status_e ICM_20948::cfgIntOpenDrain ( bool open_drain ){ + ICM_20948_INT_PIN_CFG_t reg; + status = ICM_20948_int_pin_cfg ( &_device, NULL, ® ); // read phase + if(status != ICM_20948_Stat_Ok){ return status; } + reg.INT1_OPEN = open_drain; // set the setting + status = ICM_20948_int_pin_cfg ( &_device, ®, NULL ); // write phase + if(status != ICM_20948_Stat_Ok){ return status; } + return status; +} + +ICM_20948_Status_e ICM_20948::cfgIntLatch ( bool latching ){ + ICM_20948_INT_PIN_CFG_t reg; + status = ICM_20948_int_pin_cfg ( &_device, NULL, ® ); // read phase + if(status != ICM_20948_Stat_Ok){ return status; } + reg.INT1_LATCH_EN = latching; // set the setting + status = ICM_20948_int_pin_cfg ( &_device, ®, NULL ); // write phase + if(status != ICM_20948_Stat_Ok){ return status; } + return status; +} + +ICM_20948_Status_e ICM_20948::cfgIntAnyReadToClear ( bool enabled ){ + ICM_20948_INT_PIN_CFG_t reg; + status = ICM_20948_int_pin_cfg ( &_device, NULL, ® ); // read phase + if(status != ICM_20948_Stat_Ok){ return status; } + reg.INT_ANYRD_2CLEAR = enabled; // set the setting + status = ICM_20948_int_pin_cfg ( &_device, ®, NULL ); // write phase + if(status != ICM_20948_Stat_Ok){ return status; } + return status; +} + +ICM_20948_Status_e ICM_20948::cfgFsyncActiveLow ( bool active_low ){ + ICM_20948_INT_PIN_CFG_t reg; + status = ICM_20948_int_pin_cfg ( &_device, NULL, ® ); // read phase + if(status != ICM_20948_Stat_Ok){ return status; } + reg.ACTL_FSYNC = active_low; // set the setting + status = ICM_20948_int_pin_cfg ( &_device, ®, NULL ); // write phase + if(status != ICM_20948_Stat_Ok){ return status; } + return status; +} + +ICM_20948_Status_e ICM_20948::cfgFsyncIntMode ( bool interrupt_mode ){ + ICM_20948_INT_PIN_CFG_t reg; + status = ICM_20948_int_pin_cfg ( &_device, NULL, ® ); // read phase + if(status != ICM_20948_Stat_Ok){ return status; } + reg.FSYNC_INT_MODE_EN = interrupt_mode; // set the setting + status = ICM_20948_int_pin_cfg ( &_device, ®, NULL ); // write phase + if(status != ICM_20948_Stat_Ok){ return status; } + return status; +} + + +// All these individual functions will use a read->set->write method to leave other settings untouched +ICM_20948_Status_e ICM_20948::intEnableI2C ( bool enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.I2C_MST_INT_EN = enable; // change the setting + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + if( en.I2C_MST_INT_EN != enable ){ + status = ICM_20948_Stat_Err; + return status; + } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnableDMP ( bool enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.DMP_INT1_EN = enable; // change the setting + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + if( en.DMP_INT1_EN != enable ){ + status = ICM_20948_Stat_Err; + return status; + } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnablePLL ( bool enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.PLL_RDY_EN = enable; // change the setting + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + if( en.PLL_RDY_EN != enable ){ + status = ICM_20948_Stat_Err; + return status; + } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnableWOM ( bool enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.WOM_INT_EN = enable; // change the setting + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + if( en.WOM_INT_EN != enable ){ + status = ICM_20948_Stat_Err; + return status; + } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnableWOF ( bool enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.REG_WOF_EN = enable; // change the setting + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + if( en.REG_WOF_EN != enable ){ + status = ICM_20948_Stat_Err; + return status; + } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnableRawDataReady ( bool enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.RAW_DATA_0_RDY_EN = enable; // change the setting + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + if( en.RAW_DATA_0_RDY_EN != enable ){ + Serial.println("mismatch error"); + status = ICM_20948_Stat_Err; + return status; + } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnableOverflowFIFO ( uint8_t bm_enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.FIFO_OVERFLOW_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings + en.FIFO_OVERFLOW_EN_1 = ((bm_enable >> 1) & 0x01); + en.FIFO_OVERFLOW_EN_2 = ((bm_enable >> 2) & 0x01); + en.FIFO_OVERFLOW_EN_3 = ((bm_enable >> 3) & 0x01); + en.FIFO_OVERFLOW_EN_4 = ((bm_enable >> 4) & 0x01); + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + return status; +} + +ICM_20948_Status_e ICM_20948::intEnableWatermarkFIFO ( uint8_t bm_enable ){ + ICM_20948_INT_enable_t en; // storage + status = ICM_20948_int_enable( &_device, NULL, &en ); // read phase + if( status != ICM_20948_Stat_Ok ){ return status; } + en.FIFO_WM_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings + en.FIFO_WM_EN_1 = ((bm_enable >> 1) & 0x01); + en.FIFO_WM_EN_2 = ((bm_enable >> 2) & 0x01); + en.FIFO_WM_EN_3 = ((bm_enable >> 3) & 0x01); + en.FIFO_WM_EN_4 = ((bm_enable >> 4) & 0x01); + status = ICM_20948_int_enable( &_device, &en, &en ); // write phase w/ readback + if( status != ICM_20948_Stat_Ok ){ return status; } + return status; +} + + + +// Interface Options +ICM_20948_Status_e ICM_20948::i2cMasterPassthrough ( bool passthrough ){ + status = ICM_20948_i2c_master_passthrough ( &_device, passthrough ); + return status; +} + +ICM_20948_Status_e ICM_20948::i2cMasterEnable ( bool enable ){ + status = ICM_20948_i2c_master_enable( &_device, enable ); + return status; +} + +ICM_20948_Status_e ICM_20948::i2cMasterConfigureSlave ( uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap ){ + status = ICM_20948_i2c_master_configure_slave ( &_device, slave, addr, reg, len, Rw, enable, data_only, grp, swap ); + return status; +} + +ICM_20948_Status_e ICM_20948::i2cMasterSLV4Transaction( uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr ){ + status = ICM_20948_i2c_master_slv4_txn( &_device, addr, reg, data, len, Rw, send_reg_addr ); + return status; +} +ICM_20948_Status_e ICM_20948::i2cMasterSingleW ( uint8_t addr, uint8_t reg, uint8_t data ){ + status = ICM_20948_i2c_master_single_w( &_device, addr, reg, &data ); + return status; +} +uint8_t ICM_20948::i2cMasterSingleR ( uint8_t addr, uint8_t reg ){ + uint8_t data; + status = ICM_20948_i2c_master_single_r( &_device, addr, reg, &data ); + return data; +} + + + + + + + + + + +ICM_20948_Status_e ICM_20948::startupDefault ( void ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + retval = checkID(); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + + retval = swReset(); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + delay(50); + + retval = sleep( false ); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + + retval = lowPower( false ); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + + retval = setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous ); // options: ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } // sensors: ICM_20948_Internal_Acc, ICM_20948_Internal_Gyr, ICM_20948_Internal_Mst + + ICM_20948_fss_t FSS; + FSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) + FSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) + retval = setFullScale( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS ); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + + ICM_20948_dlpcfg_t dlpcfg; + dlpcfg.a = acc_d473bw_n499bw; + dlpcfg.g = gyr_d361bw4_n376bw5; + retval = setDLPFcfg( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg ); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + + retval = enableDLPF( ICM_20948_Internal_Acc, false ); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + retval = enableDLPF( ICM_20948_Internal_Gyr, false ); + if( retval != ICM_20948_Stat_Ok ){ status = retval; return status; } + + _has_magnetometer = true; + retval = startupMagnetometer(); + if(( retval != ICM_20948_Stat_Ok) && ( retval != ICM_20948_Stat_NotImpl )){ status = retval; return status; } + if( retval == ICM_20948_Stat_NotImpl ){ + // This is a temporary fix. + // Ultimately we *should* be able to configure the I2C master to handle the + // magnetometer no matter what interface (SPI / I2C) we are using. + + // Should try testing I2C master functionality on a bare ICM chip w/o TXS0108 level shifter... + + _has_magnetometer = false; + retval = ICM_20948_Stat_Ok; // reset the retval because we handled it in this cases + } + + status = retval; + return status; +} + +ICM_20948_Status_e ICM_20948::startupMagnetometer ( void ){ + return ICM_20948_Stat_NotImpl; // By default we assume that we cannot access the magnetometer +} + +ICM_20948_Status_e ICM_20948::getMagnetometerData ( ICM_20948_AGMT_t* pagmt ){ + return ICM_20948_Stat_NotImpl; // By default we assume that we cannot access the magnetometer +} + + + + + + + + + +// direct read/write +ICM_20948_Status_e ICM_20948::read ( uint8_t reg, uint8_t* pdata, uint32_t len){ + status = ICM_20948_execute_r( &_device, reg, pdata, len ); + return status; +} + +ICM_20948_Status_e ICM_20948::write ( uint8_t reg, uint8_t* pdata, uint32_t len){ + status = ICM_20948_execute_w( &_device, reg, pdata, len ); + return status; +} + +byte ICM_20948_I2C::begin(bool fusion){ + // Associate + _ad0 = ICM_20948_ARD_UNUSED_PIN; + _ad0val = false; + + _addr = ICM_20948_I2C_ADDR_AD0; + if( _ad0val ){ _addr = ICM_20948_I2C_ADDR_AD1; } + + // Set pinmodes + if(_ad0 != ICM_20948_ARD_UNUSED_PIN){ pinMode(_ad0, OUTPUT); } + + // Set pins to default positions + if(_ad0 != ICM_20948_ARD_UNUSED_PIN){ digitalWrite(_ad0, _ad0val); } + + if (!initI2C(100000)) return 0; + + // Set up the serif + _serif.write = ICM_20948_write_I2C; + _serif.read = ICM_20948_read_I2C; + _serif.user = (void*)this; // refer to yourself in the user field + + // Link the serif + _device._serif = &_serif; + + // Perform default startup + status = startupDefault(); + if( status != ICM_20948_Stat_Ok ){ + return 0; + } + + if (fusion && !quaterion) { + quaterion = new CQuaterion; + } + + return 2; +} + +ICM_20948_Status_e ICM_20948_I2C::startupMagnetometer ( void ){ + // If using the magnetometer through passthrough: + i2cMasterPassthrough( true ); // Set passthrough mode to try to access the magnetometer (by default I2C master is disabled but you still have to enable the passthrough) + + // Try to set up magnetometer + AK09916_CNTL2_Reg_t reg; + reg.MODE = AK09916_mode_cont_100hz; + + ICM_20948_Status_e retval = writeMag( AK09916_REG_CNTL2, (uint8_t*)®, sizeof(AK09916_CNTL2_Reg_t) ); + status = retval; + if(status == ICM_20948_Stat_Ok){ + _has_magnetometer = true; + } + return status; +} + + +ICM_20948_Status_e ICM_20948_I2C::magWhoIAm( void ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + const uint8_t len = 2; + uint8_t whoiam[len]; + retval = readMag( AK09916_REG_WIA1, whoiam, len ); + status = retval; + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + if( (whoiam[0] == (MAG_AK09916_WHO_AM_I >> 8)) && ( whoiam[1] == (MAG_AK09916_WHO_AM_I & 0xFF)) ){ + retval = ICM_20948_Stat_Ok; + status = retval; + return status; + } + retval = ICM_20948_Stat_WrongID; + status = retval; + return status; +} + +bool ICM_20948_I2C::magIsConnected( void ){ + if( magWhoIAm() != ICM_20948_Stat_Ok ){ + return false; + } + return true; +} + +ICM_20948_Status_e ICM_20948_I2C::getMagnetometerData ( ICM_20948_AGMT_t* pagmt ){ + + const uint8_t reqd_len = 9; // you must read all the way through the status2 register to re-enable the next measurement + uint8_t buff[reqd_len]; + + status = readMag( AK09916_REG_ST1, buff, reqd_len ); + if( status != ICM_20948_Stat_Ok ){ + return status; + } + + pagmt->mag.axes.x = ((buff[2] << 8) | (buff[1] & 0xFF)); + pagmt->mag.axes.y = ((buff[4] << 8) | (buff[3] & 0xFF)); + pagmt->mag.axes.z = ((buff[6] << 8) | (buff[5] & 0xFF)); + + return status; +} + +ICM_20948_Status_e ICM_20948_I2C::readMag( uint8_t reg, uint8_t* pdata, uint8_t len ){ + // write sub-address + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( MAG_AK09916_I2C_ADDR << 1 ) | WRITE_BIT, ACK_CHECK_EN); + i2c_master_write_byte(cmd, reg, ACK_CHECK_EN); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + if (ret != ESP_OK) return ICM_20948_Stat_Err; + // read data + cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( MAG_AK09916_I2C_ADDR << 1 ) | READ_BIT, ACK_CHECK_EN); + if (len > 1) { + i2c_master_read(cmd, pdata, len - 1, ACK_VAL); + } + i2c_master_read_byte(cmd, pdata + len - 1, NACK_VAL); + i2c_master_stop(cmd); + ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK ? ICM_20948_Stat_Ok : ICM_20948_Stat_NoData; +} + +ICM_20948_Status_e ICM_20948_I2C::writeMag( uint8_t reg, uint8_t* pdata, uint8_t len ){ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, ( MAG_AK09916_I2C_ADDR << 1 ) | WRITE_BIT, ACK_CHECK_EN); + i2c_master_write_byte(cmd, reg, ACK_CHECK_EN); + i2c_master_write(cmd, pdata, len, ACK_CHECK_DIS); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 1000 / portTICK_RATE_MS); + i2c_cmd_link_delete(cmd); + return ret == ESP_OK ? ICM_20948_Stat_Ok : ICM_20948_Stat_Err; +} + +bool ICM_20948_I2C::read(float* acc, float* gyr, float* mag, float* tmp, ORIENTATION* ori) +{ + if(!dataReady() || ICM_20948_get_agmt( &_device, &agmt ) != ICM_20948_Stat_Ok){ + return false; + } + if( _has_magnetometer ){ + getMagnetometerData( &agmt ); + } + if (acc) { + acc[0] = accX() / 1000; + acc[1] = accY() / 1000; + acc[2] = accZ() / 1000; + } + if (gyr) { + gyr[0] = gyrX(); + gyr[1] = gyrY(); + gyr[2] = gyrZ(); + } + if (mag) { + mag[0] = magX(); + mag[1] = magY(); + mag[2] = magZ(); + } + if (tmp) { + *tmp = temp(); + } + if (quaterion && acc && gyr && mag) { + quaterion->MadgwickQuaternionUpdate(acc[0], acc[1], acc[2], gyr[0]*PI/180.0f, gyr[1]*PI/180.0f, gyr[2]*PI/180.0f, mag[0], mag[1], mag[2]); + quaterion->getOrientation(ori); + } + return true; +} diff --git a/esp32/libraries/FreematicsPlus/FreematicsMEMS.h b/esp32/libraries/FreematicsPlus/FreematicsMEMS.h new file mode 100644 index 0000000..e83b721 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsMEMS.h @@ -0,0 +1,422 @@ +/************************************************************************* +* Freematics MEMS motion sensor helper classes +* Distributed under BSD license +* Visit https://freematics.com for more information +* (C)2016-2020 Stanley Huang +*************************************************************************/ + +#ifndef FREEMATICS_MEMS +#define FREEMATICS_MEMS + +#include "FreematicsBase.h" +#include "utility/ICM_20948_C.h" // The C backbone +#include "utility/ICM_42627.h" + +// See also MPU-9250 Register Map and Descriptions, Revision 4.0, +// RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in above +// document; the MPU9250 and MPU9150 are virtually identical but the latter has +// a different register map + +//Magnetometer Registers +#define AK8963_ADDRESS 0x0C +#define WHO_AM_I_AK8963 0x00 // should return 0x48 +#define INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_CNTL2 0x0B +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 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_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F + +#define SELF_TEST_A 0x10 + +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F + +// Duration counter threshold for motion interrupt generation, 1 kHz rate, +// LSB = 1 ms +#define MOT_DUR 0x20 +// Zero-motion detection threshold bits [7:0] +#define ZMOT_THR 0x21 +// Duration counter threshold for zero motion interrupt generation, 16 Hz rate, +// LSB = 64 ms +#define ZRMOT_DUR 0x22 + +#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_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E + +#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 +#define AK8963_ADDRESS 0x0C // Address of magnetometer + +enum { + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_16G +}; + +enum { + GFS_250DPS = 0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS +}; + +enum { + MFS_14BITS = 0, // 0.6 mG per LSB + MFS_16BITS // 0.15 mG per LSB +}; + +// Specify sensor full scale +#define Ascale AFS_2G +#define Gscale GFS_250DPS + +#define mRes (10.*4912./32760.0) + +#if Ascale == AFS_2G + #define aRes (2.0/32768.0) +#elif Ascale == AFS_4G + #define aRes (4.0/32768.0) +#elif Ascale == AFS_8G + #define aRes (8.0/32768.0) +#elif Ascale == AFS_16G + #define aRes (16.0/32768.0) +#endif + +#if Gscale == GFS_250DPS + #define gRes (250.0/32768.0) +#elif Gscale == GFS_500DPS + #define gRes (500.0/32768.0) +#elif Gscale == GFS_1000DPS + #define gRes (1000.0/32768.0) +#elif Gscale == GFS_2000DPS + #define gRes (2000.0/32768.0) +#endif + +// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read +#define Mmode 0x02 + +#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral +#define Ki 0.0f + +class CQuaterion +{ +public: + void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); + void getOrientation(ORIENTATION* ori); +private: + float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion + // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) + float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) + float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) + float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta + float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value + uint32_t firstUpdate = 0; // used to calculate integration interval + uint32_t lastUpdate = 0; + float deltat = 0.0f; +}; + +class MEMS_I2C +{ +public: + MEMS_I2C() {}; + virtual ~MEMS_I2C() { uninitI2C(); }; + virtual byte begin(bool fusion = false) = 0; + virtual void end() { uninitI2C(); } + virtual bool read(float* acc, float* gyr = 0, float* mag = 0, float* temp = 0, ORIENTATION* ori = 0) = 0; +protected: + bool initI2C(unsigned long clock); + void uninitI2C(); +}; + +class MPU9250 : public MEMS_I2C +{ +public: + byte begin(bool fusion = false); + bool read(float* acc, float* gyr = 0, float* mag = 0, float* temp = 0, ORIENTATION* ori = 0); +private: + void writeByte(uint8_t, uint8_t); + uint8_t readByte(uint8_t); + bool readBytes(uint8_t, uint8_t, uint8_t *); + void readAccelData(int16_t *); + int16_t readTempData(); + void init(); + void getAres(); + void getMres(); + void getGres(); + void readGyroData(int16_t *); + void readMagData(int16_t *); + bool initAK8963(float *); + void calibrateMPU9250(float * gyroBias, float * accelBias); + void MPU9250SelfTest(float * destination); + void writeByteAK(uint8_t, uint8_t); + uint8_t readByteAK(uint8_t); + bool readBytesAK(uint8_t, uint8_t, uint8_t *); + float gyroBias[3] = {0}; + float accelBias[3] = {0}; // Bias corrections for gyro and accelerometer + float magCalibration[3] = {0}; + int16_t accelCount[3] = {0}; + int16_t gyroCount[3] = {0}; + int16_t magCount[3] = {0}; // Stores the 16-bit signed magnetometer sensor output + CQuaterion* quaterion = 0; +}; + +class ICM_42627 : public MEMS_I2C +{ +public: + byte begin(bool fusion = false); + bool read(float* acc, float* gyr = 0, float* mag = 0, float* temp = 0, ORIENTATION* ori = 0); +private: + void writeByte(uint8_t, uint8_t); + uint8_t readByte(uint8_t); + bool readBytes(uint8_t, uint8_t, uint8_t *); + void init(); + void readAccelData(int16_t data[]); + void readGyroData(int16_t data[]); + int16_t readTempData(); +}; + +#define ICM_20948_ARD_UNUSED_PIN 0xFF + +// Base +class ICM_20948 { +private: +protected: + ICM_20948_Device_t _device; + bool _has_magnetometer; + + float getTempC ( int16_t val ); + float getGyrDPS ( int16_t axis_val ); + float getAccMG ( int16_t axis_val ); + float getMagUT ( int16_t axis_val ); + +public: + ICM_20948() {}; + virtual ~ICM_20948() {}; + + ICM_20948_AGMT_t agmt; // Acceleometer, Gyroscope, Magenetometer, and Temperature data + ICM_20948_AGMT_t getAGMT ( void ); // Updates the agmt field in the object and also returns a copy directly + + float magX ( void );// micro teslas + float magY ( void );// micro teslas + float magZ ( void );// micro teslas + + float accX ( void );// milli g's + float accY ( void );// milli g's + float accZ ( void );// milli g's + + float gyrX ( void );// degrees per second + float gyrY ( void );// degrees per second + float gyrZ ( void );// degrees per second + + float temp ( void );// degrees celsius + + + ICM_20948_Status_e status; // Status from latest operation + const char* statusString ( ICM_20948_Status_e stat = ICM_20948_Stat_NUM ); // Returns a human-readable status message. Defaults to status member, but prints string for supplied status if supplied + + // Device Level + ICM_20948_Status_e setBank ( uint8_t bank ); // Sets the bank + ICM_20948_Status_e swReset ( void ); // Performs a SW reset + ICM_20948_Status_e sleep ( bool on = false ); // Set sleep mode for the chip + ICM_20948_Status_e lowPower ( bool on = true ); // Set low power mode for the chip + ICM_20948_Status_e setClockSource ( ICM_20948_PWR_MGMT_1_CLKSEL_e source ); // Choose clock source + ICM_20948_Status_e checkID ( void ); // Return 'ICM_20948_Stat_Ok' if whoami matches ICM_20948_WHOAMI + + bool dataReady ( void ); // Returns 'true' if data is ready + uint8_t getWhoAmI ( void ); // Return whoami in out prarmeter + bool isConnected ( void ); // Returns true if communications with the device are sucessful + + + // Internal Sensor Options + ICM_20948_Status_e setSampleMode ( uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode ); // Use to set accel, gyro, and I2C master into cycled or continuous modes + ICM_20948_Status_e setFullScale ( uint8_t sensor_id_bm, ICM_20948_fss_t fss ); + ICM_20948_Status_e setDLPFcfg ( uint8_t sensor_id_bm, ICM_20948_dlpcfg_t cfg ); + ICM_20948_Status_e enableDLPF ( uint8_t sensor_id_bm, bool enable ); + ICM_20948_Status_e setSampleRate ( uint8_t sensor_id_bm, ICM_20948_smplrt_t smplrt ); + + // Interrupts on INT and FSYNC Pins + ICM_20948_Status_e clearInterrupts ( void ); + + ICM_20948_Status_e cfgIntActiveLow ( bool active_low ); + ICM_20948_Status_e cfgIntOpenDrain ( bool open_drain ); + ICM_20948_Status_e cfgIntLatch ( bool latching ); // If not latching then the interrupt is a 50 us pulse + ICM_20948_Status_e cfgIntAnyReadToClear ( bool enabled ); // If enabled, *ANY* read will clear the INT_STATUS register. So if you have multiple interrupt sources enabled be sure to read INT_STATUS first + ICM_20948_Status_e cfgFsyncActiveLow ( bool active_low ); + ICM_20948_Status_e cfgFsyncIntMode ( bool interrupt_mode ); // Can ue FSYNC as an interrupt input that sets the I2C Master Status register's PASS_THROUGH bit + + ICM_20948_Status_e intEnableI2C ( bool enable ); + ICM_20948_Status_e intEnableDMP ( bool enable ); + ICM_20948_Status_e intEnablePLL ( bool enable ); + ICM_20948_Status_e intEnableWOM ( bool enable ); + ICM_20948_Status_e intEnableWOF ( bool enable ); + ICM_20948_Status_e intEnableRawDataReady ( bool enable ); + ICM_20948_Status_e intEnableOverflowFIFO ( uint8_t bm_enable ); + ICM_20948_Status_e intEnableWatermarkFIFO ( uint8_t bm_enable ); + + // Interface Options + ICM_20948_Status_e i2cMasterPassthrough ( bool passthrough = true ); + ICM_20948_Status_e i2cMasterEnable ( bool enable = true ); + ICM_20948_Status_e i2cMasterConfigureSlave ( uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw = true, bool enable = true, bool data_only = false, bool grp = false, bool swap = false ); + + ICM_20948_Status_e i2cMasterSLV4Transaction( uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr = true ); + ICM_20948_Status_e i2cMasterSingleW ( uint8_t addr, uint8_t reg, uint8_t data ); + uint8_t i2cMasterSingleR ( uint8_t addr, uint8_t reg ); + + + // Default Setup + ICM_20948_Status_e startupDefault ( void ); + virtual ICM_20948_Status_e startupMagnetometer ( void ); + virtual ICM_20948_Status_e getMagnetometerData ( ICM_20948_AGMT_t* pagmt ); + + + // direct read/write + ICM_20948_Status_e read ( uint8_t reg, uint8_t* pdata, uint32_t len); + ICM_20948_Status_e write ( uint8_t reg, uint8_t* pdata, uint32_t len); + + CQuaterion* quaterion = 0; +}; + +class ICM_20948_I2C : public MEMS_I2C, public ICM_20948 { +public: + uint8_t _addr; + uint8_t _ad0; + bool _ad0val; + ICM_20948_Serif_t _serif; + + virtual ICM_20948_Status_e readMag( uint8_t reg, uint8_t* pdata, uint8_t len ); + virtual ICM_20948_Status_e writeMag( uint8_t reg, uint8_t* pdata, uint8_t len ); + + byte begin(bool fusion = false); + bool read(float* acc, float* gyr = 0, float* mag = 0, float* tmp = 0, ORIENTATION* ori = 0); + + ICM_20948_Status_e startupMagnetometer( void ); + ICM_20948_Status_e magWhoIAm( void ); + bool magIsConnected( void ); + ICM_20948_Status_e getMagnetometerData ( ICM_20948_AGMT_t* pagmt ); +}; +#endif diff --git a/esp32/libraries/FreematicsPlus/FreematicsNetwork.cpp b/esp32/libraries/FreematicsPlus/FreematicsNetwork.cpp new file mode 100644 index 0000000..83a05e2 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsNetwork.cpp @@ -0,0 +1,936 @@ +/************************************************************************* +* Helper classes for various network communication devices +* Distributed under BSD license +* Visit https://freematics.com for more inform +on +* (C)2012-2021 Developed by Stanley Huang +*************************************************************************/ + +#include +#include "FreematicsBase.h" +#include "FreematicsNetwork.h" + +String HTTPClient::genHeader(HTTP_METHOD method, const char* path, bool keepAlive, const char* payload, int payloadSize) +{ + String header; + // generate a simplest HTTP header + header = method == METHOD_GET ? "GET " : "POST "; + header += path; + header += " HTTP/1.1\r\nConnection: "; + header += keepAlive ? "keep-alive" : "close"; + header += "\r\nHost: "; + header += m_host; + if (method != METHOD_GET) { + header += "\r\nContent-length: "; + header += String(payloadSize); + } + header += "\r\n\r\n"; + return header; +} + +/******************************************************************************* + Implementation for WiFi (on top of Arduino WiFi library) +*******************************************************************************/ + +bool ClientWIFI::setup(unsigned int timeout) +{ + for (uint32_t t = millis(); millis() - t < timeout;) { + if (WiFi.status() == WL_CONNECTED) { + return true; + } + delay(50); + } + return false; +} + +String ClientWIFI::getIP() +{ + return WiFi.localIP().toString(); +} + +bool ClientWIFI::begin(const char* ssid, const char* password) +{ + //listAPs(); + WiFi.begin(ssid, password); +#ifndef ARDUINO_ESP32C3_DEV + WiFi.setTxPower(WIFI_POWER_8_5dBm); +#endif + return true; +} + +void ClientWIFI::end() +{ + WiFi.disconnect(true); +} + +void ClientWIFI::listAPs() +{ + int n = WiFi.scanNetworks(); + if (n <= 0) { + Serial.println("No WiFi AP found"); + } else { + Serial.println("Nearby WiFi APs:"); + for (int i = 0; i < n; ++i) { + // Print SSID and RSSI for each network found + Serial.print(i + 1); + Serial.print(": "); + Serial.print(WiFi.SSID(i)); + Serial.print(" ("); + Serial.print(WiFi.RSSI(i)); + Serial.println("dB)"); + } + } +} + +bool WifiUDP::open(const char* host, uint16_t port) +{ + if (udp.beginPacket(host, port)) { + udpIP = udp.remoteIP(); + udpPort = port; + if (udp.endPacket()) { + return true; + } + } + return false; +} + +bool WifiUDP::send(const char* data, unsigned int len) +{ + if (udp.beginPacket(udpIP, udpPort)) { + if (udp.write((uint8_t*)data, len) == len && udp.endPacket()) { + return true; + } + } + return false; +} + +int WifiUDP::receive(char* buffer, int bufsize, unsigned int timeout) +{ + uint32_t t = millis(); + do { + int bytes = udp.parsePacket(); + if (bytes > 0) { + bytes = udp.read(buffer, bufsize); + return bytes; + } + delay(1); + } while (millis() - t < timeout); + return 0; +} + +String WifiUDP::queryIP(const char* host) +{ + return udpIP.toString(); +} + +void WifiUDP::close() +{ + udp.stop(); +} + +bool WifiHTTP::open(const char* host, uint16_t port) +{ + if (!host) return true; + if (client.connect(host, port)) { + m_state = HTTP_CONNECTED; + m_host = host; + return true; + } else { + m_state = HTTP_ERROR; + return false; + } +} + +void WifiHTTP::close() +{ + client.stop(); + m_state = HTTP_DISCONNECTED; +} + +bool WifiHTTP::send(HTTP_METHOD method, const char* path, bool keepAlive, const char* payload, int payloadSize) +{ + String header = genHeader(method, path, keepAlive, payload, payloadSize); + int len = header.length(); + if (client.write(header.c_str(), len) != len) { + m_state = HTTP_DISCONNECTED; + return false; + } + if (payloadSize) { + if (client.write(payload, payloadSize) != payloadSize) { + m_state = HTTP_ERROR; + return false; + } + } + m_state = HTTP_SENT; + return true; +} + +char* WifiHTTP::receive(char* buffer, int bufsize, int* pbytes, unsigned int timeout) +{ + int bytes = 0; + int contentBytes = 0; + int contentLen = 0; + char* content = 0; + bool keepAlive = true; + + for (uint32_t t = millis(); millis() - t < timeout && bytes < bufsize; ) { + if (!client.available()) { + delay(1); + continue; + } + buffer[bytes++] = client.read(); + buffer[bytes] = 0; + if (content) { + if (++contentBytes == contentLen) break; + } else if (strstr(buffer, "\r\n\r\n")) { + // parse HTTP header + char *p = strstr(buffer, "/1.1 "); + if (!p) p = strstr(buffer, "/1.0 "); + if (p) { + if (p) m_code = atoi(p + 5); + } + keepAlive = strstr(buffer, ": close\r\n") == 0; + p = strstr(buffer, "Content-Length: "); + if (!p) p = strstr(buffer, "Content-length: "); + if (p) { + contentLen = atoi(p + 16); + } + content = buffer + bytes; + } + } + if (!content) { + m_state = HTTP_ERROR; + return 0; + } + + m_state = HTTP_CONNECTED; + if (pbytes) *pbytes = contentBytes; + if (!keepAlive) close(); + return content; +} + +/******************************************************************************* + SIM7600/SIM7070/SIM5360 +*******************************************************************************/ +bool CellSIMCOM::begin(CFreematics* device) +{ + getBuffer(); + m_device = device; + for (byte n = 0; n < 30; n++) { + device->xbTogglePower(200); + device->xbPurge(); + if (!check(2000)) continue; + if (sendCommand("ATE0\r") && sendCommand("ATI\r")) { + // retrieve module info + //Serial.print(m_buffer); + char *p = strstr(m_buffer, "Model:"); + if (!p) { + sendCommand("AT+SIMCOMATI\r"); + p = strstr(m_buffer, "QCN:"); + if (p) { + char *q = strchr(p += 4, '_'); + if (q) { + int l = q - p; + if (l >= sizeof(m_model)) l = sizeof(m_model) - 1; + memcpy(m_model, p, l); + m_model[l] = 0; + } + } + m_type = CELL_SIM7070; + } else { + p = strchr(p, '_'); + if (p++) { + int i = 0; + while (i < sizeof(m_model) - 1 && p[i] && p[i] != '\r' && p[i] != '\n') { + m_model[i] = p[i]; + i++; + } + m_model[i] = 0; + } + m_type = strstr(m_model, "5360") ? CELL_SIM5360 : CELL_SIM7600; + } + p = strstr(m_buffer, "IMEI:"); + if (p) strncpy(IMEI, p[5] == ' ' ? p + 6 : p + 5, sizeof(IMEI) - 1); + return true; + } + } + end(); + return false; +} + +void CellSIMCOM::end() +{ + setGPS(false); + if (m_type == CELL_SIM7070) { + if (!sendCommand("AT+CPOWD=1\r", 1000, "NORMAL POWER DOWN")) { + if (m_device) m_device->xbTogglePower(2510); + } + } else { + if (!sendCommand("AT+CPOF\r")) { + if (m_device) m_device->xbTogglePower(2510); + } + } +} + +bool CellSIMCOM::setup(const char* apn, const char* username, const char* password, unsigned int timeout) +{ + uint32_t t = millis(); + bool success = false; + + if (m_type == CELL_SIM7070) { + do { + do { + success = false; + sendCommand("AT+CFUN=1\r"); + do { + delay(500); + if (sendCommand("AT+CGREG?\r",1000, "+CGREG: 0,")) { + char *p = strstr(m_buffer, "+CGREG: 0,"); + if (p) { + char ret = *(p + 10); + success = ret == '1' || ret == '5'; + } + } + } while (!success && millis() - t < timeout); + if (!success) break; + success = sendCommand("AT+CGACT?\r", 1000, "+CGACT: 1,"); + break; + } while (millis() - t < timeout); + if (!success) break; + + sendCommand("AT+CGNAPN\r"); + if (apn && *apn) { + if (username && password) { + sprintf(m_buffer, "AT+CNCFG=0,0,\"%s\",\"%s\",\"%s\",3\r", apn, username, password); + } else { + sprintf(m_buffer, "AT+CNCFG=0,0,\"%s\"\r", apn); + } + sendCommand(m_buffer); + } + sendCommand("AT+CNACT=0,1\r"); + sendCommand("AT+CNSMOD?\r"); + sendCommand("AT+CSCLK=0\r"); + } while(0); + } else { + do { + do { + m_device->xbWrite("AT+CPSI?\r"); + m_buffer[0] = 0; + const char* answers[] = {"NO SERVICE", ",Online", ",Offline", ",Low Power Mode"}; + int ret = m_device->xbReceive(m_buffer, RECV_BUF_SIZE, 500, answers, 4); + if (ret == 2) { + success = true; + break; + } + if (ret == -1 || ret == 4) break; + delay(500); + } while (millis() - t < timeout); + if (!success) break; + + success = false; + do { + delay(100); + if (sendCommand("AT+CREG?\r", 1000, "+CREG: 0,")) { + char *p = strstr(m_buffer, "+CREG: 0,"); + success = (p && (*(p + 9) == '1' || *(p + 9) == '5')); + } + } while (!success && millis() - t < timeout); + if (!success) break; + + if (m_type == CELL_SIM7600) { + success = false; + do { + delay(100); + if (sendCommand("AT+CGREG?\r",1000, "+CGREG: 0,")) { + char *p = strstr(m_buffer, "+CGREG: 0,"); + success = (p && (*(p + 10) == '1' || *(p + 10) == '5')); + } + } while (!success && millis() - t < timeout); + if (!success) break; + } + + if (apn && *apn) { + sprintf(m_buffer, "AT+CGSOCKCONT=1,\"IP\",\"%s\"\r", apn); + sendCommand(m_buffer); + if (username && password) { + sprintf(m_buffer, "AT+CSOCKAUTH=1,1,\"%s\",\"%s\"\r", username, password); + sendCommand(m_buffer); + } + } + + sendCommand("AT+CSOCKSETPN=1\r"); + sendCommand("AT+CIPMODE=0\r"); + sendCommand("AT+NETOPEN\r"); + } while(0); + } + if (!success) Serial.println(m_buffer); + return success; +} + +bool CellSIMCOM::setGPS(bool on) +{ + if (on) { + if (m_type == CELL_SIM7070) { + sendCommand("AT+CGNSPWR=1\r"); + sendCommand("AT+CGNSMOD=1,1,0,0,0\r"); + if (sendCommand("AT+CGNSINF\r", 1000, "+CGNSINF:")) { + if (!m_gps) { + m_gps = new GPS_DATA; + memset(m_gps, 0, sizeof(GPS_DATA)); + } + return true; + } + } else { + sendCommand("AT+CVAUXV=61\r", 100); + sendCommand("AT+CVAUXS=1\r", 100); + for (byte n = 0; n < 3; n++) { + if ((sendCommand("AT+CGPS=1,1\r") && sendCommand("AT+CGPSINFO=1\r")) || sendCommand("AT+CGPS?\r", 100, "+CGPS: 1")) { + if (!m_gps) { + m_gps = new GPS_DATA; + memset(m_gps, 0, sizeof(GPS_DATA)); + } + return true; + } + sendCommand("AT+CGPS=0\r", 100); + } + } + } else if (m_gps) { + if (m_type == CELL_SIM7070) { + sendCommand("AT+CGNSPWR=0\r"); + } else { + //sendCommand("AT+CVAUXS=0\r"); + sendCommand("AT+CGPS=0\r", 100); + } + GPS_DATA *g = m_gps; + m_gps = 0; + delete g; + return true; + } + return false; +} + +bool CellSIMCOM::getLocation(GPS_DATA** pgd) +{ + if (m_gps) { + if (pgd) *pgd = m_gps; + return m_gps->ts != 0; + } else { + return false; + } +} + +String CellSIMCOM::getIP() +{ + if (m_type == CELL_SIM7070) { + sendCommand("AT+CNACT=0,1\r"); + for (int i = 0; i < 30; i++) { + delay(500); + if (sendCommand("AT+CNACT?\r", 1000)) { + char *ip = strstr(m_buffer, "+CNACT:"); + if (ip) { + ip = strchr(ip, '\"'); + if (ip++ && *ip != '0') { + char *q = strchr(ip, '\"'); + if (q) *q = 0; + return ip; + } + } + } + } + } else { + uint32_t t = millis(); + do { + if (sendCommand("AT+IPADDR\r", 3000, "\r\nOK\r\n")) { + char *p = strstr(m_buffer, "+IPADDR:"); + if (p) { + char *ip = p + 9; + if (*ip != '0') { + char *q = strchr(ip, '\r'); + if (q) *q = 0; + return ip; + } + } + } + delay(500); + } while (millis() - t < 15000); + } + return ""; +} + +int CellSIMCOM::RSSI() +{ + if (sendCommand("AT+CSQ\r")) { + char *p = strchr(m_buffer, ':'); + if (p) { + int csq = atoi(p + 2); + if (csq != 99) { + return csq * 2 - 113; + } + } + } + return 0; +} + +String CellSIMCOM::getOperatorName() +{ + if (sendCommand("AT+COPS?\r")) { + char *p = strstr(m_buffer, ",\""); + if (p) { + p += 2; + char *s = strchr(p, '\"'); + if (s) *s = 0; + return p; + } + } + return ""; +} + +bool CellSIMCOM::check(unsigned int timeout) +{ + uint32_t t = millis(); + do { + if (sendCommand("AT\r", 250)) return true; + } while (millis() - t < timeout); + return false; +} + +bool CellSIMCOM::checkSIM(const char* pin) +{ + bool success; + if (pin && *pin) { + snprintf(m_buffer, RECV_BUF_SIZE, "AT+CPIN=\"%s\"\r", pin); + sendCommand(m_buffer); + } + for (byte n = 0; n < 20 && !(success = sendCommand("AT+CPIN?\r", 500, ": READY")); n++); + return success; +} + +String CellSIMCOM::queryIP(const char* host) +{ + if (m_type == CELL_SIM7070) { + sprintf(m_buffer, "AT+CDNSGIP=\"%s\",1,3000\r", host); + if (sendCommand(m_buffer, 10000, "+CDNSGIP:")) { + char *p = strstr(m_buffer, host); + if (p) { + p = strstr(p, "\",\""); + if (p) { + char *ip = p + 3; + p = strchr(ip, '\"'); + if (p) *p = 0; + return ip; + } + } + } + } else { + sprintf(m_buffer, "AT+CDNSGIP=\"%s\"\r", host); + if (sendCommand(m_buffer, 10000)) { + char *p = strstr(m_buffer, host); + if (p) { + p = strstr(p, ",\""); + if (p) { + char *ip = p + 2; + p = strchr(ip, '\"'); + if (p) *p = 0; + return ip; + } + } + } + } + return ""; +} + +bool CellSIMCOM::sendCommand(const char* cmd, unsigned int timeout, const char* expected) +{ + if (cmd) { + m_device->xbWrite(cmd); + delay(10); + } + m_buffer[0] = 0; + const char* answers[] = {"\r\nOK", "\r\nERROR"}; + byte ret = m_device->xbReceive(m_buffer, RECV_BUF_SIZE, timeout, expected ? &expected : answers, expected ? 1 : 2); + inbound(); + return ret == 1; +} + +float CellSIMCOM::parseDegree(const char* s) +{ + char *p; + unsigned long left = atol(s); + unsigned long tenk_minutes = (left % 100UL) * 100000UL; + if ((p = strchr(s, '.'))) + { + unsigned long mult = 10000; + while (isdigit(*++p)) + { + tenk_minutes += mult * (*p - '0'); + mult /= 10; + } + } + return (left / 100) + (float)tenk_minutes / 6 / 1000000; +} + +void CellSIMCOM::checkGPS() +{ + if (!m_gps) return; + // check and parse GPS data + if (m_type == CELL_SIM7070) { + if (sendCommand("AT+CGNSINF\r", 100, "+CGNSINF:")) do { + char *p; + if (!(p = strchr(m_buffer, ':'))) break; + p += 2; + if (strncmp(p, "1,1,", 4)) break; + p += 4; + m_gps->time = atol(p + 8) * 100 + atoi(p + 15); + *(p + 8) = 0; + int day = atoi(p + 6); + *(p + 6) = 0; + int month = atoi(p + 4); + *(p + 4) = 0; + int year = atoi(p + 2); + m_gps->date = year + month * 100 + day * 10000; + if (!(p = strchr(p + 9, ','))) break; + m_gps->lat = atof(++p); + if (!(p = strchr(p, ','))) break; + m_gps->lng = atof(++p); + if (!(p = strchr(p, ','))) break; + m_gps->alt = atof(++p); + if (!(p = strchr(p, ','))) break; + m_gps->speed = atof(++p) * 1000 / 1852; + if (!(p = strchr(p, ','))) break; + m_gps->heading = atoi(++p); + m_gps->ts = millis(); + } while (0); + } +} + +void CellSIMCOM::inbound() +{ + if (m_type == CELL_SIM7070) { + if (strstr(m_buffer, "+CADATAIND: 0") || strstr(m_buffer, "+SHREAD:")) { + m_incoming = 1; + } + } else { + char *p; + if (m_gps && (p = strstr(m_buffer, "+CGPSINFO:"))) do { + if (!(p = strchr(p, ':'))) break; + if (*(++p) == ',') break; + m_gps->lat = parseDegree(p); + if (!(p = strchr(p, ','))) break; + if (*(++p) == 'S') m_gps->lat = -m_gps->lat; + if (!(p = strchr(p, ','))) break; + m_gps->lng = parseDegree(++p); + if (!(p = strchr(p, ','))) break; + if (*(++p) == 'W') m_gps->lng = -m_gps->lng; + if (!(p = strchr(p, ','))) break; + m_gps->date = atoi(++p); + if (!(p = strchr(p, ','))) break; + m_gps->time = atof(++p) * 100; + if (!(p = strchr(p, ','))) break; + m_gps->alt = atof(++p); + if (!(p = strchr(p, ','))) break; + m_gps->speed = atof(++p); + if (!(p = strchr(p, ','))) break; + m_gps->heading = atoi(++p); + m_gps->ts = millis(); + } while (0); + + if (strstr(m_buffer, "+IPD") || strstr(m_buffer, "RECV EVENT")) { + Serial.println("[CELL] Incoming data"); + m_incoming = 1; + } + } +} + +char* CellSIMCOM::getBuffer() +{ + if (!m_buffer) m_buffer = (char*)malloc(RECV_BUF_SIZE); + return m_buffer; +} + +bool CellUDP::open(const char* host, uint16_t port) +{ + if (host) { + udpIP = queryIP(host); + if (!udpIP.length()) { + udpIP = host; + } + udpPort = port; + } + if (!udpIP.length()) return false; + if (m_type == CELL_SIM7070) { + sendCommand("AT+CNACT=0,1\r"); + sendCommand("AT+CACID=0\r"); + sprintf(m_buffer, "AT+CAOPEN=0,0,\"UDP\",\"%s\",%u\r", udpIP.c_str(), udpPort); + if (!sendCommand(m_buffer, 3000)) { + Serial.println(m_buffer); + return false; + } + return true; + } else { + sprintf(m_buffer, "AT+CIPOPEN=0,\"UDP\",\"%s\",%u,8000\r", udpIP.c_str(), udpPort); + if (!sendCommand(m_buffer, 3000)) { + Serial.println(m_buffer); + return false; + } + return true; + } +} + +bool CellUDP::close() +{ + if (m_type == CELL_SIM7070) { + sendCommand("AT+CACLOSE=0\r"); + return sendCommand("AT+CNACT=0,0\r"); + } else { + return sendCommand("AT+CIPCLOSE=0\r"); + } +} + +bool CellUDP::send(const char* data, unsigned int len) +{ + if (m_type == CELL_SIM7070) { + sendCommand("AT+CASTATE?\r"); + sprintf(m_buffer, "AT+CASEND=0,%u\r", len); + sendCommand(m_buffer, 100, "\r\n>"); + if (sendCommand(data, 1000)) return true; + } else { + int n = sprintf(m_buffer, "AT+CIPSEND=0,%u,\"%s\",%u\r", len, udpIP.c_str(), udpPort); + m_device->xbWrite(m_buffer, n); + delay(10); + m_device->xbWrite(data, len); + const char* answers[] = {"\r\nERROR", "OK\r\n\r\n+CIPSEND:", "\r\nRECV FROM:"}; + byte ret = m_device->xbReceive(m_buffer, RECV_BUF_SIZE, 1000, answers, 3); + if (ret > 1) return true; + } + return false; +} + +char* CellUDP::receive(int* pbytes, unsigned int timeout) +{ + if (m_type == CELL_SIM7070) { + if (!m_incoming && timeout) sendCommand(0, timeout, "+CADATAIND: 0"); + if (!m_incoming) return 0; + m_incoming = 0; + if (sendCommand("AT+CARECV=0,384\r", timeout)) { + char *p = strstr(m_buffer, "+CARECV: "); + if (p) { + if (pbytes) *pbytes = atoi(p + 9); + p = strchr(m_buffer, ','); + return p ? p + 1 : m_buffer; + } + } + } else { + if (!m_incoming && timeout) sendCommand(0, timeout, "+IPD"); + if (m_incoming) { + m_incoming = 0; + char *p = strstr(m_buffer, "+IPD"); + if (p) { + *p = '-'; // mark this datagram as checked + int len = atoi(p + 4); + if (pbytes) *pbytes = len; + p = strchr(p, '\n'); + if (p) { + if (strlen(++p) > len) *(p + len) = 0; + return p; + } + } + } + } + return 0; +} + +void CellHTTP::init() +{ + if (m_type != CELL_SIM7070) { + sendCommand("AT+CHTTPSSTOP\r"); + sendCommand("AT+CHTTPSSTART\r"); + } +} + +bool CellHTTP::open(const char* host, uint16_t port) +{ + if (m_type == CELL_SIM7070) { + sendCommand("AT+CNACT=0,1\r"); + sendCommand("AT+CACID=0\r"); + + bool useSSL = (port == 443); + if (useSSL) { + sendCommand("AT+SHSSL=1,\"\"\r"); + sendCommand("AT+CSSLCFG=\"ignorertctime\",1,1\r"); + sendCommand("AT+CSSLCFG=\"SSLVERSION\",1,3\r"); + sprintf(m_buffer, "AT+CSSLCFG=\"sni\",1,\"%s\"\r", host); + sendCommand(m_buffer); + } + + sprintf(m_buffer, "AT+SHCONF=\"URL\",\"%s://%s:%u\"\r", useSSL ? "https" : "http", host, port); + if (!sendCommand(m_buffer)) { + return false; + } + sendCommand("AT+SHCONF=\"HEADERLEN\",256\r"); + sendCommand("AT+SHCONF=\"BODYLEN\",1024\r"); + sendCommand("AT+SHCONN\r", HTTP_CONN_TIMEOUT); + if (sendCommand("AT+SHSTATE?\r")) { + if (strstr(m_buffer, "+SHSTATE: 1")) { + m_state = HTTP_CONNECTED; + m_host = host; + sendCommand("AT+SHCHEAD\r"); + sendCommand("AT+SHAHEAD=\"User-Agent\",\"curl/7.47.0\"\r"); + sendCommand("AT+SHAHEAD=\"Cache-control\",\"no-cache\"\r"); + sendCommand("AT+SHAHEAD=\"Connection\",\"keep-alive\"\r"); + sendCommand("AT+SHAHEAD=\"Accept\",\"*/*\"\r"); + m_state = HTTP_CONNECTED; + return true; + } + } + } else { + memset(m_buffer, 0, RECV_BUF_SIZE); + sprintf(m_buffer, "AT+CHTTPSOPSE=\"%s\",%u,%u\r", host, port, port == 443 ? 2: 1); + if (sendCommand(m_buffer, 1000)) { + if (sendCommand(0, HTTP_CONN_TIMEOUT, "+CHTTPSOPSE:")) { + m_state = HTTP_CONNECTED; + m_host = host; + return true; + } + } + } + Serial.println(m_buffer); + m_state = HTTP_ERROR; + return false; +} + +bool CellHTTP::close() +{ + m_state = HTTP_DISCONNECTED; + if (m_type == CELL_SIM7070) { + return sendCommand("AT+SHDISC\r"); + } else if (m_type == CELL_SIM5360) { + return sendCommand("AT+CHTTPSCLSE\r", 1000, "+CHTTPSCLSE:"); + } else { + return sendCommand("AT+CIPCLOSE=0\r"); + } +} + +bool CellHTTP::send(HTTP_METHOD method, const char* path, bool keepAlive, const char* payload, int payloadSize) +{ + if (m_type == CELL_SIM7070) { + if (method == METHOD_POST) { + sprintf(m_buffer, "AT+SHBOD=%u,1000\r", payloadSize); + if (sendCommand(m_buffer, 1000, "\r\n>")) { + sendCommand(payload); + } + } + snprintf(m_buffer, RECV_BUF_SIZE, "AT+SHREQ=\"%s\",%u\r", path, method == METHOD_GET ? 1 : 3); + if (sendCommand(m_buffer, HTTP_CONN_TIMEOUT)) { + char *p; + int len = 0; + if (strstr(m_buffer, "+SHREQ:") || sendCommand(0, HTTP_CONN_TIMEOUT, "+SHREQ:")) { + if ((p = strstr(m_buffer, "+SHREQ:")) && (p = strchr(p, ','))) { + m_code = atoi(++p); + if ((p = strchr(p, ','))) len = atoi(++p); + } + } + if (len > 0) { + if (len > RECV_BUF_SIZE - 16) len = RECV_BUF_SIZE - 16; + sprintf(m_buffer, "AT+SHREAD=0,%u\r", len); + if (sendCommand(m_buffer)) { + m_state = HTTP_SENT; + return true; + } + } + } + } else { + String header = genHeader(method, path, keepAlive, payload, payloadSize); + int len = header.length(); + sprintf(m_buffer, "AT+CHTTPSSEND=%u\r", len + payloadSize); + if (!sendCommand(m_buffer, 100, ">")) { + m_state = HTTP_DISCONNECTED; + return false; + } + // send HTTP header + m_device->xbWrite(header.c_str()); + // send POST payload if any + if (payload) m_device->xbWrite(payload, payloadSize); + if (sendCommand(0, 200, "+CHTTPSSEND:")) { + m_state = HTTP_SENT; + return true; + } + } + Serial.println(m_buffer); + m_state = HTTP_ERROR; + return false; +} + +char* CellHTTP::receive(int* pbytes, unsigned int timeout) +{ + if (m_type == CELL_SIM7070) { + if (!m_incoming && timeout) sendCommand(0, timeout, "+SHREAD:"); + if (!m_incoming) return 0; + + m_incoming = 0; + m_state = HTTP_CONNECTED; + + char *p = strstr(m_buffer, "+SHREAD:"); + if (p) { + int bytes = atoi(p += 9); + if (pbytes) *pbytes = bytes; + p = strchr(p, '\n'); + if (p++) { + *(p + bytes) = 0; + return p; + } + } + return 0; + } else { + // start receiving + int received = 0; + char* payload = 0; + bool keepalive; + + if (!m_incoming && timeout) sendCommand(0, timeout, "RECV EVENT"); + if (!m_incoming) return 0; + m_incoming = 0; + + // to be compatible with SIM5360 + bool legacy = false; + char *p = strstr(m_buffer, "RECV EVENT"); + if (p && *(p - 1) == ' ') legacy = true; + + /* + +CHTTPSRECV:XX\r\n + [payload]\r\n + +CHTTPSRECV:0\r\n + */ + // TODO: implement for multiple chunks of data + // only process first chunk now + sprintf(m_buffer, "AT+CHTTPSRECV=%u\r", RECV_BUF_SIZE - 36); + if (sendCommand(m_buffer, timeout, legacy ? "\r\n+CHTTPSRECV: 0" : "\r\n+CHTTPSRECV:0")) { + char *p = strstr(m_buffer, "\r\n+CHTTPSRECV: DATA"); + if (p) { + if ((p = strchr(p, ','))) { + received = atoi(p + 1); + char *q = strchr(p, '\n'); + payload = q ? (q + 1) : p; + if (m_buffer + RECV_BUF_SIZE - payload > received) { + payload[received] = 0; + } + } + } + } + if (received == 0) { + m_state = HTTP_ERROR; + return 0; + } + + p = strstr(payload, "/1.1 "); + if (!p) p = strstr(payload, "/1.0 "); + if (p) { + if (p) m_code = atoi(p + 5); + } + keepalive = strstr(m_buffer, ": close\r\n") == 0; + + m_state = HTTP_CONNECTED; + if (!keepalive) close(); + if (pbytes) *pbytes = received; + return payload; + } +} diff --git a/esp32/libraries/FreematicsPlus/FreematicsNetwork.h b/esp32/libraries/FreematicsPlus/FreematicsNetwork.h new file mode 100644 index 0000000..ffe7509 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsNetwork.h @@ -0,0 +1,159 @@ +/************************************************************************* +* Telematics Data Logger Class +* Distributed under BSD license +* Developed by Stanley Huang https://www.facebook.com/stanleyhuangyc +*************************************************************************/ + +#ifndef FREEMATICS_NETWORK +#define FREEMATICS_NETWORK + +#include +#include +#include + +#include "esp_system.h" +#include "esp_log.h" +#include "esp_wifi.h" +#include "nvs_flash.h" + +#include "FreematicsBase.h" + +#define XBEE_BAUDRATE 115200 +#define HTTP_CONN_TIMEOUT 5000 + +#define RECV_BUF_SIZE 512 + +typedef enum { + METHOD_GET = 0, + METHOD_POST, +} HTTP_METHOD; + +typedef enum { + HTTP_DISCONNECTED = 0, + HTTP_CONNECTED, + HTTP_SENT, + HTTP_ERROR, +} HTTP_STATES; + +typedef struct { + float lat; + float lng; + uint8_t year; /* year past 2000, e.g. 15 for 2015 */ + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; +} NET_LOCATION; + +class HTTPClient +{ +public: + HTTP_STATES state() { return m_state; } + uint16_t code() { return m_code; } +protected: + String genHeader(HTTP_METHOD method, const char* path, bool keepAlive, const char* payload, int payloadSize); + HTTP_STATES m_state = HTTP_DISCONNECTED; + uint16_t m_code = 0; + String m_host; +}; + +class ClientWIFI +{ +public: + bool begin(const char* ssid, const char* password); + void end(); + bool setup(unsigned int timeout = 5000); + String getIP(); + int getSignal() { return 0; } + const char* deviceName() { return "WiFi"; } + void listAPs(); + bool connected() { return WiFi.isConnected(); } + int RSSI() { return WiFi.RSSI(); } +protected: +}; + +class WifiUDP : public ClientWIFI +{ +public: + bool open(const char* host, uint16_t port); + void close(); + bool send(const char* data, unsigned int len); + int receive(char* buffer, int bufsize, unsigned int timeout = 100); + String queryIP(const char* host); +private: + IPAddress udpIP; + uint16_t udpPort; + WiFiUDP udp; +}; + +class WifiHTTP : public HTTPClient, public ClientWIFI +{ +public: + bool open(const char* host = 0, uint16_t port = 0); + void close(); + bool send(HTTP_METHOD method, const char* path, bool keepAlive, const char* payload = 0, int payloadSize = 0); + char* receive(char* buffer, int bufsize, int* pbytes = 0, unsigned int timeout = HTTP_CONN_TIMEOUT); +private: + WiFiClient client; +}; + +typedef enum { + CELL_SIM7600 = 0, + CELL_SIM7070 = 1, + CELL_SIM5360 = 2 +} CELL_TYPE; + +class CellSIMCOM +{ +public: + virtual bool begin(CFreematics* device); + virtual void end(); + virtual bool setup(const char* apn, const char* username = 0, const char* password = 0, unsigned int timeout = 30000); + virtual bool setGPS(bool on); + virtual String getIP(); + int RSSI(); + String getOperatorName(); + bool checkSIM(const char* pin = 0); + virtual String queryIP(const char* host); + virtual bool getLocation(GPS_DATA** pgd); + bool check(unsigned int timeout = 0); + char* getBuffer(); + const char* deviceName() { return m_model; } + char IMEI[16] = {0}; +protected: + bool sendCommand(const char* cmd, unsigned int timeout = 1000, const char* expected = 0); + virtual void inbound(); + virtual void checkGPS(); + float parseDegree(const char* s); + char* m_buffer = 0; + char m_model[12] = {0}; + CFreematics* m_device = 0; + GPS_DATA* m_gps = 0; + CELL_TYPE m_type = CELL_SIM7600; + int m_incoming = 0; +}; + +class CellUDP : public CellSIMCOM +{ +public: + bool open(const char* host, uint16_t port); + bool close(); + bool send(const char* data, unsigned int len); + char* receive(int* pbytes = 0, unsigned int timeout = 5000); +protected: + String udpIP; + uint16_t udpPort = 0; +}; + +class CellHTTP : public HTTPClient, public CellSIMCOM +{ +public: + void init(); + bool open(const char* host = 0, uint16_t port = 0); + bool close(); + bool send(HTTP_METHOD method, const char* path, bool keepAlive, const char* payload = 0, int payloadSize = 0); + char* receive(int* pbytes = 0, unsigned int timeout = HTTP_CONN_TIMEOUT); +}; + +#endif diff --git a/esp32/libraries/FreematicsPlus/FreematicsOBD.cpp b/esp32/libraries/FreematicsPlus/FreematicsOBD.cpp new file mode 100644 index 0000000..4a7f095 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsOBD.cpp @@ -0,0 +1,562 @@ +/************************************************************************* +* Arduino Library for Freematics ONE+ +* Distributed under BSD license +* Visit https://freematics.com for more information +* (C)2012-2019 Developed by Stanley Huang +*************************************************************************/ + +#include +#include "FreematicsBase.h" +#include "FreematicsOBD.h" + +int dumpLine(char* buffer, int len) +{ + int bytesToDump = len >> 1; + for (int i = 0; i < len; i++) { + // find out first line end and discard the first line + if (buffer[i] == '\r' || buffer[i] == '\n') { + // go through all following \r or \n if any + while (++i < len && (buffer[i] == '\r' || buffer[i] == '\n')); + bytesToDump = i; + break; + } + } + memmove(buffer, buffer + bytesToDump, len - bytesToDump); + return bytesToDump; +} + +uint16_t hex2uint16(const char *p) +{ + char c = *p; + uint16_t i = 0; + for (uint8_t n = 0; c && n < 4; c = *(++p)) { + if (c >= 'A' && c <= 'F') { + c -= 7; + } else if (c>='a' && c<='f') { + c -= 39; + } else if (c == ' ' && n == 2) { + continue; + } else if (c < '0' || c > '9') { + break; + } + i = (i << 4) | (c & 0xF); + n++; + } + return i; +} + +byte hex2uint8(const char *p) +{ + byte c1 = *p; + byte c2 = *(p + 1); + if (c1 >= 'A' && c1 <= 'F') + c1 -= 7; + else if (c1 >= 'a' && c1 <= 'f') + c1 -= 39; + else if (c1 < '0' || c1 > '9') + return 0; + + if (c2 == 0) + return (c1 & 0xf); + else if (c2 >= 'A' && c2 <= 'F') + c2 -= 7; + else if (c2 >= 'a' && c2 <= 'f') + c2 -= 39; + else if (c2 < '0' || c2 > '9') + return 0; + + return c1 << 4 | (c2 & 0xf); +} + +/************************************************************************* +* OBD-II UART Bridge +*************************************************************************/ + +bool COBD::readPID(byte pid, int& result) +{ + char buffer[64]; + char* data = 0; + sprintf(buffer, "%02X%02X\r", dataMode, pid); + link->send(buffer); + idleTasks(); + int ret = link->receive(buffer, sizeof(buffer), OBD_TIMEOUT_SHORT); + if (ret > 0 && !checkErrorMessage(buffer)) { + char *p = buffer; + while ((p = strstr(p, "41 "))) { + p += 3; + byte curpid = hex2uint8(p); + if (curpid == pid) { + errors = 0; + while (*p && *p != ' ') p++; + while (*p == ' ') p++; + if (*p) { + data = p; + break; + } + } + } + } + + if (!data) { + errors++; + return false; + } + result = normalizeData(pid, data); + return true; +} + +byte COBD::readPID(const byte pid[], byte count, int result[]) +{ + byte results = 0; + for (byte n = 0; n < count; n++) { + if (readPID(pid[n], result[n])) { + results++; + } + } + return results; +} + +int COBD::readDTC(uint16_t codes[], byte maxCodes) +{ + /* + Response example: + 0: 43 04 01 08 01 09 + 1: 01 11 01 15 00 00 00 + */ + int codesRead = 0; + if (!link) return 0; + for (int n = 0; n < 6; n++) { + char buffer[128]; + sprintf(buffer, n == 0 ? "03\r" : "03%02X\r", n); + link->send(buffer); + if (link->receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) > 0) { + if (!strstr(buffer, "NO DATA")) { + char *p = strstr(buffer, "43"); + if (p) { + while (codesRead < maxCodes && *p) { + p += 6; + if (*p == '\r') { + p = strchr(p, ':'); + if (!p) break; + p += 2; + } + uint16_t code = hex2uint16(p); + if (code == 0) break; + codes[codesRead++] = code; + } + } + break; + } + } + } + return codesRead; +} + +void COBD::clearDTC() +{ + char buffer[32]; + link->send("04\r"); + link->receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG); +} + +int COBD::normalizeData(byte pid, char* data) +{ + int result; + switch (pid) { + case PID_RPM: + case PID_EVAP_SYS_VAPOR_PRESSURE: // kPa + result = getLargeValue(data) >> 2; + break; + case PID_FUEL_PRESSURE: // kPa + result = getSmallValue(data) * 3; + break; + case PID_COOLANT_TEMP: + case PID_INTAKE_TEMP: + case PID_AMBIENT_TEMP: + case PID_ENGINE_OIL_TEMP: + result = getTemperatureValue(data); + break; + case PID_THROTTLE: + case PID_COMMANDED_EGR: + case PID_COMMANDED_EVAPORATIVE_PURGE: + case PID_FUEL_LEVEL: + case PID_RELATIVE_THROTTLE_POS: + case PID_ABSOLUTE_THROTTLE_POS_B: + case PID_ABSOLUTE_THROTTLE_POS_C: + case PID_ACC_PEDAL_POS_D: + case PID_ACC_PEDAL_POS_E: + case PID_ACC_PEDAL_POS_F: + case PID_COMMANDED_THROTTLE_ACTUATOR: + case PID_ENGINE_LOAD: + case PID_ABSOLUTE_ENGINE_LOAD: + case PID_ETHANOL_FUEL: + case PID_HYBRID_BATTERY_PERCENTAGE: + result = getPercentageValue(data); + break; + case PID_MAF_FLOW: // grams/sec + result = getLargeValue(data) / 100; + break; + case PID_TIMING_ADVANCE: + result = (int)(getSmallValue(data) / 2) - 64; + break; + case PID_DISTANCE: // km + case PID_DISTANCE_WITH_MIL: // km + case PID_TIME_WITH_MIL: // minute + case PID_TIME_SINCE_CODES_CLEARED: // minute + case PID_RUNTIME: // second + case PID_FUEL_RAIL_PRESSURE: // kPa + case PID_ENGINE_REF_TORQUE: // Nm + result = getLargeValue(data); + break; + case PID_CONTROL_MODULE_VOLTAGE: // V + result = getLargeValue(data) / 1000; + break; + case PID_ENGINE_FUEL_RATE: // L/h + result = getLargeValue(data) / 20; + break; + case PID_ENGINE_TORQUE_DEMANDED: // % + case PID_ENGINE_TORQUE_PERCENTAGE: // % + result = (int)getSmallValue(data) - 125; + break; + case PID_SHORT_TERM_FUEL_TRIM_1: + case PID_LONG_TERM_FUEL_TRIM_1: + case PID_SHORT_TERM_FUEL_TRIM_2: + case PID_LONG_TERM_FUEL_TRIM_2: + case PID_EGR_ERROR: + result = ((int)getSmallValue(data) - 128) * 100 / 128; + break; + case PID_FUEL_INJECTION_TIMING: + result = ((int32_t)getLargeValue(data) - 26880) / 128; + break; + case PID_CATALYST_TEMP_B1S1: + case PID_CATALYST_TEMP_B2S1: + case PID_CATALYST_TEMP_B1S2: + case PID_CATALYST_TEMP_B2S2: + result = getLargeValue(data) / 10 - 40; + break; + case PID_AIR_FUEL_EQUIV_RATIO: // 0~200 + result = (long)getLargeValue(data) * 200 / 65536; + break; + case PID_ODOMETER: + if (strlen(data) < 11) + result = -1; + else + result = (uint32_t)hex2uint8(data) << 24 | (uint32_t)hex2uint8(data + 3) << 16 | (uint32_t)hex2uint8(data + 6) << 8 | hex2uint8(data + 9); + break; + default: + result = getSmallValue(data); + } + return result; +} + +char* COBD::getResponse(byte& pid, char* buffer, byte bufsize) +{ + if (!link) return 0; + while (link->receive(buffer, bufsize, OBD_TIMEOUT_SHORT) > 0) { + char *p = buffer; + while ((p = strstr(p, "41 "))) { + p += 3; + byte curpid = hex2uint8(p); + if (pid == 0) pid = curpid; + if (curpid == pid) { + errors = 0; + p += 2; + if (*p == ' ') + return p + 1; + } + } + } + return 0; +} + +void COBD::enterLowPowerMode() +{ + char buf[32]; + if (link) { + reset(); + delay(1000); + link->sendCommand("ATLP\r", buf, sizeof(buf), 1000); + } +} + + +void COBD::leaveLowPowerMode() +{ + // send any command to wake up + char buf[32]; + if (!link) return; + for (byte n = 0; n < 30 && !link->sendCommand("ATI\r", buf, sizeof(buf), 1000); n++); +} + +char* COBD::getResultValue(char* buf) +{ + char* p = buf; + for (;;) { + if (isdigit(*p) || *p == '-') { + return p; + } + p = strchr(p, '\r'); + if (!p) break; + if (*(++p) == '\n') p++; + } + return 0; +} + +float COBD::getVoltage() +{ + char buf[32]; + if (link && link->sendCommand("ATRV\r", buf, sizeof(buf), 500) > 0) { + char* p = getResultValue(buf); + if (p) return (float)atof(p); + } + return 0; +} + +bool COBD::getVIN(char* buffer, byte bufsize) +{ + for (byte n = 0; n < 2; n++) { + if (link && link->sendCommand("0902\r", buffer, bufsize, OBD_TIMEOUT_LONG)) { + int len = hex2uint16(buffer); + char *p = strstr(buffer + 4, "0: 49 02 01"); + if (p) { + char *q = buffer; + p += 11; // skip the header + do { + while (*(++p) == ' '); + for (;;) { + *(q++) = hex2uint8(p); + while (*p && *p != ' ') p++; + while (*p == ' ') p++; + if (!*p || *p == '\r') break; + } + p = strchr(p, ':'); + } while(p); + *q = 0; + if (q - buffer == len - 3) { + return true; + } + } + } + delay(100); + } + return false; +} + +bool COBD::isValidPID(byte pid) +{ + pid--; + byte i = pid >> 3; + byte b = 0x80 >> (pid & 0x7); + return (pidmap[i] & b) != 0; +} + +bool COBD::init(OBD_PROTOCOLS protocol, bool quick) +{ + const char *initcmd[] = {"ATE0\r", "ATH0\r"}; + char buffer[64]; + bool success = false; + + if (!link) { + return false; + } + + m_state = OBD_DISCONNECTED; + for (byte n = 0; n < 3; n++) { + if (link->sendCommand("ATZ\r", buffer, sizeof(buffer), OBD_TIMEOUT_SHORT)) { + success = true; + break; + } + } + if (!success) return false; + for (byte i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { + link->sendCommand(initcmd[i], buffer, sizeof(buffer), OBD_TIMEOUT_SHORT); + } + if (protocol != PROTO_AUTO) { + sprintf(buffer, "ATSP %X\r", protocol); + if (!link->sendCommand(buffer, buffer, sizeof(buffer), OBD_TIMEOUT_SHORT) || !strstr(buffer, "OK")) { + return false; + } + } + if (protocol == PROTO_J1939) { + m_state = OBD_CONNECTED; + errors = 0; + return true; + } + + success = false; + if (quick) { + int value; + if (!readPID(PID_SPEED, value)) return false; + } else { + for (byte n = 0; n < 2; n++) { + int value; + if (readPID(PID_SPEED, value)) { + success = true; + break; + } + } + if (!success) { + return false; + } + } + + // load pid map + memset(pidmap, 0xff, sizeof(pidmap)); + for (byte i = 0; i < 8; i++) { + byte pid = i * 0x20; + sprintf(buffer, "%02X%02X\r", dataMode, pid); + link->send(buffer); + if (!link->receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) || checkErrorMessage(buffer)) { + break; + } + for (char *p = buffer; (p = strstr(p, "41 ")); ) { + p += 3; + if (hex2uint8(p) == pid) { + p += 2; + for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) { + pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1); + } + success = true; + } + } + } + + if (success) { + m_state = OBD_CONNECTED; + errors = 0; + } + return success; +} + +void COBD::reset() +{ + char buf[32]; + if (link) link->sendCommand("ATR\r", buf, sizeof(buf), OBD_TIMEOUT_SHORT); +} + +void COBD::uninit() +{ + char buf[32]; + if (link) link->sendCommand("ATPC\r", buf, sizeof(buf), OBD_TIMEOUT_SHORT); +} + +byte COBD::checkErrorMessage(const char* buffer) +{ + const char *errmsg[] = {"UNABLE", "ERROR", "TIMEOUT", "NO DATA"}; + for (byte i = 0; i < sizeof(errmsg) / sizeof(errmsg[0]); i++) { + if (strstr(buffer, errmsg[i])) return i + 1; + } + return 0; +} + +uint8_t COBD::getPercentageValue(char* data) +{ + return (uint16_t)hex2uint8(data) * 100 / 255; +} + +uint16_t COBD::getLargeValue(char* data) +{ + return hex2uint16(data); +} + +uint8_t COBD::getSmallValue(char* data) +{ + return hex2uint8(data); +} + +int16_t COBD::getTemperatureValue(char* data) +{ + return (int)hex2uint8(data) - 40; +} + +void COBD::setHeaderID(uint32_t num) +{ + if (link) { + char buf[32]; + sprintf(buf, "ATSH %X\r", num & 0xffffff); + link->sendCommand(buf, buf, sizeof(buf), 1000); + sprintf(buf, "ATCP %X\r", num & 0x1f); + link->sendCommand(buf, buf, sizeof(buf), 1000); + } +} + +void COBD::sniff(bool enabled) +{ + if (link) { + char buf[32]; + link->sendCommand(enabled ? "ATM1\r" : "ATM0\r", buf, sizeof(buf), 1000); + } +} + +void COBD::setHeaderFilter(uint32_t num) +{ + if (link) { + char buf[32]; + sprintf(buf, "ATCF %X\r", num); + link->sendCommand(buf, buf, sizeof(buf), 1000); + } +} + +void COBD::setHeaderMask(uint32_t bitmask) +{ + if (link) { + char buf[32]; + sprintf(buf, "ATCM %X\r", bitmask); + link->sendCommand(buf, buf, sizeof(buf), 1000); + } +} + +int COBD::receiveData(byte* buf, int len) +{ + if (!link) return 0; + int n = 0; + for (n = 0; n < len; ) { + int c = link->read(); + if (c == -1 || c == '\r') break; + buf[n++] = c; + } + if (n == 0) return 0; + int bytes = 0; + len = n; + if (buf[0] == '$') { + for (n = 1; n < len && buf[n] != ','; n++); + for (; n < len && buf[n] == ','; bytes++) { + byte d = hex2uint8((const char*)buf + n + 1); + n += 3; + if (buf[n] != ',' && buf[n] != '\r') { + if (d != hex2uint8((const char*)buf + n)) break; + n += 2; + } + buf[bytes] = d; + } + } else { + for (n = 0; n < len; bytes++) { + buf[bytes] = hex2uint8((const char*)buf + n); + n += 2; + if (buf[n++] != ' ') break; + } + } + return bytes; +} + +void COBD::setCANID(uint16_t id) +{ + if (link) { + char buf[32]; + sprintf(buf, "ATSH %X\r", id); + link->sendCommand(buf, buf, sizeof(buf), 1000); + } +} + +int COBD::sendCANMessage(byte msg[], int len, char* buf, int bufsize) +{ + if (!link) return 0; + char cmd[258]; + if (len * 2 >= sizeof(cmd) - 1) len = sizeof(cmd) / 2 - 2; + for (int n = 0; n < len; n++) { + sprintf(cmd + n * 2, "%02X", msg[n]); + } + cmd[len * 2] = '\r'; + cmd[len * 2 + 1] = 0; + return link->sendCommand(cmd, buf, bufsize, 100); +} diff --git a/esp32/libraries/FreematicsPlus/FreematicsOBD.h b/esp32/libraries/FreematicsPlus/FreematicsOBD.h new file mode 100644 index 0000000..55eb2fc --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsOBD.h @@ -0,0 +1,87 @@ +/************************************************************************* +* Arduino Library for Freematics ONE/ONE+ +* Distributed under BSD license +* Visit http://freematics.com/products/freematics-one for more information +* (C)2012-2017 Stanley Huang link = link; } + // initialize OBD-II connection + bool init(OBD_PROTOCOLS protocol = PROTO_AUTO, bool quick = false); + // reset OBD-II connection + void reset(); + // un-initialize OBD-II connection + void uninit(); + // set serial baud rate + bool setBaudRate(unsigned long baudrate); + // get connection state + OBD_STATES getState() { return m_state; } + // read specified OBD-II PID value + bool readPID(byte pid, int& result); + // read multiple OBD-II PID values, return number of values obtained + byte readPID(const byte pid[], byte count, int result[]); + // set device into low power mode + void enterLowPowerMode(); + // wake up device from low power mode + void leaveLowPowerMode(); + // read diagnostic trouble codes (return number of DTCs read) + int readDTC(uint16_t codes[], byte maxCodes = 1); + // clear diagnostic trouble code + void clearDTC(); + // get battery voltage (works without ECU) + float getVoltage(); + // get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE + bool getVIN(char* buffer, byte bufsize); + // determine if the PID is supported + bool isValidPID(byte pid); + // specify custom CAN header ID + void setHeaderID(uint32_t num); + // toggle CAN sniffing mode, call setHeaderFilter and setHeaderMask before start sniffing + void sniff(bool enabled = true); + // set CAN bus header filter + void setHeaderFilter(uint32_t num); + // set CAN bus header filter bitmask + void setHeaderMask(uint32_t bitmask); + // receive sniffed data + int receiveData(byte* buf, int len); + // set CAN ID for sending message + void setCANID(uint16_t id); + // send CAN message + int sendCANMessage(byte msg[], int len, char* buf, int bufsize); + // set current PID mode + byte dataMode = 1; + // occurrence of errors + byte errors = 0; + // bit map of supported PIDs + byte pidmap[4 * 8] = {0}; + // link object pointer + CLink* link = 0; +protected: + virtual void idleTasks() { delay(5); } + char* getResponse(byte& pid, char* buffer, byte bufsize); + uint8_t getPercentageValue(char* data); + uint16_t getLargeValue(char* data); + uint8_t getSmallValue(char* data); + int16_t getTemperatureValue(char* data); + int normalizeData(byte pid, char* data); + byte checkErrorMessage(const char* buffer); + char* getResultValue(char* buf); + OBD_STATES m_state = OBD_DISCONNECTED; +}; + +#endif diff --git a/esp32/libraries/FreematicsPlus/FreematicsPlus.cpp b/esp32/libraries/FreematicsPlus/FreematicsPlus.cpp new file mode 100644 index 0000000..41ea808 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsPlus.cpp @@ -0,0 +1,971 @@ +/************************************************************************* +* Arduino library for ESP32 based Freematics ONE+ and Freematics Esprit +* Distributed under BSD license +* Visit https://freematics.com for more information +* (C)2017-2019 Developed by Stanley Huang +*************************************************************************/ + +#include +#include +#include +#include +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "esp_system.h" +#include "esp_pm.h" +#include "esp_task_wdt.h" +#include "nvs_flash.h" +#include "driver/uart.h" +#include "esp_log.h" +#include "soc/uart_struct.h" +#include "soc/rtc_cntl_reg.h" + +#include "FreematicsPlus.h" +#include "FreematicsGPS.h" + +#ifdef ARDUINO_ESP32C3_DEV +#include "driver/temp_sensor.h" +#else +#include "soc/sens_reg.h" +#endif + +#define VERBOSE_LINK 0 +#define VERBOSE_XBEE 0 + +static TinyGPS gps; +static bool gpsHasDecodedData = false; +static uart_port_t gpsUARTNum = GPS_UART_NUM; +static int pinGPSRx = PIN_GPS_UART_RXD; +static int pinGPSTx = PIN_GPS_UART_TXD; +static Task taskGPS; +static GPS_DATA gpsData = {0}; +// u-blox M10 commands for setting GGA to 5Hz, RMC to 1Hz and disabling NAV_PVT +static const uint8_t gpsSettings[] = {0xB5, 0x62, 0x06, 0x8A, 0x13, 0x00, 0x01, 0x01, 0x00, 0x00, 0xBB, 0x00, 0x91, 0x20, 0x02, 0xAC, 0x00, 0x91, 0x20, 0x0A, 0x07, 0x00, 0x91, 0x20, 0x00, 0x32, 0x74}; + +#ifndef ARDUINO_ESP32C3_DEV + +static uint32_t inline getCycleCount() +{ + uint32_t ccount; + __asm__ __volatile__("esync; rsr %0,ccount":"=a" (ccount)); + return ccount; +} + +static uint8_t inline readRxPin() +{ +#if PIN_GPS_UART_RXD < 32 + return (uint8_t)(GPIO.in >> PIN_GPS_UART_RXD) << 7; +#else + return (uint8_t)(GPIO.in1.val >> (PIN_GPS_UART_RXD - 32)) << 7; +#endif +} + +static void inline setTxPinHigh() +{ +#if PIN_GPS_UART_TXD < 32 + GPIO.out_w1ts = ((uint32_t)1 << PIN_GPS_UART_TXD); +#else + GPIO.out1_w1ts.val = ((uint32_t)1 << (PIN_GPS_UART_TXD - 32)); +#endif +} + +static void inline setTxPinLow() +{ +#if PIN_GPS_UART_TXD < 32 + GPIO.out_w1tc = ((uint32_t)1 << PIN_GPS_UART_TXD); +#else + GPIO.out1_w1tc.val = ((uint32_t)1 << (PIN_GPS_UART_TXD - 32)); +#endif +} + +static void softSerialTx(uint32_t baudrate, uint8_t c) +{ + uint32_t start = getCycleCount(); + uint32_t duration; + // start bit + setTxPinLow(); + for (uint32_t i = 1; i <= 8; i++, c >>= 1) { + duration = i * F_CPU / baudrate; + while (getCycleCount() - start < duration); + if (c & 0x1) + setTxPinHigh(); + else + setTxPinLow(); + } + duration = (uint32_t)9 * F_CPU / baudrate; + while (getCycleCount() - start < duration); + setTxPinHigh(); + duration = (uint32_t)10 * F_CPU / baudrate; + while (getCycleCount() - start < duration); +} + +static void gps_soft_decode_task(void* inst) +{ + // start receiving and decoding + for (;;) { + uint8_t c = 0; + do { + taskYIELD(); + } while (readRxPin()); + uint32_t start = getCycleCount(); + uint32_t duration; + for (uint32_t i = 1; i <= 7; i++) { + taskYIELD(); + duration = i * F_CPU / GPS_SOFT_BAUDRATE + F_CPU / GPS_SOFT_BAUDRATE / 3; + while (getCycleCount() - start < duration); + c = (c | readRxPin()) >> 1; + } + if (gps.encode(c)) { + gpsHasDecodedData = true; + } + duration = (uint32_t)9 * F_CPU / GPS_SOFT_BAUDRATE + F_CPU / GPS_SOFT_BAUDRATE / 2; + do { + taskYIELD(); + } while (getCycleCount() - start < duration); + } +} + +#endif + +static void gps_decode_task(void* inst) +{ + for (;;) { + uint8_t c = 0; + int len = uart_read_bytes(gpsUARTNum, &c, 1, 60000 / portTICK_RATE_MS); + if (len != 1) continue; + //Serial.print((char)c); + if (gps.encode(c)) { + gpsHasDecodedData = true; + } + } +} + +// get chip temperature sensor +int readChipTemperature() +{ +#ifdef ARDUINO_ESP32C3_DEV + static bool inited = false; + float tsens_out = 0; + if (!inited) { + temp_sensor_config_t temp_sensor = TSENS_CONFIG_DEFAULT(); + temp_sensor_get_config(&temp_sensor); + temp_sensor.dac_offset = TSENS_DAC_DEFAULT; + temp_sensor_set_config(temp_sensor); + temp_sensor_start(); + inited = true; + } + temp_sensor_read_celsius(&tsens_out); + return tsens_out; +#else + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR, 3, SENS_FORCE_XPD_SAR_S); + SET_PERI_REG_BITS(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_CLK_DIV, 10, SENS_TSENS_CLK_DIV_S); + CLEAR_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_POWER_UP); + CLEAR_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_DUMP_OUT); + SET_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_POWER_UP_FORCE); + SET_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_POWER_UP); + ets_delay_us(100); + SET_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_DUMP_OUT); + ets_delay_us(5); + int res = GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR3_REG, SENS_TSENS_OUT, SENS_TSENS_OUT_S); + return (res - 32) * 5 / 9; +#endif +} + +int readChipHallSensor() +{ + return 0; // FIXME +} + +bool Task::create(void (*task)(void*), const char* name, int priority, int stacksize) +{ + if (xHandle) return false; + /* Create the task, storing the handle. */ + BaseType_t xReturned = xTaskCreate(task, name, stacksize, (void*)this, priority, &xHandle); + return xReturned == pdPASS; +} + +void Task::destroy() +{ + if (xHandle) { + TaskHandle_t x = xHandle; + xHandle = 0; + vTaskDelete(x); + } +} + +void Task::sleep(uint32_t ms) +{ + vTaskDelay(ms / portTICK_PERIOD_MS); +} + +bool Task::running() +{ + return xHandle != 0; +} + +void Task::suspend() +{ + if (xHandle) vTaskSuspend(xHandle); +} + +void Task::resume() +{ + if (xHandle) vTaskResume(xHandle); +} + +Mutex::Mutex() +{ + xSemaphore = xSemaphoreCreateMutex(); + xSemaphoreGive(xSemaphore); +} + +void Mutex::lock() +{ + xSemaphoreTake(xSemaphore, portMAX_DELAY); +} + +void Mutex::unlock() +{ + xSemaphoreGive(xSemaphore); +} + +bool CLink_UART::begin(unsigned int baudrate, int rxPin, int txPin) +{ +#if VERBOSE_LINK + Serial.println("[UART BEGIN]"); +#endif + uart_config_t uart_config = { + .baud_rate = (int)baudrate, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .rx_flow_ctrl_thresh = 122, + }; + //Configure UART parameters + uart_param_config(LINK_UART_NUM, &uart_config); + //Set UART pins + uart_set_pin(LINK_UART_NUM, txPin, rxPin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + //Install UART driver + if (uart_driver_install(LINK_UART_NUM, LINK_UART_BUF_SIZE, 0, 0, NULL, 0) != ESP_OK) + return false; + return true; +} + +void CLink_UART::end() +{ +#if VERBOSE_LINK + Serial.println("[UART END]"); +#endif + uart_driver_delete(LINK_UART_NUM); +} + +int CLink_UART::receive(char* buffer, int bufsize, unsigned int timeout) +{ + unsigned char n = 0; + unsigned long startTime = millis(); + unsigned long elapsed; + for (;;) { + elapsed = millis() - startTime; + if (elapsed > timeout) break; + if (n >= bufsize - 1) break; + int len = uart_read_bytes(LINK_UART_NUM, (uint8_t*)buffer + n, bufsize - n - 1, 1); + if (len < 0) break; + if (len == 0) continue; + buffer[n + len] = 0; + if (strstr(buffer + n, "\r>")) { + n = n + len - 1; + buffer[n] = 0; + break; + } + n += len; + if (strstr(buffer, "...")) { + buffer[0] = 0; + n = 0; + timeout += OBD_TIMEOUT_LONG; + } + } +#if VERBOSE_LINK + Serial.print("[UART RECV]"); + Serial.println(buffer); +#endif + return n; +} + +bool CLink_UART::send(const char* str) +{ +#if VERBOSE_LINK + Serial.print("[UART SEND]"); + Serial.println(str); +#endif + int len = strlen(str); + return uart_write_bytes(LINK_UART_NUM, str, len) == len; +} + +int CLink_UART::sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout) +{ + send(cmd); + return receive(buf, bufsize, timeout); +} + +int CLink_UART::read() +{ + uint8_t c; + if (uart_read_bytes(LINK_UART_NUM, &c, 1, 1) == 1) + return c; + else + return -1; +} + +bool CLink_UART::changeBaudRate(unsigned int baudrate) +{ + char buf[32]; + sprintf(buf, "ATBR1 %X\r", baudrate); + sendCommand(buf, buf, sizeof(buf), 1000); + delay(50); + end(); + return begin(baudrate); +} + +bool CLink_SPI::begin(unsigned int freq, int rxPin, int txPin) +{ +#if VERBOSE_LINK + Serial.println("[SPI BEGIN]"); +#endif + pinMode(PIN_LINK_SPI_READY, INPUT); + pinMode(PIN_LINK_SPI_CS, OUTPUT); + digitalWrite(PIN_LINK_SPI_CS, HIGH); + delay(50); + for (uint32_t t = millis(); millis() - t < 50; ) { + if (digitalRead(PIN_LINK_SPI_READY) == LOW) return false; + } + SPI.begin(); + SPI.setFrequency(freq); + esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_ON); + return true; +} + +void CLink_SPI::end() +{ +#if VERBOSE_LINK + Serial.println("[SPI END]"); +#endif + SPI.end(); +} + +int CLink_SPI::receive(char* buffer, int bufsize, unsigned int timeout) +{ + int n = 0; + bool eos = false; + bool matched = false; + portMUX_TYPE m = portMUX_INITIALIZER_UNLOCKED; + uint32_t t = millis(); + do { + while (digitalRead(PIN_LINK_SPI_READY) == HIGH) { + if (millis() - t > 3000) return -1; + delay(1); + } +#if VERBOSE_LINK + Serial.println("[SPI RECV]"); +#endif + portENTER_CRITICAL(&m); + digitalWrite(PIN_LINK_SPI_CS, LOW); + while (digitalRead(PIN_LINK_SPI_READY) == LOW && millis() - t < timeout) { + char c = SPI.transfer(' '); + if (c == 0 && c == 0xff) continue; + if (!eos) eos = (c == 0x9); + if (eos) continue; + if (!matched) { + // match header + if (n == 0 && c != header[0]) continue; + if (n == bufsize - 1) continue; + buffer[n++] = c; + if (n == sizeof(header)) { + matched = memcmp(buffer, header, sizeof(header)) == 0; + if (matched) { + n = 0; + } else { + memmove(buffer, buffer + 1, --n); + } + } + continue; + } + if (n > 3 && c == '.' && buffer[n - 1] == '.' && buffer[n - 2] == '.') { + // SEARCHING... + n = 0; + timeout += OBD_TIMEOUT_LONG; + } else { + if (n == bufsize - 1) { + int bytesDumped = dumpLine(buffer, n); + n -= bytesDumped; +#if VERBOSE_LINK + Serial.println("[SPI BUFFER FULL]"); +#endif + } + buffer[n++] = c; + } + } + digitalWrite(PIN_LINK_SPI_CS, HIGH); + portEXIT_CRITICAL(&m); + } while (!eos && millis() - t < timeout); +#if VERBOSE_LINK + if (!eos) { + // timed out + Serial.println("[SPI RECV TIMEOUT]"); + } +#endif + buffer[n] = 0; +#if VERBOSE_LINK + Serial.print("[SPI RECV]"); + Serial.println(buffer); +#endif + // wait for READY pin to restore high level so SPI bus is released + if (eos) while (digitalRead(PIN_LINK_SPI_READY) == LOW) delay(1); + return n; +} + +bool CLink_SPI::send(const char* str) +{ + if (digitalRead(PIN_LINK_SPI_READY) == LOW) { +#if VERBOSE_LINK + Serial.println("[SPI NOT READY]"); +#endif + return false; + } + portMUX_TYPE m = portMUX_INITIALIZER_UNLOCKED; +#if VERBOSE_LINK + Serial.print("[SPI SEND]"); + Serial.println(str); +#endif + int len = strlen(str); + uint8_t tail = 0x1B; + portENTER_CRITICAL(&m); + digitalWrite(PIN_LINK_SPI_CS, LOW); + delay(1); + SPI.writeBytes((uint8_t*)header, sizeof(header)); + SPI.writeBytes((uint8_t*)str, len); + SPI.writeBytes((uint8_t*)&tail, 1); + delay(1); + digitalWrite(PIN_LINK_SPI_CS, HIGH); + portEXIT_CRITICAL(&m); + return true; +} + +int CLink_SPI::sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout) +{ + uint32_t t = millis(); + int n = 0; + for (byte i = 0; i < 30 && millis() - t < timeout; i++) { + if (!send(cmd)) { + delay(50); + continue; + } + n = receive(buf, bufsize, timeout); + if (n == -1) { + Serial.print('_'); + n = 0; + continue; + } + if (n == 0 || (buf[1] != 'O' && !memcmp(buf + 5, "NO DATA", 7))) { + // data not ready + delay(50); + } else { + break; + } + } + return n; +} + +void FreematicsESP32::gpsEnd(bool powerOff) +{ + if (m_flags & FLAG_GNSS_USE_LINK) { + if (powerOff) { + char buf[16]; + link->sendCommand("ATGPSOFF\r", buf, sizeof(buf), 0); + } + } else { + taskGPS.destroy(); + if (m_flags & FLAG_GNSS_SOFT_SERIAL) { +#ifndef ARDUINO_ESP32C3_DEV + setTxPinLow(); +#endif + } else { + uart_driver_delete(gpsUARTNum); + } + if (powerOff && m_pinGPSPower) digitalWrite(m_pinGPSPower, LOW); + } +} + +bool FreematicsESP32::gpsBeginExt(int baudrate) +{ + if (devType > 0 && devType <= 13) { +#ifdef ARDUINO_ESP32C3_DEV + pinGPSRx = 18; + pinGPSTx = 19; +#else + pinGPSRx = 32; + pinGPSTx = 33; +#endif + m_pinGPSPower = PIN_GPS_POWER2; + } else { + pinGPSRx = PIN_GPS_UART_RXD; + pinGPSTx = PIN_GPS_UART_TXD; + } + // switch on GNSS power + if (m_pinGPSPower) { + pinMode(m_pinGPSPower, OUTPUT); + digitalWrite(m_pinGPSPower, HIGH); + } + if (!(m_flags & FLAG_GNSS_SOFT_SERIAL)) { + uart_config_t uart_config = { + .baud_rate = baudrate, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .rx_flow_ctrl_thresh = 122, + }; + + // configure UART parameters + uart_param_config(gpsUARTNum, &uart_config); + // set UART pins + uart_set_pin(gpsUARTNum, pinGPSTx, pinGPSRx, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + // install UART driver + uart_driver_install(gpsUARTNum, UART_BUF_SIZE, 0, 0, NULL, 0); + // apply GNSS settings + delay(300); + gpsSendCommand(gpsSettings, sizeof(gpsSettings)); + // start decoding task + taskGPS.create(gps_decode_task, "GPS", 1); + } else { +#ifndef ARDUINO_ESP32C3_DEV + pinMode(PIN_GPS_UART_RXD, INPUT); + pinMode(PIN_GPS_UART_TXD, OUTPUT); + setTxPinHigh(); + delay(300); +#ifndef ARDUINO_ESP32C3_DEV + if (m_flags & FLAG_GNSS_SOFT_SERIAL) { + // set GNSS baudrate to 38400bps for M10 + const uint8_t packet[] = {0x0, 0x0, 0xB5, 0x62, 0x06, 0x8A, 0x0C, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x52, 0x40, 0x00, 0x96, 0x00, 0x00, 0xC7, 0x2B}; + // set GNSS baudrate to 38400bps for M8 + //const uint8_t packet[] = {0x0, 0x0, 0xB5, 0x62, 0x06, 0x0, 0x14, 0x0, 0x01, 0x0, 0x0, 0x0, 0xD0, 0x08, 0x0, 0x0, 0x0, 0x96, 0x0, 0x0, 0x7, 0x0, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x93, 0x90}; + for (int i = 0; i < sizeof(packet); i++) softSerialTx(baudrate, packet[i]); + } +#endif + // start GPS decoding task (soft serial) + taskGPS.create(gps_soft_decode_task, "GPS", 1); +#endif + } + + // test run for a while to see if there is data decoded + uint16_t s1 = 0, s2 = 0; + gps.stats(&s1, 0); + for (int i = 0; i < 11; i++) { + delay(100); + gps.stats(&s2, 0); + if (s1 != s2) { +#ifndef ARDUINO_ESP32C3_DEV + if (m_flags & FLAG_GNSS_SOFT_SERIAL) { + // apply GNSS settings + for (int i = 0; i < sizeof(gpsSettings); i++) softSerialTx(GPS_SOFT_BAUDRATE, gpsSettings[i]); + } +#endif + return true; + } + } + // turn off GNSS power if no data in + gpsEnd(); + return false; +} + +bool FreematicsESP32::gpsBegin() +{ + if (!link) return false; + m_flags |= FLAG_GNSS_USE_LINK; + + char buf[256]; + link->sendCommand("ATGPSON\r", buf, sizeof(buf), 100); + delay(300); + // apply GNSS settings + int n = sprintf(buf, "ATGDS"); + for (int i = 0; i < sizeof(gpsSettings); i++) n += sprintf(buf + n, "%02X ", gpsSettings[i]); + buf[n - 1] = '\r'; + link->sendCommand(buf, buf, sizeof(buf), 100); + + uint32_t t = millis(); + bool success = false; + do { + memset(buf, 0, sizeof(buf)); + if (gpsGetNMEA(buf, sizeof(buf)) > 0 && strstr(buf, ("$GNGGA"))) { + success = true; + break; + } + } while (millis() - t < 1000); + if (success) { + m_pinGPSPower = 0; + return true; + } + gpsEnd(); + m_flags &= ~FLAG_GNSS_USE_LINK; + return false; +} + +bool FreematicsESP32::gpsGetData(GPS_DATA** pgd) +{ + if (pgd) *pgd = &gpsData; + if (m_flags & FLAG_GNSS_USE_LINK) { + char buf[160]; + if (!link || link->sendCommand("ATGPS\r", buf, sizeof(buf), 100) == 0) { + return false; + } + char *s = strstr(buf, "$GNIFO,"); + if (!s) return false; + s += 7; + float lat = 0; + float lng = 0; + float alt = 0; + bool good = false; + do { + uint32_t date = atoi(s); + if (!(s = strchr(s, ','))) break; + uint32_t time = atoi(++s); + if (!(s = strchr(s, ','))) break; + gpsData.date = date; + gpsData.time = time; + lat = (float)atoi(++s) / 1000000; + if (!(s = strchr(s, ','))) break; + lng = (float)atoi(++s) / 1000000; + if (!(s = strchr(s, ','))) break; + alt = (float)atoi(++s) / 100; + good = true; + if (!(s = strchr(s, ','))) break; + gpsData.speed = (float)atoi(++s) / 100; + if (!(s = strchr(s, ','))) break; + gpsData.heading = atoi(++s) / 100; + if (!(s = strchr(s, ','))) break; + gpsData.sat = atoi(++s); + if (!(s = strchr(s, ','))) break; + gpsData.hdop = atoi(++s); + } while(0); + if (good && (gpsData.lat || gpsData.lng)) { + // filter out invalid coordinates + good = (abs(lat * 1000000 - gpsData.lat * 1000000) < 100000 && abs(lng * 1000000 - gpsData.lng * 1000000) < 100000); + } + if (!good) return false; + gpsData.lat = lat; + gpsData.lng = lng; + gpsData.alt = alt; + gpsData.ts = millis(); + return true; + } else { + gps.stats(&gpsData.sentences, &gpsData.errors); + if (!gpsHasDecodedData) return false; + long lat, lng; + bool good = true; + gps.get_position(&lat, &lng, 0); + if (gpsData.lat || gpsData.lng) { + // filter out invalid coordinates + good = (abs(lat - gpsData.lat * 1000000) < 100000 && abs(lng - gpsData.lng * 1000000) < 100000); + } + if (!good) return false; + gpsData.ts = millis(); + gpsData.lat = (float)lat / 1000000; + gpsData.lng = (float)lng / 1000000; + gps.get_datetime((unsigned long*)&gpsData.date, (unsigned long*)&gpsData.time, 0); + long alt = gps.altitude(); + if (alt != TinyGPS::GPS_INVALID_ALTITUDE) gpsData.alt = (float)alt / 100; + unsigned long knot = gps.speed(); + if (knot != TinyGPS::GPS_INVALID_SPEED) gpsData.speed = (float)knot / 100; + unsigned long course = gps.course(); + if (course < 36000) gpsData.heading = course / 100; + unsigned short sat = gps.satellites(); + if (sat != TinyGPS::GPS_INVALID_SATELLITES) gpsData.sat = sat; + unsigned long hdop = gps.hdop(); + gpsData.hdop = hdop > 2550 ? 255 : hdop / 10; + gpsHasDecodedData = false; + return true; + } +} + +int FreematicsESP32::gpsGetNMEA(char* buffer, int bufsize) +{ + if (m_flags & FLAG_GNSS_USE_LINK) { + return link->sendCommand("ATGRR\r", buffer, bufsize, 200); + } else { + return 0; + } +} + +void FreematicsESP32::gpsSendCommand(const uint8_t* cmd, int len) +{ + if (m_flags & FLAG_GNSS_SOFT_SERIAL) { + #ifndef ARDUINO_ESP32C3_DEV + for (int n = 0; n < len; n++) { + softSerialTx(GPS_SOFT_BAUDRATE, cmd[n]); + } +#endif + } else { + uart_write_bytes(gpsUARTNum, cmd, len); + } +} + +bool FreematicsESP32::xbBegin(unsigned long baudrate, int pinRx, int pinTx) +{ + uart_config_t uart_config = { + .baud_rate = (int)baudrate, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .rx_flow_ctrl_thresh = 122, + }; + +#if VERBOSE_XBEE + Serial.print("Bee Rx:"); + Serial.print(pinRx); + Serial.print(" Tx:"); + Serial.println(pinTx); +#endif + //Configure UART parameters + uart_param_config(BEE_UART_NUM, &uart_config); + //Set UART pins + uart_set_pin(BEE_UART_NUM, pinTx, pinRx, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + //Install UART driver + uart_driver_install(BEE_UART_NUM, UART_BUF_SIZE, 0, 0, NULL, 0); + +#ifdef PIN_BEE_PWR + pinMode(PIN_BEE_PWR, OUTPUT); + digitalWrite(PIN_BEE_PWR, LOW); + xbTogglePower(); +#endif + return true; +} + +void FreematicsESP32::xbEnd() +{ + uart_driver_delete(BEE_UART_NUM); +} + +void FreematicsESP32::xbWrite(const char* cmd) +{ + uart_write_bytes(BEE_UART_NUM, cmd, strlen(cmd)); +#if VERBOSE_XBEE + Serial.print("=== SENT@"); + Serial.print(millis()); + Serial.println(" ==="); + Serial.println(cmd); + Serial.println("=================="); +#endif +} + +void FreematicsESP32::xbWrite(const char* data, int len) +{ + uart_write_bytes(BEE_UART_NUM, data, len); +} + +int FreematicsESP32::xbRead(char* buffer, int bufsize, unsigned int timeout) +{ + return uart_read_bytes(BEE_UART_NUM, buffer, bufsize, timeout / portTICK_RATE_MS); +} + +int FreematicsESP32::xbReceive(char* buffer, int bufsize, unsigned int timeout, const char** expected, byte expectedCount) +{ + int bytesRecv = 0; + uint32_t t = millis(); + do { + if (bytesRecv >= bufsize - 16) { + bytesRecv -= dumpLine(buffer, bytesRecv); + } + int n = xbRead(buffer + bytesRecv, bufsize - bytesRecv - 1, 50); + if (n > 0) { +#if VERBOSE_XBEE + Serial.print("=== RECV@"); + Serial.print(millis()); + Serial.println(" ==="); + buffer[bytesRecv + n] = 0; + Serial.print(buffer + bytesRecv); + Serial.println("=================="); +#endif + bytesRecv += n; + buffer[bytesRecv] = 0; + for (byte i = 0; i < expectedCount; i++) { + // match expected string(s) + if (expected[i] && strstr(buffer, expected[i])) return i + 1; + } + } else if (n == -1) { + // an erroneous reading +#if VERBOSE_XBEE + Serial.print("RECV ERROR"); +#endif + break; + } + } while (millis() - t < timeout); + buffer[bytesRecv] = 0; + return bytesRecv == 0 ? 0 : -1; +} + +void FreematicsESP32::xbPurge() +{ + uart_flush(BEE_UART_NUM); +} + +void FreematicsESP32::xbTogglePower(unsigned int duration) +{ +#ifdef PIN_BEE_PWR + digitalWrite(PIN_BEE_PWR, HIGH); +#if VERBOSE_XBEE + Serial.print("Pin "); + Serial.print(PIN_BEE_PWR); + Serial.println(" pull up"); +#endif + delay(duration); + digitalWrite(PIN_BEE_PWR, LOW); +#if VERBOSE_XBEE + Serial.print("Pin "); + Serial.print(PIN_BEE_PWR); + Serial.println(" pull down"); +#endif +#endif +} + +void FreematicsESP32::buzzer(int freq) +{ +#ifdef PIN_BUZZER + if (freq) { + ledcWriteTone(0, freq); + ledcWrite(0, 255); + } else { + ledcWrite(0, 0); + } +#endif +} + +byte FreematicsESP32::getDeviceType() +{ + if (!link) return 0; + char buf[32]; + if (link && link->sendCommand("ATI\r", buf, sizeof(buf), 1000)) { + char *p = strstr(buf, "OBD"); + if (p && (p = strchr(p, ' '))) { + p += 2; + if (isdigit(*p) && *(p + 1) == '.' && isdigit(*(p + 2))) { + devType = (*p - '0') * 10 + (*(p + 2) - '0'); + return devType; + } + } + } + return 0; +} + +bool FreematicsESP32::reactivateLink() +{ + if (!link) return false; + for (int n = 0; n < 30; n++) { + char buf[32]; + if (link->sendCommand("ATI\r", buf, sizeof(buf), 1000)) return true; + } + return false; +} + +void FreematicsESP32::resetLink() +{ +#ifdef PIN_LINK_RESET + if (devType >= 14) { + digitalWrite(PIN_LINK_RESET, LOW); + delay(50); + digitalWrite(PIN_LINK_RESET, HIGH); + delay(1000); + return; + } +#endif + char buf[16]; + if (link) link->sendCommand("ATR\r", buf, sizeof(buf), 100); +} + +bool FreematicsESP32::begin(bool useCoProc, bool useCellular) +{ + // set wifi max power + esp_wifi_set_max_tx_power(80); + + // set watchdog timeout to 600 seconds + esp_task_wdt_init(600, 0); + + m_flags = 0; + m_pinGPSPower = PIN_GPS_POWER; + +#if PIN_BUZZER + // set up buzzer + ledcSetup(0, 2000, 8); + ledcAttachPin(PIN_BUZZER, 0); +#endif + + if (useCoProc) do { + if (link) return false; +#ifdef PIN_LINK_RESET + pinMode(PIN_LINK_RESET, OUTPUT); + digitalWrite(PIN_LINK_RESET, HIGH); +#endif + CLink_UART *linkUART = new CLink_UART; +#if 0 + linkUART->begin(115200); + char buf[16]; + // lift baudrate to 512Kbps + linkUART->sendCommand("ATBR1 7D000\r", buf, sizeof(buf), 50); + linkUART->end(); + delay(50); +#endif + if (linkUART->begin(115200)) { + link = linkUART; + for (byte n = 0; n < 3 && !getDeviceType(); n++); + if (devType) { + m_flags |= FLAG_USE_UART_LINK; + break; + } + link = 0; + linkUART->end(); + } + delete linkUART; + linkUART = 0; +#if 0 + CLink_SPI *linkSPI = new CLink_SPI; + if (linkSPI->begin()) { + link = linkSPI; + for (byte n = 0; n < 10 && !getDeviceType(); n++); + if (devType) { + m_pinGPSPower = PIN_GPS_POWER2; + break; + } + link = 0; + linkSPI->end(); + } + delete linkSPI; + linkSPI = 0; +#endif + useCoProc = false; + } while(0); + + if (useCellular) { + int pinRx = PIN_BEE_UART_RXD; + int pinTx = PIN_BEE_UART_TXD; + if (devType == 13) { + pinRx = 32; + pinTx = 33; + } else if ((devType == 11 && !(m_flags & FLAG_USE_UART_LINK)) || devType == 12 || devType == 0) { +#ifndef ARDUINO_ESP32C3_DEV + //pinRx = 16; + //pinTx = 17; +#endif + } + pinMode(PIN_BEE_PWR, OUTPUT); + digitalWrite(PIN_BEE_PWR, LOW); + xbBegin(XBEE_BAUDRATE, pinRx, pinTx); + m_flags |= FLAG_USE_CELL; + if (useCoProc) { + m_flags |= FLAG_GNSS_SOFT_SERIAL; + } + } + gpsUARTNum = useCoProc ? GPS_UART_NUM : LINK_UART_NUM; + return devType != 0; +} diff --git a/esp32/libraries/FreematicsPlus/FreematicsPlus.h b/esp32/libraries/FreematicsPlus/FreematicsPlus.h new file mode 100644 index 0000000..3b59f6e --- /dev/null +++ b/esp32/libraries/FreematicsPlus/FreematicsPlus.h @@ -0,0 +1,187 @@ +/************************************************************************* +* Arduino library for ESP32 based Freematics ONE+ and Freematics Esprit +* Distributed under BSD license +* Visit https://freematics.com for more information +* (C)2017-2019 Developed by Stanley Huang +*************************************************************************/ +#ifndef FREEMATICS_PLUS +#define FREEMATICS_PLUS + +#include +#include "esp_system.h" +#include "esp_partition.h" +#include "nvs_flash.h" +#include "nvs.h" +#include "esp_spi_flash.h" +#include "soc/rtc.h" +#include "FreematicsBase.h" +#include "FreematicsNetwork.h" +#include "FreematicsMEMS.h" +#include "FreematicsOBD.h" +extern "C" { +#include "utility/ble_spp_server.h" +} + +#define PIN_SD_CS 5 + +#define PIN_LINK_SPI_CS 2 +#define PIN_LINK_SPI_READY 13 +#define SPI_FREQ 1000000 + +#if CONFIG_IDF_TARGET_ESP32C3 && !defined(ARDUINO_ESP32C3_DEV) +#define ARDUINO_ESP32C3_DEV +#endif + +#ifndef ARDUINO_ESP32C3_DEV +// ESP32 variants with 3 hardware serial UART +#define LINK_UART_NUM UART_NUM_2 +#define UART_COUNT 3 +#define PIN_LINK_RESET 15 +#define PIN_BUZZER 25 +#define PIN_BEE_PWR 27 +#define PIN_BEE_UART_RXD 35 +#define PIN_BEE_UART_TXD 2 +#define PIN_LED 4 +#else +// ESP32-C3 has 2 hardware serial UART +#define LINK_UART_NUM UART_NUM_1 +#define UART_COUNT 2 +#define PIN_BEE_PWR 8 +#define PIN_BEE_UART_RXD 18 +#define PIN_BEE_UART_TXD 19 +#endif +#define LINK_UART_BAUDRATE 115200 + +#define LINK_UART_BUF_SIZE 256 +#define PIN_LINK_UART_RX 13 +#define PIN_LINK_UART_TX 14 + +#define BEE_UART_NUM UART_NUM_1 +#define BEE_BAUDRATE 115200L + +#define PIN_GPS_POWER 12 +#define PIN_GPS_POWER2 15 +#define PIN_GPS_UART_RXD 34 +#define PIN_GPS_UART_TXD 26 +#define PIN_GPS_UART_RXD2 32 +#define PIN_GPS_UART_TXD2 33 +#define GPS_UART_NUM UART_NUM_1 +#define GPS_SOFT_BAUDRATE 38400L + +#define PIN_MOLEX_2 34 +#define PIN_MOLEX_4 26 +#define PIN_MOLEX_VCC 12 + +#define UART_BUF_SIZE 256 +#define NMEA_BUF_SIZE 512 + +#define FLAG_USE_CELL 0x2 +#define FLAG_USE_UART_LINK 0x4 +#define FLAG_GNSS_SOFT_SERIAL 0x8 +#define FLAG_GNSS_USE_LINK 0x10 + +int readChipTemperature(); +int readChipHallSensor(); +uint16_t getFlashSize(); /* KB */ + +class Task +{ +public: + bool create(void (*task)(void*), const char* name, int priority = 0, int stacksize = 1024); + void destroy(); + void suspend(); + void resume(); + bool running(); + void sleep(uint32_t ms); +private: + TaskHandle_t xHandle; +}; + +class Mutex +{ +public: + Mutex(); + void lock(); + void unlock(); +private: + QueueHandle_t xSemaphore; +}; + +class CLink_UART : public CLink { +public: + bool begin(unsigned int baudrate = LINK_UART_BAUDRATE, int rxPin = PIN_LINK_UART_RX, int txPin = PIN_LINK_UART_TX); + void end(); + // send command and receive response + int sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout = 1000); + // receive data from UART + int receive(char* buffer, int bufsize, unsigned int timeout); + // write data to UART + bool send(const char* str); + // read one byte from UART + int read(); + // change serial baudrate + bool changeBaudRate(unsigned int baudrate); +}; + +class CLink_SPI : public CLink { +public: + bool begin(unsigned int freq = SPI_FREQ, int rxPin = 0, int txPin = 0); + void end(); + // send command and receive response + int sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout = 1000); + // receive data from SPI + int receive(char* buffer, int bufsize, unsigned int timeout); + // write data to SPI + bool send(const char* str); +private: + const uint8_t header[4] = {0x24, 0x4f, 0x42, 0x44}; +}; + +class FreematicsESP32 : public CFreematics +{ +public: + bool begin(bool useCoProc = true, bool useCellular = true); + // start GPS + bool gpsBegin(); + // start GPS + bool gpsBeginExt(int baudrate = 115200); + // turn off GPS + void gpsEnd(bool powerOff = true); + // get parsed GPS data (returns the number of data parsed since last invoke) + bool gpsGetData(GPS_DATA** pgd); + // get buffered NMEA data + int gpsGetNMEA(char* buffer, int bufsize); + // send command string to GPS + void gpsSendCommand(const uint8_t* cmd, int len); + // start xBee UART communication + bool xbBegin(unsigned long baudrate = BEE_BAUDRATE, int pinRx = PIN_BEE_UART_RXD, int pinTx = PIN_BEE_UART_TXD); + void xbEnd(); + // read data to xBee UART + int xbRead(char* buffer, int bufsize, unsigned int timeout = 1000); + // send data to xBee UART + void xbWrite(const char* cmd); + // send data to xBee UART + void xbWrite(const char* data, int len); + // receive data from xBee UART (returns 0/1/2) + int xbReceive(char* buffer, int bufsize, unsigned int timeout = 1000, const char** expected = 0, byte expectedCount = 0); + // purge xBee UART buffer + void xbPurge(); + // toggle xBee module power + void xbTogglePower(unsigned int duration = 200); + // control internal buzzer (if present) + void buzzer(int freq); + // reset co-processor + void resetLink(); + // reactivate co-processor + bool reactivateLink(); + // get device type + byte getDeviceType(); + // co-processor firmware version number + byte devType = 0; + // co-processor link + CLink *link = 0; +private: + byte m_flags = 0; + byte m_pinGPSPower = 0; +}; +#endif diff --git a/esp32/libraries/FreematicsPlus/keywords.txt b/esp32/libraries/FreematicsPlus/keywords.txt new file mode 100644 index 0000000..72a77a4 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/keywords.txt @@ -0,0 +1,51 @@ +################################################################################ +# Datatypes (KEYWORD1) +################################################################################ +COBDSPI KEYWORD1 +MPU9250_ACC KEYWORD1 +MPU9250_9DOF KEYWORD1 +MPU9250_DMP KEYWORD1 + +################################################################################ +# Methods and Functions (KEYWORD2) +################################################################################ +begin KEYWORD2 +end KEYWORD2 +init KEYWORD2 +uninit KEYWORD2 +reset KEYWORD2 +getState KEYWORD2 +sleep KEYWORD2 +enterLowPowerMode KEYWORD2 +leaveLowPowerMode KEYWORD2 + +readPID KEYWORD2 +readDTC KEYWORD2 +clearDTC KEYWORD2 +getVoltage KEYWORD2 +getVIN KEYWORD2 + +gpsInit KEYWORD2 +gpsGetData KEYWORD2 +gpsGetRawData KEYWORD2 +gpsSendCommand KEYWORD2 +internalGPS KEYWORD2 + +xbBegin KEYWORD2 +xbRead KEYWORD2 +xbWrite KEYWORD2 +xbReceive KEYWORD2 +xbPurge KEYWORD2 +xbTogglePower KEYWORD2 + +################################################################################ +# Constants (LITERAL1) +################################################################################ +OBD_PROTOCOLS LITERAL1 +OBD_STATES LITERAL1 +GPS_DATA LITERAL1 +NET_LOCATION LITERAL1 +INV_X_GYRO LITERAL1 +INV_Y_GYRO LITERAL1 +INV_Z_GYRO LITERAL1 +ORIENTATION LITERAL1 \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/library.json b/esp32/libraries/FreematicsPlus/library.json new file mode 100644 index 0000000..95bfc9d --- /dev/null +++ b/esp32/libraries/FreematicsPlus/library.json @@ -0,0 +1,18 @@ +{ + "name": "FreematicsPlus", + "description": "A cost-effective Arduino programmable vehicle telemetry device in form of OBD dongle with optional BLE or cellular network support.", + "keywords": "obd, freematics, telematics", + "authors": + { + "name": "Stanley Huang", + "email": "stanleyhuangyc@gmail.com", + "url": "https://freematics.com" + }, + "repository": { + "type": "git", + "url": "https://github.com/stanleyhuangyc/Freematics.git" + }, + "include": "libraries/FreematicsPlus", + "frameworks": "arduino", + "platforms": "*" +} diff --git a/esp32/libraries/FreematicsPlus/library.properties b/esp32/libraries/FreematicsPlus/library.properties new file mode 100644 index 0000000..e889115 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/library.properties @@ -0,0 +1,10 @@ +name=FreematicsPlus +version=1.1 +author=Stanley Huang +maintainer=Stanley Huang +sentence=Arduino library for Freematics ONE+ +paragraph=Arduino library for Freematics ONE+ +category=Communication +url=https://freematics.com +architectures=esp32 +includes=FreematicsPlus.h \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/utility/AK09916_ENUMERATIONS.h b/esp32/libraries/FreematicsPlus/utility/AK09916_ENUMERATIONS.h new file mode 100644 index 0000000..2b4e31e --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/AK09916_ENUMERATIONS.h @@ -0,0 +1,19 @@ +#ifndef _AK09916_ENUMERATIONS_H_ +#define _AK09916_ENUMERATIONS_H_ + + + + +typedef enum{ + AK09916_mode_power_down = 0x00, + AK09916_mode_single = (0x01 << 0), + AK09916_mode_cont_10hz = (0x01 << 1), + AK09916_mode_cont_20hz = (0x02 << 1), + AK09916_mode_cont_50hz = (0x03 << 1), + AK09916_mode_cont_100hz = (0x04 << 1), + AK09916_mode_self_test = (0x01 << 4), +}AK09916_mode_e; + + + +#endif // _AK09916_ENUMERATIONS_H_ \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/utility/AK09916_REGISTERS.h b/esp32/libraries/FreematicsPlus/utility/AK09916_REGISTERS.h new file mode 100644 index 0000000..c852c01 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/AK09916_REGISTERS.h @@ -0,0 +1,78 @@ +#ifndef _AK09916_REGISTERS_H_ +#define _AK09916_REGISTERS_H_ + +#include + +typedef enum{ + AK09916_REG_WIA1 = 0x00, + AK09916_REG_WIA2, + // discontinuity + AK09916_REG_ST1 = 0x10, + AK09916_REG_HXL, + AK09916_REG_HXH, + AK09916_REG_HYL, + AK09916_REG_HYH, + AK09916_REG_HZL, + AK09916_REG_HZH, + // discontinuity + AK09916_REG_ST2 = 0x18, + // discontinuity + AK09916_REG_CNTL2 = 0x31, + AK09916_REG_CNTL3, +}AK09916_Reg_Addr_e; + + typedef struct{ + uint8_t WIA1; + }AK09916_WIA1_Reg_t; + + typedef struct{ + uint8_t WIA2; + }AK09916_WIA2_Reg_t; + + typedef struct{ + uint8_t DRDY : 1; + uint8_t DOR : 1 ; + uint8_t reserved_0 : 6 ; + }AK09916_ST1_Reg_t; + + // typedef struct{ + + // }AK09916_HXL_Reg_t; + + // typedef struct{ + + // }AK09916_HXH_Reg_t; + // typedef struct{ + + // }AK09916_HYL_Reg_t; + // typedef struct{ + + // }AK09916_HYH_Reg_t; + // typedef struct{ + + // }AK09916_HZL_Reg_t; + // typedef struct{ + + // }AK09916_HZH_Reg_t; + + typedef struct{ + uint8_t reserved_0 : 3; + uint8_t HOFL : 1; + uint8_t reserved_1 : 4; + }AK09916_ST2_Reg_t; + + typedef struct{ + uint8_t MODE : 5; + uint8_t reserved_0 : 3; + }AK09916_CNTL2_Reg_t; + + typedef struct{ + uint8_t SRST : 1; + uint8_t reserved_0 : 7; + }AK09916_CNTL3_Reg_t; + + +#endif // _AK09916_REGISTERS_H_ + + + diff --git a/esp32/libraries/FreematicsPlus/utility/ICM_20948_C.c b/esp32/libraries/FreematicsPlus/utility/ICM_20948_C.c new file mode 100644 index 0000000..0fc6951 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ICM_20948_C.c @@ -0,0 +1,556 @@ +#include "ICM_20948_C.h" +#include "ICM_20948_REGISTERS.h" +#include "AK09916_REGISTERS.h" + + + +const ICM_20948_Serif_t NullSerif = { + NULL, // write + NULL, // read + NULL, // user +}; + +// Private function prototypes + + + + + + +// Function definitions +ICM_20948_Status_e ICM_20948_link_serif( ICM_20948_Device_t* pdev, const ICM_20948_Serif_t* s ){ + if(s == NULL){ return ICM_20948_Stat_ParamErr; } + if(pdev == NULL){ return ICM_20948_Stat_ParamErr; } + pdev->_serif = s; + return ICM_20948_Stat_Ok; +} + +ICM_20948_Status_e ICM_20948_execute_w( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ){ + if( pdev->_serif->write == NULL ){ return ICM_20948_Stat_NotImpl; } + return (*pdev->_serif->write)( regaddr, pdata, len, pdev->_serif->user ); +} + +ICM_20948_Status_e ICM_20948_execute_r( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ){ + if( pdev->_serif->read == NULL ){ return ICM_20948_Stat_NotImpl; } + return (*pdev->_serif->read)( regaddr, pdata, len, pdev->_serif->user ); +} + + + + +// Single-shot I2C on Master IF +ICM_20948_Status_e ICM_20948_i2c_master_slv4_txn( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr ){ + // Thanks MikeFair! // https://github.com/kriswiner/MPU9250/issues/86 + + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + addr = (((Rw) ? 0x80 : 0x00) | addr ); + + retval = ICM_20948_set_bank( pdev, 3 ); + retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_ADDR, (uint8_t*)&addr, 1 ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + retval = ICM_20948_set_bank( pdev, 3 ); + retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_REG, (uint8_t*)®, 1 ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + ICM_20948_I2C_SLV4_CTRL_t ctrl; + ctrl.EN = 1; + ctrl.INT_EN = false; + ctrl.DLY = 0; + ctrl.REG_DIS = !send_reg_addr; + + // ICM_20948_I2C_MST_STATUS_t i2c_mst_status; + // bool txn_failed = false; + uint16_t nByte = 0; + + while( nByte < len ){ + if( !Rw ){ + retval = ICM_20948_set_bank( pdev, 3 ); + retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_DO, (uint8_t*)&(data[nByte]), 1 ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + } + + // Kick off txn + retval = ICM_20948_set_bank( pdev, 3 ); + retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_SLV4_CTRL_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + // // long tsTimeout = millis() + 3000; // Emergency timeout for txn (hard coded to 3 secs) + // uint32_t max_cycles = 1000; + // uint32_t count = 0; + // bool slave4Done = false; + // while (!slave4Done) { + // retval = ICM_20948_set_bank( pdev, 0 ); + // retval = ICM_20948_execute_r( pdev, AGB0_REG_I2C_MST_STATUS, &i2c_mst_status, 1 ); + + // slave4Done = ( i2c_mst_status.I2C_SLV4_DONE /*| (millis() > tsTimeout) */ ); // todo: avoid forever-loops + // slave4Done |= (count >= max_cycles); + // count++; + // } + // txn_failed = (i2c_mst_status.I2C_SLV4_NACK /* & (1 << I2C_SLV4_NACK_BIT)) | (millis() > tsTimeout) */); + // txn_failed |= (count >= max_cycles); + // if (txn_failed) break; + + // if ( Rw ){ + // retval = ICM_20948_set_bank( pdev, 3 ); + // retval = ICM_20948_execute_r( pdev, AGB3_REG_I2C_SLV4_DI, &data[nByte], 1 ); + // } + + nByte++; + } + // if( txn_failed ){ return ICM_20948_Stat_Err; } + return retval; +} + +ICM_20948_Status_e ICM_20948_i2c_master_single_w( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ){ + return ICM_20948_i2c_master_slv4_txn( pdev, addr, reg, data, 1, false, true ); +} + +ICM_20948_Status_e ICM_20948_i2c_master_single_r( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ){ + return ICM_20948_i2c_master_slv4_txn( pdev, addr, reg, data, 1, true, true ); +} + + + + +ICM_20948_Status_e ICM_20948_set_bank( ICM_20948_Device_t* pdev, uint8_t bank ){ + if( bank > 3 ){ return ICM_20948_Stat_ParamErr; } // Only 4 possible banks + bank = (bank << 4) & 0x30; // bits 5:4 of REG_BANK_SEL + return ICM_20948_execute_w( pdev, REG_BANK_SEL, &bank, 1 ); +} + +ICM_20948_Status_e ICM_20948_sw_reset( ICM_20948_Device_t* pdev ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + ICM_20948_PWR_MGMT_1_t reg; + + ICM_20948_set_bank(pdev, 0); // Must be in the right bank + + retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + reg.DEVICE_RESET = 1; + + retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + return retval; +} + +ICM_20948_Status_e ICM_20948_sleep ( ICM_20948_Device_t* pdev, bool on ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + ICM_20948_PWR_MGMT_1_t reg; + + ICM_20948_set_bank(pdev, 0); // Must be in the right bank + + retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + if(on){ reg.SLEEP = 1; } + else{ reg.SLEEP = 0; } + + retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + return retval; +} + +ICM_20948_Status_e ICM_20948_low_power ( ICM_20948_Device_t* pdev, bool on ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + ICM_20948_PWR_MGMT_1_t reg; + + ICM_20948_set_bank(pdev, 0); // Must be in the right bank + + retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + if(on){ reg.LP_EN = 1; } + else{ reg.LP_EN = 0; } + + retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + return retval; +} + +ICM_20948_Status_e ICM_20948_set_clock_source ( ICM_20948_Device_t* pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + ICM_20948_PWR_MGMT_1_t reg; + + ICM_20948_set_bank(pdev, 0); // Must be in the right bank + + retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + reg.CLKSEL = source; + + retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + return retval; +} + + + +ICM_20948_Status_e ICM_20948_get_who_am_i( ICM_20948_Device_t* pdev, uint8_t* whoami ){ + if( whoami == NULL ){ return ICM_20948_Stat_ParamErr; } + ICM_20948_set_bank(pdev, 0); // Must be in the right bank + return ICM_20948_execute_r( pdev, AGB0_REG_WHO_AM_I, whoami, 1 ); +} + +ICM_20948_Status_e ICM_20948_check_id( ICM_20948_Device_t* pdev ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + uint8_t whoami = 0x00; + retval = ICM_20948_get_who_am_i( pdev, &whoami ); + if( retval != ICM_20948_Stat_Ok){ return retval; } + if( whoami != ICM_20948_WHOAMI ){ return ICM_20948_Stat_WrongID; } + return retval; +} + +ICM_20948_Status_e ICM_20948_data_ready( ICM_20948_Device_t* pdev ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + ICM_20948_INT_STATUS_1_t reg; + retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank + if( retval != ICM_20948_Stat_Ok){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_STATUS_1, (uint8_t*)®, sizeof(ICM_20948_INT_STATUS_1_t)); + if( retval != ICM_20948_Stat_Ok){ return retval; } + if( !reg.RAW_DATA_0_RDY_INT ){ retval = ICM_20948_Stat_NoData; } + return retval; +} + + + + + + + +// Interrupt Configuration +ICM_20948_Status_e ICM_20948_int_pin_cfg ( ICM_20948_Device_t* pdev, ICM_20948_INT_PIN_CFG_t* write, ICM_20948_INT_PIN_CFG_t* read ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank + if( write != NULL ){ // write first, if available + retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)write, sizeof(ICM_20948_INT_PIN_CFG_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + } + if( read != NULL ){ // then read, to allow for verification + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)read, sizeof(ICM_20948_INT_PIN_CFG_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + } + return retval; +} + +ICM_20948_Status_e ICM_20948_int_enable ( ICM_20948_Device_t* pdev, ICM_20948_INT_enable_t* write, ICM_20948_INT_enable_t* read ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + ICM_20948_INT_ENABLE_t en_0; + ICM_20948_INT_ENABLE_1_t en_1; + ICM_20948_INT_ENABLE_2_t en_2; + ICM_20948_INT_ENABLE_3_t en_3; + + retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank + + if( write != NULL ){ // If the write pointer is not NULL then write to the registers BEFORE reading + en_0.I2C_MST_INT_EN = write->I2C_MST_INT_EN; + en_0.DMP_INT1_EN = write->DMP_INT1_EN; + en_0.PLL_READY_EN = write->PLL_RDY_EN; + en_0.WOM_INT_EN = write->WOM_INT_EN; + en_0.REG_WOF_EN = write->REG_WOF_EN; + en_1.RAW_DATA_0_RDY_EN = write->RAW_DATA_0_RDY_EN; + en_2.individual.FIFO_OVERFLOW_EN_4 = write->FIFO_OVERFLOW_EN_4; + en_2.individual.FIFO_OVERFLOW_EN_3 = write->FIFO_OVERFLOW_EN_3; + en_2.individual.FIFO_OVERFLOW_EN_2 = write->FIFO_OVERFLOW_EN_2; + en_2.individual.FIFO_OVERFLOW_EN_1 = write->FIFO_OVERFLOW_EN_1; + en_2.individual.FIFO_OVERFLOW_EN_0 = write->FIFO_OVERFLOW_EN_0; + en_3.individual.FIFO_WM_EN_4 = write->FIFO_WM_EN_4; + en_3.individual.FIFO_WM_EN_3 = write->FIFO_WM_EN_3; + en_3.individual.FIFO_WM_EN_2 = write->FIFO_WM_EN_2; + en_3.individual.FIFO_WM_EN_1 = write->FIFO_WM_EN_1; + en_3.individual.FIFO_WM_EN_0 = write->FIFO_WM_EN_0; + + retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE, (uint8_t*)&en_0, sizeof(ICM_20948_INT_ENABLE_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_1, (uint8_t*)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_2, (uint8_t*)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_3, (uint8_t*)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + } + + if( read != NULL ){ // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read) + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE, (uint8_t*)&en_0, sizeof(ICM_20948_INT_ENABLE_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_1, (uint8_t*)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_2, (uint8_t*)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_3, (uint8_t*)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; } + + read->I2C_MST_INT_EN = en_0.I2C_MST_INT_EN; + read->DMP_INT1_EN = en_0.DMP_INT1_EN; + read->PLL_RDY_EN = en_0.PLL_READY_EN; + read->WOM_INT_EN = en_0.WOM_INT_EN; + read->REG_WOF_EN = en_0.REG_WOF_EN; + read->RAW_DATA_0_RDY_EN = en_1.RAW_DATA_0_RDY_EN; + read->FIFO_OVERFLOW_EN_4 = en_2.individual.FIFO_OVERFLOW_EN_4; + read->FIFO_OVERFLOW_EN_3 = en_2.individual.FIFO_OVERFLOW_EN_3; + read->FIFO_OVERFLOW_EN_2 = en_2.individual.FIFO_OVERFLOW_EN_2; + read->FIFO_OVERFLOW_EN_1 = en_2.individual.FIFO_OVERFLOW_EN_1; + read->FIFO_OVERFLOW_EN_0 = en_2.individual.FIFO_OVERFLOW_EN_0; + read->FIFO_WM_EN_4 = en_3.individual.FIFO_WM_EN_4; + read->FIFO_WM_EN_3 = en_3.individual.FIFO_WM_EN_3; + read->FIFO_WM_EN_2 = en_3.individual.FIFO_WM_EN_2; + read->FIFO_WM_EN_1 = en_3.individual.FIFO_WM_EN_1; + read->FIFO_WM_EN_0 = en_3.individual.FIFO_WM_EN_0; + } + + return retval; +} + + + + + + + + + +ICM_20948_Status_e ICM_20948_set_sample_mode( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + ICM_20948_LP_CONFIG_t reg; + + if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr | ICM_20948_Internal_Mst ) ) ){ return ICM_20948_Stat_SensorNotSupported; } + + retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank + if( retval != ICM_20948_Stat_Ok){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_LP_CONFIG, (uint8_t*)®, sizeof(ICM_20948_LP_CONFIG_t)); + if( retval != ICM_20948_Stat_Ok){ return retval; } + + if( sensors & ICM_20948_Internal_Acc ){ reg.ACCEL_CYCLE = mode; } // Set all desired sensors to this setting + if( sensors & ICM_20948_Internal_Gyr ){ reg.GYRO_CYCLE = mode; } + if( sensors & ICM_20948_Internal_Mst ){ reg.I2C_MST_CYCLE = mode; } + + retval = ICM_20948_execute_w( pdev, AGB0_REG_LP_CONFIG, (uint8_t*)®, sizeof(ICM_20948_LP_CONFIG_t)); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + return retval; +} + +ICM_20948_Status_e ICM_20948_set_full_scale ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; } + + if( sensors & ICM_20948_Internal_Acc ){ + ICM_20948_ACCEL_CONFIG_t reg; + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t)); + reg.ACCEL_FS_SEL = fss.a; + retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t)); + } + if( sensors & ICM_20948_Internal_Gyr ){ + ICM_20948_GYRO_CONFIG_1_t reg; + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t)); + reg.GYRO_FS_SEL = fss.g; + retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t)); + } + return retval; +} + +ICM_20948_Status_e ICM_20948_set_dlpf_cfg ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; } + + if( sensors & ICM_20948_Internal_Acc ){ + ICM_20948_ACCEL_CONFIG_t reg; + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t)); + reg.ACCEL_DLPFCFG = cfg.a; + retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t)); + } + if( sensors & ICM_20948_Internal_Gyr ){ + ICM_20948_GYRO_CONFIG_1_t reg; + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t)); + reg.GYRO_DLPFCFG = cfg.g; + retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t)); + } + return retval; +} + +ICM_20948_Status_e ICM_20948_enable_dlpf ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, bool enable ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; } + + if( sensors & ICM_20948_Internal_Acc ){ + ICM_20948_ACCEL_CONFIG_t reg; + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t)); + if( enable ){ reg.ACCEL_FCHOICE = 1; } + else{ reg.ACCEL_FCHOICE = 0; } + retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t)); + } + if( sensors & ICM_20948_Internal_Gyr ){ + ICM_20948_GYRO_CONFIG_1_t reg; + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t)); + if( enable ){ reg.GYRO_FCHOICE = 1; } + else{ reg.GYRO_FCHOICE = 0; } + retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t)); + } + return retval; +} + +ICM_20948_Status_e ICM_20948_set_sample_rate ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; } + + if( sensors & ICM_20948_Internal_Acc ){ + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + uint8_t div1 = (smplrt.a << 8); + uint8_t div2 = (smplrt.a & 0xFF); + retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_SMPLRT_DIV_1, &div1, 1); + retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_SMPLRT_DIV_2, &div2, 1); + } + if( sensors & ICM_20948_Internal_Gyr ){ + retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank + uint8_t div = (smplrt.g); + retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_SMPLRT_DIV, &div, 1); + } + return retval; +} + + + + +// Interface Things +ICM_20948_Status_e ICM_20948_i2c_master_passthrough ( ICM_20948_Device_t* pdev, bool passthrough ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + ICM_20948_INT_PIN_CFG_t reg; + retval = ICM_20948_set_bank(pdev, 0); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)®, sizeof(ICM_20948_INT_PIN_CFG_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + reg.BYPASS_EN = passthrough; + retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)®, sizeof(ICM_20948_INT_PIN_CFG_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + return retval; +} + +ICM_20948_Status_e ICM_20948_i2c_master_enable ( ICM_20948_Device_t* pdev, bool enable ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + // Disable BYPASS_EN + retval = ICM_20948_i2c_master_passthrough( pdev, false ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + ICM_20948_I2C_MST_CTRL_t ctrl; + retval = ICM_20948_set_bank(pdev, 3); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + ctrl.I2C_MST_CLK = 0x07; // corresponds to 345.6 kHz, good for up to 400 kHz + ctrl.I2C_MST_P_NSR = 1; + retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + ICM_20948_USER_CTRL_t reg; + retval = ICM_20948_set_bank(pdev, 0); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + retval = ICM_20948_execute_r( pdev, AGB0_REG_USER_CTRL, (uint8_t*)®, sizeof(ICM_20948_USER_CTRL_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + if( enable ){ reg.I2C_MST_EN = 1; } + else{ reg.I2C_MST_EN = 0; } + retval = ICM_20948_execute_w( pdev, AGB0_REG_USER_CTRL, (uint8_t*)®, sizeof(ICM_20948_USER_CTRL_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + return retval; +} + +ICM_20948_Status_e ICM_20948_i2c_master_configure_slave ( ICM_20948_Device_t* pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap ){ + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + + uint8_t slv_addr_reg; + uint8_t slv_reg_reg; + uint8_t slv_ctrl_reg; + + switch( slave ){ + case 0 : slv_addr_reg = AGB3_REG_I2C_SLV0_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV0_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV0_CTRL; break; + case 1 : slv_addr_reg = AGB3_REG_I2C_SLV1_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV1_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV1_CTRL; break; + case 2 : slv_addr_reg = AGB3_REG_I2C_SLV2_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV2_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV2_CTRL; break; + case 3 : slv_addr_reg = AGB3_REG_I2C_SLV3_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV3_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV3_CTRL; break; + default : + return ICM_20948_Stat_ParamErr; + } + + // Set the slave address and the Rw flag + ICM_20948_I2C_SLVX_ADDR_t address; + address.ID = addr; + if( Rw ){ address.RNW = 1; } + retval = ICM_20948_execute_w( pdev, slv_addr_reg, (uint8_t*)&address, sizeof(ICM_20948_I2C_SLVX_ADDR_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + // Set the slave sub-address (reg) + ICM_20948_I2C_SLVX_REG_t subaddress; + subaddress.REG = reg; + retval = ICM_20948_execute_w( pdev, slv_reg_reg, (uint8_t*)&subaddress, sizeof(ICM_20948_I2C_SLVX_REG_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + // Set up the control info + ICM_20948_I2C_SLVX_CTRL_t ctrl; + ctrl.LENG = len; + ctrl.EN = enable; + ctrl.REG_DIS = data_only; + ctrl.GRP = grp; + ctrl.BYTE_SW = swap; + retval = ICM_20948_execute_w( pdev, slv_ctrl_reg, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_SLVX_CTRL_t) ); + if( retval != ICM_20948_Stat_Ok ){ return retval; } + + return retval; +} + + + + + + + + + +// Higher Level +ICM_20948_Status_e ICM_20948_get_agmt ( ICM_20948_Device_t* pdev, ICM_20948_AGMT_t* pagmt ){ + if( pagmt == NULL ){ return ICM_20948_Stat_ParamErr; } + + ICM_20948_Status_e retval = ICM_20948_Stat_Ok; + const uint8_t numbytes = 14; + uint8_t buff[numbytes]; + + // Get readings + retval |= ICM_20948_set_bank( pdev, 0 ); + retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB0_REG_ACCEL_XOUT_H, buff, numbytes ); + + pagmt->acc.axes.x = ((buff[0] << 8) | (buff[1] & 0xFF)); + pagmt->acc.axes.y = ((buff[2] << 8) | (buff[3] & 0xFF)); + pagmt->acc.axes.z = ((buff[4] << 8) | (buff[5] & 0xFF)); + + pagmt->gyr.axes.x = ((buff[6] << 8) | (buff[7] & 0xFF)); + pagmt->gyr.axes.y = ((buff[8] << 8) | (buff[9] & 0xFF)); + pagmt->gyr.axes.z = ((buff[10] << 8) | (buff[11] & 0xFF)); + + pagmt->tmp.val = ((buff[12] << 8) | (buff[13] & 0xFF)); + + // ToDo: get magnetometer readings + // pagmt->mag.axes.x = + // pagmt->mag.axes.y = + // pagmt->mag.axes.z = + + + // Get settings to be able to compute scaled values + retval |= ICM_20948_set_bank( pdev, 2 ); + ICM_20948_ACCEL_CONFIG_t acfg; + retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG, (uint8_t*)&acfg, 1*sizeof(acfg) ); + pagmt->fss.a = acfg.ACCEL_FS_SEL; // Worth noting that without explicitly setting the FS range of the accelerometer it was showing the register value for +/- 2g but the reported values were actually scaled to the +/- 16g range + // Wait a minute... now it seems like this problem actually comes from the digital low-pass filter. When enabled the value is 1/8 what it should be... + retval |= ICM_20948_set_bank( pdev, 2 ); + ICM_20948_GYRO_CONFIG_1_t gcfg1; + retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&gcfg1, 1*sizeof(gcfg1) ); + pagmt->fss.g = gcfg1.GYRO_FS_SEL; + ICM_20948_ACCEL_CONFIG_2_t acfg2; + retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG_2, (uint8_t*)&acfg2, 1*sizeof(acfg2) ); + + return retval; +} diff --git a/esp32/libraries/FreematicsPlus/utility/ICM_20948_C.h b/esp32/libraries/FreematicsPlus/utility/ICM_20948_C.h new file mode 100644 index 0000000..617de33 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ICM_20948_C.h @@ -0,0 +1,237 @@ +/* + +This is a C-compatible interface to the features presented by the ICM 20948 9-axis device +The imementation of the interface is flexible + +*/ + + +#ifndef _ICM_20948_C_H_ +#define _ICM_20948_C_H_ + +#include +#include +#include + +#include "ICM_20948_REGISTERS.h" +#include "ICM_20948_ENUMERATIONS.h" // This is to give users access to usable value definiitons +#include "AK09916_ENUMERATIONS.h" + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +#define ICM_20948_I2C_ADDR_AD0 0x68 // Or 0x69 when AD0 is high +#define ICM_20948_I2C_ADDR_AD1 0x69 // +#define ICM_20948_WHOAMI 0xEA + +#define MAG_AK09916_I2C_ADDR 0x0C +#define MAG_AK09916_WHO_AM_I 0x4809 +#define MAG_REG_WHO_AM_I 0x00 + +typedef enum{ + ICM_20948_Stat_Ok = 0x00, // The only return code that means all is well + ICM_20948_Stat_Err, // A general error + ICM_20948_Stat_NotImpl, // Returned by virtual functions that are not implemented + ICM_20948_Stat_ParamErr, + ICM_20948_Stat_WrongID, + ICM_20948_Stat_InvalSensor, // Tried to apply a function to a sensor that does not support it (e.g. DLPF to the temperature sensor) + ICM_20948_Stat_NoData, + ICM_20948_Stat_SensorNotSupported, + + ICM_20948_Stat_NUM, + ICM_20948_Stat_Unknown, +}ICM_20948_Status_e; + +typedef enum{ + ICM_20948_Internal_Acc = (1 << 0), + ICM_20948_Internal_Gyr = (1 << 1), + ICM_20948_Internal_Mag = (1 << 2), + ICM_20948_Internal_Tmp = (1 << 3), + ICM_20948_Internal_Mst = (1 << 4), // I2C Master Ineternal +}ICM_20948_InternalSensorID_bm; // A bitmask of internal sensor IDs + +typedef union{ + int16_t i16bit[3]; + uint8_t u8bit[6]; +}ICM_20948_axis3bit16_t; + +typedef union{ + int16_t i16bit; + uint8_t u8bit[2]; +}ICM_20948_axis1bit16_t; + +typedef struct{ + uint8_t a : 2; + uint8_t g : 2; + uint8_t reserved_0 : 4; +}ICM_20948_fss_t; // Holds full-scale settings to be able to extract measurements with units + +typedef struct{ + uint8_t a; + uint8_t g; +}ICM_20948_dlpcfg_t; // Holds digital low pass filter settings. Members are type ICM_20948_ACCEL_CONFIG_DLPCFG_e + +typedef struct{ + uint16_t a; + uint8_t g; +}ICM_20948_smplrt_t; + +typedef struct{ + uint8_t I2C_MST_INT_EN : 1; + uint8_t DMP_INT1_EN : 1; + uint8_t PLL_RDY_EN : 1; + uint8_t WOM_INT_EN : 1; + uint8_t REG_WOF_EN : 1; + uint8_t RAW_DATA_0_RDY_EN : 1; + uint8_t FIFO_OVERFLOW_EN_4 : 1; + uint8_t FIFO_OVERFLOW_EN_3 : 1; + uint8_t FIFO_OVERFLOW_EN_2 : 1; + uint8_t FIFO_OVERFLOW_EN_1 : 1; + uint8_t FIFO_OVERFLOW_EN_0 : 1; + uint8_t FIFO_WM_EN_4 : 1; + uint8_t FIFO_WM_EN_3 : 1; + uint8_t FIFO_WM_EN_2 : 1; + uint8_t FIFO_WM_EN_1 : 1; + uint8_t FIFO_WM_EN_0 : 1; +}ICM_20948_INT_enable_t; + +typedef union{ + ICM_20948_axis3bit16_t raw; + struct{ + int16_t x; + int16_t y; + int16_t z; + }axes; +}ICM_20948_axis3named_t; + +typedef struct{ + ICM_20948_axis3named_t acc; + ICM_20948_axis3named_t gyr; + ICM_20948_axis3named_t mag; + union{ + ICM_20948_axis1bit16_t raw; + int16_t val; + }tmp; + ICM_20948_fss_t fss; // Full-scale range settings for this measurement +}ICM_20948_AGMT_t; + +typedef struct{ + ICM_20948_Status_e (*write)( uint8_t regaddr, uint8_t* pdata, uint32_t len, void* user); + ICM_20948_Status_e (*read)( uint8_t regaddr, uint8_t* pdata, uint32_t len, void* user); + // void (*delay)(uint32_t ms); + void* user; +}ICM_20948_Serif_t; // This is the vtable of serial interface functions +extern const ICM_20948_Serif_t NullSerif; // Here is a default for initialization (NULL) + +typedef struct{ + const ICM_20948_Serif_t* _serif; // Pointer to the assigned Serif (Serial Interface) vtable +}ICM_20948_Device_t; // Definition of device struct type + + +// Here's the list of what I want to be able to do: +/* + +perform a generic startup routine that sets most things in the optimal performance range +Read / check against Who Am I +Add magnetometer to auxillary I2C bus and read it's values from the sensor values locations, configure ODR when accelerometer and gyro are both disabled +read raw accel and gyro values +configure accel/gyro update rates and dlpf's +read raw temp values +configure temperature sensor +load DMP firmware into the device +read DMP results from the device +configure interrupts + - configure interrupt and FSYNC pins + - configure which interrupts activate the interrupt pin +respond to interrupts on INT +configure FIFO (and use it) + + + +callbacks for the user to respond to interrupt events + + +*/ + +// ICM_20948_Status_e ICM_20948_Startup( ICM_20948_Device_t* pdev ); // For the time being this performs a standardized startup routine + + +ICM_20948_Status_e ICM_20948_link_serif( ICM_20948_Device_t* pdev, const ICM_20948_Serif_t* s ); // Links a SERIF structure to the device + +// use the device's serif to perform a read or write +ICM_20948_Status_e ICM_20948_execute_r( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ); // Executes a R or W witht he serif vt as long as the pointers are not null +ICM_20948_Status_e ICM_20948_execute_w( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ); + + +// Single-shot I2C on Master IF +ICM_20948_Status_e ICM_20948_i2c_master_slv4_txn( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr ); +ICM_20948_Status_e ICM_20948_i2c_master_single_w( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ); +ICM_20948_Status_e ICM_20948_i2c_master_single_r( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ); + + +// Device Level +ICM_20948_Status_e ICM_20948_set_bank ( ICM_20948_Device_t* pdev, uint8_t bank ); // Sets the bank +ICM_20948_Status_e ICM_20948_sw_reset ( ICM_20948_Device_t* pdev ); // Performs a SW reset +ICM_20948_Status_e ICM_20948_sleep ( ICM_20948_Device_t* pdev, bool on ); // Set sleep mode for the chip +ICM_20948_Status_e ICM_20948_low_power ( ICM_20948_Device_t* pdev, bool on ); // Set low power mode for the chip +ICM_20948_Status_e ICM_20948_set_clock_source ( ICM_20948_Device_t* pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source ); // Choose clock source +ICM_20948_Status_e ICM_20948_get_who_am_i ( ICM_20948_Device_t* pdev, uint8_t* whoami ); // Return whoami in out prarmeter +ICM_20948_Status_e ICM_20948_check_id ( ICM_20948_Device_t* pdev ); // Return 'ICM_20948_Stat_Ok' if whoami matches ICM_20948_WHOAMI +ICM_20948_Status_e ICM_20948_data_ready ( ICM_20948_Device_t* pdev ); // Returns 'Ok' if data is ready + +// Interrupt Configuration +ICM_20948_Status_e ICM_20948_int_pin_cfg ( ICM_20948_Device_t* pdev, ICM_20948_INT_PIN_CFG_t* write, ICM_20948_INT_PIN_CFG_t* read ); // Set the INT pin configuration +ICM_20948_Status_e ICM_20948_int_enable ( ICM_20948_Device_t* pdev, ICM_20948_INT_enable_t* write, ICM_20948_INT_enable_t* read ); // Write and or read the interrupt enable information. If non-null the write operation occurs before the read, so as to verify that the write was successful + +// Internal Sensor Options +ICM_20948_Status_e ICM_20948_set_sample_mode ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode ); // Use to set accel, gyro, and I2C master into cycled or continuous modes +ICM_20948_Status_e ICM_20948_set_full_scale ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss ); +ICM_20948_Status_e ICM_20948_set_dlpf_cfg ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg ); +ICM_20948_Status_e ICM_20948_enable_dlpf ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, bool enable ); +ICM_20948_Status_e ICM_20948_set_sample_rate ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt ); + +// Interface Things +ICM_20948_Status_e ICM_20948_i2c_master_passthrough ( ICM_20948_Device_t* pdev, bool passthrough ); +ICM_20948_Status_e ICM_20948_i2c_master_enable ( ICM_20948_Device_t* pdev, bool enable ); +ICM_20948_Status_e ICM_20948_i2c_master_configure_slave ( ICM_20948_Device_t* pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap ); + + + +// Higher Level +ICM_20948_Status_e ICM_20948_get_agmt ( ICM_20948_Device_t* pdev, ICM_20948_AGMT_t* p ); + + + + + + + +// ToDo: + +/* + Want to access magnetometer throught the I2C master interface... + + // If using the I2C master to read from the magnetometer + // Enable the I2C master to talk to the magnetometer through the ICM 20948 + myICM.i2cMasterEnable( true ); + SERIAL_PORT.print(F("Enabling the I2C master returned ")); SERIAL_PORT.println(myICM.statusString()); + myICM.i2cMasterConfigureSlave ( 0, MAG_AK09916_I2C_ADDR, REG_ST1, 9, true, true, false, false, false ); + SERIAL_PORT.print(F("Configuring the magnetometer slave returned ")); SERIAL_PORT.println(myICM.statusString()); + + // Operate the I2C master in duty-cycled mode + myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ); // options: ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled +*/ + + + + + + + + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* _ICM_20948_C_H_ */ \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/utility/ICM_20948_ENUMERATIONS.h b/esp32/libraries/FreematicsPlus/utility/ICM_20948_ENUMERATIONS.h new file mode 100644 index 0000000..4457bed --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ICM_20948_ENUMERATIONS.h @@ -0,0 +1,263 @@ +/* + +This file contains a useful c translation of the datasheet register map values + +*/ + +#ifndef _ICM_20948_ENUMERATIONS_H_ +#define _ICM_20948_ENUMERATIONS_H_ + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + + + + + + // // Generalized + // REG_BANK_SEL = 0x7F, + + // // Gyroscope and Accelerometer + // // User Bank 0 + // AGB0_REG_WHO_AM_I = 0x00, + // // Break + // AGB0_REG_USER_CTRL = 0x03, + // // Break + // AGB0_REG_LP_CONFIG = 0x05, + +typedef enum{ + ICM_20948_Sample_Mode_Continuous = 0x00, + ICM_20948_Sample_Mode_Cycled, +}ICM_20948_LP_CONFIG_CYCLE_e; + + + + // AGB0_REG_PWR_MGMT_1, + +typedef enum{ + ICM_20948_Clock_Internal_20MHz = 0x00, + ICM_20948_Clock_Auto, + ICM_20948_Clock_TimingReset = 0x07 +}ICM_20948_PWR_MGMT_1_CLKSEL_e; + + // AGB0_REG_PWR_MGMT_2, + // // Break + // AGB0_REG_INT_PIN_CONFIG = 0x0F, + // AGB0_REG_INT_ENABLE, + // AGB0_REG_INT_ENABLE_1, + // AGB0_REG_INT_ENABLE_2, + // AGB0_REG_INT_ENABLE_3, + // // Break + // AGB0_REG_I2C_MST_STATUS = 0x17, + // // Break + // AGB0_REG_INT_STATUS = 0x19, + // AGB0_REG_INT_STATUS_1, + // AGB0_REG_INT_STATUS_2, + // AGB0_REG_INT_STATUS_3, + // // Break + // AGB0_REG_DELAY_TIMEH = 0x28, + // AGB0_REG_DELAY_TIMEL, + // // Break + // AGB0_REG_ACCEL_XOUT_H = 0x2D, + // AGB0_REG_ACCEL_XOUT_L, + // AGB0_REG_ACCEL_YOUT_H, + // AGB0_REG_ACCEL_YOUT_L, + // AGB0_REG_ACCEL_ZOUT_H, + // AGB0_REG_ACCEL_ZOUT_L, + // AGB0_REG_GYRO_XOUT_H, + // AGB0_REG_GYRO_XOUT_L, + // AGB0_REG_GYRO_YOUT_H, + // AGB0_REG_GYRO_YOUT_L, + // AGB0_REG_GYRO_ZOUT_H, + // AGB0_REG_GYRO_ZOUT_L, + // AGB0_REG_TEMP_OUT_H, + // AGB0_REG_TEMP_OUT_L, + // AGB0_REG_EXT_SLV_SENS_DATA_00, + // AGB0_REG_EXT_SLV_SENS_DATA_01, + // AGB0_REG_EXT_SLV_SENS_DATA_02, + // AGB0_REG_EXT_SLV_SENS_DATA_03, + // AGB0_REG_EXT_SLV_SENS_DATA_04, + // AGB0_REG_EXT_SLV_SENS_DATA_05, + // AGB0_REG_EXT_SLV_SENS_DATA_06, + // AGB0_REG_EXT_SLV_SENS_DATA_07, + // AGB0_REG_EXT_SLV_SENS_DATA_08, + // AGB0_REG_EXT_SLV_SENS_DATA_09, + // AGB0_REG_EXT_SLV_SENS_DATA_10, + // AGB0_REG_EXT_SLV_SENS_DATA_11, + // AGB0_REG_EXT_SLV_SENS_DATA_12, + // AGB0_REG_EXT_SLV_SENS_DATA_13, + // AGB0_REG_EXT_SLV_SENS_DATA_14, + // AGB0_REG_EXT_SLV_SENS_DATA_15, + // AGB0_REG_EXT_SLV_SENS_DATA_16, + // AGB0_REG_EXT_SLV_SENS_DATA_17, + // AGB0_REG_EXT_SLV_SENS_DATA_18, + // AGB0_REG_EXT_SLV_SENS_DATA_19, + // AGB0_REG_EXT_SLV_SENS_DATA_20, + // AGB0_REG_EXT_SLV_SENS_DATA_21, + // AGB0_REG_EXT_SLV_SENS_DATA_22, + // AGB0_REG_EXT_SLV_SENS_DATA_23, + // // Break + // AGB0_REG_FIFO_EN_1 = 0x66, + // AGB0_REG_FIFO_EN_2, + // AGB0_REG_FIFO_MODE, + // // Break + // AGB0_REG_FIFO_COUNT_H = 0x70, + // AGB0_REG_FIFO_COUNT_L, + // AGB0_REG_FIFO_R_W, + // // Break + // AGB0_REG_DATA_RDY_STATUS = 0x74, + // // Break + // AGB0_REG_FIFO_CFG = 0x76, + // // Break + // AGB0_REG_MEM_START_ADDR = 0x7C, // Hmm, Invensense thought they were sneaky not listing these locations on the datasheet... + // AGB0_REG_MEM_R_W = 0x7D, // These three locations seem to be able to access some memory within the device + // AGB0_REG_MEM_BANK_SEL = 0x7E, // And that location is also where the DMP image gets loaded + // AGB0_REG_REG_BANK_SEL = 0x7F, + + // // Bank 1 + // AGB1_REG_SELF_TEST_X_GYRO = 0x02, + // AGB1_REG_SELF_TEST_Y_GYRO, + // AGB1_REG_SELF_TEST_Z_GYRO, + // // Break + // AGB1_REG_SELF_TEST_X_ACCEL = 0x0E, + // AGB1_REG_SELF_TEST_Y_ACCEL, + // AGB1_REG_SELF_TEST_Z_ACCEL, + // // Break + // AGB1_REG_XA_OFFS_H = 0x14, + // AGB1_REG_XA_OFFS_L, + // // Break + // AGB1_REG_YA_OFFS_H = 0x17, + // AGB1_REG_YA_OFFS_L, + // // Break + // AGB1_REG_ZA_OFFS_H = 0x1A, + // AGB1_REG_ZA_OFFS_L, + // // Break + // AGB1_REG_TIMEBASE_CORRECTION_PLL = 0x28, + // // Break + // AGB1_REG_REG_BANK_SEL = 0x7F, + + // // Bank 2 + // AGB2_REG_GYRO_SMPLRT_DIV = 0x00, + +/* +Gyro sample rate divider. Divides the internal sample rate to generate the sample +rate that controls sensor data output rate, FIFO sample rate, and DMP sequence rate. +NOTE: This register is only effective when FCHOICE = 1’b1 (FCHOICE_B register bit is 1’b0), and +(0 < DLPF_CFG < 7). +ODR is computed as follows: +1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]) +*/ + + // AGB2_REG_GYRO_CONFIG_1, + +typedef enum{ // Full scale range options in degrees per second + dps250 = 0x00, + dps500, + dps1000, + dps2000, +}ICM_20948_GYRO_CONFIG_1_FS_SEL_e; + +typedef enum{ // Format is dAbwB_nXbwZ - A is integer part of 3db BW, B is fraction. X is integer part of nyquist bandwidth, Y is fraction + gyr_d196bw6_n229bw8 = 0x00, + gyr_d151bw8_n187bw6, + gyr_d119bw5_n154bw3, + gyr_d51bw2_n73bw3, + gyr_d23bw9_n35bw9, + gyr_d11bw6_n17bw8, + gyr_d5bw7_n8bw9, + gyr_d361bw4_n376bw5, +}ICM_20948_GYRO_CONFIG_1_DLPCFG_e; + + // AGB2_REG_GYRO_CONFIG_2, + // AGB2_REG_XG_OFFS_USRH, + // AGB2_REG_XG_OFFS_USRL, + // AGB2_REG_YG_OFFS_USRH, + // AGB2_REG_YG_OFFS_USRL, + // AGB2_REG_ZG_OFFS_USRH, + // AGB2_REG_ZG_OFFS_USRL, + // AGB2_REG_ODR_ALIGN_EN, + // // Break + // AGB2_REG_ACCEL_SMPLRT_DIV_1 = 0x10, + // AGB2_REG_ACCEL_SMPLRT_DIV_2, + // AGB2_REG_ACCEL_INTEL_CTRL, + // AGB2_REG_ACCEL_WOM_THR, + // AGB2_REG_ACCEL_CONFIG, + +typedef enum{ + gpm2 = 0x00, + gpm4, + gpm8, + gpm16, +}ICM_20948_ACCEL_CONFIG_FS_SEL_e; + +typedef enum{ // Format is dAbwB_nXbwZ - A is integer part of 3db BW, B is fraction. X is integer part of nyquist bandwidth, Y is fraction + acc_d246bw_n265bw = 0x00, + acc_d246bw_n265bw_1, + acc_d111bw4_n136bw, + acc_d50bw4_n68bw8, + acc_d23bw9_n34bw4, + acc_d11bw5_n17bw, + acc_d5bw7_n8bw3, + acc_d473bw_n499bw, +}ICM_20948_ACCEL_CONFIG_DLPCFG_e; + + // AGB2_REG_ACCEL_CONFIG_2, + // // Break + // AGB2_REG_FSYNC_CONFIG = 0x52, + // AGB2_REG_TEMP_CONFIG, + // AGB2_REG_MOD_CTRL_USR, + // // Break + // AGB2_REG_REG_BANK_SEL = 0x7F, + + // // Bank 3 + // AGB3_REG_I2C_MST_ODR_CONFIG = 0x00, + // AGB3_REG_I2C_MST_CTRL, + // AGB3_REG_I2C_MST_DELAY_CTRL, + // AGB3_REG_I2C_SLV0_ADDR, + // AGB3_REG_I2C_SLV0_REG, + // AGB3_REG_I2C_SLV0_CTRL, + // AGB3_REG_I2C_SLV0_DO, + // AGB3_REG_I2C_SLV1_ADDR, + // AGB3_REG_I2C_SLV1_REG, + // AGB3_REG_I2C_SLV1_CTRL, + // AGB3_REG_I2C_SLV1_DO, + // AGB3_REG_I2C_SLV2_ADDR, + // AGB3_REG_I2C_SLV2_REG, + // AGB3_REG_I2C_SLV2_CTRL, + // AGB3_REG_I2C_SLV2_DO, + // AGB3_REG_I2C_SLV3_ADDR, + // AGB3_REG_I2C_SLV3_REG, + // AGB3_REG_I2C_SLV3_CTRL, + // AGB3_REG_I2C_SLV3_DO, + // AGB3_REG_I2C_SLV4_ADDR, + // AGB3_REG_I2C_SLV4_REG, + // AGB3_REG_I2C_SLV4_CTRL, + // AGB3_REG_I2C_SLV4_DO, + // AGB3_REG_I2C_SLV4_DI, + // // Break + // AGB3_REG_REG_BANK_SEL = 0x7F, + + // // Magnetometer + // M_REG_WIA2 = 0x01, + // // Break + // M_REG_ST1 = 0x10, + // M_REG_HXL, + // M_REG_HXH, + // M_REG_HYL, + // M_REG_HYH, + // M_REG_HZL, + // M_REG_HZH, + // M_REG_ST2, + // // Break + // M_REG_CNTL2 = 0x31, + // M_REG_CNTL3, + // M_REG_TS1, + // M_REG_TS2, + + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* _ICM_20948_ENUMERATIONS_H_ */ \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/utility/ICM_20948_REGISTERS.h b/esp32/libraries/FreematicsPlus/utility/ICM_20948_REGISTERS.h new file mode 100644 index 0000000..bccdc0b --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ICM_20948_REGISTERS.h @@ -0,0 +1,719 @@ +/* + +This file contains a useful c translation of the datasheet register map + +*/ + +#ifndef _ICM_20948_REGISTERS_H_ +#define _ICM_20948_REGISTERS_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +typedef enum{ + // Generalized + REG_BANK_SEL = 0x7F, + + // Gyroscope and Accelerometer + // User Bank 0 + AGB0_REG_WHO_AM_I = 0x00, + // Break + AGB0_REG_USER_CTRL = 0x03, + // Break + AGB0_REG_LP_CONFIG = 0x05, + AGB0_REG_PWR_MGMT_1, + AGB0_REG_PWR_MGMT_2, + // Break + AGB0_REG_INT_PIN_CONFIG = 0x0F, + AGB0_REG_INT_ENABLE, + AGB0_REG_INT_ENABLE_1, + AGB0_REG_INT_ENABLE_2, + AGB0_REG_INT_ENABLE_3, + // Break + AGB0_REG_I2C_MST_STATUS = 0x17, + // Break + AGB0_REG_INT_STATUS = 0x19, + AGB0_REG_INT_STATUS_1, + AGB0_REG_INT_STATUS_2, + AGB0_REG_INT_STATUS_3, + // Break + AGB0_REG_DELAY_TIMEH = 0x28, + AGB0_REG_DELAY_TIMEL, + // Break + AGB0_REG_ACCEL_XOUT_H = 0x2D, + AGB0_REG_ACCEL_XOUT_L, + AGB0_REG_ACCEL_YOUT_H, + AGB0_REG_ACCEL_YOUT_L, + AGB0_REG_ACCEL_ZOUT_H, + AGB0_REG_ACCEL_ZOUT_L, + AGB0_REG_GYRO_XOUT_H, + AGB0_REG_GYRO_XOUT_L, + AGB0_REG_GYRO_YOUT_H, + AGB0_REG_GYRO_YOUT_L, + AGB0_REG_GYRO_ZOUT_H, + AGB0_REG_GYRO_ZOUT_L, + AGB0_REG_TEMP_OUT_H, + AGB0_REG_TEMP_OUT_L, + AGB0_REG_EXT_SLV_SENS_DATA_00, + AGB0_REG_EXT_SLV_SENS_DATA_01, + AGB0_REG_EXT_SLV_SENS_DATA_02, + AGB0_REG_EXT_SLV_SENS_DATA_03, + AGB0_REG_EXT_SLV_SENS_DATA_04, + AGB0_REG_EXT_SLV_SENS_DATA_05, + AGB0_REG_EXT_SLV_SENS_DATA_06, + AGB0_REG_EXT_SLV_SENS_DATA_07, + AGB0_REG_EXT_SLV_SENS_DATA_08, + AGB0_REG_EXT_SLV_SENS_DATA_09, + AGB0_REG_EXT_SLV_SENS_DATA_10, + AGB0_REG_EXT_SLV_SENS_DATA_11, + AGB0_REG_EXT_SLV_SENS_DATA_12, + AGB0_REG_EXT_SLV_SENS_DATA_13, + AGB0_REG_EXT_SLV_SENS_DATA_14, + AGB0_REG_EXT_SLV_SENS_DATA_15, + AGB0_REG_EXT_SLV_SENS_DATA_16, + AGB0_REG_EXT_SLV_SENS_DATA_17, + AGB0_REG_EXT_SLV_SENS_DATA_18, + AGB0_REG_EXT_SLV_SENS_DATA_19, + AGB0_REG_EXT_SLV_SENS_DATA_20, + AGB0_REG_EXT_SLV_SENS_DATA_21, + AGB0_REG_EXT_SLV_SENS_DATA_22, + AGB0_REG_EXT_SLV_SENS_DATA_23, + // Break + AGB0_REG_FIFO_EN_1 = 0x66, + AGB0_REG_FIFO_EN_2, + AGB0_REG_FIFO_MODE, + // Break + AGB0_REG_FIFO_COUNT_H = 0x70, + AGB0_REG_FIFO_COUNT_L, + AGB0_REG_FIFO_R_W, + // Break + AGB0_REG_DATA_RDY_STATUS = 0x74, + // Break + AGB0_REG_FIFO_CFG = 0x76, + // Break + AGB0_REG_MEM_START_ADDR = 0x7C, // Hmm, Invensense thought they were sneaky not listing these locations on the datasheet... + AGB0_REG_MEM_R_W = 0x7D, // These three locations seem to be able to access some memory within the device + AGB0_REG_MEM_BANK_SEL = 0x7E, // And that location is also where the DMP image gets loaded + AGB0_REG_REG_BANK_SEL = 0x7F, + + // Bank 1 + AGB1_REG_SELF_TEST_X_GYRO = 0x02, + AGB1_REG_SELF_TEST_Y_GYRO, + AGB1_REG_SELF_TEST_Z_GYRO, + // Break + AGB1_REG_SELF_TEST_X_ACCEL = 0x0E, + AGB1_REG_SELF_TEST_Y_ACCEL, + AGB1_REG_SELF_TEST_Z_ACCEL, + // Break + AGB1_REG_XA_OFFS_H = 0x14, + AGB1_REG_XA_OFFS_L, + // Break + AGB1_REG_YA_OFFS_H = 0x17, + AGB1_REG_YA_OFFS_L, + // Break + AGB1_REG_ZA_OFFS_H = 0x1A, + AGB1_REG_ZA_OFFS_L, + // Break + AGB1_REG_TIMEBASE_CORRECTION_PLL = 0x28, + // Break + AGB1_REG_REG_BANK_SEL = 0x7F, + + // Bank 2 + AGB2_REG_GYRO_SMPLRT_DIV = 0x00, + AGB2_REG_GYRO_CONFIG_1, + AGB2_REG_GYRO_CONFIG_2, + AGB2_REG_XG_OFFS_USRH, + AGB2_REG_XG_OFFS_USRL, + AGB2_REG_YG_OFFS_USRH, + AGB2_REG_YG_OFFS_USRL, + AGB2_REG_ZG_OFFS_USRH, + AGB2_REG_ZG_OFFS_USRL, + AGB2_REG_ODR_ALIGN_EN, + // Break + AGB2_REG_ACCEL_SMPLRT_DIV_1 = 0x10, + AGB2_REG_ACCEL_SMPLRT_DIV_2, + AGB2_REG_ACCEL_INTEL_CTRL, + AGB2_REG_ACCEL_WOM_THR, + AGB2_REG_ACCEL_CONFIG, + AGB2_REG_ACCEL_CONFIG_2, + // Break + AGB2_REG_FSYNC_CONFIG = 0x52, + AGB2_REG_TEMP_CONFIG, + AGB2_REG_MOD_CTRL_USR, + // Break + AGB2_REG_REG_BANK_SEL = 0x7F, + + // Bank 3 + AGB3_REG_I2C_MST_ODR_CONFIG = 0x00, + AGB3_REG_I2C_MST_CTRL, + AGB3_REG_I2C_MST_DELAY_CTRL, + AGB3_REG_I2C_SLV0_ADDR, + AGB3_REG_I2C_SLV0_REG, + AGB3_REG_I2C_SLV0_CTRL, + AGB3_REG_I2C_SLV0_DO, + AGB3_REG_I2C_SLV1_ADDR, + AGB3_REG_I2C_SLV1_REG, + AGB3_REG_I2C_SLV1_CTRL, + AGB3_REG_I2C_SLV1_DO, + AGB3_REG_I2C_SLV2_ADDR, + AGB3_REG_I2C_SLV2_REG, + AGB3_REG_I2C_SLV2_CTRL, + AGB3_REG_I2C_SLV2_DO, + AGB3_REG_I2C_SLV3_ADDR, + AGB3_REG_I2C_SLV3_REG, + AGB3_REG_I2C_SLV3_CTRL, + AGB3_REG_I2C_SLV3_DO, + AGB3_REG_I2C_SLV4_ADDR, + AGB3_REG_I2C_SLV4_REG, + AGB3_REG_I2C_SLV4_CTRL, + AGB3_REG_I2C_SLV4_DO, + AGB3_REG_I2C_SLV4_DI, + // Break + AGB3_REG_REG_BANK_SEL = 0x7F, + + // Magnetometer + M_REG_WIA2 = 0x01, + // Break + M_REG_ST1 = 0x10, + M_REG_HXL, + M_REG_HXH, + M_REG_HYL, + M_REG_HYH, + M_REG_HZL, + M_REG_HZH, + M_REG_ST2, + // Break + M_REG_CNTL2 = 0x31, + M_REG_CNTL3, + M_REG_TS1, + M_REG_TS2, +}ICM_20948_Reg_Addr_e; // These enums are not needed for the user, so they stay in this scope (simplifies naming among other things) + + + +// Type definitions for the registers +typedef struct{ + uint8_t WHO_AM_I; +}ICM_20948_WHO_AM_I_t; + +typedef struct{ + uint8_t reserved_0 : 1; + uint8_t I2C_MST_RST : 1; + uint8_t SRAM_RST : 1; + uint8_t DMP_RST : 1; + uint8_t I2C_IF_DIS : 1; + uint8_t I2C_MST_EN : 1; + uint8_t FIFO_EN : 1; + uint8_t DMP_EN : 1; +}ICM_20948_USER_CTRL_t; + +typedef struct{ + uint8_t reserved_0 : 4; + uint8_t GYRO_CYCLE : 1; + uint8_t ACCEL_CYCLE : 1; + uint8_t I2C_MST_CYCLE : 1; + uint8_t reserved_1 : 1; +}ICM_20948_LP_CONFIG_t; + +typedef struct{ + uint8_t CLKSEL : 3; + uint8_t TEMP_DIS : 1; + uint8_t reserved_0 : 1; + uint8_t LP_EN : 1; + uint8_t SLEEP : 1; + uint8_t DEVICE_RESET : 1; +}ICM_20948_PWR_MGMT_1_t; + +typedef struct{ + uint8_t DISABLE_GYRO : 3; + uint8_t DIABLE_ACCEL : 3; + uint8_t reserved_0 : 2; +}ICM_20948_PWR_MGMT_2_t; + +typedef struct{ + uint8_t reserved_0 : 1; + uint8_t BYPASS_EN : 1; + uint8_t FSYNC_INT_MODE_EN : 1; + uint8_t ACTL_FSYNC : 1; + uint8_t INT_ANYRD_2CLEAR : 1; + uint8_t INT1_LATCH_EN : 1; + uint8_t INT1_OPEN : 1; + uint8_t INT1_ACTL : 1; +}ICM_20948_INT_PIN_CFG_t; + +typedef struct{ + uint8_t I2C_MST_INT_EN : 1; + uint8_t DMP_INT1_EN : 1; + uint8_t PLL_READY_EN : 1; + uint8_t WOM_INT_EN : 1; + uint8_t reserved_0 : 3; + uint8_t REG_WOF_EN : 1; +}ICM_20948_INT_ENABLE_t; + +typedef struct{ + uint8_t RAW_DATA_0_RDY_EN : 1; + uint8_t reserved_0 : 7; +}ICM_20948_INT_ENABLE_1_t; + +typedef union{ + struct{ + uint8_t FIFO_OVERFLOW_EN_40 : 5; + uint8_t reserved_0 : 3; + }grouped; + struct{ + uint8_t FIFO_OVERFLOW_EN_0 : 1; + uint8_t FIFO_OVERFLOW_EN_1 : 1; + uint8_t FIFO_OVERFLOW_EN_2 : 1; + uint8_t FIFO_OVERFLOW_EN_3 : 1; + uint8_t FIFO_OVERFLOW_EN_4 : 1; + uint8_t reserved_0 : 3; + }individual; +}ICM_20948_INT_ENABLE_2_t; + +// typedef struct{ +// uint8_t FIFO_OVERFLOW_EN_40 : 5; +// uint8_t reserved_0 : 3; +// }ICM_20948_INT_ENABLE_2_t; + +typedef union{ + struct{ + uint8_t FIFO_WM_EN_40 : 5; + uint8_t reserved_0 : 3; + }grouped; + struct{ + uint8_t FIFO_WM_EN_0 : 1; + uint8_t FIFO_WM_EN_1 : 1; + uint8_t FIFO_WM_EN_2 : 1; + uint8_t FIFO_WM_EN_3 : 1; + uint8_t FIFO_WM_EN_4 : 1; + uint8_t reserved_0 : 3; + }individual; +}ICM_20948_INT_ENABLE_3_t; + +// typedef struct{ +// uint8_t FIFO_WM_EN_40 : 5; +// uint8_t reserved_0 : 3; +// }ICM_20948_INT_ENABLE_3_t; + +typedef struct{ + uint8_t I2C_SLV0_NACK : 1; + uint8_t I2C_SLV1_NACK : 1; + uint8_t I2C_SLV2_NACK : 1; + uint8_t I2C_SLV3_NACK : 1; + uint8_t I2C_SLV4_NACK : 1; + uint8_t I2C_LOST_ARB : 1; + uint8_t I2C_SLV4_DONE : 1; + uint8_t PASS_THROUGH : 1; +}ICM_20948_I2C_MST_STATUS_t; + +typedef struct{ + uint8_t I2C_MST_INT : 1; + uint8_t DMP_INT1 : 1; + uint8_t PLL_RDY_INT : 1; + uint8_t WOM_INT : 1; + uint8_t reserved_0 : 4; +}ICM_20948_INT_STATUS_t; + +typedef struct{ + uint8_t RAW_DATA_0_RDY_INT : 1; + uint8_t reserved_0 : 7; +}ICM_20948_INT_STATUS_1_t; + +// typedef union{ +// struct{ +// uint8_t FIFO_OVERFLOW_INT_40 : 5; +// uint8_t reserved_0 : 3; +// }grouped; +// struct{ +// uint8_t FIFO_OVERFLOW_INT_0 : 1; +// uint8_t FIFO_OVERFLOW_INT_1 : 1; +// uint8_t FIFO_OVERFLOW_INT_2 : 1; +// uint8_t FIFO_OVERFLOW_INT_3 : 1; +// uint8_t FIFO_OVERFLOW_INT_4 : 1; +// uint8_t reserved_0 : 3; +// }individual; +// }ICM_20948_INT_STATUS_2_t; + +typedef struct{ + uint8_t FIFO_OVERFLOW_INT_40 : 5; + uint8_t reserved_0 : 3; +}ICM_20948_INT_STATUS_2_t; + +// typedef union{ +// struct{ +// uint8_t FIFO_WM_INT_40 : 5; +// uint8_t reserved_0 : 3; +// }grouped; +// struct{ +// uint8_t FIFO_WM_INT_0 : 1; +// uint8_t FIFO_WM_INT_1 : 1; +// uint8_t FIFO_WM_INT_2 : 1; +// uint8_t FIFO_WM_INT_3 : 1; +// uint8_t FIFO_WM_INT_4 : 1; +// uint8_t reserved_0 : 3; +// }individual; +// }ICM_20948_INT_STATUS_3_t; + +typedef struct{ + uint8_t FIFO_WM_INT40 : 5; + uint8_t reserved_0 : 3; +}ICM_20948_INT_STATUS_3_t; + +typedef struct{ + uint8_t DELAY_TIMEH; +}ICM_20948_DELAY_TIMEH_t; + +typedef struct{ + uint8_t DELAY_TIMEL; +}ICM_20948_DELAY_TIMEL_t; + +typedef struct{ + uint8_t ACCEL_XOUT_H; +}ICM_20948_ACCEL_XOUT_H_t; + +typedef struct{ + uint8_t ACCEL_XOUT_L; +}ICM_20948_ACCEL_XOUT_L_t; + +typedef struct{ + uint8_t ACCEL_YOUT_H; +}ICM_20948_ACCEL_YOUT_H_t; + +typedef struct{ + uint8_t ACCEL_YOUT_L; +}ICM_20948_ACCEL_YOUT_L_t; + +typedef struct{ + uint8_t ACCEL_ZOUT_H; +}ICM_20948_ACCEL_ZOUT_H_t; + +typedef struct{ + uint8_t ACCEL_ZOUT_L; +}ICM_20948_ACCEL_ZOUT_L_t; + +typedef struct{ + uint8_t GYRO_XOUT_H; +}ICM_20948_GYRO_XOUT_H_t; + +typedef struct{ + uint8_t GYRO_XOUT_L; +}ICM_20948_GYRO_XOUT_L_t; + +typedef struct{ + uint8_t GYRO_YOUT_H; +}ICM_20948_GYRO_YOUT_H_t; + +typedef struct{ + uint8_t GYRO_YOUT_L; +}ICM_20948_GYRO_YOUT_L_t; + +typedef struct{ + uint8_t GYRO_ZOUT_H; +}ICM_20948_GYRO_ZOUT_H_t; + +typedef struct{ + uint8_t GYRO_ZOUT_L; +}ICM_20948_GYRO_ZOUT_L_t; + +typedef struct{ + uint8_t TEMP_OUT_H; +}ICM_20948_TEMP_OUT_H_t; + +typedef struct{ + uint8_t TEMP_OUT_L; +}ICM_20948_TEMP_OUT_L_t; + +typedef struct{ + uint8_t DATA; // Note: this is not worth copying 24 times, despite there being 24 registers like this one +}ICM_20948_EXT_SLV_SENS_DATA_t; + +typedef struct{ + uint8_t SLV_0_FIFO_EN : 1; + uint8_t SLV_1_FIFO_EN : 1; + uint8_t SLV_2_FIFO_EN : 1; + uint8_t SLV_3_FIFO_EN : 1; + uint8_t reserved_0 : 4; +}ICM_20948_FIFO_EN_1_t; + +typedef struct{ + uint8_t TEMP_FIFO_EN : 1; + uint8_t GYRO_X_FIFO_EN : 1; + uint8_t GYRO_Y_FIFO_EN : 1; + uint8_t GYRO_Z_FIFO_EN : 1; + uint8_t ACCEL_FIFO_EN : 1; + uint8_t reserved_0 : 3; +}ICM_20948_FIFO_EN_2_t; + +typedef struct{ + uint8_t FIFO_RESET : 5; + uint8_t reserved_0 : 3; +}ICM_20948_FIFO_RST_t; + +typedef struct{ + uint8_t FIFO_MODE : 5; + uint8_t reserved_0 : 3; +}ICM_20948_FIFO_MODE_t; + +typedef struct{ + uint8_t FIFO_COUNTH; +}ICM_20948_FIFO_COUNTH_t; + +typedef struct{ + uint8_t FIFO_COUNTL; +}ICM_20948_FIFO_COUNTL_t; + +typedef struct{ + uint8_t FIFO_R_W; // Reading from or writing to this register actually reads from or writes to the FIFO +}ICM_20948_FIFO_R_W_t; + +typedef struct{ + uint8_t RAW_DATA_RDY : 4; + uint8_t reserved_0 : 3; + uint8_t WOF_STATUS : 1; +}ICM_20948_DATA_RDY_STATUS_t; + +typedef struct{ + uint8_t FIFO_CFG : 1; + uint8_t reserved_0 : 7; +}ICM_20948_FIFO_CFG_t; + + + +// User bank 1 Types + +typedef struct{ + uint8_t XG_ST_DATA; +}ICM_20948_SELF_TEST_X_GYRO_t; + +typedef struct{ + uint8_t YG_ST_DATA; +}ICM_20948_SELF_TEST_Y_GYRO_t; + +typedef struct{ + uint8_t ZG_ST_DATA; +}ICM_20948_SELF_TEST_Z_GYRO_t; + +typedef struct{ + uint8_t XA_ST_DATA; +}ICM_20948_SELF_TEST_X_ACCEL_t; + +typedef struct{ + uint8_t YA_ST_DATA; +}ICM_20948_SELF_TEST_Y_ACCEL_t; + +typedef struct{ + uint8_t ZA_ST_DATA; +}ICM_20948_SELF_TEST_Z_ACCEL_t; + +typedef struct{ + uint8_t XA_OFFS_14_7; +}ICM_20948_XA_OFFS_H_t; + +typedef struct{ + uint8_t reserved_0 : 1; + uint8_t XA_OFFS_6_0 : 7; +}ICM_20948_XA_OFFS_L_t; + +typedef struct{ + uint8_t YA_OFFS_14_7; +}ICM_20948_YA_OFFS_H_t; + +typedef struct{ + uint8_t reserved_0 : 1; + uint8_t YA_OFFS_6_0 : 7; +}ICM_20948_YA_OFFS_L_t; + +typedef struct{ + uint8_t ZA_OFFS_14_7; +}ICM_20948_ZA_OFFS_H_t; + +typedef struct{ + uint8_t reserved_0 : 1; + uint8_t ZA_OFFS_6_0 : 7; +}ICM_20948_ZA_OFFS_L_t; + +typedef struct{ + uint8_t TBC_PLL; +}ICM_20948_TIMEBASE_CORRECTION_PLL_t; + + + +// User Bank 2 Types +typedef struct{ + uint8_t GYRO_SMPLRT_DIV; +}ICM_20948_GYRO_SMPLRT_DIV_t; + +typedef struct{ + uint8_t GYRO_FCHOICE : 1; + uint8_t GYRO_FS_SEL : 2; + uint8_t GYRO_DLPFCFG : 3; + uint8_t reserved_0 : 2; +}ICM_20948_GYRO_CONFIG_1_t; + +typedef struct{ + uint8_t GYRO_AVGCFG : 3; + uint8_t ZGYRO_CTEN : 1; + uint8_t YGYRO_CTEN : 1; + uint8_t XGYRO_CTEN : 1; + uint8_t reserved_0 : 2; +}ICM_20948_GYRO_CONFIG_2_t; + +typedef struct{ + uint8_t XG_OFFS_USER_H; +}ICM_20948_XG_OFFS_USRH_t; + +typedef struct{ + uint8_t XG_OFFS_USER_L; +}ICM_20948_XG_OFFS_USRL_t; + +typedef struct{ + uint8_t YG_OFFS_USER_H; +}ICM_20948_YG_OFFS_USRH_t; + +typedef struct{ + uint8_t YG_OFFS_USER_L; +}ICM_20948_YG_OFFS_USRL_t; + +typedef struct{ + uint8_t ZG_OFFS_USER_H; +}ICM_20948_ZG_OFFS_USRH_t; + +typedef struct{ + uint8_t ZG_OFFS_USER_L; +}ICM_20948_ZG_OFFS_USRL_t; + +typedef struct{ + uint8_t ODR_ALIGN_EN : 1; + uint8_t reserved_0 : 7; +}ICM_20948_ODR_ALIGN_EN_t; + +typedef struct{ + uint8_t ACCEL_SMPLRT_DIV_11_8 : 4; + uint8_t reserved_0 : 4; +}ICM_20948_ACCEL_SMPLRT_DIV_1_t; + +typedef struct{ + uint8_t ACCEL_SMPLRT_DIV_7_0; +}ICM_20948_ACCEL_SMPLRT_DIV_2_t; + +typedef struct{ + uint8_t ACCEL_INTEL_MODE_INT : 1; + uint8_t ACCEL_INTEL_EN : 1; + uint8_t reserved_0 : 6; +}ICM_20948_ACCEL_INTEL_CTRL_t; + +typedef struct{ + uint8_t WOM_THRESHOLD; +}ICM_20948_ACCEL_WOM_THR_t; + +typedef struct{ + uint8_t ACCEL_FCHOICE : 1; + uint8_t ACCEL_FS_SEL : 2; + uint8_t ACCEL_DLPFCFG : 3; + uint8_t reserved_0 : 2; +}ICM_20948_ACCEL_CONFIG_t; + +typedef struct{ + uint8_t DEC3_CFG : 2; + uint8_t AZ_ST_EN : 1; + uint8_t AY_ST_EN : 1; + uint8_t AX_ST_EN : 1; + uint8_t reserved_0 : 3; +}ICM_20948_ACCEL_CONFIG_2_t; + +typedef struct{ + uint8_t EXT_SYNC_SET : 4; + uint8_t WOF_EDGE_INT : 1; + uint8_t WOF_DEGLITCH_EN : 1; + uint8_t reserved_0 : 1; + uint8_t DELAY_TIME_EN : 1; +}ICM_20948_FSYNC_CONFIG_t; + +typedef struct{ + uint8_t TEMP_DLPFCFG : 3; + uint8_t reserved_0 : 5; +}ICM_20948_TEMP_CONFIG_t; + +typedef struct{ + uint8_t REG_LP_DMP_EN : 1; + uint8_t reserved_0 : 7; +}ICM_20948_MOD_CTRL_USR_t; + + + +// Bank 3 Types + +typedef struct{ + uint8_t I2C_MST_ODR_CONFIG : 4; + uint8_t reserved_0 : 4; +}ICM_20948_I2C_MST_ODR_CONFIG_t; + +typedef struct{ + uint8_t I2C_MST_CLK : 4; + uint8_t I2C_MST_P_NSR : 1; + uint8_t reserved_0 : 2; + uint8_t MULT_MST_EN : 1; +}ICM_20948_I2C_MST_CTRL_t; + +typedef struct{ + uint8_t I2C_SLV0_DELAY_EN : 1; + uint8_t I2C_SLV1_DELAY_EN : 1; + uint8_t I2C_SLV2_DELAY_EN : 1; + uint8_t I2C_SLV3_DELAY_EN : 1; + uint8_t I2C_SLV4_DELAY_EN : 1; + uint8_t reserved_0 : 2; + uint8_t DELAY_ES_SHADOW : 1; +}ICM_20948_I2C_MST_DELAY_CTRL_t; + +typedef struct{ + uint8_t ID : 7; + uint8_t RNW : 1; +}ICM_20948_I2C_SLVX_ADDR_t; + +typedef struct{ + uint8_t REG; +}ICM_20948_I2C_SLVX_REG_t; + +typedef struct{ + uint8_t LENG : 4; + uint8_t GRP : 1; + uint8_t REG_DIS : 1; + uint8_t BYTE_SW : 1; + uint8_t EN : 1; +}ICM_20948_I2C_SLVX_CTRL_t; + +typedef struct{ + uint8_t DO; +}ICM_20948_I2C_SLVX_DO_t; + +typedef struct{ + uint8_t DLY : 5; + uint8_t REG_DIS : 1; + uint8_t INT_EN : 1; + uint8_t EN : 1; +}ICM_20948_I2C_SLV4_CTRL_t; + +typedef struct{ + uint8_t DI; +}ICM_20948_I2C_SLV4_DI_t; + + +// Bank select register! + +typedef struct{ + uint8_t reserved_0 : 4; + uint8_t USER_BANK : 2; + uint8_t reserved_1 : 2; +}ICM_20948_REG_BANK_SEL_t; + + + + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* _ICM_20948_REGISTERS_H_ */ \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/utility/ICM_42627.h b/esp32/libraries/FreematicsPlus/utility/ICM_42627.h new file mode 100644 index 0000000..c95d6ec --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ICM_42627.h @@ -0,0 +1,66 @@ +#ifndef _ICM_42627_H +#define _ICM_42627_H + + +#define ICM_42627_DeviceID 0x20 + +// PWR_MGMT0 +#define GYRO_MODE_OFF 0<<2 +#define GYRO_MODE_LN 3<<2 +#define GYRO_MODE_Standby 1<<2 + +#define ACCEL_MODE_OFF 1<<0 +#define ACCEL_MODE_LP 2<<0 //LOW POWER +#define ACCEL_MODE_LN 3<<0 //LOW Noise + +#define IDLE_OFF 1<<4 +#define IDLE_ON 0<<4 + +#define TEMP_DIS_ON 0<<5 +#define TEMP_DIS_OFF 1<<5 + +//GYRO_CONFIG0 +#define GYRO_ODR_1KHZ 6<<0 +#define GYRO_FS_SEL_250dps 3<<5 + +//CONFIG0_REG +#define ACCEL_ODR_1KHZ 6<<0 +#define ACCEL_FS_SEL_2G 3<<5 + +//INT_STATUS +#define UI_FSYNC_INT 1<<6 +#define PLL_RDY_INT 1<<5 +#define RESET_DONE_INT 1<<4 +#define DATA_RDY_INT 1<<3 +#define FIFO_THS_INT 1<<2 +#define FIFO_FULL_INT 1<<1 +#define AGC_RDY_INT 1<<0 + +#define ICM_42627_ADDRESS 0x68 //I2C Address +#define WHO_AM_I_ICM42627 0x75 // Should return 0x20 + +//ICM_42627 REG +#define PWR_MGMT0_REG 0x4E +#define GYRO_CONFIG0_REG 0x4F +#define ACCEL_CONFIG0_REG 0x50 +#define SELF_TEST_CONFIG_REG 0x70 +#define INT_STATUS_REG 0x2D + +#define TEMP_OUT_H_REG 0x1D +#define TEMP_OUT_L_REG 0x1E + +#define ACCEL_XOUT_H_REG 0x1F +#define ACCEL_XOUT_L_REG 0x20 +#define ACCEL_YOUT_H_REG 0x21 +#define ACCEL_YOUT_L_REG 0x22 +#define ACCEL_ZOUT_H_REG 0x23 +#define ACCEL_ZOUT_L_REG 0x24 + +#define GYRO_XOUT_H_REG 0x25 +#define GYRO_XOUT_L_REG 0x26 +#define GYRO_YOUT_H_REG 0x27 +#define GYRO_YOUT_L_REG 0x28 +#define GYRO_ZOUT_H_REG 0x29 +#define GYRO_ZOUT_L_REG 0x30 + +#endif \ No newline at end of file diff --git a/esp32/libraries/FreematicsPlus/utility/OBD.h b/esp32/libraries/FreematicsPlus/utility/OBD.h new file mode 100644 index 0000000..0500d6f --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/OBD.h @@ -0,0 +1,77 @@ +// Mode 1 PIDs +#define PID_ENGINE_LOAD 0x04 +#define PID_COOLANT_TEMP 0x05 +#define PID_SHORT_TERM_FUEL_TRIM_1 0x06 +#define PID_LONG_TERM_FUEL_TRIM_1 0x07 +#define PID_SHORT_TERM_FUEL_TRIM_2 0x08 +#define PID_LONG_TERM_FUEL_TRIM_2 0x09 +#define PID_FUEL_PRESSURE 0x0A +#define PID_INTAKE_MAP 0x0B +#define PID_RPM 0x0C +#define PID_SPEED 0x0D +#define PID_TIMING_ADVANCE 0x0E +#define PID_INTAKE_TEMP 0x0F +#define PID_MAF_FLOW 0x10 +#define PID_THROTTLE 0x11 +#define PID_AUX_INPUT 0x1E +#define PID_RUNTIME 0x1F +#define PID_DISTANCE_WITH_MIL 0x21 +#define PID_COMMANDED_EGR 0x2C +#define PID_EGR_ERROR 0x2D +#define PID_COMMANDED_EVAPORATIVE_PURGE 0x2E +#define PID_FUEL_LEVEL 0x2F +#define PID_WARMS_UPS 0x30 +#define PID_DISTANCE 0x31 +#define PID_EVAP_SYS_VAPOR_PRESSURE 0x32 +#define PID_BAROMETRIC 0x33 +#define PID_CATALYST_TEMP_B1S1 0x3C +#define PID_CATALYST_TEMP_B2S1 0x3D +#define PID_CATALYST_TEMP_B1S2 0x3E +#define PID_CATALYST_TEMP_B2S2 0x3F +#define PID_CONTROL_MODULE_VOLTAGE 0x42 +#define PID_ABSOLUTE_ENGINE_LOAD 0x43 +#define PID_AIR_FUEL_EQUIV_RATIO 0x44 +#define PID_RELATIVE_THROTTLE_POS 0x45 +#define PID_AMBIENT_TEMP 0x46 +#define PID_ABSOLUTE_THROTTLE_POS_B 0x47 +#define PID_ABSOLUTE_THROTTLE_POS_C 0x48 +#define PID_ACC_PEDAL_POS_D 0x49 +#define PID_ACC_PEDAL_POS_E 0x4A +#define PID_ACC_PEDAL_POS_F 0x4B +#define PID_COMMANDED_THROTTLE_ACTUATOR 0x4C +#define PID_TIME_WITH_MIL 0x4D +#define PID_TIME_SINCE_CODES_CLEARED 0x4E +#define PID_ETHANOL_FUEL 0x52 +#define PID_FUEL_RAIL_PRESSURE 0x59 +#define PID_HYBRID_BATTERY_PERCENTAGE 0x5B +#define PID_ENGINE_OIL_TEMP 0x5C +#define PID_FUEL_INJECTION_TIMING 0x5D +#define PID_ENGINE_FUEL_RATE 0x5E +#define PID_ENGINE_TORQUE_DEMANDED 0x61 +#define PID_ENGINE_TORQUE_PERCENTAGE 0x62 +#define PID_ENGINE_REF_TORQUE 0x63 +#define PID_ODOMETER 0xA6 + +typedef enum { + PROTO_AUTO = 0x0, + PROTO_ISO_9141_2 = 0x3, + PROTO_KWP2000_5KBPS = 0x4, + PROTO_KWP2000_FAST = 0x5, + PROTO_ISO15765_11B_500K = 0x6, + PROTO_ISO15765_29B_500K = 0x7, + PROTO_ISO15765_29B_250K = 0x8, + PROTO_ISO15765_11B_250K = 0x9, + PROTO_J1939 = 0xB, + PROTO_ISO11898_11B_500K = 0xC, + PROTO_ISO11898_29B_500K = 0xD, + PROTO_ISO11898_11B_250K = 0xE, + PROTO_ISO11898_29B_250K = 0xF +} OBD_PROTOCOLS; + +// states +typedef enum { + OBD_DISCONNECTED = 0, + OBD_CONNECTING = 1, + OBD_CONNECTED = 2, + OBD_FAILED = 3 +} OBD_STATES; diff --git a/esp32/libraries/FreematicsPlus/utility/ble_spp_server.c b/esp32/libraries/FreematicsPlus/utility/ble_spp_server.c new file mode 100644 index 0000000..5f890a6 --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ble_spp_server.c @@ -0,0 +1,441 @@ +/* + This example code is in the Public Domain (or CC0 licensed, at your option.) + + Unless required by applicable law or agreed to in writing, this + software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR + CONDITIONS OF ANY KIND, either express or implied. +*/ + + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/event_groups.h" +#include "esp_system.h" +#include "esp_log.h" +#include "nvs_flash.h" +#include "esp_bt.h" +#include "string.h" +#include "esp_gap_ble_api.h" +#include "esp_gatts_api.h" +#include "esp_bt_defs.h" +#include "esp_bt_main.h" +#include "driver/uart.h" +#include "ble_spp_server.h" +#include "esp32-hal-bt.h" + +#define GATTS_TABLE_TAG "GATTS_SPP_SERVER" + +#define SPP_PROFILE_NUM 1 +#define SPP_PROFILE_APP_IDX 0 +#define ESP_SPP_APP_ID 0x56 +#define SAMPLE_DEVICE_NAME "FreematicsPlus" //The Device Name Characteristics in GAP +#define SPP_SVC_INST_ID 0 + +/// SPP Service +static const uint16_t spp_service_uuid = 0xABF0; +/// Characteristic UUID +#define ESP_GATT_UUID_SPP_COMMAND_RECEIVE 0xFFE1 +#define ESP_GATT_UUID_SPP_COMMAND_NOTIFY 0xFFE2 +#define ESP_GATT_UUID_SPP_DATA_RECEIVE 0xFFE3 +#define ESP_GATT_UUID_SPP_DATA_NOTIFY 0xFFE4 + +#ifdef SUPPORT_HEARTBEAT +#define ESP_GATT_UUID_SPP_HEARTBEAT 0xABF5 +#endif + +static uint8_t spp_adv_data[23] = { + /* Flags */ + 0x02,0x01,0x06, + /* Complete List of 16-bit Service Class UUIDs */ + 0x03,0x03,0xF0,0xAB, + /* Complete Local Name in advertising */ + 0x0F,0x09, 'F', 'r', 'e', 'e', 'm', 'a', 't', 'i', 'c', 's', 'P', 'l', 'u', 's' +}; + +static uint16_t spp_mtu_size = 23; +static uint16_t spp_conn_id = 0xffff; +static esp_gatt_if_t spp_gatts_if = 0xff; +static xQueueHandle cmd_cmd_queue = NULL; + +static bool enable_data_ntf = false; +static bool is_connected = false; +static esp_bd_addr_t spp_remote_bda = {0x0,}; + +static uint16_t spp_handle_table[SPP_IDX_NB]; + +static esp_ble_adv_params_t spp_adv_params = { + .adv_int_min = 0x20, + .adv_int_max = 0x40, + .adv_type = ADV_TYPE_IND, + .own_addr_type = BLE_ADDR_TYPE_PUBLIC, + .channel_map = ADV_CHNL_ALL, + .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY, +}; + +struct gatts_profile_inst { + esp_gatts_cb_t gatts_cb; + uint16_t gatts_if; + uint16_t app_id; + uint16_t conn_id; + uint16_t service_handle; + esp_gatt_srvc_id_t service_id; + uint16_t char_handle; + esp_bt_uuid_t char_uuid; + esp_gatt_perm_t perm; + esp_gatt_char_prop_t property; + uint16_t descr_handle; + esp_bt_uuid_t descr_uuid; +}; + +static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param); + +/* One gatt-based profile one app_id and one gatts_if, this array will store the gatts_if returned by ESP_GATTS_REG_EVT */ +static struct gatts_profile_inst spp_profile_tab[SPP_PROFILE_NUM] = { + [SPP_PROFILE_APP_IDX] = { + .gatts_cb = gatts_profile_event_handler, + .gatts_if = ESP_GATT_IF_NONE, /* Not get the gatt_if, so initial is ESP_GATT_IF_NONE */ + }, +}; + +/* + * SPP PROFILE ATTRIBUTES + **************************************************************************************** + */ + +#define CHAR_DECLARATION_SIZE (sizeof(uint8_t)) +static const uint16_t primary_service_uuid = ESP_GATT_UUID_PRI_SERVICE; +static const uint16_t character_declaration_uuid = ESP_GATT_UUID_CHAR_DECLARE; +static const uint16_t character_client_config_uuid = ESP_GATT_UUID_CHAR_CLIENT_CONFIG; + +static const uint8_t char_prop_read_notify = ESP_GATT_CHAR_PROP_BIT_READ|ESP_GATT_CHAR_PROP_BIT_NOTIFY; +static const uint8_t char_prop_read_write = ESP_GATT_CHAR_PROP_BIT_WRITE_NR|ESP_GATT_CHAR_PROP_BIT_WRITE; + +///SPP Service - data receive characteristic, read&write without response +//static const uint16_t spp_data_receive_uuid = ESP_GATT_UUID_SPP_DATA_RECEIVE; +//static const uint8_t spp_data_receive_val[20] = {0x00}; + +///SPP Service - data notify characteristic, notify&read +//static const uint16_t spp_data_notify_uuid = ESP_GATT_UUID_SPP_DATA_NOTIFY; +//static const uint8_t spp_data_notify_val[20] = {0x00}; +//static const uint8_t spp_data_notify_ccc[2] = {0x00, 0x00}; + +///SPP Service - command characteristic, read&write without response +static const uint16_t spp_command_uuid = ESP_GATT_UUID_SPP_COMMAND_RECEIVE; +static const uint8_t spp_command_val[10] = {0x00}; + +///SPP Service - status characteristic, notify&read +static const uint16_t spp_status_uuid = ESP_GATT_UUID_SPP_COMMAND_NOTIFY; +static const uint8_t spp_status_val[10] = {0x00}; +static const uint8_t spp_status_ccc[2] = {0x00, 0x00}; + +///Full HRS Database Description - Used to add attributes into the database +static const esp_gatts_attr_db_t spp_gatt_db[SPP_IDX_NB] = +{ + //SPP - Service Declaration + [SPP_IDX_SVC] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&primary_service_uuid, ESP_GATT_PERM_READ, + sizeof(spp_service_uuid), sizeof(spp_service_uuid), (uint8_t *)&spp_service_uuid}}, + +#if 0 + //SPP - data receive characteristic Declaration + [SPP_IDX_SPP_DATA_RECV_CHAR] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, + CHAR_DECLARATION_SIZE,CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write}}, + + //SPP - data receive characteristic Value + [SPP_IDX_SPP_DATA_RECV_VAL] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_data_receive_uuid, ESP_GATT_PERM_READ|ESP_GATT_PERM_WRITE, + SPP_DATA_MAX_LEN,sizeof(spp_data_receive_val), (uint8_t *)spp_data_receive_val}}, + + //SPP - data notify characteristic Declaration + [SPP_IDX_SPP_DATA_NOTIFY_CHAR] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, + CHAR_DECLARATION_SIZE,CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_notify}}, + + //SPP - data notify characteristic Value + [SPP_IDX_SPP_DATA_NTY_VAL] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_data_notify_uuid, ESP_GATT_PERM_READ, + SPP_DATA_MAX_LEN, sizeof(spp_data_notify_val), (uint8_t *)spp_data_notify_val}}, + + //SPP - data notify characteristic - Client Characteristic Configuration Descriptor + [SPP_IDX_SPP_DATA_NTF_CFG] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ|ESP_GATT_PERM_WRITE, + sizeof(uint16_t),sizeof(spp_data_notify_ccc), (uint8_t *)spp_data_notify_ccc}}, +#endif + + //SPP - command characteristic Declaration + [SPP_IDX_SPP_COMMAND_CHAR] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, + CHAR_DECLARATION_SIZE,CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write}}, + + //SPP - command characteristic Value + [SPP_IDX_SPP_COMMAND_VAL] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_command_uuid, ESP_GATT_PERM_READ|ESP_GATT_PERM_WRITE, + SPP_CMD_MAX_LEN,sizeof(spp_command_val), (uint8_t *)spp_command_val}}, + + //SPP - status characteristic Declaration + [SPP_IDX_SPP_STATUS_CHAR] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, + CHAR_DECLARATION_SIZE,CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_notify}}, + + //SPP - status characteristic Value + [SPP_IDX_SPP_STATUS_VAL] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_status_uuid, ESP_GATT_PERM_READ, + SPP_STATUS_MAX_LEN,sizeof(spp_status_val), (uint8_t *)spp_status_val}}, + + //SPP - status characteristic - Client Characteristic Configuration Descriptor + [SPP_IDX_SPP_STATUS_CFG] = + {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ|ESP_GATT_PERM_WRITE, + sizeof(uint16_t),sizeof(spp_status_ccc), (uint8_t *)spp_status_ccc}}, +}; + +static uint8_t find_char_and_desr_index(uint16_t handle) +{ + uint8_t error = 0xff; + + for(int i = 0; i < SPP_IDX_NB ; i++){ + if(handle == spp_handle_table[i]){ + return i; + } + } + + return error; +} + +void ble_send(int spp_index, void* data, int len) +{ + // spp_index: SPP_IDX_SPP_STATUS_VAL, SPP_IDX_SPP_DATA_NTY_VAL + esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[spp_index], len, (uint8_t*)data, false); +} + +char* ble_recv_command(int timeout) +{ + char *cmd = 0; + if(cmd_cmd_queue && xQueueReceive(cmd_cmd_queue, &cmd, timeout / portTICK_PERIOD_MS)) { + return cmd; + } + return 0; +} + +void ble_send_response(void* data, int len, char* ptr_to_free) +{ + if (ptr_to_free) free(ptr_to_free); + if (len > 0) { + esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_STATUS_VAL], len, (uint8_t*)data, false); + } +} + +static void gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param) +{ + esp_err_t err; + ESP_LOGE(GATTS_TABLE_TAG, "GAP_EVT, event %d\n", event); + + switch (event) { + case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT: + esp_ble_gap_start_advertising(&spp_adv_params); + break; + case ESP_GAP_BLE_ADV_START_COMPLETE_EVT: + //advertising start complete event to indicate advertising start successfully or failed + if((err = param->adv_start_cmpl.status) != ESP_BT_STATUS_SUCCESS) { + ESP_LOGE(GATTS_TABLE_TAG, "Advertising start failed: %s\n", esp_err_to_name(err)); + } + break; + default: + break; + } +} + +static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param) +{ + esp_ble_gatts_cb_param_t *p_data = (esp_ble_gatts_cb_param_t *) param; + uint8_t res = 0xff; + + ESP_LOGI(GATTS_TABLE_TAG, "event = %x\n",event); + switch (event) { + case ESP_GATTS_REG_EVT: + ESP_LOGI(GATTS_TABLE_TAG, "%s %d\n", __func__, __LINE__); + esp_ble_gap_set_device_name(SAMPLE_DEVICE_NAME); + + ESP_LOGI(GATTS_TABLE_TAG, "%s %d\n", __func__, __LINE__); + esp_ble_gap_config_adv_data_raw((uint8_t *)spp_adv_data, sizeof(spp_adv_data)); + + ESP_LOGI(GATTS_TABLE_TAG, "%s %d\n", __func__, __LINE__); + esp_ble_gatts_create_attr_tab(spp_gatt_db, gatts_if, SPP_IDX_NB, SPP_SVC_INST_ID); + break; + case ESP_GATTS_READ_EVT: + res = find_char_and_desr_index(p_data->read.handle); + if(res == SPP_IDX_SPP_COMMAND_VAL){ + //TODO:client read the status characteristic + esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_COMMAND_VAL], 2, (uint8_t*)"OK", false); + } + break; + case ESP_GATTS_WRITE_EVT: { + res = find_char_and_desr_index(p_data->write.handle); + if(p_data->write.is_prep == false){ + ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_WRITE_EVT : handle = %d\n", res); + if(res == SPP_IDX_SPP_COMMAND_VAL){ + uint8_t * spp_cmd_buff = (uint8_t *)malloc(p_data->write.len + 1); + if(spp_cmd_buff == NULL){ + ESP_LOGE(GATTS_TABLE_TAG, "%s malloc failed\n", __func__); + break; + } + memcpy(spp_cmd_buff,p_data->write.value,p_data->write.len); + spp_cmd_buff[p_data->write.len] = 0; + if (xQueueSend(cmd_cmd_queue,&spp_cmd_buff,0) == errQUEUE_FULL) { + char *temp; + xQueueReceive(cmd_cmd_queue, &temp, 0); + xQueueSend(cmd_cmd_queue,&spp_cmd_buff,0); + } + }else if(res == SPP_IDX_SPP_DATA_NTF_CFG){ + if((p_data->write.len == 2)&&(p_data->write.value[0] == 0x01)&&(p_data->write.value[1] == 0x00)){ + enable_data_ntf = true; + }else if((p_data->write.len == 2)&&(p_data->write.value[0] == 0x00)&&(p_data->write.value[1] == 0x00)){ + enable_data_ntf = false; + } + } + else if(res == SPP_IDX_SPP_DATA_RECV_VAL){ + int len = p_data->write.len; + esp_log_buffer_char(GATTS_TABLE_TAG,(char *)(p_data->write.value),len); + //process_cmd(p_data->write.value, len); + }else{ + //TODO: + } + }else if((p_data->write.is_prep == true)&&(res == SPP_IDX_SPP_DATA_RECV_VAL)){ + ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_PREP_WRITE_EVT : handle = %d\n", res); + } + break; + } + case ESP_GATTS_EXEC_WRITE_EVT:{ + ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_EXEC_WRITE_EVT\n"); + if(p_data->exec_write.exec_write_flag){ + } + break; + } + case ESP_GATTS_MTU_EVT: + spp_mtu_size = p_data->mtu.mtu; + break; + case ESP_GATTS_CONF_EVT: + break; + case ESP_GATTS_UNREG_EVT: + break; + case ESP_GATTS_DELETE_EVT: + break; + case ESP_GATTS_START_EVT: + break; + case ESP_GATTS_STOP_EVT: + break; + case ESP_GATTS_CONNECT_EVT: + spp_conn_id = p_data->connect.conn_id; + spp_gatts_if = gatts_if; + is_connected = true; + memcpy(&spp_remote_bda,&p_data->connect.remote_bda,sizeof(esp_bd_addr_t)); + break; + case ESP_GATTS_DISCONNECT_EVT: + is_connected = false; + enable_data_ntf = false; + esp_ble_gap_start_advertising(&spp_adv_params); + break; + case ESP_GATTS_OPEN_EVT: + break; + case ESP_GATTS_CANCEL_OPEN_EVT: + break; + case ESP_GATTS_CLOSE_EVT: + break; + case ESP_GATTS_LISTEN_EVT: + break; + case ESP_GATTS_CONGEST_EVT: + break; + case ESP_GATTS_CREAT_ATTR_TAB_EVT:{ + ESP_LOGI(GATTS_TABLE_TAG, "The number handle =%x\n",param->add_attr_tab.num_handle); + if (param->add_attr_tab.status != ESP_GATT_OK){ + ESP_LOGE(GATTS_TABLE_TAG, "Create attribute table failed, error code=0x%x", param->add_attr_tab.status); + } + else if (param->add_attr_tab.num_handle != SPP_IDX_NB){ + ESP_LOGE(GATTS_TABLE_TAG, "Create attribute table abnormally, num_handle (%d) doesn't equal to HRS_IDX_NB(%d)", param->add_attr_tab.num_handle, SPP_IDX_NB); + } + else { + memcpy(spp_handle_table, param->add_attr_tab.handles, sizeof(spp_handle_table)); + esp_ble_gatts_start_service(spp_handle_table[SPP_IDX_SVC]); + } + break; + } + default: + break; + } +} + + +static void gatts_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param) +{ + ESP_LOGI(GATTS_TABLE_TAG, "EVT %d, gatts if %d\n", event, gatts_if); + + /* If event is register event, store the gatts_if for each profile */ + if (event == ESP_GATTS_REG_EVT) { + if (param->reg.status == ESP_GATT_OK) { + spp_profile_tab[SPP_PROFILE_APP_IDX].gatts_if = gatts_if; + } else { + ESP_LOGI(GATTS_TABLE_TAG, "Reg app failed, app_id %04x, status %d\n",param->reg.app_id, param->reg.status); + return; + } + } + + do { + int idx; + for (idx = 0; idx < SPP_PROFILE_NUM; idx++) { + if (gatts_if == ESP_GATT_IF_NONE || /* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */ + gatts_if == spp_profile_tab[idx].gatts_if) { + if (spp_profile_tab[idx].gatts_cb) { + spp_profile_tab[idx].gatts_cb(event, gatts_if, param); + } + } + } + } while (0); +} + +void ble_init(const char* adv_name) +{ + esp_err_t ret; + //esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT(); + + if (adv_name) strncpy((char*)spp_adv_data + 9, adv_name, 13); + + if(!btStarted() && !btStart()){ + ESP_LOGE(GATTS_TABLE_TAG, "%s BT init failed\n", __func__); + return; + } + +/* + esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT); + + ret = esp_bt_controller_init(&bt_cfg); + if (ret) { + ESP_LOGE(GATTS_TABLE_TAG, "%s init controller failed: %s\n", __func__, esp_err_to_name(ret)); + return; + } + + ret = esp_bt_controller_enable(ESP_BT_MODE_BLE); + if (ret) { + ESP_LOGE(GATTS_TABLE_TAG, "%s enable controller failed: %s\n", __func__, esp_err_to_name(ret)); + return; + } +*/ + + ESP_LOGI(GATTS_TABLE_TAG, "%s init bluetooth\n", __func__); + ret = esp_bluedroid_init(); + if (ret) { + ESP_LOGE(GATTS_TABLE_TAG, "%s init bluetooth failed: %s\n", __func__, esp_err_to_name(ret)); + return; + } + ret = esp_bluedroid_enable(); + if (ret) { + ESP_LOGE(GATTS_TABLE_TAG, "%s enable bluetooth failed: %s\n", __func__, esp_err_to_name(ret)); + return; + } + + esp_ble_gatts_register_callback(gatts_event_handler); + esp_ble_gap_register_callback(gap_event_handler); + esp_ble_gatts_app_register(ESP_SPP_APP_ID); + + cmd_cmd_queue = xQueueCreate(4, sizeof(void*)); + return; +} diff --git a/esp32/libraries/FreematicsPlus/utility/ble_spp_server.h b/esp32/libraries/FreematicsPlus/utility/ble_spp_server.h new file mode 100644 index 0000000..53bf05f --- /dev/null +++ b/esp32/libraries/FreematicsPlus/utility/ble_spp_server.h @@ -0,0 +1,52 @@ +/* + This example code is in the Public Domain (or CC0 licensed, at your option.) + + Unless required by applicable law or agreed to in writing, this + software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR + CONDITIONS OF ANY KIND, either express or implied. +*/ + + +/* + * DEFINES + **************************************************************************************** + */ +//#define SUPPORT_HEARTBEAT +#define SPP_DEBUG_MODE + +#define spp_sprintf(s,...) sprintf((char*)(s), ##__VA_ARGS__) +#define SPP_DATA_MAX_LEN (512) +#define SPP_CMD_MAX_LEN (20) +#define SPP_STATUS_MAX_LEN (20) +#define SPP_DATA_BUFF_MAX_LEN (2*1024) +///Attributes State Machine +enum{ + SPP_IDX_SVC, + + SPP_IDX_SPP_DATA_RECV_CHAR, + SPP_IDX_SPP_DATA_RECV_VAL, + + SPP_IDX_SPP_DATA_NOTIFY_CHAR, + SPP_IDX_SPP_DATA_NTY_VAL, + SPP_IDX_SPP_DATA_NTF_CFG, + + SPP_IDX_SPP_COMMAND_CHAR, + SPP_IDX_SPP_COMMAND_VAL, + + SPP_IDX_SPP_STATUS_CHAR, + SPP_IDX_SPP_STATUS_VAL, + SPP_IDX_SPP_STATUS_CFG, + +#ifdef SUPPORT_HEARTBEAT + SPP_IDX_SPP_HEARTBEAT_CHAR, + SPP_IDX_SPP_HEARTBEAT_VAL, + SPP_IDX_SPP_HEARTBEAT_CFG, +#endif + + SPP_IDX_NB, +}; + +void ble_init(const char* adv_name); +void ble_send(int spp_index, void* data, int len); +char* ble_recv_command(int timeout); +void ble_send_response(void* data, int len, char* ptr_to_free); diff --git a/esp32/libraries/TinyGPS/TinyGPS.cpp b/esp32/libraries/TinyGPS/TinyGPS.cpp new file mode 100644 index 0000000..a0cbd07 --- /dev/null +++ b/esp32/libraries/TinyGPS/TinyGPS.cpp @@ -0,0 +1,456 @@ +/* +TinyGPS - a small GPS library for Arduino providing basic NMEA parsing +Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers. +Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson. +Copyright (C) 2008-2012 Mikal Hart +All rights reserved. + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#include "TinyGPS.h" + +#define _GPRMC_TERM "GPRMC" +#define _GPGGA_TERM "GPGGA" + +TinyGPS::TinyGPS() + : _time(GPS_INVALID_TIME) + , _date(GPS_INVALID_DATE) + , _latitude(GPS_INVALID_ANGLE) + , _longitude(GPS_INVALID_ANGLE) + , _altitude(GPS_INVALID_ALTITUDE) + , _speed(GPS_INVALID_SPEED) + , _course(GPS_INVALID_ANGLE) + , _hdop(GPS_INVALID_HDOP) + , _numsats(GPS_INVALID_SATELLITES) + , _last_time_fix(GPS_INVALID_FIX_TIME) + , _last_position_fix(GPS_INVALID_FIX_TIME) + , _parity(0) + , _is_checksum_term(false) + , _sentence_type(_GPS_SENTENCE_OTHER) + , _term_number(0) + , _term_offset(0) + , _gps_data_good(false) +#ifndef _GPS_NO_STATS + , _good_sentences(0) + , _failed_checksum(0) +#endif +{ + _term[0] = '\0'; +} + +// +// public methods +// + +bool TinyGPS::encode(char c) +{ + bool valid_sentence = false; + + switch(c) + { + case ',': // term terminators + _parity ^= ','; + case '\r': + case '\n': + case '*': + if (_term_offset < sizeof(_term)) + { + _term[_term_offset] = 0; + valid_sentence = term_complete(); + } + ++_term_number; + _term_offset = 0; + _is_checksum_term = c == '*'; + return valid_sentence; + + case '$': // sentence begin + _term_number = _term_offset = 0; + _parity = 0; + _sentence_type = _GPS_SENTENCE_OTHER; + _is_checksum_term = false; + _gps_data_good = false; + return valid_sentence; + } + + // ordinary characters + if (_term_offset < sizeof(_term) - 1) + _term[_term_offset++] = c; + if (!_is_checksum_term) + _parity ^= (byte)c; + + return valid_sentence; +} + +#ifndef _GPS_NO_STATS +void TinyGPS::stats(unsigned short *sentences, unsigned short *failed_cs) +{ + if (sentences) *sentences = _good_sentences; + if (failed_cs) *failed_cs = _failed_checksum; +} +#endif + +// +// internal utilities +// +byte TinyGPS::from_hex(char a) +{ + if (a >= 'A' && a <= 'F') + return a - 'A' + 10; + else if (a >= 'a' && a <= 'f') + return a - 'a' + 10; + else + return a - '0'; +} + +byte TinyGPS::hex2uint8(const char *p) +{ + byte c1 = *p; + byte c2 = *(p + 1); + if (c1 >= 'A' && c1 <= 'F') + c1 -= 7; + else if (c1 >= 'a' && c1 <= 'f') + c1 -= 39; + else if (c1 < '0' || c1 > '9') + return 0; + + if (c2 == 0) + return (c1 & 0xf); + else if (c2 >= 'A' && c2 <= 'F') + c2 -= 7; + else if (c2 >= 'a' && c2 <= 'f') + c2 -= 39; + else if (c2 < '0' || c2 > '9') + return 0; + + return c1 << 4 | (c2 & 0xf); +} + +unsigned long TinyGPS::parse_decimal() +{ + char *p = _term; + bool isneg = *p == '-'; + if (isneg) ++p; + unsigned long ret = 100UL * gpsatol(p); + while (gpsisdigit(*p)) ++p; + if (*p == '.') + { + if (gpsisdigit(p[1])) + { + ret += 10 * (p[1] - '0'); + if (gpsisdigit(p[2])) + ret += p[2] - '0'; + } + } + return isneg ? -ret : ret; +} + +unsigned long TinyGPS::parse_degrees() +{ + char *p; + unsigned long left = gpsatol(_term); + unsigned long tenk_minutes = (left % 100UL) * 100000UL; + for (p=_term; gpsisdigit(*p); ++p); + if (*p == '.') + { + unsigned long mult = 10000; + while (gpsisdigit(*++p)) + { + tenk_minutes += mult * (*p - '0'); + mult /= 10; + } + } + return (left / 100) * 1000000 + tenk_minutes / 6; +} + +#define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number) + +// Processes a just-completed term +// Returns true if new sentence has just passed checksum test and is validated +bool TinyGPS::term_complete() +{ + if (_is_checksum_term) + { + byte checksum = hex2uint8(_term); + if (checksum == _parity) + { +#ifndef _GPS_NO_STATS + ++_good_sentences; +#endif + if (_gps_data_good) + { + _last_time_fix = _new_time_fix; + _last_position_fix = _new_position_fix; + + switch(_sentence_type) + { + case _GPS_SENTENCE_GPRMC: + _time = _new_time; + _date = _new_date; + _latitude = _new_latitude; + _longitude = _new_longitude; + _speed = _new_speed; + _course = _new_course; + break; + case _GPS_SENTENCE_GPGGA: + _altitude = _new_altitude; + _time = _new_time; + _latitude = _new_latitude; + _longitude = _new_longitude; + _numsats = _new_numsats; + _hdop = _new_hdop; + break; + } + + return true; + } + } + +#ifndef _GPS_NO_STATS + else + ++_failed_checksum; +#endif + return false; + } + + // the first term determines the sentence type + if (_term_number == 0) + { + if (!gpsstrcmp(_term, _GPRMC_TERM)) + _sentence_type = _GPS_SENTENCE_GPRMC; + else if (!gpsstrcmp(_term, _GPGGA_TERM)) + _sentence_type = _GPS_SENTENCE_GPGGA; + else + _sentence_type = _GPS_SENTENCE_OTHER; + return false; + } + + if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) + switch(COMBINE(_sentence_type, _term_number)) + { + case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences + case COMBINE(_GPS_SENTENCE_GPGGA, 1): + _new_time = parse_decimal(); + _new_time_fix = millis(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity + _gps_data_good = _term[0] == 'A'; + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude + case COMBINE(_GPS_SENTENCE_GPGGA, 2): + _new_latitude = parse_degrees(); + _new_position_fix = millis(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S + case COMBINE(_GPS_SENTENCE_GPGGA, 3): + if (_term[0] == 'S') + _new_latitude = -_new_latitude; + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude + case COMBINE(_GPS_SENTENCE_GPGGA, 4): + _new_longitude = parse_degrees(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W + case COMBINE(_GPS_SENTENCE_GPGGA, 5): + if (_term[0] == 'W') + _new_longitude = -_new_longitude; + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC) + _new_speed = parse_decimal(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC) + _new_course = parse_decimal(); + break; + case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC) + _new_date = gpsatol(_term); + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA) + _gps_data_good = _term[0] > '0'; + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA) + _new_numsats = (unsigned char)atoi(_term); + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP + _new_hdop = parse_decimal(); + break; + case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA) + _new_altitude = parse_decimal(); + break; + } + + return false; +} + +long TinyGPS::gpsatol(const char *str) +{ + long ret = 0; + while (gpsisdigit(*str)) + ret = 10 * ret + *str++ - '0'; + return ret; +} + +int TinyGPS::gpsstrcmp(const char *str1, const char *str2) +{ + if (*str1 == *str2) { + str1 += 2; + str2 += 2; + while (*str1 && *str1 == *str2) + ++str1, ++str2; + } + return *str1; +} + +/* static */ +float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) +{ + // returns distance in meters between two positions, both specified + // as signed decimal-degrees latitude and longitude. Uses great-circle + // distance computation for hypothetical sphere of radius 6372795 meters. + // Because Earth is no exact sphere, rounding errors may be up to 0.5%. + // Courtesy of Maarten Lamers + float delta = radians(long1-long2); + float sdlong = sin(delta); + float cdlong = cos(delta); + lat1 = radians(lat1); + lat2 = radians(lat2); + float slat1 = sin(lat1); + float clat1 = cos(lat1); + float slat2 = sin(lat2); + float clat2 = cos(lat2); + delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); + delta = sq(delta); + delta += sq(clat2 * sdlong); + delta = sqrt(delta); + float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); + delta = atan2(delta, denom); + return delta * 6372795; +} + +float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) +{ + // returns course in degrees (North=0, West=270) from position 1 to position 2, + // both specified as signed decimal-degrees latitude and longitude. + // Because Earth is no exact sphere, calculated course may be off by a tiny fraction. + // Courtesy of Maarten Lamers + float dlon = radians(long2-long1); + lat1 = radians(lat1); + lat2 = radians(lat2); + float a1 = sin(dlon) * cos(lat2); + float a2 = sin(lat1) * cos(lat2) * cos(dlon); + a2 = cos(lat1) * sin(lat2) - a2; + a2 = atan2(a1, a2); + if (a2 < 0.0) + { + a2 += TWO_PI; + } + return degrees(a2); +} + +const char *TinyGPS::cardinal (float course) +{ + static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"}; + + int direction = (int)((course + 11.25f) / 22.5f); + return directions[direction % 16]; +} + +// lat/long in hundred thousandths of a degree and age of fix in milliseconds +void TinyGPS::get_position(long *latitude, long *longitude, unsigned long *fix_age) +{ + if (latitude) *latitude = _latitude; + if (longitude) *longitude = _longitude; + if (fix_age) { + if (_last_position_fix == GPS_INVALID_FIX_TIME) + *fix_age = GPS_INVALID_AGE; + else + *fix_age = millis() - _last_position_fix; + } +} + +// date as ddmmyy, time as hhmmsscc, and age in milliseconds +void TinyGPS::get_datetime(unsigned long *date, unsigned long *time, unsigned long *age) +{ + if (date) *date = _date; + if (time) *time = _time; + if (age) { + if (_last_time_fix == GPS_INVALID_FIX_TIME) + *age = GPS_INVALID_AGE; + else + *age = millis() - _last_time_fix; + } +} + +void TinyGPS::f_get_position(float *latitude, float *longitude, unsigned long *fix_age) +{ + long lat, lon; + get_position(&lat, &lon, fix_age); + *latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 100000.0); + *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 100000.0); +} + +void TinyGPS::crack_datetime(int *year, byte *month, byte *day, + byte *hour, byte *minute, byte *second, byte *hundredths, unsigned long *age) +{ + unsigned long date, time; + get_datetime(&date, &time, age); + if (year) + { + *year = date % 100; + *year += *year > 80 ? 1900 : 2000; + } + if (month) *month = (date / 100) % 100; + if (day) *day = date / 10000; + if (hour) *hour = time / 1000000; + if (minute) *minute = (time / 10000) % 100; + if (second) *second = (time / 100) % 100; + if (hundredths) *hundredths = time % 100; +} + +float TinyGPS::f_altitude() +{ + return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0; +} + +float TinyGPS::f_course() +{ + return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0; +} + +float TinyGPS::f_speed_knots() +{ + return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0; +} + +float TinyGPS::f_speed_mph() +{ + float sk = f_speed_knots(); + return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * f_speed_knots(); +} + +float TinyGPS::f_speed_mps() +{ + float sk = f_speed_knots(); + return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * f_speed_knots(); +} + +float TinyGPS::f_speed_kmph() +{ + float sk = f_speed_knots(); + return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * f_speed_knots(); +} + +const float TinyGPS::GPS_INVALID_F_ANGLE = 1000.0; +const float TinyGPS::GPS_INVALID_F_ALTITUDE = 1000000.0; +const float TinyGPS::GPS_INVALID_F_SPEED = -1.0; diff --git a/esp32/libraries/TinyGPS/TinyGPS.h b/esp32/libraries/TinyGPS/TinyGPS.h new file mode 100644 index 0000000..834567a --- /dev/null +++ b/esp32/libraries/TinyGPS/TinyGPS.h @@ -0,0 +1,148 @@ +/* +TinyGPS - a small GPS library for Arduino providing basic NMEA parsing +Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers. +Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson. +Copyright (C) 2008-2012 Mikal Hart +All rights reserved. + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef TinyGPS_h +#define TinyGPS_h + +#include "Arduino.h" + +#define _GPS_VERSION 12 // software version of this library +#define _GPS_MPH_PER_KNOT 1.15077945 +#define _GPS_MPS_PER_KNOT 0.51444444 +#define _GPS_KMPH_PER_KNOT 1.852 +#define _GPS_MILES_PER_METER 0.00062137112 +#define _GPS_KM_PER_METER 0.001 + +class TinyGPS +{ +public: + enum { + GPS_INVALID_AGE = 0xFFFFFFFF, GPS_INVALID_ANGLE = 999999999, + GPS_INVALID_ALTITUDE = 999999999, GPS_INVALID_DATE = 0, + GPS_INVALID_TIME = 0xFFFFFFFF, GPS_INVALID_SPEED = 999999999, + GPS_INVALID_FIX_TIME = 0xFFFFFFFF, GPS_INVALID_SATELLITES = 0xFF, + GPS_INVALID_HDOP = 0xFFFFFFFF + }; + + static const float GPS_INVALID_F_ANGLE, GPS_INVALID_F_ALTITUDE, GPS_INVALID_F_SPEED; + + TinyGPS(); + bool encode(char c); // process one character received from GPS + TinyGPS &operator << (char c) {encode(c); return *this;} + + // lat/long in hundred thousandths of a degree and age of fix in milliseconds + void get_position(long *latitude, long *longitude, unsigned long *fix_age = 0); + + // date as ddmmyy, time as hhmmsscc, and age in milliseconds + void get_datetime(unsigned long *date, unsigned long *time, unsigned long *age = 0); + + // signed altitude in centimeters (from GPGGA sentence) + inline long altitude() { return _altitude; } + + // course in last full GPRMC sentence in 100th of a degree + inline unsigned long course() { return _course; } + + // speed in last full GPRMC sentence in 100ths of a knot + inline unsigned long speed() { return _speed; } + + // satellites used in last full GPGGA sentence + inline unsigned short satellites() { return _numsats; } + + // horizontal dilution of precision in 100ths + inline unsigned long hdop() { return _hdop; } + + void f_get_position(float *latitude, float *longitude, unsigned long *fix_age = 0); + void crack_datetime(int *year, byte *month, byte *day, + byte *hour, byte *minute, byte *second, byte *hundredths = 0, unsigned long *fix_age = 0); + float f_altitude(); + float f_course(); + float f_speed_knots(); + float f_speed_mph(); + float f_speed_mps(); + float f_speed_kmph(); + + static int library_version() { return _GPS_VERSION; } + + static float distance_between (float lat1, float long1, float lat2, float long2); + static float course_to (float lat1, float long1, float lat2, float long2); + static const char *cardinal(float course); + +#ifndef _GPS_NO_STATS + void stats(unsigned short *good_sentences, unsigned short *failed_cs); +#endif + +private: + enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER}; + + // properties + unsigned long _time, _new_time; + unsigned long _date, _new_date; + long _latitude, _new_latitude; + long _longitude, _new_longitude; + long _altitude, _new_altitude; + unsigned long _speed, _new_speed; + unsigned long _course, _new_course; + unsigned long _hdop, _new_hdop; + unsigned short _numsats, _new_numsats; + + unsigned long _last_time_fix, _new_time_fix; + unsigned long _last_position_fix, _new_position_fix; + + // parsing state variables + byte _parity; + bool _is_checksum_term; + char _term[15]; + byte _sentence_type; + byte _term_number; + byte _term_offset; + bool _gps_data_good; + + // statistics +#ifndef _GPS_NO_STATS + // statistics + unsigned short _good_sentences; + unsigned short _failed_checksum; +#endif + + // internal utilities + byte from_hex(char a); + byte hex2uint8(const char* p); + unsigned long parse_decimal(); + unsigned long parse_degrees(); + bool term_complete(); + bool gpsisdigit(char c) { return c >= '0' && c <= '9'; } + long gpsatol(const char *str); + int gpsstrcmp(const char *str1, const char *str2); +}; + +#if !defined(ARDUINO) +// Arduino 0012 workaround +#undef int +#undef char +#undef long +#undef byte +#undef float +#undef abs +#undef round +#endif + +#endif diff --git a/esp32/libraries/httpd/httpd.c b/esp32/libraries/httpd/httpd.c new file mode 100644 index 0000000..ad200e1 --- /dev/null +++ b/esp32/libraries/httpd/httpd.c @@ -0,0 +1,1784 @@ +/****************************************************************************** +* MiniWeb - a mini and high efficiency HTTP server implementation +* Developed by Stanley Huang +* Based on the original MiniWeb developed and hosted at +* https://sourceforge.net/projects/miniweb/ +* Distributed under BSD license +* +* 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 +#include +#include +#include +#include +#include +#include +#include "httpd.h" +#include "httpint.h" + +//////////////////////////////////////////////////////////////////////////// +// global variables +//////////////////////////////////////////////////////////////////////////// +const char* status200[] = { + "OK", /* 200 */ + "Created", /* 201 */ + "Accepted", /* 202 */ + "Non-Authoritative Information", /* 203 */ + "No Content", /* 204 */ + "Reset Content", /* 205 */ + "Partial Content", /* 206 */ +}; + +const char* status300[] = { + "Multiple Choices", /* 300 */ + "Moved Permanently", /* 301 */ + "Found", /* 302 */ + "See Other", /* 303 */ + "Not Modified", /* 304 */ + "Use Proxy", /* 305 */ + "", /* 306 */ + "Temporary Redirect", /* 307 */ +}; + +const char* status400[] = { + "Bad Request", /* 400 */ + "Unauthorized", /* 401 */ + "", /* 402 */ + "Forbidden", /* 403 */ + "Not Found", /* 404 */ + "Method Not Allowed", /* 405 */ + "Not Acceptable", /* 406 */ + "Proxy Authentication Required", /* 407 */ + "Request Timeout", /* 408 */ + "Conflict", /* 409 */ + "Gone", /* 410 */ + "Length Required", /* 411 */ + "Precondition Failed", /* 412 */ + "Request Entity Too Large", /* 413 */ + "Request-URI Too Long", /* 414 */ +}; + +const char* status500[] = { + "Internal Server Error", /* 500 */ + "Not Implemented", /* 501 */ + "Bad Gateway", /* 502 */ + "Service Unavailable", /* 503 */ + "Gateway Timeout", /* 504 */ +}; + +const char* contentTypeTable[]={ + "application/octet-stream", + "text/html", + "text/xml", + "text/plain", + "application/vnd.mozilla.xul+xml", + "text/css", + "application/x-javascript", + "image/png", + "image/jpeg", + "image/gif", + "application/x-shockwave-flash", + "audio/mpeg", + "video/mpeg", + "video/avi", + "video/mp4", + "video/quicktime", + "video/x-mpeg-avc", + "video/flv", + "video/MP2T", + "video/3gpp", + "video/x-ms-asf", + "application/octet-stream", + "application/x-datastream", + "application/x-mpegURL", + "application/sdp", + "application/binhex", + "application/json", +}; + +const char* defaultPages[]={"index.html"}; + +//////////////////////////////////////////////////////////////////////////// +// API callsc +//////////////////////////////////////////////////////////////////////////// + +const char *dayNames="Sun\0Mon\0Tue\0Wed\0Thu\0Fri\0Sat"; +const char *monthNames="Jan\0Feb\0Mar\0Apr\0May\0Jun\0Jul\0Aug\0Sep\0Oct\0Nov\0Dec"; +const char *httpDateTimeFormat="%s, %02d %s %d %02d:%02d:%02d GMT"; + +char* mwGetVarValue(HttpVariables* vars, const char *varname, const char *defval) +{ + if (vars && varname) { + int i; + for (i=0; (vars+i)->name; i++) { + if (!strcmp((vars+i)->name,varname)) { + return (vars+i)->value; + } + } + } + return (char*)defval; +} + +int mwGetVarValueInt(HttpVariables* vars, const char *varname, int defval) +{ + if (vars && varname) { + int i; + for (i=0; (vars+i)->name; i++) { + if (!strcmp((vars+i)->name,varname)) { + char *p = (vars+i)->value; + return *p ? atoi(p) : defval; + } + } + } + return defval; +} + +int64_t mwGetVarValueInt64(HttpVariables* vars, const char *varname) +{ + if (vars && varname) { + int i; + for (i = 0; (vars + i)->name; i++) { + if (!strcmp((vars + i)->name, varname)) { + char *p = (vars + i)->value; + return *p ? atoll(p) : 0; + } + } + } + return 0; +} + +float mwGetVarValueFloat(HttpVariables* vars, const char *varname) +{ + if (vars && varname) { + int i; + for (i = 0; (vars + i)->name; i++) { + if (!strcmp((vars + i)->name, varname)) { + char *p = (vars + i)->value; + return *p ? (float)atof(p) : 0; + } + } + } + return 0; +} + +static unsigned int hex2uint32(const char *p) +{ + register char c; + register unsigned int i = 0; + for(c=*p;;){ + if (c>='A' && c<='F') + c-=7; + else if (c>='a' && c<='f') + c-=39; + else if (c<'0' || c>'9') + break; + i=(i<<4)|(c&0xF); + c=*(++p); + } + return i; +} + +unsigned int mwGetVarValueHex(HttpVariables* vars, const char *varname, unsigned int defval) +{ + int i; + if (vars && varname) { + for (i=0; (vars+i)->name; i++) { + if (!strcmp((vars+i)->name,varname)) { + char *p = (vars+i)->value; + return p ? hex2uint32(p) : defval; + } + } + } + return defval; +} + +int mwGetHttpDateTime(time_t timer, char *buf, int bufsize) +{ + struct tm *btm; + btm=gmtime(&timer); + return snprintf(buf, bufsize, httpDateTimeFormat, + dayNames+(btm->tm_wday<<2), + btm->tm_mday, + monthNames+(btm->tm_mon<<2), + 1900+btm->tm_year, + btm->tm_hour, + btm->tm_min, + btm->tm_sec); +} + +void mwInitParam(HttpParam* hp, int port, const char* pchWebPath) +{ + memset(hp, 0, sizeof(HttpParam)); + hp->httpPort = port; + hp->maxClients = HTTP_MAX_CLIENTS_DEFAULT; + if (hp->pchWebPath) { + free(hp->pchWebPath); + hp->pchWebPath = 0; + } + hp->pchWebPath = strdup(pchWebPath ? pchWebPath : ""); +} + +//////////////////////////////////////////////////////////////////////////// +// mwServerStart +// Start the webserver +//////////////////////////////////////////////////////////////////////////// +int mwServerStart(HttpParam* hp) +{ + if (hp->bWebserverRunning) { + return -1; + } + + if (hp->maxClients == 0) { + SYSLOG(LOG_INFO,"Maximum clients not set\n"); + return -1; + } + if (!(hp->listenSocket=_mwStartListening(hp))) { + return -1; + } + + hp->hsSocketQueue = calloc(hp->maxClients, sizeof(HttpSocket)); + hp->bKillWebserver=FALSE; + hp->bWebserverRunning=TRUE; + return 0; +} + +//////////////////////////////////////////////////////////////////////////// +// mwServerShutdown +// Shutdown the webserver +//////////////////////////////////////////////////////////////////////////// +int mwServerShutdown(HttpParam* hp) +{ + int i; + if (hp->bKillWebserver || !hp->bWebserverRunning) return -1; + + _mwCloseAllConnections(hp); + + // signal webserver thread to quit + hp->bKillWebserver=TRUE; + + // and wait for thread to exit + for (i=0;hp->bWebserverRunning && i<30;i++) msleep(100); + + return 0; +} + +int mwGetLocalFileName(HttpFilePath* hfp) +{ + char ch; + char *p = (char*)hfp->cFilePath; + char *s = (char*)hfp->pchHttpPath; + char *upLevel = NULL; + + hfp->pchExt=NULL; + hfp->fTailSlash=0; + if (*s == '~') { + s++; + } else if (hfp->pchRootPath) { + p+=_mwStrCopy(hfp->cFilePath,hfp->pchRootPath); + if (*(p-1)!=SLASH) { + *p=SLASH; + *(++p)=0; + } + } + while ((ch=*s) && ch!='?' && (int)(p-hfp->cFilePath)cFilePath)-1) { + if (ch=='%') { + *(p++) = _mwDecodeCharacter(++s); + s += 2; + } else if (ch=='/') { + *p=SLASH; + upLevel=(++p); + while (*(++s)=='/'); + continue; + } else if (ch=='+') { + *(p++)=' '; + s++; + } else if (ch=='.') { + if (upLevel && !memcmp(s+1, "./", 2)) { + s+=2; + p=upLevel; + } else { + *(p++)='.'; + hfp->pchExt=p; + while (*(++s)=='.'); //avoid '..' appearing in filename for security issue + } + } else { + *(p++)=*(s++); + } + } + if (*(p-1)==SLASH) { + p--; + hfp->fTailSlash=1; + } + *p=0; + return (int)(p - hfp->cFilePath); +} + +//////////////////////////////////////////////////////////////////////////// +// Internal (private) helper functions +//////////////////////////////////////////////////////////////////////////// + +SOCKET _mwStartListening(HttpParam* hp) +{ + SOCKET listenSocket; + struct sockaddr_in sinAddress; + int iRc; + + // create a new socket + listenSocket=socket(AF_INET,SOCK_STREAM,0); + if (listenSocket <= 0) { + return 0; + } + +#ifndef ARDUINO + // allow reuse of port number + { + int iOptVal=1; + iRc=setsockopt(listenSocket,SOL_SOCKET,SO_REUSEADDR, + (char*)&iOptVal,sizeof(iOptVal)); + if (iRc<0) return 0; + } +#endif + + // bind it to the http port + memset(&sinAddress,0,sizeof(struct sockaddr_in)); + sinAddress.sin_family=AF_INET; + // INADDR_ANY is 0 + //sinAddress.sin_addr.s_addr=htonl(hp->dwBindIP); + sinAddress.sin_addr.s_addr = hp->hlBindIP; + sinAddress.sin_port = htons(hp->httpPort); // http port + iRc=bind(listenSocket,(struct sockaddr*)&sinAddress, sizeof(struct sockaddr_in)); + if (iRc<0) { + return 0; + } + +#ifndef WIN32 + // set to non-blocking to avoid lockout issue (see Section 15.6 + // in Unix network programming book) + { + int iSockFlags; + iSockFlags = fcntl(listenSocket, F_GETFL, 0); + iSockFlags |= O_NONBLOCK; + fcntl(listenSocket, F_SETFL, iSockFlags); + } +#else + { + u_long iMode = 1; // non-blocking mode + ioctlsocket(listenSocket, FIONBIO, &iMode); + } +#endif + + // listen on the socket for incoming calls + if (listen(listenSocket, hp->maxClients)) { + return 0; + } + + // create UDP socket + if (hp->udpPort) { + hp->udpSocket = socket(AF_INET, SOCK_DGRAM, 0); + memset(&sinAddress, 0, sizeof(struct sockaddr_in)); + sinAddress.sin_family = AF_INET; + sinAddress.sin_addr.s_addr = hp->hlBindIP; + sinAddress.sin_port = htons(hp->udpPort); // UDP port + bind(hp->udpSocket, (struct sockaddr*)&sinAddress, sizeof(struct sockaddr_in)); + } + + return listenSocket; +} + +void _mwInitSocketData(HttpSocket *phsSocket) +{ + memset(&phsSocket->response,0,sizeof(HttpResponse)); + if (!phsSocket->buffer) + phsSocket->buffer = malloc(HTTP_BUFFER_SIZE); + phsSocket->request.startByte = 0; + phsSocket->request.pucHost = 0; + phsSocket->request.pucReferer = 0; + phsSocket->request.pucTransport = 0; + phsSocket->request.pucPath = 0; + phsSocket->request.headerSize = 0; + phsSocket->request.payloadSize = 0; + phsSocket->request.iCSeq = 0; + phsSocket->request.pucAuthInfo = NULL; + phsSocket->response.statusCode = 200; + phsSocket->fp = 0; + phsSocket->flags = 0; + phsSocket->pucData = phsSocket->buffer; + phsSocket->contentLength = 0; + phsSocket->bufferSize = HTTP_BUFFER_SIZE; + phsSocket->handler = NULL; + phsSocket->mimeType = NULL; +} + +static int _mwGetConnFromIP(HttpParam* hp, IPADDR ip) +{ + int i; + int connThisIP = 0; + for (i = 0; i < hp->maxClients; i++) { + if (!hp->hsSocketQueue[i].socket) continue; + if (hp->hsSocketQueue[i].ipAddr.laddr == ip.laddr) { + connThisIP++; + } + } + return connThisIP; +} + +void _mwCloseAllConnections(HttpParam* hp) +{ + int i; + + if (hp->listenSocket) { + closesocket(hp->listenSocket); + hp->listenSocket = 0; + } + for (i = 0; i < hp->maxClients; i++) { + if (hp->hsSocketQueue[i].socket) { + closesocket(hp->hsSocketQueue[i].socket); + hp->hsSocketQueue[i].socket = 0; + } + } +} + +//////////////////////////////////////////////////////////////////////////// +// _mwHttpThread +// Webserver independant processing thread. Handles all connections +//////////////////////////////////////////////////////////////////////////// +void mwHttpLoop(HttpParam *hp, uint32_t timeout) +{ + HttpSocket *phsSocketCur; + SOCKET socket; + struct sockaddr_in sinaddr; + int iRc; + int i; + + time_t tmCurrentTime; + SOCKET iSelectMaxFds; + fd_set fdsSelectRead; + fd_set fdsSelectWrite; + + // clear descriptor sets + FD_ZERO(&fdsSelectRead); + FD_ZERO(&fdsSelectWrite); + FD_SET(hp->listenSocket,&fdsSelectRead); + iSelectMaxFds=hp->listenSocket; + + if (hp->udpSocket) { + FD_SET(hp->udpSocket, &fdsSelectRead); + if (hp->udpSocket > iSelectMaxFds) iSelectMaxFds = hp->udpSocket; + } + + // get current time + tmCurrentTime=time(NULL); + // build descriptor sets and close timed out sockets + for (i = 0; i < hp->maxClients; i++) { + phsSocketCur = hp->hsSocketQueue + i; + + // get socket fd + socket = phsSocketCur->socket; + if (!socket) continue; + + { + int iError=0; + socklen_t iOptSize = sizeof(int); + if (getsockopt(socket,SOL_SOCKET,SO_ERROR,(char*)&iError,&iOptSize)) { + // if a socket contains a error, close it + SYSLOG(LOG_INFO,"[%d] Socket no longer vaild.\n",socket); + phsSocketCur->flags=FLAG_CONN_CLOSE; + _mwCloseSocket(hp, phsSocketCur); + continue; + } + } + // check expiration timer (for non-listening, in-use sockets) + if (tmCurrentTime > phsSocketCur->tmExpirationTime) { + // close connection + phsSocketCur->flags=FLAG_CONN_CLOSE; + _mwCloseSocket(hp, phsSocketCur); + } else { + if (ISFLAGSET(phsSocketCur,FLAG_SENDING)) { + // add to write descriptor set + FD_SET(socket,&fdsSelectWrite); + } else { + // add to read descriptor set + FD_SET(socket,&fdsSelectRead); + } + // check if new max socket + if (socket>iSelectMaxFds) { + iSelectMaxFds=socket; + } + } + } + + { + struct timeval tvSelectWait; + // initialize select delay + tvSelectWait.tv_sec = timeout / 1000; + tvSelectWait.tv_usec = (timeout % 1000) * 1000; // note: using timeval here -> usec not nsec + + // and check sockets (may take a while!) + iRc=select(iSelectMaxFds+1, &fdsSelectRead, &fdsSelectWrite, + NULL, &tvSelectWait); + } + + if (iRc <= 0) { + return; + } + + // check if any udp socket to read + if (hp->udpSocket && FD_ISSET(hp->udpSocket, &fdsSelectRead)) { + hp->pfnIncomingUDP(hp); + } + + // check which sockets are read/write able + for (i = 0; i < hp->maxClients; i++) { + BOOL bRead; + BOOL bWrite; + + phsSocketCur = hp->hsSocketQueue + i; + + // get socket fd + socket = phsSocketCur->socket; + if (!socket) continue; + + // get read/write status for socket + bRead = FD_ISSET_BOOL(socket, &fdsSelectRead); + bWrite = FD_ISSET_BOOL(socket, &fdsSelectWrite); + + if (bRead || bWrite) { + iRc = -1; + if (ISFLAGSET(phsSocketCur,FLAG_SENDING) && bWrite) { + iRc=_mwProcessWriteSocket(hp, phsSocketCur); + } else if (bRead) { + SETFLAG(phsSocketCur, FLAG_RECEIVING); + iRc=_mwProcessReadSocket(hp,phsSocketCur); + } + if (iRc == 0) { + // reset expiration timer + phsSocketCur->tmExpirationTime = time(NULL) + HTTP_EXPIRATION_TIME; + } else { + if (iRc == -1) { + SETFLAG(phsSocketCur, FLAG_CONN_CLOSE); + } + _mwCloseSocket(hp, phsSocketCur); + } + } + } + + // check if any socket to accept and accept the socket + if (FD_ISSET(hp->listenSocket, &fdsSelectRead)) do { + // find empty slot + phsSocketCur = 0; + for (i = 0; i < hp->maxClients; i++) { + if (hp->hsSocketQueue[i].socket == 0) { + phsSocketCur = hp->hsSocketQueue + i; + break; + } + } + + if (!phsSocketCur) { + // all slots occupied + // find longest waiting idle socket and close it + time_t earliest = 0; + for (i = 0; i < hp->maxClients; i++) { + if (!ISFLAGSET((hp->hsSocketQueue + i), FLAG_RECEIVING | FLAG_SENDING) + && (earliest == 0 || hp->hsSocketQueue[i].tmExpirationTime < earliest)) { + phsSocketCur = hp->hsSocketQueue + i; + earliest = hp->hsSocketQueue[i].tmExpirationTime; + } + } + if (!phsSocketCur) { + SYSLOG(LOG_INFO,"Connection denied\n"); + break; + } else { + SETFLAG(phsSocketCur, FLAG_CONN_CLOSE); + _mwCloseSocket(hp, phsSocketCur); + } + } + + phsSocketCur->socket = _mwAcceptSocket(hp,&sinaddr); + if (phsSocketCur->socket == 0) + break; + phsSocketCur->ipAddr.laddr=ntohl(sinaddr.sin_addr.s_addr); + SYSLOG(LOG_INFO,"[%d] Client IP: %d.%d.%d.%d\n", + phsSocketCur->socket, + phsSocketCur->ipAddr.caddr[3], + phsSocketCur->ipAddr.caddr[2], + phsSocketCur->ipAddr.caddr[1], + phsSocketCur->ipAddr.caddr[0]); + + hp->stats.clientCount++; + + //fill structure with data + _mwInitSocketData(phsSocketCur); + phsSocketCur->tmExpirationTime = time(NULL) + HTTP_EXPIRATION_TIME; + + //update max client count + if (hp->stats.clientCount>hp->stats.clientCountMax) hp->stats.clientCountMax=hp->stats.clientCount; + } while(0); +} // end of _mwHttpThread + +void mwServerExit(HttpParam* hp) +{ + int i; + // cleanup + _mwCloseAllConnections(hp); + for (i = 0; i < hp->maxClients; i++) { + if (hp->hsSocketQueue[i].buffer) free(hp->hsSocketQueue[i].buffer); + } + free(hp->hsSocketQueue); + hp->hsSocketQueue = 0; + + // clear state vars + hp->bKillWebserver = FALSE; + hp->bWebserverRunning = FALSE; +} +//////////////////////////////////////////////////////////////////////////// +// _mwAcceptSocket +// Accept an incoming connection +//////////////////////////////////////////////////////////////////////////// +SOCKET _mwAcceptSocket(HttpParam* hp,struct sockaddr_in *sinaddr) +{ + socklen_t namelen=sizeof(struct sockaddr); + SOCKET socket = accept(hp->listenSocket, (struct sockaddr*)sinaddr,&namelen); + +#ifndef WIN32 + // set to non-blocking to stop sends from locking up thread + { + int iSockFlags; + iSockFlags = fcntl(socket, F_GETFL, 0); + iSockFlags |= O_NONBLOCK; + fcntl(socket, F_SETFL, iSockFlags); + } +#endif + + return socket; +} // end of _mwAcceptSocket + +int _mwBuildHttpHeader(HttpParam* hp, HttpSocket *phsSocket, time_t contentDateTime, char* buffer) +{ + char *p = buffer; + char *end = buffer + 512; + const char *status; + BOOL keepalive = !ISFLAGSET(phsSocket,FLAG_CONN_CLOSE); + + if (phsSocket->response.statusCode >= 200 && phsSocket->response.statusCode < 200 + sizeof(status200) / sizeof(status200[0])) { + status = status200[phsSocket->response.statusCode - 200]; + } else if (phsSocket->response.statusCode >= 300 && phsSocket->response.statusCode < 300 + sizeof(status300) / sizeof(status300[0])) { + status = status300[phsSocket->response.statusCode - 300]; + } else if (phsSocket->response.statusCode >= 400 && phsSocket->response.statusCode < 400 + sizeof(status400) / sizeof(status400[0])) { + status = status400[phsSocket->response.statusCode - 400]; + } else if (phsSocket->response.statusCode >= 500 && phsSocket->response.statusCode < 500 + sizeof(status500) / sizeof(status500[0])) { + status = status500[phsSocket->response.statusCode - 500]; + } else { + status = ""; + } + + p += snprintf(p, end - p, HTTP200_HEADER, +#ifdef ENABLE_RTSP + (phsSocket->flags & (FLAG_REQUEST_DESCRIBE | FLAG_REQUEST_SETUP)) ? "RTSP/1.0" : "HTTP/1.1", +#else + "HTTP/1.1", +#endif + phsSocket->response.statusCode, status, + HTTP_SERVER_NAME, + keepalive ? "keep-alive" : "close"); + + if (keepalive) { + p += snprintf(p, end - p, "Keep-Alive: timeout=%u, max=1000\r\n", HTTP_KEEPALIVE_TIME); + } + if (!(hp->flags & FLAG_DISABLE_RANGE)) { + p += snprintf(p, end - p, "Accept-Ranges: bytes\r\n"); + } + + if ((int)contentDateTime > 0) { + p += snprintf(p, end - p, "Last-Modified: "); + p += mwGetHttpDateTime(contentDateTime, p, end - p); + strcpy(p, "\r\n"); + p+=2; + } + if (phsSocket->request.iCSeq) { + p += snprintf(p, end - p, "CSeq: %d\r\n", phsSocket->request.iCSeq); + } + if (phsSocket->response.contentLength > 0) { + p += snprintf(p, end - p, "Content-Type: %s\r\n", phsSocket->mimeType ? phsSocket->mimeType : contentTypeTable[phsSocket->response.fileType]); + if (phsSocket->request.startByte) { + p += snprintf(p, end - p, "Content-Range: bytes %u-%u/*\r\n", + phsSocket->request.startByte, phsSocket->response.contentLength); + } + } + if (!(phsSocket->flags & FLAG_CHUNK)) { + p+=snprintf(p, end - p,"Content-Length: %u\r\n", phsSocket->response.contentLength); + } else { + p += sprintf(p, "Transfer-Encoding: chunked\r\n"); + } + if (phsSocket->response.statusCode == 301 || phsSocket->response.statusCode == 307) { + p += sprintf(p, "Location: %s\r\n", phsSocket->pucData); + } + strcpy(p, "\r\n"); + return (int)(p- buffer + 2); +} + +int mwParseQueryString(UrlHandlerParam* up) +{ + if (up->iVarCount==-1) { + //parsing variables from query string + char *p,*s; + // get start of query string + s = strchr(up->pucRequest, '?'); + if (s) { + *(s++) = 0; + } else if (ISFLAGSET(up->hs,FLAG_REQUEST_POST)){ + s = up->hs->request.pucPayload; + if (s && s[0] == '<') s = 0; + } + if (s && *s) { + int i; + int n = 1; + //get number of variables + for (p = s; *p ; p++) { + if (*p < 32 || *p > 127) + return 0; + if (*p == '&') n++; + } + up->pxVars = calloc(n + 1, sizeof(HttpVariables)); + up->iVarCount = n; + //store variable name and value + for (i = 0, p = s; i < n; p++) { + switch (*p) { + case '=': + if (!(up->pxVars + i)->name) { + *p = 0; + (up->pxVars + i)->name = s; + s=p+1; + } + break; + case 0: + case '&': + *p = 0; + if ((up->pxVars + i)->name) { + (up->pxVars + i)->value = s; + mwDecodeString(s); + } else { + (up->pxVars + i)->name = s; + (up->pxVars + i)->value = p; + } + s = p + 1; + i++; + break; + } + } + (up->pxVars + n)->name = NULL; + } + } + return up->iVarCount; +} + +//////////////////////////////////////////////////////////////////////////// +// _mwBase64Encode +// buffer size of out_str is (in_len * 4 / 3 + 1) +//////////////////////////////////////////////////////////////////////////// +void _mwBase64Encode(const char *in_str, int in_len, char *out_str) +{ + const char base64[] ="ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + int curr_out_len = 0; + int i = 0; + char a, b, c; + + out_str[0] = '\0'; + + if (in_len <= 0) return; + + while (i < in_len) { + a = in_str[i]; + b = (i + 1 >= in_len) ? 0 : in_str[i + 1]; + c = (i + 2 >= in_len) ? 0 : in_str[i + 2]; + if (i + 2 < in_len) { + out_str[curr_out_len++] = (base64[(a >> 2) & 0x3F]); + out_str[curr_out_len++] = (base64[((a << 4) & 0x30) + ((b >> 4) & 0xf)]); + out_str[curr_out_len++] = (base64[((b << 2) & 0x3c) + ((c >> 6) & 0x3)]); + out_str[curr_out_len++] = (base64[c & 0x3F]); + } + else if (i + 1 < in_len) { + out_str[curr_out_len++] = (base64[(a >> 2) & 0x3F]); + out_str[curr_out_len++] = (base64[((a << 4) & 0x30) + ((b >> 4) & 0xf)]); + out_str[curr_out_len++] = (base64[((b << 2) & 0x3c) + ((c >> 6) & 0x3)]); + out_str[curr_out_len++] = '='; + } + else { + out_str[curr_out_len++] = (base64[(a >> 2) & 0x3F]); + out_str[curr_out_len++] = (base64[((a << 4) & 0x30) + ((b >> 4) & 0xf)]); + out_str[curr_out_len++] = '='; + out_str[curr_out_len++] = '='; + } + i += 3; + } + + out_str[curr_out_len] = '\0'; +} + +//////////////////////////////////////////////////////////////////////////// +// _mwGetBaisAuthorization +// RETURN VALUE: Authorization string, need to free by caller +//////////////////////////////////////////////////////////////////////////// +int _mwGetBaisAuthorization(const char* username, const char* password, char* out /*OUT*/) +{ + const char prefix[] = "Basic "; + int len = (int)(strlen(username) + 1 + strlen(password)); + int out_len = sizeof(prefix) + (len * 4 / 3 + 1) + 2; + char *tmp, *p; + + if (out_len >= MAX_AUTH_INFO_LEN) return -1; + + tmp = (char*)malloc(len + 1); + sprintf(tmp, "%s:%s", username, password); + strcpy(out, prefix); + p = out + sizeof(prefix) - 1; + _mwBase64Encode(tmp, len, p); + p += strlen(p); + p[0] = '\r'; p[1] = '\n'; p[2] = '\0'; + + free(tmp); + + return 0; +} + +//////////////////////////////////////////////////////////////////////////// +// _mwSend401AuthorizationRequired +//////////////////////////////////////////////////////////////////////////// +void _mwSend401AuthorizationRequired(HttpParam* hp, HttpSocket* phsSocket, int reason) +{ + char hdr[128]; + const char *body = NULL; + int hdrsize = 0; + int bodylen; + if (reason == AUTH_REQUIRED) { + body = "Authentication required"; + } + else { + body = "Authentication failed"; + } + bodylen = strlen(body); + + hdrsize = snprintf(hdr, sizeof(hdr), HTTP401_HEADER, "Login", bodylen); + + SYSLOG(LOG_INFO,"[%d] Authorization Required\n",phsSocket->socket); + // send Authorization Required + send(phsSocket->socket, hdr, hdrsize, 0); + send(phsSocket->socket, body, bodylen, 0); + //Below is the work around + SETFLAG(phsSocket, FLAG_CONN_CLOSE); + _mwCloseSocket(hp, phsSocket); +} + +//////////////////////////////////////////////////////////////////////////// +// _mwBasicAuthorizationHandlers +// Basic Authorization implement +// RETURN VALUE: +// -1 (failed) +// 0 (no need to authorization) +// 1 (successed) +// 2 (Authorization needed) +//////////////////////////////////////////////////////////////////////////// +int _mwBasicAuthorizationHandlers(HttpParam* hp, HttpSocket* phsSocket) +{ + AuthHandler* pah; + char* path = phsSocket->request.pucPath; + int ret = AUTH_NO_NEED; + + if (phsSocket->ipAddr.laddr == INADDR_LOOPBACK) { + return ret; + } + for (pah = hp->pxAuthHandler; pah && pah->pchUrlPrefix; pah++) { + //printf("\req path:%s prefix:0x%x, username:0x%x, password:0x%x\n", path, pah->pchUrlPrefix, pah->pchUsername, pah->pchPassword); + if (*pah->pchUrlPrefix && strncmp(path, pah->pchUrlPrefix, strlen(pah->pchUrlPrefix)) != 0) continue; + + if (pah->pchUsername == NULL || *pah->pchUsername == '\0' || + pah->pchPassword == NULL || *pah->pchPassword == '\0') continue; + + if (*pah->pchAuthString == '\0') { + if (_mwGetBaisAuthorization(pah->pchUsername, pah->pchPassword, pah->pchAuthString) != 0) continue; + } + if (phsSocket->request.pucAuthInfo == NULL) { + ret = AUTH_REQUIRED; //Need to auth + break; + } + else if (strncmp(phsSocket->request.pucAuthInfo, pah->pchAuthString, strlen(pah->pchAuthString)) == 0) { //FIXME: + phsSocket->request.pucAuthInfo = pah->pchOtherInfo ? pah->pchOtherInfo : "group=admin"; + ret = AUTH_SUCCESSED; //successed + break; + } + else { + ret = AUTH_FAILED; //Failed, try next + } + } + + return ret; +} + +int _mwCheckUrlHandlers(HttpParam* hp, HttpSocket* phsSocket) +{ + UrlHandler* puh; + UrlHandlerParam up; + int ret=0; + char* path = phsSocket->request.pucPath; + while (*path && *path == '/') path++; + up.pxVars=NULL; + for (puh=hp->pxUrlHandler; puh && puh->pchUrlPrefix; puh++) { + size_t prefixLen=strlen(puh->pchUrlPrefix); + if (puh->pfnUrlHandler && ((prefixLen == 0 && *path == 0) || (prefixLen && !strncmp(path,puh->pchUrlPrefix,prefixLen)))) { + //URL prefix matches + memset(&up, 0, sizeof(up)); + up.hp=hp; + up.hs = phsSocket; + up.bufSize=(int)phsSocket->bufferSize; + up.pucRequest=path+prefixLen; + up.pucHeader=phsSocket->buffer; + up.pucBuffer=phsSocket->pucData; + up.pucBuffer[0]=0; + up.pucPayload = phsSocket->request.pucPayload; + up.payloadSize = phsSocket->request.payloadSize; + up.iVarCount=-1; + phsSocket->handler = puh; + if (strchr(up.pucRequest, '?')) mwParseQueryString(&up); + ret=(*puh->pfnUrlHandler)(&up); + if (!ret) continue; + if (phsSocket->response.statusCode >= 500) phsSocket->flags |= FLAG_CONN_CLOSE; + phsSocket->flags|=ret; + phsSocket->response.fileType = up.contentType; + if (ret & FLAG_DATA_RAW) { + SETFLAG(phsSocket, FLAG_DATA_RAW); + phsSocket->pucData=up.pucBuffer; + phsSocket->contentLength=up.contentLength; + phsSocket->response.contentLength=up.contentLength; + if (ret & FLAG_TO_FREE) { + phsSocket->ptr=up.pucBuffer; //keep the pointer which will be used to free memory later + } + } else if (ret & FLAG_DATA_STREAM) { + SETFLAG(phsSocket, FLAG_DATA_STREAM); + phsSocket->pucData = up.pucBuffer; + phsSocket->contentLength = up.contentLength; + phsSocket->response.contentLength = 0; + } else if (ret & FLAG_DATA_FILE) { + SETFLAG(phsSocket, FLAG_DATA_FILE); + if (up.pucBuffer[0]) { + free(phsSocket->request.pucPath); + phsSocket->request.pucPath=strdup(up.pucBuffer); + } + } else if (ret & FLAG_DATA_REDIRECT) { + phsSocket->pucData = up.pucBuffer; + } + _mwFreeJSONPairs(&up); + break; + } + } + if (up.pxVars) free(up.pxVars); + return ret; +} + +//////////////////////////////////////////////////////////////////////////// +// _mwProcessReadSocket +// Process a socket (read) +//////////////////////////////////////////////////////////////////////////// +int _mwProcessReadSocket(HttpParam* hp, HttpSocket* phsSocket) +{ + int iLength = recv(phsSocket->socket, + phsSocket->pucData+phsSocket->contentLength, + (int)(phsSocket->bufferSize - phsSocket->contentLength - 1), 0); + if (iLength <= 0) { + return -1; + } + // add in new data received + phsSocket->contentLength += iLength; + phsSocket->pucData[phsSocket->contentLength] = 0; + + // check if end of request + if (phsSocket->request.headerSize==0) { + int i=0; + char *path = 0; + char *headerEnd = strstr(phsSocket->buffer, HTTP_HEADER_END); + + if (!headerEnd) + return 0; + + // reach the end of the header + //check request type + if (!memcmp(phsSocket->buffer, "GET", 3)) { + SETFLAG(phsSocket,FLAG_REQUEST_GET); + path = phsSocket->pucData + 5; + } else if (!memcmp(phsSocket->buffer, "POST", 4)) { + SETFLAG(phsSocket,FLAG_REQUEST_POST); + path = phsSocket->pucData + 6; + } else { + SYSLOG(LOG_INFO,"[%d] Unsupported method\n",phsSocket->socket); + phsSocket->request.pucPath = 0; + return -1; + } + + // count connections from this IP and duplicated connection + if (hp->maxClientsPerIP && _mwGetConnFromIP(hp, phsSocket->ipAddr) > hp->maxClientsPerIP) { + // too many connection from the same IP + SYSLOG(LOG_INFO,"[%d] Too many connections from same IP\n", phsSocket->socket); + send(phsSocket->socket, HTTP403_HEADER, sizeof(HTTP403_HEADER) - 1, 0); + return -1; + } + + phsSocket->request.headerSize = (int)(headerEnd - phsSocket->buffer + 4); + if (_mwParseHttpHeader(phsSocket)) { + SYSLOG(LOG_INFO,"Error parsing request\n"); + return -1; + } else { + int pathLen; + + if ((hp->flags & FLAG_DISABLE_RANGE) && phsSocket->request.startByte > 0) { + send(phsSocket->socket, HTTP403_HEADER, sizeof(HTTP403_HEADER) - 1, 0); + return -1; + } + + // keep request path + for (i = 0; i < MAX_REQUEST_PATH_LEN; i++) { + if ((path[i] == ' ' && (!strncmp(path + i + 1, "HTTP/", 5) || !strncmp(path + i + 1, "RTSP/", 5))) + || path[i] == '\r') { + break; + } + } + pathLen = i; + if (pathLen >= MAX_REQUEST_PATH_LEN) { + return -1; + } + + phsSocket->request.pucPath = malloc(pathLen + 1); + memcpy(phsSocket->request.pucPath, path, pathLen); + phsSocket->request.pucPath[pathLen] = 0; + SYSLOG(LOG_INFO, "[%d] Request path: %s\n", phsSocket->socket, phsSocket->request.pucPath); + + if (ISFLAGSET(phsSocket,FLAG_REQUEST_POST)) { + if (!phsSocket->request.pucPayload) { + // first receive of payload, prepare for next receive + if (phsSocket->request.payloadSize > MAX_POST_PAYLOAD_SIZE) phsSocket->request.payloadSize = MAX_POST_PAYLOAD_SIZE; + phsSocket->bufferSize = phsSocket->request.payloadSize + 1; + phsSocket->request.pucPayload = malloc(phsSocket->bufferSize); + phsSocket->pucData = phsSocket->request.pucPayload; + // payload length already received + phsSocket->contentLength -= phsSocket->request.headerSize; + // copy already received payload to payload buffer + if (phsSocket->contentLength > phsSocket->request.payloadSize) { + phsSocket->contentLength = phsSocket->request.payloadSize; + } + memcpy(phsSocket->request.pucPayload, phsSocket->buffer + phsSocket->request.headerSize, phsSocket->contentLength); + phsSocket->request.pucPayload[phsSocket->contentLength]=0; + } + } + } + } + + if (phsSocket->request.payloadSize > 0 && phsSocket->contentLength < (int)phsSocket->request.payloadSize) { + // there is more data + return 0; + } + + // add header zero terminator + phsSocket->buffer[phsSocket->request.headerSize]=0; + + if (phsSocket->request.headerSize) { + phsSocket->pucData = phsSocket->buffer + phsSocket->request.headerSize + 4; + phsSocket->bufferSize = HTTP_BUFFER_SIZE - phsSocket->request.headerSize - 4; + } else { + phsSocket->pucData = phsSocket->buffer; + phsSocket->bufferSize = HTTP_BUFFER_SIZE; + } + + hp->stats.reqCount++; + phsSocket->reqCount++; + + if (hp->pxAuthHandler != NULL) { + int ret = _mwBasicAuthorizationHandlers(hp, phsSocket); + switch (ret) { + case AUTH_NO_NEED: //No need to auth + case AUTH_SUCCESSED: //Successed + break; + case AUTH_REQUIRED: //Authorization needed + case AUTH_FAILED: + default://Failed + _mwSend401AuthorizationRequired(hp, phsSocket, ret); + return 0; + } + } + + if (!hp->pxUrlHandler || !_mwCheckUrlHandlers(hp,phsSocket)) + SETFLAG(phsSocket,FLAG_DATA_FILE); + + // set state to SENDING (actual sending will occur on next select) + CLRFLAG(phsSocket,FLAG_RECEIVING) + if (phsSocket->request.iHttpVer == 0) { + CLRFLAG(phsSocket, FLAG_CHUNK); + } + if (ISFLAGSET(phsSocket,FLAG_DATA_RAW | FLAG_DATA_STREAM)) { + SETFLAG(phsSocket,FLAG_SENDING); + return _mwStartSendRawData(hp, phsSocket); + } else if (ISFLAGSET(phsSocket,FLAG_DATA_FILE)) { + SETFLAG(phsSocket,FLAG_SENDING); + return _mwStartSendFile(hp,phsSocket); + } else if (ISFLAGSET(phsSocket,FLAG_DATA_REDIRECT)) { + _mwRedirect(phsSocket, phsSocket->pucData); + return 0; + } + SYSLOG(LOG_INFO,"Invalid data flag specified\n"); + return -1; +} // end of _mwProcessReadSocket + +//////////////////////////////////////////////////////////////////////////// +// _mwProcessWriteSocket +// Process a socket (write) +//////////////////////////////////////////////////////////////////////////// +int _mwProcessWriteSocket(HttpParam *hp, HttpSocket* phsSocket) +{ + if (phsSocket->contentLength<=0 && !ISFLAGSET(phsSocket,FLAG_DATA_STREAM)) { + return 1; + } + //SYSLOG(LOG_INFO,"[%d] sending data\n",phsSocket->socket); + if (ISFLAGSET(phsSocket,FLAG_DATA_RAW|FLAG_DATA_STREAM)) { + return _mwSendRawDataChunk(hp, phsSocket); + } else if (ISFLAGSET(phsSocket,FLAG_DATA_FILE)) { + return _mwSendFileChunk(hp, phsSocket); + } else { + SYSLOG(LOG_INFO,"Invalid content source\n"); + return -1; + } +} // end of _mwProcessWriteSocket + +//////////////////////////////////////////////////////////////////////////// +// _mwCloseSocket +// Close an open connection +//////////////////////////////////////////////////////////////////////////// +void _mwCloseSocket(HttpParam* hp, HttpSocket* phsSocket) +{ + if (phsSocket->socket == 0) return; + if (phsSocket->fp) { + fclose(phsSocket->fp); + phsSocket->fp = 0; + hp->stats.openedFileCount--; + } + if (phsSocket->request.pucPayload) { + free(phsSocket->request.pucPayload); + phsSocket->request.pucPayload = 0; + } + if (phsSocket->handler && (ISFLAGSET(phsSocket,FLAG_DATA_STREAM) || ISFLAGSET(phsSocket,FLAG_CLOSE_CALLBACK | FLAG_CONN_CLOSE) == (FLAG_CLOSE_CALLBACK | FLAG_CONN_CLOSE))) { + UrlHandlerParam up; + UrlHandler* pfnHandler = (UrlHandler*)phsSocket->handler; + memset(&up, 0, sizeof(up)); + up.hs = phsSocket; + up.hp = hp; + //notify the handler of closed connection + (pfnHandler->pfnUrlHandler)(&up); + //unbind handler + phsSocket->handler = 0; + } + if (ISFLAGSET(phsSocket,FLAG_TO_FREE) && phsSocket->ptr) { + free(phsSocket->ptr); + phsSocket->ptr=NULL; + CLRFLAG(phsSocket, FLAG_TO_FREE); + } + if (phsSocket->request.pucPath) { + free(phsSocket->request.pucPath); + phsSocket->request.pucPath = 0; + } + if (!ISFLAGSET(phsSocket,FLAG_CONN_CLOSE) && phsSocket->reqCount < HTTP_KEEPALIVE_MAX) { + _mwInitSocketData(phsSocket); + //reset flag bits + phsSocket->tmExpirationTime=time(NULL)+HTTP_KEEPALIVE_TIME; + return; + } + closesocket(phsSocket->socket); + phsSocket->reqCount = 0; + hp->stats.clientCount--; + SYSLOG(LOG_INFO,"[%d] Socket closed, %u connections\n",phsSocket->socket, hp->stats.clientCount); + phsSocket->socket = 0; + phsSocket->reqCount=0; +} // end of _mwCloseSocket + +int _mwStrCopy(char *dest, const char *src) +{ + int i; + for (i=0; src[i]; i++) { + dest[i]=src[i]; + } + dest[i]=0; + return i; +} + +int _mwStrHeadMatch(char** pbuf1, const char* buf2) { + unsigned int i; + char* buf1 = *pbuf1; + int x; + for(i=0;buf2[i];i++) { + if ((x=toupper((int)buf1[i])-toupper((int)buf2[i]))) return 0; + } + *pbuf1 = buf1 + i; + return i; +} + +void _mwSendErrorPage(SOCKET socket, const char* header, const char* body) +{ + char hdr[128]; + int len = (int)strlen(body); + int hdrsize = snprintf(hdr, sizeof(hdr), header, HTTP_SERVER_NAME, len); + send(socket, hdr, hdrsize, 0); + send(socket, body, len, 0); +} + +#ifdef WIN32 +#define OPEN_FLAG O_RDONLY|0x8000 +#else +#define OPEN_FLAG O_RDONLY +#endif + +//////////////////////////////////////////////////////////////////////////// +// _mwStartSendFile +// Setup for sending of a file +//////////////////////////////////////////////////////////////////////////// +int _mwStartSendFile2(HttpParam* hp, HttpSocket* phsSocket, const char* filePath) +{ + struct stat st = { 0 }; + HttpFilePath hfp; + BOOL isDirPath = FALSE; + + if (filePath == NULL) + return -1; + + if (!ISFLAGSET(phsSocket, FLAG_ABSOLUTE_PATH)) { + hfp.pchRootPath = hp->pchWebPath; + hfp.pchHttpPath = filePath; + mwGetLocalFileName(&hfp); + } + else { + strncpy(hfp.cFilePath, filePath, sizeof(hfp.cFilePath) - 1); + } + + if (stat(hfp.cFilePath, &st) == 0) { + isDirPath = (st.st_mode & S_IFDIR); + } + if (!*filePath) isDirPath = TRUE; + + // open file + if (!isDirPath) { + phsSocket->fp = fopen(hfp.cFilePath, "rb"); + } + + if (!phsSocket->fp) { + char *p; + + if (!isDirPath) { + // file/dir not found + return -1; + } + + //requesting for directory, first try opening default pages + for (p = hfp.cFilePath; *p; p++); + *(p++)=SLASH; + for (int i = 0; ifp = fopen(hfp.cFilePath, "rb"); + if (phsSocket->fp) { + hfp.pchExt = strchr(defaultPages[i], '.') + 1; + break; + } + } + + if (!phsSocket->fp && (hp->flags & FLAG_DIR_LISTING)) { + SETFLAG(phsSocket,FLAG_DATA_RAW); + if (!hfp.fTailSlash) { + p=phsSocket->request.pucPath; + while(*p) p++; //seek to the end of the string + strcpy(p, "/"); //add a tailing slash + while(--p>(char*)phsSocket->request.pucPath) { + if (*p=='/') { + p++; + break; + } + } + _mwRedirect(phsSocket,p); + return _mwStartSendRawData(hp, phsSocket); + } + } + } + + if (phsSocket->fp) { + hp->stats.openedFileCount++; + fseek(phsSocket->fp, 0, SEEK_END); + long fileSize = ftell(phsSocket->fp); + phsSocket->response.contentLength = fileSize - phsSocket->request.startByte; + if (phsSocket->response.contentLength <= 0) { + phsSocket->request.startByte = 0; + phsSocket->response.contentLength = fileSize; + } + if (phsSocket->request.startByte) { + fseek(phsSocket->fp, (long)phsSocket->request.startByte, SEEK_SET); + phsSocket->response.statusCode = 206; + } else { + fseek(phsSocket->fp, 0, SEEK_SET); + } + if (!phsSocket->response.fileType && hfp.pchExt) { + phsSocket->response.fileType=mwGetContentType(hfp.pchExt); + } + } else { + return -1; + } + + //SYSLOG(LOG_INFO,"File/requested size: %d/%d\n",st.st_size,phsSocket->response.contentLength); + + // build http header + phsSocket->contentLength=_mwBuildHttpHeader( + hp, + phsSocket, + st.st_mtime, + phsSocket->pucData); + + phsSocket->response.headerBytes = phsSocket->contentLength; + phsSocket->response.sentBytes = 0; + return 0; +} // end of _mwStartSendFile2 + +//////////////////////////////////////////////////////////////////////////// +// _mwStartSendFile +// Setup for sending of a file +//////////////////////////////////////////////////////////////////////////// +int _mwStartSendFile(HttpParam* hp, HttpSocket* phsSocket) +{ + int ret; +#if MAX_OPEN_FILES + if (hp->stats.openedFileCount >= MAX_OPEN_FILES) return 0; +#endif + ret = _mwStartSendFile2(hp, phsSocket, phsSocket->request.pucPath); + if (ret != 0) { + SYSLOG(LOG_INFO,"[%d] Not found - %s\n",phsSocket->socket, phsSocket->request.pucPath); + _mwSendErrorPage(phsSocket->socket, HTTP404_HEADER, HTTP404_BODY); + } + return ret; +} //end of _mwStartSendFile + +//////////////////////////////////////////////////////////////////////////// +// _mwSendFileChunk +// Send a chunk of a file +//////////////////////////////////////////////////////////////////////////// +int _mwSendFileChunk(HttpParam *hp, HttpSocket* phsSocket) +{ + int iBytesWritten; + int iBytesRead; + + if (phsSocket->contentLength > 0) { + if ((phsSocket->flags & FLAG_CHUNK) && ISFLAGSET(phsSocket, FLAG_HEADER_SENT)) { + char buf[16]; + iBytesRead = snprintf(buf, sizeof(buf), "%X\r\n", phsSocket->contentLength); + iBytesWritten = send(phsSocket->socket, buf, iBytesRead, 0); + } + // send a chunk of data + iBytesWritten=send(phsSocket->socket, phsSocket->pucData,(int)phsSocket->contentLength, 0); + if (iBytesWritten<=0) { + // close connection + SETFLAG(phsSocket,FLAG_CONN_CLOSE); + fclose(phsSocket->fp); + hp->stats.openedFileCount--; + phsSocket->fp = 0; + return -1; + } + SETFLAG(phsSocket, FLAG_HEADER_SENT); + hp->stats.totalSentBytes+=iBytesWritten; + phsSocket->response.sentBytes+=iBytesWritten; + phsSocket->pucData+=iBytesWritten; + phsSocket->contentLength-=iBytesWritten; + // if only partial data sent just return wait the remaining data to be sent next time + if (phsSocket->contentLength>0) return 0; + } + + // used all buffered data - load next chunk of file + phsSocket->pucData=phsSocket->buffer; + iBytesRead = fread(phsSocket->buffer, 1, HTTP_BUFFER_SIZE, phsSocket->fp); + if (iBytesRead == -1 && errno == 8) + return 0; // try reading again next time + if (iBytesRead<=0) { + // finished with a file + int remainBytes = (int)(phsSocket->response.contentLength + phsSocket->response.headerBytes - phsSocket->response.sentBytes); + if (remainBytes > 0) { + if (remainBytes>HTTP_BUFFER_SIZE) remainBytes=HTTP_BUFFER_SIZE; + memset(phsSocket->buffer,0,remainBytes); + phsSocket->contentLength=remainBytes; + return 0; + } else { + if (phsSocket->flags & FLAG_CHUNK) { + send(phsSocket->socket, "0\r\n\r\n", 5, 0); + } + if (phsSocket->fp) { + fclose(phsSocket->fp); + phsSocket->fp = 0; + hp->stats.openedFileCount--; + } + return 1; + } + } + phsSocket->contentLength=iBytesRead; + return 0; +} // end of _mwSendFileChunk + +//////////////////////////////////////////////////////////////////////////// +// _mwStartSendRawData +// Start sending raw data blocks +//////////////////////////////////////////////////////////////////////////// +int _mwStartSendRawData(HttpParam *hp, HttpSocket* phsSocket) +{ + if (ISFLAGSET(phsSocket, FLAG_CUSTOM_HEADER)) { + return _mwSendRawDataChunk(hp, phsSocket); + } else { + char header[HTTP200_HDR_EST_SIZE]; + int offset=0,hdrsize,bytes; + hdrsize=_mwBuildHttpHeader(hp, phsSocket, 0, header); + // send http header + do { + bytes=send(phsSocket->socket, header+offset, hdrsize-offset, 0); + if (bytes<=0) break; + offset+=bytes; + hp->stats.totalSentBytes+=bytes; + } while (offsetflags & FLAG_CHUNK) { + char buf[16]; + int bytes = snprintf(buf, sizeof(buf), "%x\r\n", phsSocket->contentLength); + iBytesWritten = send(phsSocket->socket, buf, bytes, 0); + if (iBytesWritten<=0) return -1; + hp->stats.totalSentBytes+=iBytesWritten; + } + // send a chunk of data + if (phsSocket->contentLength > 0) { + iBytesWritten=(int)send(phsSocket->socket, phsSocket->pucData, phsSocket->contentLength, 0); + if (iBytesWritten<=0) { + // failure - close connection + return -1; + } else { + hp->stats.totalSentBytes+=iBytesWritten; + phsSocket->response.sentBytes+=iBytesWritten; + phsSocket->pucData+=iBytesWritten; + phsSocket->contentLength-=iBytesWritten; + } + } else { + if (ISFLAGSET(phsSocket,FLAG_DATA_STREAM) && phsSocket->handler) { + //load next chuck of raw data + UrlHandlerParam up; + UrlHandler* pfnHandler = (UrlHandler*)phsSocket->handler; + memset(&up, 0, sizeof(up)); + up.hs = phsSocket; + up.hp = hp; + up.pucBuffer=phsSocket->buffer; + up.bufSize=HTTP_BUFFER_SIZE; + if ((pfnHandler->pfnUrlHandler)(&up) == 0) { + if (phsSocket->flags & FLAG_CHUNK) { + iBytesWritten = send(phsSocket->socket, "0\r\n\r\n", 5, 0); + if (iBytesWritten<=0) return -1; + } + hp->stats.totalSentBytes+=iBytesWritten; + SETFLAG(phsSocket, FLAG_CONN_CLOSE); + return 1; // EOF + } + phsSocket->contentLength = up.contentLength; + phsSocket->pucData = up.pucBuffer; + } else { + if (phsSocket->flags & FLAG_CHUNK) { + iBytesWritten = send(phsSocket->socket, "0\r\n\r\n", 5, 0); + if (iBytesWritten<=0) return -1; + hp->stats.totalSentBytes+=iBytesWritten; + } + return 1; + } + } + return 0; +} // end of _mwSendRawDataChunk + +//////////////////////////////////////////////////////////////////////////// +// _mwRedirect +// Setup for redirect to another file +//////////////////////////////////////////////////////////////////////////// +void _mwRedirect(HttpSocket* phsSocket, char* pchPath) +{ + /* + char* path; + // raw (not file) data send mode + SETFLAG(phsSocket,FLAG_DATA_RAW); + // messages is HTML + phsSocket->response.fileType=HTTPFILETYPE_HTML; + + // build redirect message + SYSLOG(LOG_INFO,"[%d] Http redirection to %s\n",phsSocket->socket,pchPath); + path = (pchPath == (char*)phsSocket->pucData) ? strdup(pchPath) : (char*)pchPath; + phsSocket->contentLength=snprintf(phsSocket->pucData, 512, HTTPBODY_REDIRECT,path); + phsSocket->response.contentLength=phsSocket->contentLength; + if (path != pchPath) free(path); + */ + char* url = strdup(pchPath); + int n = snprintf(phsSocket->pucData, phsSocket->contentLength, HTTP301_HEADER, HTTP_SERVER_NAME, url); + free(url); + send(phsSocket->socket, phsSocket->pucData, n, 0); +} // end of _mwRedirect + +//////////////////////////////////////////////////////////////////////////// +// _mwStrStrNoCase +// Case insensitive version of ststr +//////////////////////////////////////////////////////////////////////////// +char* _mwStrStrNoCase(char* pchHaystack, char* pchNeedle) +{ + char* pchReturn=NULL; + + while(*pchHaystack!='\0' && pchReturn==NULL) { + if (toupper((int)*pchHaystack)==toupper((int)pchNeedle[0])) { + char* pchTempHay=pchHaystack; + char* pchTempNeedle=pchNeedle; + // start of match + while(*pchTempHay!='\0') { + if(toupper((int)*pchTempHay)!=toupper((int)*pchTempNeedle)) { + // failed to match + break; + } + pchTempNeedle++; + pchTempHay++; + if (*pchTempNeedle=='\0') { + // completed match + pchReturn=pchHaystack; + break; + } + } + } + pchHaystack++; + } + + return pchReturn; +} // end of _mwStrStrNoCase + +//////////////////////////////////////////////////////////////////////////// +// _mwDecodeCharacter +// Convert and individual character +//////////////////////////////////////////////////////////////////////////// +char _mwDecodeCharacter(char* s) +{ + register unsigned char v; + if (!*s) return 0; + if (*s>='a' && *s<='f') + v = *s-('a'-'A'+7); + else if (*s>='A' && *s<='F') + v = *s-7; + else + v = *s; + if (*(++s)==0) return v; + v <<= 4; + if (*s>='a' && *s<='f') + v |= (*s-('a'-'A'+7)) & 0xf; + else if (*s>='A' && *s<='F') + v |= (*s-7) & 0xf; + else + v |= *s & 0xf; + return v; +} // end of _mwDecodeCharacter + +//////////////////////////////////////////////////////////////////////////// +// _mwDecodeString +// This function converts URLd characters back to ascii. For example +// %3A is '.' +//////////////////////////////////////////////////////////////////////////// +void mwDecodeString(char* pchString) +{ + int bEnd=FALSE; + char* pchInput=pchString; + char* pchOutput=pchString; + + do { + switch (*pchInput) { + case '%': + if (*(pchInput+1)=='\0' || *(pchInput+2)=='\0') { + // something not right - terminate the string and abort + *pchOutput='\0'; + bEnd=TRUE; + } else { + *pchOutput=_mwDecodeCharacter(pchInput+1); + pchInput+=3; + } + break; + case '+': + *pchOutput=' '; + pchInput++; + break; + case '\0': + bEnd=TRUE; + // drop through + default: + // copy character + *pchOutput=*pchInput; + pchInput++; + } + pchOutput++; + } while (!bEnd); +} // end of mwDecodeString + +int mwGetContentType(const char *pchExtname) +{ + DWORD dwExt = 0; + // check type of file requested + if (pchExtname[1]=='\0') { + return HTTPFILETYPE_OCTET; + } else if (pchExtname[2]=='\0') { + memcpy(&dwExt, pchExtname, 2); + switch (GETDWORD(pchExtname) & 0xffdfdf) { + case FILEEXT_JS: return HTTPFILETYPE_JS; + case FILEEXT_TS: return HTTPFILETYPE_TS; + } + } else if (pchExtname[3]=='\0' || pchExtname[3]=='?') { + //identify 3-char file extensions + memcpy(&dwExt, pchExtname, sizeof(dwExt)); + switch (dwExt & 0xffdfdfdf) { + case FILEEXT_HTM: return HTTPFILETYPE_HTML; + case FILEEXT_XML: return HTTPFILETYPE_XML; + case FILEEXT_XSL: return HTTPFILETYPE_XML; + case FILEEXT_TEXT: return HTTPFILETYPE_TEXT; + case FILEEXT_XUL: return HTTPFILETYPE_XUL; + case FILEEXT_CSS: return HTTPFILETYPE_CSS; + case FILEEXT_PNG: return HTTPFILETYPE_PNG; + case FILEEXT_JPG: return HTTPFILETYPE_JPEG; + case FILEEXT_GIF: return HTTPFILETYPE_GIF; + case FILEEXT_SWF: return HTTPFILETYPE_SWF; + case FILEEXT_MPA: return HTTPFILETYPE_MPA; + case FILEEXT_MPG: return HTTPFILETYPE_MPEG; + case FILEEXT_AVI: return HTTPFILETYPE_AVI; + case FILEEXT_MP4: return HTTPFILETYPE_MP4; + case FILEEXT_MOV: return HTTPFILETYPE_MOV; + case FILEEXT_264: return HTTPFILETYPE_264; + case FILEEXT_FLV: return HTTPFILETYPE_FLV; + case FILEEXT_3GP: return HTTPFILETYPE_3GP; + case FILEEXT_ASF: return HTTPFILETYPE_ASF; + case FILEEXT_SDP: return HTTPFILETYPE_SDP; + } + } else if (pchExtname[4]=='\0' || pchExtname[4]=='?') { + memcpy(&dwExt, pchExtname, sizeof(dwExt)); + //logic-and with 0xdfdfdfdf gets the uppercase of 4 chars + switch (dwExt & 0xdfdfdfdf) { + case FILEEXT_HTML: return HTTPFILETYPE_HTML; + case FILEEXT_MPEG: return HTTPFILETYPE_MPEG; + case FILEEXT_M3U8: return HTTPFILETYPE_M3U8; + } + } + return HTTPFILETYPE_OCTET; +} + +int _mwGrabToken(char *pchToken, char chDelimiter, char *pchBuffer, int iMaxTokenSize) +{ + char *p=pchToken; + int iCharCopied=0; + + while (*p && *p!=chDelimiter && iCharCopied < iMaxTokenSize - 1) { + *(pchBuffer++)=*(p++); + iCharCopied++; + } + *pchBuffer='\0'; + return (*p==chDelimiter)?iCharCopied:0; +} + +int _mwParseHttpHeader(HttpSocket* phsSocket) +{ + int iLen; + char *p=phsSocket->buffer; //pointer to header data + HttpRequest *req=&phsSocket->request; + + CLRFLAG(phsSocket, FLAG_MULTIPART); + + p = strstr(phsSocket->buffer, "HTTP/1."); + if (!p) return -1; + p += 7; + req->iHttpVer = (*p - '0'); + //parse the rest of header + for(;;) { + //look for a valid field name + while (*p && *p!='\r') p++; //travel to '\r' + if (!*p || !memcmp(p, HTTP_HEADER_END, sizeof(HTTP_HEADER_END))) + break; + p+=2; //skip "\r\n" + + if (_mwStrHeadMatch(&p,"Connection: ")) { + if (_mwStrHeadMatch(&p,"close")) { + SETFLAG(phsSocket,FLAG_CONN_CLOSE); + } else if (_mwStrHeadMatch(&p,"Keep-Alive")) { + CLRFLAG(phsSocket,FLAG_CONN_CLOSE); //FIXME!! + } + } else if (_mwStrHeadMatch(&p, "Content-Length: ")) { + char buf[32]; + p+=_mwGrabToken(p,'\r',buf,sizeof(buf)); + phsSocket->request.payloadSize = atoi(buf); + } else if (_mwStrHeadMatch(&p, "CSeq: ")) { + phsSocket->request.iCSeq = atoi(p); + } else if (_mwStrHeadMatch(&p,"Referer: ")) { + phsSocket->request.pucReferer= p; + } else if (_mwStrHeadMatch(&p,"Range: bytes=")) { + char buf[32]; + int iEndByte; + iLen=_mwGrabToken(p,'-',buf,sizeof(buf)); + if (iLen==0) continue; + p+=iLen; + phsSocket->request.startByte=atoi(buf); + iLen=_mwGrabToken(p,'/',buf,sizeof(buf)); + if (iLen==0) continue; + p+=iLen; + iEndByte = atoi(buf); + if (iEndByte > 0) + phsSocket->response.contentLength = (int)(iEndByte-phsSocket->request.startByte+1); + } else if (_mwStrHeadMatch(&p,"Host: ")) { + phsSocket->request.pucHost = p; + } else if (_mwStrHeadMatch(&p,"Transport: ")) { + phsSocket->request.pucTransport = p; + } else if (_mwStrHeadMatch(&p,"Authorization: ")) { + phsSocket->request.pucAuthInfo = p; + } else if (_mwStrHeadMatch(&p,"X-Forwarded-For: ")) { + int i; + for (i = 3; i >= 0 && *p; i--) { + phsSocket->ipAddr.caddr[i] = atoi(p); + while (*p && *p != '\r' && *(p++) != '.'); + } + } + } + return 0; //end of header +} + +//////////////////////////////////////////////////////////////////////////// +// _mwCheckAuthentication +// Check if a connected peer is authenticated +//////////////////////////////////////////////////////////////////////////// +BOOL _mwCheckAuthentication(HttpParam *hp, HttpSocket* phsSocket) +{ + if (!ISFLAGSET(phsSocket,FLAG_AUTHENTICATION)) + return TRUE; + if (hp->dwAuthenticatedNode != phsSocket->ipAddr.laddr) { + // Not authenticated + hp->stats.authFailCount++; + return FALSE; + } + // Extend authentication period + hp->tmAuthExpireTime = time(NULL) + HTTPAUTHTIMEOUT; + return TRUE; +} + +//////////////////////////// END OF FILE /////////////////////////////////// diff --git a/esp32/libraries/httpd/httpd.h b/esp32/libraries/httpd/httpd.h new file mode 100644 index 0000000..f9b70fa --- /dev/null +++ b/esp32/libraries/httpd/httpd.h @@ -0,0 +1,336 @@ +/****************************************************************************** +* MiniWeb - a mini and high efficiency HTTP server implementation +* Distributed under BSD license +******************************************************************************/ + +#ifndef _HTTPAPI_H_ +#define _HTTPAPI_H_ + +#include +#include +#include +#include +#include "httppil.h" + +#define FD_ISSET_BOOL(n, p) ((FD_ISSET(n,p)) >> ((n) % NFDBITS)) + +#ifndef min +#define min(x,y) (x>y?y:x) +#endif + +#ifdef HTTP_DEBUG +#define DBG printf +#else +#define DBG +#endif +#define LOG_ERR 1 + +#define ASSERT +#define GETDWORD(ptrData) (*(DWORD*)(ptrData)) +#define SETDWORD(ptrData,data) (*(DWORD*)(ptrData)=data) +#define GETWORD(ptrData) (*(WORD*)(ptrData)) +#define SETWORD(ptrData,data) (*(WORD*)(ptrData)=data) +#ifndef BIG_ENDINE +#define DEFDWORD(char1,char2,char3,char4) ((char1)+((char2)<<8)+((char3)<<16)+((char4)<<24)) +#define DEFWORD(char1,char2) (char1+(char2<<8)) +#else +#define DEFDWORD(char1,char2,char3,char4) ((char4)+((char3)<<8)+((char2)<<16)+((char1)<<24)) +#define DEFWORD(char1,char2) (char2+(char1<<8)) +#endif + +/////////////////////////////////////////////////////////////////////// +// Public definitions +/////////////////////////////////////////////////////////////////////// + +#ifdef __cplusplus +extern "C" { +#endif + +// file types +typedef enum { + HTTPFILETYPE_UNKNOWN = 0, + HTTPFILETYPE_HTML, + HTTPFILETYPE_XML, + HTTPFILETYPE_TEXT, + HTTPFILETYPE_XUL, + HTTPFILETYPE_CSS, + HTTPFILETYPE_JS, + HTTPFILETYPE_PNG, + HTTPFILETYPE_JPEG, + HTTPFILETYPE_GIF, + HTTPFILETYPE_SWF, + HTTPFILETYPE_MPA, + HTTPFILETYPE_MPEG, + HTTPFILETYPE_AVI, + HTTPFILETYPE_MP4, + HTTPFILETYPE_MOV, + HTTPFILETYPE_264, + HTTPFILETYPE_FLV, + HTTPFILETYPE_TS, + HTTPFILETYPE_3GP, + HTTPFILETYPE_ASF, + HTTPFILETYPE_OCTET, + HTTPFILETYPE_STREAM, + HTTPFILETYPE_M3U8, + HTTPFILETYPE_SDP, + HTTPFILETYPE_HEX, + HTTPFILETYPE_JSON, +} HttpFileType; + +///////////////////////////////////////////////////////////////////////////// +// typedefs +///////////////////////////////////////////////////////////////////////////// + +#define FLAG_REQUEST_GET 0x1 +#define FLAG_REQUEST_POST 0x2 +#define FLAG_HEADER_SENT 0x80 +#define FLAG_CONN_CLOSE 0x100 +#define FLAG_ABSOLUTE_PATH 0x200 +#define FLAG_AUTHENTICATION 0x400 +#define FLAG_MORE_CONTENT 0x800 +#define FLAG_TO_FREE 0x1000 +#define FLAG_CHUNK 0x2000 +#define FLAG_CLOSE_CALLBACK 0x4000 + +#define FLAG_DATA_FILE 0x10000 +#define FLAG_DATA_RAW 0x20000 +#define FLAG_DATA_REDIRECT 0x80000 +#define FLAG_DATA_STREAM 0x100000 +#define FLAG_CUSTOM_HEADER 0x200000 +#define FLAG_MULTIPART 0x400000 + +#define FLAG_RECEIVING 0x40000000 +#define FLAG_SENDING 0x80000000 + +#define SETFLAG(hs,bit) (hs->flags|=(bit)); +#define CLRFLAG(hs,bit) (hs->flags&=~(bit)); +#define ISFLAGSET(hs,bit) (hs->flags&(bit)) + +typedef union { + unsigned long laddr; + unsigned short saddr[2]; + unsigned char caddr[4]; +} IPADDR; + +typedef struct { + int iHttpVer; + unsigned int startByte; + char *pucPath; + const char *pucReferer; + char* pucHost; + int headerSize; + char* pucPayload; + unsigned int payloadSize; + int iCSeq; + const char* pucTransport; + const char* pucAuthInfo; +} HttpRequest; + +typedef struct { + int statusCode; + int headerBytes; + int sentBytes; + unsigned int contentLength; + HttpFileType fileType; +} HttpResponse; + +typedef struct { + char *name; + char *value; +} HttpVariables; + +// Callback function protos +typedef int (*PFN_UDP_CALLBACK)(void* hp); + +typedef struct { + uint32_t reqCount; + size_t totalSentBytes; + uint32_t authFailCount; + uint16_t clientCount; + uint16_t clientCountMax; + uint16_t openedFileCount; +} HttpStats; + +#ifndef ARDUINO +#define HTTP_BUFFER_SIZE (64*1024 /*bytes*/) +#define MAX_POST_PAYLOAD_SIZE (128*1024 /*bytes*/) +#define HTTP_MAX_CLIENTS_DEFAULT 32 +#else +#define HTTP_BUFFER_SIZE (16*1024 /*bytes*/) +#define MAX_POST_PAYLOAD_SIZE (16*1024 /*bytes*/) +#define HTTP_MAX_CLIENTS_DEFAULT 16 +#endif + +// per connection/socket structure +typedef struct _HttpSocket{ + SOCKET socket; + IPADDR ipAddr; + HttpRequest request; + HttpResponse response; + char *pucData; + uint32_t bufferSize; // the size of buffer pucData pointing to + uint32_t contentLength; + FILE* fp; + uint32_t flags; + void* handler; // http handler function address + void* ptr; + time_t tmExpirationTime; + char* mimeType; + char* buffer; + uint16_t reqCount; +} HttpSocket; + +typedef enum { + JSON_TYPE_STRING = 0, + JSON_TYPE_DECIMAL, + JSON_TYPE_BOOLEAN, +} JSONValueType; + +typedef struct { + char* name; + char* value; + JSONValueType type; +} NameValuePair; + +typedef struct { + void* hp; + HttpSocket* hs; + const char *pucRequest; + HttpVariables* pxVars; + int iVarCount; + char *pucHeader; + char *pucBuffer; + unsigned int bufSize; + char *pucPayload; + unsigned int payloadSize; + unsigned int contentLength; + HttpFileType contentType; + NameValuePair* json; + int jsonPairCount; +} UrlHandlerParam; + +typedef int (*PFNURLCALLBACK)(UrlHandlerParam*); + +typedef struct { + const char* pchUrlPrefix; + PFNURLCALLBACK pfnUrlHandler; +} UrlHandler; + +#define AUTH_NO_NEED (0) +#define AUTH_SUCCESSED (1) +#define AUTH_REQUIRED (2) +#define AUTH_FAILED (-1) + +#define MAX_AUTH_INFO_LEN 64 + +typedef struct { + const char* pchUrlPrefix; + const char* pchUsername; + const char* pchPassword; + const char* pchOtherInfo; + char pchAuthString[MAX_AUTH_INFO_LEN]; +} AuthHandler; + +#define FLAG_DIR_LISTING 1 +#define FLAG_DISABLE_RANGE 2 + +typedef struct _httpParam { + HttpSocket* hsSocketQueue; /* socket queue*/ + uint16_t maxClients; + uint16_t maxClientsPerIP; + unsigned int flags; + SOCKET listenSocket; + SOCKET udpSocket; + uint16_t httpPort; + uint16_t udpPort; + char* pchWebPath; + UrlHandler *pxUrlHandler; /* pointer to URL handler array */ + AuthHandler *pxAuthHandler; /* pointer to authorization handler array */ + // incoming udp callback + PFN_UDP_CALLBACK pfnIncomingUDP; + // misc + DWORD dwAuthenticatedNode; + time_t tmAuthExpireTime; + HttpStats stats; + DWORD hlBindIP; + BOOL bKillWebserver; + BOOL bWebserverRunning; +} HttpParam; + +typedef struct { + const char* pchRootPath; + const char* pchHttpPath; + char cFilePath[MAX_PATH]; + char* pchExt; + int fTailSlash; +} HttpFilePath; + +/////////////////////////////////////////////////////////////////////// +// Return codes +/////////////////////////////////////////////////////////////////////// +// for post callback +#define WEBPOST_OK (0) +#define WEBPOST_AUTHENTICATED (1) +#define WEBPOST_NOTAUTHENTICATED (2) +#define WEBPOST_AUTHENTICATIONON (3) +#define WEBPOST_AUTHENTICATIONOFF (4) + +// for multipart file uploads +#define HTTPUPLOAD_MORECHUNKS (0) +#define HTTPUPLOAD_FIRSTCHUNK (1) +#define HTTPUPLOAD_LASTCHUNK (2) + +/////////////////////////////////////////////////////////////////////// +// Public functions +/////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////// +// mwInitParam. Init the context structure with default values +/////////////////////////////////////////////////////////////////////// +void mwInitParam(HttpParam* hp, int port, const char* pchWebPath); + +/////////////////////////////////////////////////////////////////////// +// mwServerStart. Startup the webserver +/////////////////////////////////////////////////////////////////////// +int mwServerStart(HttpParam* hp); +void mwServerExit(HttpParam* hp); + +/////////////////////////////////////////////////////////////////////// +// mwHttpLoop. Enter webserver loop +/////////////////////////////////////////////////////////////////////// +void mwHttpLoop(HttpParam *hp, uint32_t timeout); + +/////////////////////////////////////////////////////////////////////// +// mwServerShutdown. Shutdown the webserver (closes connections and +// releases resources) +/////////////////////////////////////////////////////////////////////// +int mwServerShutdown(HttpParam* hp); + +/////////////////////////////////////////////////////////////////////// +// mwSetRcvBufSize. Change the TCP windows size of acceped sockets +/////////////////////////////////////////////////////////////////////// +int mwSetRcvBufSize(WORD wSize); + +/////////////////////////////////////////////////////////////////////// +// Default subst, post and file-upload callback processing +/////////////////////////////////////////////////////////////////////// +int mwGetHttpDateTime(time_t tm, char *buf, int bufsize); +int mwGetLocalFileName(HttpFilePath* hfp); +char* mwGetVarValue(HttpVariables* vars, const char *varname, const char *defval); +int mwGetVarValueInt(HttpVariables* vars, const char *varname, int defval); +unsigned int mwGetVarValueHex(HttpVariables* vars, const char *varname, unsigned int defval); +int64_t mwGetVarValueInt64(HttpVariables* vars, const char *varname); +float mwGetVarValueFloat(HttpVariables* vars, const char *varname); +int mwParseQueryString(UrlHandlerParam* up); +int mwGetContentType(const char *pchExtname); +void mwDecodeString(char* s); +NameValuePair* mwGetJSONData(UrlHandlerParam* up, const char* name); +int mwParseJSONString(UrlHandlerParam* up); + +#ifdef __cplusplus +} +#endif + +#endif // _HTTPAPI_H + +////////////////////////// END OF FILE //////////////////////////////// diff --git a/esp32/libraries/httpd/httpint.h b/esp32/libraries/httpd/httpint.h new file mode 100644 index 0000000..c2f9d9a --- /dev/null +++ b/esp32/libraries/httpd/httpint.h @@ -0,0 +1,119 @@ +/****************************************************************************** +* MiniWeb http implementation internal header file +* Distributed under BSD license +******************************************************************************/ + +#ifndef _HTTPINT_H_ +#define _HTTPINT_H_ + +///////////////////////////////////////////////////////////////////////////// +// defines +///////////////////////////////////////////////////////////////////////////// + +// HTTP messages/part messages +#ifndef HTTP_SERVER_NAME +#define HTTP_SERVER_NAME "MiniWeb" +#endif +#define HTTP200_HEADER "%s %d %s\r\nServer: %s\r\nCache-control: no-cache\r\nConnection: %s\r\n" +#define HTTP200_HDR_EST_SIZE ((sizeof(HTTP200_HEADER)+256)&(-4)) +#define HTTP403_HEADER "HTTP/1.1 403 Forbidden\r\nContent-Length: 0\r\n\r\n" +#define HTTP404_HEADER "HTTP/1.1 404 Not Found\r\nServer: %s\r\nContent-Length: %d\r\nContent-Type: text/html\r\n\r\n" +#define HTTP404_BODY "404 Not Found

Not Found

The requested URL has no content.

" +#define HTTPBODY_REDIRECT "" +#define HTTP301_HEADER "HTTP/1.1 301 Moved Permanently\r\nServer: %s\r\nLocation: %s\r\nContent-Length: 0\r\n\r\n" +#define HTTP401_HEADER "HTTP/1.1 401 Authorization Required\r\nWWW-Authenticate: Basic realm=\"%s\"\r\nContent-Length: %d\r\nContent-Type: text/html\r\n\r\n" +//#define HTTP401_BODY "401 Authorization Required

Authorization Required

This server could not verify that you are authorized to access the resource requested

" +#define HTTP_CONTENTLENGTH "Content-Length:" +#define HTTP_MULTIPARTHEADER "multipart/form-data" +#define HTTP_MULTIPARTCONTENT "Content-Disposition: form-data; name=" +#define HTTP_MULTIPARTBOUNDARY "boundary=" +#define HTTP_FILENAME "filename=" +#define HTTP_HEADER_END "\r\n\r\n" +#define HTTP_SUBST_PATTERN (WORD)(('$' << 8) + '$') + +// Define file extensions +#define FILEEXT_HTM DEFDWORD('H','T','M',0) +#define FILEEXT_XML DEFDWORD('X','M','L',0) +#define FILEEXT_XSL DEFDWORD('X','S','L',0) +#define FILEEXT_TEXT DEFDWORD('T','X','T',0) +#define FILEEXT_XUL DEFDWORD('X','U','L',0) +#define FILEEXT_GIF DEFDWORD('G','I','F',0) +#define FILEEXT_JPG DEFDWORD('J','P','G',0) +#define FILEEXT_PNG DEFDWORD('P','N','G',0) +#define FILEEXT_CSS DEFDWORD('C','S','S',0) +#define FILEEXT_JS DEFDWORD('J','S',0,0) +#define FILEEXT_SWF DEFDWORD('S','W','F',0) +#define FILEEXT_HTML DEFDWORD('H','T','M','L') +#define FILEEXT_MPG DEFDWORD('M','P','G',0) +#define FILEEXT_MPEG DEFDWORD('M','P','E','G') +#define FILEEXT_MPA DEFDWORD('M','P','3' - 32,0) +#define FILEEXT_AVI DEFDWORD('A','V','I',0) +#define FILEEXT_MP4 DEFDWORD('M','P','4' - 32,0) +#define FILEEXT_MOV DEFDWORD('M','O','V',0) +#define FILEEXT_FLV DEFDWORD('F','L','V',0) +#define FILEEXT_3GP DEFDWORD('3' - 32, 'G','P',0) +#define FILEEXT_ASF DEFDWORD('A','S','F',0) +#define FILEEXT_264 DEFDWORD('2' - 32, '6' - 32, '4' - 32, 0) +#define FILEEXT_TS DEFDWORD('T', 'S', 0, 0) +#define FILEEXT_M3U8 DEFDWORD('M', '3' - 32, 'U', '8' - 32) +#define FILEEXT_SDP DEFDWORD('S', 'D', 'P', 0) + +// Settings for http server +#ifndef ARDUINO +#define HTTP_EXPIRATION_TIME (120/*secs*/) +#define HTTP_KEEPALIVE_TIME (15/*secs*/) +#define HTTP_KEEPALIVE_MAX (1000 /*requests*/) +#define MAX_REQUEST_PATH_LEN (512/*bytes*/) +#else +#define HTTP_EXPIRATION_TIME (30/*secs*/) +#define HTTP_KEEPALIVE_TIME (15/*secs*/) +#define HTTP_KEEPALIVE_MAX (100 /*requests*/) +#define MAX_REQUEST_PATH_LEN (128/*bytes*/) +#define MAX_OPEN_FILES 16 +#endif +#define MAX_RECV_RETRIES (3/*times*/) +#define HTTPAUTHTIMEOUT (60/*secs*/) +#define HTTPSUBSTEXPANSION (0/*bytes*/) +#define HTTPHEADERSIZE (512/*bytes*/) +#define HTTPMAXRECVBUFFER HTTP_BUFFER_SIZE +#define HTTPUPLOAD_CHUNKSIZE (HTTPMAXRECVBUFFER / 2/*bytes*/) +#define MAX_REQUEST_SIZE (2*1024 /*bytes*/) + +#define SLASH '/' + +#define LOG_INFO stdout +#define SYSLOG fprintf + +///////////////////////////////////////////////////////////////////////////// +// local helper function prototypes +///////////////////////////////////////////////////////////////////////////// +SOCKET _mwAcceptSocket(HttpParam* hp, struct sockaddr_in *sinaddr); +void _mwDenySocket(HttpParam* hp,struct sockaddr_in *sinaddr); +int _mwProcessReadSocket(HttpParam* hp, HttpSocket* phsSocket); +int _mwProcessWriteSocket(HttpParam *hp, HttpSocket* phsSocket); +void _mwCloseSocket(HttpParam* hp, HttpSocket* phsSocket); +int _mwStartSendFile(HttpParam* hp, HttpSocket* phsSocket); +int _mwSendFileChunk(HttpParam *hp, HttpSocket* phsSocket); +char* _mwStrStrNoCase(char* pchHaystack, char* pchNeedle); +void _mwRedirect(HttpSocket* phsSocket, char* pchFilename); +int _mwSendRawDataChunk(HttpParam *hp, HttpSocket* phsSocket); +int _mwStartSendRawData(HttpParam *hp, HttpSocket* phsSocket); +int _mwGetToken(char* pchBuffer,int iTokenNumber,char** ppchToken); +char _mwDecodeCharacter(char* pchEncodedChar); +int _mwLoadFileChunk(HttpParam *hp, HttpSocket* phsSocket); +BOOL _mwCheckAuthentication(HttpParam *hp, HttpSocket* phsSocket); +int _GetContentType(char *pchFilename); +int _mwCheckAccess(HttpSocket* phsSocket); +int _mwGetContentType(char *pchExtname); +int _mwSendHttpHeader(HttpSocket* phsSocket); +char* _mwStrDword(char* pchHaystack, DWORD dwSub, DWORD dwCharMask); +SOCKET _mwStartListening(HttpParam* hp); +int _mwParseHttpHeader(HttpSocket* phsSocket); +int _mwStrCopy(char *dest, const char *src); +int _mwStrHeadMatch(char** pbuf1, const char* buf2); +void _mwSetSocketOpts(SOCKET socket); +void _mwSendErrorPage(SOCKET socket, const char* header, const char* body); +void _mwCloseAllConnections(HttpParam* hp); +void _mwFreeJSONPairs(UrlHandlerParam* up); +#endif +////////////////////////// END OF FILE ////////////////////////////////////// diff --git a/esp32/libraries/httpd/httpjson.c b/esp32/libraries/httpd/httpjson.c new file mode 100644 index 0000000..7a79f13 --- /dev/null +++ b/esp32/libraries/httpd/httpjson.c @@ -0,0 +1,147 @@ +///////////////////////////////////////////////////////////////////////////// +// +// httpjson.c +// +// JSON parser +// +///////////////////////////////////////////////////////////////////////////// + +#ifndef WINCE +#include +#include +#include +#endif +#include +#include +#include +#include +#include "httpd.h" +#include "httpint.h" + +void _mwFreeJSONPairs(UrlHandlerParam* up) +{ + if (up->json) { + int i; + for (i = 0; i < up->jsonPairCount; i++) { + free(up->json[i].name); + free(up->json[i].value); + } + free(up->json); + up->json = 0; + up->jsonPairCount = 0; + } +} + +NameValuePair* mwGetJSONData(UrlHandlerParam* up, const char* name) +{ + int i; + for (i = 0; i < up->jsonPairCount; i++) { + if (!strcmp(up->json[i].name, name)) { + return up->json + i; + } + } + return 0; +} + +int mwParseJSONString(UrlHandlerParam* up) +{ + char *p = up->pucPayload; + if (!p) return 0; + + NameValuePair pair; + char keybase[256] = { 0 }; + int curLevel = 0; + _mwFreeJSONPairs(up); + + while (*p) { + switch (*p) { + case '{': + curLevel++; + p++; + break; + case '}': + curLevel--; + if (*keybase) { + char *q; + for (q = keybase + strlen(keybase) - 2; q >= keybase && *q != '.'; q--); + *(q + 1) = 0; + } + p++; + break; + case '\"': + { + char *q; + ; + if (!(q = strchr(p + 1, '\"'))) continue; + pair.name = p + 1; + if (!(p = strchr(q + 1, ':'))) continue; + while (*(++p) == ' ' || *p == '\r' || *p == '\n'); + int i = up->jsonPairCount; + if (*p == '\"') { + pair.type = JSON_TYPE_STRING; + pair.value = ++p; + if (!(p = strchr(p, '\"'))) continue; + *q = 0; + *p = 0; + up->jsonPairCount++; + up->json = realloc(up->json, sizeof(NameValuePair) * up->jsonPairCount); + up->json[i].name = malloc(strlen(keybase) + strlen(pair.name) + 1); + strcpy(up->json[i].name, keybase); + strcat(up->json[i].name, pair.name); + up->json[i].value = strdup(pair.value); + up->json[i].type = JSON_TYPE_STRING; + *q = '\"'; + *p = '\"'; + p++; + } + else if (*p == '{') { + // object + *q = 0; + if (strlen(keybase) + strlen(pair.name) + 1 < sizeof(keybase)) { + strcat(keybase, pair.name); + strcat(keybase, "."); + } + *q = '\"'; + } + else if (*p == '[') { + // array + p++; + } + else { + pair.value = p; + if (!strncmp(p, "true", 4)) { + p += 4; + if (isalpha((int)*p) || isdigit((int)*p)) continue; + pair.type = JSON_TYPE_BOOLEAN; + } + else if (!strncmp(p, "false", 5)) { + p += 5; + if (isalpha((int)*p) || isdigit((int)*p)) continue; + pair.type = JSON_TYPE_BOOLEAN; + } + else { + pair.type = JSON_TYPE_DECIMAL; + while (*(++p) == '.' || *p == '-' || isdigit((int)*p)); + } + char c = *p; + *q = 0; + *p = 0; + up->jsonPairCount++; + up->json = realloc(up->json, sizeof(NameValuePair) * up->jsonPairCount); + up->json[i].name = malloc(strlen(keybase) + strlen(pair.name) + 1); + strcpy(up->json[i].name, keybase); + strcat(up->json[i].name, pair.name); + up->json[i].value = strdup(pair.value); + up->json[i].type = JSON_TYPE_DECIMAL; + *q = '\"'; + *p = c; + } + } + break; + default: + p++; + break; + } + } + return up->jsonPairCount; +} \ No newline at end of file diff --git a/esp32/libraries/httpd/httppil.c b/esp32/libraries/httpd/httppil.c new file mode 100644 index 0000000..93e7afc --- /dev/null +++ b/esp32/libraries/httpd/httppil.c @@ -0,0 +1,169 @@ +/****************************************************************************** +* MiniWeb platform independent layer +* Distributed under BSD license +******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include "httppil.h" + +#ifndef WIN32 +#include +#include +#ifndef ESP8266 +#include +#include +#endif +#endif + +char *GetTimeString() +{ + static char buf[16]; + time_t tm=time(NULL); + memcpy(buf,ctime(&tm)+4,15); + buf[15]=0; + return buf; +} + +#if defined(ARDUINO) + +int IsDir(const char* pchName) +{ + return 0; +} + +int ReadDir(const char* pchDir, char* pchFileNameBuf) +{ + return 0; +} + +int IsFileExist(const char* filename) +{ + return 0; +} + +#else + +int IsDir(const char* pchName) +{ +#ifdef WIN32 + WIN32_FIND_DATA f; + HANDLE hFind = FindFirstFile(pchName, &f); + FindClose(hFind); + if (hFind != INVALID_HANDLE_VALUE && (f.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)) + return 1; + return 0; +#else + struct stat stDirInfo; + if (stat( pchName, &stDirInfo) < 0) return 0; + return S_ISDIR(stDirInfo.st_mode); +#endif +} + +int ReadDir(const char* pchDir, char* pchFileNameBuf) +{ +#if defined(WIN32) + static HANDLE hFind=NULL; + WIN32_FIND_DATA finddata; + + if (!pchFileNameBuf) { + if (hFind) { + FindClose(hFind); + hFind=NULL; + } + return 0; + } + if (pchDir) { + char *p; + int len; + if (!IsDir(pchDir)) return -1; + if (hFind) FindClose(hFind); + len = strlen(pchDir); + p = malloc(len + 5); + snprintf(p, len + 5, "%s\\*.*", pchDir); + hFind=FindFirstFile(p,&finddata); + free(p); + if (hFind==INVALID_HANDLE_VALUE) { + hFind=NULL; + return -1; + } + strcpy(pchFileNameBuf,finddata.cFileName); + return finddata.nFileSizeLow; + } + if (!hFind) return -1; + if (!FindNextFile(hFind,&finddata)) { + FindClose(hFind); + hFind=NULL; + return -1; + } + strcpy(pchFileNameBuf,finddata.cFileName); +#else + static DIR *stDirIn=NULL; + struct dirent *stFiles; + + if (!pchFileNameBuf) { + if (stDirIn) { + closedir(stDirIn); + stDirIn=NULL; + } + return 0; + } + if (pchDir) { + if (!IsDir(pchDir)) return -1; + if (stDirIn) closedir(stDirIn); + stDirIn = opendir( pchDir); + } + if (!stDirIn) return -1; + stFiles = readdir(stDirIn); + if (!stFiles) { + closedir(stDirIn); + stDirIn=NULL; + return -1; + } + strcpy(pchFileNameBuf, stFiles->d_name); +#endif + return 0; +} + +int IsFileExist(const char* filename) +{ +#ifdef WIN32 + WIN32_FIND_DATA f; + HANDLE hFind = FindFirstFile(filename, &f); + if (hFind == INVALID_HANDLE_VALUE) + return 0; + FindClose(hFind); + return 1; +#else + struct stat stat_ret; + if (stat(filename, &stat_ret) != 0) return 0; + + return S_ISREG(stat_ret.st_mode); +#endif +} + +#endif + +#ifndef WIN32 + +#ifndef ARDUINO +uint32_t GetTickCount() +{ + struct timeval ts; + gettimeofday(&ts,0); + return ts.tv_sec * 1000 + ts.tv_usec / 1000; +} +#endif + +uint64_t GetTickCount64() +{ + struct timeval ts; + gettimeofday(&ts, 0); + return (uint64_t)ts.tv_sec * 1000 + ts.tv_usec / 1000; +} +#endif diff --git a/esp32/libraries/httpd/httppil.h b/esp32/libraries/httpd/httppil.h new file mode 100644 index 0000000..1aa00ac --- /dev/null +++ b/esp32/libraries/httpd/httppil.h @@ -0,0 +1,137 @@ +/****************************************************************************** +* MiniWeb platform independent layer header file +* Distributed under BSD license +******************************************************************************/ + +#ifndef _HTTPPIL_H_ +#define _HTTPPIL_H_ + +#if defined(SYS_MINGW) && !defined(WIN32) +#define WIN32 +#endif + +#ifdef ARDUINO +#include +#endif + +#include + +#if defined(WIN32) +#include +#include + +#elif defined(ESP32) + +#include +#include +#include +#include "string.h" + +#elif defined(ESP8266) + +//#define LWIP_INTERNAL +#define LWIP_COMPAT_SOCKETS 1 + +#include +#include +#include +#include "lwip/opt.h" +#include "lwip/err.h" +#include "lwip/dns.h" + +#else + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if !defined(O_BINARY) +#define O_BINARY 0 +#endif +#endif + +#if defined(WIN32) + +#define ssize_t size_t +#define socklen_t int +#define open _open +#define read _read +#define write _write +#define close _close +#define lseek _lseek +#define strdup _strdup +#define dup2 _dup2 +#define dup _dup +#define pipe _pipe +#define spawnvpe _spawnvpe +#define spawnvp _spawnvp +#define atoll _atoi64 + +#else + +#ifndef ARDUINO +#define closesocket close +#endif + +#ifndef MAX_PATH +#define MAX_PATH 256 +#endif +#ifndef FALSE +#define FALSE 0 +#endif +#ifndef TRUE +#define TRUE 1 +#endif + +typedef int SOCKET; +typedef unsigned int DWORD; +typedef unsigned short int WORD; +typedef unsigned char BYTE; +#ifndef ESP8266 +typedef unsigned char BOOL; +#endif +#endif +typedef unsigned char OCTET; + +#if defined(_WIN32_WCE) || defined(WIN32) +#define msleep(ms) (Sleep(ms)) +#else +#define msleep(ms) (usleep(ms<<10)) +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +int InitSocket(); +void UninitSocket(); +char *GetTimeString(); +int ReadDir(const char* pchDir, char* pchFileNameBuf); +int IsFileExist(const char* filename); +int IsDir(const char* pchName); + +#ifdef WIN32 +#define S_ISREG(m) (((m) & S_IFMT) == S_IFREG) +#define S_ISDIR(m) (((m) & S_IFMT) == S_IFDIR) +#else +#ifdef ARDUINO +#define GetTickCount millis +#else +uint32_t GetTickCount(); +#endif +uint64_t GetTickCount64(); +#endif +#ifdef __cplusplus +} +#endif + +#endif diff --git a/esp32/telelogger/.vscode/extensions.json b/esp32/telelogger/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/esp32/telelogger/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/esp32/telelogger/dashboard/cross.png b/esp32/telelogger/dashboard/cross.png deleted file mode 100644 index b3829c9..0000000 Binary files a/esp32/telelogger/dashboard/cross.png and /dev/null differ diff --git a/esp32/telelogger/dashboard/dashboard.js b/esp32/telelogger/dashboard/dashboard.js deleted file mode 100644 index 4c39f96..0000000 --- a/esp32/telelogger/dashboard/dashboard.js +++ /dev/null @@ -1,126 +0,0 @@ -var source; -var origin; - -window.addEventListener("message", receiveMessage, false); - -function receiveMessage(event) { - //event.source.postMessage(event.data,event.origin); - source = event.source; - origin = event.origin; - processInput(event.data); -} - -function sendMessage(data) { - if (source) source.postMessage(data, origin); -} - -function checkData(data, key) -{ - var i; - if ((i = data.lastIndexOf(key)) >= 0) { - var j = data.indexOf("\r", i); - return j >= 0 ? data.substr(i + key.length, j - i - key.length) : data.substr(i + key.length); - } - return null; -} - -var imgTick = ""; -var imgCross = ""; - -var con = ""; -var inited = false; - -function processInput(data) -{ - var i; - var ret; - if (con.length > 1024) con = con.substr(512); - con += data; - if (!inited) { - if (ret = checkData(con, "FLASH:")) { - document.getElementById("flash_size").innerText = ret; - } - if (ret = checkData(con, "PSRAM:")) { - document.getElementById("psram_size").innerText = ret.substr(0, 2) == "E " ? "N/A" : ret; - } - if (ret = checkData(con, "TYPE:")) { - var type = parseInt(ret); - var typeName = "Unknown"; - if (type == 11 || type == 16) { - typeName = "Freematics ONE+ Model A"; - } else if (type >= 12 && type <= 14) { - typeName = "Freematics ONE+ Model B"; - } else if (type == 15) { - typeName = "Freematics ONE+ Model H"; - } - document.getElementById("devtype").innerText = typeName; - } - if (ret = checkData(con, "DEVICE ID:")) { - document.getElementById("devid").value = ret; - } - if (ret = checkData(con, "RTC:")) { - document.getElementById("rtc").innerText = ret; - } - if (ret = checkData(con, "SD:")) { - document.getElementById("sd_size").innerHTML = ret; - } - if (ret = checkData(con, "NO SD CARD") != null) { - document.getElementById("sd_size").innerHTML = "NO CARD"; - } - if (ret = checkData(con, "GNSS:")) { - document.getElementById("gps").innerHTML = ret.indexOf("NO") >= 0 ? imgCross : imgTick; - } - if (ret = checkData(con, "OBD:")) { - document.getElementById("obd").innerHTML = ret.indexOf("NO") >= 0 ? imgCross : imgTick; - } - if (ret = checkData(con, "MEMS:")) { - document.getElementById("mems").innerHTML = ret.indexOf("NO") >= 0 ? imgCross : (imgTick + " " + ret); - } - if (ret = checkData(con, "HTTPD:")) { - document.getElementById("wifi").innerHTML = ret.indexOf("NO") >= 0 ? imgCross : imgTick; - } - if (ret = checkData(con, "WiFi IP:")) { - document.getElementById("wifi").innerHTML = imgTick + " IP:" + ret; - } - if (ret = checkData(con, "IMEI:")) { - document.getElementById("imei").innerText = "IMEI:" + ret; - } - if (ret = checkData(con, "CELL:")) { - document.getElementById("cellinfo").innerHTML = ret == "NO" ? imgCross : (imgTick + " " + ret); - } - if ((ret = checkData(con, "NO SIM CARD")) != null) { - document.getElementById("imei").innerHTML = "| NO SIM CARD"; - } - if (ret = checkData(con, "Operator:")) { - document.getElementById("imei").innerText = "| " + ret; - } - if (ret = checkData(con, "Unable to connect") != null) { - document.getElementById("server").innerHTML = imgCross; - } - if (ret = checkData(con, "LOGIN")) { - document.getElementById("server").innerText = "Connecting to server" + ret; - } - } - if (ret = checkData(con, "[NET]")) { - document.getElementById("server").innerText = ret; - inited = true; - } - if (ret = checkData(con, "[BUF]")) { - document.getElementById("buffer").innerText = ret; - } - if (ret = checkData(con, "[FILE]")) { - document.getElementById("file").innerText = ret; - } - if (ret = checkData(con, "[GPS]")) { - document.getElementById("gps").innerText = ret; - } - if (ret = checkData(con, "[WIFI]")) { - document.getElementById("wifi").innerText = ret; - } - if (ret = checkData(con, "[CELL]")) { - document.getElementById("cell").innerText = ret; - } - if (ret = checkData(con, "RSSI:")) { - document.getElementById("rssi").innerText = "| RSSI:" + ret; - } -} diff --git a/esp32/telelogger/dashboard/index.html b/esp32/telelogger/dashboard/index.html deleted file mode 100644 index 89248b6..0000000 --- a/esp32/telelogger/dashboard/index.html +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - -
    -
  • Device ID:
    • -
  • Device Type:
    • -
  • FLASH:
    • -
  • PSRAM:
    • -
  • SD Card:
    • -
  • Motion Sensor:
    • -
  • GNSS:
    • -
  • OBD:
    • -
  • Cellular Module:
    • -
  • Cellular:
    • -
  • Wi-Fi:
    • -
  • Buffer:
    • -
  • Server:
    • -
  • File:
    • -
- - - diff --git a/esp32/telelogger/dashboard/tick.png b/esp32/telelogger/dashboard/tick.png deleted file mode 100644 index b7eed91..0000000 Binary files a/esp32/telelogger/dashboard/tick.png and /dev/null differ diff --git a/esp32/telelogger/platformio.ini b/esp32/telelogger/platformio.ini index 8b13c72..923ee67 100644 --- a/esp32/telelogger/platformio.ini +++ b/esp32/telelogger/platformio.ini @@ -31,4 +31,4 @@ board_build.partitions = huge_app.csv src_dir=. [env] -lib_extra_dirs=../../libraries +lib_extra_dirs=../libraries