add libraries
This commit is contained in:
parent
56a4e0fac9
commit
3033e73840
|
@ -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();
|
||||||
|
}
|
|
@ -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;
|
||||||
|
};
|
||||||
|
|
|
@ -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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
};
|
|
@ -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));
|
||||||
|
}
|
|
@ -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);
|
||||||
|
};
|
|
@ -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
|
|
@ -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": "*"
|
||||||
|
}
|
|
@ -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
|
|
@ -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
|
|
@ -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;
|
|
@ -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
|
File diff suppressed because it is too large
Load Diff
|
@ -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
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
|
@ -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
|
|
@ -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);
|
||||||
|
}
|
|
@ -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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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
|
|
@ -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
|
|
@ -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": "*"
|
||||||
|
}
|
|
@ -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
|
|
@ -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_
|
|
@ -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_
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,556 @@
|
||||||
|
#include "ICM_20948_C.h"
|
||||||
|
#include "ICM_20948_REGISTERS.h"
|
||||||
|
#include "AK09916_REGISTERS.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
const ICM_20948_Serif_t NullSerif = {
|
||||||
|
NULL, // write
|
||||||
|
NULL, // read
|
||||||
|
NULL, // user
|
||||||
|
};
|
||||||
|
|
||||||
|
// Private function prototypes
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Function definitions
|
||||||
|
ICM_20948_Status_e ICM_20948_link_serif( ICM_20948_Device_t* pdev, const ICM_20948_Serif_t* s ){
|
||||||
|
if(s == NULL){ return ICM_20948_Stat_ParamErr; }
|
||||||
|
if(pdev == NULL){ return ICM_20948_Stat_ParamErr; }
|
||||||
|
pdev->_serif = s;
|
||||||
|
return ICM_20948_Stat_Ok;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_execute_w( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ){
|
||||||
|
if( pdev->_serif->write == NULL ){ return ICM_20948_Stat_NotImpl; }
|
||||||
|
return (*pdev->_serif->write)( regaddr, pdata, len, pdev->_serif->user );
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_execute_r( ICM_20948_Device_t* pdev, uint8_t regaddr, uint8_t* pdata, uint32_t len ){
|
||||||
|
if( pdev->_serif->read == NULL ){ return ICM_20948_Stat_NotImpl; }
|
||||||
|
return (*pdev->_serif->read)( regaddr, pdata, len, pdev->_serif->user );
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Single-shot I2C on Master IF
|
||||||
|
ICM_20948_Status_e ICM_20948_i2c_master_slv4_txn( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data, uint8_t len, bool Rw, bool send_reg_addr ){
|
||||||
|
// Thanks MikeFair! // https://github.com/kriswiner/MPU9250/issues/86
|
||||||
|
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
addr = (((Rw) ? 0x80 : 0x00) | addr );
|
||||||
|
|
||||||
|
retval = ICM_20948_set_bank( pdev, 3 );
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_ADDR, (uint8_t*)&addr, 1 );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
retval = ICM_20948_set_bank( pdev, 3 );
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_REG, (uint8_t*)®, 1 );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
ICM_20948_I2C_SLV4_CTRL_t ctrl;
|
||||||
|
ctrl.EN = 1;
|
||||||
|
ctrl.INT_EN = false;
|
||||||
|
ctrl.DLY = 0;
|
||||||
|
ctrl.REG_DIS = !send_reg_addr;
|
||||||
|
|
||||||
|
// ICM_20948_I2C_MST_STATUS_t i2c_mst_status;
|
||||||
|
// bool txn_failed = false;
|
||||||
|
uint16_t nByte = 0;
|
||||||
|
|
||||||
|
while( nByte < len ){
|
||||||
|
if( !Rw ){
|
||||||
|
retval = ICM_20948_set_bank( pdev, 3 );
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_DO, (uint8_t*)&(data[nByte]), 1 );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
}
|
||||||
|
|
||||||
|
// Kick off txn
|
||||||
|
retval = ICM_20948_set_bank( pdev, 3 );
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_SLV4_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_SLV4_CTRL_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
// // long tsTimeout = millis() + 3000; // Emergency timeout for txn (hard coded to 3 secs)
|
||||||
|
// uint32_t max_cycles = 1000;
|
||||||
|
// uint32_t count = 0;
|
||||||
|
// bool slave4Done = false;
|
||||||
|
// while (!slave4Done) {
|
||||||
|
// retval = ICM_20948_set_bank( pdev, 0 );
|
||||||
|
// retval = ICM_20948_execute_r( pdev, AGB0_REG_I2C_MST_STATUS, &i2c_mst_status, 1 );
|
||||||
|
|
||||||
|
// slave4Done = ( i2c_mst_status.I2C_SLV4_DONE /*| (millis() > tsTimeout) */ ); // todo: avoid forever-loops
|
||||||
|
// slave4Done |= (count >= max_cycles);
|
||||||
|
// count++;
|
||||||
|
// }
|
||||||
|
// txn_failed = (i2c_mst_status.I2C_SLV4_NACK /* & (1 << I2C_SLV4_NACK_BIT)) | (millis() > tsTimeout) */);
|
||||||
|
// txn_failed |= (count >= max_cycles);
|
||||||
|
// if (txn_failed) break;
|
||||||
|
|
||||||
|
// if ( Rw ){
|
||||||
|
// retval = ICM_20948_set_bank( pdev, 3 );
|
||||||
|
// retval = ICM_20948_execute_r( pdev, AGB3_REG_I2C_SLV4_DI, &data[nByte], 1 );
|
||||||
|
// }
|
||||||
|
|
||||||
|
nByte++;
|
||||||
|
}
|
||||||
|
// if( txn_failed ){ return ICM_20948_Stat_Err; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_i2c_master_single_w( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ){
|
||||||
|
return ICM_20948_i2c_master_slv4_txn( pdev, addr, reg, data, 1, false, true );
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_i2c_master_single_r( ICM_20948_Device_t* pdev, uint8_t addr, uint8_t reg, uint8_t* data ){
|
||||||
|
return ICM_20948_i2c_master_slv4_txn( pdev, addr, reg, data, 1, true, true );
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_set_bank( ICM_20948_Device_t* pdev, uint8_t bank ){
|
||||||
|
if( bank > 3 ){ return ICM_20948_Stat_ParamErr; } // Only 4 possible banks
|
||||||
|
bank = (bank << 4) & 0x30; // bits 5:4 of REG_BANK_SEL
|
||||||
|
return ICM_20948_execute_w( pdev, REG_BANK_SEL, &bank, 1 );
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_sw_reset( ICM_20948_Device_t* pdev ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
ICM_20948_PWR_MGMT_1_t reg;
|
||||||
|
|
||||||
|
ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
reg.DEVICE_RESET = 1;
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_sleep ( ICM_20948_Device_t* pdev, bool on ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
ICM_20948_PWR_MGMT_1_t reg;
|
||||||
|
|
||||||
|
ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
if(on){ reg.SLEEP = 1; }
|
||||||
|
else{ reg.SLEEP = 0; }
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_low_power ( ICM_20948_Device_t* pdev, bool on ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
ICM_20948_PWR_MGMT_1_t reg;
|
||||||
|
|
||||||
|
ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
if(on){ reg.LP_EN = 1; }
|
||||||
|
else{ reg.LP_EN = 0; }
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_set_clock_source ( ICM_20948_Device_t* pdev, ICM_20948_PWR_MGMT_1_CLKSEL_e source ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
ICM_20948_PWR_MGMT_1_t reg;
|
||||||
|
|
||||||
|
ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
reg.CLKSEL = source;
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_PWR_MGMT_1, (uint8_t*)®, sizeof(ICM_20948_PWR_MGMT_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_get_who_am_i( ICM_20948_Device_t* pdev, uint8_t* whoami ){
|
||||||
|
if( whoami == NULL ){ return ICM_20948_Stat_ParamErr; }
|
||||||
|
ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
return ICM_20948_execute_r( pdev, AGB0_REG_WHO_AM_I, whoami, 1 );
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_check_id( ICM_20948_Device_t* pdev ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
uint8_t whoami = 0x00;
|
||||||
|
retval = ICM_20948_get_who_am_i( pdev, &whoami );
|
||||||
|
if( retval != ICM_20948_Stat_Ok){ return retval; }
|
||||||
|
if( whoami != ICM_20948_WHOAMI ){ return ICM_20948_Stat_WrongID; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_data_ready( ICM_20948_Device_t* pdev ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
ICM_20948_INT_STATUS_1_t reg;
|
||||||
|
retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
if( retval != ICM_20948_Stat_Ok){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_STATUS_1, (uint8_t*)®, sizeof(ICM_20948_INT_STATUS_1_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok){ return retval; }
|
||||||
|
if( !reg.RAW_DATA_0_RDY_INT ){ retval = ICM_20948_Stat_NoData; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Interrupt Configuration
|
||||||
|
ICM_20948_Status_e ICM_20948_int_pin_cfg ( ICM_20948_Device_t* pdev, ICM_20948_INT_PIN_CFG_t* write, ICM_20948_INT_PIN_CFG_t* read ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
if( write != NULL ){ // write first, if available
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)write, sizeof(ICM_20948_INT_PIN_CFG_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
}
|
||||||
|
if( read != NULL ){ // then read, to allow for verification
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)read, sizeof(ICM_20948_INT_PIN_CFG_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
}
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_int_enable ( ICM_20948_Device_t* pdev, ICM_20948_INT_enable_t* write, ICM_20948_INT_enable_t* read ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
ICM_20948_INT_ENABLE_t en_0;
|
||||||
|
ICM_20948_INT_ENABLE_1_t en_1;
|
||||||
|
ICM_20948_INT_ENABLE_2_t en_2;
|
||||||
|
ICM_20948_INT_ENABLE_3_t en_3;
|
||||||
|
|
||||||
|
retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
|
||||||
|
if( write != NULL ){ // If the write pointer is not NULL then write to the registers BEFORE reading
|
||||||
|
en_0.I2C_MST_INT_EN = write->I2C_MST_INT_EN;
|
||||||
|
en_0.DMP_INT1_EN = write->DMP_INT1_EN;
|
||||||
|
en_0.PLL_READY_EN = write->PLL_RDY_EN;
|
||||||
|
en_0.WOM_INT_EN = write->WOM_INT_EN;
|
||||||
|
en_0.REG_WOF_EN = write->REG_WOF_EN;
|
||||||
|
en_1.RAW_DATA_0_RDY_EN = write->RAW_DATA_0_RDY_EN;
|
||||||
|
en_2.individual.FIFO_OVERFLOW_EN_4 = write->FIFO_OVERFLOW_EN_4;
|
||||||
|
en_2.individual.FIFO_OVERFLOW_EN_3 = write->FIFO_OVERFLOW_EN_3;
|
||||||
|
en_2.individual.FIFO_OVERFLOW_EN_2 = write->FIFO_OVERFLOW_EN_2;
|
||||||
|
en_2.individual.FIFO_OVERFLOW_EN_1 = write->FIFO_OVERFLOW_EN_1;
|
||||||
|
en_2.individual.FIFO_OVERFLOW_EN_0 = write->FIFO_OVERFLOW_EN_0;
|
||||||
|
en_3.individual.FIFO_WM_EN_4 = write->FIFO_WM_EN_4;
|
||||||
|
en_3.individual.FIFO_WM_EN_3 = write->FIFO_WM_EN_3;
|
||||||
|
en_3.individual.FIFO_WM_EN_2 = write->FIFO_WM_EN_2;
|
||||||
|
en_3.individual.FIFO_WM_EN_1 = write->FIFO_WM_EN_1;
|
||||||
|
en_3.individual.FIFO_WM_EN_0 = write->FIFO_WM_EN_0;
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE, (uint8_t*)&en_0, sizeof(ICM_20948_INT_ENABLE_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_1, (uint8_t*)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_2, (uint8_t*)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_ENABLE_3, (uint8_t*)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
}
|
||||||
|
|
||||||
|
if( read != NULL ){ // If read pointer is not NULL then read the registers (if write is not NULL then this should read back the results of write into read)
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE, (uint8_t*)&en_0, sizeof(ICM_20948_INT_ENABLE_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_1, (uint8_t*)&en_1, sizeof(ICM_20948_INT_ENABLE_1_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_2, (uint8_t*)&en_2, sizeof(ICM_20948_INT_ENABLE_2_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_ENABLE_3, (uint8_t*)&en_3, sizeof(ICM_20948_INT_ENABLE_3_t)); if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
read->I2C_MST_INT_EN = en_0.I2C_MST_INT_EN;
|
||||||
|
read->DMP_INT1_EN = en_0.DMP_INT1_EN;
|
||||||
|
read->PLL_RDY_EN = en_0.PLL_READY_EN;
|
||||||
|
read->WOM_INT_EN = en_0.WOM_INT_EN;
|
||||||
|
read->REG_WOF_EN = en_0.REG_WOF_EN;
|
||||||
|
read->RAW_DATA_0_RDY_EN = en_1.RAW_DATA_0_RDY_EN;
|
||||||
|
read->FIFO_OVERFLOW_EN_4 = en_2.individual.FIFO_OVERFLOW_EN_4;
|
||||||
|
read->FIFO_OVERFLOW_EN_3 = en_2.individual.FIFO_OVERFLOW_EN_3;
|
||||||
|
read->FIFO_OVERFLOW_EN_2 = en_2.individual.FIFO_OVERFLOW_EN_2;
|
||||||
|
read->FIFO_OVERFLOW_EN_1 = en_2.individual.FIFO_OVERFLOW_EN_1;
|
||||||
|
read->FIFO_OVERFLOW_EN_0 = en_2.individual.FIFO_OVERFLOW_EN_0;
|
||||||
|
read->FIFO_WM_EN_4 = en_3.individual.FIFO_WM_EN_4;
|
||||||
|
read->FIFO_WM_EN_3 = en_3.individual.FIFO_WM_EN_3;
|
||||||
|
read->FIFO_WM_EN_2 = en_3.individual.FIFO_WM_EN_2;
|
||||||
|
read->FIFO_WM_EN_1 = en_3.individual.FIFO_WM_EN_1;
|
||||||
|
read->FIFO_WM_EN_0 = en_3.individual.FIFO_WM_EN_0;
|
||||||
|
}
|
||||||
|
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_set_sample_mode( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_LP_CONFIG_CYCLE_e mode ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
ICM_20948_LP_CONFIG_t reg;
|
||||||
|
|
||||||
|
if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr | ICM_20948_Internal_Mst ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
|
||||||
|
|
||||||
|
retval = ICM_20948_set_bank(pdev, 0); // Must be in the right bank
|
||||||
|
if( retval != ICM_20948_Stat_Ok){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_LP_CONFIG, (uint8_t*)®, sizeof(ICM_20948_LP_CONFIG_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok){ return retval; }
|
||||||
|
|
||||||
|
if( sensors & ICM_20948_Internal_Acc ){ reg.ACCEL_CYCLE = mode; } // Set all desired sensors to this setting
|
||||||
|
if( sensors & ICM_20948_Internal_Gyr ){ reg.GYRO_CYCLE = mode; }
|
||||||
|
if( sensors & ICM_20948_Internal_Mst ){ reg.I2C_MST_CYCLE = mode; }
|
||||||
|
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_LP_CONFIG, (uint8_t*)®, sizeof(ICM_20948_LP_CONFIG_t));
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_set_full_scale ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_fss_t fss ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
|
||||||
|
|
||||||
|
if( sensors & ICM_20948_Internal_Acc ){
|
||||||
|
ICM_20948_ACCEL_CONFIG_t reg;
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t));
|
||||||
|
reg.ACCEL_FS_SEL = fss.a;
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t));
|
||||||
|
}
|
||||||
|
if( sensors & ICM_20948_Internal_Gyr ){
|
||||||
|
ICM_20948_GYRO_CONFIG_1_t reg;
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t));
|
||||||
|
reg.GYRO_FS_SEL = fss.g;
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t));
|
||||||
|
}
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_set_dlpf_cfg ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_dlpcfg_t cfg ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
|
||||||
|
|
||||||
|
if( sensors & ICM_20948_Internal_Acc ){
|
||||||
|
ICM_20948_ACCEL_CONFIG_t reg;
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t));
|
||||||
|
reg.ACCEL_DLPFCFG = cfg.a;
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t));
|
||||||
|
}
|
||||||
|
if( sensors & ICM_20948_Internal_Gyr ){
|
||||||
|
ICM_20948_GYRO_CONFIG_1_t reg;
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t));
|
||||||
|
reg.GYRO_DLPFCFG = cfg.g;
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t));
|
||||||
|
}
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_enable_dlpf ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, bool enable ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
|
||||||
|
|
||||||
|
if( sensors & ICM_20948_Internal_Acc ){
|
||||||
|
ICM_20948_ACCEL_CONFIG_t reg;
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
retval |= ICM_20948_execute_r( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t));
|
||||||
|
if( enable ){ reg.ACCEL_FCHOICE = 1; }
|
||||||
|
else{ reg.ACCEL_FCHOICE = 0; }
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_CONFIG, (uint8_t*)®, sizeof(ICM_20948_ACCEL_CONFIG_t));
|
||||||
|
}
|
||||||
|
if( sensors & ICM_20948_Internal_Gyr ){
|
||||||
|
ICM_20948_GYRO_CONFIG_1_t reg;
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
retval |= ICM_20948_execute_r( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t));
|
||||||
|
if( enable ){ reg.GYRO_FCHOICE = 1; }
|
||||||
|
else{ reg.GYRO_FCHOICE = 0; }
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_CONFIG_1, (uint8_t*)®, sizeof(ICM_20948_GYRO_CONFIG_1_t));
|
||||||
|
}
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_set_sample_rate ( ICM_20948_Device_t* pdev, ICM_20948_InternalSensorID_bm sensors, ICM_20948_smplrt_t smplrt ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
if( !(sensors & ( ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr ) ) ){ return ICM_20948_Stat_SensorNotSupported; }
|
||||||
|
|
||||||
|
if( sensors & ICM_20948_Internal_Acc ){
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
uint8_t div1 = (smplrt.a << 8);
|
||||||
|
uint8_t div2 = (smplrt.a & 0xFF);
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_SMPLRT_DIV_1, &div1, 1);
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_ACCEL_SMPLRT_DIV_2, &div2, 1);
|
||||||
|
}
|
||||||
|
if( sensors & ICM_20948_Internal_Gyr ){
|
||||||
|
retval |= ICM_20948_set_bank(pdev, 2); // Must be in the right bank
|
||||||
|
uint8_t div = (smplrt.g);
|
||||||
|
retval |= ICM_20948_execute_w( pdev, AGB2_REG_GYRO_SMPLRT_DIV, &div, 1);
|
||||||
|
}
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Interface Things
|
||||||
|
ICM_20948_Status_e ICM_20948_i2c_master_passthrough ( ICM_20948_Device_t* pdev, bool passthrough ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
ICM_20948_INT_PIN_CFG_t reg;
|
||||||
|
retval = ICM_20948_set_bank(pdev, 0);
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)®, sizeof(ICM_20948_INT_PIN_CFG_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
reg.BYPASS_EN = passthrough;
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_INT_PIN_CONFIG, (uint8_t*)®, sizeof(ICM_20948_INT_PIN_CFG_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_i2c_master_enable ( ICM_20948_Device_t* pdev, bool enable ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
// Disable BYPASS_EN
|
||||||
|
retval = ICM_20948_i2c_master_passthrough( pdev, false );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
ICM_20948_I2C_MST_CTRL_t ctrl;
|
||||||
|
retval = ICM_20948_set_bank(pdev, 3);
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
ctrl.I2C_MST_CLK = 0x07; // corresponds to 345.6 kHz, good for up to 400 kHz
|
||||||
|
ctrl.I2C_MST_P_NSR = 1;
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB3_REG_I2C_MST_CTRL, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_MST_CTRL_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
ICM_20948_USER_CTRL_t reg;
|
||||||
|
retval = ICM_20948_set_bank(pdev, 0);
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
retval = ICM_20948_execute_r( pdev, AGB0_REG_USER_CTRL, (uint8_t*)®, sizeof(ICM_20948_USER_CTRL_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
if( enable ){ reg.I2C_MST_EN = 1; }
|
||||||
|
else{ reg.I2C_MST_EN = 0; }
|
||||||
|
retval = ICM_20948_execute_w( pdev, AGB0_REG_USER_CTRL, (uint8_t*)®, sizeof(ICM_20948_USER_CTRL_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
ICM_20948_Status_e ICM_20948_i2c_master_configure_slave ( ICM_20948_Device_t* pdev, uint8_t slave, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap ){
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
|
||||||
|
uint8_t slv_addr_reg;
|
||||||
|
uint8_t slv_reg_reg;
|
||||||
|
uint8_t slv_ctrl_reg;
|
||||||
|
|
||||||
|
switch( slave ){
|
||||||
|
case 0 : slv_addr_reg = AGB3_REG_I2C_SLV0_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV0_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV0_CTRL; break;
|
||||||
|
case 1 : slv_addr_reg = AGB3_REG_I2C_SLV1_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV1_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV1_CTRL; break;
|
||||||
|
case 2 : slv_addr_reg = AGB3_REG_I2C_SLV2_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV2_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV2_CTRL; break;
|
||||||
|
case 3 : slv_addr_reg = AGB3_REG_I2C_SLV3_ADDR; slv_reg_reg = AGB3_REG_I2C_SLV3_REG; slv_ctrl_reg = AGB3_REG_I2C_SLV3_CTRL; break;
|
||||||
|
default :
|
||||||
|
return ICM_20948_Stat_ParamErr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the slave address and the Rw flag
|
||||||
|
ICM_20948_I2C_SLVX_ADDR_t address;
|
||||||
|
address.ID = addr;
|
||||||
|
if( Rw ){ address.RNW = 1; }
|
||||||
|
retval = ICM_20948_execute_w( pdev, slv_addr_reg, (uint8_t*)&address, sizeof(ICM_20948_I2C_SLVX_ADDR_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
// Set the slave sub-address (reg)
|
||||||
|
ICM_20948_I2C_SLVX_REG_t subaddress;
|
||||||
|
subaddress.REG = reg;
|
||||||
|
retval = ICM_20948_execute_w( pdev, slv_reg_reg, (uint8_t*)&subaddress, sizeof(ICM_20948_I2C_SLVX_REG_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
// Set up the control info
|
||||||
|
ICM_20948_I2C_SLVX_CTRL_t ctrl;
|
||||||
|
ctrl.LENG = len;
|
||||||
|
ctrl.EN = enable;
|
||||||
|
ctrl.REG_DIS = data_only;
|
||||||
|
ctrl.GRP = grp;
|
||||||
|
ctrl.BYTE_SW = swap;
|
||||||
|
retval = ICM_20948_execute_w( pdev, slv_ctrl_reg, (uint8_t*)&ctrl, sizeof(ICM_20948_I2C_SLVX_CTRL_t) );
|
||||||
|
if( retval != ICM_20948_Stat_Ok ){ return retval; }
|
||||||
|
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Higher Level
|
||||||
|
ICM_20948_Status_e ICM_20948_get_agmt ( ICM_20948_Device_t* pdev, ICM_20948_AGMT_t* pagmt ){
|
||||||
|
if( pagmt == NULL ){ return ICM_20948_Stat_ParamErr; }
|
||||||
|
|
||||||
|
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||||||
|
const uint8_t numbytes = 14;
|
||||||
|
uint8_t buff[numbytes];
|
||||||
|
|
||||||
|
// Get readings
|
||||||
|
retval |= ICM_20948_set_bank( pdev, 0 );
|
||||||
|
retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB0_REG_ACCEL_XOUT_H, buff, numbytes );
|
||||||
|
|
||||||
|
pagmt->acc.axes.x = ((buff[0] << 8) | (buff[1] & 0xFF));
|
||||||
|
pagmt->acc.axes.y = ((buff[2] << 8) | (buff[3] & 0xFF));
|
||||||
|
pagmt->acc.axes.z = ((buff[4] << 8) | (buff[5] & 0xFF));
|
||||||
|
|
||||||
|
pagmt->gyr.axes.x = ((buff[6] << 8) | (buff[7] & 0xFF));
|
||||||
|
pagmt->gyr.axes.y = ((buff[8] << 8) | (buff[9] & 0xFF));
|
||||||
|
pagmt->gyr.axes.z = ((buff[10] << 8) | (buff[11] & 0xFF));
|
||||||
|
|
||||||
|
pagmt->tmp.val = ((buff[12] << 8) | (buff[13] & 0xFF));
|
||||||
|
|
||||||
|
// ToDo: get magnetometer readings
|
||||||
|
// pagmt->mag.axes.x =
|
||||||
|
// pagmt->mag.axes.y =
|
||||||
|
// pagmt->mag.axes.z =
|
||||||
|
|
||||||
|
|
||||||
|
// Get settings to be able to compute scaled values
|
||||||
|
retval |= ICM_20948_set_bank( pdev, 2 );
|
||||||
|
ICM_20948_ACCEL_CONFIG_t acfg;
|
||||||
|
retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG, (uint8_t*)&acfg, 1*sizeof(acfg) );
|
||||||
|
pagmt->fss.a = acfg.ACCEL_FS_SEL; // Worth noting that without explicitly setting the FS range of the accelerometer it was showing the register value for +/- 2g but the reported values were actually scaled to the +/- 16g range
|
||||||
|
// Wait a minute... now it seems like this problem actually comes from the digital low-pass filter. When enabled the value is 1/8 what it should be...
|
||||||
|
retval |= ICM_20948_set_bank( pdev, 2 );
|
||||||
|
ICM_20948_GYRO_CONFIG_1_t gcfg1;
|
||||||
|
retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_GYRO_CONFIG_1, (uint8_t*)&gcfg1, 1*sizeof(gcfg1) );
|
||||||
|
pagmt->fss.g = gcfg1.GYRO_FS_SEL;
|
||||||
|
ICM_20948_ACCEL_CONFIG_2_t acfg2;
|
||||||
|
retval |= ICM_20948_execute_r( pdev, (uint8_t)AGB2_REG_ACCEL_CONFIG_2, (uint8_t*)&acfg2, 1*sizeof(acfg2) );
|
||||||
|
|
||||||
|
return retval;
|
||||||
|
}
|
|
@ -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_ */
|
|
@ -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_ */
|
|
@ -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_ */
|
|
@ -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
|
|
@ -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;
|
|
@ -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;
|
||||||
|
}
|
|
@ -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);
|
|
@ -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;
|
|
@ -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
|
File diff suppressed because it is too large
Load Diff
|
@ -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 ////////////////////////////////
|
|
@ -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 //////////////////////////////////////
|
|
@ -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;
|
||||||
|
}
|
|
@ -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
|
|
@ -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
|
|
@ -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"
|
||||||
|
]
|
||||||
|
}
|
Binary file not shown.
Before Width: | Height: | Size: 1.5 KiB |
|
@ -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;
|
|
||||||
}
|
|
||||||
}
|
|
|
@ -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>
|
|
Binary file not shown.
Before Width: | Height: | Size: 1.5 KiB |
|
@ -31,4 +31,4 @@ board_build.partitions = huge_app.csv
|
||||||
src_dir=.
|
src_dir=.
|
||||||
|
|
||||||
[env]
|
[env]
|
||||||
lib_extra_dirs=../../libraries
|
lib_extra_dirs=../libraries
|
||||||
|
|
Loading…
Reference in New Issue