From 23e8cc8333a0f7c38d263503e79d842a98d6806c Mon Sep 17 00:00:00 2001 From: unlockable Date: Thu, 7 Sep 2023 13:40:56 +0800 Subject: [PATCH] Connect and find sensor --- 8By8/8By8.ino | 270 ++++++++++++---- 8By8/REG.h | 284 +++++++++++++++++ 8By8/wit_c_sdk.c | 502 ++++++++++++++++++++++++++++++ 8By8/wit_c_sdk.h | 135 ++++++++ HardwareDesign/HardwareDesign.ino | 4 +- 5 files changed, 1136 insertions(+), 59 deletions(-) create mode 100644 8By8/REG.h create mode 100644 8By8/wit_c_sdk.c create mode 100644 8By8/wit_c_sdk.h diff --git a/8By8/8By8.ino b/8By8/8By8.ino index 6b40e8e..0dc8d58 100644 --- a/8By8/8By8.ino +++ b/8By8/8By8.ino @@ -2,13 +2,146 @@ #include "TimerOne.h" #include +#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 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 Triple angle; + + 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 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) != WIT_HAL_OK) { + Serial.print("\r\nSet Content Angle Error\r\n"); + return false; + } + else { + Serial.println("Set Content Angle success"); + } + return true; + } + + static void read_data_from_sensor_interrupt() { + while (Serial1.available()) { + WitSerialDataIn(Serial.read()); + } + + if (SensorReader::s_cDataUpdate) { + if (SensorReader::s_cDataUpdate & ANGLE_UPDATE) { + Serial.println("Updated!"); + 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; + } + } + } + + static Triple get_angle() { + return SensorReader::angle; + } +}; + class Cube { private: static int layer_count; @@ -65,11 +198,6 @@ public: } }; -int Cube::layer_count = 0; -int Cube::brightness_count = 0; -int Cube::LED_status[8][8] = {0}; -int bright = 3; - enum Operators { plus, minus, @@ -425,10 +553,21 @@ public: } }; +int Cube::layer_count = 0; +int Cube::brightness_count = 0; +int Cube::LED_status[8][8] = {0}; + double *Symbol::x_value_ptr, *Symbol::y_value_ptr; +const uint32_t SensorReader::c_uiBaud[8] = {0, 4800, 9600, 19200, + 38400, 57600, 115200, 230400}; +volatile byte SensorReader::s_cDataUpdate; +Triple SensorReader::angle; + double x, y; +const String allowed_chars = "0123456789+-*/^()xy"; + void setup() { for (int i = 22; i < 46; i++) { pinMode(i, OUTPUT); @@ -436,67 +575,84 @@ void setup() { Timer1.initialize(); Timer1.setPeriod(TIME_PER_LAYER_IN_US); - Timer1.attachInterrupt(Cube::display); Serial.begin(9600); Symbol::x_value_ptr = &x; Symbol::y_value_ptr = &y; + + if (!SensorReader::init()) { + Serial.println("Initialize sensor failed."); + } + + Timer1.attachInterrupt(Cube::display); } void loop() { - String input = ""; - String allowed_chars = "0123456789+-*/^()xy"; - while (true) { - if (Serial.available()) { - byte input_char = Serial.read(); - // Serial.print("Pos: "); - // Serial.println(allowed_chars.indexOf(input_char)); - if (allowed_chars.indexOf(input_char) != -1) { - input.concat((char)input_char); - // Serial.print("String: "); - // Serial.println(input); - continue; - } + Triple angle; - if (input_char == 0x3) { - break; - } - } - } - Serial.print("Read: "); - Serial.println(input); - Calculator calculator(input); - int x_translate, y_translate; + angle = SensorReader::get_angle(); + Serial.print("x: "); + Serial.println(angle.x); + Serial.print("y: "); + Serial.println(angle.y); + Serial.print("z: "); + Serial.println(angle.z); - for (x_translate = 0; x_translate < 4; x_translate++) { - for (y_translate = 0; y_translate < 4; y_translate++) { - Cube::clear(); + delay(500); - for (int i = 0; i < 8; i++) { - for (int j = 0; j < 8; j++) { - x = i - x_translate; - y = j - y_translate; - double result = calculator.evaluate(); - int z; - if (!(isinf(result) || isnan(result))) { - z = round(result); - Cube::set_status(i, j, z, 3); - } - else { - z = 0; - } - // Serial.print(x); - // Serial.print(" "); - // Serial.print(y); - // Serial.print(" "); - // Serial.println(z); - } - } - delay(1000); - } - } + // String input = ""; + // Serial.println("Input expression"); + // while (true) { + // if (Serial.available()) { + // byte input_char = Serial.read(); + // // Serial.print("Pos: "); + // // Serial.println(allowed_chars.indexOf(input_char)); + // if (allowed_chars.indexOf(input_char) != -1) { + // input.concat((char)input_char); + // // Serial.print("String: "); + // // Serial.println(input); + // continue; + // } - // int z = round(calculator.evaluate()); - // Serial.println(z); - Serial.print("Here"); + // if (input_char == 0x3) { + // break; + // } + // } + // } + // Serial.print("Read: "); + // Serial.println(input); + // Calculator calculator(input); + // int x_offset, y_offset; + + // for (x_offset = 0; x_offset < 4; x_offset++) { + // for (y_offset = 0; y_offset < 4; y_offset++) { + // Cube::clear(); + + // for (int i = 0; i < 8; i++) { + // for (int j = 0; j < 8; j++) { + // x = i - x_offset; + // y = j - y_offset; + // double result = calculator.evaluate(); + // int z; + // if (!(isinf(result) || isnan(result))) { + // z = round(result); + // Cube::set_status(i, j, z, 3); + // } + // else { + // z = 0; + // } + // // Serial.print(x); + // // Serial.print(" "); + // // Serial.print(y); + // // Serial.print(" "); + // // Serial.println(z); + // } + // } + // delay(1000); + // } + // } + + // // int z = round(calculator.evaluate()); + // // Serial.println(z); + // Serial.print("Here"); } \ No newline at end of file diff --git a/8By8/REG.h b/8By8/REG.h new file mode 100644 index 0000000..b0c1dfa --- /dev/null +++ b/8By8/REG.h @@ -0,0 +1,284 @@ +#ifndef __AHRSREG_H +#define __AHRSREG_H + +#ifdef __cplusplus +extern "C" { +#endif +#define REGSIZE 0x90 + +#define SAVE 0x00 +#define CALSW 0x01 +#define RSW 0x02 +#define RRATE 0x03 +#define BAUD 0x04 +#define AXOFFSET 0x05 +#define AYOFFSET 0x06 +#define AZOFFSET 0x07 +#define GXOFFSET 0x08 +#define GYOFFSET 0x09 +#define GZOFFSET 0x0a +#define HXOFFSET 0x0b +#define HYOFFSET 0x0c +#define HZOFFSET 0x0d +#define D0MODE 0x0e +#define D1MODE 0x0f +#define D2MODE 0x10 +#define D3MODE 0x11 +#define D0PWMH 0x12 +#define D1PWMH 0x13 +#define D2PWMH 0x14 +#define D3PWMH 0x15 +#define D0PWMT 0x16 +#define D1PWMT 0x17 +#define D2PWMT 0x18 +#define D3PWMT 0x19 +#define IICADDR 0x1a +#define LEDOFF 0x1b +#define MAGRANGX 0x1c +#define MAGRANGY 0x1d +#define MAGRANGZ 0x1e +#define BANDWIDTH 0x1f +#define GYRORANGE 0x20 +#define ACCRANGE 0x21 +#define SLEEP 0x22 +#define ORIENT 0x23 +#define AXIS6 0x24 +#define FILTK 0x25 +#define GPSBAUD 0x26 +#define READADDR 0x27 +#define BWSCALE 0x28 +#define MOVETHR 0x28 +#define MOVESTA 0x29 +#define ACCFILT 0x2A +#define GYROFILT 0x2b +#define MAGFILT 0x2c +#define POWONSEND 0x2d +#define VERSION 0x2e +#define CCBW 0x2f +#define YYMM 0x30 +#define DDHH 0x31 +#define MMSS 0x32 +#define MS 0x33 +#define AX 0x34 +#define AY 0x35 +#define AZ 0x36 +#define GX 0x37 +#define GY 0x38 +#define GZ 0x39 +#define HX 0x3a +#define HY 0x3b +#define HZ 0x3c +#define Roll 0x3d +#define Pitch 0x3e +#define Yaw 0x3f +#define TEMP 0x40 +#define D0Status 0x41 +#define D1Status 0x42 +#define D2Status 0x43 +#define D3Status 0x44 +#define PressureL 0x45 +#define PressureH 0x46 +#define HeightL 0x47 +#define HeightH 0x48 +#define LonL 0x49 +#define LonH 0x4a +#define LatL 0x4b +#define LatH 0x4c +#define GPSHeight 0x4d +#define GPSYAW 0x4e +#define GPSVL 0x4f +#define GPSVH 0x50 +#define q0 0x51 +#define q1 0x52 +#define q2 0x53 +#define q3 0x54 +#define SVNUM 0x55 +#define PDOP 0x56 +#define HDOP 0x57 +#define VDOP 0x58 +#define DELAYT 0x59 +#define XMIN 0x5a +#define XMAX 0x5b +#define BATVAL 0x5c +#define ALARMPIN 0x5d +#define YMIN 0x5e +#define YMAX 0x5f +#define GYROZSCALE 0x60 +#define GYROCALITHR 0x61 +#define ALARMLEVEL 0x62 +#define GYROCALTIME 0x63 +#define REFROLL 0x64 +#define REFPITCH 0x65 +#define REFYAW 0x66 +#define GPSTYPE 0x67 +#define TRIGTIME 0x68 +#define KEY 0x69 +#define WERROR 0x6a +#define TIMEZONE 0x6b +#define CALICNT 0x6c +#define WZCNT 0x6d +#define WZTIME 0x6e +#define WZSTATIC 0x6f +#define ACCSENSOR 0x70 +#define GYROSENSOR 0x71 +#define MAGSENSOR 0x72 +#define PRESSENSOR 0x73 +#define MODDELAY 0x74 + +#define ANGLEAXIS 0x75 +#define XRSCALE 0x76 +#define YRSCALE 0x77 +#define ZRSCALE 0x78 + +#define XREFROLL 0x79 +#define YREFPITCH 0x7a +#define ZREFYAW 0x7b + +#define ANGXOFFSET 0x7c +#define ANGYOFFSET 0x7d +#define ANGZOFFSET 0x7e + +#define NUMBERID1 0x7f +#define NUMBERID2 0x80 +#define NUMBERID3 0x81 +#define NUMBERID4 0x82 +#define NUMBERID5 0x83 +#define NUMBERID6 0x84 + +#define XA85PSCALE 0x85 +#define XA85NSCALE 0x86 +#define YA85PSCALE 0x87 +#define YA85NSCALE 0x88 +#define XA30PSCALE 0x89 +#define XA30NSCALE 0x8a +#define YA30PSCALE 0x8b +#define YA30NSCALE 0x8c + +#define CHIPIDL 0x8D +#define CHIPIDH 0x8E +#define REGINITFLAG REGSIZE-1 + + +/* AXIS6 */ +#define ALGRITHM9 0 +#define ALGRITHM6 1 + +/************CALSW**************/ +#define NORMAL 0x00 +#define CALGYROACC 0x01 +#define CALMAG 0x02 +#define CALALTITUDE 0x03 +#define CALANGLEZ 0x04 +#define CALACCL 0x05 +#define CALACCR 0x06 +#define CALMAGMM 0x07 +#define CALREFANGLE 0x08 +#define CALMAG2STEP 0x09 +//#define CALACCX 0x09 +//#define ACC45PRX 0x0A +//#define ACC45NRX 0x0B +//#define CALACCY 0x0C +//#define ACC45PRY 0x0D +//#define ACC45NRY 0x0E +//#define CALREFANGLER 0x0F +//#define CALACCINIT 0x10 +//#define CALREFANGLEINIT 0x11 +#define CALHEXAHEDRON 0x12 + +/************OUTPUTHEAD**************/ +#define WIT_TIME 0x50 +#define WIT_ACC 0x51 +#define WIT_GYRO 0x52 +#define WIT_ANGLE 0x53 +#define WIT_MAGNETIC 0x54 +#define WIT_DPORT 0x55 +#define WIT_PRESS 0x56 +#define WIT_GPS 0x57 +#define WIT_VELOCITY 0x58 +#define WIT_QUATER 0x59 +#define WIT_GSA 0x5A +#define WIT_REGVALUE 0x5F + +/************RSW**************/ +#define RSW_TIME 0x01 +#define RSW_ACC 0x02 +#define RSW_GYRO 0x04 +#define RSW_ANGLE 0x08 +#define RSW_MAG 0x10 +#define RSW_PORT 0x20 +#define RSW_PRESS 0x40 +#define RSW_GPS 0x80 +#define RSW_V 0x100 +#define RSW_Q 0x200 +#define RSW_GSA 0x400 +#define RSW_MASK 0xfff + +/**RRATE*****/ +#define RRATE_NONE 0x0d +#define RRATE_02HZ 0x01 +#define RRATE_05HZ 0x02 +#define RRATE_1HZ 0x03 +#define RRATE_2HZ 0x04 +#define RRATE_5HZ 0x05 +#define RRATE_10HZ 0x06 +#define RRATE_20HZ 0x07 +#define RRATE_50HZ 0x08 +#define RRATE_100HZ 0x09 +#define RRATE_125HZ 0x0a //only WT931 +#define RRATE_200HZ 0x0b +#define RRATE_ONCE 0x0c + +/* BAUD */ +#define WIT_BAUD_4800 1 +#define WIT_BAUD_9600 2 +#define WIT_BAUD_19200 3 +#define WIT_BAUD_38400 4 +#define WIT_BAUD_57600 5 +#define WIT_BAUD_115200 6 +#define WIT_BAUD_230400 7 +#define WIT_BAUD_460800 8 +#define WIT_BAUD_921600 9 + +/*CAN BAUD*/ +#define CAN_BAUD_1000000 0 +#define CAN_BAUD_800000 1 +#define CAN_BAUD_500000 2 +#define CAN_BAUD_400000 3 +#define CAN_BAUD_250000 4 +#define CAN_BAUD_200000 5 +#define CAN_BAUD_125000 6 +#define CAN_BAUD_100000 7 +#define CAN_BAUD_80000 8 +#define CAN_BAUD_50000 9 +#define CAN_BAUD_40000 10 +#define CAN_BAUD_20000 11 +#define CAN_BAUD_10000 12 +#define CAN_BAUD_5000 13 +#define CAN_BAUD_3000 14 + +/* KEY */ +#define KEY_UNLOCK 0xB588 + +/* SAVE */ +#define SAVE_PARAM 0x00 +#define SAVE_SWRST 0xFF + +/* ORIENT */ +#define ORIENT_HERIZONE 0 +#define ORIENT_VERTICLE 1 + +/* BANDWIDTH */ +#define BANDWIDTH_256HZ 0 +#define BANDWIDTH_184HZ 1 +#define BANDWIDTH_94HZ 2 +#define BANDWIDTH_44HZ 3 +#define BANDWIDTH_21HZ 4 +#define BANDWIDTH_10HZ 5 +#define BANDWIDTH_5HZ 6 + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/8By8/wit_c_sdk.c b/8By8/wit_c_sdk.c new file mode 100644 index 0000000..5490601 --- /dev/null +++ b/8By8/wit_c_sdk.c @@ -0,0 +1,502 @@ +#include "wit_c_sdk.h" + +static SerialWrite p_WitSerialWriteFunc = NULL; +static WitI2cWrite p_WitI2cWriteFunc = NULL; +static WitI2cRead p_WitI2cReadFunc = NULL; +static CanWrite p_WitCanWriteFunc = NULL; +static RegUpdateCb p_WitRegUpdateCbFunc = NULL; +static DelaymsCb p_WitDelaymsFunc = NULL; + +static uint8_t s_ucAddr = 0xff; +static uint8_t s_ucWitDataBuff[WIT_DATA_BUFF_SIZE]; +static uint32_t s_uiWitDataCnt = 0, s_uiProtoclo = 0, s_uiReadRegIndex = 0; +int16_t sReg[REGSIZE]; + + +#define FuncW 0x06 +#define FuncR 0x03 + +static const uint8_t __auchCRCHi[256] = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, + 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, + 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, + 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, + 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, + 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40 +}; +static const uint8_t __auchCRCLo[256] = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, + 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, + 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, + 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, + 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, + 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, + 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, + 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, + 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, + 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, + 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, + 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, + 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, + 0x40 +}; + + +static uint16_t __CRC16(uint8_t *puchMsg, uint16_t usDataLen) +{ + uint8_t uchCRCHi = 0xFF; + uint8_t uchCRCLo = 0xFF; + uint8_t uIndex; + int i = 0; + uchCRCHi = 0xFF; + uchCRCLo = 0xFF; + for (; i= 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; +} + + + diff --git a/8By8/wit_c_sdk.h b/8By8/wit_c_sdk.h new file mode 100644 index 0000000..9455ff6 --- /dev/null +++ b/8By8/wit_c_sdk.h @@ -0,0 +1,135 @@ +#ifndef __WIT_C_SDK_H +#define __WIT_C_SDK_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#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 */ diff --git a/HardwareDesign/HardwareDesign.ino b/HardwareDesign/HardwareDesign.ino index 2ae4c73..d58c3ad 100644 --- a/HardwareDesign/HardwareDesign.ino +++ b/HardwareDesign/HardwareDesign.ino @@ -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"); } }