cyberes
/
freematics-traccar-encrypted
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 4 Wiki Activity

add libraries

This commit is contained in:
Cyberes 2024-06-30 18:59:23 -06:00
parent 56a4e0fac9
commit 3033e73840
50 changed files with 15148 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

787
esp32/libraries/FreematicsOLED/FreematicsOLED.cpp Normal file
View File

@ -0,0 +1,787 @@
/*************************************************************************
* Simple OLED Text & Bitmap Display Library for Arduino
* Distributed under BSD
* (C)2013-2018 Stanley Huang <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#include <Wire.h>
#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();
}

63
esp32/libraries/FreematicsOLED/FreematicsOLED.h Normal file
View File

@ -0,0 +1,63 @@
/*************************************************************************
* Simple OLED Text & Bitmap Display Library for Arduino
* Distributed under BSD
* (C)2013-2018 Stanley Huang <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
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;
};

65
esp32/libraries/FreematicsONE/FreematicsBase.h Normal file
View File

@ -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 <support@freematics.com.au>
*************************************************************************/
#ifndef FREEMATICS_BASE
#define FREEMATICS_BASE
#include <Arduino.h>
// 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

643
esp32/libraries/FreematicsONE/FreematicsMEMS.cpp Normal file
View File

@ -0,0 +1,643 @@
/*************************************************************************
* Freematics MPU6050 helper class
* Distributed under BSD license
* Visit http://freematics.com for more information
* (C)2016-2018 Stanley Huang <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#include <Wire.h>
#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<<FS); // Set full scale range for the gyro to 250 dps
writeByte(ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
writeByte(ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
readBytes(ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
readBytes(GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}
for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
aAvg[ii] /= 200;
gAvg[ii] /= 200;
}
// Configure the accelerometer for self-test
writeByte(ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
writeByte(GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
delay(25); // Delay a while to let the device stabilize
for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
readBytes(ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
readBytes(GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}
for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
aSTAvg[ii] /= 200;
gSTAvg[ii] /= 200;
}
// Configure the gyro and accelerometer for normal operation
writeByte(ACCEL_CONFIG, 0x00);
writeByte(GYRO_CONFIG, 0x00);
delay(25); // Delay a while to let the device stabilize
// Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
selfTest[0] = readByte(SELF_TEST_X_ACCEL); // X-axis accel self-test results
selfTest[1] = readByte(SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
selfTest[2] = readByte(SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
selfTest[3] = readByte(SELF_TEST_X_GYRO); // X-axis gyro self-test results
selfTest[4] = readByte(SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
selfTest[5] = readByte(SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
// Retrieve factory self-test value from self-test code reads
factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
// To get percent, must multiply by 100
for (int i = 0; i < 3; i++) {
destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences
destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences
}
}
// Wire.h read and write protocols
void MPU9250_ACC::writeByte(uint8_t subAddress, uint8_t data)
{
Wire.beginTransmission(MPU9250_ADDRESS); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}
uint8_t MPU9250_ACC::readByte(uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
Wire.beginTransmission(MPU9250_ADDRESS); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom((uint8_t)MPU9250_ADDRESS, (uint8_t) 1); // Read one byte from slave register address
data = Wire.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
void MPU9250_ACC::readBytes(uint8_t subAddress, uint8_t count, uint8_t * dest)
{
Wire.beginTransmission(MPU9250_ADDRESS); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom((uint8_t)MPU9250_ADDRESS, count); // Read bytes from slave register address
uint8_t i = 0;
while (Wire.available() && i < count) {
dest[i++] = Wire.read();
}
}
void MPU9250_9DOF::writeByteAK(uint8_t subAddress, uint8_t data)
{
Wire.beginTransmission(MPU9250_ADDRESS); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}
uint8_t MPU9250_9DOF::readByteAK(uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
Wire.beginTransmission(AK8963_ADDRESS); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom((uint8_t)AK8963_ADDRESS, (uint8_t) 1); // Read one byte from slave register address
data = Wire.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
void MPU9250_9DOF::readBytesAK(uint8_t subAddress, uint8_t count, uint8_t * dest)
{
Wire.beginTransmission(AK8963_ADDRESS); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom((uint8_t)AK8963_ADDRESS, count); // Read bytes from slave register address
uint8_t i = 0;
while (Wire.available() && i < count) {
dest[i++] = Wire.read();
}
}
void MPU9250_ACC::initMPU9250()
{
// 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_ACC::begin(bool fusion)
{
Wire.begin();
Wire.setClock(400000);
//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) return 0;
initMPU9250();
return (c == 0x71) ? 2 : 1;
}
bool MPU9250_ACC::read(float* acc, float* gyr, float* mag, int16_t* 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 (temp) {
int t = readTempData();
*temp = (float)t / 33.387 + 210;
}
return true;
}
byte MPU9250_9DOF::begin(bool fusion)
{
byte ret = 0;
Wire.begin();
Wire.setClock(400000);
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
initMPU9250();
initAK8963(magCalibration);
ret = (c == 0x71) ? 2 : 1;
break;
}
if (ret && fusion && !quaterion) {
quaterion = new CQuaterion;
}
return ret;
}
bool MPU9250_9DOF::read(float* acc, float* gyr, float* mag, int16_t* 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
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 / 33.387 + 210;
}
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;
}

285
esp32/libraries/FreematicsONE/FreematicsMEMS.h Normal file
View File

@ -0,0 +1,285 @@
/*************************************************************************
* Freematics MPU6050 helper class
* Distributed under BSD license
* Visit http://freematics.com for more information
* (C)2016-2018 Stanley Huang <stanley@freematics.com.au>
*************************************************************************/
#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

713
esp32/libraries/FreematicsONE/FreematicsNetwork.cpp Normal file
View File

@ -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 <stanley@freematics.com.au
*************************************************************************/
#include <Arduino.h>
#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;
}
}

97
esp32/libraries/FreematicsONE/FreematicsNetwork.h Normal file
View File

@ -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 <stanley@freematics.com.au
*************************************************************************/
#pragma once
#include <Arduino.h>
#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;
};

737
esp32/libraries/FreematicsONE/FreematicsONE.cpp Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#include <SPI.h>
#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));
}

200
esp32/libraries/FreematicsONE/FreematicsONE.h Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#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);
};

58
esp32/libraries/FreematicsONE/keywords.txt Normal file
View File

@ -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

18
esp32/libraries/FreematicsONE/library.json Normal file
View File

@ -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": "*"
}

10
esp32/libraries/FreematicsONE/library.properties Normal file
View File

@ -0,0 +1,10 @@
name=FreematicsONE
version=1.0
author=Stanley Huang <support@freematics.com.au>
maintainer=Stanley Huang <support@freematics.com.au>
sentence=Arduino library for Freematics ONE
paragraph=Arduino library for Freematics ONE
category=Communication
url=http://freematics.com
architectures=avr
includes=FreematicsONE.h

96
esp32/libraries/FreematicsPlus/FreematicsBase.h Normal file
View File

@ -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 <stanley@freematics.com.au
*************************************************************************/
#ifndef FREEMATICS_BASE
#define FREEMATICS_BASE
#include <Arduino.h>
// 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

450
esp32/libraries/FreematicsPlus/FreematicsGPS.cpp Normal file
View File

@ -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;

145
esp32/libraries/FreematicsPlus/FreematicsGPS.h Normal file
View File

@ -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

1563
esp32/libraries/FreematicsPlus/FreematicsMEMS.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

422
esp32/libraries/FreematicsPlus/FreematicsMEMS.h Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#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

936
esp32/libraries/FreematicsPlus/FreematicsNetwork.cpp Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#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;
}
}

159
esp32/libraries/FreematicsPlus/FreematicsNetwork.h Normal file
View File

@ -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 <Arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#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

562
esp32/libraries/FreematicsPlus/FreematicsOBD.cpp Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#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);
}

87
esp32/libraries/FreematicsPlus/FreematicsOBD.h Normal file
View File

@ -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 <support@freematics.com.au
*************************************************************************/
#ifndef FREEMATICS_OBD
#define FREEMATICS_OBD
#include "utility/OBD.h"
#define OBD_TIMEOUT_SHORT 1000 /* ms */
#define OBD_TIMEOUT_LONG 10000 /* ms */
int dumpLine(char* buffer, int len);
uint16_t hex2uint16(const char *p);
byte hex2uint8(const char *p);
class COBD
{
public:
void begin(CLink* link) { this->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

971
esp32/libraries/FreematicsPlus/FreematicsPlus.cpp Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#include <Arduino.h>
#include <SPI.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#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;
}

187
esp32/libraries/FreematicsPlus/FreematicsPlus.h Normal file
View File

@ -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 <stanley@freematics.com.au>
*************************************************************************/
#ifndef FREEMATICS_PLUS
#define FREEMATICS_PLUS
#include <Arduino.h>
#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

51
esp32/libraries/FreematicsPlus/keywords.txt Normal file
View File

@ -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

18
esp32/libraries/FreematicsPlus/library.json Normal file
View File

@ -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": "*"
}

10
esp32/libraries/FreematicsPlus/library.properties Normal file
View File

@ -0,0 +1,10 @@
name=FreematicsPlus
version=1.1
author=Stanley Huang <support@freematics.com.au>
maintainer=Stanley Huang <support@freematics.com.au>
sentence=Arduino library for Freematics ONE+
paragraph=Arduino library for Freematics ONE+
category=Communication
url=https://freematics.com
architectures=esp32
includes=FreematicsPlus.h

19
esp32/libraries/FreematicsPlus/utility/AK09916_ENUMERATIONS.h Normal file
View File

@ -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_

78
esp32/libraries/FreematicsPlus/utility/AK09916_REGISTERS.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef _AK09916_REGISTERS_H_
#define _AK09916_REGISTERS_H_
#include <stdint.h>
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_

556
esp32/libraries/FreematicsPlus/utility/ICM_20948_C.c Normal file
View File

@ -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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
reg.ACCEL_FS_SEL = fss.a;
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, sizeof(ICM_20948_ACCEL_CONFIG_t));
reg.ACCEL_DLPFCFG = cfg.a;
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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*)&reg, 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;
}

237
esp32/libraries/FreematicsPlus/utility/ICM_20948_C.h Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#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_ */

263
esp32/libraries/FreematicsPlus/utility/ICM_20948_ENUMERATIONS.h Normal file
View File

@ -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_ */

719
esp32/libraries/FreematicsPlus/utility/ICM_20948_REGISTERS.h Normal file
View File

@ -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 <stdint.h>
#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_ */

66
esp32/libraries/FreematicsPlus/utility/ICM_42627.h Normal file
View File

@ -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

77
esp32/libraries/FreematicsPlus/utility/OBD.h Normal file
View File

@ -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;

441
esp32/libraries/FreematicsPlus/utility/ble_spp_server.c Normal file
View File

@ -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;
}

52
esp32/libraries/FreematicsPlus/utility/ble_spp_server.h Normal file
View File

@ -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);

456
esp32/libraries/TinyGPS/TinyGPS.cpp Normal file
View File

@ -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;

148
esp32/libraries/TinyGPS/TinyGPS.h Normal file
View File

@ -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

1784
esp32/libraries/httpd/httpd.c Normal file
View File

File diff suppressed because it is too large Load Diff

336
esp32/libraries/httpd/httpd.h Normal file
View File

@ -0,0 +1,336 @@
/******************************************************************************
* MiniWeb - a mini and high efficiency HTTP server implementation
* Distributed under BSD license
******************************************************************************/
#ifndef _HTTPAPI_H_
#define _HTTPAPI_H_
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#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 ////////////////////////////////

119
esp32/libraries/httpd/httpint.h Normal file
View File

@ -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 "<html><head><title>404 Not Found</title></head><body><h1>Not Found</h1><p>The requested URL has no content.</p></body></html>"
#define HTTPBODY_REDIRECT "<html><head><meta http-equiv=\"refresh\" content=\"0; URL=%s\"></head><body></body></html>"
#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 "<html><head><title>401 Authorization Required</title></head><body><h1>Authorization Required</h1><p>This server could not verify that you are authorized to access the resource requested</p></body></html>"
#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 //////////////////////////////////////

147
esp32/libraries/httpd/httpjson.c Normal file
View File

@ -0,0 +1,147 @@
/////////////////////////////////////////////////////////////////////////////
//
// httpjson.c
//
// JSON parser
//
/////////////////////////////////////////////////////////////////////////////
#ifndef WINCE
#include <fcntl.h>
#include <errno.h>
#include <sys/stat.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#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;
}

169
esp32/libraries/httpd/httppil.c Normal file
View File

@ -0,0 +1,169 @@
/******************************************************************************
* MiniWeb platform independent layer
* Distributed under BSD license
******************************************************************************/
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <ctype.h>
#include <fcntl.h>
#include "httppil.h"
#ifndef WIN32
#include <sys/types.h>
#include <unistd.h>
#ifndef ESP8266
#include <sys/time.h>
#include <dirent.h>
#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

137
esp32/libraries/httpd/httppil.h Normal file
View File

@ -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 <Arduino.h>
#endif
#include <stdint.h>
#if defined(WIN32)
#include <windows.h>
#include <io.h>
#elif defined(ESP32)
#include <lwip/err.h>
#include <lwip/sockets.h>
#include <lwip/netdb.h>
#include "string.h"
#elif defined(ESP8266)
//#define LWIP_INTERNAL
#define LWIP_COMPAT_SOCKETS 1
#include <lwip/err.h>
#include <lwip/sockets.h>
#include <lwip/netdb.h>
#include "lwip/opt.h"
#include "lwip/err.h"
#include "lwip/dns.h"
#else
#include <stdlib.h>
#include <unistd.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <sys/select.h>
#include <sys/stat.h>
#include <netdb.h>
#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

10
esp32/telelogger/.vscode/extensions.json vendored Normal file
View File

@ -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"
]
}

BIN
esp32/telelogger/dashboard/cross.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.5 KiB

126
esp32/telelogger/dashboard/dashboard.js
View File

@ -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 = "<img src='tick.png'/>";
var imgCross = "<img src='cross.png'/>";
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;
}
}

33
esp32/telelogger/dashboard/index.html
View File

@ -1,33 +0,0 @@
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8" />
<style type="text/css">
body{ margin:0; padding:0; border:none;font-size:16px; color:#3f3c8e; background:#dce4ed; font-family: Verdana, Geneva, Tahoma, sans-serif}
a:link, a:visited{ color:#fff; text-decoration:underline;}
a:hover, a:active{ color:#FFC; text-decoration:underline;}
input { font-size: 16px; text-align:center; }
span { font-weight:bold; }
li { line-height: 28px; }
</style>
</head>
<body>
<ul>
<li>Device ID: <input type="text" id="devid" readonly /></li>
<li>Device Type: <span id="devtype"></span></li>
<li>FLASH: <span id="flash_size"></span></li>
<li>PSRAM: <span id="psram_size"></span></li>
<li>SD Card: <span id="sd_size"></span></li>
<li>Motion Sensor: <span id="mems"></span></li>
<li>GNSS: <span id="gps"></span></li>
<li>OBD: <span id="obd"></span></li>
<li>Cellular Module: <span id="cellinfo"></span> <span id="imei"></span></span></li>
<li>Cellular: <span id="cell"></span> <span id="rssi"></span></span></li>
<li>Wi-Fi: <span id="wifi"></span></li>
<li>Buffer: <span id="buffer"></span></li>
<li>Server: <span id="server"></span></li>
<li>File: <span id="file"></span></li>
</ul>
<script language="javascript" src="dashboard.js"></script>
</body>
</html>

BIN
esp32/telelogger/dashboard/tick.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.5 KiB

2
esp32/telelogger/platformio.ini
View File

@ -31,4 +31,4 @@ board_build.partitions = huge_app.csv
src_dir=.
[env]
lib_extra_dirs=../../libraries
lib_extra_dirs=../libraries