diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/README.md b/arc_design_contest/2018/HUST_inverted_pendulum/README.md new file mode 100644 index 00000000..30d53231 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/README.md @@ -0,0 +1,174 @@ +# mobile_inverted_pendulum_robots +This application, which implements a mobile inverted pendulum robots, is designed to show how to develop a **Motion Control** using embARC. +VIDEO URL: [DEMO VIDEO][14] + +* [Introduction](#introduction) + * [Function](#function) + * [System Architecture](#system-architecture) +* [Hardware and Software Setup](#hardware-and-software-setup) + * [Required Hardware](#required-hardware) + * [Required Software](#required-software) + * [Hardware Connection](#hardware-connection) +* [User Manual](#user-manual) + * [Before Running This Application](#before-running-this-application) + * [Run This Application](#run-this-application) + +## Introduction +This project was successful in achieving a two-wheeled autonomous robot based on the inverted pendulum model.EMSK works as controller, it will deal with sensor datas and interact with user via bluetooth. We can view all data on serial terminal, and sent instructions to the robot to change its motion mode. + +### Function +- **Upright** + The robot can keep itself upright even if there is a great disturbances. + [![upright.gif](https://s8.postimg.cc/4lsw5oi1h/upright.gif)](https://postimg.cc/image/6qd96rjo1/) +- **Linear Motion** + Under upright condition, the robot has excellent ability to linear motion. + [![linear_motion.gif](https://s8.postimg.cc/fwvjtyovp/linear_motion.gif)](https://postimg.cc/image/gmec6bpf5/) +- **Angular Rotation** + Not only linear motion, but also angular rotation can be implemented. + [![angular_rotation.gif](https://s8.postimg.cc/6qd96m6rp/angular_rotation.gif)](https://postimg.cc/image/jhrfd4gjl/) +- **Over the Seesaw** + Based on the above, the robot can autonomously through the seesaw + [![over_the_seesaw.gif](https://s8.postimg.cc/wlwzpvt6t/over_the_seesaw.gif)](https://postimg.cc/image/hd72c3zi9/) + +### System Architecture +![system architecture][4] + +## Hardware and Software Setup +### Required Hardware +- 1 [DesignWare ARC EM Starter Kit(EMSK V2.3)][5] +- 2 [BLE module(HC-05)][6] +- 1 [USB To TTL module(CP2102)][7] +- 1 [IMU module(MPU6050)][8] +- 1 [Motor Driver][9] +- 1 [Voltage Conversion module(7805)][10] +- 1 NiCd Battery +- 1 SD Card +- 1 SPI to QEI AND PWM Expanding-board +- 1 Robot Platform + ![Robot Platform][11] +### Required Software +- ARC GNU Toolset 2017.03 +- Serial port terminal, such as putty, tera-term or minicom + +### Hardware Connection +1. Connect BLE HC-05 module to Pmod J1(Using UART interface) +2. Connect IMU MPU6050 module to Pmode J2(Using I2C interface) +3. Connect Expansion Board to Pmode J6, J5(Using J6 SPI interface, J5 using for mechanical fixing) + +## User Manual +### Before running this application +Download source code of invert-pendulum from github, and ensure install a serial port terminal and arc-gnu toolset in your PC. +The hardware resources are allocated as following table. + +| Hardware resource | Function | +|-------------------|-----------------------------------| +| MPU6050 | Gyroscope and acceleration sensor | +| HC-05 | Provide bluetooth connection | +| CP2102 | USB TO TTL | +| MC7805 | Linear Voltage Regulator | +| ROBOTS PLATFORM | integrated module | + +### Run This Application +Here take EMSK2.3, CUR_CORE = arcem7d with GNU Toolset for example to show how to run this application. +1. We need to use embARC 2nd bootloader to automatically load application binary for different EMSK and run. See **example/bootloader Example** for reference. +2. Open your serial terminal such as Tera-Term on PC, and configure it to right COM port and 115200bps. +3. Interact using EMSK and serial port terminal. + +#### makefile +- Selected FreeRTOS in **/application/invered_pendulum/scr/makefile**, then you can use [FreeRTOS API][13] in your application: + + #Selected OS + OS_SEL ?= freertos + +- Target options about EMSK and toolchain: + + BOARD ?= emsk + BD_VER ?= 23 + CUR_CORE ?= arcem7d + TOOLCHAIN = gnu + +- The relative series of the root directory, here the path of the makefile is +**/application/invered_pendulum/scr/makefile**: + + # + # root dir of embarc + # + EMBARC_ROOT = ../../../.. + +- Directories of source files and header files, notice that it **is not recursive**: + + # application source dirs + APPL_CSRC_DIR = . ./TskDbg ./Queue ./TskMotor ./drivers ./TskTop ./FastMath_Table + APPL_ASMSRC_DIR = . + + # application include dirs + APPL_INC_DIR = . ./TskDbg ./Queue ./TskTop ./drivers ./TskMotor ./FastMath_Table + +See [embARC Example User Guide][14], **"Options to Hard-Code in the Application Makefile"** for more detailed information about **Makefile Options**. + +#### Driver +Placing the drivers' source code in **application/invered_pendulum/scr/drivers** folder, C source and header file in the folder. + +| file | Function | +|-----------------|---------------------------------------------| +| imu.c/imu.h | get imu sensor data(int16_t raw data) | +| spi.c/spi.h | transfer qei data(int16_t) and pwm(int16_t) | +| timer.c/timer.h | Timer 1,2ms interrupt for motroTick | + +#### FastMath_Table +The **FastMath_Table** is used to fast atan function, ranging in -0.98rad~0.45rad(table data is 1024 times of actual angle).The table index is +equal to tangent value enlarges 128 times and add 192. + + /* + * n = m << 7 + 192, y = round(x << 10) + * m: tangent value + * n: array index + * x: atan(m) + * y: array[n] + */ + +#### Queue +The **Queue** realizes queue functions through linear table, type:char or int. + + QueueInit + QueueClear + QueueLength + EnQueue + DeQueue +Drawback: it implement queue by arrays, which will waste memory space and +fix data type. + +#### TskDbg +The **TskDbg** mainly uses for UART transmission. + +| file | Function | +|---------------------|-----------------------------| +| cmd.c/cmd.h | interact with user | +| dbgUart.c/dbgUart.h | uart transfer and uart Task | + +### TskMotor +The **TskMotor** completes data processing and algorithm application. + +#### Other +The file **algorithm.s** implements algorithms through assembly instructions. +The file **assert.c** is useful to handling exceptions. +The file **physParmas.h** contains macro define for data factor and customize typedef struct. + + +[0]: ./doc/screenshot/upright.gif "upright" +[1]: ./doc/screenshot/linear_motion.gif "linear motion" +[2]: ./doc/screenshot/angular_rotation.gif "angular rotation" +[3]: ./doc/screenshot/over_the_seesaw.gif "over the seesaw" +[4]: ./doc/screenshot/system.svg "system architecture" +[5]: https://www.synopsys.com/dw/ipdir.php?ds=arc_em_starter_kit "DesignWare ARC EM Starter Kit(EMSK)" +[6]: http://www.electronics-lab.com/?s=HC-05 "BLE HC-05 module" +[7]: https://www.sparkfun.com/products/retired/198 "USB To TTL module" +[8]: http://playground.arduino.cc/Main/MPU-6050 "IMU MPU6050 module" +[9]: http://www.landzo.cn/forum.php?mod=viewthread&tid=10541&extra=page%3D1&page=1 "Motor Driver" +[10]: http://www.electronics-lab.com/?s=7805 "Voltage Conversion 7805 module" +[11]: ./doc/screenshot/Platform.jpg "Robot Platform" +[12]: https://www.freertos.org/a00106.html "FreeRTOS API" +[13]: http://embarc.org/embarc_osp/doc/embARC_Document/html/page_example.html " embARC Example User Guide" +[14]: http://v.youku.com/v_show/id_XMzYzMDkzNzg3Ng==.html?spm=a2h3j.8428770.3416059.1 + + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/CP1202.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/CP1202.jpg new file mode 100644 index 00000000..0ee9b7df Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/CP1202.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/HC-05.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/HC-05.jpg new file mode 100644 index 00000000..9b0b052d Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/HC-05.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/MPU6050.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/MPU6050.jpg new file mode 100644 index 00000000..340297cb Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/MPU6050.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/Motor_Driver.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/Motor_Driver.jpg new file mode 100644 index 00000000..de4378a3 Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/Motor_Driver.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/Platform.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/Platform.jpg new file mode 100644 index 00000000..9b726b33 Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/Platform.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/expanding-board.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/expanding-board.jpg new file mode 100644 index 00000000..6c89f5af Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/expanding-board.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/pc.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/pc.jpg new file mode 100644 index 00000000..294ec595 Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/pc.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/robot2.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/robot2.jpg new file mode 100644 index 00000000..2e37717b Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/robot2.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/roll-pitch-yaw-angles.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/roll-pitch-yaw-angles.jpg new file mode 100644 index 00000000..729ac6bc Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/roll-pitch-yaw-angles.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/system.jpg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/system.jpg new file mode 100644 index 00000000..e38d2d31 Binary files /dev/null and b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/system.jpg differ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/system.svg b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/system.svg new file mode 100644 index 00000000..eab9b443 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/doc/screenshot/system.svg @@ -0,0 +1,16516 @@ + + + + + + + + + + + + + + + + + + 页-1 + + + + 工作表.64 + + 圆角的矩形.23 + UI Interface + + + + + + + + + + + + + + + + + + + + + + UI Interface + + 普通双箭头 + + + + + + + + 工作表.31 + BLE + + + + BLE + + 工作表.32 + + 工作表.19 + + + + + + 圆角的矩形.30 + + + + + + + + + + + + + + + + + + + + + + + 圆角的矩形 + + + + + + + + + + + + + + + + + + + + + + 工作表.3 + ARC EMSK + + + + ARC EMSK + + 工作表.4 + + + + + + + + + + + + + + + + + + + + + + 工作表.5 + Function: + + + + Function: + + 菱形 + + + + + + + 工作表.11 + + 五角星 + + + + + + + + + + + + + + 工作表.8 + Robot Upright + + + + Robot Upright + + + 工作表.12 + + 五角星.9 + + + + + + + + + + + + + + 工作表.10 + Linear Motion + + + + Linear Motion + + + 工作表.13 + + 五角星.9 + + + + + + + + + + + + + + 工作表.15 + Angular Rotation + + + + Angular Rotation + + + 工作表.16 + + 五角星.9 + + + + + + + + + + + + + + 工作表.18 + Seesaw + + + + Seesaw + + + 普通双箭头.34 + + + + + + + + 圆角的矩形.36 + Motor Control + + + + + + + + + + + + + + + + + + + + + + Motor Control + + 圆角的矩形.38 + Speed Measure + + + + + + + + + + + + + + + + + + + + + + Speed Measure + + 丁字箭头 + + + + + + + + + + + 工作表.41 + SPI + + + + SPI + + 工作表.42 + PWM + + + + PWM + + 工作表.43 + QEI + + + + QEI + + 圆角的矩形.45 + Posture Solution + + + + + + + + + + + + + + + + + + + + + + Posture Solution + + 普通双箭头.46 + + + + + + + + 工作表.47 + I2C + + + + I2C + + 工作表.50 + + 工作表.48 + + + + + + 圆角的矩形.49 + + + + + + + + + + + + + + + + + + + + + + + 普通箭头 + + + + + + + + 工作表.56 + + + + 工作表.61 + + 工作表.59 + + + + 工作表.60 + + + + + 工作表.62 + + + + + + 普通箭头.63 + + + + + + + + + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/FastMath_Table/FastMath_Table.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/FastMath_Table/FastMath_Table.c new file mode 100644 index 00000000..ded82037 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/FastMath_Table/FastMath_Table.c @@ -0,0 +1,43 @@ + +/* + * n = m << 7 + 192, y = round(x << 10) + * m: tangent value + * n: array index + * x: atan(m) + * y: array[n] + */ + +const short atan_tables[256] = { + -1006, -1004, -1001, -999, -996, -994, -991, -989, + -986, -983, -981, -978, -976, -973, -970, -967, + -965, -962, -959, -956, -953, -951, -948, -945, + -942, -939, -936, -933, -930, -927, -924, -921, + -918, -914, -911, -908, -905, -902, -898, -895, + -892, -888, -885, -882, -878, -875, -871, -868, + -864, -861, -857, -854, -850, -846, -843, -839, + -835, -831, -828, -824, -820, -816, -812, -808, + -804, -800, -796, -792, -788, -784, -780, -775, + -771, -767, -763, -758, -754, -750, -745, -741, + -736, -732, -727, -722, -718, -713, -708, -703, + -699, -694, -689, -684, -679, -674, -669, -664, + -659, -654, -649, -643, -638, -633, -628, -622, + -617, -611, -606, -600, -595, -589, -583, -578, + -572, -566, -560, -555, -549, -543, -537, -531, + -525, -519, -512, -506, -500, -494, -487, -481, + -475, -468, -462, -455, -449, -442, -436, -429, + -422, -416, -409, -402, -395, -388, -381, -374, + -367, -360, -353, -346, -339, -332, -325, -317, + -310, -303, -296, -288, -281, -273, -266, -258, + -251, -243, -236, -228, -221, -213, -205, -198, + -190, -182, -174, -167, -159, -151, -143, -135, + -127, -119, -112, -104, -96, -88, -80, -72, + -64, -56, -48, -40, -32, -24, -16, -8, + 0, 8, 16, 24, 32, 40, 48, 56, + 64, 72, 80, 88, 96, 104, 112, 119, + 127, 135, 143, 151, 159, 167, 174, 182, + 190, 198, 205, 213, 221, 228, 236, 243, + 251, 258, 266, 273, 281, 288, 296, 303, + 310, 317, 325, 332, 339, 346, 353, 360, + 367, 374, 381, 388, 395, 402, 409, 416, + 422, 429, 436, 442, 449, 455, 462, 468 +}; \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/FastMath_Table/FastMath_Table.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/FastMath_Table/FastMath_Table.h new file mode 100644 index 00000000..04b5b2d2 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/FastMath_Table/FastMath_Table.h @@ -0,0 +1,6 @@ +#ifndef FASTMATH_TABLE_H +#define FASTMATH_TABLE_H + +extern const short atan_tables[256]; + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/FreeRTOSConfig.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/FreeRTOSConfig.h new file mode 100644 index 00000000..23b5dfdb --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/FreeRTOSConfig.h @@ -0,0 +1,113 @@ +/* + FreeRTOS V8.0.0 - Copyright (C) 2014 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that has become a de facto standard. * + * * + * Help yourself get started quickly and support the FreeRTOS * + * project by purchasing a FreeRTOS tutorial book, reference * + * manual, or both from: http://www.FreeRTOS.org/Documentation * + * * + * Thank you! * + * * + *************************************************************************** + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation >>!AND MODIFIED BY!<< the FreeRTOS exception. + + >>! NOTE: The modification to the GPL is included to allow you to distribute + >>! a combined work that includes FreeRTOS without being obliged to provide + >>! the source code for proprietary components outside of the FreeRTOS + >>! kernel. + + FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY + WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + FOR A PARTICULAR PURPOSE. Full license text is available from the following + link: http://www.freertos.org/a00114.html + + 1 tab == 4 spaces! + + *************************************************************************** + * * + * Having a problem? Start by reading the FAQ "My application does * + * not run, what could be wrong?" * + * * + * http://www.FreeRTOS.org/FAQHelp.html * + * * + *************************************************************************** + + http://www.FreeRTOS.org - Documentation, books, training, latest versions, + license and Real Time Engineers Ltd. contact details. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.OpenRTOS.com - Real Time Engineers ltd license FreeRTOS to High + Integrity Systems to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +#include +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE + * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. + * + * See http://www.freertos.org/a00110.html. + *----------------------------------------------------------*/ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ( ( unsigned long ) BOARD_CPU_CLOCK ) +#define configTICK_RATE_HZ ( ( TickType_t ) 1000 ) +#define configMAX_PRIORITIES ( 16 ) +#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 104 ) +#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 18 * 1024 ) ) +#define configMAX_TASK_NAME_LEN ( 10 ) +#define configUSE_TRACE_FACILITY 1 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 1 +#define configUSE_MUTEXES 1 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) + + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 + +#endif /* FREERTOS_CONFIG_H */ \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/Kalman/Kalman1Var.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Kalman/Kalman1Var.c new file mode 100644 index 00000000..dbf30e80 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Kalman/Kalman1Var.c @@ -0,0 +1,44 @@ +/* + * Kalman1Var.cpp + * + * Created on: Aug 4, 2016 + * Author: loywong + */ + + +#include + +void kalReset(Kalman1Var *kalVal) +{ + kalVal->xpos = 0.f; + kalVal->Ppos = 0.f; +} + +void kalParamCorr(Kalman1Var *kalVal, float r, float q, float x0, float P0) +{ + kalVal->Q = q; + kalVal->R = r; + kalVal->xpos = x0; + kalVal->Ppos = P0; +} + +void Predict(Kalman1Var *kalVal, float u) +{ +// xˆ -k = A xˆ k – 1 + B u k – 1 + kalVal->xpri = kalVal->A * kalVal->xpos + kalVal->B * u; +// P -k = A P k – 1 A T + Q + kalVal->Ppri = kalVal->A * kalVal->Ppos * kalVal->A + kalVal->Q; +} + +float Correct(Kalman1Var *kalVal, float z) +{ +// K k = P -k H T ( H P -k H T + R ) – 1 + kalVal->K = kalVal->Ppri * kalVal->H / \ + (kalVal->H * kalVal->Ppri * kalVal->H + kalVal->R); +// xˆ k = xˆ -k + K k ( z k – H xˆ -k ) + kalVal->xpos = kalVal->xpri + kalVal->K * (z - kalVal->H * kalVal->xpri); +// P k = ( I – K k H ) P -k + kalVal->Ppos = (1 - kalVal->K * kalVal->H) * kalVal->Ppri; + + return kalVal->xpos; +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/Kalman/Kalman1Var.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Kalman/Kalman1Var.h new file mode 100644 index 00000000..4d8ed38c --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Kalman/Kalman1Var.h @@ -0,0 +1,24 @@ + +/* + * Kalman1Var.h + * + * Created on: Aug 4, 2016 + * Author: loywong + */ + + +#ifndef KALMAN_KALMAN1VAR_H_ +#define KALMAN_KALMAN1VAR_H_ + +typedef struct +{ + float A, B, H, Q, R; + float xpri, Ppri; + float K, xpos, Ppos; +}Kalman1Var; + +void Predict(Kalman1Var *kalVal, float u); +float Correct(Kalman1Var *kalVal, float z); +void kalReset(Kalman1Var *kalVal); + +#endif /* KALMAN_KALMAN1VAR_H_ */ \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/Pid/Pid.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Pid/Pid.c new file mode 100644 index 00000000..d577e400 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Pid/Pid.c @@ -0,0 +1,61 @@ +#include "Pid.h" + +#define USEI +#define USED + +void pidParamCorr(Pid *pid, float p, float i, float d, float n) +{ + pid->p = p; + pid->i = i; + pid->d = d; + pid->n = n; + pid->accI = .0f; + pid->accD = .0f; +} + +void pidReset(Pid *pid) +{ + pid->accI = .0f; + pid->accD = .0f; +} + +float Tick(Pid *pid, float diff) +{ + float pout; +#ifdef USED + float dout; +#endif + + pout = diff * pid->p; + +#ifdef USEI + pid->accI += diff * pid->i * pid->ts; + if(pid->accI > pid->accIMax) + pid->accI = pid->accIMax; + else if(pid->accI < pid->accIMin) + pid->accI = pid->accIMin; +#endif + +#ifdef USED + dout = (diff * pid->d - pid->accD) * pid->n; + pid->accD += dout * pid->ts; + if(pid->accD > pid->accDMax) + pid->accD = pid->accDMax; + else if(pid->accD < pid->accDMin) + pid->accD = pid->accDMin; +#endif + +#ifdef USEI + pout += pid->accI; +#endif +#ifdef USED + pout += dout; +#endif + + if(pout > pid->accIMax) + pout = pid->accIMax; + else if(pout < pid->accIMin) + pout = pid->accIMin; + + return pout; +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/Pid/Pid.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Pid/Pid.h new file mode 100644 index 00000000..85e4a017 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Pid/Pid.h @@ -0,0 +1,14 @@ +#ifndef __PID_H__ +#define __PID_H__ + +typedef struct +{ + float p, i, d, n, ts; + float accI, accD, accIMax, accIMin, accDMax, accDMin; +}Pid; + +void pidParamCorr(Pid *pid, float p, float i, float d, float n); +float Tick(Pid *pid, float diff); +void pidReset(Pid *pid); + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/Queue/Queue.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Queue/Queue.c new file mode 100644 index 00000000..7bf74562 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Queue/Queue.c @@ -0,0 +1,133 @@ +/* + *simple queue, if using template, don't need differ char and int + */ + +#include "embARC.h" + +#include "Queue.h" +#include +#include + +/** [q_init_char: initialize queue (type:char)] */ +void q_init_char(Queue *q, int alen) +{ + q->array = malloc(sizeof(char) * alen); + configASSERT(q->array!=NULL); + q->num = NULL; + q->alen = alen; + q->head = 0; + q->tail = 0; +} + +/** [q_init_char: initialize queue (type:int)] */ +void q_init_int(Queue *q, int alen) +{ + q->array = NULL; + q->num = malloc(sizeof(int) * alen); + configASSERT(q->num!=NULL); + q->alen = alen; + q->head = 0; + q->tail = 0; +} + +/** [q_clr: clear queue, but don't free the memory] */ +void q_clr(Queue *q) +{ + q->head = 0; + q->tail = 0; +} + +/** [q_l: calc queue current length] */ +int q_l(Queue *q) +{ + int rtn; + if(q->tail >= q->head) + { + rtn = q->tail - q->head; + } + else + { + rtn = q->alen + q->tail - q->head; + } + return rtn; +} + +/** [q_en_char: enqueue (char) until queue is full, ingore next datas] */ +bool q_en_char(Queue *q, char c) +{ + bool rtn = false; + if(q_l(q) < q->alen - 1) + { + q->array[q->tail] = c; + if(q->tail == q->alen - 1) + { + q->tail = 0; + } + else + { + q->tail++; + } + rtn = true; + } + return rtn; +} + +/** [q_en_int: enqueue (int) until queue is full, ingore next datas] */ +bool q_en_int(Queue *q, int c) +{ + bool rtn = false; + if(q_l(q) < q->alen - 1) + { + q->num[q->tail] = c; + if(q->tail == q->alen - 1) + { + q->tail = 0; + } + else + { + q->tail++; + } + rtn = true; + } + return rtn; +} + +/** [q_en_char: dequeue (char) until queue is empty, keep final value] */ +bool q_de_char(Queue *q, char *c) +{ + bool rtn = false; + if(q_l(q) > 0) + { + *c = q->array[q->head]; + if(q->head == q->alen - 1) + { + q->head = 0; + } + else + { + q->head++; + } + rtn = true; + } + return rtn; +} + +/** [q_en_int: dequeue (int) until queue is empty, keep final value] */ +bool q_de_int(Queue *q, int *c) +{ + bool rtn = false; + if(q_l(q) > 0) + { + *c = q->num[q->head]; + if(q->head == q->alen - 1) + { + q->head = 0; + } + else + { + q->head++; + } + rtn = true; + } + return rtn; +} diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/Queue/Queue.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Queue/Queue.h new file mode 100644 index 00000000..6d2458da --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/Queue/Queue.h @@ -0,0 +1,24 @@ +#ifndef _QUEUE_H_ +#define _QUEUE_H_ + +#include + +typedef struct Queue +{ + char *array; + int *num; + int alen; + int head; + int tail; +}Queue; + +extern void q_init_char(Queue *q, int alen); +extern void q_init_int(Queue *q, int alen); +extern void q_clr(Queue *q); +extern int q_l(Queue *q); +extern bool q_en_char(Queue *q, char c); +extern bool q_en_int(Queue *q, int c); +extern bool q_de_char(Queue *q, char *c); +extern bool q_de_int(Queue *q, int *c); + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/cmd.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/cmd.c new file mode 100644 index 00000000..c0ee67e8 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/cmd.c @@ -0,0 +1,326 @@ +#include "cmd.h" +#include "dbgUart.h" +#include "TskMotor.h" + +#include "embARC.h" +#include "embARC_debug.h" + +#include +#include +#include + +VarAndName varname[varnum] = { + /** enlarge 2 */ + {&lqr.k1, "ka"}, + {&lqr.k2, "kb"}, + {&lqr.k3, "kc"}, + {&lqr.k4, "kd"}, + /** enlarge 128 */ + {&AcclLPFParam, "lpfa"}, + {&SpdLPFParam, "lpfv"}, + {&AngLPFParam, "lpfang"}, + {&GyroLPFParam, "lpfg"}, + {&PRINT_PERIOD, "putTs"}, + /** enlarge 256 */ + {&angPid.p, "angp"}, + {&angPid.i, "angi"}, + {&angPid.d, "angd"}, + {&angPid.n, "angn"}, + /** enlarge 32768 */ + {&AngleKal.Q, "kalq"}, + {&AngleKal.R, "kalr"}, + /** enlarge (1<<21) */ + {&ANGLEDIFF, "angmin"}, + {&ANGLIMIT, "angmax"}, + {&desire.Velocity, "lv"}, + {&desire.Timeout, "ts"}, + {&desire.Acc, "acc"}, + {&desire.Omega, "av"}, + {&desire.ThetaZ, "angz"}, + {&desire.ThetaY, "angy"} +}; + +char input[120]; +char *cmd; + +void paramCorr(void) +{ + /** *d = "space", for string decomposition */ + const char *d = " "; + bool exit_flag = 0, gsensor_flag = 0; + float temp, angSave = -1106667; + int temp_int; + MsgType msg; + + putStr("start: start motor\r\n"); + vTaskDelay(100); + putStr("stop: stop motor\r\n"); + vTaskDelay(100); + putStr("angle: Enable angle\r\n"); + vTaskDelay(100); + putStr("stop: stop motor\r\n"); + vTaskDelay(100); + putStr("put: print data\r\n"); + vTaskDelay(100); + putStr("time,spd,kal,imu,pwm,lqr\r\n"); + vTaskDelay(100); + putStr("q: stop data print\r\n"); + vTaskDelay(100); + putStr("Format: str num\r\n"); + vTaskDelay(100); + putStr("angpid kal lqr desire\r\n"); + vTaskDelay(100); + putStr("lpfParam, angle range \r\n"); + vTaskDelay(100); + putStr("print period \r\n"); + vTaskDelay(100); + + while(1){ + + putStr("input:\r"); + vTaskDelay(50); + + /** wait for cmd input */ + UartGetLine(input); + + /** string decomposition, obtain command */ + cmd = strtok(input, d); + + for(int i = 0; i < varnum; i++){ + //start system + if(!strcmp(cmd, "k")) { + gsensor_flag = 0; + *varname[22].value = angSave; + putStr("start acqZeros\r\n"); + vTaskDelay(50); + msg = EnableAcqZeros; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + putStr("please don't touch within 2s\r\n"); + vTaskDelay(2000); + + putStr("Enable Motor\r\n"); + vTaskDelay(50); + msg = DisableAcqZeros; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + msg = EnableMotors; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + msg = EnableAng; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + /* + * APP control + * w -- lv + 0.1m/s + * b -- lv - 0.1m/s + * l -- yaw angle + 0.2rad + * r -- yaw angle - 0.2rad + * t -- lv = 0m/s + * g -- start posture control + * c -- change center of gravity + */ + else if(!strcmp(cmd, "w")) { + *varname[17].value += 209715; + putStr("v=%5.3f", *varname[17].value / 2097152.f); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "b")) { + *varname[17].value -= 209715; + putStr("v=%5.3f", *varname[17].value / 2097152.f); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "l")) { + *varname[21].value += 419430; + putStr("ang=%5.3f", *varname[21].value / 2097152.f); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "r")) { + *varname[21].value -= 419430; + putStr("yaw=%5.3f", *varname[21].value / 2097152.f); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "t")) { + *varname[17].value = 0; + putStr("v=%5.3f", *varname[17].value / 2097152.f); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "s")) { + putStr("disable motor\r\n"); + vTaskDelay(50); + msg = DisableMotors; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "g")){ + putStr("Enable gravity modify\r\n"); + gsensor_flag = 1; + break; + } + + else if(!strcmp(cmd, "c") && (gsensor_flag==1)){ + cmd = strtok(NULL, d); + temp = atof(cmd); + *varname[22].value += (int)(temp * 1048576); + putStr("pitch=%5.3f", *varname[22].value / 2097152.f); + break; + } + + else if(!strcmp(cmd, "zero")){ + putStr("Enable acqZeros\r\n"); + vTaskDelay(50); + msg = EnableAcqZeros; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "angle")){ + putStr("Enable angle\r\n"); + vTaskDelay(50); + msg = EnableAng; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "put")){ + putStr("print data\r\n"); + vTaskDelay(50); + msg = EnablePutStr; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + break; + } + + else if(!strcmp(cmd, "time")){ + putStr("calc time\r\n"); + vTaskDelay(50); + PRINT_TIME = true; + break; + } + + else if(!strcmp(cmd, "lqr")){ + DEBUG_LQR = true; + vTaskDelay(20); + msg = EnablePutStr; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + putStr("print lqr param\r\n"); + break; + } + + else if(!strcmp(cmd, "imu")){ + PRINT_IMU = true; + vTaskDelay(20); + msg = EnablePutStr; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + putStr("print imu data\r\n"); + break; + } + + else if(!strcmp(cmd, "pwm")){ + PRINT_PWM = true; + vTaskDelay(20); + msg = EnablePutStr; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + putStr("print pwm qei data\r\n"); + break; + } + + else if(!strcmp(cmd, "q")){ + PRINT_IMU = false; + PRINT_PWM = false; + DEBUG_LQR = false; + vTaskDelay(10); + msg = DisablePutStr; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + putStr("stop data print\r\n"); + break; + } + + else if(!strcmp(cmd, varname[i].name)) { + vTaskDelay(50); + /** string decomposition, obtain params variation */ + cmd = strtok(NULL, d); + temp = atof(cmd); + + if(isnan(temp)) + temp = *varname[i].value; + switch(i){ + case 0: + case 1: + case 2: + case 3: + temp_int = (int)(temp * 2); + putStr("\t %s = %5.3f (was %5.3f) \r\n", varname[i].name, + *varname[i].value / 2.f + temp, *varname[i].value / 2.f); + break; + case 4: + case 5: + case 6: + case 7: + case 8: + temp_int = (int)(temp * 128); + putStr("\t %s = %5.3f (was %5.3f) \r\n", varname[i].name, + *varname[i].value / 128.f + temp, *varname[i].value / 128.f); + break; + case 9: + case 10: + case 11: + case 12: + temp_int = (int)(temp * 256); + putStr("\t %s = %5.3f (was %5.3f)\r\n", varname[i].name, + *varname[i].value / 256.f + temp, *varname[i].value / 256.f); + break; + case 13: + case 14: + temp_int = (int)(temp * 32768); + putStr("\t %s = %5.3f (was %5.3f)\r\n", varname[i].name, + *varname[i].value / 32768.f + temp, *varname[i].value / 32768.f); + break; + default: + temp_int = (int)(temp * 2097152); + putStr("\t %s = %5.4f (was %5.4f)\r\n", varname[i].name, + *varname[i].value / 2097152.f + temp, *varname[i].value / 2097152.f); + break; + } + + vTaskDelay(100); + *varname[i].value += temp_int; + + if(i < 17) + putStr("data change after input stop\r\n"); + else { + putStr("data change right now\r\n"); + msg = EnableSeqSet; + xQueueSend(motMbCmd, &msg, portMAX_DELAY); + } + break; + } + + else if(!strcmp(cmd, "exit")) { + exit_flag = 1; + break; + } + + vTaskDelay(50); + } + + if(exit_flag){ + break; + } + } +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/cmd.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/cmd.h new file mode 100644 index 00000000..78081d0f --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/cmd.h @@ -0,0 +1,16 @@ +#ifndef __CMD_H__ +#define __CMD_H__ + +#define varnum 23 + +/* Mail */ +typedef struct{ + int * value; + char * name; +}VarAndName; + +//const int varnum = 16 + 4 + 2 + 4 + 4; +VarAndName varname[varnum]; +void paramCorr(void); + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/dbgUart.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/dbgUart.c new file mode 100644 index 00000000..1d7d64e3 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/dbgUart.c @@ -0,0 +1,172 @@ +#include "embARC.h" +#include "embARC_debug.h" + +#include "dbgUart.h" +#include "Queue/Queue.h" + +const int dbgTskPrio = 2; +const int dbgTskStkSize = 512; + +QueueHandle_t dbgMbCmd; +/* + * for print data(isn't ASCII) + * transfer putStr len + * aviod 0 identifying as a terminator + */ +QueueHandle_t dbgMbNum; + +Queue QUartRx; + +char msg[128]; + +static bool InitFlag = 0; +static DEV_UART *dbg_uart; +static uint8_t RxBuff; +static DEV_BUFFER UartRxBuff; +static DEV_BUFFER UartTxBuff; + +int putData(char *num, uint8_t size) +{ + BaseType_t rtn; + for(int i = 0; i < size; i++) + msg[i] = num[i]; + rtn=xQueueSend(dbgMbCmd, msg, portMAX_DELAY); + configASSERT(rtn==pdPASS); + rtn=xQueueSend(dbgMbNum, &size, portMAX_DELAY); + configASSERT(rtn==pdPASS); + return rtn; +} + +int putStr(const char *format, ...) +{ + BaseType_t rtn; + + va_list arg; + va_start(arg, format); + + // ARMCC microlib does not properly handle a size of 0. + // As a workaround supply a dummy buffer with a size of 1. + char dummy_buf[1]; + int len = vsnprintf(dummy_buf, sizeof(dummy_buf), format, arg); + if (len < 128) { + vsprintf(msg, format, arg); + rtn=xQueueSend(dbgMbCmd, msg, portMAX_DELAY); + configASSERT(rtn==pdPASS); + } + else { + len = 7; + vsprintf(msg, "error\r\n", arg); + rtn=xQueueSend(dbgMbCmd, msg, portMAX_DELAY); + configASSERT(rtn==pdPASS); + } + rtn=xQueueSend(dbgMbNum, &len, portMAX_DELAY); + configASSERT(rtn==pdPASS); + + va_end(arg); + return len; +} + +static void UartRxISR(void *ptr) +{ + int len; + char c; + dbg_uart->uart_control(UART_CMD_SET_RXINT, (void *)0); + + /** read uart char & enqueue */ + dbg_uart->uart_read((void *)&c, 1); + q_en_char(&QUartRx, c); + + dbg_uart->uart_control(UART_CMD_SET_RXINT, (void *)1); +} + +void UartGetLine(char *line) +{ + + char c; + bool b; + if(line == NULL) + return; + + while(1) + { + /** string dequeue, end with '\r\n' */ + do{ + b = q_de_char(&QUartRx, &c); + vTaskDelay(5); + }while(!b); + if(((*line++ = c) == '\n') && (*(line-2) == '\r')) + break; + } + /** add '\0' to the end of string, contribute to strcmp*/ + *(line-2) = '\0'; + *(line-1) = '\0'; + return; +} + +int UartWrite(char *str, uint8_t len) +{ + + return (int)dbg_uart->uart_write((const void *)(str), len); + //DEV_BUFFER_INIT(&UartTxBuff, str, len); + //dbg_uart->uart_control(UART_CMD_SET_TXINT_BUF, (void *)&UartTxBuff); + //dbg_uart->uart_control(UART_CMD_SET_TXINT, (void *)1); + +} + +void UartInit(void) +{ + + if (InitFlag) { + return; + } + + dbg_uart = uart_get_dev(DW_UART_0_ID); + dbg_uart->uart_open(BOARD_CONSOLE_UART_BAUD); + + dbg_uart->uart_control(UART_CMD_SET_RXINT_BUF, (void *)NULL); + dbg_uart->uart_control(UART_CMD_SET_RXCB, (void *)&UartRxISR); + dbg_uart->uart_control(UART_CMD_SET_RXINT, (void *)1); + + putStr("DBG:%s, %s\r\n", __DATE__, __TIME__); + #if defined(__GNU__) + putStr("GNU,%s\r\n", __VERSION__); + #else + putStr("Metaware,%s\r\n", __VERSION__); + #endif + + InitFlag = 1; +} + +void dbgTask(void *pvParameters) +{ + BaseType_t rtn; + uint8_t len; + + while(true){ + if(xQueueReceive(dbgMbCmd, msg, ( TickType_t ) 0) && xQueueReceive(dbgMbNum, &len, ( TickType_t ) 0)) + { + UartWrite(msg, len); + } + } +} + +void dbgInit() +{ + BaseType_t rtn; + + q_init_char(&QUartRx, 128); + + dbgMbCmd = xQueueCreate(10, 128); + configASSERT(dbgMbCmd); + + dbgMbNum = xQueueCreate(10, sizeof(uint8_t)); + configASSERT(dbgMbNum); + /* + * Initialize UART port 0 + */ + UartInit(); + + // Create tasks + rtn=xTaskCreate(dbgTask, "dbgTask", dbgTskStkSize, NULL, dbgTskPrio, NULL); + configASSERT(rtn==pdPASS); +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/dbgUart.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/dbgUart.h new file mode 100644 index 00000000..246294bf --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskDbg/dbgUart.h @@ -0,0 +1,12 @@ +#ifndef _DBGUART_H_ +#define _DBGUART_H_ + +#include "embARC.h" + +extern void dbgInit(); +extern void UartInit(void); +extern void UartGetLine(char *line); +extern int putStr(const char *format, ...); +extern int putData(char *num, uint8_t size); + +#endif /* _DBGUART_H_ */ \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskMotor/TskMotor.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskMotor/TskMotor.c new file mode 100644 index 00000000..7e4c40d3 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskMotor/TskMotor.c @@ -0,0 +1,559 @@ +#include "TskMotor.h" +#include "dbgUart.h" +#include "timer.h" +#include "imu.h" +#include "spi.h" +#include "FastMath_table.h" +#include "Queue/Queue.h" + +#include "embARC.h" +#include "embARC_debug.h" + +#include +#include + +/* + * motorTask Priority & stackSize + */ +const int motorTskPrio = 6; +const int motorTskStkSize = 512; + +/* + * multi-task communication + */ +QueueHandle_t motMbCmd; + +/** function control */ +bool AngEnabled = false; +bool AcqZerosEnabled = false; +bool MotorEnabled = false; +bool SeqSetEnabled = false; +bool PutStrEnabled = false; + +/** for debug */ +bool DEBUG_LQR = false; +bool PRINT_IMU = false; +bool PRINT_PWM = false; +bool PRINT_TIME = false; +/* + * print period, T = PRINT_PERIOD * 2ms + * dynamically adjusted according to actual demand + */ +int PRINT_PERIOD = LPF_FACTOR; + +/* + * Accl 1-order Lowpass Filter + * y(n) = (1-a) * y(n-1) + a * x + * float q1.7 + * set a -1~0.99219 -> -128 ~ 127 + * + * factor 128 + */ +int AcclLPFParam = (int)(0.02f * LPF_FACTOR); + +int SpdLPFParam = (int)(0.02f * LPF_FACTOR); +int AngLPFParam = (int)(2.f * LPF_FACTOR); +int GyroLPFParam = (int)(1.f * LPF_FACTOR); + +/* + * vertical status starting and termination threshold + * + * unit:rad factor 65536 + */ +int ANGLEDIFF = (int)(0.08f * 2097152); +int ANGLIMIT = (int)(0.4f * 2097152); + +/* kalman params, factor 65536 */ +Kalman1Var AngleKal = { + .Q = 20, /** 0.0006 * 32768 */ + .R = 3244, /** 0.099 * 32768 */ + .xpos = 0, + .Ppos = 0 +}; + +/** lqr params, factor 1*/ +lqr_q lqr = { + .k1 = -60, + .k2 = -40, //-40 for normal mode, -60 for + .k3 = 300, + .k4 = 16 +}; + +/* + * setpoint: + * Velocity --> linear speed + * Timeout --> period of Velocity + * Acc --> linear acceleration + * Omega --> (y axial)pitch angular velocity + * ThetaZ --> (z axial)yaw angle + * ThetaY --> (y axial)pitch angle + * Factor: 65536 + * + * |<- timeout ->| + * ----------------- + * /| \ + * / |Velocity \-----------> desire.Acc + * 0m/s / | \ 0m/s + * -----/ -|- \------ + * + */ +VelOmega desire = { + .Velocity = 0, + .Timeout = 0, + .Acc = 1258291, /** -0.6 * (1 << 21) */ + .Omega = 0, + .ThetaZ = 0, + .ThetaY = -1096181 /** -0.5227 * (1 << 21) */ +}; + +/** pid params, factor 256, avoid overflow*/ +Pid angPid = { + .p = 140, + .i = 0, + .d = 0, + .n = 256, + .accI = 0, + .accD = 0 +}; + +/* + * for atan LUT, index = SAT16((AcclX / AcclZ) << 7 + 192) (-1.5~0.45-> 0~255) + * accl2Ang = Table[index] + * ATTENTION: accl2Ang = real_angle << 10 + */ +extern int16_t accl2AngleIndex(ImuValues *imuValsV, int16_t *AcclX, int16_t *AcclZ, int16_t *LPFParam); + +extern void kalmanPredict(Kalman1Var *kalVal, int32_t u); +extern int32_t kalmanCorrect(Kalman1Var *kalVal, int32_t z); +extern int32_t lqrCalc(lqr_q *lqr, int32_t pos, int32_t vel, int32_t ang, int32_t av); +extern int32_t pidTick(Pid *pid, int32_t diff); + +void motorTask(void *pvParameters) +{ + BaseType_t rtn; + MsgType msg; + int32_t ercd; + uint64_t start_us = 0, end_us = 0; + + int data[8]; + char cmdf[2] = {0x03, 0xFC}; + char cmdr[2] = {0xFC, 0x03}; + + /** velocity timeout, default value: 65535 * 2ms, about 2 minutes*/ + int timeCnt = 0, timeout = 65535, tsCtl = 0; + + /** angle status check, if less than starting threshold 1s, enter upright mode*/ + int16_t status = 0; + + /* + * -------------------------|--------------------------- + * gyro | accl + * -------------------------|--------------------------- + * unit: m/s^2 factor: 115 | unit: m/s^2 factor: 115 + * -------------------------|--------------------------- + * angular Velocity | linear acceleration + * | | | + * |AVG Filter | |a_X/a_Y ratio + * | | | + * gyro bias | pitch angle tan value + * | | | + * | | |kalman Filter + * | | | + * gyro relative value ----|--->pitch angle + * ------------------------|--------------------------- + * H -> Horizontal, V -> vertical + */ + ImuValues imuValsH, imuValsV; + int16_t staticCnt = 0, staticCntCyced = 0; + int16_t GyroX = 0, GyroY = 0, AcclX = 0, AcclZ = 0; + int16_t XBias = 164, ZBias = 2785; + int16_t GyroXZero = 0, GyroYZero = 0; + int32_t accl2Ang = 0, acclIndex = 0; + int32_t gyroXZeroAcc = 0, gyroYZeroAcc = 0; + int32_t angle1 = 0, angle = 0, lqrGyroY = 0; + int16_t AngParam = 0, YawParam = 0; + /* + * speed and pos related values + */ + Queue spd; + Queue ang; + int16_t qei[2] = {0, 0}, qei1Tmp = 0, pwm[2] = {0, 0}; + int32_t pwmDeal[2] = {0, 0}; + int angXout = 0, speed = 0, pos = 0, posDes = 0; + int angTmp = 0, spdTmp = 0; + int32_t lqrOut = 0, pidOut = 0; + int posL = 0, posR = 0; + putStr("MOT TIME:%s, %s\r\n", __DATE__, __TIME__); + + /** initialize spi */ + ercd = cpld_spi_init(); + configASSERT(ercd == E_OK); + + /** ensure motor stationary*/ + pwm[0] = pwm[1] = 0; + ercd = spi_write_pwm(&pwm[0], 0); + configASSERT(ercd == E_OK); + ercd = spi_write_pwm(&pwm[1], 1); + configASSERT(ercd == E_OK); + + /** initialize imu */ + ercd = imuInit(IMU_VERTICAL_IIC_ID); + configASSERT(ercd == E_OK); + + /** accelerated sequence, to avoid mutation*/ + q_init_int(&spd, SPDSEQLENMAX); + + /** yaw sequence, 2rad/s */ + q_init_int(&ang, ANGSEQLENMAX); + + /** lauch 2Ms tick interrupt */ + base_timer_init(); + + vTaskDelay(10); + + while(1) + { + + /** semphore keep 2ms-loop */ + rtn = xSemaphoreTake(SemMotTick, 2); + configASSERT(rtn == pdPASS); + + if(PRINT_TIME) + start_us = board_get_cur_us(); + + + /* + * read encder + * -qei[0] -- R + * qei[1] -- L + */ + ercd = spi_read_qei(&qei[0], 0); + configASSERT(ercd == E_OK); + ercd = spi_read_qei(&qei[1], 1); + configASSERT(ercd == E_OK); + + /* + * TODO:SPI exist data error + * For avoid fault data + */ + if(qei[1] > 2040) qei[1] = qei1Tmp; + qei1Tmp = qei[1]; + + /** read imu type:int16_t */ + ercd = imuGetValues(&imuValsV, IMU_VERTICAL_IIC_ID); + configASSERT(ercd == E_OK); + + /** relative value = initial value - bias */ + GyroY = -imuValsV.angvY + GyroYZero; + GyroX = imuValsV.angvX - GyroXZero; + + /** imu adj zeros, auto adj zeros if static 1.28s */ + if(AcqZerosEnabled) + { + staticCnt++; + staticCntCyced = (staticCnt & 0x0FFF); // cyced per 8.192s(@Ts=2ms) + + if(staticCntCyced == 127) // desire static 254ms(@Ts=2ms), adj zero start + { + gyroYZeroAcc = 0; + gyroXZeroAcc = 0; + } + else if(staticCntCyced >= 128 && staticCntCyced < 640) + { + gyroXZeroAcc += imuValsV.angvX; + gyroYZeroAcc += imuValsV.angvY; + } + else if(staticCntCyced == 640) // adj zero finish + { + GyroXZero = gyroXZeroAcc >> 9; + GyroYZero = gyroYZeroAcc >> 9; + putStr("Y%d, X%d\r\n", + GyroYZero, GyroXZero); + } + } + else + staticCnt = 0; + + /** timeout for line velocity & yaw angle */ + if(timeout <= 0) + { + + timeout = 65535; + + /** decelerated sequence */ + if(desire.Velocity != 0) + { + SeqSetEnabled = true; + desire.Velocity = 0.f; + } + } + + if(SeqSetEnabled) + { + SeqSetEnabled = false; + + /* + * based on current value, recalculate relative sequence + * update timeout + */ + q_clr(&spd); + q_clr(&ang); + + ercd = calcSeq(&spd, spdTmp, desire.Velocity, desire.Acc); + configASSERT(ercd < SPDSEQLENMAX); + + ercd = calcSeq(&ang, angTmp, desire.ThetaZ, desire.Acc); + configASSERT(ercd < ANGSEQLENMAX); + + if(desire.Timeout > 0) timeout = desire.Timeout * TS_RECI >> 16; + } + + if(AngEnabled) + { + /* + * calculate angle + * LPF for accl, avoid shaking + * change atan LUT to function, to a better accuracy + * change kalman filter to dynamic complementary filter + */ + AcclX = ((LPF_FACTOR - AcclLPFParam) * AcclX - AcclLPFParam * (imuValsV.acclX + XBias)) / LPF_FACTOR; + AcclZ = ((LPF_FACTOR - AcclLPFParam) * AcclZ - AcclLPFParam * (imuValsV.acclZ + ZBias)) / LPF_FACTOR; + accl2Ang = (int)(atan2f(AcclX, AcclZ) * ANG_FACTOR); + + AngParam = saturate(51 + AngLPFParam * abs(desire.ThetaY - accl2Ang) / 936 * abs(GyroY) / ANG_FACTOR, 128, -128); + angle1 = ((LPF_FACTOR - AngParam) * accl2Ang + AngParam * (angle1 + GyroY * GYRO_FACTOR / TS_RECI)) / LPF_FACTOR; + angle = angle1;; + } + + if(MotorEnabled) + { + if(status < 499) + { + + /** check start-up status, 0.1rad * 2^21 */ + if(((desire.ThetaY - angle) < ANGLEDIFF) && ((angle - desire.ThetaY) < ANGLEDIFF)) + { + status++; + } + else status = 0; + + /** angle delta value < ANGLEDIFF keep 1s=2ms*500, start controller*/ + if(status == 499) + { + putStr("ON\r\n"); + //AcclX = 0; + //AcclZ = 0; + //angle1 = 0; + angXout = 0; + speed = 0; + pos = 0; + posDes = 0; + spdTmp = 0; + angTmp = 0; + //lqrGyroY = 0; + SeqSetEnabled = true; + } + } + + else + { + timeout--; + + /** due to sensor question, through LR wheel difference value to obtain yaw angle */ + /** accumulate yaw angle, delta theta = s(L - R) / W, 6250 = 1 / T / W */ + YawParam = saturate(100 + 500 * abs(desire.ThetaY - angle) / ANG_FACTOR, 128, -128); + angXout += ((YawParam * (-qei[1] * ENCL_FACTOR - qei[0] * ENCR_FACTOR) / 185) + (LPF_FACTOR - YawParam) * (GyroX * GYRO_FACTOR / TS_RECI)) / LPF_FACTOR; + + /** calculate current speed, Low-pass filter*/ + speed = ((LPF_FACTOR - SpdLPFParam) * speed + SpdLPFParam * (qei[1] * ENCL_FACTOR - qei[0] * ENCR_FACTOR) / 2) / LPF_FACTOR; + /** accumulate actual position */ + pos += (qei[1] * ENCL_FACTOR - qei[0] * ENCR_FACTOR)/ TS_RECI / 2; + /** dequeue, if queue if empty, keep final value*/ + q_de_int(&spd, &spdTmp); + q_de_int(&ang, &angTmp); + + /** calcilate setting position */ + posDes += spdTmp / TS_RECI; + + pidOut = pidTick(&angPid, angTmp - angXout); + + lqrGyroY = ((LPF_FACTOR - GyroLPFParam) * lqrGyroY + GyroLPFParam * GyroY * GYRO_FACTOR) / LPF_FACTOR; + + lqrOut = lqrCalc(&lqr, + // saturate(posDes - pos, 65000, -65000), + posDes - pos, + spdTmp - speed, + desire.ThetaY - angle, + desire.Omega - lqrGyroY + ); + + if(((desire.ThetaY - angle) > ANGLIMIT) || ((angle - desire.ThetaY) > ANGLIMIT)) + { + /** avoid reset pwm fail */ + if(status < 600) + { + lqrOut = lqrOut / (status - 499); + pidOut = pidOut / (status - 499); + status++; + } + else + { + lqrOut = 0; + pidOut = 0; + status++; + if(status == 610) + status = 0; + } + } + + /* + * (lqrOut >> (k_factor + 5)) / battery voltage --> normalization + * pidOut range[-1, 1] + * pwm = lqrOut/ Vref + pidOut + * saturate pwm to [-65000, 65000], equal to [-0.99, 0.99] << 16 + * pwm >> 5 to be 12 bits + */ + pwmDeal[0] = saturate((lqrOut >> 10) - pidOut , 64200, -64200); + pwmDeal[1] = saturate((lqrOut >> 10) + pidOut , 64200, -64200); + Asm("asrsr %0, %1, %2" : "=r"(pwm[0]) : "r"(pwmDeal[0]), "r"(5)); + Asm("asrsr %0, %1, %2" : "=r"(pwm[1]) : "r"(pwmDeal[1]), "r"(5)); + + ercd = spi_write_pwm(&pwm[0], 0); + configASSERT(ercd == E_OK); + ercd = spi_write_pwm(&pwm[1], 1); + configASSERT(ercd == E_OK); + //tsCtl = 0; + } + } + + if(PutStrEnabled){ + if(timeCnt > PRINT_PERIOD){ + /* + * MPU & PC protocol + * cmdf data cmdr + * [0x03 0xFC ... 0xFC 0x03] + * data can't change to ASCII code + */ + putData(cmdf, 2); + /* approximately 1024 timers large*/ + if(DEBUG_LQR){ + data[0] = angle >> 11; + data[1] = GyroY; + data[2] = pos >> 11; + data[3] = speed >> 11; + data[4] = lqrOut >> 12; + data[5] = angXout >> 11; + data[6] = pidOut; + data[7] = end_us; + putData(data, 8 * sizeof(int)); + } + + else if(PRINT_PWM){ + data[0] = qei[1] * ENCL_FACTOR >> 11; + data[1] = -qei[0] * ENCR_FACTOR >> 11; + data[2] = pwm[0]; + data[3] = pwm[1]; + data[4] = spdTmp >> 11; + data[5] = angTmp >> 11; + data[6] = timeout; + data[7] = end_us; + putData(data, 8 * sizeof(int)); + } + + else if(PRINT_IMU){ + data[0] = GyroY; + data[1] = GyroX; + data[2] = AcclX >> 1; + data[3] = AcclZ >> 1; + data[4] = accl2Ang >> 11; + data[5] = angle >> 11; + data[6] = AngParam; + data[7] = end_us; + putData(data, 8 * sizeof(int)); + } + + putData(cmdr, 2); + + timeCnt = 0; + } + timeCnt++; + + } + + if(xQueueReceive(motMbCmd, &msg, (TickType_t)0)) + { + switch(msg & 0xFFFF0000) + { + case EnableMotors: + spdTmp = 0.f; + angTmp = 0.f; + MotorEnabled = true; + break; + + case DisableMotors: + // write pwm 0 + pwm[0] = 0; + pwm[1] = 0; + ercd = spi_write_pwm(&pwm[0], 0); + configASSERT(ercd == E_OK); + ercd = spi_write_pwm(&pwm[1], 1); + configASSERT(ercd == E_OK); + + angPid.accI = 0; + angPid.accD = 0; + + AngleKal.xpos = 0; + AngleKal.Ppos = 0; + + status = 0; + MotorEnabled = false; + break; + + case EnableAng: + AngEnabled = true; + break; + case DisableAng: + AngEnabled = false; + break; + case EnableAcqZeros: + AcqZerosEnabled = true; + break; + case DisableAcqZeros: + AcqZerosEnabled = false; + break; + case EnableSeqSet: + SeqSetEnabled = true; + break; + case DisableSeqSet: + SeqSetEnabled = false; + break; + case EnablePutStr: + PutStrEnabled = true; + break; + case DisablePutStr: + PutStrEnabled = false; + break; + default: + break; + } + } + + if(PRINT_TIME) + end_us = board_get_cur_us() - start_us; + } + +} + +void motorInit() +{ + + BaseType_t rtn; + + motMbCmd = xQueueCreate(4, sizeof(MsgType)); + configASSERT(motMbCmd); + + // Create tasks + rtn=xTaskCreate(motorTask, "motorTask", motorTskStkSize, NULL, motorTskPrio, NULL); + configASSERT(rtn==pdPASS); + +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskMotor/TskMotor.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskMotor/TskMotor.h new file mode 100644 index 00000000..e4df4136 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskMotor/TskMotor.h @@ -0,0 +1,166 @@ +#ifndef _TSKMOTOR_H_ +#define _TSKMOTOR_H_ + +#include "physParams.h" +#include "embARC.h" +#include "Queue/Queue.h" + +typedef enum +{ + EnableMotors = 0x00010000, + DisableMotors = 0x00020000, + EnableAng = 0x00030000, + DisableAng = 0x00040000, + EnableAcqZeros = 0x00050000, + DisableAcqZeros = 0x00060000, + EnableSeqSet = 0x00070000, + DisableSeqSet = 0x00080000, + EnablePutStr = 0x00090000, + DisablePutStr = 0x000A0000 + +}MsgType; + +extern QueueHandle_t motMbCmd; +extern Kalman1Var AngleKal; +extern lqr_q lqr; +extern VelOmega desire; +extern Pid angPid; +extern int AcclLPFParam; +extern int SpdLPFParam; +extern int AngLPFParam; +extern int GyroLPFParam; +extern int ANGLEDIFF; +extern int ANGLIMIT; +extern int PRINT_PERIOD; +extern bool PRINT_TIME; +extern bool DEBUG_LQR; +extern bool PRINT_IMU; +extern bool PRINT_PWM; + +/** [saturate: data between min and max] */ +Inline int saturate(int data, int max, int min) +{ + int value = (data > max)? max : ((data < min)? min : data); + return value; +} + +/** [saturate: data between min and max] */ +Inline int deadzone(int data, int plimit, int nlimit) +{ + int value = (data > plimit)? data : ((data < nlimit)? data : 0); + return value; +} + +/** [calcSeq: Enqueue (data calc based on Ts) */ +Inline int calcSeq(Queue *q, int32_t p0, int32_t p1, int32_t acc) +{ + int i, imax; + if(p0 == p1) + return 0; + else + { + if(p1 < p0) acc = -acc; + /** t = delta_p / acc, imax = t / Ts */ + imax = (int)((p1 - p0) * TS_RECI / acc); + + if(imax > SPDSEQLENMAX - 1) + return (SPDSEQLENMAX + 1); + else{ + for(i = 1; i <= imax; i++) + { + q_en_int(q, p0 + (int)(i * acc / TS_RECI)); + } + q_en_int(q, p1); + } + return (imax + 1); + } +} + +/** [lqrCalc_q: calculate lqrOut, lqrOut -> voltage] */ +Inline int32_t lqrCalc_q(lqr_q *lqr, int32_t pos, int32_t vel, int32_t ang, int32_t av) +{ + + int32_t res = lqr->k1 * pos + lqr->k2 * vel + lqr->k3 * ang + lqr->k4 * av; + + return res; +} + +/** [kalmanPredict_q: kalman predict xpri & Ppri] */ +Inline void kalmanPredict_q(Kalman1Var *kalVal, int32_t u) +{ + /* + * x(k|(k-1)) = A * x((k-1)|(k-1)) + B * u(k) + * enlarge gyro and ensure xpri factor(16) + */ + kalVal->xpri = kalVal->xpos + ((B * u * GYRO_FACTOR) >> 15); + + /* + * p(k|(k-1)) = A * p((k-1)|(k-1)) * A.Tran() + Q + * A = A.Tran() = 1 << 16 + * for ensure amplification factor, ingore multiplication associated with A + */ + kalVal->Ppri = kalVal->Ppos + kalVal->Q; +} + +/** [kalmanPredict_q: kalman correct K & Ppos & xpos] */ +Inline int32_t kalmanCorrect_q(Kalman1Var *kalVal, int32_t z) +{ + /* + * K(k) = p(k|(k-1)) * H.Tran() * ((H * p(k|(k-1)) * H.Tran() + R)).Inv() + * H = H.Tran() = 1 << 16, ingore multiplication, so + * K(k) = p(k|(k-1)) / ( p(k|(k-1)) + R) + * for ensure amplification factor, K(k) = K(k) << 16 + */ + kalVal->K = (kalVal->Ppri << 15) / (kalVal->Ppri + kalVal->R); + /* + * x(k|k) = x(k|(k-1)) + K(k) * (z(k) - H * x(k|(k-1))) + * H = 1 << 16, ingore multiplication + */ + kalVal->xpos = kalVal->xpri + (kalVal->K * (z * ANG_FACTOR - kalVal->xpri) >> 15); + /* + * p(k|k) = ( I – K(k) * H ) * p(k|(k-1)) + * H = 1 << 16, ingore multiplication + */ + kalVal->Ppos = (I - kalVal->K) * kalVal->Ppri >> 16; + + return kalVal->xpos; +} + +/** [pidTick_q: calculate pidOut, pidOut -> normalization (factor 2^16)] */ +Inline int32_t pidTick_q(Pid *pid, int32_t diff) +{ + int32_t pout; + int32_t dout; + + pout = ((diff) * pid->p) >> 8; + + pid->accI += (diff * pid->i / 500) >> 8; + + if(pid->accI > 65536) + pid->accI = 65536; + else if(pid->accI < -65536) + pid->accI = -65536; + + dout = ((diff * pid->d) >> 8 - pid->accD) * pid->n >> 8; + pid->accD += dout / 500; + + if(pid->accD > 65536) + pid->accD = 65536; + else if(pid->accD < -65536) + pid->accD = -65536; + + pout += pid->accI; + + pout += dout; + + if(pout >65536) + pout = 65536; + else if(pout < -65536) + pout = -65536; + + return pout; +} + +void motorInit(void); + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskTop/TskTop.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskTop/TskTop.c new file mode 100644 index 00000000..06597d07 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskTop/TskTop.c @@ -0,0 +1,38 @@ +#include "embARC.h" +#include "embARC_debug.h" + +#include "dbgUart.h" +#include "cmd.h" +#include "TskMotor.h" + +const int topTskPrio = 2; +const int topTskStkSize = 128; + +void topTask(void *pvParameters) +{ + /** first initialize debug Task, print system information */ + dbgInit(); + /** then initialize motor Task */ + motorInit(); + vTaskDelay(50); + + putStr("TOP TIME:%s, %s\r\n", __DATE__, __TIME__); + vTaskDelay(100); + + while(1) + { + /** processing control command */ + paramCorr(); + } + +} + +void topInit() +{ + BaseType_t rtn; + + // Create tasks + rtn=xTaskCreate(topTask, "topTask", topTskStkSize, NULL, topTskPrio, NULL); + configASSERT(rtn==pdPASS); + +} diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskTop/TskTop.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskTop/TskTop.h new file mode 100644 index 00000000..ba417135 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/TskTop/TskTop.h @@ -0,0 +1,6 @@ +#ifndef _TskTop_H_ +#define _TskTop_H_ + +void topInit(); + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/algorithm.s b/arc_design_contest/2018/HUST_inverted_pendulum/scr/algorithm.s new file mode 100644 index 00000000..958d0521 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/algorithm.s @@ -0,0 +1,132 @@ + .file "algorithm.s" + +/* entry for lqr algorithm */ + .text + .global kalmanPredict, kalmanCorrect, accl2AngleIndex, lqrCalc, pidTick + .align 4 + +kalmanPredict: + mpy r1,r1,0x42 ;B*u + ld_s r3,[r0,0x14] ;xpos + ld_s r2,[r0,0x18] ;Ppos + ld r4,[r0] ;Q + add_s r2,r2,r4 ;Q+Ppos + st r2,[r0,0x10] ;Ppri = Q+Ppos + asrsr r1,r1,0x0F ;RND32((B*u)>>15) + add_s r1,r3,r1 ;xpos + RND32((B*u)>>15) + j_s.d [blink] + st r1,[r0,0xc] ;xpri = xpos + RND32((B*u)>>15) + nop_s + +kalmanCorrect: + ld r5,[r0,16] ;Ppri + ld r4,[r0,4] ;R + ld r6,[r0,12] ;xpri + asl r2,r5,0x0F ;Ppri << 15 + add r4,r4,r5 ;Ppri + R + asrsr r3,r4,0x1 ;(Ppri + R) / 2 + add_s r2,r2,r3 ;(Ppri << 15) + (Ppri + R) / 2 + div r4,r2,r4 ;((Ppri << 15) + (Ppri + R) / 2) / (Ppri + R) + rsub r3,r4,0x1000 ;I - k + subs r1,r1,r6 ;z - xpri + mpy r3,r3,r5 ;(I - k) * Ppri + st r4,[r0,8] ;k = ((Ppri << 15) + (Ppri + R) / 2) / (Ppri + R) + mpy r1,r1,r4 ;(z - xpri) * k + asrsr r3,r3,0x0F ;RND32((I - k) * Ppri>>15) + st r3,[r0,0x18] ;Ppos = RND32((I - k) * Ppri>>15) + asrsr r1,r1,0x0F ;RND32((z - xpri) * k>>15) + add r1,r1,r6 ;xpri + RND32((z - xpri) * k>>15) + st r1,[r0,0x14] ;xpos = xpri + RND32((z - xpri) * k>>15) + j_s.d [blink] + mov_s r0,r1 + nop_s + +accl2AngleIndex: + ldh.x r6,[r1] ;AcclX + ldh.x r7,[r2] ;AcclZ + ldh r4,[r0,0] ;imuVals.acclX + ldh r5,[r0,4] ;imuVals.acclZ + ldh r0,[r3] ;AcclLPFParam + rsub r8,r0,128 ;128 - AcclLPFParam + mpyw r6,r8,r6 ;AcclX * (128 - AcclLPFParam) + mpyw r4,r0,r4 ;imuVals.acclX * AcclLPFParam + sub r6,r6,r4 ;AcclX * (128 - AcclLPFParam) - imuVals.acclX * AcclLPFParam + asrsr r6,r6,7 ;RND32(AcclX * (128 - AcclLPFParam) - imuVals.acclX * AcclLPFParam) >> 7 + sth r6,[r1] ;AcclX = RND32(AcclX * (128 - AcclLPFParam) - imuVals.acclX * AcclLPFParam) >> 7; + mpyw r7,r8,r7 ;AcclZ * (128 - AcclLPFParam) + mpyw r5,r0,r5 ;imuVals.acclZ * AcclLPFParam + sub r7,r7,r5 ;AcclZ * (128 - AcclLPFParam) - imuVals.acclZ * AcclLPFParam + asrsr r7,r7,7 ;RND32(AcclZ * (128 - AcclLPFParam) - imuVals.acclZ * AcclLPFParam) >> 7 + sth r7,[r2] ;AcclZ = RND32(AcclZ * (128 - AcclLPFParam) - imuVals.acclZ * AcclLPFParam) >> 7; + asls r6,r6,7 ;AcclX * 128 + div r0,r6,r7 ;AcclX * 128 / AcclZ + adds r0,r0,192 ;index = AcclX * 128 / AcclZ + 192 + asls r0,r0,7 ;index << 7 (0~255 -> 0~32768) + abssh r0,r0 ;SAT16(abs(index)), keep abs(index)<255 + + j_s.d [blink] + asrsr r0,r0,7 + nop_s + +lqrCalc: + ld r5,[r0] + ld r7,[r0,4] + ld r6,[r0,8] + ld_s r0,[r0,0xC] + mpy r1,r5,r1 + mpy r2,r7,r2 + mpy r3,r6,r3 + add_s r1,r1,r2 + mpy r0,r0,r4 + add_s r1,r1,r3 + j_s.d [blink] + add_s r0,r0,r1 + nop_s + +pidTick: + ld_s r3,[r0,0] ;p + ld r4,[r0,4] ;i + ld r5,[r0,8] ;d + ld r6,[r0,0x0c] ;n + ld r7,[r0,0x10] ;accI + ld r8,[r0,0x14] ;accD + mpy_s r3,r3,r1 ;p*diff + asrsr r3,r3,8 ;pout=p*diff>>8 + mpy r5,r5,r1 ;d*diff + asrsr r5,r5,8 ;d*diff>>8 + sub r5,r5,r8 ;d*diff>>8 - accD + mpy r5,r5,r6 ;(d*diff>>8 - accD)*n + asrsr r5,r5,8 ;dout=(d*diff>>8 - accD)*n >> 8 + mpymu r2,r5,0x10624dd3 + asr r5,r5,0x1f + and r6,r5,0x10624dd3 + sub r2,r2,r6 + asr_s r2,r2,0x5 + sub r6,r2,r6 + # div r6,r5,500 ;dout*Ts + add r8,r6,r8 ;accD+=dout*Ts + mpy r4,r4,r1 ;i*diff + asls r8,r8,10 ;SAT32(accD<<10), keep abs(accD)<65536 + mpymu r2,r4,0x10624dd3 + asrsr r8,r8,10 ;accD >> 10 + asr r4,r4,0x1f + and r1,r4,0x10624dd3 + sub r2,r2,r1 + asr_s r2,r2,0x5 + sub r4,r2,r4 + # div r4,r4,500 ;i*diff*Ts + st r8,[r0,0x14] + asrsr r4,r4,8 ;i*diff*Ts>>8 + add r7,r7,r4 ;accI+=i*diff*Ts>>8 + add r3,r3,r8 ;pout+=acclD + asls r7,r7,10 ;SAT32(accI<<10), keep abs(accI)<65536 + asrsr r7,r7,10 ;accI >> 10 + st r7,[r0,0x10] + add r3,r3,r7 ;pout+=accI + asls r0,r3,10 ;SAT32(pout<<10), keep abs(pout)<65536 + j_s.d [blink] + asrsr r0,r0,15 ;pout >> 15 + nop_s + + + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/assert.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/assert.c new file mode 100644 index 00000000..4c1217a7 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/assert.c @@ -0,0 +1,8 @@ +#include "dbgUart.h" + +void vApplicationAssert(const char *file, unsigned int line) +{ + putStr("fail in %s, line %d\r\n", file, line); + vTaskDelay(100); + while(1); +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpKey.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpKey.h new file mode 100644 index 00000000..c8f62a2a --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpKey.h @@ -0,0 +1,494 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef DMPKEY_H__ +#define DMPKEY_H__ + +#define KEY_CFG_25 (0) +#define KEY_CFG_24 (KEY_CFG_25 + 1) +#define KEY_CFG_26 (KEY_CFG_24 + 1) +#define KEY_CFG_27 (KEY_CFG_26 + 1) +#define KEY_CFG_21 (KEY_CFG_27 + 1) +#define KEY_CFG_20 (KEY_CFG_21 + 1) +#define KEY_CFG_TAP4 (KEY_CFG_20 + 1) +#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1) +#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1) +#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1) +#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1) +#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1) +#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1) +#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1) +#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1) +#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1) +#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1) +#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1) +#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1) +#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1) +#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1) +#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1) +#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1) +#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1) +#define KEY_FCFG_1 (KEY_CFG_23 + 1) +#define KEY_FCFG_3 (KEY_FCFG_1 + 1) +#define KEY_FCFG_2 (KEY_FCFG_3 + 1) +#define KEY_CFG_3D (KEY_FCFG_2 + 1) +#define KEY_CFG_3B (KEY_CFG_3D + 1) +#define KEY_CFG_3C (KEY_CFG_3B + 1) +#define KEY_FCFG_5 (KEY_CFG_3C + 1) +#define KEY_FCFG_4 (KEY_FCFG_5 + 1) +#define KEY_FCFG_7 (KEY_FCFG_4 + 1) +#define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1) +#define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1) +#define KEY_FCFG_6 (KEY_FCFG_AZ + 1) +#define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1) +#define KEY_CFG_12 (KEY_FCFG_LSB4 + 1) +#define KEY_CFG_14 (KEY_CFG_12 + 1) +#define KEY_CFG_15 (KEY_CFG_14 + 1) +#define KEY_CFG_16 (KEY_CFG_15 + 1) +#define KEY_CFG_18 (KEY_CFG_16 + 1) +#define KEY_CFG_6 (KEY_CFG_18 + 1) +#define KEY_CFG_7 (KEY_CFG_6 + 1) +#define KEY_CFG_4 (KEY_CFG_7 + 1) +#define KEY_CFG_5 (KEY_CFG_4 + 1) +#define KEY_CFG_2 (KEY_CFG_5 + 1) +#define KEY_CFG_3 (KEY_CFG_2 + 1) +#define KEY_CFG_1 (KEY_CFG_3 + 1) +#define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1) +#define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1) +#define KEY_CFG_9 (KEY_CFG_8 + 1) +#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1) +#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1) +#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1) +#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1) +#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1) +#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1) +#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1) +#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1) +#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1) +#define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1) + +/* MPU6050 keys */ +#define KEY_CFG_ACCEL_FILTER (KEY_CFG_LP_QUAT + 1) +#define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1) +#define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1) + +#define KEY_D_0_22 (KEY_TEMPLABEL + 1) +#define KEY_D_0_24 (KEY_D_0_22 + 1) +#define KEY_D_0_36 (KEY_D_0_24 + 1) +#define KEY_D_0_52 (KEY_D_0_36 + 1) +#define KEY_D_0_96 (KEY_D_0_52 + 1) +#define KEY_D_0_104 (KEY_D_0_96 + 1) +#define KEY_D_0_108 (KEY_D_0_104 + 1) +#define KEY_D_0_163 (KEY_D_0_108 + 1) +#define KEY_D_0_188 (KEY_D_0_163 + 1) +#define KEY_D_0_192 (KEY_D_0_188 + 1) +#define KEY_D_0_224 (KEY_D_0_192 + 1) +#define KEY_D_0_228 (KEY_D_0_224 + 1) +#define KEY_D_0_232 (KEY_D_0_228 + 1) +#define KEY_D_0_236 (KEY_D_0_232 + 1) + +#define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1) +#define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1) +#define KEY_D_1_4 (KEY_D_1_2 + 1) +#define KEY_D_1_8 (KEY_D_1_4 + 1) +#define KEY_D_1_10 (KEY_D_1_8 + 1) +#define KEY_D_1_24 (KEY_D_1_10 + 1) +#define KEY_D_1_28 (KEY_D_1_24 + 1) +#define KEY_D_1_36 (KEY_D_1_28 + 1) +#define KEY_D_1_40 (KEY_D_1_36 + 1) +#define KEY_D_1_44 (KEY_D_1_40 + 1) +#define KEY_D_1_72 (KEY_D_1_44 + 1) +#define KEY_D_1_74 (KEY_D_1_72 + 1) +#define KEY_D_1_79 (KEY_D_1_74 + 1) +#define KEY_D_1_88 (KEY_D_1_79 + 1) +#define KEY_D_1_90 (KEY_D_1_88 + 1) +#define KEY_D_1_92 (KEY_D_1_90 + 1) +#define KEY_D_1_96 (KEY_D_1_92 + 1) +#define KEY_D_1_98 (KEY_D_1_96 + 1) +#define KEY_D_1_100 (KEY_D_1_98 + 1) +#define KEY_D_1_106 (KEY_D_1_100 + 1) +#define KEY_D_1_108 (KEY_D_1_106 + 1) +#define KEY_D_1_112 (KEY_D_1_108 + 1) +#define KEY_D_1_128 (KEY_D_1_112 + 1) +#define KEY_D_1_152 (KEY_D_1_128 + 1) +#define KEY_D_1_160 (KEY_D_1_152 + 1) +#define KEY_D_1_168 (KEY_D_1_160 + 1) +#define KEY_D_1_175 (KEY_D_1_168 + 1) +#define KEY_D_1_176 (KEY_D_1_175 + 1) +#define KEY_D_1_178 (KEY_D_1_176 + 1) +#define KEY_D_1_179 (KEY_D_1_178 + 1) +#define KEY_D_1_218 (KEY_D_1_179 + 1) +#define KEY_D_1_232 (KEY_D_1_218 + 1) +#define KEY_D_1_236 (KEY_D_1_232 + 1) +#define KEY_D_1_240 (KEY_D_1_236 + 1) +#define KEY_D_1_244 (KEY_D_1_240 + 1) +#define KEY_D_1_250 (KEY_D_1_244 + 1) +#define KEY_D_1_252 (KEY_D_1_250 + 1) +#define KEY_D_2_12 (KEY_D_1_252 + 1) +#define KEY_D_2_96 (KEY_D_2_12 + 1) +#define KEY_D_2_108 (KEY_D_2_96 + 1) +#define KEY_D_2_208 (KEY_D_2_108 + 1) +#define KEY_FLICK_MSG (KEY_D_2_208 + 1) +#define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1) +#define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1) +#define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1) +#define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1) +#define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1) +#define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1) +#define KEY_D_2_244 (KEY_D_2_224 + 1) +#define KEY_D_2_248 (KEY_D_2_244 + 1) +#define KEY_D_2_252 (KEY_D_2_248 + 1) + +#define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1) +#define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1) +#define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1) +#define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1) +#define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1) +#define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1) +#define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1) +#define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1) +#define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE +1) +#define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1) +#define KEY_D_CR_TIME_G (KEY_D_OUTPUT_ENABLE + 1) +#define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1) +#define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1) +#define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1) +#define KEY_D_CS_TAY (KEY_D_CS_TAX + 1) +#define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1) +#define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1) +#define KEY_D_CS_TGY (KEY_D_CS_TGX + 1) +#define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1) +#define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1) +#define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1) +#define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1) +#define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1) + +/* Compass keys */ +#define KEY_CPASS_BIAS_X (KEY_D_CS_TQ3 + 1) +#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1) +#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1) +#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1) +#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1) +#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1) +#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1) +#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1) +#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1) +#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1) +#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1) +#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1) + +/* Gesture Keys */ +#define KEY_DMP_TAPW_MIN (KEY_CPASS_MTX_22 + 1) +#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN + 1) +#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X + 1) +#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y + 1) +#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z + 1) +#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1) +#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1) +#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1) +#define KEY_D_ACT0 (KEY_DMP_ORIENT + 1) +#define KEY_D_ACSX (KEY_D_ACT0 + 1) +#define KEY_D_ACSY (KEY_D_ACSX + 1) +#define KEY_D_ACSZ (KEY_D_ACSY + 1) + +#define KEY_X_GRT_Y_TMP (KEY_D_ACSZ + 1) +#define KEY_SKIP_X_GRT_Y_TMP (KEY_X_GRT_Y_TMP + 1) +#define KEY_SKIP_END_COMPARE (KEY_SKIP_X_GRT_Y_TMP + 1) +#define KEY_END_COMPARE_Y_X_TMP2 (KEY_SKIP_END_COMPARE + 1) +#define KEY_CFG_ANDROID_ORIENT_INT (KEY_END_COMPARE_Y_X_TMP2 + 1) +#define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_ANDROID_ORIENT_INT + 1) +#define KEY_END_COMPARE_Y_X_TMP (KEY_NO_ORIENT_INTERRUPT + 1) +#define KEY_END_ORIENT_1 (KEY_END_COMPARE_Y_X_TMP + 1) +#define KEY_END_COMPARE_Y_X (KEY_END_ORIENT_1 + 1) +#define KEY_END_ORIENT (KEY_END_COMPARE_Y_X + 1) +#define KEY_X_GRT_Y (KEY_END_ORIENT + 1) +#define KEY_NOT_TIME_MINUS_1 (KEY_X_GRT_Y + 1) +#define KEY_END_COMPARE_Y_X_TMP3 (KEY_NOT_TIME_MINUS_1 + 1) +#define KEY_X_GRT_Y_TMP2 (KEY_END_COMPARE_Y_X_TMP3 + 1) + +/* Authenticate Keys */ +#define KEY_D_AUTH_OUT (KEY_X_GRT_Y_TMP2 + 1) +#define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1) +#define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1) +#define KEY_D_AUTH_B (KEY_D_AUTH_A + 1) + +/* Pedometer standalone only keys */ +#define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1) +#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1) +#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1) +#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1) +#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1) +#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1) +#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1) +#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1) +#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1) +#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1) +#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1) +#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1) +#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1) +#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1) +#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1) +#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1) +#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1) +#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR + 1) +#define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1) + +/*Host Based No Motion*/ +#define KEY_D_HOST_NO_MOT (KEY_D_PEDSTD_DECI + 1) + +/* EIS keys */ +#define KEY_P_EIS_FIFO_FOOTER (KEY_D_HOST_NO_MOT + 1) +#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER + 1) +#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT + 1) +#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE + 1) +#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT + 1) +#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC + 1) +#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT + 1) +#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY + 1) +#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER + 1) +#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC + 1) +#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH + 1) +#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH + 1) +#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH + 1) +#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH + 1) +#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH + 1) +#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H + 1) +#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H + 1) +#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H + 1) +#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H + 1) +#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H + 1) +#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H + 1) +#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH + 1) +#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH + 1) +#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH + 1) +#define KEY_DMP_INTY_H (KEY_DMP_INTX_H + 1) +#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H + 1) +//#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H + 1) +//#define KEY_DMP_HPY_H (KEY_DMP_HPX_H + 1) +//#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H + 1) + +/* Stream keys */ +#define KEY_STREAM_P_GYRO_Z (KEY_DMP_INTZ_H + 1) +#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z + 1) +#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y + 1) +#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X + 1) +#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP + 1) +#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y + 1) +#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X + 1) +#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z + 1) +#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y + 1) +#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X + 1) +#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER + 1) + +#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z + 1) + +typedef struct { + unsigned short key; + unsigned short addr; +} tKeyLabel; + +#define DINA0A 0x0a +#define DINA22 0x22 +#define DINA42 0x42 +#define DINA5A 0x5a + +#define DINA06 0x06 +#define DINA0E 0x0e +#define DINA16 0x16 +#define DINA1E 0x1e +#define DINA26 0x26 +#define DINA2E 0x2e +#define DINA36 0x36 +#define DINA3E 0x3e +#define DINA46 0x46 +#define DINA4E 0x4e +#define DINA56 0x56 +#define DINA5E 0x5e +#define DINA66 0x66 +#define DINA6E 0x6e +#define DINA76 0x76 +#define DINA7E 0x7e + +#define DINA00 0x00 +#define DINA08 0x08 +#define DINA10 0x10 +#define DINA18 0x18 +#define DINA20 0x20 +#define DINA28 0x28 +#define DINA30 0x30 +#define DINA38 0x38 +#define DINA40 0x40 +#define DINA48 0x48 +#define DINA50 0x50 +#define DINA58 0x58 +#define DINA60 0x60 +#define DINA68 0x68 +#define DINA70 0x70 +#define DINA78 0x78 + +#define DINA04 0x04 +#define DINA0C 0x0c +#define DINA14 0x14 +#define DINA1C 0x1C +#define DINA24 0x24 +#define DINA2C 0x2c +#define DINA34 0x34 +#define DINA3C 0x3c +#define DINA44 0x44 +#define DINA4C 0x4c +#define DINA54 0x54 +#define DINA5C 0x5c +#define DINA64 0x64 +#define DINA6C 0x6c +#define DINA74 0x74 +#define DINA7C 0x7c + +#define DINA01 0x01 +#define DINA09 0x09 +#define DINA11 0x11 +#define DINA19 0x19 +#define DINA21 0x21 +#define DINA29 0x29 +#define DINA31 0x31 +#define DINA39 0x39 +#define DINA41 0x41 +#define DINA49 0x49 +#define DINA51 0x51 +#define DINA59 0x59 +#define DINA61 0x61 +#define DINA69 0x69 +#define DINA71 0x71 +#define DINA79 0x79 + +#define DINA25 0x25 +#define DINA2D 0x2d +#define DINA35 0x35 +#define DINA3D 0x3d +#define DINA4D 0x4d +#define DINA55 0x55 +#define DINA5D 0x5D +#define DINA6D 0x6d +#define DINA75 0x75 +#define DINA7D 0x7d + +#define DINADC 0xdc +#define DINAF2 0xf2 +#define DINAAB 0xab +#define DINAAA 0xaa +#define DINAF1 0xf1 +#define DINADF 0xdf +#define DINADA 0xda +#define DINAB1 0xb1 +#define DINAB9 0xb9 +#define DINAF3 0xf3 +#define DINA8B 0x8b +#define DINAA3 0xa3 +#define DINA91 0x91 +#define DINAB6 0xb6 +#define DINAB4 0xb4 + + +#define DINC00 0x00 +#define DINC01 0x01 +#define DINC02 0x02 +#define DINC03 0x03 +#define DINC08 0x08 +#define DINC09 0x09 +#define DINC0A 0x0a +#define DINC0B 0x0b +#define DINC10 0x10 +#define DINC11 0x11 +#define DINC12 0x12 +#define DINC13 0x13 +#define DINC18 0x18 +#define DINC19 0x19 +#define DINC1A 0x1a +#define DINC1B 0x1b + +#define DINC20 0x20 +#define DINC21 0x21 +#define DINC22 0x22 +#define DINC23 0x23 +#define DINC28 0x28 +#define DINC29 0x29 +#define DINC2A 0x2a +#define DINC2B 0x2b +#define DINC30 0x30 +#define DINC31 0x31 +#define DINC32 0x32 +#define DINC33 0x33 +#define DINC38 0x38 +#define DINC39 0x39 +#define DINC3A 0x3a +#define DINC3B 0x3b + +#define DINC40 0x40 +#define DINC41 0x41 +#define DINC42 0x42 +#define DINC43 0x43 +#define DINC48 0x48 +#define DINC49 0x49 +#define DINC4A 0x4a +#define DINC4B 0x4b +#define DINC50 0x50 +#define DINC51 0x51 +#define DINC52 0x52 +#define DINC53 0x53 +#define DINC58 0x58 +#define DINC59 0x59 +#define DINC5A 0x5a +#define DINC5B 0x5b + +#define DINC60 0x60 +#define DINC61 0x61 +#define DINC62 0x62 +#define DINC63 0x63 +#define DINC68 0x68 +#define DINC69 0x69 +#define DINC6A 0x6a +#define DINC6B 0x6b +#define DINC70 0x70 +#define DINC71 0x71 +#define DINC72 0x72 +#define DINC73 0x73 +#define DINC78 0x78 +#define DINC79 0x79 +#define DINC7A 0x7a +#define DINC7B 0x7b + +#define DIND40 0x40 + + +#define DINA80 0x80 +#define DINA90 0x90 +#define DINAA0 0xa0 +#define DINAC9 0xc9 +#define DINACB 0xcb +#define DINACD 0xcd +#define DINACF 0xcf +#define DINAC8 0xc8 +#define DINACA 0xca +#define DINACC 0xcc +#define DINACE 0xce +#define DINAD8 0xd8 +#define DINADD 0xdd +#define DINAF8 0xf0 +#define DINAFE 0xfe + +#define DINBF8 0xf8 +#define DINAC0 0xb0 +#define DINAC1 0xb1 +#define DINAC2 0xb4 +#define DINAC3 0xb5 +#define DINAC4 0xb8 +#define DINAC5 0xb9 +#define DINBC0 0xc0 +#define DINBC2 0xc2 +#define DINBC4 0xc4 +#define DINBC6 0xc6 + + + +#endif // DMPKEY_H__ diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpTest.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpTest.c new file mode 100644 index 00000000..b775e5aa --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpTest.c @@ -0,0 +1,214 @@ +/** + * \file + * \ingroup EMBARC_APP_BAREMETAL_BLINKY + * \brief main source file for blinky example + */ + +/** + * \addtogroup EMBARC_APP_BAREMETAL_BLINKY + * @{ + */ +/* embARC HAL */ +#include "embARC.h" +#include "embARC_debug.h" +#include "imu.h" +#include "inv_mpu.h" +#include "inv_mpu_dmp_motion_driver.h" +#include "dbgUart.h" +#include + +#define DEFAULT_MPU_HZ 200 + +/* Platform-specific information. Kinda like a boardfile. */ +struct platform_data_s { + signed char orientation[9]; +}; + +/* The sensors can be mounted onto the board in any orientation. The mounting + * matrix seen below tells the MPL how to rotate the raw data from the + * driver(s). + * TODO: The following matrices refer to the configuration on internal test + * boards at Invensense. If needed, please modify the matrices to match the + * chip-to-body matrix for your particular set up. + */ +static struct platform_data_s gyro_pdata = { + .orientation = { 1, 0, 0, + 0, -1, 0, + 0, 0, -1} +}; + +static unsigned short inv_row_2_scale(const signed char *row) +{ + unsigned short b; + + if (row[0] > 0) + b = 0; + else if (row[0] < 0) + b = 4; + else if (row[1] > 0) + b = 1; + else if (row[1] < 0) + b = 5; + else if (row[2] > 0) + b = 2; + else if (row[2] < 0) + b = 6; + else + b = 7; // error + return b; +} + +/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. +* @param[in] mtx Orientation matrix to convert to a scalar. +* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the +* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent +* the column the one is on for the second row with bit number 5 being the sign. +* The next 2 bits (6 and 7) represent the column the one is on for the third row with +* bit number 8 being the sign. In binary the identity matrix would therefor be: +* 010_001_000 or 0x88 in hex. +*/ +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) +{ + + unsigned short scalar; + + /* + XYZ 010_001_000 Identity Matrix + XZY 001_010_000 + YXZ 010_000_001 + YZX 000_010_001 + ZXY 001_000_010 + ZYX 000_001_010 + */ + + scalar = inv_row_2_scale(mtx); + scalar |= inv_row_2_scale(mtx + 3) << 3; + scalar |= inv_row_2_scale(mtx + 6) << 6; + + + return scalar; +} + +int dmp_init(void) +{ + long gyroZero[3] = {-44, 35, -38}; + + mpu6050_enable(); + + if(!mpu_init()) + { + if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) + putStr("mpu_set_sensor complete ......\r\n"); + vTaskDelay(50); + if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) + putStr("mpu_configure_fifo complete ......\r\n"); + vTaskDelay(50); + if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) + putStr("mpu_set_sample_rate complete ......\r\n"); + vTaskDelay(50); + if(!mpu_set_gyro_bias_reg(gyroZero)) + putStr("mpu_set_gyro_bias_reg complete ......\r\n"); + vTaskDelay(50); + if(!dmp_load_motion_driver_firmware()) + putStr("dmp_load_motion_driver_firmware complete ......\r\n"); + vTaskDelay(50); + if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation))) + putStr("dmp_set_orientation complete ......\r\n"); + vTaskDelay(50); + if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | + DMP_FEATURE_SEND_RAW_GYRO)) + //DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) + putStr("dmp_enable_feature complete ......\r\n"); + vTaskDelay(50); + if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) + putStr("dmp_set_fifo_rate complete ......\r\n"); + vTaskDelay(50); + if(!mpu_set_dmp_state(1)) + putStr("mpu_set_dmp_state complete ......\r\n"); + vTaskDelay(50); + } +} + +uint8_t read_dmp(float* Pitch,float* Roll,float* Yaw, float *gout) +{ + short gyro[3], accel[3], sensors; + float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; + unsigned long sensor_timestamp; + unsigned char more; + long quat[4]; + float q30 = (float)(1 << 30); + if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return 1; + + gout[0] = gyro[0] / 937.0f; + gout[1] = gyro[1] / 937.0f; + gout[2] = gyro[2] / 937.0f; + + if (sensors & INV_WXYZ_QUAT) + { + q0=quat[0] / q30; + q1=quat[1] / q30; + q2=quat[2] / q30; + q3=quat[3] / q30; + *Pitch = (float)asin(-2 * q1 * q3 + 2 * q0* q2)*57.3f; + //*Roll = (float)atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); + *Yaw = (float)atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3f; + return 0; + } + else + return 2; +} + +const int topTskPrio = 4; +const int topTskStkSize = 128; + +/** + * \brief Test hardware board without any peripheral + */ +int topTask(void) +{ + uint64_t start_us, end_us; + float pitch, roll, yaw, gout[3]; + int i = 0, rtn = 0; + + dbgInit(); + + dmp_init(); + + //vTaskDelay(8000); + + while(1){ + start_us = board_get_cur_us(); + rtn = read_dmp(&pitch, &roll, &yaw, gout); + end_us = board_get_cur_us() - start_us; + if(i > 1){ + putStr("rtn:%d\r\n", rtn); + putStr("X:%.3f, Y:%.3f, Z:%.3f\r\n", + gout[0], gout[1], gout[2]); + putStr("P:%.3f, R:%.3f, Y:%.3f, T:%d\r\n", + pitch, roll, yaw, end_us); + i = 0; + } + else i++; + vTaskDelay(5); + } + + return E_SYS; +} + +int topInit(void) +{ + BaseType_t rtn; + + // Create tasks + rtn=xTaskCreate(topTask, "topTask", topTskStkSize, NULL, topTskPrio, NULL); + configASSERT(rtn==pdPASS); +} + +int dmpTest(void) +{ + topInit(); + vTaskStartScheduler(); + return 0; +} + +/** @} */ \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmp_imu.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmp_imu.c new file mode 100644 index 00000000..424b5939 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmp_imu.c @@ -0,0 +1,112 @@ +/* embARC HAL */ +#include "embARC.h" +#include "embARC_debug.h" + +/* custom HAL */ +#include "imu.h" + +static DEV_IIC *emsk_imu_sensor; /* IMU6050 sensor object */ + +int board_get_ms(unsigned long *count) +{ + count = (unsigned long)OSP_GET_CUR_MS(); + return 0; +} + +/** + * \brief write mpu6050 register + * \param slv_addr slave address + * \param reg_addr register address + * \param length the length of + * \param *data data to be written + * \retval >=0 write success, return bytes written + * \retval !E_OK write failed + */ +int mpu6050_reg_write(unsigned char slave_addr, unsigned char reg_addr, + unsigned char length, unsigned char const *data) +{ + int32_t ercd = E_OK; + + emsk_imu_sensor = iic_get_dev(DW_IIC_0_ID); + + EMSK_IMU_SENSOR_CHECK_EXP_NORTN(emsk_imu_sensor != NULL); + + /* make sure set the imu sensor's slave address */ + emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_TAR_ADDR, CONV2VOID(slave_addr)); + /** write register address then read register value */ + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_RESTART)); + ercd = emsk_imu_sensor->iic_write(®_addr, 1); + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_STOP)); + ercd = emsk_imu_sensor->iic_write(data, length); + + if(ercd == length){ + ercd = E_OK; + } +error_exit: + return ercd; +} + +/** + * \brief read mpu6050 register + * \param slv_addr slave address + * \param reg_addr register address + * \param length the length of seq + * \param *data data + * \retval >=0 read success, return bytes read + * \retval !E_OK read failed + */ + int mpu6050_reg_read(unsigned char slave_addr, unsigned char reg_addr, + unsigned char length, unsigned char *data) +{ + int32_t ercd = E_OK; + + emsk_imu_sensor = iic_get_dev(DW_IIC_0_ID); + + EMSK_IMU_SENSOR_CHECK_EXP_NORTN(emsk_imu_sensor != NULL); + + /** make sure set the temp sensor's slave address */ + emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_TAR_ADDR, CONV2VOID(slave_addr)); + /* write register address then read register value */ + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_RESTART)); + + /* send data reading request to mpu6050 */ + ercd = emsk_imu_sensor->iic_write(®_addr, 1); + + emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_STOP)); + /* read len data from mpu6050 */ + ercd = emsk_imu_sensor->iic_read(data, length); + + if(ercd == length){ + ercd = E_OK; + } + +error_exit: + return ercd; +} + + +/** + * \brief imu sensor initialize + * \retval E_OK initialize success + * \retval !E_OK initialize failed + */ +int mpu6050_enable(void) +{ + int ercd = E_OK; + + emsk_imu_sensor = iic_get_dev(DW_IIC_0_ID); + + EMSK_IMU_SENSOR_CHECK_EXP_NORTN(emsk_imu_sensor != NULL); + + /* the device working as master */ + ercd = emsk_imu_sensor->iic_open(DEV_MASTER_MODE, IIC_SPEED_FAST); + if ((ercd == E_OK) || (ercd == E_OPNED)) { + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_TAR_ADDR, CONV2VOID(IMU_SENSOR_ADDR)); + } + +error_exit: + return ercd; +} + + + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmp_imu.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmp_imu.h new file mode 100644 index 00000000..96df4877 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmp_imu.h @@ -0,0 +1,26 @@ +#ifndef _IMU_H_ +#define _IMU_H_ + +/*!< using IIC 1 interface */ +#define IMU_VERTICAL_IIC_ID DW_IIC_0_ID +#define IMU_HORIZONTAL_IIC_ID DW_IIC_1_ID + +/** + * I2C address of PmodIMU2 can be selected via AD0: 0x68, 0x69 + * AD0 connect to GND is 0x68 + */ +/*!< MPU6050 address */ +#define MPU6050_A0_PIN 0 /*!< I2C Serial Bus Address Selection Pin */ +#define MPU6050_ADDRESS (0x68 + MPU6050_A0_PIN) + +/*!< imu sensor address */ +#define IMU_SENSOR_ADDR MPU6050_ADDRESS + +#define EMSK_IMU_SENSOR_CHECK_EXP_NORTN(EXPR) CHECK_EXP_NOERCD(EXPR, error_exit) + +extern int mpu6050_reg_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data); +extern int mpu6050_reg_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data); +extern int board_get_ms(unsigned long *count); +extern int mpu6050_enable(void); +#endif /* _IMU_H_ */ + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpmap.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpmap.h new file mode 100644 index 00000000..391ba144 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/dmpmap.h @@ -0,0 +1,264 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef DMPMAP_H +#define DMPMAP_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define DMP_PTAT 0 +#define DMP_XGYR 2 +#define DMP_YGYR 4 +#define DMP_ZGYR 6 +#define DMP_XACC 8 +#define DMP_YACC 10 +#define DMP_ZACC 12 +#define DMP_ADC1 14 +#define DMP_ADC2 16 +#define DMP_ADC3 18 +#define DMP_BIASUNC 20 +#define DMP_FIFORT 22 +#define DMP_INVGSFH 24 +#define DMP_INVGSFL 26 +#define DMP_1H 28 +#define DMP_1L 30 +#define DMP_BLPFSTCH 32 +#define DMP_BLPFSTCL 34 +#define DMP_BLPFSXH 36 +#define DMP_BLPFSXL 38 +#define DMP_BLPFSYH 40 +#define DMP_BLPFSYL 42 +#define DMP_BLPFSZH 44 +#define DMP_BLPFSZL 46 +#define DMP_BLPFMTC 48 +#define DMP_SMC 50 +#define DMP_BLPFMXH 52 +#define DMP_BLPFMXL 54 +#define DMP_BLPFMYH 56 +#define DMP_BLPFMYL 58 +#define DMP_BLPFMZH 60 +#define DMP_BLPFMZL 62 +#define DMP_BLPFC 64 +#define DMP_SMCTH 66 +#define DMP_0H2 68 +#define DMP_0L2 70 +#define DMP_BERR2H 72 +#define DMP_BERR2L 74 +#define DMP_BERR2NH 76 +#define DMP_SMCINC 78 +#define DMP_ANGVBXH 80 +#define DMP_ANGVBXL 82 +#define DMP_ANGVBYH 84 +#define DMP_ANGVBYL 86 +#define DMP_ANGVBZH 88 +#define DMP_ANGVBZL 90 +#define DMP_BERR1H 92 +#define DMP_BERR1L 94 +#define DMP_ATCH 96 +#define DMP_BIASUNCSF 98 +#define DMP_ACT2H 100 +#define DMP_ACT2L 102 +#define DMP_GSFH 104 +#define DMP_GSFL 106 +#define DMP_GH 108 +#define DMP_GL 110 +#define DMP_0_5H 112 +#define DMP_0_5L 114 +#define DMP_0_0H 116 +#define DMP_0_0L 118 +#define DMP_1_0H 120 +#define DMP_1_0L 122 +#define DMP_1_5H 124 +#define DMP_1_5L 126 +#define DMP_TMP1AH 128 +#define DMP_TMP1AL 130 +#define DMP_TMP2AH 132 +#define DMP_TMP2AL 134 +#define DMP_TMP3AH 136 +#define DMP_TMP3AL 138 +#define DMP_TMP4AH 140 +#define DMP_TMP4AL 142 +#define DMP_XACCW 144 +#define DMP_TMP5 146 +#define DMP_XACCB 148 +#define DMP_TMP8 150 +#define DMP_YACCB 152 +#define DMP_TMP9 154 +#define DMP_ZACCB 156 +#define DMP_TMP10 158 +#define DMP_DZH 160 +#define DMP_DZL 162 +#define DMP_XGCH 164 +#define DMP_XGCL 166 +#define DMP_YGCH 168 +#define DMP_YGCL 170 +#define DMP_ZGCH 172 +#define DMP_ZGCL 174 +#define DMP_YACCW 176 +#define DMP_TMP7 178 +#define DMP_AFB1H 180 +#define DMP_AFB1L 182 +#define DMP_AFB2H 184 +#define DMP_AFB2L 186 +#define DMP_MAGFBH 188 +#define DMP_MAGFBL 190 +#define DMP_QT1H 192 +#define DMP_QT1L 194 +#define DMP_QT2H 196 +#define DMP_QT2L 198 +#define DMP_QT3H 200 +#define DMP_QT3L 202 +#define DMP_QT4H 204 +#define DMP_QT4L 206 +#define DMP_CTRL1H 208 +#define DMP_CTRL1L 210 +#define DMP_CTRL2H 212 +#define DMP_CTRL2L 214 +#define DMP_CTRL3H 216 +#define DMP_CTRL3L 218 +#define DMP_CTRL4H 220 +#define DMP_CTRL4L 222 +#define DMP_CTRLS1 224 +#define DMP_CTRLSF1 226 +#define DMP_CTRLS2 228 +#define DMP_CTRLSF2 230 +#define DMP_CTRLS3 232 +#define DMP_CTRLSFNLL 234 +#define DMP_CTRLS4 236 +#define DMP_CTRLSFNL2 238 +#define DMP_CTRLSFNL 240 +#define DMP_TMP30 242 +#define DMP_CTRLSFJT 244 +#define DMP_TMP31 246 +#define DMP_TMP11 248 +#define DMP_CTRLSF2_2 250 +#define DMP_TMP12 252 +#define DMP_CTRLSF1_2 254 +#define DMP_PREVPTAT 256 +#define DMP_ACCZB 258 +#define DMP_ACCXB 264 +#define DMP_ACCYB 266 +#define DMP_1HB 272 +#define DMP_1LB 274 +#define DMP_0H 276 +#define DMP_0L 278 +#define DMP_ASR22H 280 +#define DMP_ASR22L 282 +#define DMP_ASR6H 284 +#define DMP_ASR6L 286 +#define DMP_TMP13 288 +#define DMP_TMP14 290 +#define DMP_FINTXH 292 +#define DMP_FINTXL 294 +#define DMP_FINTYH 296 +#define DMP_FINTYL 298 +#define DMP_FINTZH 300 +#define DMP_FINTZL 302 +#define DMP_TMP1BH 304 +#define DMP_TMP1BL 306 +#define DMP_TMP2BH 308 +#define DMP_TMP2BL 310 +#define DMP_TMP3BH 312 +#define DMP_TMP3BL 314 +#define DMP_TMP4BH 316 +#define DMP_TMP4BL 318 +#define DMP_STXG 320 +#define DMP_ZCTXG 322 +#define DMP_STYG 324 +#define DMP_ZCTYG 326 +#define DMP_STZG 328 +#define DMP_ZCTZG 330 +#define DMP_CTRLSFJT2 332 +#define DMP_CTRLSFJTCNT 334 +#define DMP_PVXG 336 +#define DMP_TMP15 338 +#define DMP_PVYG 340 +#define DMP_TMP16 342 +#define DMP_PVZG 344 +#define DMP_TMP17 346 +#define DMP_MNMFLAGH 352 +#define DMP_MNMFLAGL 354 +#define DMP_MNMTMH 356 +#define DMP_MNMTML 358 +#define DMP_MNMTMTHRH 360 +#define DMP_MNMTMTHRL 362 +#define DMP_MNMTHRH 364 +#define DMP_MNMTHRL 366 +#define DMP_ACCQD4H 368 +#define DMP_ACCQD4L 370 +#define DMP_ACCQD5H 372 +#define DMP_ACCQD5L 374 +#define DMP_ACCQD6H 376 +#define DMP_ACCQD6L 378 +#define DMP_ACCQD7H 380 +#define DMP_ACCQD7L 382 +#define DMP_ACCQD0H 384 +#define DMP_ACCQD0L 386 +#define DMP_ACCQD1H 388 +#define DMP_ACCQD1L 390 +#define DMP_ACCQD2H 392 +#define DMP_ACCQD2L 394 +#define DMP_ACCQD3H 396 +#define DMP_ACCQD3L 398 +#define DMP_XN2H 400 +#define DMP_XN2L 402 +#define DMP_XN1H 404 +#define DMP_XN1L 406 +#define DMP_YN2H 408 +#define DMP_YN2L 410 +#define DMP_YN1H 412 +#define DMP_YN1L 414 +#define DMP_YH 416 +#define DMP_YL 418 +#define DMP_B0H 420 +#define DMP_B0L 422 +#define DMP_A1H 424 +#define DMP_A1L 426 +#define DMP_A2H 428 +#define DMP_A2L 430 +#define DMP_SEM1 432 +#define DMP_FIFOCNT 434 +#define DMP_SH_TH_X 436 +#define DMP_PACKET 438 +#define DMP_SH_TH_Y 440 +#define DMP_FOOTER 442 +#define DMP_SH_TH_Z 444 +#define DMP_TEMP29 448 +#define DMP_TEMP30 450 +#define DMP_XACCB_PRE 452 +#define DMP_XACCB_PREL 454 +#define DMP_YACCB_PRE 456 +#define DMP_YACCB_PREL 458 +#define DMP_ZACCB_PRE 460 +#define DMP_ZACCB_PREL 462 +#define DMP_TMP22 464 +#define DMP_TAP_TIMER 466 +#define DMP_TAP_THX 468 +#define DMP_TAP_THY 472 +#define DMP_TAP_THZ 476 +#define DMP_TAPW_MIN 478 +#define DMP_TMP25 480 +#define DMP_TMP26 482 +#define DMP_TMP27 484 +#define DMP_TMP28 486 +#define DMP_ORIENT 488 +#define DMP_THRSH 490 +#define DMP_ENDIANH 492 +#define DMP_ENDIANL 494 +#define DMP_BLPFNMTCH 496 +#define DMP_BLPFNMTCL 498 +#define DMP_BLPFNMXH 500 +#define DMP_BLPFNMXL 502 +#define DMP_BLPFNMYH 504 +#define DMP_BLPFNMYL 506 +#define DMP_BLPFNMZH 508 +#define DMP_BLPFNMZL 510 +#ifdef __cplusplus +} +#endif +#endif // DMPMAP_H diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu.c new file mode 100644 index 00000000..5d2d6855 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu.c @@ -0,0 +1,3248 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @addtogroup DRIVERS Sensor Driver Layer + * @brief Hardware drivers to communicate with sensors via I2C. + * + * @{ + * @file inv_mpu.c + * @brief An I2C-based driver for Invensense gyroscopes. + * @details This driver currently works for the following devices: + * MPU6050 + * MPU6500 + * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) + * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) + */ +#include +#include +#include +#include +#include +#include "inv_mpu.h" + +/* embARC HAL */ +#include "embARC.h" +#include "embARC_debug.h" + +/* The following functions must be defined for this platform: + * i2c_write(unsigned char slave_addr, unsigned char reg_addr, + * unsigned char length, unsigned char const *data) + * i2c_read(unsigned char slave_addr, unsigned char reg_addr, + * unsigned char length, unsigned char *data) + * delay_ms(uint32_t ms, uint8_t os_compat) + * get_ms(unsigned long *count) + * fabsf(float x) + * min(int a, int b) + */ +#include "imu.h" + +#define i2c_write mpu6050_reg_write +#define i2c_read mpu6050_reg_read +#define delay_ms board_delay_ms +#define get_ms board_get_ms + +#define log_i(...) do {} while (0) +#define log_e(...) do {} while (0) +/* fabs is for doubles. fabsf is for floats. */ +#define labs abs +#define fabs fabsf +#define min(a,b) ((a> 3 & 0x03 */ + unsigned char gyro_fsr; + /* Matches accel_cfg >> 3 & 0x03 */ + unsigned char accel_fsr; + /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ + unsigned char sensors; + /* Matches config register. */ + unsigned char lpf; + unsigned char clk_src; + /* Sample rate, NOT rate divider. */ + unsigned short sample_rate; + /* Matches fifo_en register. */ + unsigned char fifo_enable; + /* Matches int enable register. */ + unsigned char int_enable; + /* 1 if devices on auxiliary I2C bus appear on the primary. */ + unsigned char bypass_mode; + /* 1 if half-sensitivity. + * NOTE: This doesn't belong here, but everything else in hw_s is const, + * and this allows us to save some precious RAM. + */ + unsigned char accel_half; + /* 1 if device in low-power accel-only mode. */ + unsigned char lp_accel_mode; + /* 1 if interrupts are only triggered on motion events. */ + unsigned char int_motion_only; + struct motion_int_cache_s cache; + /* 1 for active low interrupts. */ + unsigned char active_low_int; + /* 1 for latched interrupts. */ + unsigned char latched_int; + /* 1 if DMP is enabled. */ + unsigned char dmp_on; + /* Ensures that DMP will only be loaded once. */ + unsigned char dmp_loaded; + /* Sampling rate used when DMP is enabled. */ + unsigned short dmp_sample_rate; +#ifdef AK89xx_SECONDARY + /* Compass sample rate. */ + unsigned short compass_sample_rate; + unsigned char compass_addr; + short mag_sens_adj[3]; +#endif +}; + +/* Information for self-test. */ +struct test_s { + unsigned long gyro_sens; + unsigned long accel_sens; + unsigned char reg_rate_div; + unsigned char reg_lpf; + unsigned char reg_gyro_fsr; + unsigned char reg_accel_fsr; + unsigned short wait_ms; + unsigned char packet_thresh; + float min_dps; + float max_dps; + float max_gyro_var; + float min_g; + float max_g; + float max_accel_var; +#ifdef MPU6500 + float max_g_offset; + unsigned short sample_wait_ms; +#endif +}; + +/* Gyro driver state variables. */ +struct gyro_state_s { + const struct gyro_reg_s *reg; + const struct hw_s *hw; + struct chip_cfg_s chip_cfg; + const struct test_s *test; +}; + +/* Filter configurations. */ +enum lpf_e { + INV_FILTER_256HZ_NOLPF2 = 0, + INV_FILTER_188HZ, + INV_FILTER_98HZ, + INV_FILTER_42HZ, + INV_FILTER_20HZ, + INV_FILTER_10HZ, + INV_FILTER_5HZ, + INV_FILTER_2100HZ_NOLPF, + NUM_FILTER +}; + +/* Full scale ranges. */ +enum gyro_fsr_e { + INV_FSR_250DPS = 0, + INV_FSR_500DPS, + INV_FSR_1000DPS, + INV_FSR_2000DPS, + NUM_GYRO_FSR +}; + +/* Full scale ranges. */ +enum accel_fsr_e { + INV_FSR_2G = 0, + INV_FSR_4G, + INV_FSR_8G, + INV_FSR_16G, + NUM_ACCEL_FSR +}; + +/* Clock sources. */ +enum clock_sel_e { + INV_CLK_INTERNAL = 0, + INV_CLK_PLL, + NUM_CLK +}; + +/* Low-power accel wakeup rates. */ +enum lp_accel_rate_e { +#if defined MPU6050 + INV_LPA_1_25HZ, + INV_LPA_5HZ, + INV_LPA_20HZ, + INV_LPA_40HZ +#elif defined MPU6500 + INV_LPA_0_3125HZ, + INV_LPA_0_625HZ, + INV_LPA_1_25HZ, + INV_LPA_2_5HZ, + INV_LPA_5HZ, + INV_LPA_10HZ, + INV_LPA_20HZ, + INV_LPA_40HZ, + INV_LPA_80HZ, + INV_LPA_160HZ, + INV_LPA_320HZ, + INV_LPA_640HZ +#endif +}; + +#define BIT_I2C_MST_VDDIO (0x80) +#define BIT_FIFO_EN (0x40) +#define BIT_DMP_EN (0x80) +#define BIT_FIFO_RST (0x04) +#define BIT_DMP_RST (0x08) +#define BIT_FIFO_OVERFLOW (0x10) +#define BIT_DATA_RDY_EN (0x01) +#define BIT_DMP_INT_EN (0x02) +#define BIT_MOT_INT_EN (0x40) +#define BITS_FSR (0x18) +#define BITS_LPF (0x07) +#define BITS_HPF (0x07) +#define BITS_CLK (0x07) +#define BIT_FIFO_SIZE_1024 (0x40) +#define BIT_FIFO_SIZE_2048 (0x80) +#define BIT_FIFO_SIZE_4096 (0xC0) +#define BIT_RESET (0x80) +#define BIT_SLEEP (0x40) +#define BIT_S0_DELAY_EN (0x01) +#define BIT_S2_DELAY_EN (0x04) +#define BITS_SLAVE_LENGTH (0x0F) +#define BIT_SLAVE_BYTE_SW (0x40) +#define BIT_SLAVE_GROUP (0x10) +#define BIT_SLAVE_EN (0x80) +#define BIT_I2C_READ (0x80) +#define BITS_I2C_MASTER_DLY (0x1F) +#define BIT_AUX_IF_EN (0x20) +#define BIT_ACTL (0x80) +#define BIT_LATCH_EN (0x20) +#define BIT_ANY_RD_CLR (0x10) +#define BIT_BYPASS_EN (0x02) +#define BITS_WOM_EN (0xC0) +#define BIT_LPA_CYCLE (0x20) +#define BIT_STBY_XA (0x20) +#define BIT_STBY_YA (0x10) +#define BIT_STBY_ZA (0x08) +#define BIT_STBY_XG (0x04) +#define BIT_STBY_YG (0x02) +#define BIT_STBY_ZG (0x01) +#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) +#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) + +#ifdef MPU6500 +#define BIT_ACCL_FC_B (0x08) +#endif + +#if defined AK8975_SECONDARY +#define SUPPORTS_AK89xx_HIGH_SENS (0x00) +#define AK89xx_FSR (9830) +#elif defined AK8963_SECONDARY +#define SUPPORTS_AK89xx_HIGH_SENS (0x10) +#define AK89xx_FSR (4915) +#endif + +#ifdef AK89xx_SECONDARY +#define AKM_REG_WHOAMI (0x00) + +#define AKM_REG_ST1 (0x02) +#define AKM_REG_HXL (0x03) +#define AKM_REG_ST2 (0x09) + +#define AKM_REG_CNTL (0x0A) +#define AKM_REG_ASTC (0x0C) +#define AKM_REG_ASAX (0x10) +#define AKM_REG_ASAY (0x11) +#define AKM_REG_ASAZ (0x12) + +#define AKM_DATA_READY (0x01) +#define AKM_DATA_OVERRUN (0x02) +#define AKM_OVERFLOW (0x80) +#define AKM_DATA_ERROR (0x40) + +#define AKM_BIT_SELF_TEST (0x40) + +#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS) +#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS) +#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS) +#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS) + +#define AKM_WHOAMI (0x48) +#endif + +#if defined MPU6050 +const struct gyro_reg_s reg = { + .who_am_i = 0x75, + .rate_div = 0x19, + .lpf = 0x1A, + .prod_id = 0x0C, + .user_ctrl = 0x6A, + .fifo_en = 0x23, + .gyro_cfg = 0x1B, + .accel_cfg = 0x1C, + .motion_thr = 0x1F, + .motion_dur = 0x20, + .fifo_count_h = 0x72, + .fifo_r_w = 0x74, + .raw_gyro = 0x43, + .raw_accel = 0x3B, + .temp = 0x41, + .int_enable = 0x38, + .dmp_int_status = 0x39, + .int_status = 0x3A, + .pwr_mgmt_1 = 0x6B, + .pwr_mgmt_2 = 0x6C, + .int_pin_cfg = 0x37, + .mem_r_w = 0x6F, + .accel_offs = 0x06, + .i2c_mst = 0x24, + .bank_sel = 0x6D, + .mem_start_addr = 0x6E, + .prgm_start_h = 0x70 +#ifdef AK89xx_SECONDARY + ,.raw_compass = 0x49, + .yg_offs_tc = 0x01, + .s0_addr = 0x25, + .s0_reg = 0x26, + .s0_ctrl = 0x27, + .s1_addr = 0x28, + .s1_reg = 0x29, + .s1_ctrl = 0x2A, + .s4_ctrl = 0x34, + .s0_do = 0x63, + .s1_do = 0x64, + .i2c_delay_ctrl = 0x67 +#endif +}; +const struct hw_s hw = { + .addr = 0x68, + .max_fifo = 1024, + .num_reg = 118, + .temp_sens = 340, + .temp_offset = -521, + .bank_size = 256 +#if defined AK89xx_SECONDARY + ,.compass_fsr = AK89xx_FSR +#endif +}; + +const struct test_s test = { + .gyro_sens = 32768/250, + .accel_sens = 32768/16, + .reg_rate_div = 0, /* 1kHz. */ + .reg_lpf = 1, /* 188Hz. */ + .reg_gyro_fsr = 0, /* 250dps. */ + .reg_accel_fsr = 0x18, /* 16g. */ + .wait_ms = 50, + .packet_thresh = 5, /* 5% */ + .min_dps = 10.f, + .max_dps = 105.f, + .max_gyro_var = 0.14f, + .min_g = 0.3f, + .max_g = 0.95f, + .max_accel_var = 0.14f +}; + +static struct gyro_state_s st = { + .reg = ®, + .hw = &hw, + .test = &test +}; +#elif defined MPU6500 +const struct gyro_reg_s reg = { + .who_am_i = 0x75, + .rate_div = 0x19, + .lpf = 0x1A, + .prod_id = 0x0C, + .user_ctrl = 0x6A, + .fifo_en = 0x23, + .gyro_cfg = 0x1B, + .accel_cfg = 0x1C, + .accel_cfg2 = 0x1D, + .lp_accel_odr = 0x1E, + .motion_thr = 0x1F, + .motion_dur = 0x20, + .fifo_count_h = 0x72, + .fifo_r_w = 0x74, + .raw_gyro = 0x43, + .raw_accel = 0x3B, + .temp = 0x41, + .int_enable = 0x38, + .dmp_int_status = 0x39, + .int_status = 0x3A, + .accel_intel = 0x69, + .pwr_mgmt_1 = 0x6B, + .pwr_mgmt_2 = 0x6C, + .int_pin_cfg = 0x37, + .mem_r_w = 0x6F, + .accel_offs = 0x77, + .i2c_mst = 0x24, + .bank_sel = 0x6D, + .mem_start_addr = 0x6E, + .prgm_start_h = 0x70 +#ifdef AK89xx_SECONDARY + ,.raw_compass = 0x49, + .s0_addr = 0x25, + .s0_reg = 0x26, + .s0_ctrl = 0x27, + .s1_addr = 0x28, + .s1_reg = 0x29, + .s1_ctrl = 0x2A, + .s4_ctrl = 0x34, + .s0_do = 0x63, + .s1_do = 0x64, + .i2c_delay_ctrl = 0x67 +#endif +}; +const struct hw_s hw = { + .addr = 0x68, + .max_fifo = 1024, + .num_reg = 128, + .temp_sens = 321, + .temp_offset = 0, + .bank_size = 256 +#if defined AK89xx_SECONDARY + ,.compass_fsr = AK89xx_FSR +#endif +}; + +const struct test_s test = { + .gyro_sens = 32768/250, + .accel_sens = 32768/2, //FSR = +-2G = 16384 LSB/G + .reg_rate_div = 0, /* 1kHz. */ + .reg_lpf = 2, /* 92Hz low pass filter*/ + .reg_gyro_fsr = 0, /* 250dps. */ + .reg_accel_fsr = 0x0, /* Accel FSR setting = 2g. */ + .wait_ms = 200, //200ms stabilization time + .packet_thresh = 200, /* 200 samples */ + .min_dps = 20.f, //20 dps for Gyro Criteria C + .max_dps = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B + .max_gyro_var = .5f, //Must exceed +50% variation for Gyro Criteria A + .min_g = .225f, //Accel must exceed Min 225 mg for Criteria B + .max_g = .675f, //Accel cannot exceed Max 675 mg for Criteria B + .max_accel_var = .5f, //Accel must be within 50% variation for Criteria A + .max_g_offset = .5f, //500 mg for Accel Criteria C + .sample_wait_ms = 10 //10ms sample time wait +}; + +static struct gyro_state_s st = { + .reg = ®, + .hw = &hw, + .test = &test +}; +#endif + +#define MAX_PACKET_LENGTH (12) +#ifdef MPU6500 +#define HWST_MAX_PACKET_LENGTH (512) +#endif + +#ifdef AK89xx_SECONDARY +static int setup_compass(void); +#define MAX_COMPASS_SAMPLE_RATE (100) +#endif + +/** + * @brief Enable/disable data ready interrupt. + * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready + * interrupt is used. + * @param[in] enable 1 to enable interrupt. + * @return 0 if successful. + */ +static int set_int_enable(unsigned char enable) +{ + unsigned char tmp; + + if (st.chip_cfg.dmp_on) { + if (enable) + tmp = BIT_DMP_INT_EN; + else + tmp = 0x00; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) + return -1; + st.chip_cfg.int_enable = tmp; + } else { + if (!st.chip_cfg.sensors) + return -1; + if (enable && st.chip_cfg.int_enable) + return 0; + if (enable) + tmp = BIT_DATA_RDY_EN; + else + tmp = 0x00; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) + return -1; + st.chip_cfg.int_enable = tmp; + } + return 0; +} + +/** + * @brief Register dump for testing. + * @return 0 if successful. + */ +int mpu_reg_dump(void) +{ + unsigned char ii; + unsigned char data; + + for (ii = 0; ii < st.hw->num_reg; ii++) { + if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w) + continue; + if (i2c_read(st.hw->addr, ii, 1, &data)) + return -1; + log_i("%#5x: %#5x\r\n", ii, data); + } + return 0; +} + +/** + * @brief Read from a single register. + * NOTE: The memory and FIFO read/write registers cannot be accessed. + * @param[in] reg Register address. + * @param[out] data Register data. + * @return 0 if successful. + */ +int mpu_read_reg(unsigned char reg, unsigned char *data) +{ + if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w) + return -1; + if (reg >= st.hw->num_reg) + return -1; + return i2c_read(st.hw->addr, reg, 1, data); +} + +/** + * @brief Initialize hardware. + * Initial configuration:\n + * Gyro FSR: +/- 2000DPS\n + * Accel FSR +/- 2G\n + * DLPF: 42Hz\n + * FIFO rate: 50Hz\n + * Clock source: Gyro PLL\n + * FIFO: Disabled.\n + * Data ready interrupt: Disabled, active low, unlatched. + * @param[in] int_param Platform-specific parameters to interrupt API. + * @return 0 if successful. + */ +//int mpu_init(struct int_param_s *int_param) +int mpu_init() +{ + unsigned char data[6]; + + /* Reset device. */ + data[0] = BIT_RESET; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + delay_ms(100, 1); + + /* Wake up chip. */ + data[0] = 0x00; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + + st.chip_cfg.accel_half = 0; + +#ifdef MPU6500 + /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the + * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO. + */ + data[0] = BIT_FIFO_SIZE_1024; + if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) + return -1; +#endif + + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.bypass_mode = 0xFF; +#ifdef AK89xx_SECONDARY + st.chip_cfg.compass_sample_rate = 0xFFFF; +#endif + /* mpu_set_sensors always preserves this setting. */ + st.chip_cfg.clk_src = INV_CLK_PLL; + /* Handled in next call to mpu_set_bypass. */ + st.chip_cfg.active_low_int = 1; + st.chip_cfg.latched_int = 0; + st.chip_cfg.int_motion_only = 0; + st.chip_cfg.lp_accel_mode = 0; + memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache)); + st.chip_cfg.dmp_on = 0; + st.chip_cfg.dmp_loaded = 0; + st.chip_cfg.dmp_sample_rate = 0; + + if (mpu_set_gyro_fsr(2000)) + return -1; + if (mpu_set_accel_fsr(2)) + return -1; + if (mpu_set_lpf(42)) + return -1; + if (mpu_set_sample_rate(50)) + return -1; + if (mpu_configure_fifo(0)) + return -1; + + //if (int_param) + //reg_int_cb(int_param); + +#ifdef AK89xx_SECONDARY + setup_compass(); + if (mpu_set_compass_sample_rate(10)) + return -1; +#else + /* Already disabled by setup_compass. */ + if (mpu_set_bypass(0)) + return -1; +#endif + + mpu_set_sensors(0); + return 0; +} + +/** + * @brief Enter low-power accel-only mode. + * In low-power accel mode, the chip goes to sleep and only wakes up to sample + * the accelerometer at one of the following frequencies: + * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz + * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz + * \n If the requested rate is not one listed above, the device will be set to + * the next highest rate. Requesting a rate above the maximum supported + * frequency will result in an error. + * \n To select a fractional wake-up frequency, round down the value passed to + * @e rate. + * @param[in] rate Minimum sampling rate, or zero to disable LP + * accel mode. + * @return 0 if successful. + */ +int mpu_lp_accel_mode(unsigned short rate) +{ + unsigned char tmp[2]; + + if (rate > 40) + return -1; + + if (!rate) { + mpu_set_int_latched(0); + tmp[0] = 0; + tmp[1] = BIT_STBY_XYZG; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) + return -1; + st.chip_cfg.lp_accel_mode = 0; + return 0; + } + /* For LP accel, we automatically configure the hardware to produce latched + * interrupts. In LP accel mode, the hardware cycles into sleep mode before + * it gets a chance to deassert the interrupt pin; therefore, we shift this + * responsibility over to the MCU. + * + * Any register read will clear the interrupt. + */ + mpu_set_int_latched(1); +#if defined MPU6050 + tmp[0] = BIT_LPA_CYCLE; + if (rate == 1) { + tmp[1] = INV_LPA_1_25HZ; + mpu_set_lpf(5); + } else if (rate <= 5) { + tmp[1] = INV_LPA_5HZ; + mpu_set_lpf(5); + } else if (rate <= 20) { + tmp[1] = INV_LPA_20HZ; + mpu_set_lpf(10); + } else { + tmp[1] = INV_LPA_40HZ; + mpu_set_lpf(20); + } + tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) + return -1; +#elif defined MPU6500 + /* Set wake frequency. */ + if (rate == 1) + tmp[0] = INV_LPA_1_25HZ; + else if (rate == 2) + tmp[0] = INV_LPA_2_5HZ; + else if (rate <= 5) + tmp[0] = INV_LPA_5HZ; + else if (rate <= 10) + tmp[0] = INV_LPA_10HZ; + else if (rate <= 20) + tmp[0] = INV_LPA_20HZ; + else if (rate <= 40) + tmp[0] = INV_LPA_40HZ; + else if (rate <= 80) + tmp[0] = INV_LPA_80HZ; + else if (rate <= 160) + tmp[0] = INV_LPA_160HZ; + else if (rate <= 320) + tmp[0] = INV_LPA_320HZ; + else + tmp[0] = INV_LPA_640HZ; + if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp)) + return -1; + /* Enable LP ACCEL mode, ACCEL_FCHOICE_B=1*/ + if (i2c_read(st.hw->addr, st.reg->accel_cfg2, 1, tmp)) + return -1; + + tmp[0] = BIT_ACCL_FC_B | tmp[0]; + if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, tmp)) + return -1; + tmp[0] = BIT_LPA_CYCLE; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp)) + return -1; +#endif + st.chip_cfg.sensors = INV_XYZ_ACCEL; + st.chip_cfg.clk_src = 0; + st.chip_cfg.lp_accel_mode = 1; + mpu_configure_fifo(0); + + return 0; +} + +/** + * @brief Read raw gyro data directly from the registers. + * @param[out] data Raw data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_gyro_reg(short *data, unsigned long *timestamp) +{ + unsigned char tmp[6]; + + if (!(st.chip_cfg.sensors & INV_XYZ_GYRO)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp)) + return -1; + data[0] = (tmp[0] << 8) | tmp[1]; + data[1] = (tmp[2] << 8) | tmp[3]; + data[2] = (tmp[4] << 8) | tmp[5]; + if (timestamp) + get_ms(timestamp); + return 0; +} + +/** + * @brief Read raw accel data directly from the registers. + * @param[out] data Raw data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_accel_reg(short *data, unsigned long *timestamp) +{ + unsigned char tmp[6]; + + if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp)) + return -1; + data[0] = (tmp[0] << 8) | tmp[1]; + data[1] = (tmp[2] << 8) | tmp[3]; + data[2] = (tmp[4] << 8) | tmp[5]; + if (timestamp) + get_ms(timestamp); + return 0; +} + +/** + * @brief Read temperature data directly from the registers. + * @param[out] data Data in q16 format. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_temperature(long *data, unsigned long *timestamp) +{ + unsigned char tmp[2]; + short raw; + + if (!(st.chip_cfg.sensors)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp)) + return -1; + raw = (tmp[0] << 8) | tmp[1]; + if (timestamp) + get_ms(timestamp); + + data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L); + return 0; +} + +/** + * @brief Read biases to the accel bias 6500 registers. + * This function reads from the MPU6500 accel offset cancellations registers. + * The format are G in +-8G format. The register is initialized with OTP + * factory trim values. + * @param[in] accel_bias returned structure with the accel bias + * @return 0 if successful. + */ +int mpu_read_6500_accel_bias(long *accel_bias) { + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x77, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x7A, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x7D, 2, &data[4])) + return -1; + accel_bias[0] = ((long)data[0]<<8) | data[1]; + accel_bias[1] = ((long)data[2]<<8) | data[3]; + accel_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; +} + +/** + * @brief Read biases to the accel bias 6050 registers. + * This function reads from the MPU6050 accel offset cancellations registers. + * The format are G in +-8G format. The register is initialized with OTP + * factory trim values. + * @param[in] accel_bias returned structure with the accel bias + * @return 0 if successful. + */ +int mpu_read_6050_accel_bias(long *accel_bias) { + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x06, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x08, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x0A, 2, &data[4])) + return -1; + accel_bias[0] = ((long)data[0]<<8) | data[1]; + accel_bias[1] = ((long)data[2]<<8) | data[3]; + accel_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; +} + +int mpu_read_6500_gyro_bias(long *gyro_bias) { + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x13, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x15, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x17, 2, &data[4])) + return -1; + gyro_bias[0] = ((long)data[0]<<8) | data[1]; + gyro_bias[1] = ((long)data[2]<<8) | data[3]; + gyro_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; +} + +/** + * @brief Push biases to the gyro bias 6500/6050 registers. + * This function expects biases relative to the current sensor output, and + * these biases will be added to the factory-supplied values. Bias inputs are LSB + * in +-1000dps format. + * @param[in] gyro_bias New biases. + * @return 0 if successful. + */ +int mpu_set_gyro_bias_reg(long *gyro_bias) +{ + unsigned char data[6] = {0, 0, 0, 0, 0, 0}; + int i=0; + for(i=0;i<3;i++) { + gyro_bias[i]= (-gyro_bias[i]); + } + data[0] = (gyro_bias[0] >> 8) & 0xff; + data[1] = (gyro_bias[0]) & 0xff; + data[2] = (gyro_bias[1] >> 8) & 0xff; + data[3] = (gyro_bias[1]) & 0xff; + data[4] = (gyro_bias[2] >> 8) & 0xff; + data[5] = (gyro_bias[2]) & 0xff; + if (i2c_write(st.hw->addr, 0x13, 2, &data[0])) + return -1; + if (i2c_write(st.hw->addr, 0x15, 2, &data[2])) + return -1; + if (i2c_write(st.hw->addr, 0x17, 2, &data[4])) + return -1; + return 0; +} + +/** + * @brief Push biases to the accel bias 6050 registers. + * This function expects biases relative to the current sensor output, and + * these biases will be added to the factory-supplied values. Bias inputs are LSB + * in +-16G format. + * @param[in] accel_bias New biases. + * @return 0 if successful. + */ +int mpu_set_accel_bias_6050_reg(const long *accel_bias) { + unsigned char data[6] = {0, 0, 0, 0, 0, 0}; + long accel_reg_bias[3] = {0, 0, 0}; + + if(mpu_read_6050_accel_bias(accel_reg_bias)) + return -1; + + accel_reg_bias[0] -= (accel_bias[0] & ~1); + accel_reg_bias[1] -= (accel_bias[1] & ~1); + accel_reg_bias[2] -= (accel_bias[2] & ~1); + + data[0] = (accel_reg_bias[0] >> 8) & 0xff; + data[1] = (accel_reg_bias[0]) & 0xff; + data[2] = (accel_reg_bias[1] >> 8) & 0xff; + data[3] = (accel_reg_bias[1]) & 0xff; + data[4] = (accel_reg_bias[2] >> 8) & 0xff; + data[5] = (accel_reg_bias[2]) & 0xff; + + if (i2c_write(st.hw->addr, 0x06, 2, &data[0])) + return -1; + if (i2c_write(st.hw->addr, 0x08, 2, &data[2])) + return -1; + if (i2c_write(st.hw->addr, 0x0A, 2, &data[4])) + return -1; + + return 0; +} + + + +/** + * @brief Push biases to the accel bias 6500 registers. + * This function expects biases relative to the current sensor output, and + * these biases will be added to the factory-supplied values. Bias inputs are LSB + * in +-16G format. + * @param[in] accel_bias New biases. + * @return 0 if successful. + */ +int mpu_set_accel_bias_6500_reg(const long *accel_bias) { + unsigned char data[6] = {0, 0, 0, 0, 0, 0}; + long accel_reg_bias[3] = {0, 0, 0}; + + if(mpu_read_6500_accel_bias(accel_reg_bias)) + return -1; + + // Preserve bit 0 of factory value (for temperature compensation) + accel_reg_bias[0] -= (accel_bias[0] & ~1); + accel_reg_bias[1] -= (accel_bias[1] & ~1); + accel_reg_bias[2] -= (accel_bias[2] & ~1); + + data[0] = (accel_reg_bias[0] >> 8) & 0xff; + data[1] = (accel_reg_bias[0]) & 0xff; + data[2] = (accel_reg_bias[1] >> 8) & 0xff; + data[3] = (accel_reg_bias[1]) & 0xff; + data[4] = (accel_reg_bias[2] >> 8) & 0xff; + data[5] = (accel_reg_bias[2]) & 0xff; + + if (i2c_write(st.hw->addr, 0x77, 2, &data[0])) + return -1; + if (i2c_write(st.hw->addr, 0x7A, 2, &data[2])) + return -1; + if (i2c_write(st.hw->addr, 0x7D, 2, &data[4])) + return -1; + + return 0; +} + + +/** + * @brief Reset FIFO read/write pointers. + * @return 0 if successful. + */ +int mpu_reset_fifo(void) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + data = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + + if (st.chip_cfg.dmp_on) { + data = BIT_FIFO_RST | BIT_DMP_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + delay_ms(50, 1); + data = BIT_DMP_EN | BIT_FIFO_EN; + if (st.chip_cfg.sensors & INV_XYZ_COMPASS) + data |= BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + if (st.chip_cfg.int_enable) + data = BIT_DMP_INT_EN; + else + data = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) + return -1; + data = 0; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) + return -1; + } else { + data = BIT_FIFO_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS)) + data = BIT_FIFO_EN; + else + data = BIT_FIFO_EN | BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) + return -1; + delay_ms(50, 1); + if (st.chip_cfg.int_enable) + data = BIT_DATA_RDY_EN; + else + data = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable)) + return -1; + } + return 0; +} + +/** + * @brief Get the gyro full-scale range. + * @param[out] fsr Current full-scale range. + * @return 0 if successful. + */ +int mpu_get_gyro_fsr(unsigned short *fsr) +{ + switch (st.chip_cfg.gyro_fsr) { + case INV_FSR_250DPS: + fsr[0] = 250; + break; + case INV_FSR_500DPS: + fsr[0] = 500; + break; + case INV_FSR_1000DPS: + fsr[0] = 1000; + break; + case INV_FSR_2000DPS: + fsr[0] = 2000; + break; + default: + fsr[0] = 0; + break; + } + return 0; +} + +/** + * @brief Set the gyro full-scale range. + * @param[in] fsr Desired full-scale range. + * @return 0 if successful. + */ +int mpu_set_gyro_fsr(unsigned short fsr) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + switch (fsr) { + case 250: + data = INV_FSR_250DPS << 3; + break; + case 500: + data = INV_FSR_500DPS << 3; + break; + case 1000: + data = INV_FSR_1000DPS << 3; + break; + case 2000: + data = INV_FSR_2000DPS << 3; + break; + default: + return -1; + } + + if (st.chip_cfg.gyro_fsr == (data >> 3)) + return 0; + if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data)) + return -1; + st.chip_cfg.gyro_fsr = data >> 3; + return 0; +} + +/** + * @brief Get the accel full-scale range. + * @param[out] fsr Current full-scale range. + * @return 0 if successful. + */ +int mpu_get_accel_fsr(unsigned char *fsr) +{ + switch (st.chip_cfg.accel_fsr) { + case INV_FSR_2G: + fsr[0] = 2; + break; + case INV_FSR_4G: + fsr[0] = 4; + break; + case INV_FSR_8G: + fsr[0] = 8; + break; + case INV_FSR_16G: + fsr[0] = 16; + break; + default: + return -1; + } + if (st.chip_cfg.accel_half) + fsr[0] <<= 1; + return 0; +} + +/** + * @brief Set the accel full-scale range. + * @param[in] fsr Desired full-scale range. + * @return 0 if successful. + */ +int mpu_set_accel_fsr(unsigned char fsr) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + switch (fsr) { + case 2: + data = INV_FSR_2G << 3; + break; + case 4: + data = INV_FSR_4G << 3; + break; + case 8: + data = INV_FSR_8G << 3; + break; + case 16: + data = INV_FSR_16G << 3; + break; + default: + return -1; + } + + if (st.chip_cfg.accel_fsr == (data >> 3)) + return 0; + if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data)) + return -1; + st.chip_cfg.accel_fsr = data >> 3; + return 0; +} + +/** + * @brief Get the current DLPF setting. + * @param[out] lpf Current LPF setting. + * 0 if successful. + */ +int mpu_get_lpf(unsigned short *lpf) +{ + switch (st.chip_cfg.lpf) { + case INV_FILTER_188HZ: + lpf[0] = 188; + break; + case INV_FILTER_98HZ: + lpf[0] = 98; + break; + case INV_FILTER_42HZ: + lpf[0] = 42; + break; + case INV_FILTER_20HZ: + lpf[0] = 20; + break; + case INV_FILTER_10HZ: + lpf[0] = 10; + break; + case INV_FILTER_5HZ: + lpf[0] = 5; + break; + case INV_FILTER_256HZ_NOLPF2: + case INV_FILTER_2100HZ_NOLPF: + default: + lpf[0] = 0; + break; + } + return 0; +} + +/** + * @brief Set digital low pass filter. + * The following LPF settings are supported: 188, 98, 42, 20, 10, 5. + * @param[in] lpf Desired LPF setting. + * @return 0 if successful. + */ +int mpu_set_lpf(unsigned short lpf) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + if (lpf >= 188) + data = INV_FILTER_188HZ; + else if (lpf >= 98) + data = INV_FILTER_98HZ; + else if (lpf >= 42) + data = INV_FILTER_42HZ; + else if (lpf >= 20) + data = INV_FILTER_20HZ; + else if (lpf >= 10) + data = INV_FILTER_10HZ; + else + data = INV_FILTER_5HZ; + + if (st.chip_cfg.lpf == data) + return 0; + if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data)) + return -1; +#ifdef MPU6500 + data = BIT_FIFO_SIZE_1024 | data; + + if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, &data)) + return -1; +#endif + st.chip_cfg.lpf = data; + return 0; +} + +/** + * @brief Get sampling rate. + * @param[out] rate Current sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_get_sample_rate(unsigned short *rate) +{ + if (st.chip_cfg.dmp_on) + return -1; + else + rate[0] = st.chip_cfg.sample_rate; + return 0; +} + +/** + * @brief Set sampling rate. + * Sampling rate must be between 4Hz and 1kHz. + * @param[in] rate Desired sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_set_sample_rate(unsigned short rate) +{ + unsigned char data; + + if (!(st.chip_cfg.sensors)) + return -1; + + if (st.chip_cfg.dmp_on) + return -1; + else { + if (st.chip_cfg.lp_accel_mode) { + if (rate && (rate <= 40)) { + /* Just stay in low-power accel mode. */ + mpu_lp_accel_mode(rate); + return 0; + } + /* Requested rate exceeds the allowed frequencies in LP accel mode, + * switch back to full-power mode. + */ + mpu_lp_accel_mode(0); + } + if (rate < 4) + rate = 4; + else if (rate > 1000) + rate = 1000; + + data = 1000 / rate - 1; + if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data)) + return -1; + + st.chip_cfg.sample_rate = 1000 / (1 + data); + +#ifdef AK89xx_SECONDARY + mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); +#endif + + /* Automatically set LPF to 1/2 sampling rate. */ + mpu_set_lpf(st.chip_cfg.sample_rate >> 1); + return 0; + } +} + +/** + * @brief Get compass sampling rate. + * @param[out] rate Current compass sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_get_compass_sample_rate(unsigned short *rate) +{ +#ifdef AK89xx_SECONDARY + rate[0] = st.chip_cfg.compass_sample_rate; + return 0; +#else + rate[0] = 0; + return -1; +#endif +} + +/** + * @brief Set compass sampling rate. + * The compass on the auxiliary I2C bus is read by the MPU hardware at a + * maximum of 100Hz. The actual rate can be set to a fraction of the gyro + * sampling rate. + * + * \n WARNING: The new rate may be different than what was requested. Call + * mpu_get_compass_sample_rate to check the actual setting. + * @param[in] rate Desired compass sampling rate (Hz). + * @return 0 if successful. + */ +int mpu_set_compass_sample_rate(unsigned short rate) +{ +#ifdef AK89xx_SECONDARY + unsigned char div; + if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE) + return -1; + + div = st.chip_cfg.sample_rate / rate - 1; + if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div)) + return -1; + st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1); + return 0; +#else + return -1; +#endif +} + +/** + * @brief Get gyro sensitivity scale factor. + * @param[out] sens Conversion from hardware units to dps. + * @return 0 if successful. + */ +int mpu_get_gyro_sens(float *sens) +{ + switch (st.chip_cfg.gyro_fsr) { + case INV_FSR_250DPS: + sens[0] = 131.f; + break; + case INV_FSR_500DPS: + sens[0] = 65.5f; + break; + case INV_FSR_1000DPS: + sens[0] = 32.8f; + break; + case INV_FSR_2000DPS: + sens[0] = 16.4f; + break; + default: + return -1; + } + return 0; +} + +/** + * @brief Get accel sensitivity scale factor. + * @param[out] sens Conversion from hardware units to g's. + * @return 0 if successful. + */ +int mpu_get_accel_sens(unsigned short *sens) +{ + switch (st.chip_cfg.accel_fsr) { + case INV_FSR_2G: + sens[0] = 16384; + break; + case INV_FSR_4G: + sens[0] = 8192; + break; + case INV_FSR_8G: + sens[0] = 4096; + break; + case INV_FSR_16G: + sens[0] = 2048; + break; + default: + return -1; + } + if (st.chip_cfg.accel_half) + sens[0] >>= 1; + return 0; +} + +/** + * @brief Get current FIFO configuration. + * @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * @param[out] sensors Mask of sensors in FIFO. + * @return 0 if successful. + */ +int mpu_get_fifo_config(unsigned char *sensors) +{ + sensors[0] = st.chip_cfg.fifo_enable; + return 0; +} + +/** + * @brief Select which sensors are pushed to FIFO. + * @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * @param[in] sensors Mask of sensors to push to FIFO. + * @return 0 if successful. + */ +int mpu_configure_fifo(unsigned char sensors) +{ + unsigned char prev; + int result = 0; + + /* Compass data isn't going into the FIFO. Stop trying. */ + sensors &= ~INV_XYZ_COMPASS; + + if (st.chip_cfg.dmp_on) + return 0; + else { + if (!(st.chip_cfg.sensors)) + return -1; + prev = st.chip_cfg.fifo_enable; + st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors; + if (st.chip_cfg.fifo_enable != sensors) + /* You're not getting what you asked for. Some sensors are + * asleep. + */ + result = -1; + else + result = 0; + if (sensors || st.chip_cfg.lp_accel_mode) + set_int_enable(1); + else + set_int_enable(0); + if (sensors) { + if (mpu_reset_fifo()) { + st.chip_cfg.fifo_enable = prev; + return -1; + } + } + } + + return result; +} + +/** + * @brief Get current power state. + * @param[in] power_on 1 if turned on, 0 if suspended. + * @return 0 if successful. + */ +int mpu_get_power_state(unsigned char *power_on) +{ + if (st.chip_cfg.sensors) + power_on[0] = 1; + else + power_on[0] = 0; + return 0; +} + +/** + * @brief Turn specific sensors on/off. + * @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * \n INV_XYZ_COMPASS + * @param[in] sensors Mask of sensors to wake. + * @return 0 if successful. + */ +int mpu_set_sensors(unsigned char sensors) +{ + unsigned char data; +#ifdef AK89xx_SECONDARY + unsigned char user_ctrl; +#endif + + if (sensors & INV_XYZ_GYRO) + data = INV_CLK_PLL; + else if (sensors) + data = 0; + else + data = BIT_SLEEP; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) { + st.chip_cfg.sensors = 0; + return -1; + } + st.chip_cfg.clk_src = data & ~BIT_SLEEP; + + data = 0; + if (!(sensors & INV_X_GYRO)) + data |= BIT_STBY_XG; + if (!(sensors & INV_Y_GYRO)) + data |= BIT_STBY_YG; + if (!(sensors & INV_Z_GYRO)) + data |= BIT_STBY_ZG; + if (!(sensors & INV_XYZ_ACCEL)) + data |= BIT_STBY_XYZA; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) { + st.chip_cfg.sensors = 0; + return -1; + } + + if (sensors && (sensors != INV_XYZ_ACCEL)) + /* Latched interrupts only used in LP accel mode. */ + mpu_set_int_latched(0); + +#ifdef AK89xx_SECONDARY +#ifdef AK89xx_BYPASS + if (sensors & INV_XYZ_COMPASS) + mpu_set_bypass(1); + else + mpu_set_bypass(0); +#else + if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) + return -1; + /* Handle AKM power management. */ + if (sensors & INV_XYZ_COMPASS) { + data = AKM_SINGLE_MEASUREMENT; + user_ctrl |= BIT_AUX_IF_EN; + } else { + data = AKM_POWER_DOWN; + user_ctrl &= ~BIT_AUX_IF_EN; + } + if (st.chip_cfg.dmp_on) + user_ctrl |= BIT_DMP_EN; + else + user_ctrl &= ~BIT_DMP_EN; + if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data)) + return -1; + /* Enable/disable I2C master mode. */ + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) + return -1; +#endif +#endif + + st.chip_cfg.sensors = sensors; + st.chip_cfg.lp_accel_mode = 0; + delay_ms(50, 1); + return 0; +} + +/** + * @brief Read the MPU interrupt status registers. + * @param[out] status Mask of interrupt bits. + * @return 0 if successful. + */ +int mpu_get_int_status(short *status) +{ + unsigned char tmp[2]; + if (!st.chip_cfg.sensors) + return -1; + if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp)) + return -1; + status[0] = (tmp[0] << 8) | tmp[1]; + return 0; +} + +/** + * @brief Get one packet from the FIFO. + * If @e sensors does not contain a particular sensor, disregard the data + * returned to that pointer. + * \n @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * \n If the FIFO has no new data, @e sensors will be zero. + * \n If the FIFO is disabled, @e sensors will be zero and this function will + * return a non-zero error code. + * @param[out] gyro Gyro data in hardware units. + * @param[out] accel Accel data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. + * @param[out] sensors Mask of sensors read from FIFO. + * @param[out] more Number of remaining packets. + * @return 0 if successful. + */ +int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, + unsigned char *sensors, unsigned char *more) +{ + /* Assumes maximum packet size is gyro (6) + accel (6). */ + unsigned char data[MAX_PACKET_LENGTH]; + unsigned char packet_size = 0; + unsigned short fifo_count, index = 0; + + if (st.chip_cfg.dmp_on) + return -1; + + sensors[0] = 0; + if (!st.chip_cfg.sensors) + return -1; + if (!st.chip_cfg.fifo_enable) + return -1; + + if (st.chip_cfg.fifo_enable & INV_X_GYRO) + packet_size += 2; + if (st.chip_cfg.fifo_enable & INV_Y_GYRO) + packet_size += 2; + if (st.chip_cfg.fifo_enable & INV_Z_GYRO) + packet_size += 2; + if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) + packet_size += 6; + + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + fifo_count = (data[0] << 8) | data[1]; + if (fifo_count < packet_size) + return 0; +// log_i("FIFO count: %hd\n", fifo_count); + if (fifo_count > (st.hw->max_fifo >> 1)) { + /* FIFO is 50% full, better check overflow bit. */ + if (i2c_read(st.hw->addr, st.reg->int_status, 1, data)) + return -1; + if (data[0] & BIT_FIFO_OVERFLOW) { + mpu_reset_fifo(); + return -2; + } + } + get_ms((unsigned long*)timestamp); + + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data)) + return -1; + more[0] = fifo_count / packet_size - 1; + sensors[0] = 0; + + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) { + accel[0] = (data[index+0] << 8) | data[index+1]; + accel[1] = (data[index+2] << 8) | data[index+3]; + accel[2] = (data[index+4] << 8) | data[index+5]; + sensors[0] |= INV_XYZ_ACCEL; + index += 6; + } + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) { + gyro[0] = (data[index+0] << 8) | data[index+1]; + sensors[0] |= INV_X_GYRO; + index += 2; + } + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) { + gyro[1] = (data[index+0] << 8) | data[index+1]; + sensors[0] |= INV_Y_GYRO; + index += 2; + } + if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) { + gyro[2] = (data[index+0] << 8) | data[index+1]; + sensors[0] |= INV_Z_GYRO; + index += 2; + } + + return 0; +} + +/** + * @brief Get one unparsed packet from the FIFO. + * This function should be used if the packet is to be parsed elsewhere. + * @param[in] length Length of one FIFO packet. + * @param[in] data FIFO packet. + * @param[in] more Number of remaining packets. + */ +int mpu_read_fifo_stream(unsigned short length, unsigned char *data, + unsigned char *more) +{ + unsigned char tmp[2]; + unsigned short fifo_count; + if (!st.chip_cfg.dmp_on) + return -1; + if (!st.chip_cfg.sensors) + return -1; + + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp)) + return -1; + fifo_count = (tmp[0] << 8) | tmp[1]; + if (fifo_count < length) { + more[0] = 0; + return -1; + } + if (fifo_count > (st.hw->max_fifo >> 1)) { + /* FIFO is 50% full, better check overflow bit. */ + if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp)) + return -1; + if (tmp[0] & BIT_FIFO_OVERFLOW) { + mpu_reset_fifo(); + return -2; + } + } + + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data)) + return -1; + more[0] = fifo_count / length - 1; + return 0; +} + +/** + * @brief Set device to bypass mode. + * @param[in] bypass_on 1 to enable bypass mode. + * @return 0 if successful. + */ +int mpu_set_bypass(unsigned char bypass_on) +{ + unsigned char tmp; + + if (st.chip_cfg.bypass_mode == bypass_on) + return 0; + + if (bypass_on) { + if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + tmp &= ~BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + delay_ms(3, 1); + tmp = BIT_BYPASS_EN; + if (st.chip_cfg.active_low_int) + tmp |= BIT_ACTL; + if (st.chip_cfg.latched_int) + tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; + if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) + return -1; + } else { + /* Enable I2C master mode if compass is being used. */ + if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + if (st.chip_cfg.sensors & INV_XYZ_COMPASS) + tmp |= BIT_AUX_IF_EN; + else + tmp &= ~BIT_AUX_IF_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) + return -1; + delay_ms(3, 1); + if (st.chip_cfg.active_low_int) + tmp = BIT_ACTL; + else + tmp = 0; + if (st.chip_cfg.latched_int) + tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; + if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) + return -1; + } + st.chip_cfg.bypass_mode = bypass_on; + return 0; +} + +/** + * @brief Set interrupt level. + * @param[in] active_low 1 for active low, 0 for active high. + * @return 0 if successful. + */ +int mpu_set_int_level(unsigned char active_low) +{ + st.chip_cfg.active_low_int = active_low; + return 0; +} + +/** + * @brief Enable latched interrupts. + * Any MPU register will clear the interrupt. + * @param[in] enable 1 to enable, 0 to disable. + * @return 0 if successful. + */ +int mpu_set_int_latched(unsigned char enable) +{ + unsigned char tmp; + if (st.chip_cfg.latched_int == enable) + return 0; + + if (enable) + tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; + else + tmp = 0; + if (st.chip_cfg.bypass_mode) + tmp |= BIT_BYPASS_EN; + if (st.chip_cfg.active_low_int) + tmp |= BIT_ACTL; + if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) + return -1; + st.chip_cfg.latched_int = enable; + return 0; +} + +#ifdef MPU6050 +static int get_accel_prod_shift(float *st_shift) +{ + unsigned char tmp[4], shift_code[3], ii; + + if (i2c_read(st.hw->addr, 0x0D, 4, tmp)) + return 0x07; + + shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); + shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); + shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); + for (ii = 0; ii < 3; ii++) { + if (!shift_code[ii]) { + st_shift[ii] = 0.f; + continue; + } + /* Equivalent to.. + * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f) + */ + st_shift[ii] = 0.34f; + while (--shift_code[ii]) + st_shift[ii] *= 1.034f; + } + return 0; +} + +static int accel_self_test(long *bias_regular, long *bias_st) +{ + int jj, result = 0; + float st_shift[3], st_shift_cust, st_shift_var; + float accel_max_z_bias, accel_max_xy_bias; + + get_accel_prod_shift(st_shift); + for(jj = 0; jj < 3; jj++) { + st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; + if (st_shift[jj]) { + st_shift_var = st_shift_cust / st_shift[jj] - 1.f; + if (fabs(st_shift_var) > test.max_accel_var) + result |= 1 << jj; + } else if ((st_shift_cust < test.min_g) || + (st_shift_cust > test.max_g)) + result |= 1 << jj; + } + + accel_max_z_bias = (.23f * 65535.f); + accel_max_xy_bias = (.18f * 65535.f); + log_i("ACCEL Bias Test:\r\n"); + log_i("Initial Biases:%ld, %ld, %ld\r\n", bias_regular[0], bias_regular[1], bias_regular[2]); + + if(result == 0) { + if(bias_regular[0]>accel_max_xy_bias) { + log_e("ACCEL FAIL X\r\n"); + result |= 1; + } + if(bias_regular[1]>accel_max_xy_bias) { + log_e("ACCEL FAIL Y\r\n"); + result |= 2; + } + if(bias_regular[2]>accel_max_z_bias) { + log_e("ACCEL FAIL Z\r\n"); + result |= 4; + } + } + + return result; +} + +static int gyro_self_test(long *bias_regular, long *bias_st) +{ + int jj, result = 0; + unsigned char tmp[3]; + float st_shift, st_shift_cust, st_shift_var; + float gyro_max_bias; + + if (i2c_read(st.hw->addr, 0x0D, 3, tmp)) + return 0x07; + + tmp[0] &= 0x1F; + tmp[1] &= 0x1F; + tmp[2] &= 0x1F; + + for (jj = 0; jj < 3; jj++) { + st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; + if (tmp[jj]) { + st_shift = 3275.f / test.gyro_sens; + while (--tmp[jj]) + st_shift *= 1.046f; + st_shift_var = st_shift_cust / st_shift - 1.f; + if (fabs(st_shift_var) > test.max_gyro_var) + result |= 1 << jj; + } else if ((st_shift_cust < test.min_dps) || + (st_shift_cust > test.max_dps)) + result |= 1 << jj; + } + + gyro_max_bias = (20.f * 65535.f); + log_i("GYRO Bias Test:\r\n"); + log_i("Initial Biases:%ld, %ld, %ld\r\n", bias_regular[0], bias_regular[1], bias_regular[2]); + if(result == 0) { + if(bias_regular[0]>gyro_max_bias) { + log_e("GYRO FAIL X\r\n"); + result |= 1; + } + if(bias_regular[1]>gyro_max_bias) { + log_e("GYRO FAIL Y\r\n"); + result |= 2; + } + if(bias_regular[2]>gyro_max_bias) { + log_e("GYRO FAIL Z\r\n"); + result |= 4; + } + } + + return result; +} + +#endif +#ifdef AK89xx_SECONDARY +static int compass_self_test(void) +{ + unsigned char tmp[6]; + unsigned char tries = 10; + int result = 0x07; + short data; + + mpu_set_bypass(1); + + tmp[0] = AKM_POWER_DOWN; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) + return 0x07; + tmp[0] = AKM_BIT_SELF_TEST; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp)) + goto AKM_restore; + tmp[0] = AKM_MODE_SELF_TEST; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) + goto AKM_restore; + + do { + delay_ms(10, 1); + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp)) + goto AKM_restore; + if (tmp[0] & AKM_DATA_READY) + break; + } while (tries--); + if (!(tmp[0] & AKM_DATA_READY)) + goto AKM_restore; + + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp)) + goto AKM_restore; + + result = 0; +#if defined MPU9150 + data = (short)(tmp[1] << 8) | tmp[0]; + if ((data > 100) || (data < -100)) + result |= 0x01; + data = (short)(tmp[3] << 8) | tmp[2]; + if ((data > 100) || (data < -100)) + result |= 0x02; + data = (short)(tmp[5] << 8) | tmp[4]; + if ((data > -300) || (data < -1000)) + result |= 0x04; +#elif defined MPU9250 + data = (short)(tmp[1] << 8) | tmp[0]; + if ((data > 200) || (data < -200)) + result |= 0x01; + data = (short)(tmp[3] << 8) | tmp[2]; + if ((data > 200) || (data < -200)) + result |= 0x02; + data = (short)(tmp[5] << 8) | tmp[4]; + if ((data > -800) || (data < -3200)) + result |= 0x04; +#endif +AKM_restore: + tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS; + i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp); + tmp[0] = SUPPORTS_AK89xx_HIGH_SENS; + i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp); + mpu_set_bypass(0); + return result; +} +#endif + +static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) +{ + unsigned char data[MAX_PACKET_LENGTH]; + unsigned char packet_count, ii; + unsigned short fifo_count; + + data[0] = 0x01; + data[1] = 0; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) + return -1; + delay_ms(200, 1); + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + data[0] = BIT_FIFO_RST | BIT_DMP_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + delay_ms(15, 1); + data[0] = st.test->reg_lpf; + if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) + return -1; + data[0] = st.test->reg_rate_div; + if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) + return -1; + if (hw_test) + data[0] = st.test->reg_gyro_fsr | 0xE0; + else + data[0] = st.test->reg_gyro_fsr; + if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) + return -1; + + if (hw_test) + data[0] = st.test->reg_accel_fsr | 0xE0; + else + data[0] = test.reg_accel_fsr; + if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) + return -1; + if (hw_test) + delay_ms(200, 1); + + /* Fill FIFO for test.wait_ms milliseconds. */ + data[0] = BIT_FIFO_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + + data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + delay_ms(test.wait_ms, 1); + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + + fifo_count = (data[0] << 8) | data[1]; + packet_count = fifo_count / MAX_PACKET_LENGTH; + gyro[0] = gyro[1] = gyro[2] = 0; + accel[0] = accel[1] = accel[2] = 0; + + for (ii = 0; ii < packet_count; ii++) { + short accel_cur[3], gyro_cur[3]; + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) + return -1; + accel_cur[0] = ((short)data[0] << 8) | data[1]; + accel_cur[1] = ((short)data[2] << 8) | data[3]; + accel_cur[2] = ((short)data[4] << 8) | data[5]; + accel[0] += (long)accel_cur[0]; + accel[1] += (long)accel_cur[1]; + accel[2] += (long)accel_cur[2]; + gyro_cur[0] = (((short)data[6] << 8) | data[7]); + gyro_cur[1] = (((short)data[8] << 8) | data[9]); + gyro_cur[2] = (((short)data[10] << 8) | data[11]); + gyro[0] += (long)gyro_cur[0]; + gyro[1] += (long)gyro_cur[1]; + gyro[2] += (long)gyro_cur[2]; + } +#ifdef EMPL_NO_64BIT + gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); + gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); + gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); + if (has_accel) { + accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / + packet_count); + accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / + packet_count); + accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / + packet_count); + /* Don't remove gravity! */ + accel[2] -= 65536L; + } +#else + gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); + gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); + gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); + accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / + packet_count); + accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / + packet_count); + accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / + packet_count); + /* Don't remove gravity! */ + if (accel[2] > 0L) + accel[2] -= 65536L; + else + accel[2] += 65536L; +#endif + + return 0; +} + +#ifdef MPU6500 +#define REG_6500_XG_ST_DATA 0x0 +#define REG_6500_XA_ST_DATA 0xD +static const unsigned short mpu_6500_st_tb[256] = { + 2620,2646,2672,2699,2726,2753,2781,2808, //7 + 2837,2865,2894,2923,2952,2981,3011,3041, //15 + 3072,3102,3133,3165,3196,3228,3261,3293, //23 + 3326,3359,3393,3427,3461,3496,3531,3566, //31 + 3602,3638,3674,3711,3748,3786,3823,3862, //39 + 3900,3939,3979,4019,4059,4099,4140,4182, //47 + 4224,4266,4308,4352,4395,4439,4483,4528, //55 + 4574,4619,4665,4712,4759,4807,4855,4903, //63 + 4953,5002,5052,5103,5154,5205,5257,5310, //71 + 5363,5417,5471,5525,5581,5636,5693,5750, //79 + 5807,5865,5924,5983,6043,6104,6165,6226, //87 + 6289,6351,6415,6479,6544,6609,6675,6742, //95 + 6810,6878,6946,7016,7086,7157,7229,7301, //103 + 7374,7448,7522,7597,7673,7750,7828,7906, //111 + 7985,8065,8145,8227,8309,8392,8476,8561, //119 + 8647,8733,8820,8909,8998,9088,9178,9270, + 9363,9457,9551,9647,9743,9841,9939,10038, + 10139,10240,10343,10446,10550,10656,10763,10870, + 10979,11089,11200,11312,11425,11539,11654,11771, + 11889,12008,12128,12249,12371,12495,12620,12746, + 12874,13002,13132,13264,13396,13530,13666,13802, + 13940,14080,14221,14363,14506,14652,14798,14946, + 15096,15247,15399,15553,15709,15866,16024,16184, + 16346,16510,16675,16842,17010,17180,17352,17526, + 17701,17878,18057,18237,18420,18604,18790,18978, + 19167,19359,19553,19748,19946,20145,20347,20550, + 20756,20963,21173,21385,21598,21814,22033,22253, + 22475,22700,22927,23156,23388,23622,23858,24097, + 24338,24581,24827,25075,25326,25579,25835,26093, + 26354,26618,26884,27153,27424,27699,27976,28255, + 28538,28823,29112,29403,29697,29994,30294,30597, + 30903,31212,31524,31839,32157,32479,32804,33132 +}; +static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug) +{ + int i, result = 0, otp_value_zero = 0; + float accel_st_al_min, accel_st_al_max; + float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max; + unsigned char regs[3]; + if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) { + if(debug) + log_i("Reading OTP Register Error.\n"); + return 0x07; + } + if(debug) + log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]); + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + ct_shift_prod[i] *= 65536.f; + ct_shift_prod[i] /= test.accel_sens; + } + else { + ct_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + if(otp_value_zero == 0) { + if(debug) + log_i("ACCEL:CRITERIA A\n"); + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + if(debug) { + log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n", + st_shift_cust[i]/1.f, bias_regular[i]/1.f, + bias_st[i]/1.f); + log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f); + } + + st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f; + + if(debug) + log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f, + test.max_accel_var/1.f); + + if (fabs(st_shift_ratio[i]) > test.max_accel_var) { + if(debug) + log_i("ACCEL Fail Axis = %d\n", i); + result |= 1 << i; //Error condition + } + } + } + else { + /* Self Test Pass/Fail Criteria B */ + accel_st_al_min = test.min_g * 65536.f; + accel_st_al_max = test.max_g * 65536.f; + + if(debug) { + log_i("ACCEL:CRITERIA B\r\n"); + log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f); + log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f); + } + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + + if(debug) + log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f); + if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) { + if(debug) + log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i); + result |= 1 << i; //Error condition + } + } + } + + if(result == 0) { + /* Self Test Pass/Fail Criteria C */ + accel_offset_max = test.max_g_offset * 65536.f; + if(debug) + log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f); + for (i = 0; i < 3; i++) { + if(fabs(bias_regular[i]) > accel_offset_max) { + if(debug) + log_i("FAILED: Accel axis:%d = %ld > 500mg\n", i, bias_regular[i]); + result |= 1 << i; //Error condition + } + } + } + + return result; +} + +static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug) +{ + int i, result = 0, otp_value_zero = 0; + float gyro_st_al_max; + float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], gyro_offset_max; + unsigned char regs[3]; + + if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) { + if(debug) + log_i("Reading OTP Register Error.\n"); + return 0x07; + } + + if(debug) + log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]); + + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + ct_shift_prod[i] *= 65536.f; + ct_shift_prod[i] /= test.gyro_sens; + } + else { + ct_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + + if(otp_value_zero == 0) { + if(debug) + log_i("GYRO:CRITERIA A\n"); + /* Self Test Pass/Fail Criteria A */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + + if(debug) { + log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n", + st_shift_cust[i]/1.f, bias_regular[i]/1.f, + bias_st[i]/1.f); + log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f); + } + + st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i]; + + if(debug) + log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f, + test.max_gyro_var/1.f); + + if (fabs(st_shift_ratio[i]) < test.max_gyro_var) { + if(debug) + log_i("Gyro Fail Axis = %d\n", i); + result |= 1 << i; //Error condition + } + } + } + else { + /* Self Test Pass/Fail Criteria B */ + gyro_st_al_max = test.max_dps * 65536.f; + + if(debug) { + log_i("GYRO:CRITERIA B\r\n"); + log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f); + } + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + + if(debug) + log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f); + if(st_shift_cust[i] < gyro_st_al_max) { + if(debug) + log_i("GYRO FAIL axis:%d greater than 60dps\n", i); + result |= 1 << i; //Error condition + } + } + } + + if(result == 0) { + /* Self Test Pass/Fail Criteria C */ + gyro_offset_max = test.min_dps * 65536.f; + if(debug) + log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f); + for (i = 0; i < 3; i++) { + if(fabs(bias_regular[i]) > gyro_offset_max) { + if(debug) + log_i("FAILED: Gyro axis:%d = %ld > 20dps\n", i, bias_regular[i]); + result |= 1 << i; //Error condition + } + } + } + return result; +} + +static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, int debug) +{ + unsigned char data[HWST_MAX_PACKET_LENGTH]; + unsigned char packet_count, ii; + unsigned short fifo_count; + int s = 0, read_size = 0, ind; + + data[0] = 0x01; + data[1] = 0; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) + return -1; + delay_ms(200, 1); + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) + return -1; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + data[0] = BIT_FIFO_RST | BIT_DMP_RST; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + delay_ms(15, 1); + data[0] = st.test->reg_lpf; + if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) + return -1; + data[0] = st.test->reg_rate_div; + if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) + return -1; + if (hw_test) + data[0] = st.test->reg_gyro_fsr | 0xE0; + else + data[0] = st.test->reg_gyro_fsr; + if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) + return -1; + + if (hw_test) + data[0] = st.test->reg_accel_fsr | 0xE0; + else + data[0] = test.reg_accel_fsr; + if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) + return -1; + + delay_ms(test.wait_ms, 1); //wait 200ms for sensors to stabilize + + /* Enable FIFO */ + data[0] = BIT_FIFO_EN; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) + return -1; + data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + + //initialize the bias return values + gyro[0] = gyro[1] = gyro[2] = 0; + accel[0] = accel[1] = accel[2] = 0; + + if(debug) + log_i("Starting Bias Loop Reads\n"); + + //start reading samples + while (s < test.packet_thresh) { + delay_ms(test.sample_wait_ms, 1); //wait 10ms to fill FIFO + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + fifo_count = (data[0] << 8) | data[1]; + packet_count = fifo_count / MAX_PACKET_LENGTH; + if ((test.packet_thresh - s) < packet_count) + packet_count = test.packet_thresh - s; + read_size = packet_count * MAX_PACKET_LENGTH; + + //burst read from FIFO + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data)) + return -1; + ind = 0; + for (ii = 0; ii < packet_count; ii++) { + short accel_cur[3], gyro_cur[3]; + accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1]; + accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3]; + accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5]; + accel[0] += (long)accel_cur[0]; + accel[1] += (long)accel_cur[1]; + accel[2] += (long)accel_cur[2]; + gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]); + gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]); + gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]); + gyro[0] += (long)gyro_cur[0]; + gyro[1] += (long)gyro_cur[1]; + gyro[2] += (long)gyro_cur[2]; + ind += MAX_PACKET_LENGTH; + } + s += packet_count; + } + + if(debug) + log_i("Samples: %d\n", s); + + //stop FIFO + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) + return -1; + + gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / s); + gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / s); + gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / s); + accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / s); + accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / s); + accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / s); + /* remove gravity from bias calculation */ + if (accel[2] > 0L) + accel[2] -= 65536L; + else + accel[2] += 65536L; + + + if(debug) { + log_i("Accel offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, accel[0]/65536.f, accel[1]/65536.f, accel[2]/65536.f); + log_i("Gyro offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, gyro[0]/65536.f, gyro[1]/65536.f, gyro[2]/65536.f); + } + + return 0; +} +/** + * @brief Trigger gyro/accel/compass self-test for MPU6500/MPU9250 + * On success/error, the self-test returns a mask representing the sensor(s) + * that failed. For each bit, a one (1) represents a "pass" case; conversely, + * a zero (0) indicates a failure. + * + * \n The mask is defined as follows: + * \n Bit 0: Gyro. + * \n Bit 1: Accel. + * \n Bit 2: Compass. + * + * @param[out] gyro Gyro biases in q16 format. + * @param[out] accel Accel biases (if applicable) in q16 format. + * @param[in] debug Debug flag used to print out more detailed logs. Must first set up logging in Motion Driver. + * @return Result mask (see above). + */ +int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) +{ + const unsigned char tries = 2; + long gyro_st[3], accel_st[3]; + unsigned char accel_result, gyro_result; +#ifdef AK89xx_SECONDARY + unsigned char compass_result; +#endif + int ii; + + int result; + unsigned char accel_fsr, fifo_sensors, sensors_on; + unsigned short gyro_fsr, sample_rate, lpf; + unsigned char dmp_was_on; + + + + if(debug) + log_i("Starting MPU6500 HWST!\r\n"); + + if (st.chip_cfg.dmp_on) { + mpu_set_dmp_state(0); + dmp_was_on = 1; + } else + dmp_was_on = 0; + + /* Get initial settings. */ + mpu_get_gyro_fsr(&gyro_fsr); + mpu_get_accel_fsr(&accel_fsr); + mpu_get_lpf(&lpf); + mpu_get_sample_rate(&sample_rate); + sensors_on = st.chip_cfg.sensors; + mpu_get_fifo_config(&fifo_sensors); + + if(debug) + log_i("Retrieving Biases\r\n"); + + for (ii = 0; ii < tries; ii++) + if (!get_st_6500_biases(gyro, accel, 0, debug)) + break; + if (ii == tries) { + /* If we reach this point, we most likely encountered an I2C error. + * We'll just report an error for all three sensors. + */ + if(debug) + log_i("Retrieving Biases Error - possible I2C error\n"); + + result = 0; + goto restore; + } + + if(debug) + log_i("Retrieving ST Biases\n"); + + for (ii = 0; ii < tries; ii++) + if (!get_st_6500_biases(gyro_st, accel_st, 1, debug)) + break; + if (ii == tries) { + + if(debug) + log_i("Retrieving ST Biases Error - possible I2C error\n"); + + /* Again, probably an I2C error. */ + result = 0; + goto restore; + } + + accel_result = accel_6500_self_test(accel, accel_st, debug); + if(debug) + log_i("Accel Self Test Results: %d\n", accel_result); + + gyro_result = gyro_6500_self_test(gyro, gyro_st, debug); + if(debug) + log_i("Gyro Self Test Results: %d\n", gyro_result); + + result = 0; + if (!gyro_result) + result |= 0x01; + if (!accel_result) + result |= 0x02; + +#ifdef AK89xx_SECONDARY + compass_result = compass_self_test(); + if(debug) + log_i("Compass Self Test Results: %d\n", compass_result); + if (!compass_result) + result |= 0x04; +#else + result |= 0x04; +#endif +restore: + if(debug) + log_i("Exiting HWST\n"); + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_gyro_fsr(gyro_fsr); + mpu_set_accel_fsr(accel_fsr); + mpu_set_lpf(lpf); + mpu_set_sample_rate(sample_rate); + mpu_set_sensors(sensors_on); + mpu_configure_fifo(fifo_sensors); + + if (dmp_was_on) + mpu_set_dmp_state(1); + + return result; +} +#endif + /* + * \n This function must be called with the device either face-up or face-down + * (z-axis is parallel to gravity). + * @param[out] gyro Gyro biases in q16 format. + * @param[out] accel Accel biases (if applicable) in q16 format. + * @return Result mask (see above). + */ +int mpu_run_self_test(long *gyro, long *accel) +{ +#ifdef MPU6050 + const unsigned char tries = 2; + long gyro_st[3], accel_st[3]; + unsigned char accel_result, gyro_result; +#ifdef AK89xx_SECONDARY + unsigned char compass_result; +#endif + int ii; +#endif + int result; + unsigned char accel_fsr, fifo_sensors, sensors_on; + unsigned short gyro_fsr, sample_rate, lpf; + unsigned char dmp_was_on; + + if (st.chip_cfg.dmp_on) { + mpu_set_dmp_state(0); + dmp_was_on = 1; + } else + dmp_was_on = 0; + + /* Get initial settings. */ + mpu_get_gyro_fsr(&gyro_fsr); + mpu_get_accel_fsr(&accel_fsr); + mpu_get_lpf(&lpf); + mpu_get_sample_rate(&sample_rate); + sensors_on = st.chip_cfg.sensors; + mpu_get_fifo_config(&fifo_sensors); + + /* For older chips, the self-test will be different. */ +#if defined MPU6050 + for (ii = 0; ii < tries; ii++) + if (!get_st_biases(gyro, accel, 0)) + break; + if (ii == tries) { + /* If we reach this point, we most likely encountered an I2C error. + * We'll just report an error for all three sensors. + */ + result = 0; + goto restore; + } + for (ii = 0; ii < tries; ii++) + if (!get_st_biases(gyro_st, accel_st, 1)) + break; + if (ii == tries) { + /* Again, probably an I2C error. */ + result = 0; + goto restore; + } + accel_result = accel_self_test(accel, accel_st); + gyro_result = gyro_self_test(gyro, gyro_st); + + result = 0; + if (!gyro_result) + result |= 0x01; + if (!accel_result) + result |= 0x02; + +#ifdef AK89xx_SECONDARY + compass_result = compass_self_test(); + if (!compass_result) + result |= 0x04; +#else + result |= 0x04; +#endif +restore: +#elif defined MPU6500 + /* For now, this function will return a "pass" result for all three sensors + * for compatibility with current test applications. + */ + get_st_biases(gyro, accel, 0); + result = 0x7; +#endif + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_gyro_fsr(gyro_fsr); + mpu_set_accel_fsr(accel_fsr); + mpu_set_lpf(lpf); + mpu_set_sample_rate(sample_rate); + mpu_set_sensors(sensors_on); + mpu_configure_fifo(fifo_sensors); + + if (dmp_was_on) + mpu_set_dmp_state(1); + + return result; +} + +/** + * @brief Write to the DMP memory. + * This function prevents I2C writes past the bank boundaries. The DMP memory + * is only accessible when the chip is awake. + * @param[in] mem_addr Memory location (bank << 8 | start address) + * @param[in] length Number of bytes to write. + * @param[in] data Bytes to write to memory. + * @return 0 if successful. + */ +int mpu_write_mem(unsigned short mem_addr, unsigned short length, + unsigned char *data) +{ + unsigned char tmp[2]; + + if (!data) + return -1; + if (!st.chip_cfg.sensors) + return -1; + + tmp[0] = (unsigned char)(mem_addr >> 8); + tmp[1] = (unsigned char)(mem_addr & 0xFF); + + /* Check bank boundaries. */ + if (tmp[1] + length > st.hw->bank_size) + return -1; + + if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) + return -1; + if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data)) + return -1; + return 0; +} + +/** + * @brief Read from the DMP memory. + * This function prevents I2C reads past the bank boundaries. The DMP memory + * is only accessible when the chip is awake. + * @param[in] mem_addr Memory location (bank << 8 | start address) + * @param[in] length Number of bytes to read. + * @param[out] data Bytes read from memory. + * @return 0 if successful. + */ +int mpu_read_mem(unsigned short mem_addr, unsigned short length, + unsigned char *data) +{ + unsigned char tmp[2]; + + if (!data) + return -1; + if (!st.chip_cfg.sensors) + return -1; + + tmp[0] = (unsigned char)(mem_addr >> 8); + tmp[1] = (unsigned char)(mem_addr & 0xFF); + + /* Check bank boundaries. */ + if (tmp[1] + length > st.hw->bank_size) + return -1; + + if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) + return -1; + if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data)) + return -1; + return 0; +} + +/** + * @brief Load and verify DMP image. + * @param[in] length Length of DMP image. + * @param[in] firmware DMP code. + * @param[in] start_addr Starting address of DMP code memory. + * @param[in] sample_rate Fixed sampling rate used when DMP is enabled. + * @return 0 if successful. + */ +int mpu_load_firmware(unsigned short length, const unsigned char *firmware, + unsigned short start_addr, unsigned short sample_rate) +{ + unsigned short ii; + unsigned short this_write; + /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */ +#define LOAD_CHUNK (16) + unsigned char cur[LOAD_CHUNK], tmp[2]; + + if (st.chip_cfg.dmp_loaded) + /* DMP should only be loaded once. */ + return -1; + + if (!firmware) + return -1; + for (ii = 0; ii < length; ii += this_write) { + this_write = min(LOAD_CHUNK, length - ii); + if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])) + return -1; + if (mpu_read_mem(ii, this_write, cur)) + return -1; + if (memcmp(firmware+ii, cur, this_write)) + return -2; + } + + /* Set program start address. */ + tmp[0] = start_addr >> 8; + tmp[1] = start_addr & 0xFF; + if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) + return -1; + + st.chip_cfg.dmp_loaded = 1; + st.chip_cfg.dmp_sample_rate = sample_rate; + return 0; +} + +/** + * @brief Enable/disable DMP support. + * @param[in] enable 1 to turn on the DMP. + * @return 0 if successful. + */ +int mpu_set_dmp_state(unsigned char enable) +{ + unsigned char tmp; + if (st.chip_cfg.dmp_on == enable) + return 0; + + if (enable) { + if (!st.chip_cfg.dmp_loaded) + return -1; + /* Disable data ready interrupt. */ + set_int_enable(0); + /* Disable bypass mode. */ + mpu_set_bypass(0); + /* Keep constant sample rate, FIFO rate controlled by DMP. */ + mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate); + /* Remove FIFO elements. */ + tmp = 0; + i2c_write(st.hw->addr, 0x23, 1, &tmp); + st.chip_cfg.dmp_on = 1; + /* Enable DMP interrupt. */ + set_int_enable(1); + mpu_reset_fifo(); + } else { + /* Disable DMP interrupt. */ + set_int_enable(0); + /* Restore FIFO settings. */ + tmp = st.chip_cfg.fifo_enable; + i2c_write(st.hw->addr, 0x23, 1, &tmp); + st.chip_cfg.dmp_on = 0; + mpu_reset_fifo(); + } + return 0; +} + +/** + * @brief Get DMP state. + * @param[out] enabled 1 if enabled. + * @return 0 if successful. + */ +int mpu_get_dmp_state(unsigned char *enabled) +{ + enabled[0] = st.chip_cfg.dmp_on; + return 0; +} + +#ifdef AK89xx_SECONDARY +/* This initialization is similar to the one in ak8975.c. */ +static int setup_compass(void) +{ + unsigned char data[4], akm_addr; + + mpu_set_bypass(1); + + /* Find compass. Possible addresses range from 0x0C to 0x0F. */ + for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { + int result; + result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data); + if (!result && (data[0] == AKM_WHOAMI)) + break; + } + + if (akm_addr > 0x0F) { + /* TODO: Handle this case in all compass-related functions. */ + log_e("Compass not found.\n"); + return -1; + } + + st.chip_cfg.compass_addr = akm_addr; + + data[0] = AKM_POWER_DOWN; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) + return -1; + delay_ms(1, 1); + + data[0] = AKM_FUSE_ROM_ACCESS; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) + return -1; + delay_ms(1, 1); + + /* Get sensitivity adjustment data from fuse ROM. */ + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) + return -1; + st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; + st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; + st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; + + data[0] = AKM_POWER_DOWN; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) + return -1; + delay_ms(1, 1); + + mpu_set_bypass(0); + + /* Set up master mode, master clock, and ES bit. */ + data[0] = 0x40; + if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) + return -1; + + /* Slave 0 reads from AKM data registers. */ + data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; + if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data)) + return -1; + + /* Compass reads start at this register. */ + data[0] = AKM_REG_ST1; + if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data)) + return -1; + + /* Enable slave 0, 8-byte reads. */ + data[0] = BIT_SLAVE_EN | 8; + if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) + return -1; + + /* Slave 1 changes AKM measurement mode. */ + data[0] = st.chip_cfg.compass_addr; + if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data)) + return -1; + + /* AKM measurement mode register. */ + data[0] = AKM_REG_CNTL; + if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data)) + return -1; + + /* Enable slave 1, 1-byte writes. */ + data[0] = BIT_SLAVE_EN | 1; + if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) + return -1; + + /* Set slave 1 data. */ + data[0] = AKM_SINGLE_MEASUREMENT; + if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data)) + return -1; + + /* Trigger slave 0 and slave 1 actions at each sample. */ + data[0] = 0x03; + if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) + return -1; + +#ifdef MPU9150 + /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ + data[0] = BIT_I2C_MST_VDDIO; + if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) + return -1; +#endif + + return 0; +} +#endif + +/** + * @brief Read raw compass data. + * @param[out] data Raw data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. Null if not needed. + * @return 0 if successful. + */ +int mpu_get_compass_reg(short *data, unsigned long *timestamp) +{ +#ifdef AK89xx_SECONDARY + unsigned char tmp[9]; + + if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS)) + return -1; + +#ifdef AK89xx_BYPASS + if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp)) + return -1; + tmp[8] = AKM_SINGLE_MEASUREMENT; + if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8)) + return -1; +#else + if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp)) + return -1; +#endif + +#if defined AK8975_SECONDARY + /* AK8975 doesn't have the overrun error bit. */ + if (!(tmp[0] & AKM_DATA_READY)) + return -2; + if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR)) + return -3; +#elif defined AK8963_SECONDARY + /* AK8963 doesn't have the data read error bit. */ + if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN)) + return -2; + if (tmp[7] & AKM_OVERFLOW) + return -3; +#endif + data[0] = (tmp[2] << 8) | tmp[1]; + data[1] = (tmp[4] << 8) | tmp[3]; + data[2] = (tmp[6] << 8) | tmp[5]; + + data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8; + data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8; + data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8; + + if (timestamp) + get_ms(timestamp); + return 0; +#else + return -1; +#endif +} + +/** + * @brief Get the compass full-scale range. + * @param[out] fsr Current full-scale range. + * @return 0 if successful. + */ +int mpu_get_compass_fsr(unsigned short *fsr) +{ +#ifdef AK89xx_SECONDARY + fsr[0] = st.hw->compass_fsr; + return 0; +#else + return -1; +#endif +} + +/** + * @brief Enters LP accel motion interrupt mode. + * The behaviour of this feature is very different between the MPU6050 and the + * MPU6500. Each chip's version of this feature is explained below. + * + * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg + * increments. + * + * \n Low-power accel mode supports the following frequencies: + * \n 1.25Hz, 5Hz, 20Hz, 40Hz + * + * \n MPU6500: + * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference + * sample. The hardware monitors the accel data and detects any large change + * over a short period of time. + * + * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg + * increments. + * + * \n MPU6500 Low-power accel mode supports the following frequencies: + * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz + * + * \n\n NOTES: + * \n The driver will round down @e thresh to the nearest supported value if + * an unsupported threshold is selected. + * \n To select a fractional wake-up frequency, round down the value passed to + * @e lpa_freq. + * \n The MPU6500 does not support a delay parameter. If this function is used + * for the MPU6500, the value passed to @e time will be ignored. + * \n To disable this mode, set @e lpa_freq to zero. The driver will restore + * the previous configuration. + * + * @param[in] thresh Motion threshold in mg. + * @param[in] time Duration in milliseconds that the accel data must + * exceed @e thresh before motion is reported. + * @param[in] lpa_freq Minimum sampling rate, or zero to disable. + * @return 0 if successful. + */ +int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, + unsigned short lpa_freq) +{ + +#if defined MPU6500 + unsigned char data[3], temp[1];; +#endif + if (lpa_freq) { +#if defined MPU6500 + unsigned char thresh_hw; + + /* 1LSb = 4mg. */ + if (thresh > 1020) + thresh_hw = 255; + else if (thresh < 4) + thresh_hw = 1; + else + thresh_hw = thresh >> 2; +#endif + + if (!time) + /* Minimum duration must be 1ms. */ + time = 1; + +#if defined MPU6500 + if (lpa_freq > 640) + /* At this point, the chip has not been re-configured, so the + * function can safely exit. + */ + return -1; +#endif + + if (!st.chip_cfg.int_motion_only) { + /* Store current settings for later. */ + if (st.chip_cfg.dmp_on) { + mpu_set_dmp_state(0); + st.chip_cfg.cache.dmp_on = 1; + } else + st.chip_cfg.cache.dmp_on = 0; + mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr); + mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr); + mpu_get_lpf(&st.chip_cfg.cache.lpf); + mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate); + st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors; + mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors); + } + +#if defined MPU6500 + /* Disable hardware interrupts. */ + set_int_enable(0); + + /* Enter full-power accel-only mode, no FIFO/DMP. */ + data[0] = 0; + data[1] = 0; + data[2] = BIT_STBY_XYZG; + if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data)) + goto lp_int_restore; + + /* Set motion threshold. */ + data[0] = thresh_hw; + if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data)) + goto lp_int_restore; + + /* Set wake frequency. */ + if (lpa_freq == 1) + data[0] = INV_LPA_1_25HZ; + else if (lpa_freq == 2) + data[0] = INV_LPA_2_5HZ; + else if (lpa_freq <= 5) + data[0] = INV_LPA_5HZ; + else if (lpa_freq <= 10) + data[0] = INV_LPA_10HZ; + else if (lpa_freq <= 20) + data[0] = INV_LPA_20HZ; + else if (lpa_freq <= 40) + data[0] = INV_LPA_40HZ; + else if (lpa_freq <= 80) + data[0] = INV_LPA_80HZ; + else if (lpa_freq <= 160) + data[0] = INV_LPA_160HZ; + else if (lpa_freq <= 320) + data[0] = INV_LPA_320HZ; + else + data[0] = INV_LPA_640HZ; + if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data)) + goto lp_int_restore; + + /* Enable motion interrupt (MPU6500 version). */ + data[0] = BITS_WOM_EN; + if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) + goto lp_int_restore; + /* Enable LP ACCEL mode, ACCEL_FCHOICE_B=1*/ + if (i2c_read(st.hw->addr, st.reg->accel_cfg2, 1, temp)) + return -1; + + /*Bypass accel DLPF. */ + data[0] = BIT_ACCL_FC_B | temp[0]; + if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) + goto lp_int_restore; + + /* Enable interrupt. */ + data[0] = BIT_MOT_INT_EN; + if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) + goto lp_int_restore; + + /* Enable cycle mode. */ + data[0] = BIT_LPA_CYCLE; + if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) + goto lp_int_restore; + st.chip_cfg.int_motion_only = 1; + return 0; +#endif + } else { + /* Don't "restore" the previous state if no state has been saved. */ + unsigned int ii; + char *cache_ptr = (char*)&st.chip_cfg.cache; + for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) { + if (cache_ptr[ii] != 0) + goto lp_int_restore; + } + /* If we reach this point, motion interrupt mode hasn't been used yet. */ + return -1; + } +lp_int_restore: + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_sensors(st.chip_cfg.cache.sensors_on); + mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr); + mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr); + mpu_set_lpf(st.chip_cfg.cache.lpf); + mpu_set_sample_rate(st.chip_cfg.cache.sample_rate); + mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors); + + if (st.chip_cfg.cache.dmp_on) + mpu_set_dmp_state(1); + +#ifdef MPU6500 + /* Disable motion interrupt (MPU6500 version). */ + data[0] = 0; + if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) + goto lp_int_restore; +#endif + + st.chip_cfg.int_motion_only = 0; + return 0; +} + +/** + * @} + */ + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu.h new file mode 100644 index 00000000..e5cfeb31 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu.h @@ -0,0 +1,133 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @addtogroup DRIVERS Sensor Driver Layer + * @brief Hardware drivers to communicate with sensors via I2C. + * + * @{ + * @file inv_mpu.h + * @brief An I2C-based driver for Invensense gyroscopes. + * @details This driver currently works for the following devices: + * MPU6050 + * MPU6500 + * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) + * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) + */ + +#ifndef _INV_MPU_H_ +#define _INV_MPU_H_ + +#define INV_X_GYRO (0x40) +#define INV_Y_GYRO (0x20) +#define INV_Z_GYRO (0x10) +#define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) +#define INV_XYZ_ACCEL (0x08) +#define INV_XYZ_COMPASS (0x01) + +struct int_param_s { +#if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430 + void (*cb)(void); + unsigned short pin; + unsigned char lp_exit; + unsigned char active_low; +#elif defined EMPL_TARGET_UC3L0 + unsigned long pin; + void (*cb)(volatile void*); + void *arg; +#endif +}; + +#define MPU_INT_STATUS_DATA_READY (0x0001) +#define MPU_INT_STATUS_DMP (0x0002) +#define MPU_INT_STATUS_PLL_READY (0x0004) +#define MPU_INT_STATUS_I2C_MST (0x0008) +#define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010) +#define MPU_INT_STATUS_ZMOT (0x0020) +#define MPU_INT_STATUS_MOT (0x0040) +#define MPU_INT_STATUS_FREE_FALL (0x0080) +#define MPU_INT_STATUS_DMP_0 (0x0100) +#define MPU_INT_STATUS_DMP_1 (0x0200) +#define MPU_INT_STATUS_DMP_2 (0x0400) +#define MPU_INT_STATUS_DMP_3 (0x0800) +#define MPU_INT_STATUS_DMP_4 (0x1000) +#define MPU_INT_STATUS_DMP_5 (0x2000) + +/* Set up APIs */ +//int mpu_init(struct int_param_s *int_param); +int mpu_init(); +int mpu_init_slave(void); +int mpu_set_bypass(unsigned char bypass_on); + +/* Configuration APIs */ +int mpu_lp_accel_mode(unsigned short rate); +int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, + unsigned short lpa_freq); +int mpu_set_int_level(unsigned char active_low); +int mpu_set_int_latched(unsigned char enable); + +int mpu_set_dmp_state(unsigned char enable); +int mpu_get_dmp_state(unsigned char *enabled); + +int mpu_get_lpf(unsigned short *lpf); +int mpu_set_lpf(unsigned short lpf); + +int mpu_get_gyro_fsr(unsigned short *fsr); +int mpu_set_gyro_fsr(unsigned short fsr); + +int mpu_get_accel_fsr(unsigned char *fsr); +int mpu_set_accel_fsr(unsigned char fsr); + +int mpu_get_compass_fsr(unsigned short *fsr); + +int mpu_get_gyro_sens(float *sens); +int mpu_get_accel_sens(unsigned short *sens); + +int mpu_get_sample_rate(unsigned short *rate); +int mpu_set_sample_rate(unsigned short rate); +int mpu_get_compass_sample_rate(unsigned short *rate); +int mpu_set_compass_sample_rate(unsigned short rate); + +int mpu_get_fifo_config(unsigned char *sensors); +int mpu_configure_fifo(unsigned char sensors); + +int mpu_get_power_state(unsigned char *power_on); +int mpu_set_sensors(unsigned char sensors); + +int mpu_read_6500_accel_bias(long *accel_bias); +int mpu_set_gyro_bias_reg(long * gyro_bias); +int mpu_set_accel_bias_6500_reg(const long *accel_bias); +int mpu_read_6050_accel_bias(long *accel_bias); +int mpu_set_accel_bias_6050_reg(const long *accel_bias); + +/* Data getter/setter APIs */ +int mpu_get_gyro_reg(short *data, unsigned long *timestamp); +int mpu_get_accel_reg(short *data, unsigned long *timestamp); +int mpu_get_compass_reg(short *data, unsigned long *timestamp); +int mpu_get_temperature(long *data, unsigned long *timestamp); + +int mpu_get_int_status(short *status); +int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, + unsigned char *sensors, unsigned char *more); +int mpu_read_fifo_stream(unsigned short length, unsigned char *data, + unsigned char *more); +int mpu_reset_fifo(void); + +int mpu_write_mem(unsigned short mem_addr, unsigned short length, + unsigned char *data); +int mpu_read_mem(unsigned short mem_addr, unsigned short length, + unsigned char *data); +int mpu_load_firmware(unsigned short length, const unsigned char *firmware, + unsigned short start_addr, unsigned short sample_rate); + +int mpu_reg_dump(void); +int mpu_read_reg(unsigned char reg, unsigned char *data); +int mpu_run_self_test(long *gyro, long *accel); +int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug); +int mpu_register_tap_cb(void (*func)(unsigned char, unsigned char)); + +#endif /* #ifndef _INV_MPU_H_ */ + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu_dmp_motion_driver.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu_dmp_motion_driver.c new file mode 100644 index 00000000..992baee7 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu_dmp_motion_driver.c @@ -0,0 +1,1334 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @addtogroup DRIVERS Sensor Driver Layer + * @brief Hardware drivers to communicate with sensors via I2C. + * + * @{ + * @file inv_mpu_dmp_motion_driver.c + * @brief DMP image and interface functions. + * @details All functions are preceded by the dmp_ prefix to + * differentiate among MPL and general driver function calls. + */ +#include +#include +#include +#include +#include +#include "imu.h" +#include "inv_mpu.h" +#include "inv_mpu_dmp_motion_driver.h" +#include "dmpKey.h" +#include "dmpmap.h" + +/* embARC HAL */ +#include "embARC.h" +#include "embARC_debug.h" + +#define get_ms board_get_ms + +/* These defines are copied from dmpDefaultMPU6050.c in the general MPL + * releases. These defines may change for each DMP image, so be sure to modify + * these values when switching to a new image. + */ +#define CFG_LP_QUAT (2712) +#define END_ORIENT_TEMP (1866) +#define CFG_27 (2742) +#define CFG_20 (2224) +#define CFG_23 (2745) +#define CFG_FIFO_ON_EVENT (2690) +#define END_PREDICTION_UPDATE (1761) +#define CGNOTICE_INTR (2620) +#define X_GRT_Y_TMP (1358) +#define CFG_DR_INT (1029) +#define CFG_AUTH (1035) +#define UPDATE_PROP_ROT (1835) +#define END_COMPARE_Y_X_TMP2 (1455) +#define SKIP_X_GRT_Y_TMP (1359) +#define SKIP_END_COMPARE (1435) +#define FCFG_3 (1088) +#define FCFG_2 (1066) +#define FCFG_1 (1062) +#define END_COMPARE_Y_X_TMP3 (1434) +#define FCFG_7 (1073) +#define FCFG_6 (1106) +#define FLAT_STATE_END (1713) +#define SWING_END_4 (1616) +#define SWING_END_2 (1565) +#define SWING_END_3 (1587) +#define SWING_END_1 (1550) +#define CFG_8 (2718) +#define CFG_15 (2727) +#define CFG_16 (2746) +#define CFG_EXT_GYRO_BIAS (1189) +#define END_COMPARE_Y_X_TMP (1407) +#define DO_NOT_UPDATE_PROP_ROT (1839) +#define CFG_7 (1205) +#define FLAT_STATE_END_TEMP (1683) +#define END_COMPARE_Y_X (1484) +#define SKIP_SWING_END_1 (1551) +#define SKIP_SWING_END_3 (1588) +#define SKIP_SWING_END_2 (1566) +#define TILTG75_START (1672) +#define CFG_6 (2753) +#define TILTL75_END (1669) +#define END_ORIENT (1884) +#define CFG_FLICK_IN (2573) +#define TILTL75_START (1643) +#define CFG_MOTION_BIAS (1208) +#define X_GRT_Y (1408) +#define TEMPLABEL (2324) +#define CFG_ANDROID_ORIENT_INT (1853) +#define CFG_GYRO_RAW_DATA (2722) +#define X_GRT_Y_TMP2 (1379) + +#define D_0_22 (22+512) +#define D_0_24 (24+512) + +#define D_0_36 (36) +#define D_0_52 (52) +#define D_0_96 (96) +#define D_0_104 (104) +#define D_0_108 (108) +#define D_0_163 (163) +#define D_0_188 (188) +#define D_0_192 (192) +#define D_0_224 (224) +#define D_0_228 (228) +#define D_0_232 (232) +#define D_0_236 (236) + +#define D_1_2 (256 + 2) +#define D_1_4 (256 + 4) +#define D_1_8 (256 + 8) +#define D_1_10 (256 + 10) +#define D_1_24 (256 + 24) +#define D_1_28 (256 + 28) +#define D_1_36 (256 + 36) +#define D_1_40 (256 + 40) +#define D_1_44 (256 + 44) +#define D_1_72 (256 + 72) +#define D_1_74 (256 + 74) +#define D_1_79 (256 + 79) +#define D_1_88 (256 + 88) +#define D_1_90 (256 + 90) +#define D_1_92 (256 + 92) +#define D_1_96 (256 + 96) +#define D_1_98 (256 + 98) +#define D_1_106 (256 + 106) +#define D_1_108 (256 + 108) +#define D_1_112 (256 + 112) +#define D_1_128 (256 + 144) +#define D_1_152 (256 + 12) +#define D_1_160 (256 + 160) +#define D_1_176 (256 + 176) +#define D_1_178 (256 + 178) +#define D_1_218 (256 + 218) +#define D_1_232 (256 + 232) +#define D_1_236 (256 + 236) +#define D_1_240 (256 + 240) +#define D_1_244 (256 + 244) +#define D_1_250 (256 + 250) +#define D_1_252 (256 + 252) +#define D_2_12 (512 + 12) +#define D_2_96 (512 + 96) +#define D_2_108 (512 + 108) +#define D_2_208 (512 + 208) +#define D_2_224 (512 + 224) +#define D_2_236 (512 + 236) +#define D_2_244 (512 + 244) +#define D_2_248 (512 + 248) +#define D_2_252 (512 + 252) + +#define CPASS_BIAS_X (35 * 16 + 4) +#define CPASS_BIAS_Y (35 * 16 + 8) +#define CPASS_BIAS_Z (35 * 16 + 12) +#define CPASS_MTX_00 (36 * 16) +#define CPASS_MTX_01 (36 * 16 + 4) +#define CPASS_MTX_02 (36 * 16 + 8) +#define CPASS_MTX_10 (36 * 16 + 12) +#define CPASS_MTX_11 (37 * 16) +#define CPASS_MTX_12 (37 * 16 + 4) +#define CPASS_MTX_20 (37 * 16 + 8) +#define CPASS_MTX_21 (37 * 16 + 12) +#define CPASS_MTX_22 (43 * 16 + 12) +#define D_EXT_GYRO_BIAS_X (61 * 16) +#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 +#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 +#define D_ACT0 (40 * 16) +#define D_ACSX (40 * 16 + 4) +#define D_ACSY (40 * 16 + 8) +#define D_ACSZ (40 * 16 + 12) + +#define FLICK_MSG (45 * 16 + 4) +#define FLICK_COUNTER (45 * 16 + 8) +#define FLICK_LOWER (45 * 16 + 12) +#define FLICK_UPPER (46 * 16 + 12) + +#define D_AUTH_OUT (992) +#define D_AUTH_IN (996) +#define D_AUTH_A (1000) +#define D_AUTH_B (1004) + +#define D_PEDSTD_BP_B (768 + 0x1C) +#define D_PEDSTD_HP_A (768 + 0x78) +#define D_PEDSTD_HP_B (768 + 0x7C) +#define D_PEDSTD_BP_A4 (768 + 0x40) +#define D_PEDSTD_BP_A3 (768 + 0x44) +#define D_PEDSTD_BP_A2 (768 + 0x48) +#define D_PEDSTD_BP_A1 (768 + 0x4C) +#define D_PEDSTD_INT_THRSH (768 + 0x68) +#define D_PEDSTD_CLIP (768 + 0x6C) +#define D_PEDSTD_SB (768 + 0x28) +#define D_PEDSTD_SB_TIME (768 + 0x2C) +#define D_PEDSTD_PEAKTHRSH (768 + 0x98) +#define D_PEDSTD_TIML (768 + 0x2A) +#define D_PEDSTD_TIMH (768 + 0x2E) +#define D_PEDSTD_PEAK (768 + 0X94) +#define D_PEDSTD_STEPCTR (768 + 0x60) +#define D_PEDSTD_TIMECTR (964) +#define D_PEDSTD_DECI (768 + 0xA0) + +#define D_HOST_NO_MOT (976) +#define D_ACCEL_BIAS (660) + +#define D_ORIENT_GAP (76) + +#define D_TILT0_H (48) +#define D_TILT0_L (50) +#define D_TILT1_H (52) +#define D_TILT1_L (54) +#define D_TILT2_H (56) +#define D_TILT2_L (58) +#define D_TILT3_H (60) +#define D_TILT3_L (62) + +#define DMP_CODE_SIZE (3062) + +static const unsigned char dmp_memory[DMP_CODE_SIZE] = { + /* bank # 0 */ + 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, + 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, + 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, + 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, + 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, + 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, + 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, + 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, + 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, + 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, + 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 1 */ + 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, + 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, + 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, + 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, + 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, + 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, + 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, + /* bank # 2 */ + 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, + 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, + 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, + 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + /* bank # 4 */ + 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, + 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, + 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, + 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, + 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, + 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, + 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, + 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, + 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, + 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, + 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, + 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, + 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, + 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, + 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, + 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, + /* bank # 5 */ + 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, + 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, + 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, + 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, + 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, + 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, + 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, + 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, + 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, + 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, + 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, + 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, + 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, + 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, + 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, + 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, + /* bank # 6 */ + 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, + 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, + 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, + 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, + 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, + 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, + 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, + 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, + 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, + 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, + 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, + 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, + 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, + 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, + 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, + 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, + /* bank # 7 */ + 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, + 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, + 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, + 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, + 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, + 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, + 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, + 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, + 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, + 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, + 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, + 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, + 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, + 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, + 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, + 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, + /* bank # 8 */ + 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, + 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, + 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, + 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, + 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, + 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, + 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, + 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, + 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, + 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, + 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, + 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, + 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, + 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, + 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, + 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, + /* bank # 9 */ + 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, + 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, + 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, + 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, + 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, + 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, + 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, + 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, + 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, + 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, + 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, + 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, + 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, + 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, + 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, + 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, + /* bank # 10 */ + 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, + 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, + 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, + 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, + 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, + 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, + 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, + 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, + 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, + 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, + 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, + 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, + 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, + 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, + /* bank # 11 */ + 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, + 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, + 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, + 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, + 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, + 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, + 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, + 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, + 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, + 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, + 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, + 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, + 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, + 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, + 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, + 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff +}; + +static const unsigned short sStartAddress = 0x0400; + +/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ + +#define INT_SRC_TAP (0x01) +#define INT_SRC_ANDROID_ORIENT (0x08) + +#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ + DMP_FEATURE_SEND_CAL_GYRO) + +#define MAX_PACKET_LENGTH (32) + +#define DMP_SAMPLE_RATE (200) +#define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) + +#define FIFO_CORRUPTION_CHECK +#ifdef FIFO_CORRUPTION_CHECK +#define QUAT_ERROR_THRESH (1L<<24) +#define QUAT_MAG_SQ_NORMALIZED (1L<<28) +#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) +#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) +#endif + +struct dmp_s { + void (*tap_cb)(unsigned char count, unsigned char direction); + void (*android_orient_cb)(unsigned char orientation); + unsigned short orient; + unsigned short feature_mask; + unsigned short fifo_rate; + unsigned char packet_length; +}; + +static struct dmp_s dmp = { + .tap_cb = NULL, + .android_orient_cb = NULL, + .orient = 0, + .feature_mask = 0, + .fifo_rate = 0, + .packet_length = 0 +}; + +/** + * @brief Load the DMP with this image. + * @return 0 if successful. + */ +int dmp_load_motion_driver_firmware(void) +{ + return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, + DMP_SAMPLE_RATE); +} + +/** + * @brief Push gyro and accel orientation to the DMP. + * The orientation is represented here as the output of + * @e inv_orientation_matrix_to_scalar. + * @param[in] orient Gyro and accel orientation in body frame. + * @return 0 if successful. + */ +int dmp_set_orientation(unsigned short orient) +{ + unsigned char gyro_regs[3], accel_regs[3]; + const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; + const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; + const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; + const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; + + gyro_regs[0] = gyro_axes[orient & 3]; + gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; + gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; + accel_regs[0] = accel_axes[orient & 3]; + accel_regs[1] = accel_axes[(orient >> 3) & 3]; + accel_regs[2] = accel_axes[(orient >> 6) & 3]; + + /* Chip-to-body, axes only. */ + if (mpu_write_mem(FCFG_1, 3, gyro_regs)) + return -1; + if (mpu_write_mem(FCFG_2, 3, accel_regs)) + return -1; + + memcpy(gyro_regs, gyro_sign, 3); + memcpy(accel_regs, accel_sign, 3); + if (orient & 4) { + gyro_regs[0] |= 1; + accel_regs[0] |= 1; + } + if (orient & 0x20) { + gyro_regs[1] |= 1; + accel_regs[1] |= 1; + } + if (orient & 0x100) { + gyro_regs[2] |= 1; + accel_regs[2] |= 1; + } + + /* Chip-to-body, sign only. */ + if (mpu_write_mem(FCFG_3, 3, gyro_regs)) + return -1; + if (mpu_write_mem(FCFG_7, 3, accel_regs)) + return -1; + dmp.orient = orient; + return 0; +} + +/** + * @brief Push gyro biases to the DMP. + * Because the gyro integration is handled in the DMP, any gyro biases + * calculated by the MPL should be pushed down to DMP memory to remove + * 3-axis quaternion drift. + * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will + * overwrite the biases written to this location once a new one is computed. + * @param[in] bias Gyro biases in q16. + * @return 0 if successful. + */ +int dmp_set_gyro_bias(long *bias) +{ + long gyro_bias_body[3]; + unsigned char regs[4]; + + gyro_bias_body[0] = bias[dmp.orient & 3]; + if (dmp.orient & 4) + gyro_bias_body[0] *= -1; + gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; + if (dmp.orient & 0x20) + gyro_bias_body[1] *= -1; + gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; + if (dmp.orient & 0x100) + gyro_bias_body[2] *= -1; + +#ifdef EMPL_NO_64BIT + gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); + gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); + gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); +#else + gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); + gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); + gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); +#endif + + regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); + regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); + regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); + regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); + if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) + return -1; + + regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); + regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); + regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); + regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); + if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) + return -1; + + regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); + regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); + regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); + regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); + return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); +} + +/** + * @brief Push accel biases to the DMP. + * These biases will be removed from the DMP 6-axis quaternion. + * @param[in] bias Accel biases in q16. + * @return 0 if successful. + */ +int dmp_set_accel_bias(long *bias) +{ + long accel_bias_body[3]; + unsigned char regs[12]; + long long accel_sf; + unsigned short accel_sens; + + mpu_get_accel_sens(&accel_sens); + accel_sf = (long long)accel_sens << 15; + Asm("nop_s"); + + accel_bias_body[0] = bias[dmp.orient & 3]; + if (dmp.orient & 4) + accel_bias_body[0] *= -1; + accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; + if (dmp.orient & 0x20) + accel_bias_body[1] *= -1; + accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; + if (dmp.orient & 0x100) + accel_bias_body[2] *= -1; + +#ifdef EMPL_NO_64BIT + accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); + accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); + accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); +#else + accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); + accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); + accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); +#endif + + regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); + regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); + regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); + regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); + regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); + regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); + regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); + regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); + regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); + regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); + regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); + regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); + return mpu_write_mem(D_ACCEL_BIAS, 12, regs); +} + +/** + * @brief Set DMP output rate. + * Only used when DMP is on. + * @param[in] rate Desired fifo rate (Hz). + * @return 0 if successful. + */ +int dmp_set_fifo_rate(unsigned short rate) +{ + const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, + 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; + unsigned short div; + unsigned char tmp[8]; + + if (rate > DMP_SAMPLE_RATE) + return -1; + div = DMP_SAMPLE_RATE / rate - 1; + tmp[0] = (unsigned char)((div >> 8) & 0xFF); + tmp[1] = (unsigned char)(div & 0xFF); + if (mpu_write_mem(D_0_22, 2, tmp)) + return -1; + if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) + return -1; + + dmp.fifo_rate = rate; + return 0; +} + +/** + * @brief Get DMP output rate. + * @param[out] rate Current fifo rate (Hz). + * @return 0 if successful. + */ +int dmp_get_fifo_rate(unsigned short *rate) +{ + rate[0] = dmp.fifo_rate; + return 0; +} + +/** + * @brief Set tap threshold for a specific axis. + * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. + * @param[in] thresh Tap threshold, in mg/ms. + * @return 0 if successful. + */ +int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) +{ + unsigned char tmp[4], accel_fsr; + float scaled_thresh; + unsigned short dmp_thresh, dmp_thresh_2; + if (!(axis & TAP_XYZ) || thresh > 1600) + return -1; + + scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; + + mpu_get_accel_fsr(&accel_fsr); + switch (accel_fsr) { + case 2: + dmp_thresh = (unsigned short)(scaled_thresh * 16384); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); + break; + case 4: + dmp_thresh = (unsigned short)(scaled_thresh * 8192); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); + break; + case 8: + dmp_thresh = (unsigned short)(scaled_thresh * 4096); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); + break; + case 16: + dmp_thresh = (unsigned short)(scaled_thresh * 2048); + /* dmp_thresh * 0.75 */ + dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); + break; + default: + return -1; + } + tmp[0] = (unsigned char)(dmp_thresh >> 8); + tmp[1] = (unsigned char)(dmp_thresh & 0xFF); + tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); + tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); + + if (axis & TAP_X) { + if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) + return -1; + if (mpu_write_mem(D_1_36, 2, tmp+2)) + return -1; + } + if (axis & TAP_Y) { + if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) + return -1; + if (mpu_write_mem(D_1_40, 2, tmp+2)) + return -1; + } + if (axis & TAP_Z) { + if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) + return -1; + if (mpu_write_mem(D_1_44, 2, tmp+2)) + return -1; + } + return 0; +} + +/** + * @brief Set which axes will register a tap. + * @param[in] axis 1, 2, and 4 for XYZ, respectively. + * @return 0 if successful. + */ +int dmp_set_tap_axes(unsigned char axis) +{ + unsigned char tmp = 0; + + if (axis & TAP_X) + tmp |= 0x30; + if (axis & TAP_Y) + tmp |= 0x0C; + if (axis & TAP_Z) + tmp |= 0x03; + return mpu_write_mem(D_1_72, 1, &tmp); +} + +/** + * @brief Set minimum number of taps needed for an interrupt. + * @param[in] min_taps Minimum consecutive taps (1-4). + * @return 0 if successful. + */ +int dmp_set_tap_count(unsigned char min_taps) +{ + unsigned char tmp; + + if (min_taps < 1) + min_taps = 1; + else if (min_taps > 4) + min_taps = 4; + + tmp = min_taps - 1; + return mpu_write_mem(D_1_79, 1, &tmp); +} + +/** + * @brief Set length between valid taps. + * @param[in] time Milliseconds between taps. + * @return 0 if successful. + */ +int dmp_set_tap_time(unsigned short time) +{ + unsigned short dmp_time; + unsigned char tmp[2]; + + dmp_time = time / (1000 / DMP_SAMPLE_RATE); + tmp[0] = (unsigned char)(dmp_time >> 8); + tmp[1] = (unsigned char)(dmp_time & 0xFF); + return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); +} + +/** + * @brief Set max time between taps to register as a multi-tap. + * @param[in] time Max milliseconds between taps. + * @return 0 if successful. + */ +int dmp_set_tap_time_multi(unsigned short time) +{ + unsigned short dmp_time; + unsigned char tmp[2]; + + dmp_time = time / (1000 / DMP_SAMPLE_RATE); + tmp[0] = (unsigned char)(dmp_time >> 8); + tmp[1] = (unsigned char)(dmp_time & 0xFF); + return mpu_write_mem(D_1_218, 2, tmp); +} + +/** + * @brief Set shake rejection threshold. + * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. + * @param[in] sf Gyro scale factor. + * @param[in] thresh Gyro threshold in dps. + * @return 0 if successful. + */ +int dmp_set_shake_reject_thresh(long sf, unsigned short thresh) +{ + unsigned char tmp[4]; + long thresh_scaled = sf / 1000 * thresh; + tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); + tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); + tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); + tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); + return mpu_write_mem(D_1_92, 4, tmp); +} + +/** + * @brief Set shake rejection time. + * Sets the length of time that the gyro must be outside of the threshold set + * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory + * 60 ms is added to this parameter. + * @param[in] time Time in milliseconds. + * @return 0 if successful. + */ +int dmp_set_shake_reject_time(unsigned short time) +{ + unsigned char tmp[2]; + + time /= (1000 / DMP_SAMPLE_RATE); + tmp[0] = time >> 8; + tmp[1] = time & 0xFF; + return mpu_write_mem(D_1_90,2,tmp); +} + +/** + * @brief Set shake rejection timeout. + * Sets the length of time after a shake rejection that the gyro must stay + * inside of the threshold before taps can be detected again. A mandatory + * 60 ms is added to this parameter. + * @param[in] time Time in milliseconds. + * @return 0 if successful. + */ +int dmp_set_shake_reject_timeout(unsigned short time) +{ + unsigned char tmp[2]; + + time /= (1000 / DMP_SAMPLE_RATE); + tmp[0] = time >> 8; + tmp[1] = time & 0xFF; + return mpu_write_mem(D_1_88,2,tmp); +} + +/** + * @brief Get current step count. + * @param[out] count Number of steps detected. + * @return 0 if successful. + */ +int dmp_get_pedometer_step_count(unsigned long *count) +{ + unsigned char tmp[4]; + if (!count) + return -1; + + if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) + return -1; + + count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | + ((unsigned long)tmp[2] << 8) | tmp[3]; + return 0; +} + +/** + * @brief Overwrite current step count. + * WARNING: This function writes to DMP memory and could potentially encounter + * a race condition if called while the pedometer is enabled. + * @param[in] count New step count. + * @return 0 if successful. + */ +int dmp_set_pedometer_step_count(unsigned long count) +{ + unsigned char tmp[4]; + + tmp[0] = (unsigned char)((count >> 24) & 0xFF); + tmp[1] = (unsigned char)((count >> 16) & 0xFF); + tmp[2] = (unsigned char)((count >> 8) & 0xFF); + tmp[3] = (unsigned char)(count & 0xFF); + return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); +} + +/** + * @brief Get duration of walking time. + * @param[in] time Walk time in milliseconds. + * @return 0 if successful. + */ +int dmp_get_pedometer_walk_time(unsigned long *time) +{ + unsigned char tmp[4]; + if (!time) + return -1; + + if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) + return -1; + + time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | + ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; + return 0; +} + +/** + * @brief Overwrite current walk time. + * WARNING: This function writes to DMP memory and could potentially encounter + * a race condition if called while the pedometer is enabled. + * @param[in] time New walk time in milliseconds. + */ +int dmp_set_pedometer_walk_time(unsigned long time) +{ + unsigned char tmp[4]; + + time /= 20; + + tmp[0] = (unsigned char)((time >> 24) & 0xFF); + tmp[1] = (unsigned char)((time >> 16) & 0xFF); + tmp[2] = (unsigned char)((time >> 8) & 0xFF); + tmp[3] = (unsigned char)(time & 0xFF); + return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); +} + +/** + * @brief Enable DMP features. + * The following \#define's are used in the input mask: + * \n DMP_FEATURE_TAP + * \n DMP_FEATURE_ANDROID_ORIENT + * \n DMP_FEATURE_LP_QUAT + * \n DMP_FEATURE_6X_LP_QUAT + * \n DMP_FEATURE_GYRO_CAL + * \n DMP_FEATURE_SEND_RAW_ACCEL + * \n DMP_FEATURE_SEND_RAW_GYRO + * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually + * exclusive. + * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also + * mutually exclusive. + * @param[in] mask Mask of features to enable. + * @return 0 if successful. + */ +int dmp_enable_feature(unsigned short mask) +{ + unsigned char tmp[10]; + + /* TODO: All of these settings can probably be integrated into the default + * DMP image. + */ + /* Set integration scale factor. */ + tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); + tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); + tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); + tmp[3] = (unsigned char)(GYRO_SF & 0xFF); + mpu_write_mem(D_0_104, 4, tmp); + + /* Send sensor data to the FIFO. */ + tmp[0] = 0xA3; + if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { + tmp[1] = 0xC0; + tmp[2] = 0xC8; + tmp[3] = 0xC2; + } else { + tmp[1] = 0xA3; + tmp[2] = 0xA3; + tmp[3] = 0xA3; + } + if (mask & DMP_FEATURE_SEND_ANY_GYRO) { + tmp[4] = 0xC4; + tmp[5] = 0xCC; + tmp[6] = 0xC6; + } else { + tmp[4] = 0xA3; + tmp[5] = 0xA3; + tmp[6] = 0xA3; + } + tmp[7] = 0xA3; + tmp[8] = 0xA3; + tmp[9] = 0xA3; + mpu_write_mem(CFG_15,10,tmp); + + /* Send gesture data to the FIFO. */ + if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) + tmp[0] = DINA20; + else + tmp[0] = 0xD8; + mpu_write_mem(CFG_27,1,tmp); + + if (mask & DMP_FEATURE_GYRO_CAL) + dmp_enable_gyro_cal(1); + else + dmp_enable_gyro_cal(0); + + if (mask & DMP_FEATURE_SEND_ANY_GYRO) { + if (mask & DMP_FEATURE_SEND_CAL_GYRO) { + tmp[0] = 0xB2; + tmp[1] = 0x8B; + tmp[2] = 0xB6; + tmp[3] = 0x9B; + } else { + tmp[0] = DINAC0; + tmp[1] = DINA80; + tmp[2] = DINAC2; + tmp[3] = DINA90; + } + mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); + } + + if (mask & DMP_FEATURE_TAP) { + /* Enable tap. */ + tmp[0] = 0xF8; + mpu_write_mem(CFG_20, 1, tmp); + dmp_set_tap_thresh(TAP_XYZ, 250); + dmp_set_tap_axes(TAP_XYZ); + dmp_set_tap_count(1); + dmp_set_tap_time(100); + dmp_set_tap_time_multi(500); + + dmp_set_shake_reject_thresh(GYRO_SF, 200); + dmp_set_shake_reject_time(40); + dmp_set_shake_reject_timeout(10); + } else { + tmp[0] = 0xD8; + mpu_write_mem(CFG_20, 1, tmp); + } + + if (mask & DMP_FEATURE_ANDROID_ORIENT) { + tmp[0] = 0xD9; + } else + tmp[0] = 0xD8; + mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); + + if (mask & DMP_FEATURE_LP_QUAT) + dmp_enable_lp_quat(1); + else + dmp_enable_lp_quat(0); + + if (mask & DMP_FEATURE_6X_LP_QUAT) + dmp_enable_6x_lp_quat(1); + else + dmp_enable_6x_lp_quat(0); + + /* Pedometer is always enabled. */ + dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; + mpu_reset_fifo(); + + dmp.packet_length = 0; + if (mask & DMP_FEATURE_SEND_RAW_ACCEL) + dmp.packet_length += 6; + if (mask & DMP_FEATURE_SEND_ANY_GYRO) + dmp.packet_length += 6; + if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) + dmp.packet_length += 16; + if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) + dmp.packet_length += 4; + + return 0; +} + +/** + * @brief Get list of currently enabled DMP features. + * @param[out] Mask of enabled features. + * @return 0 if successful. + */ +int dmp_get_enabled_features(unsigned short *mask) +{ + mask[0] = dmp.feature_mask; + return 0; +} + +/** + * @brief Calibrate the gyro data in the DMP. + * After eight seconds of no motion, the DMP will compute gyro biases and + * subtract them from the quaternion output. If @e dmp_enable_feature is + * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be + * subtracted from the gyro output. + * @param[in] enable 1 to enable gyro calibration. + * @return 0 if successful. + */ +int dmp_enable_gyro_cal(unsigned char enable) +{ + if (enable) { + unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; + return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); + } else { + unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; + return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); + } +} + +/** + * @brief Generate 3-axis quaternions from the DMP. + * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually + * exclusive. + * @param[in] enable 1 to enable 3-axis quaternion. + * @return 0 if successful. + */ +int dmp_enable_lp_quat(unsigned char enable) +{ + unsigned char regs[4]; + if (enable) { + regs[0] = DINBC0; + regs[1] = DINBC2; + regs[2] = DINBC4; + regs[3] = DINBC6; + } + else + memset(regs, 0x8B, 4); + + mpu_write_mem(CFG_LP_QUAT, 4, regs); + + return mpu_reset_fifo(); +} + +/** + * @brief Generate 6-axis quaternions from the DMP. + * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually + * exclusive. + * @param[in] enable 1 to enable 6-axis quaternion. + * @return 0 if successful. + */ +int dmp_enable_6x_lp_quat(unsigned char enable) +{ + unsigned char regs[4]; + if (enable) { + regs[0] = DINA20; + regs[1] = DINA28; + regs[2] = DINA30; + regs[3] = DINA38; + } else + memset(regs, 0xA3, 4); + + mpu_write_mem(CFG_8, 4, regs); + + return mpu_reset_fifo(); +} + +/** + * @brief Decode the four-byte gesture data and execute any callbacks. + * @param[in] gesture Gesture data from DMP packet. + * @return 0 if successful. + */ +static int decode_gesture(unsigned char *gesture) +{ + unsigned char tap, android_orient; + + android_orient = gesture[3] & 0xC0; + tap = 0x3F & gesture[3]; + + if (gesture[1] & INT_SRC_TAP) { + unsigned char direction, count; + direction = tap >> 3; + count = (tap % 8) + 1; + if (dmp.tap_cb) + dmp.tap_cb(direction, count); + } + + if (gesture[1] & INT_SRC_ANDROID_ORIENT) { + if (dmp.android_orient_cb) + dmp.android_orient_cb(android_orient >> 6); + } + + return 0; +} + +/** + * @brief Specify when a DMP interrupt should occur. + * A DMP interrupt can be configured to trigger on either of the two + * conditions below: + * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). + * \n b. A tap event has been detected. + * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. + * @return 0 if successful. + */ +int dmp_set_interrupt_mode(unsigned char mode) +{ + const unsigned char regs_continuous[11] = + {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; + const unsigned char regs_gesture[11] = + {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; + + switch (mode) { + case DMP_INT_CONTINUOUS: + return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, + (unsigned char*)regs_continuous); + case DMP_INT_GESTURE: + return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, + (unsigned char*)regs_gesture); + default: + return -1; + } +} + +/** + * @brief Get one packet from the FIFO. + * If @e sensors does not contain a particular sensor, disregard the data + * returned to that pointer. + * \n @e sensors can contain a combination of the following flags: + * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO + * \n INV_XYZ_GYRO + * \n INV_XYZ_ACCEL + * \n INV_WXYZ_QUAT + * \n If the FIFO has no new data, @e sensors will be zero. + * \n If the FIFO is disabled, @e sensors will be zero and this function will + * return a non-zero error code. + * @param[out] gyro Gyro data in hardware units. + * @param[out] accel Accel data in hardware units. + * @param[out] quat 3-axis quaternion data in hardware units. + * @param[out] timestamp Timestamp in milliseconds. + * @param[out] sensors Mask of sensors read from FIFO. + * @param[out] more Number of remaining packets. + * @return 0 if successful. + */ +int dmp_read_fifo(short *gyro, short *accel, long *quat, + unsigned long *timestamp, short *sensors, unsigned char *more) +{ + unsigned char fifo_data[MAX_PACKET_LENGTH]; + unsigned char ii = 0; + + /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can + * cache this value and save some cycles. + */ + sensors[0] = 0; + + /* Get a packet. */ + if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) + return -1; + + /* Parse DMP packet. */ + if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { +#ifdef FIFO_CORRUPTION_CHECK + long quat_q14[4], quat_mag_sq; +#endif + quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | + ((long)fifo_data[2] << 8) | fifo_data[3]; + quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | + ((long)fifo_data[6] << 8) | fifo_data[7]; + quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | + ((long)fifo_data[10] << 8) | fifo_data[11]; + quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | + ((long)fifo_data[14] << 8) | fifo_data[15]; + ii += 16; +#ifdef FIFO_CORRUPTION_CHECK + /* We can detect a corrupted FIFO by monitoring the quaternion data and + * ensuring that the magnitude is always normalized to one. This + * shouldn't happen in normal operation, but if an I2C error occurs, + * the FIFO reads might become misaligned. + * + * Let's start by scaling down the quaternion data to avoid long long + * math. + */ + quat_q14[0] = quat[0] >> 16; + quat_q14[1] = quat[1] >> 16; + quat_q14[2] = quat[2] >> 16; + quat_q14[3] = quat[3] >> 16; + quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + + quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; + if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || + (quat_mag_sq > QUAT_MAG_SQ_MAX)) { + /* Quaternion is outside of the acceptable threshold. */ + mpu_reset_fifo(); + sensors[0] = 0; + return -1; + } + sensors[0] |= INV_WXYZ_QUAT; +#endif + } + + if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { + accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; + accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; + accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; + ii += 6; + sensors[0] |= INV_XYZ_ACCEL; + } + + if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { + gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; + gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; + gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; + ii += 6; + sensors[0] |= INV_XYZ_GYRO; + } + + /* Gesture data is at the end of the DMP packet. Parse it and call + * the gesture callbacks (if registered). + */ + if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) + decode_gesture(fifo_data + ii); + + get_ms(timestamp); + return 0; +} + +/** + * @brief Register a function to be executed on a tap event. + * The tap direction is represented by one of the following: + * \n TAP_X_UP + * \n TAP_X_DOWN + * \n TAP_Y_UP + * \n TAP_Y_DOWN + * \n TAP_Z_UP + * \n TAP_Z_DOWN + * @param[in] func Callback function. + * @return 0 if successful. + */ +int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) +{ + dmp.tap_cb = func; + return 0; +} + +/** + * @brief Register a function to be executed on a android orientation event. + * @param[in] func Callback function. + * @return 0 if successful. + */ +int dmp_register_android_orient_cb(void (*func)(unsigned char)) +{ + dmp.android_orient_cb = func; + return 0; +} + +/** + * @} + */ + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu_dmp_motion_driver.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu_dmp_motion_driver.h new file mode 100644 index 00000000..352e9e8e --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/dmp_mpu6050/inv_mpu_dmp_motion_driver.h @@ -0,0 +1,97 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @addtogroup DRIVERS Sensor Driver Layer + * @brief Hardware drivers to communicate with sensors via I2C. + * + * @{ + * @file inv_mpu_dmp_motion_driver.h + * @brief DMP image and interface functions. + * @details All functions are preceded by the dmp_ prefix to + * differentiate among MPL and general driver function calls. + */ +#ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ +#define _INV_MPU_DMP_MOTION_DRIVER_H_ + +#define TAP_X (0x01) +#define TAP_Y (0x02) +#define TAP_Z (0x04) +#define TAP_XYZ (0x07) + +#define TAP_X_UP (0x01) +#define TAP_X_DOWN (0x02) +#define TAP_Y_UP (0x03) +#define TAP_Y_DOWN (0x04) +#define TAP_Z_UP (0x05) +#define TAP_Z_DOWN (0x06) + +#define ANDROID_ORIENT_PORTRAIT (0x00) +#define ANDROID_ORIENT_LANDSCAPE (0x01) +#define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02) +#define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03) + +#define DMP_INT_GESTURE (0x01) +#define DMP_INT_CONTINUOUS (0x02) + +#define DMP_FEATURE_TAP (0x001) +#define DMP_FEATURE_ANDROID_ORIENT (0x002) +#define DMP_FEATURE_LP_QUAT (0x004) +#define DMP_FEATURE_PEDOMETER (0x008) +#define DMP_FEATURE_6X_LP_QUAT (0x010) +#define DMP_FEATURE_GYRO_CAL (0x020) +#define DMP_FEATURE_SEND_RAW_ACCEL (0x040) +#define DMP_FEATURE_SEND_RAW_GYRO (0x080) +#define DMP_FEATURE_SEND_CAL_GYRO (0x100) + +#define INV_WXYZ_QUAT (0x100) + +/* Set up functions. */ +int dmp_load_motion_driver_firmware(void); +int dmp_set_fifo_rate(unsigned short rate); +int dmp_get_fifo_rate(unsigned short *rate); +int dmp_enable_feature(unsigned short mask); +int dmp_get_enabled_features(unsigned short *mask); +int dmp_set_interrupt_mode(unsigned char mode); +int dmp_set_orientation(unsigned short orient); +int dmp_set_gyro_bias(long *bias); +int dmp_set_accel_bias(long *bias); + +/* Tap functions. */ +int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)); +int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh); +int dmp_set_tap_axes(unsigned char axis); +int dmp_set_tap_count(unsigned char min_taps); +int dmp_set_tap_time(unsigned short time); +int dmp_set_tap_time_multi(unsigned short time); +int dmp_set_shake_reject_thresh(long sf, unsigned short thresh); +int dmp_set_shake_reject_time(unsigned short time); +int dmp_set_shake_reject_timeout(unsigned short time); + +/* Android orientation functions. */ +int dmp_register_android_orient_cb(void (*func)(unsigned char)); + +/* LP quaternion functions. */ +int dmp_enable_lp_quat(unsigned char enable); +int dmp_enable_6x_lp_quat(unsigned char enable); + +/* Pedometer functions. */ +int dmp_get_pedometer_step_count(unsigned long *count); +int dmp_set_pedometer_step_count(unsigned long count); +int dmp_get_pedometer_walk_time(unsigned long *time); +int dmp_set_pedometer_walk_time(unsigned long time); + +/* DMP gyro calibration functions. */ +int dmp_enable_gyro_cal(unsigned char enable); + +/* Read function. This function should be called whenever the MPU interrupt is + * detected. + */ +int dmp_read_fifo(short *gyro, short *accel, long *quat, + unsigned long *timestamp, short *sensors, unsigned char *more); + +#endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */ + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/imu.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/imu.c new file mode 100644 index 00000000..9c859b10 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/imu.c @@ -0,0 +1,172 @@ +/* embARC HAL */ +#include "embARC.h" +#include "embARC_debug.h" + +/* custom HAL */ +#include "imu.h" + + +/* MPU6050 registers */ +#define MPU6050_REG_SAMPLE_RATE 0x19 /* Gyro sample rate */ +#define MPU6050_REG_EXT_SYNC_DISABLE 0x1A /* Ext sync disable */ +#define MPU6050_REG_GYRO_FULL_SCALE 0x1B /* Gyro full scale */ +#define MPU6050_REG_ACCEL_FULL_SCALE 0x1C /* Accel full scale */ +#define MPU6050_REG_POWER_MANAGEMENT 0x6B /* Power management */ +#define MPU6050_REG_ACCEL_XOUT_H 0x3B /* Accel_x out[15:8] */ +#define MPU6050_REG_ACCEL_XOUT_L 0x3C /* Accel_x out[7:1] */ +#define MPU6050_REG_ACCEL_YOUT_H 0x3D /* Accel_y out[15:8] */ +#define MPU6050_REG_ACCEL_YOUT_L 0x3E /* Accel_y out[7:1] */ +#define MPU6050_REG_ACCEL_ZOUT_H 0x3F /* Accel_z out[15:8] */ +#define MPU6050_REG_ACCEL_ZOUT_L 0x40 /* Accel_z out[7:1] */ +#define MPU6050_REG_TEMP_OUT_H 0x41 /* Temp out[15:8] */ +#define MPU6050_REG_TEMP_OUT_L 0x42 /* Temp out[7:1] */ +#define MPU6050_REG_GYRO_XOUT_H 0x43 /* Gyro_x out[15:8] */ +#define MPU6050_REG_GYRO_XOUT_L 0x44 /* Gyro_x out[7:1] */ +#define MPU6050_REG_GYRO_YOUT_H 0x45 /* Gyro_y out[15:8] */ +#define MPU6050_REG_GYRO_YOUT_L 0x46 /* Gyro_y out[7:1] */ +#define MPU6050_REG_GYRO_ZOUT_H 0x47 /* Gyro_z out[15:8] */ +#define MPU6050_REG_GYRO_ZOUT_L 0x48 /* Gyro_z out[7:1] */ + +/* imu data received buffer */ +union _imuData +{ + char buf[14]; + //char buf[14]; + struct + { + char axh, axl, ayh, ayl, azh, azl; + char th, tl; + char gxh, gxl, gyh, gyl, gzh, gzl; + }; +} imuData; + +/* configure related register, using the auto increase function */ +static uint8_t imu_init_seq0[] = { + MPU6050_REG_SAMPLE_RATE, + 0x00, /* 0x19: sr = gyro rate / 1 */ + 0x01, /* 0x1A: ext sync disable, dlpf = 1(accl 1k, gyro 1k) */ + 0x18, /* 0x1B: gyro fs = 2k deg/sec (34.6/s)*/ + 0x00 /* 0x1C: accl fs = 2g (19.6m/sq.s) */ +}; + +static uint8_t imu_init_seq1[] = { + MPU6050_REG_POWER_MANAGEMENT, + 0x00 /* 0x6B: no sleep */ +}; + +static DEV_IIC *emsk_imu_sensor; /* IMU6050 sensor object */ + +/** + * \brief write mpu6050 register + * \param *seq register address and value to be written + * \param len the length of seq + * \retval >=0 write success, return bytes written + * \retval !E_OK write failed + */ +static int32_t mpu6050_reg_write(uint8_t *seq, uint8_t len, uint8_t i2cId) +{ + int32_t ercd = E_PAR; + + emsk_imu_sensor = iic_get_dev(i2cId); + + EMSK_IMU_SENSOR_CHECK_EXP_NORTN(emsk_imu_sensor != NULL); + + /* make sure set the imu sensor's slave address */ + emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_TAR_ADDR, CONV2VOID(IMU_SENSOR_ADDR)); + + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_STOP)); + ercd = emsk_imu_sensor->iic_write(seq, len); + +error_exit: + return ercd; +} + +/** + * \brief read mpu6050 register + * \param seq register address + * \param *val returned value + * \param len the length of returned value + * \retval >=0 read success, return bytes read + * \retval !E_OK read failed + */ +static int32_t mpu6050_reg_read(uint8_t seq, uint8_t *val, uint8_t len, uint8_t i2cId) +{ + int32_t ercd = E_PAR; + + emsk_imu_sensor = iic_get_dev(i2cId); + + EMSK_IMU_SENSOR_CHECK_EXP_NORTN(emsk_imu_sensor != NULL); + + /** make sure set the temp sensor's slave address */ + emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_TAR_ADDR, CONV2VOID(IMU_SENSOR_ADDR)); + /* write register address then read register value */ + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_RESTART)); + + /* send data reading request to mpu6050 */ + ercd = emsk_imu_sensor->iic_write(&seq, 1); + + emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_NEXT_COND, CONV2VOID(IIC_MODE_STOP)); + /* read len data from mpu6050 */ + ercd = emsk_imu_sensor->iic_read(val, len); + +error_exit: + return ercd; +} + +/** + * \brief imu sensor initialize + * \retval E_OK initialize success + * \retval !E_OK initialize failed + */ +int imuInit(uint8_t i2cId) +{ + int ercd = E_OK; + + emsk_imu_sensor = iic_get_dev(i2cId); + + EMSK_IMU_SENSOR_CHECK_EXP_NORTN(emsk_imu_sensor != NULL); + + /* the device working as master */ + ercd = emsk_imu_sensor->iic_open(DEV_MASTER_MODE, IIC_SPEED_FAST); + if ((ercd == E_OK) || (ercd == E_OPNED)) { + ercd = emsk_imu_sensor->iic_control(IIC_CMD_MST_SET_TAR_ADDR, CONV2VOID(IMU_SENSOR_ADDR)); + + /* write value to mpu6050 to set registers */ + mpu6050_reg_write(imu_init_seq1, 2, i2cId); + mpu6050_reg_write(imu_init_seq0, 5, i2cId); + } + +error_exit: + return ercd; +} + +/** + * \brief read imu sensor value + * \param receive imu data(raw value) + * \retval E_OK read success + * \retval !E_OK read failed + */ +int imuGetValues(ImuValues* imuVal, uint8_t i2cId) +{ + int ercd = E_OK; + + /* read 14 data from mpu6050 */ + ercd = mpu6050_reg_read(MPU6050_REG_ACCEL_XOUT_H, imuData.buf, 14, i2cId); + + if (ercd != 14) { + ercd = E_OBJ; + } else { + imuVal->acclX = (short)((imuData.axh << 8) | imuData.axl); + imuVal->acclY = (short)((imuData.ayh << 8) | imuData.ayl); + imuVal->acclZ = (short)((imuData.azh << 8) | imuData.azl); + + imuVal->angvX = (short)((imuData.gxh << 8) | imuData.gxl); + imuVal->angvY = (short)((imuData.gyh << 8) | imuData.gyl); + imuVal->angvZ = (short)((imuData.gzh << 8) | imuData.gzl); + + ercd = E_OK; + } + +error_exit: + return ercd; +} diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/imu.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/imu.h new file mode 100644 index 00000000..474e93a3 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/imu.h @@ -0,0 +1,32 @@ +#ifndef _IMU_H_ +#define _IMU_H_ + +/*!< using IIC 1 interface */ +#define IMU_VERTICAL_IIC_ID DW_IIC_0_ID +#define IMU_HORIZONTAL_IIC_ID DW_IIC_1_ID + +/** + * I2C address of PmodIMU2 can be selected via AD0: 0x68, 0x69 + * AD0 connect to GND is 0x68 + */ +/*!< MPU6050 address */ +#define MPU6050_A0_PIN 0 /*!< I2C Serial Bus Address Selection Pin */ +#define MPU6050_ADDRESS (0x68 + MPU6050_A0_PIN) + +/*!< imu sensor address */ +#define IMU_SENSOR_ADDR MPU6050_ADDRESS + +#define EMSK_IMU_SENSOR_CHECK_EXP_NORTN(EXPR) CHECK_EXP_NOERCD(EXPR, error_exit) + +/*!< struct for imu data storaged */ +typedef struct __imuValues +{ + short acclX, acclY, acclZ; + short angvX, angvY, angvZ; +} ImuValues; + +int imuInit(uint8_t i2cId); +int imuGetValues(ImuValues* imuVal, uint8_t i2cId); + +#endif /* _IMU_H_ */ + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/spi.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/spi.c new file mode 100644 index 00000000..00d79ee2 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/spi.c @@ -0,0 +1,213 @@ +/* + *Protocols + *CPLD: 16bits datas continuous transfer + *EMSK: 8bits datas transfer, alternating reader and writing + */ + +#include "embARC.h" +#include "embARC_debug.h" +#include "spi.h" + +/* + * 16bits data, upper 3-bit : command + * ---------------------------------- + * 15 | 1/0 -> start transfer + * ------|--------------------------- + * 14 | 1/0 -> qei/pwm + * ------|--------------------------- + * 13 | 1/0 -> L/R + * ------|--------------------------- + * 12-0 | data + * ---------------------------------- + */ +#define QEI_L 0xC0 +#define QEI_R 0xE0 +#define PWM_L 0x80 +#define PWM_R 0xA0 + +#define SPI_ID DW_SPI_0_ID +#define CS_PORT DEV_GPIO_PORT_A +#define CS_PIN DEV_GPIO_PIN_28 +#define CS_MASK 1<spi_open(DEV_MASTER_MODE, 1000000); + + if (ercd != E_OK && ercd != E_OPNED) { + return ercd; + } + + /* + * Set designware spi device data frame size:16 bits + * However struct dev_spi_transfer tx_buf,rx_buf -> uint8_t + * it doesn't work as expected + */ + //spi->spi_control(SPI_CMD_SET_DFS, CONV2VOID(16)); + spi->spi_control(SPI_CMD_SET_CLK_MODE, CONV2VOID(SPI_CPOL_0_CPHA_1)); + /* + * avoid starting transmission + */ + spi->spi_control(SPI_CMD_SET_DUMMY_DATA, CONV2VOID(0x00)); + + return E_OK; +} + +int32_t gpio_open(void) +{ + int32_t ercd = 0; + + gpio = gpio_get_dev(CS_PORT); + configASSERT(gpio != NULL); + + ercd = gpio->gpio_open(CS_MASK | (1<gpio_control(GPIO_CMD_SET_BIT_DIR_OUTPUT, (void *)(CS_MASK | (1<gpio_write(CS_MASK, CS_MASK); + + return E_OK; +} + +static void spi_select(void) +{ + /*select spi slave device*/ + spi->spi_control(SPI_CMD_MST_SEL_DEV, CONV2VOID(EMSK_SPI_LINE_0)); + /* write 0 to CS pin, pull-down */ + gpio->gpio_write((~CS_MASK), CS_MASK); +} + +static void spi_deselect(void) +{ + /* write 1 to CS pin, pull-up */ + gpio->gpio_write(CS_MASK, CS_MASK); + /*deselect spi slave device*/ + spi->spi_control(SPI_CMD_MST_DSEL_DEV, CONV2VOID(EMSK_SPI_LINE_0)); +} + +/* + * \brief read qei[2](16bits)(continuous 2x8bits) + * \param array to store data + * \retval spi status + */ + +// int32_t spi_read_qei(int16_t *qei) +int32_t spi_read_qei(int16_t *qei, int dir) +{ + int32_t ercd = 0; + static uint64_t cnt; + + DEV_SPI_TRANSFER xfer; + char qeiTmp[2]={0, 0}; + unsigned int cs_cpu_status; + + DEV_SPI_XFER_SET_TXBUF(&xfer, NULL, 0, 0); + DEV_SPI_XFER_SET_RXBUF(&xfer, qeiTmp, 0, 2); + DEV_SPI_XFER_SET_NEXT(&xfer, NULL); + + spi_select(); + + switch(dir){ + case 0: + spi->spi_control(SPI_CMD_SET_DUMMY_DATA, CONV2VOID(QEI_L)); + break; + case 1: + spi->spi_control(SPI_CMD_SET_DUMMY_DATA, CONV2VOID(QEI_R)); + break; + } + ercd = spi->spi_control(SPI_CMD_TRANSFER_POLLING, CONV2VOID(&xfer)); + + spi_deselect(); + + *qei = (qeiTmp[0] << 8) | (qeiTmp[1] & 0xFF); + if(*qei & 0x0800) *qei = *qei | 0xF000; + + return ercd; + +} + +/* + * \brief write pwm[2](data:signed 12bits) (split 4x8bits datas) + Format: + ----------------------------------------- + L | data[0] | data[1] + -----|---------------------|-------------- + | pwm_l | (pwm[0].hi) | (pwm[1].lo) + -----|---------------------|-------------- + R | data[0] | data[1] + -----|---------------------|-------------- + | pwm_r | (pwm[1].hi) | (pwm[2].lo) + ----------------------------------------- + * \param array to transfer + * \retval spi status + */ + +int32_t spi_write_pwm(int16_t *pwm, int dir) +{ + int32_t ercd = 0; + + DEV_SPI_TRANSFER xfer; + + int8_t local_buf[2]; + + switch(dir){ + /* + * convert pwm to 8bits + */ + case 0: + local_buf[0] = (int8_t)((pwm[0] >> 8) & 0x0F | PWM_L); + local_buf[1] = (int8_t)((pwm[0] << 8) >> 8); + break; + case 1: + local_buf[0] = (int8_t)((pwm[0] >> 8) & 0x0F | PWM_R); + local_buf[1] = (int8_t)((pwm[0] << 8) >> 8); + break; + + } + + DEV_SPI_XFER_SET_TXBUF(&xfer, local_buf, 0, 2); + DEV_SPI_XFER_SET_RXBUF(&xfer, NULL, 2, 0); + DEV_SPI_XFER_SET_NEXT(&xfer, NULL); + + spi_select(); + + ercd = spi->spi_control(SPI_CMD_TRANSFER_POLLING, CONV2VOID(&xfer)); + + spi_deselect(); + + return ercd; + +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/spi.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/spi.h new file mode 100644 index 00000000..ef0f724f --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/spi.h @@ -0,0 +1,7 @@ +#ifndef __SPI_H_ +#define __SPI_H_ + +int32_t cpld_spi_init(void); +int32_t spi_read_qei(int16_t *qei, int dir); +int32_t spi_write_pwm(int16_t *pwm, int dir); +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/timer.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/timer.c new file mode 100644 index 00000000..9786c5d3 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/timer.c @@ -0,0 +1,48 @@ +/* embARC HAL */ +#include "embARC.h" +#include "embARC_debug.h" + +#ifndef BASE_TIMER_HZ +#define BASE_TIMER_HZ (500) +#endif + +/** Tick semaphore for motorTask */ +SemaphoreHandle_t SemMotTick; + +/** interrupt reset count */ +static uint32_t cyc_hz_count = (BOARD_CPU_CLOCK / BASE_TIMER_HZ); + +/** + * \brief timer interrupt. + * the Interrupt frequency is based on the defined \ref BASE_TIMER_HZ + */ +static void base_timer_isr(void *ptr) +{ + BaseType_t rtn, Wake; + timer_int_clear(TIMER_1); + + /** 2ms give a semaphore */ + rtn = xSemaphoreGiveFromISR(SemMotTick, &Wake); + configASSERT((rtn == pdPASS) || (rtn == errQUEUE_FULL)); +} + +/** + * \brief init timer and interrupt + * \details + * This function is called in \ref motorTask, and + * it initializes the 2-MS timer interrupt + */ +void base_timer_init(void) +{ + + SemMotTick = xSemaphoreCreateBinary(); + configASSERT(SemMotTick != NULL); + + if (timer_present(TIMER_1)) { + int_disable(INTNO_TIMER1); /* disable first then enable */ + int_handler_install(INTNO_TIMER1, base_timer_isr); + timer_start(TIMER_1, TIMER_CTRL_IE|TIMER_CTRL_NH, cyc_hz_count); /* start 2ms timer interrupt */ + + int_enable(INTNO_TIMER1); + } +} diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/timer.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/timer.h new file mode 100644 index 00000000..96449f71 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/drivers/timer.h @@ -0,0 +1,8 @@ +#ifndef _TIMER_H_ +#define _TIMER_H_ + +extern SemaphoreHandle_t SemMotTick; +static void base_timer_isr(void *ptr); +void base_timer_init(void); + +#endif \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/lqr/lqr.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/lqr/lqr.c new file mode 100644 index 00000000..f0905a5c --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/lqr/lqr.c @@ -0,0 +1,17 @@ +#include "lqr.h" +#include "physParams.h" + +// void lqrParamCorr(lqr *lqr, lqrParam *K) +// { +// lqr->k1 = K->k1; +// lqr->k2 = K->k2; +// lqr->k3 = K->k3; +// lqr->k4 = K->k4; +// } + +float calc(lqr *lqr, float pos, float vel, float ang, float av) +{ + float vol; + vol = lqr->k1 * pos + lqr->k2 * vel + lqr->k3 * ang + lqr->k4 * av; + return vol; +} \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/lqr/lqr.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/lqr/lqr.h new file mode 100644 index 00000000..ab771871 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/lqr/lqr.h @@ -0,0 +1,14 @@ +#ifndef _LQR_H_ +#define _LQR_H_ + +#include "physParams.h" + +typedef struct +{ + float k1, k2, k3, k4; +}lqr; + +//void lqrParamCorr(lqr *lqr, lqrParam *K); +float calc(lqr *lqr, float pos, float vel, float ang, float av); + +#endif /* LQR_H_ */ \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/main.c b/arc_design_contest/2018/HUST_inverted_pendulum/scr/main.c new file mode 100644 index 00000000..ef2892d8 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/main.c @@ -0,0 +1,70 @@ +/* ------------------------------------------ + * Copyright (c) 2017, Synopsys, Inc. All rights reserved. + + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + + * 1) Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + + * 2) Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + + * 3) Neither the name of the Synopsys, Inc., nor the names of its contributors may + * be used to endorse or promote products derived from this software without + * specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON + * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +--------------------------------------------- */ + +/** + * \defgroup EMBARC_APP_FREERTOS_KERNEL embARC FreeRTOS Kernel Example + * \ingroup EMBARC_APPS_TOTAL + * \ingroup EMBARC_APPS_OS_FREERTOS + * \brief embARC Example for testing FreeRTOS task switch and interrupt/exception handling + * + * \details + * ### Extra Required Tools + * + * ### Extra Required Peripherals + * + * ### Design Concept + * This example is designed to show the functionality of FreeRTOS. + * + * ### Usage Manual + * Test case for show how FreeRTOS is working by task switching and interrupt/exception processing. + * ![ScreenShot of freertos-demo under freertos](pic/images/example/emsk/emsk_freertos_demo.jpg) + * + * ### Extra Comments + * + */ + +#include "embARC.h" +#include "embARC_debug.h" +#include + +#include "TskTop.h" + +/** + * \brief call FreeRTOS API, create and start tasks + */ +int main(void) +{ + + topInit(); + vTaskStartScheduler(); + return 0; + +} + diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/makefile b/arc_design_contest/2018/HUST_inverted_pendulum/scr/makefile new file mode 100644 index 00000000..9ff92cb5 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/makefile @@ -0,0 +1,27 @@ +# Application name +APPL ?= invert-pendulum-dsp + +# Selected OS +OS_SEL ?= freertos + +# +# root dir of embARC +# +EMBARC_ROOT = ../../.. + +MID_SEL = common fatfs + +# application source dirs +APPL_CSRC_DIR = . ./TskDbg ./Queue ./TskMotor ./drivers ./TskTop ./FastMath_Table +APPL_ASMSRC_DIR = . + +# application include dirs +APPL_INC_DIR = . ./TskDbg ./Queue ./TskTop ./drivers ./TskMotor ./FastMath_Table + +# include current project makefile +COMMON_COMPILE_PREREQUISITES += makefile + +### Options above must be added before include options.mk ### +# include key embARC build system makefile +override EMBARC_ROOT := $(strip $(subst \,/,$(EMBARC_ROOT))) +include $(EMBARC_ROOT)/options/options.mk \ No newline at end of file diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/physParams.h b/arc_design_contest/2018/HUST_inverted_pendulum/scr/physParams.h new file mode 100644 index 00000000..a0bdd6a5 --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/physParams.h @@ -0,0 +1,70 @@ +#ifndef USER_PHYSPARAMS_H +#define USER_PHYSPARAMS_H + +#define SPDSEQLENMAX 512 +#define ANGSEQLENMAX 1024 + +/** control period*/ +#define Ts 0.002f +#define TS_RECI 500 /** 1/Ts*/ + +/** EncUnit = 0.067 * pi / 15000 / 4 / 0.002 */ +#define EncUnit 0.067f * 3.14159f / 0.002f / 60000.f +/*!< imu sensor data convert coefficient */ +#define GyroUnit 1.0642252e-3f; // rad/s/LSB @ fs=2000deg/s +#define AcclUnit 5.9855042e-4f; // m/sq.s/LSB @ fs=2g + +/* + * LPF params << 10 + * y(n) = (1-a) * y(n-1) + a * x + * float q1.7 + * set a -1~0.99902 -> -512 ~ 511 + */ +#define LPF_FACTOR 128 + +/** kalman params << 15 */ +#define A 32768 /** 1 << 15 */ +#define A_TRAN 32768 /** 1 << 15 */ +#define B 66 /** (1 << 15) * Ts */ +#define H 32768 /** 1 << 15 */ +#define H_TRAN 32768 /** 1 << 15 */ +#define I 32768 /** 1 << 15 */ + +/** lqr params << 21 */ +#define GYRO_FACTOR 2232 /** (1 << 21) * GyroUnit */ +#define ANG_FACTOR 2097152 /** (1 << 21)) */ +#define ENCL_FACTOR 3679 /** (1 << 21)) * EncUnit */ +#define ENCR_FACTOR 3679 /** (1 << 21)) * EncUnit */ + +/* + * 1-order kalman + */ +typedef struct +{ + //int16_t A, B, H (constant, A = 1, B = Ts, H = 1) + int Q, R, K; + int xpri, Ppri; + int xpos, Ppos; +}Kalman1Var; + +typedef struct +{ + int k1, k2, k3, k4; +}lqr_q; + +typedef struct +{ + int p, i, d, n; + int accI, accD; +}Pid; + +typedef struct{ + int Velocity; + int Timeout; + int Acc; + int Omega; + int ThetaZ; + int ThetaY; +}VelOmega; + +#endif diff --git a/arc_design_contest/2018/HUST_inverted_pendulum/scr/readme.md b/arc_design_contest/2018/HUST_inverted_pendulum/scr/readme.md new file mode 100644 index 00000000..5c6023dd --- /dev/null +++ b/arc_design_contest/2018/HUST_inverted_pendulum/scr/readme.md @@ -0,0 +1,2 @@ +Version Log +- 5.3 install imu rotate 90° ( z-axis ) so accl Y,Z -> X,Z gyro X -> Y diff --git a/arc_design_contest/2018/README.md b/arc_design_contest/2018/README.md deleted file mode 100644 index e69de29b..00000000