Merge branch '函数图像_传感器'
This commit is contained in:
unlockable
209
.clang-format
Normal file
209
.clang-format
Normal 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
329
8By8/8By8.ino
@@ -1,182 +1,231 @@
|
||||
#include "ListE.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 CLOCK_START_PIN 30
|
||||
#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 {
|
||||
private:
|
||||
static int layer_count;
|
||||
static int brightness_count;
|
||||
static int layer_count;
|
||||
static int brightness_count;
|
||||
static bool blinking_LED_status;
|
||||
static bool blinking_LED_status;
|
||||
|
||||
public:
|
||||
// Every int represents a row of 8 LED status.
|
||||
static uint16_t LED_status[8][8];
|
||||
static uint16_t LED_brightness[8][8];
|
||||
static uint8_t LED_blinking_status[8][8];
|
||||
static int LED_status[8][8];
|
||||
|
||||
static void set_blinking(int x, int y, int z) {
|
||||
// 1 = enable blinking
|
||||
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);
|
||||
}
|
||||
|
||||
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() {
|
||||
// Serial.println("Here");
|
||||
if (brightness_count >= 2) {
|
||||
digitalWrite(SW_START_PIN + layer_count, LOW);
|
||||
layer_count = (layer_count + 1) % 8;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void display() {
|
||||
// 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;
|
||||
brightness_count = (brightness_count + 1) % 3;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
for (int j = 0; j < 8; j++) {
|
||||
// In LED_status:
|
||||
// 0 = off
|
||||
// 4 = brightest
|
||||
digitalWrite(BUS_START_PIN + j,
|
||||
((LED_status[layer_count][i] >> (j * 2)) & 3) >=
|
||||
(3 - brightness_count));
|
||||
}
|
||||
digitalWrite(CLOCK_START_PIN + i, HIGH);
|
||||
digitalWrite(CLOCK_START_PIN + i, LOW);
|
||||
for (int j = 0; j < 8; j++) {
|
||||
// In LED_status:
|
||||
// 0 = off
|
||||
// 4 = brightest
|
||||
digitalWrite(BUS_START_PIN + j, ((LED_status[layer_count][i] >> (j * 2)) & 3) >= (3 - brightness_count));
|
||||
}
|
||||
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) {
|
||||
if (x >= 8 || x < 0 || y >= 8 || y < 0 || z >= 8 || z < 0) {
|
||||
return;
|
||||
return;
|
||||
}
|
||||
brightness %= 4;
|
||||
LED_status[z][x] =
|
||||
(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;
|
||||
}
|
||||
LED_status[z][x] = (LED_status[z][x] & (~(3 << (y * 2)))) | (brightness << (y * 2));
|
||||
}
|
||||
};
|
||||
|
||||
int Cube::layer_count = 0;
|
||||
int Cube::brightness_count = 0;
|
||||
uint16_t 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 Cube::LED_status[8][8] = {0};
|
||||
int bright = 3;
|
||||
|
||||
void setup() {
|
||||
for (int i = 22; i < 46; i++) {
|
||||
pinMode(i, OUTPUT);
|
||||
}
|
||||
for (int i = 22; i < 46; i++) {
|
||||
pinMode(i, OUTPUT);
|
||||
}
|
||||
|
||||
Timer1.initialize();
|
||||
Timer1.setPeriod(TIME_PER_LAYER_IN_US);
|
||||
Timer1.attachInterrupt(Cube::display);
|
||||
Timer1.initialize();
|
||||
Timer1.setPeriod(TIME_PER_LAYER_IN_US);
|
||||
Timer1.attachInterrupt(Cube::display);
|
||||
|
||||
cli();
|
||||
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();
|
||||
Serial.begin(115200);
|
||||
|
||||
// Cube::set_blinking(1, 1, 1);
|
||||
Serial.begin(115200);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
for (int i = 7; i >= 0; i--) {
|
||||
for (int j = 0; j < 8; j++) {
|
||||
for (int k = 0; k < 8; k++) {
|
||||
Cube::set_status(i, j, k, 3);
|
||||
delay(5);
|
||||
}
|
||||
|
||||
for (int i = 7; i >= 0; i--) {
|
||||
for (int j = 0; j < 8; j++) {
|
||||
for (int k = 0; k < 8; k++) {
|
||||
Cube::set_status(i, j, k, bright);
|
||||
delay(5);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
bright = (bright + 1) % 4;
|
||||
}
|
||||
|
||||
ISR(TIMER4_COMPA_vect) { Cube::do_blinking(); }
|
||||
148
8By8/ListE.h
Normal file
148
8By8/ListE.h
Normal 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
105
8By8/Node.h
Normal 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
284
8By8/REG.h
Normal 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
502
8By8/wit_c_sdk.c
Normal 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
135
8By8/wit_c_sdk.h
Normal 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 */
|
||||
@@ -218,10 +218,10 @@ void setup() {
|
||||
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");
|
||||
else {
|
||||
Serial1.begin(c_uiBaud[WIT_BAUD_115200]);
|
||||
Serial1.begin(c_uiBaud[WIT_BAUD_9600]);
|
||||
Serial.print(" 115200 Baud rate modified successfully\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user