不再需要诱导

.clang-format

@@ -0,0 +1,209 @@
+---
+Language:        Cpp
+# BasedOnStyle:  LLVM
+AccessModifierOffset: -4
+AlignAfterOpenBracket: Align
+AlignArrayOfStructures: None
+AlignConsecutiveAssignments:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    true
+AlignConsecutiveBitFields:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveDeclarations:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveMacros:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignEscapedNewlines: Right
+AlignOperands:   Align
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortEnumsOnASingleLine: true
+AllowShortBlocksOnASingleLine: Never
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: false
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: MultiLine
+AttributeMacros:
+  - __capability
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:
+  AfterCaseLabel:  false
+  AfterClass:      false
+  AfterControlStatement: Never
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     true
+  BeforeElse:      true
+  BeforeLambdaBody: false
+  BeforeWhile:     false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeConceptDeclarations: Always
+BreakBeforeBraces: Custom
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     80
+CommentPragmas:  '^ IWYU pragma:'
+QualifierAlignment: Leave
+CompactNamespaces: false
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DeriveLineEnding: true
+DerivePointerAlignment: false
+DisableFormat:   false
+EmptyLineAfterAccessModifier: Never
+EmptyLineBeforeAccessModifier: LogicalBlock
+ExperimentalAutoDetectBinPacking: false
+PackConstructorInitializers: BinPack
+BasedOnStyle:    ''
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+AllowAllConstructorInitializersOnNextLine: true
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IfMacros:
+  - KJ_IF_MAYBE
+IncludeBlocks:   Preserve
+IncludeCategories:
+  - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
+    Priority:        2
+    SortPriority:    0
+    CaseSensitive:   false
+  - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
+    Priority:        3
+    SortPriority:    0
+    CaseSensitive:   false
+  - Regex:           '.*'
+    Priority:        1
+    SortPriority:    0
+    CaseSensitive:   false
+IncludeIsMainRegex: '(Test)?$'
+IncludeIsMainSourceRegex: ''
+IndentAccessModifiers: false
+IndentCaseLabels: false
+IndentCaseBlocks: false
+IndentGotoLabels: true
+IndentPPDirectives: None
+IndentExternBlock: AfterExternBlock
+IndentRequiresClause: true
+IndentWidth:     4
+IndentWrappedFunctionNames: false
+InsertBraces:    false
+InsertTrailingCommas: None
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true
+LambdaBodyIndentation: Signature
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Auto
+ObjCBlockIndentWidth: 2
+ObjCBreakBeforeNestedBlockParam: true
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakOpenParenthesis: 0
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 60
+PenaltyIndentedWhitespace: 0
+PointerAlignment: Right
+PPIndentWidth:   -1
+ReferenceAlignment: Pointer
+ReflowComments:  true
+RemoveBracesLLVM: false
+RequiresClausePosition: OwnLine
+SeparateDefinitionBlocks: Leave
+ShortNamespaceLines: 1
+SortIncludes:    CaseSensitive
+SortJavaStaticImport: Before
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCaseColon: false
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeParensOptions:
+  AfterControlStatements: true
+  AfterForeachMacros: true
+  AfterFunctionDefinitionName: false
+  AfterFunctionDeclarationName: false
+  AfterIfMacros:   true
+  AfterOverloadedOperator: false
+  AfterRequiresInClause: false
+  AfterRequiresInExpression: false
+  BeforeNonEmptyParentheses: false
+SpaceAroundPointerQualifiers: Default
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles:  Never
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInLineCommentPrefix:
+  Minimum:         1
+  Maximum:         -1
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
+BitFieldColonSpacing: Both
+Standard:        Latest
+StatementAttributeLikeMacros:
+  - Q_EMIT
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth:        4
+UseCRLF:         false
+UseTab:          Never
+

8By8/ListE.h

@@ -0,0 +1,148 @@
+#include "Node.h"
+
+template <class E> class List {
+private:
+    Node<E> *head;
+    int _length;
+    int getIndexInRange(int index) const;
+
+public:
+    List();
+    List(const E &newItem);
+    List(const E *newItemList, const int itemCount);
+    List(const List<E> &otherList);
+    ~List();
+    // Append _newItem at last. Returns the List itself.
+    List<E> &append(const E &_newItem);
+    // Throws IndexError if index is not in the range of [-_length, _length -
+    // 1]. Returns the List itself.
+    E pop();
+    // Clear all nodes.
+    List<E> &clear();
+    const E &peek() const;
+    List<E> &operator=(const List<E> &otherList);
+    E &operator[](const int index);
+    const E &operator[](const int index) const;
+
+    int length() const;
+
+    bool empty() const;
+};
+
+template <class E> int List<E>::getIndexInRange(int index) const {
+    if (index < 0) {
+        index += this->_length;
+    }
+    if (index < 0 || index >= _length) {
+        return -1;
+    }
+    return index;
+}
+
+template <class E> List<E>::List() : head(NULL), _length(0){};
+
+template <class E>
+List<E>::List(const E &newItem) : head(new Node<E>(newItem)), _length(1){};
+
+template <class E> List<E>::List(const E *newItemList, const int itemCount) {
+    for (int i = 0; i < itemCount; i++) {
+        this->append(newItemList[i]);
+    }
+};
+
+template <class E> List<E>::List(const List<E> &otherList) {
+    if (otherList.length() > 0) {
+        this->head = new Node<E>(otherList[0]);
+        this->_length = 1;
+        Node<E> *nextPtr = otherList.head->getNextPtr();
+        while (nextPtr != NULL) {
+            this->append((*nextPtr).getContent());
+            nextPtr = (*nextPtr).getNextPtr();
+        }
+    }
+    else {
+        this->head = NULL;
+        this->_length = 0;
+    }
+}
+
+template <class E> List<E>::~List() {
+    if (this->head != NULL) {
+        this->head->destruct();
+    }
+    head = NULL;
+}
+
+template <class E> List<E> &List<E>::append(const E &_newItem) {
+    if (this->head == NULL) {
+        this->head = new Node<E>(_newItem);
+    }
+    else {
+        this->head->getTail().setNext(new Node<E>(_newItem));
+    }
+    this->_length++;
+    return *this;
+}
+
+template <class E> E List<E>::pop() {
+    int index = this->_length - 1;
+
+    Node<E> removed = this->head->getByIndex(index);
+
+    this->_length--;
+    if (index == 0) {
+        Node<E> *newHead = this->head->getNextPtr();
+        delete this->head;
+        this->head = newHead;
+    }
+    else {
+        Node<E> &prev = this->head->getByIndex(index - 1);
+        delete prev.getNextPtr();
+        prev.setNext(removed.getNextPtr());
+    }
+    return removed.getContent();
+}
+
+template <class E> List<E> &List<E>::clear() {
+    if (this->_length == 0) {
+        return *this;
+    }
+    this->head->destruct();
+    this->head = NULL;
+    this->_length = 0;
+    return *this;
+}
+
+template <class E> const E&List<E>::peek() const {
+    return this->head->getByIndex(this->_length - 1).getContent();
+}
+
+template <class E> List<E> &List<E>::operator=(const List<E> &otherList) {
+    this->clear();
+    if (otherList.length() > 0) {
+        this->head = new Node<E>(otherList[0]);
+        this->_length = 1;
+        Node<E> *nextPtr = otherList.head->getNextPtr();
+        while (nextPtr != NULL) {
+            this->append((*nextPtr).getContent());
+            nextPtr = (*nextPtr).getNextPtr();
+        }
+    }
+    return *this;
+}
+
+template <class E> E &List<E>::operator[](const int index) {
+    return this->head->getByIndex(index).getContent();
+}
+
+template <class E> const E &List<E>::operator[](const int index) const {
+    return this->head->getByIndex(index).getContent();
+}
+
+template <class E> int List<E>::length() const {
+    return this->_length;
+}
+
+template <class E> bool List<E>::empty() const {
+    return this->_length == 0;
+}

8By8/Node.h

@@ -0,0 +1,105 @@
+#pragma once
+
+template <class E> class Node {
+private:
+    E content;
+    Node *nextNode;
+
+public:
+    Node();
+    Node(E _content, Node<E> *_nextNode = NULL);
+    ~Node();
+    // Returns a reference to the content.
+    E &getContent();
+    // Returns a reference to the next Node. May return NULL if this Node is the
+    // last one.
+    Node<E> *getNextPtr();
+    // Set the next Node _nextNode. Returns original pointer.
+    Node<E> *setNext(Node<E> *_nextNode);
+    // Returns a reference to the Tail.
+    Node<E> &getTail();
+    // Returns a reference to the Node of given index.
+    Node<E> &getByIndex(const int index);
+    // Compare the content with the argument.
+    bool operator==(const E &other);
+    // Destructs the Node itself and all the node behind it.
+    void destruct();
+};
+
+template <class E> class Iterator {
+private:
+    Node<E> *current;
+
+public:
+    Iterator(Node<E> *pos);
+    bool hasNext();
+    // Return the same as hasNext();
+    operator bool();
+    E &next();
+};
+
+template <class E> Node<E>::Node() : content(0), nextNode(NULL){};
+
+template <class E>
+Node<E>::Node(E _content, Node *_nextNode)
+    : content(_content), nextNode(_nextNode){};
+
+template <class E> Node<E>::~Node(){};
+
+template <class E> E &Node<E>::getContent() {
+    return this->content;
+}
+
+template <class E> Node<E> *Node<E>::getNextPtr() {
+    return this->nextNode;
+}
+
+template <class E> Node<E> *Node<E>::setNext(Node<E> *_nextNode) {
+    Node<E> *temp = this->nextNode;
+    this->nextNode = _nextNode;
+    return temp;
+}
+
+template <class E> Node<E> &Node<E>::getTail() {
+    if (this->nextNode == NULL) {
+        return *this;
+    }
+    return this->nextNode->getTail();
+}
+
+template <class E> Node<E> &Node<E>::getByIndex(const int index) {
+    if (index == 0) {
+        return *this;
+    }
+    return this->nextNode->getByIndex(index - 1);
+}
+
+template <class E> bool Node<E>::operator==(const E &other) {
+    return this->content == other;
+}
+
+template <class E> void Node<E>::destruct() {
+    if (this->nextNode != NULL) {
+        this->nextNode->destruct();
+    }
+    delete this;
+}
+
+template <class E> Iterator<E>::Iterator(Node<E> *start) : current(start){};
+
+template <class E> bool Iterator<E>::hasNext() {
+    return this->current != NULL;
+}
+
+template <class E> Iterator<E>::operator bool() {
+    return this->hasNext();
+}
+
+template <class E> E &Iterator<E>::next() {
+    if (!this->hasNext()) {
+        throw(IndexError("Out of bound!"));
+    }
+    E &temp = this->current->getContent();
+    this->current = this->current->getNextPtr();
+    return temp;
+}

8By8/REG.h

@@ -0,0 +1,284 @@
+#ifndef __AHRSREG_H
+#define __AHRSREG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#define REGSIZE 0x90
+
+#define SAVE 		0x00
+#define CALSW 		0x01
+#define RSW 		0x02
+#define RRATE		0x03
+#define BAUD 		0x04
+#define AXOFFSET	0x05
+#define AYOFFSET	0x06
+#define AZOFFSET	0x07
+#define GXOFFSET	0x08
+#define GYOFFSET	0x09
+#define GZOFFSET	0x0a
+#define HXOFFSET	0x0b
+#define HYOFFSET	0x0c
+#define HZOFFSET	0x0d
+#define D0MODE		0x0e
+#define D1MODE		0x0f
+#define D2MODE		0x10
+#define D3MODE		0x11
+#define D0PWMH		0x12
+#define D1PWMH		0x13
+#define D2PWMH		0x14
+#define D3PWMH		0x15
+#define D0PWMT		0x16
+#define D1PWMT		0x17
+#define D2PWMT		0x18
+#define D3PWMT		0x19
+#define IICADDR		0x1a
+#define LEDOFF 		0x1b
+#define MAGRANGX	0x1c
+#define MAGRANGY	0x1d
+#define MAGRANGZ	0x1e
+#define BANDWIDTH	0x1f
+#define GYRORANGE	0x20
+#define ACCRANGE	0x21
+#define SLEEP       0x22
+#define ORIENT		0x23
+#define AXIS6       0x24
+#define FILTK       0x25
+#define GPSBAUD		0x26
+#define READADDR	0x27
+#define BWSCALE		0x28
+#define MOVETHR		0x28
+#define MOVESTA		0x29
+#define ACCFILT		0x2A
+#define GYROFILT	0x2b
+#define MAGFILT		0x2c
+#define POWONSEND	0x2d
+#define VERSION		0x2e
+#define CCBW			  0x2f
+#define YYMM				0x30
+#define DDHH				0x31
+#define MMSS				0x32
+#define MS					0x33
+#define AX					0x34
+#define AY					0x35
+#define AZ					0x36
+#define GX					0x37
+#define GY					0x38
+#define GZ					0x39
+#define HX					0x3a
+#define HY					0x3b
+#define HZ					0x3c
+#define Roll				0x3d
+#define Pitch				0x3e
+#define Yaw					0x3f
+#define TEMP				0x40
+#define D0Status		0x41
+#define D1Status		0x42
+#define D2Status		0x43
+#define D3Status		0x44
+#define PressureL		0x45
+#define PressureH		0x46
+#define HeightL			0x47
+#define HeightH			0x48
+#define LonL				0x49
+#define LonH				0x4a
+#define LatL				0x4b
+#define LatH				0x4c
+#define GPSHeight       0x4d
+#define GPSYAW          0x4e
+#define GPSVL				0x4f
+#define GPSVH				0x50
+#define q0					0x51
+#define q1					0x52
+#define q2					0x53
+#define q3					0x54
+#define SVNUM				0x55
+#define PDOP				0x56
+#define HDOP				0x57
+#define VDOP				0x58
+#define DELAYT			0x59
+#define XMIN            0x5a
+#define XMAX            0x5b
+#define BATVAL          0x5c
+#define ALARMPIN        0x5d
+#define YMIN            0x5e
+#define YMAX            0x5f
+#define GYROZSCALE		0x60
+#define GYROCALITHR     0x61
+#define ALARMLEVEL      0x62
+#define GYROCALTIME		0x63
+#define REFROLL			0x64
+#define REFPITCH		0x65
+#define REFYAW			0x66
+#define GPSTYPE     0x67
+#define TRIGTIME    0x68
+#define KEY         0x69
+#define WERROR      0x6a
+#define TIMEZONE    0x6b
+#define CALICNT     0x6c
+#define WZCNT       0x6d
+#define WZTIME      0x6e
+#define WZSTATIC    0x6f
+#define ACCSENSOR 	0x70
+#define GYROSENSOR 	0x71
+#define MAGSENSOR 	0x72
+#define PRESSENSOR 	0x73
+#define MODDELAY    0x74
+
+#define ANGLEAXIS   0x75
+#define XRSCALE			0x76    
+#define YRSCALE			0x77
+#define ZRSCALE			0x78
+
+#define XREFROLL		0x79    
+#define YREFPITCH		0x7a
+#define ZREFYAW			0x7b
+
+#define ANGXOFFSET		0x7c    
+#define ANGYOFFSET		0x7d
+#define ANGZOFFSET		0x7e
+
+#define NUMBERID1    0x7f
+#define NUMBERID2    0x80
+#define NUMBERID3    0x81
+#define NUMBERID4    0x82
+#define NUMBERID5    0x83
+#define NUMBERID6    0x84
+
+#define XA85PSCALE       0x85
+#define XA85NSCALE       0x86
+#define YA85PSCALE       0x87
+#define YA85NSCALE       0x88
+#define XA30PSCALE       0x89
+#define XA30NSCALE       0x8a
+#define YA30PSCALE       0x8b
+#define YA30NSCALE       0x8c
+
+#define CHIPIDL     0x8D
+#define CHIPIDH     0x8E
+#define REGINITFLAG       REGSIZE-1 
+
+
+/* AXIS6 */
+#define ALGRITHM9 0
+#define ALGRITHM6 1
+
+/************CALSW**************/
+#define NORMAL          0x00
+#define CALGYROACC      0x01
+#define CALMAG          0x02
+#define CALALTITUDE     0x03
+#define CALANGLEZ       0x04
+#define CALACCL         0x05
+#define CALACCR         0x06
+#define CALMAGMM        0x07
+#define CALREFANGLE		0x08
+#define CALMAG2STEP		0x09
+//#define CALACCX       0x09
+//#define ACC45PRX      0x0A
+//#define ACC45NRX      0x0B
+//#define CALACCY       0x0C
+//#define ACC45PRY      0x0D
+//#define ACC45NRY     	0x0E
+//#define CALREFANGLER  0x0F
+//#define CALACCINIT    0x10
+//#define CALREFANGLEINIT 0x11
+#define CALHEXAHEDRON		0x12
+
+/************OUTPUTHEAD**************/
+#define WIT_TIME        0x50
+#define WIT_ACC         0x51
+#define WIT_GYRO        0x52
+#define WIT_ANGLE       0x53
+#define WIT_MAGNETIC    0x54
+#define WIT_DPORT       0x55
+#define WIT_PRESS       0x56
+#define WIT_GPS         0x57
+#define WIT_VELOCITY    0x58
+#define WIT_QUATER      0x59
+#define WIT_GSA         0x5A
+#define WIT_REGVALUE    0x5F
+
+/************RSW**************/
+#define RSW_TIME 	0x01
+#define RSW_ACC		0x02
+#define RSW_GYRO	0x04
+#define RSW_ANGLE	0x08
+#define RSW_MAG		0x10
+#define RSW_PORT	0x20
+#define RSW_PRESS	0x40
+#define RSW_GPS		0x80
+#define RSW_V       0x100
+#define RSW_Q       0x200
+#define	RSW_GSA		0x400
+#define RSW_MASK	0xfff
+
+/**RRATE*****/
+#define RRATE_NONE	0x0d
+#define RRATE_02HZ	0x01
+#define RRATE_05HZ	0x02
+#define RRATE_1HZ 	0x03
+#define RRATE_2HZ 	0x04
+#define RRATE_5HZ 	0x05
+#define RRATE_10HZ	0x06
+#define RRATE_20HZ	0x07
+#define RRATE_50HZ	0x08
+#define RRATE_100HZ	0x09
+#define RRATE_125HZ	0x0a	//only WT931
+#define RRATE_200HZ	0x0b
+#define RRATE_ONCE 	0x0c
+
+/* BAUD */
+#define WIT_BAUD_4800	  1
+#define WIT_BAUD_9600	  2
+#define WIT_BAUD_19200	  3
+#define WIT_BAUD_38400	  4
+#define WIT_BAUD_57600	  5
+#define WIT_BAUD_115200	  6
+#define WIT_BAUD_230400	  7
+#define WIT_BAUD_460800	  8
+#define WIT_BAUD_921600	  9
+
+/*CAN BAUD*/
+#define CAN_BAUD_1000000	 	0
+#define CAN_BAUD_800000	  		1
+#define CAN_BAUD_500000	  		2
+#define CAN_BAUD_400000	  		3
+#define CAN_BAUD_250000	  		4
+#define CAN_BAUD_200000   		5
+#define CAN_BAUD_125000	  		6
+#define CAN_BAUD_100000  		7
+#define CAN_BAUD_80000	  		8
+#define CAN_BAUD_50000  		9
+#define CAN_BAUD_40000	  		10
+#define CAN_BAUD_20000  		11
+#define CAN_BAUD_10000	  		12
+#define CAN_BAUD_5000	  		13
+#define CAN_BAUD_3000	  		14
+
+/* KEY */
+#define KEY_UNLOCK	0xB588
+
+/* SAVE */
+#define SAVE_PARAM	0x00
+#define SAVE_SWRST	0xFF
+
+/* ORIENT */
+#define ORIENT_HERIZONE	0
+#define ORIENT_VERTICLE	1
+
+/* BANDWIDTH */
+#define BANDWIDTH_256HZ	0
+#define BANDWIDTH_184HZ	1
+#define BANDWIDTH_94HZ	2
+#define BANDWIDTH_44HZ	3
+#define BANDWIDTH_21HZ	4
+#define BANDWIDTH_10HZ	5
+#define BANDWIDTH_5HZ	6
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

8By8/wit_c_sdk.c

@@ -0,0 +1,502 @@
+#include "wit_c_sdk.h"
+
+static SerialWrite p_WitSerialWriteFunc = NULL;
+static WitI2cWrite p_WitI2cWriteFunc = NULL;
+static WitI2cRead p_WitI2cReadFunc = NULL;
+static CanWrite p_WitCanWriteFunc = NULL;
+static RegUpdateCb p_WitRegUpdateCbFunc = NULL;
+static DelaymsCb p_WitDelaymsFunc = NULL;
+
+static uint8_t s_ucAddr = 0xff;
+static uint8_t s_ucWitDataBuff[WIT_DATA_BUFF_SIZE];
+static uint32_t s_uiWitDataCnt = 0, s_uiProtoclo = 0, s_uiReadRegIndex = 0;
+int16_t sReg[REGSIZE];
+
+
+#define FuncW 0x06
+#define FuncR 0x03
+
+static const uint8_t __auchCRCHi[256] = {
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
+    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
+    0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
+    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
+    0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
+    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40
+};
+static const uint8_t __auchCRCLo[256] = {
+    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
+    0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
+    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
+    0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
+    0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
+    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
+    0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
+    0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
+    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
+    0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
+    0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
+    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
+    0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
+    0x40
+};
+
+
+static uint16_t __CRC16(uint8_t *puchMsg, uint16_t usDataLen)
+{
+    uint8_t uchCRCHi = 0xFF;
+    uint8_t uchCRCLo = 0xFF;
+    uint8_t uIndex;
+    int i = 0;
+    uchCRCHi = 0xFF;
+    uchCRCLo = 0xFF;
+    for (; i<usDataLen; i++)
+    {
+        uIndex = uchCRCHi ^ puchMsg[i];
+        uchCRCHi = uchCRCLo ^ __auchCRCHi[uIndex];
+        uchCRCLo = __auchCRCLo[uIndex] ;
+    }
+    return (uint16_t)(((uint16_t)uchCRCHi << 8) | (uint16_t)uchCRCLo) ;
+}
+static uint8_t __CaliSum(uint8_t *data, uint32_t len)
+{
+    uint32_t i;
+    uint8_t ucCheck = 0;
+    for(i=0; i<len; i++) ucCheck += *(data + i);
+    return ucCheck;
+}
+int32_t WitSerialWriteRegister(SerialWrite Write_func)
+{
+    if(!Write_func)return WIT_HAL_INVAL;
+    p_WitSerialWriteFunc = Write_func;
+    return WIT_HAL_OK;
+}
+static void CopeWitData(uint8_t ucIndex, uint16_t *p_data, uint32_t uiLen)
+{
+    uint32_t uiReg1 = 0, uiReg2 = 0, uiReg1Len = 0, uiReg2Len = 0;
+    uint16_t *p_usReg1Val = p_data;
+    uint16_t *p_usReg2Val = p_data+3;
+    
+    uiReg1Len = 4;
+    switch(ucIndex)
+    {
+        case WIT_ACC:   uiReg1 = AX;    uiReg1Len = 3;  uiReg2 = TEMP;  uiReg2Len = 1;  break;
+        case WIT_ANGLE: uiReg1 = Roll;  uiReg1Len = 3;  uiReg2 = VERSION;  uiReg2Len = 1;  break;
+        case WIT_TIME:  uiReg1 = YYMM;	break;
+        case WIT_GYRO:  uiReg1 = GX;  uiLen = 3;break;
+        case WIT_MAGNETIC: uiReg1 = HX;  uiLen = 3;break;
+        case WIT_DPORT: uiReg1 = D0Status;  break;
+        case WIT_PRESS: uiReg1 = PressureL;  break;
+        case WIT_GPS:   uiReg1 = LonL;  break;
+        case WIT_VELOCITY: uiReg1 = GPSHeight;  break;
+        case WIT_QUATER:    uiReg1 = q0;  break;
+        case WIT_GSA:   uiReg1 = SVNUM;  break;
+        case WIT_REGVALUE:  uiReg1 = s_uiReadRegIndex;  break;
+		default:
+			return ;
+
+    }
+    if(uiLen == 3)
+    {
+        uiReg1Len = 3;
+        uiReg2Len = 0;
+    }
+    if(uiReg1Len)
+	{
+		memcpy(&sReg[uiReg1], p_usReg1Val, uiReg1Len<<1);
+		p_WitRegUpdateCbFunc(uiReg1, uiReg1Len);
+	}
+    if(uiReg2Len)
+	{
+		memcpy(&sReg[uiReg2], p_usReg2Val, uiReg2Len<<1);
+		p_WitRegUpdateCbFunc(uiReg2, uiReg2Len);
+	}
+}
+
+void WitSerialDataIn(uint8_t ucData)
+{
+    uint16_t usCRC16, usTemp, i, usData[4];
+    uint8_t ucSum;
+
+    if(p_WitRegUpdateCbFunc == NULL)return ;
+    s_ucWitDataBuff[s_uiWitDataCnt++] = ucData;
+    switch(s_uiProtoclo)
+    {
+        case WIT_PROTOCOL_NORMAL:
+            if(s_ucWitDataBuff[0] != 0x55)
+            {
+                s_uiWitDataCnt--;
+                memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
+                return ;
+            }
+            if(s_uiWitDataCnt >= 11)
+            {
+                ucSum = __CaliSum(s_ucWitDataBuff, 10);
+                if(ucSum != s_ucWitDataBuff[10])
+                {
+                    s_uiWitDataCnt--;
+                    memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
+                    return ;
+                }
+                usData[0] = ((uint16_t)s_ucWitDataBuff[3] << 8) | (uint16_t)s_ucWitDataBuff[2];
+                usData[1] = ((uint16_t)s_ucWitDataBuff[5] << 8) | (uint16_t)s_ucWitDataBuff[4];
+                usData[2] = ((uint16_t)s_ucWitDataBuff[7] << 8) | (uint16_t)s_ucWitDataBuff[6];
+                usData[3] = ((uint16_t)s_ucWitDataBuff[9] << 8) | (uint16_t)s_ucWitDataBuff[8];
+                CopeWitData(s_ucWitDataBuff[1], usData, 4);
+                s_uiWitDataCnt = 0;
+            }
+        break;
+        case WIT_PROTOCOL_MODBUS:
+            if(s_uiWitDataCnt > 2)
+            {
+                if(s_ucWitDataBuff[1] != FuncR)
+                {
+                    s_uiWitDataCnt--;
+                    memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
+                    return ;
+                }
+                if(s_uiWitDataCnt < (s_ucWitDataBuff[2] + 5))return ;
+                usTemp = ((uint16_t)s_ucWitDataBuff[s_uiWitDataCnt-2] << 8) | s_ucWitDataBuff[s_uiWitDataCnt-1];
+                usCRC16 = __CRC16(s_ucWitDataBuff, s_uiWitDataCnt-2);
+                if(usTemp != usCRC16)
+                {
+                    s_uiWitDataCnt--;
+                    memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt);
+                    return ;
+                }
+                usTemp = s_ucWitDataBuff[2] >> 1;
+                for(i = 0; i < usTemp; i++)
+                {
+                    sReg[i+s_uiReadRegIndex] = ((uint16_t)s_ucWitDataBuff[(i<<1)+3] << 8) | s_ucWitDataBuff[(i<<1)+4];
+                }
+                p_WitRegUpdateCbFunc(s_uiReadRegIndex, usTemp);
+                s_uiWitDataCnt = 0;
+            }
+        break;
+        case WIT_PROTOCOL_CAN:
+        case WIT_PROTOCOL_I2C:
+        s_uiWitDataCnt = 0;
+        break;
+    }
+    if(s_uiWitDataCnt == WIT_DATA_BUFF_SIZE)s_uiWitDataCnt = 0;
+}
+int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func)
+{
+    if(!write_func)return WIT_HAL_INVAL;
+    if(!read_func)return WIT_HAL_INVAL;
+    p_WitI2cWriteFunc = write_func;
+    p_WitI2cReadFunc = read_func;
+    return WIT_HAL_OK;
+}
+int32_t WitCanWriteRegister(CanWrite Write_func)
+{
+    if(!Write_func)return WIT_HAL_INVAL;
+    p_WitCanWriteFunc = Write_func;
+    return WIT_HAL_OK;
+}
+void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen)
+{
+	uint16_t usData[3];
+    if(p_WitRegUpdateCbFunc == NULL)return ;
+    if(ucLen < 8)return ;
+    switch(s_uiProtoclo)
+    {
+        case WIT_PROTOCOL_CAN:
+            if(ucData[0] != 0x55)return ;
+            usData[0] = ((uint16_t)ucData[3] << 8) | ucData[2];
+            usData[1] = ((uint16_t)ucData[5] << 8) | ucData[4];
+            usData[2] = ((uint16_t)ucData[7] << 8) | ucData[6];
+            CopeWitData(ucData[1], usData, 3);
+            break;
+        case WIT_PROTOCOL_NORMAL:
+        case WIT_PROTOCOL_MODBUS:
+        case WIT_PROTOCOL_I2C:
+            break;
+    }
+}
+int32_t WitRegisterCallBack(RegUpdateCb update_func)
+{
+    if(!update_func)return WIT_HAL_INVAL;
+    p_WitRegUpdateCbFunc = update_func;
+    return WIT_HAL_OK;
+}
+int32_t WitWriteReg(uint32_t uiReg, uint16_t usData)
+{
+    uint16_t usCRC;
+    uint8_t ucBuff[8];
+    if(uiReg >= REGSIZE)return WIT_HAL_INVAL;
+    switch(s_uiProtoclo)
+    {
+        case WIT_PROTOCOL_NORMAL:
+            if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
+            ucBuff[0] = 0xFF;
+            ucBuff[1] = 0xAA;
+            ucBuff[2] = uiReg & 0xFF;
+            ucBuff[3] = usData & 0xff;
+            ucBuff[4] = usData >> 8;
+            p_WitSerialWriteFunc(ucBuff, 5);
+            break;
+        case WIT_PROTOCOL_MODBUS:
+            if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
+            ucBuff[0] = s_ucAddr;
+            ucBuff[1] = FuncW;
+            ucBuff[2] = uiReg >> 8;
+            ucBuff[3] = uiReg & 0xFF;
+            ucBuff[4] = usData >> 8;
+            ucBuff[5] = usData & 0xff;
+            usCRC = __CRC16(ucBuff, 6);
+            ucBuff[6] = usCRC >> 8;
+            ucBuff[7] = usCRC & 0xff;
+            p_WitSerialWriteFunc(ucBuff, 8);
+            break;
+        case WIT_PROTOCOL_CAN:
+            if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY;
+            ucBuff[0] = 0xFF;
+            ucBuff[1] = 0xAA;
+            ucBuff[2] = uiReg & 0xFF;
+            ucBuff[3] = usData & 0xff;
+            ucBuff[4] = usData >> 8;
+            p_WitCanWriteFunc(s_ucAddr, ucBuff, 5);
+            break;
+        case WIT_PROTOCOL_I2C:
+            if(p_WitI2cWriteFunc == NULL)return WIT_HAL_EMPTY;
+            ucBuff[0] = usData & 0xff;
+            ucBuff[1] = usData >> 8;
+			if(p_WitI2cWriteFunc(s_ucAddr << 1, uiReg, ucBuff, 2) != 1)
+			{
+				//printf("i2c write fail\r\n");
+			}
+        break;
+	default: 
+            return WIT_HAL_INVAL;        
+    }
+    return WIT_HAL_OK;
+}
+int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum)
+{
+    uint16_t usTemp, i;
+    uint8_t ucBuff[8];
+    if((uiReg + uiReadNum) >= REGSIZE)return WIT_HAL_INVAL;
+    switch(s_uiProtoclo)
+    {
+        case WIT_PROTOCOL_NORMAL:
+            if(uiReadNum > 4)return WIT_HAL_INVAL;
+            if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
+            ucBuff[0] = 0xFF;
+            ucBuff[1] = 0xAA;
+            ucBuff[2] = 0x27;
+            ucBuff[3] = uiReg & 0xff;
+            ucBuff[4] = uiReg >> 8;
+            p_WitSerialWriteFunc(ucBuff, 5);
+            break;
+        case WIT_PROTOCOL_MODBUS:
+            if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY;
+            usTemp = uiReadNum << 1;
+            if((usTemp + 5) > WIT_DATA_BUFF_SIZE)return WIT_HAL_NOMEM;
+            ucBuff[0] = s_ucAddr;
+            ucBuff[1] = FuncR;
+            ucBuff[2] = uiReg >> 8;
+            ucBuff[3] = uiReg & 0xFF;
+            ucBuff[4] = uiReadNum >> 8;
+            ucBuff[5] = uiReadNum & 0xff;
+            usTemp = __CRC16(ucBuff, 6);
+            ucBuff[6] = usTemp >> 8;
+            ucBuff[7] = usTemp & 0xff;
+            p_WitSerialWriteFunc(ucBuff, 8);
+            break;
+        case WIT_PROTOCOL_CAN:
+            if(uiReadNum > 3)return WIT_HAL_INVAL;
+            if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY;
+            ucBuff[0] = 0xFF;
+            ucBuff[1] = 0xAA;
+            ucBuff[2] = 0x27;
+            ucBuff[3] = uiReg & 0xff;
+            ucBuff[4] = uiReg >> 8;
+            p_WitCanWriteFunc(s_ucAddr, ucBuff, 5);
+            break;
+        case WIT_PROTOCOL_I2C:
+            if(p_WitI2cReadFunc == NULL)return WIT_HAL_EMPTY;
+            usTemp = uiReadNum << 1;
+            if(WIT_DATA_BUFF_SIZE < usTemp)return WIT_HAL_NOMEM;
+            if(p_WitI2cReadFunc(s_ucAddr << 1, uiReg, s_ucWitDataBuff, usTemp) == 1)
+            {
+                if(p_WitRegUpdateCbFunc == NULL)return WIT_HAL_EMPTY;
+                for(i = 0; i < uiReadNum; i++)
+                {
+                    sReg[i+uiReg] = ((uint16_t)s_ucWitDataBuff[(i<<1)+1] << 8) | s_ucWitDataBuff[i<<1];
+                }
+                p_WitRegUpdateCbFunc(uiReg, uiReadNum);
+            }
+			
+            break;
+		default: 
+            return WIT_HAL_INVAL;
+    }
+    s_uiReadRegIndex = uiReg;
+
+    return WIT_HAL_OK;
+}
+int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr)
+{
+	if(uiProtocol > WIT_PROTOCOL_I2C)return WIT_HAL_INVAL;
+    s_uiProtoclo = uiProtocol;
+    s_ucAddr = ucAddr;
+    s_uiWitDataCnt = 0;
+    return WIT_HAL_OK;
+}
+void WitDeInit(void)
+{
+    p_WitSerialWriteFunc = NULL;
+    p_WitI2cWriteFunc = NULL;
+    p_WitI2cReadFunc = NULL;
+    p_WitCanWriteFunc = NULL;
+    p_WitRegUpdateCbFunc = NULL;
+    s_ucAddr = 0xff;
+    s_uiWitDataCnt = 0;
+    s_uiProtoclo = 0;
+}
+
+int32_t WitDelayMsRegister(DelaymsCb delayms_func)
+{
+    if(!delayms_func)return WIT_HAL_INVAL;
+    p_WitDelaymsFunc = delayms_func;
+    return WIT_HAL_OK;
+}
+
+char CheckRange(short sTemp,short sMin,short sMax)
+{
+    if ((sTemp>=sMin)&&(sTemp<=sMax)) return 1;
+    else return 0;
+}
+/*Acceleration calibration demo*/
+int32_t WitStartAccCali(void)
+{
+/*
+	First place the equipment horizontally, and then perform the following operations
+*/
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	    return  WIT_HAL_ERROR;// unlock reg
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(CALSW, CALGYROACC) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+int32_t WitStopAccCali(void)
+{
+	if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(SAVE, SAVE_PARAM) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+/*Magnetic field calibration*/
+int32_t WitStartMagCali(void)
+{
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(CALSW, CALMAGMM) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+int32_t WitStopMagCali(void)
+{
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+/*change Band*/
+int32_t WitSetUartBaud(int32_t uiBaudIndex)
+{
+	if(!CheckRange(uiBaudIndex,WIT_BAUD_4800,WIT_BAUD_230400))
+	{
+		return WIT_HAL_INVAL;
+	}
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+/*change Can Band*/
+int32_t WitSetCanBaud(int32_t uiBaudIndex)
+{
+	if(!CheckRange(uiBaudIndex,CAN_BAUD_1000000,CAN_BAUD_3000))
+	{
+		return WIT_HAL_INVAL;
+	}
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+/*change Bandwidth*/
+int32_t WitSetBandwidth(int32_t uiBaudWidth)
+{	
+	if(!CheckRange(uiBaudWidth,BANDWIDTH_256HZ,BANDWIDTH_5HZ))
+	{
+		return WIT_HAL_INVAL;
+	}
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(BANDWIDTH, uiBaudWidth) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+
+/*change output rate */
+int32_t WitSetOutputRate(int32_t uiRate)
+{	
+	if(!CheckRange(uiRate,RRATE_02HZ,RRATE_NONE))
+	{
+		return WIT_HAL_INVAL;
+	}
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(RRATE, uiRate) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+
+/*change WitSetContent */
+int32_t WitSetContent(int32_t uiRsw)
+{	
+	if(!CheckRange(uiRsw,RSW_TIME,RSW_MASK))
+	{
+		return WIT_HAL_INVAL;
+	}
+	if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	if(s_uiProtoclo == WIT_PROTOCOL_MODBUS)	p_WitDelaymsFunc(20);
+	else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1);
+	else ;
+	if(WitWriteReg(RSW, uiRsw) != WIT_HAL_OK)	return  WIT_HAL_ERROR;
+	return WIT_HAL_OK;
+}
+
+
+

8By8/wit_c_sdk.h

@@ -0,0 +1,135 @@
+#ifndef __WIT_C_SDK_H
+#define __WIT_C_SDK_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include "REG.h"
+
+
+#define WIT_HAL_OK      (0)     /**< There is no error */
+#define WIT_HAL_BUSY    (-1)    /**< Busy */
+#define WIT_HAL_TIMEOUT (-2)    /**< Timed out */
+#define WIT_HAL_ERROR   (-3)    /**< A generic error happens */
+#define WIT_HAL_NOMEM   (-4)    /**< No memory */
+#define WIT_HAL_EMPTY   (-5)    /**< The resource is empty */
+#define WIT_HAL_INVAL   (-6)    /**< Invalid argument */
+
+#define WIT_DATA_BUFF_SIZE  256
+
+#define WIT_PROTOCOL_NORMAL 0
+#define WIT_PROTOCOL_MODBUS 1
+#define WIT_PROTOCOL_CAN    2
+#define WIT_PROTOCOL_I2C    3
+
+
+/* serial function */
+typedef void (*SerialWrite)(uint8_t *p_ucData, uint32_t uiLen);
+int32_t WitSerialWriteRegister(SerialWrite write_func);
+void WitSerialDataIn(uint8_t ucData);
+
+/* iic function */
+
+/*
+    i2c write function example
+
+    int32_t WitI2cWrite(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen)
+    {
+        i2c_start();
+        i2c_send(ucAddr);
+        if(i2c_wait_ask() != SUCCESS)return 0;
+        i2c_send(ucReg);
+        if(i2c_wait_ask() != SUCCESS)return 0;
+        for(uint32_t i = 0; i < uiLen; i++)
+        {
+            i2c_send(*p_ucVal++); 
+            if(i2c_wait_ask() != SUCCESS)return 0;
+        }
+        i2c_stop();
+        return 1;
+    }
+*/
+typedef int32_t (*WitI2cWrite)(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen);
+/*
+    i2c read function example
+
+    int32_t WitI2cRead(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen)
+    {
+        i2c_start();
+        i2c_send(ucAddr);
+        if(i2c_wait_ask() != SUCCESS)return 0;
+        i2c_send(ucReg);
+        if(i2c_wait_ask() != SUCCESS)return 0;
+        
+        i2c_start();
+        i2c_send(ucAddr+1);
+        for(uint32_t i = 0; i < uiLen; i++)
+        {
+            if(i+1 == uiLen)*p_ucVal++ = i2c_read(0);  //last byte no ask
+            else *p_ucVal++ = i2c_read(1);  //  ask
+        }
+        i2c_stop();
+        return 1;
+    }
+*/
+typedef int32_t (*WitI2cRead)(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen);
+int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func);
+
+/* can function */
+typedef void (*CanWrite)(uint8_t ucStdId, uint8_t *p_ucData, uint32_t uiLen);
+int32_t WitCanWriteRegister(CanWrite write_func);
+
+/* Delayms function */
+typedef void (*DelaymsCb)(uint16_t ucMs);
+int32_t WitDelayMsRegister(DelaymsCb delayms_func);
+
+
+void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen);
+
+
+typedef void (*RegUpdateCb)(uint32_t uiReg, uint32_t uiRegNum);
+int32_t WitRegisterCallBack(RegUpdateCb update_func);
+int32_t WitWriteReg(uint32_t uiReg, uint16_t usData);
+int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum);
+int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr);
+void WitDeInit(void);
+
+
+
+/**
+  ******************************************************************************
+  * @file    wit_c_sdk.h
+  * @author  Wit
+  * @version V1.0
+  * @date    05-May-2022
+  * @brief   This file provides all Configure sensor function.
+  ******************************************************************************
+  * @attention
+  *
+  *        http://wit-motion.cn/
+  *
+  ******************************************************************************
+  */
+int32_t WitStartAccCali(void);
+int32_t WitStopAccCali(void);
+int32_t WitStartMagCali(void);
+int32_t WitStopMagCali(void);
+int32_t WitSetUartBaud(int32_t uiBaudIndex);
+int32_t WitSetBandwidth(int32_t uiBaudWidth);
+int32_t WitSetOutputRate(int32_t uiRate);
+int32_t WitSetContent(int32_t uiRsw);
+int32_t WitSetCanBaud(int32_t uiBaudIndex);
+
+char CheckRange(short sTemp,short sMin,short sMax);
+
+extern int16_t sReg[REGSIZE];
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __WIT_C_SDK_H */

HardwareDesign/HardwareDesign.ino

@@ -1,240 +1,74 @@
 #include "REG.h"
 #include "wit_c_sdk.h"
 
-// #define ACC_UPDATE 0x01
-// #define GYRO_UPDATE 0x02
-// #define ANGLE_UPDATE 0x04
-// #define MAG_UPDATE 0x08
-// #define READ_UPDATE 0x80
-
-// char s_cDataUpdate = 0;
-// const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
-//                               38400, 57600, 115200, 230400};
-
-// float fAcc[3], fGyro[3], fAngle[3];
-
-// bool setup_ok = true;
-
-// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
-// static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum);
-// static void Delayms(uint16_t ucMs);
-// static void AutoScanSensor();
-
-// void setup() {
-//   Serial.begin(115200);
-//   WitInit(WIT_PROTOCOL_NORMAL, 0x50);
-//   WitSerialWriteRegister(SensorUartSend);
-//   WitRegisterCallBack(SensorDataUpdate);
-//   WitDelayMsRegister(Delayms);
-//   AutoScanSensor();
-
-//   // Ensure the setting for the sensor is the default
-//   if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) {
-//     Serial.print("Set Bandwidth Error\r\n");
-//     setup_ok = false;
-//     return;
-//   }
-
-//   // if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) {
-//   //   Serial.print("Set baud error\r\n");
-//   //   setup_ok = false;
-//   //   return;
-//   // } else {
-//   //   Serial1.begin(115200);
-//   // }
-
-//   if (WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) {
-//     Serial.print("Can't set report rate to 10Hz!\r\n");
-//     setup_ok = false;
-//     return;
-//   }
-
-//   if (WitSetContent(RSW_ACC | RSW_GYRO | RSW_ANGLE | RSW_MAG) != WIT_HAL_OK)
-//   {
-//     Serial.print("Set RSW Error\r\n");
-//     setup_ok = false;
-//     return;
-//   }
-// }
-
-// void loop() {
-//   while (Serial1.available()) {
-//     WitSerialDataIn(Serial1.read());
-//   }
-
-//   // if (s_cDataUpdate) {
-//   //   for (int i = 0; i < 3; i++) {
-//   //     fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f;      // Unit: g(9.8m/s^2)
-//   //     fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f;   // Unit: deg/s
-//   //     fAngle[i] = sReg[Roll + i] / 32768.0f * 180.0f; // Unit: deg
-//   //   }
-
-//   //   if (s_cDataUpdate & ACC_UPDATE) {
-//   //     Serial.print("acc:");
-//   //     Serial.print(fAcc[0], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAcc[1], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAcc[2], 3);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~ACC_UPDATE;
-//   //   }
-//   //   if (s_cDataUpdate & GYRO_UPDATE) {
-//   //     Serial.print("gyro:");
-//   //     Serial.print(fGyro[0], 1);
-//   //     Serial.print(" ");
-//   //     Serial.print(fGyro[1], 1);
-//   //     Serial.print(" ");
-//   //     Serial.print(fGyro[2], 1);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~GYRO_UPDATE;
-//   //   }
-//   //   if (s_cDataUpdate & ANGLE_UPDATE) {
-//   //     Serial.print("angle:");
-//   //     Serial.print(fAngle[0], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAngle[1], 3);
-//   //     Serial.print(" ");
-//   //     Serial.print(fAngle[2], 3);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~ANGLE_UPDATE;
-//   //   }
-//   //   if (s_cDataUpdate & MAG_UPDATE) {
-//   //     Serial.print("mag:");
-//   //     Serial.print(sReg[HX]);
-//   //     Serial.print(" ");
-//   //     Serial.print(sReg[HY]);
-//   //     Serial.print(" ");
-//   //     Serial.print(sReg[HZ]);
-//   //     Serial.print("\r\n");
-//   //     s_cDataUpdate &= ~MAG_UPDATE;
-//   //   }
-//   //   s_cDataUpdate = 0;
-//   // }
-// }
-
-// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
-//   Serial1.print("Sensor says: ");
-//   Serial1.write(p_data, uiSize);
-//   Serial1.flush();
-// };
-
-// static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum) {
-//   int i;
-//   for (i = 0; i < uiRegNum; i++) {
-//     switch (uiReg) {
-//     case AZ:
-//       s_cDataUpdate |= ACC_UPDATE;
-//       break;
-//     case GZ:
-//       s_cDataUpdate |= GYRO_UPDATE;
-//       break;
-//     case HZ:
-//       s_cDataUpdate |= MAG_UPDATE;
-//       break;
-//     case Yaw:
-//       s_cDataUpdate |= ANGLE_UPDATE;
-//       break;
-//     default:
-//       s_cDataUpdate |= READ_UPDATE;
-//       break;
-//     }
-//     uiReg++;
-//   }
-//   Serial.println(s_cDataUpdate);
-// };
-
-// static void Delayms(uint16_t ucMs) { delay(ucMs); }
-
-// static void AutoScanSensor() {
-//   int i, iRetry;
-
-//   for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
-//     Serial1.begin(c_uiBaud[i]);
-//     Serial1.flush();
-//     iRetry = 2;
-//     s_cDataUpdate = 0;
-//     do {
-//       WitReadReg(AX, 3);
-//       delay(200);
-//       while (Serial1.available()) {
-//         WitSerialDataIn(Serial1.read());
-//       }
-//       if (s_cDataUpdate != 0) {
-//         Serial.print(c_uiBaud[i]);
-//         Serial.print(" baud find sensor\r\n\r\n");
-//         return;
-//       }
-//       iRetry--;
-//     } while (iRetry);
-//   }
-//   Serial.print("can not find sensor\r\n");
-//   Serial.print("please check your connection\r\n");
-// }
-
-/*
-Test on MEGA 2560. use WT901CTTL sensor
-
-WT901CTTL     MEGA 2560a
-    VCC <--->  5V/3.3V
-    TX  <--->  19(TX1)
-    RX  <--->  18(RX1)
-    GND <--->  GND
-*/
-
 #define ACC_UPDATE 0x01
 #define GYRO_UPDATE 0x02
 #define ANGLE_UPDATE 0x04
 #define MAG_UPDATE 0x08
 #define READ_UPDATE 0x80
-static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
 
-static void CmdProcess(void);
-static void AutoScanSensor(void);
-static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
-static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
-static void Delayms(uint16_t ucMs);
+char s_cDataUpdate = 0;
 const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
                               38400, 57600, 115200, 230400};
 
-int i;
 float fAcc[3], fGyro[3], fAngle[3];
 
+bool setup_ok = true;
+
+static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
+static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum);
+static void Delayms(uint16_t ucMs);
+static void AutoScanSensor();
+
 void setup() {
-  // put your setup code here, to run once:
   Serial.begin(115200);
   WitInit(WIT_PROTOCOL_NORMAL, 0x50);
   WitSerialWriteRegister(SensorUartSend);
-  WitRegisterCallBack(SensorDataUpdata);
+  WitRegisterCallBack(SensorDataUpdate);
   WitDelayMsRegister(Delayms);
   AutoScanSensor();
 
-  if (WitStartAccCali() == WIT_HAL_OK) {
-    Serial.print("Start Calibri");
-    delay(1000);
-    Serial.print("Stop calibri");
-    WitStopAccCali();
-  } else {
-    Serial.print("Cannot start calibri acc\r\n");
+  // Ensure the setting for the sensor is the default
+  if (WitSetBandwidth(BANDWIDTH_256HZ) != WIT_HAL_OK) {
+    Serial.print("Set Bandwidth Error\r\n");
+    setup_ok = false;
+    return;
   }
 
-  if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK)
-    Serial.print("\r\nSet Baud Error\r\n");
-  else {
-    Serial1.begin(c_uiBaud[WIT_BAUD_115200]);
-    Serial.print(" 115200 Baud rate modified successfully\r\n");
+  // if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) {
+  //   Serial.print("Set baud error\r\n");
+  //   setup_ok = false;
+  //   return;
+  // } else {
+  //   Serial1.begin(115200);
+  // }
+
+  if (WitSetOutputRate(RRATE_1HZ) != WIT_HAL_OK) {
+    Serial.print("Can't set report rate to 10Hz!\r\n");
+    setup_ok = false;
+    return;
+  }
+
+  if (WitSetContent(RSW_ACC | RSW_GYRO | RSW_ANGLE | RSW_MAG) != WIT_HAL_OK)
+  {
+    Serial.print("Set RSW Error\r\n");
+    setup_ok = false;
+    return;
   }
 }
+
 void loop() {
   while (Serial1.available()) {
     WitSerialDataIn(Serial1.read());
   }
+
   if (s_cDataUpdate) {
-    for (i = 0; i < 3; i++) {
-      fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f * 9.8f; // Unit: m/s^2
-      fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f; // Unit: deg/s
+    for (int i = 0; i < 3; i++) {
+      fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f;      // Unit: g(9.8m/s^2)
+      fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f;   // Unit: deg/s
       fAngle[i] = sReg[Roll + i] / 32768.0f * 180.0f; // Unit: deg
     }
+    Serial.print("Here");
     if (s_cDataUpdate & ACC_UPDATE) {
       Serial.print("acc:");
       Serial.print(fAcc[0], 3);
@@ -282,9 +116,9 @@ void loop() {
 static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
   Serial1.write(p_data, uiSize);
   Serial1.flush();
-}
-static void Delayms(uint16_t ucMs) { delay(ucMs); }
-static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
+};
+
+static void SensorDataUpdate(uint32_t uiReg, uint32_t uiRegNum) {
   int i;
   for (i = 0; i < uiRegNum; i++) {
     switch (uiReg) {
@@ -306,9 +140,12 @@ static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
     }
     uiReg++;
   }
-}
+  // Serial.println(s_cDataUpdate);
+};
 
-static void AutoScanSensor(void) {
+static void Delayms(uint16_t ucMs) { delay(ucMs); }
+
+static void AutoScanSensor() {
   int i, iRetry;
 
   for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
@@ -333,3 +170,165 @@ static void AutoScanSensor(void) {
   Serial.print("can not find sensor\r\n");
   Serial.print("please check your connection\r\n");
 }
+
+/*
+Test on MEGA 2560. use WT901CTTL sensor
+
+WT901CTTL     MEGA 2560a
+    VCC <--->  5V/3.3V
+    TX  <--->  19(TX1)
+    RX  <--->  18(RX1)
+    GND <--->  GND
+*/
+
+// #define ACC_UPDATE 0x01
+// #define GYRO_UPDATE 0x02
+// #define ANGLE_UPDATE 0x04
+// #define MAG_UPDATE 0x08
+// #define READ_UPDATE 0x80
+// static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;
+
+// static void CmdProcess(void);
+// static void AutoScanSensor(void);
+// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
+// static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
+// static void Delayms(uint16_t ucMs);
+// const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
+//                               38400, 57600, 115200, 230400};
+
+// int i;
+// float fAcc[3], fGyro[3], fAngle[3];
+
+// void setup() {
+//   // put your setup code here, to run once:
+//   Serial.begin(115200);
+//   WitInit(WIT_PROTOCOL_NORMAL, 0x50);
+//   WitSerialWriteRegister(SensorUartSend);
+//   WitRegisterCallBack(SensorDataUpdata);
+//   WitDelayMsRegister(Delayms);
+//   AutoScanSensor();
+
+//   // if (WitStartAccCali() == WIT_HAL_OK) {
+//   //   Serial.print("Start Calibri");
+//   //   delay(1000);
+//   //   Serial.print("Stop calibri");
+//   //   WitStopAccCali();
+//   // } else {
+//   //   Serial.print("Cannot start calibri acc\r\n");
+//   // }
+
+//   if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
+//     Serial.print("\r\nSet Baud Error\r\n");
+//   else {
+//     Serial1.begin(c_uiBaud[WIT_BAUD_9600]);
+//     Serial.print(" 9600 Baud rate modified successfully\r\n");
+//   }
+// }
+// void loop() {
+//   while (Serial1.available()) {
+//     WitSerialDataIn(Serial1.read());
+//   }
+//   if (s_cDataUpdate) {
+//     for (i = 0; i < 3; i++) {
+//       fAcc[i] = sReg[AX + i] / 32768.0f * 16.0f * 9.8f; // Unit: m/s^2
+//       fGyro[i] = sReg[GX + i] / 32768.0f * 2000.0f; // Unit: deg/s
+//       fAngle[i] = sReg[Roll + i] / 32768.0f * 180.0f; // Unit: deg
+//     }
+//     if (s_cDataUpdate & ACC_UPDATE) {
+//       Serial.print("acc:");
+//       Serial.print(fAcc[0], 3);
+//       Serial.print(" ");
+//       Serial.print(fAcc[1], 3);
+//       Serial.print(" ");
+//       Serial.print(fAcc[2], 3);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~ACC_UPDATE;
+//     }
+//     if (s_cDataUpdate & GYRO_UPDATE) {
+//       Serial.print("gyro:");
+//       Serial.print(fGyro[0], 1);
+//       Serial.print(" ");
+//       Serial.print(fGyro[1], 1);
+//       Serial.print(" ");
+//       Serial.print(fGyro[2], 1);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~GYRO_UPDATE;
+//     }
+//     if (s_cDataUpdate & ANGLE_UPDATE) {
+//       Serial.print("angle:");
+//       Serial.print(fAngle[0], 3);
+//       Serial.print(" ");
+//       Serial.print(fAngle[1], 3);
+//       Serial.print(" ");
+//       Serial.print(fAngle[2], 3);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~ANGLE_UPDATE;
+//     }
+//     if (s_cDataUpdate & MAG_UPDATE) {
+//       Serial.print("mag:");
+//       Serial.print(sReg[HX]);
+//       Serial.print(" ");
+//       Serial.print(sReg[HY]);
+//       Serial.print(" ");
+//       Serial.print(sReg[HZ]);
+//       Serial.print("\r\n");
+//       s_cDataUpdate &= ~MAG_UPDATE;
+//     }
+//     s_cDataUpdate = 0;
+//   }
+// }
+
+// static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
+//   Serial1.write(p_data, uiSize);
+//   Serial1.flush();
+// }
+// static void Delayms(uint16_t ucMs) { delay(ucMs); }
+// static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
+//   int i;
+//   for (i = 0; i < uiRegNum; i++) {
+//     switch (uiReg) {
+//     case AZ:
+//       s_cDataUpdate |= ACC_UPDATE;
+//       break;
+//     case GZ:
+//       s_cDataUpdate |= GYRO_UPDATE;
+//       break;
+//     case HZ:
+//       s_cDataUpdate |= MAG_UPDATE;
+//       break;
+//     case Yaw:
+//       s_cDataUpdate |= ANGLE_UPDATE;
+//       break;
+//     default:
+//       s_cDataUpdate |= READ_UPDATE;
+//       break;
+//     }
+//     uiReg++;
+//   }
+// }
+
+// static void AutoScanSensor(void) {
+//   int i, iRetry;
+
+//   for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
+//     Serial1.begin(c_uiBaud[i]);
+//     Serial1.flush();
+//     iRetry = 2;
+//     s_cDataUpdate = 0;
+//     do {
+//       WitReadReg(AX, 3);
+//       delay(200);
+//       while (Serial1.available()) {
+//         WitSerialDataIn(Serial1.read());
+//       }
+//       if (s_cDataUpdate != 0) {
+//         Serial.print(c_uiBaud[i]);
+//         Serial.print(" baud find sensor\r\n\r\n");
+//         return;
+//       }
+//       iRetry--;
+//     } while (iRetry);
+//   }
+//   Serial.print("can not find sensor\r\n");
+//   Serial.print("please check your connection\r\n");
+// }

Test/Test.ino

@@ -1,50 +1,14 @@
-byte status = 0;
-byte layer = 0;
-
 void setup() {
-  Serial.begin(115200);
+  pinMode(A7, INPUT_PULLUP);
+  pinMode(2, INPUT_PULLUP);
   for (int i = 0; i < 8; i++) {
     pinMode(22 + i, OUTPUT);
+    digitalWrite(22 + i, HIGH);
   }
-  // pinMode(30, OUTPUT);
-  // pinMode(31, OUTPUT);
-
-  for (int i = 0; i < 8; i++) {
-    pinMode(30 + i, OUTPUT);
-  }
-
-  for (int i = 0; i < 8; i++) {
-    pinMode(38 + i, OUTPUT);
-  }
+  Serial.begin(115200);
 }
 
-int column = 0;
-
 void loop() {
-  if (Serial.available()) {
-    int num = Serial.read();
-    if (num >= '0' && num <= '7') {
-      num = num - '0';
-      status ^= 1 << num;
-
-      for (int i = 0; i < 8; i++) {
-        digitalWrite(22 + i, status >> i & 1);
-      }
-
-      Serial.println(status, BIN);
-    } else if (num >= 'a' && num <= 'h') {
-      num = num - 'a';
-      column = num;
-      Serial.print("LOAD");
-      Serial.print(num);
-      Serial.println(status, BIN);
-      digitalWrite(30 + column, HIGH);
-      digitalWrite(30 + column, LOW);
-    }
-    else if (num == 'z') {
-      digitalWrite(38 + layer, LOW);
-      layer = (layer + 1) % 8;
-      digitalWrite(38 + layer, HIGH);
-    }
-  }
+  Serial.println(digitalRead(2));
+  delay(100);
 }