unlockable/HardwareDesign
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch '函数图像_传感器'

Browse Source
This commit is contained in:
unlockable
2023-09-08 13:57:07 +08:00
parent bfa985f830 b09d680754
commit 35eb89d5a9
8 changed files with 1574 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

209
.clang-format Normal file
View File

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

329
8By8/8By8.ino
View File

@@ -1,182 +1,231 @@
#include "ListE.h"
#include "TimerOne.h" #include "TimerOne.h"
#include <string.h>
#define TIME_PER_LAYER_IN_US 700 #include "REG.h"
#include "wit_c_sdk.h"
#define TIME_PER_LAYER_IN_US 800
#define BUS_START_PIN 22 #define BUS_START_PIN 22
#define CLOCK_START_PIN 30 #define CLOCK_START_PIN 30
#define SW_START_PIN 38 #define SW_START_PIN 38
#define JOYSTICK_VRX A1
#define JOYSTICK_VRY A0
#define JOYSTICK_SWITCH 2
#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80
#define DEBUG false
struct Triple {
float x;
float y;
float z;
};
class SensorReader {
private:
static const uint32_t c_uiBaud[8];
static volatile byte s_cDataUpdate;
static void AutoScanSensor() {
int iRetry;
for (int i = 0; i < sizeof(SensorReader::c_uiBaud) /
sizeof(SensorReader::c_uiBaud[0]);
i++) {
Serial1.begin(SensorReader::c_uiBaud[i]);
Serial1.flush();
iRetry = 2;
SensorReader::s_cDataUpdate = 0;
do {
WitReadReg(AX, 3);
delay(200);
while (Serial1.available()) {
WitSerialDataIn(Serial1.read());
}
if (SensorReader::s_cDataUpdate != 0) {
Serial.print(SensorReader::c_uiBaud[i]);
Serial.print(" baud find sensor\r\n\r\n");
return;
}
iRetry--;
} while (iRetry);
}
Serial.println("Can not find sensor, please check your connection");
}
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
Serial1.write(p_data, uiSize);
Serial1.flush();
}
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
int i;
for (i = 0; i < uiRegNum; i++) {
switch (uiReg) {
case AZ:
SensorReader::s_cDataUpdate |= ACC_UPDATE;
break;
case GZ:
SensorReader::s_cDataUpdate |= GYRO_UPDATE;
break;
case HZ:
SensorReader::s_cDataUpdate |= MAG_UPDATE;
break;
case Yaw:
SensorReader::s_cDataUpdate |= ANGLE_UPDATE;
break;
default:
SensorReader::s_cDataUpdate |= READ_UPDATE;
break;
}
uiReg++;
}
}
static void Delayms(uint16_t ucMs) {
delay(ucMs);
}
public:
static Triple angle, acceleration;
static bool init() {
WitInit(WIT_PROTOCOL_NORMAL, 0x50);
WitSerialWriteRegister(SensorReader::SensorUartSend);
WitRegisterCallBack(SensorReader::SensorDataUpdata);
WitDelayMsRegister(SensorReader::Delayms);
SensorReader::AutoScanSensor();
if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK) {
Serial.println("Set Baud Error");
return false;
}
else {
Serial1.begin(SensorReader::c_uiBaud[WIT_BAUD_9600]);
// Serial.println("9600 Baud rate modified successfully");
}
if (WitSetContent(RSW_ANGLE | RSW_ACC) != WIT_HAL_OK) {
Serial.println("Set send content: angle, acc Error");
return false;
}
if (WitSetOutputRate(RRATE_2HZ) != WIT_HAL_OK) {
Serial.print("Set report rate failed");
return false;
}
return true;
}
static void read_data_from_sensor_interrupt() {
// Serial.println("Updated!");
while (Serial1.available()) {
WitSerialDataIn(Serial1.read());
}
if (SensorReader::s_cDataUpdate) {
if (SensorReader::s_cDataUpdate & ANGLE_UPDATE) {
SensorReader::angle.x = sReg[Roll] / 32768.0f * 180.0f;
SensorReader::angle.y = sReg[Roll + 1] / 32768.0f * 180.0f;
SensorReader::angle.z =
sReg[Roll + 2] / 32768.0f * 180.0f; // Unit: deg
SensorReader::s_cDataUpdate &= ~ANGLE_UPDATE;
SensorReader::s_cDataUpdate = 0;
}
if (SensorReader::s_cDataUpdate & ACC_UPDATE) {
SensorReader::acceleration.x =
sReg[AX] / 32768.0f * 16.0f * 9.8f;
SensorReader::acceleration.y =
sReg[AX + 1] / 32768.0f * 16.0f * 9.8f;
SensorReader::acceleration.z =
sReg[AX + 2] / 32768.0f * 16.0f * 9.8f;
}
}
}
};
class Cube { class Cube {
private: private:
static int layer_count; static int layer_count;
static int brightness_count; static int brightness_count;
static bool blinking_LED_status; static bool blinking_LED_status;
static bool blinking_LED_status;
public: public:
// Every int represents a row of 8 LED status. // Every int represents a row of 8 LED status.
static uint16_t LED_status[8][8]; static int LED_status[8][8];
static uint16_t LED_brightness[8][8];
static uint8_t LED_blinking_status[8][8];
static void set_blinking(int x, int y, int z) { static void display() {
// 1 = enable blinking // Serial.println("Here");
if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) { if (brightness_count >= 2) {
return; digitalWrite(SW_START_PIN + layer_count, LOW);
} layer_count = (layer_count + 1) % 8;
LED_blinking_status[z][x] = LED_blinking_status[z][x] | (1 << y);
}
static void unset_blinking(int x, int y, int z) {
if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
return;
}
LED_blinking_status[z][x] = LED_blinking_status[z][x] & (~1 << y);
LED_status[z][x] = (LED_status[z][x] & (~(3 << (y * 2)))) |
(LED_brightness[z][x] & (3 << (y * 2)));
}
static void do_blinking() {
blinking_LED_status ^= 1;
for (int i = 0; i < 8; i++) {
for (int j = 0; j < 8; j++) {
for (int k = 0; k < 8; k++) {
if (LED_blinking_status[i][j] >> k & 1) {
if (blinking_LED_status) {
LED_status[i][j] = LED_brightness[i][j];
} else {
LED_status[i][j] = LED_status[i][j] & (~(3 << (k * 2)));
}
}
} }
}
}
}
static void display() { brightness_count = (brightness_count + 1) % 3;
// Serial.println("Here");
if (brightness_count >= 2) {
digitalWrite(SW_START_PIN + layer_count, LOW);
layer_count = (layer_count + 1) % 8;
}
brightness_count = (brightness_count + 1) % 3;
for (int i = 0; i < 8; i++) { for (int i = 0; i < 8; i++) {
for (int j = 0; j < 8; j++) { for (int j = 0; j < 8; j++) {
// In LED_status: // In LED_status:
// 0 = off // 0 = off
// 4 = brightest // 4 = brightest
digitalWrite(BUS_START_PIN + j, digitalWrite(BUS_START_PIN + j, ((LED_status[layer_count][i] >> (j * 2)) & 3) >= (3 - brightness_count));
((LED_status[layer_count][i] >> (j * 2)) & 3) >= }
(3 - brightness_count)); digitalWrite(CLOCK_START_PIN + i, HIGH);
} digitalWrite(CLOCK_START_PIN + i, LOW);
digitalWrite(CLOCK_START_PIN + i, HIGH);
digitalWrite(CLOCK_START_PIN + i, LOW);
} }
digitalWrite(SW_START_PIN + layer_count, HIGH); digitalWrite(SW_START_PIN + layer_count, HIGH);
} }
static void set_status(int x, int y, int z, int brightness) { static void set_status(int x, int y, int z, int brightness) {
if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) { if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
return; return;
} }
brightness %= 4; brightness %= 4;
LED_status[z][x] = LED_status[z][x] = (LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
(LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
LED_brightness[z][x] =
(LED_brightness[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
}
static int get_status(int x, int y, int z) {
return LED_brightness[z][x] >> (y * 2) & 3;
}
static void clear() {
for (int i = 0; i < 8; i++) {
for (int j = 0; j < 8; j++) {
LED_brightness[i][j] = 0;
LED_status[i][j] = 0;
}
}
}
static void draw_line(int x, int y, int z, int length, int direction,
int brightness) {
if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
return;
}
if (direction >= 3 || direction < 0 || length <= 0 || brightness >= 4 ||
brightness < 0) {
return;
}
// 0: x
// 1: y
// 2: z
switch (direction) {
case 0:
for (int i = 0; i < length; i++) {
set_status(x + i, y, z, brightness);
}
break;
case 1:
for (int i = 0; i < length; i++) {
set_status(x, y + i, z, brightness);
}
break;
case 2:
for (int i = 0; i < length; i++) {
set_status(x, y, z + i, brightness);
}
break;
}
} }
}; };
int Cube::layer_count = 0; int Cube::layer_count = 0;
int Cube::brightness_count = 0; int Cube::brightness_count = 0;
uint16_t Cube::LED_status[8][8] = {0}; int Cube::LED_status[8][8] = {0};
uint16_t Cube::LED_brightness[8][8] = {0};
uint8_t Cube::LED_blinking_status[8][8] = {0};
bool Cube::blinking_LED_status = false;
int bright = 3; int bright = 3;
void setup() { void setup() {
for (int i = 22; i < 46; i++) { for (int i = 22; i < 46; i++) {
pinMode(i, OUTPUT); pinMode(i, OUTPUT);
} }
Timer1.initialize(); Timer1.initialize();
Timer1.setPeriod(TIME_PER_LAYER_IN_US); Timer1.setPeriod(TIME_PER_LAYER_IN_US);
Timer1.attachInterrupt(Cube::display); Timer1.attachInterrupt(Cube::display);
cli(); Serial.begin(115200);
TCCR4A = 0; // set entire TCCR1A register to 0
TCCR4B = 0; // same for TCCR1B
TCNT4 = 0; // initialize counter value to 0
// set compare match register for 1hz increments
OCR4A = 7500 / 1; // = (16*10^6) / (1*1024) - 1 (must be <65536)
// turn on CTC mode
TCCR4B |= (1 << WGM12);
// Set CS12 and CS10 bits for 1024 prescaler
TCCR4B |= (1 << CS12) | (1 << CS10);
// enable timer compare interrupt
TIMSK4 |= (1 << OCIE4A);
sei();
// Cube::set_blinking(1, 1, 1);
Serial.begin(115200);
} }
void loop() { void loop() {
for (int i = 7; i >= 0; i--) {
for (int j = 0; j < 8; j++) { for (int i = 7; i >= 0; i--) {
for (int k = 0; k < 8; k++) { for (int j = 0; j < 8; j++) {
Cube::set_status(i, j, k, 3); for (int k = 0; k < 8; k++) {
delay(5); Cube::set_status(i, j, k, bright);
} delay(5);
}
}
} }
} bright = (bright + 1) % 4;
} }
ISR(TIMER4_COMPA_vect) { Cube::do_blinking(); } ISR(TIMER4_COMPA_vect) { Cube::do_blinking(); }

148
8By8/ListE.h Normal file
View File

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

105
8By8/Node.h Normal file
View File

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

284
8By8/REG.h Normal file
View File

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

502
8By8/wit_c_sdk.c Normal file
View File

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

135
8By8/wit_c_sdk.h Normal file
View File

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

4
HardwareDesign/HardwareDesign.ino
View File

@@ -218,10 +218,10 @@ void setup() {
Serial.print("Cannot start calibri acc\r\n"); Serial.print("Cannot start calibri acc\r\n");
} }
if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK) if (WitSetUartBaud(WIT_BAUD_9600) != WIT_HAL_OK)
Serial.print("\r\nSet Baud Error\r\n"); Serial.print("\r\nSet Baud Error\r\n");
else { else {
Serial1.begin(c_uiBaud[WIT_BAUD_115200]); Serial1.begin(c_uiBaud[WIT_BAUD_9600]);
Serial.print(" 115200 Baud rate modified successfully\r\n"); Serial.print(" 115200 Baud rate modified successfully\r\n");
} }
} }