aleks/hoverboard-sideboard-hack-GD
1
0
Fork 0
mirror of https://github.com/EFeru/hoverboard-sideboard-hack-GD.git synced 2025-07-26 17:19:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added Platformio support

Browse Source 
- firmware can now be built in Platformio too
- minor bug fixes
- added LED board picture
This commit is contained in:
EmanuelFeru 2020-02-13 18:18:02 +01:00
parent b69942e80e
commit 882d4b0115
31 changed files with 801 additions and 3602 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.gitignore vendored
View File

@ -3,12 +3,11 @@
.vscode/
MDK-ARM/Listing/
MDK-ARM/Objects/
MDK-ARM/*.uvgui
MDK-ARM/*.uvgui.*
build/
!build/VARIANT_DEBUG/firmware.hex
!build/VARIANT_DEBUG/firmware.bin
!build/VARIANT_DEBUG/firmware.elf
!build/VARIANT_DEBUG/firmware.axf
!build/VARIANT_HOVERBOARD/firmware.hex
!build/VARIANT_HOVERBOARD/firmware.bin
!build/VARIANT_HOVERBOARD/firmware.elf
!build/VARIANT_HOVERBOARD/firmware.axf
!build/VARIANT_HOVERBOARD/firmware.elf

11
.travis.yml
View File

@ -45,3 +45,14 @@ jobs:
- export PATH=$HOME/arm-gcc-toolchain/bin:$PATH
before_script: arm-none-eabi-gcc --version
# # Not yet supportted, see https://community.platformio.org/t/build-gd32-project-with-platformio/11944
# - name: platformio
# script: platformio run
# language: python
# python:
# - "2.7"
# install:
# - pip install -U platformio
# - platformio update
# cache:
# - directories: "~/.platformio"

73
GD32F130C6_FLASH.ld → GD32F130C6T_FLASH.ld
View File

@ -1,19 +1,24 @@
/*
*
* Automatically generated file
* PlatformIO default linker script template for STM32 F1/F2/F3/F4/F7/L0/L1/L4
*
*/
/* Entry Point */
ENTRY(Reset_Handler)
/* Highest address of the user mode stack */
_estack = 0x20000400;
/* Highest address of the user mode stack */
_estack = 0x20001000; /* end of RAM */
/* Generate a link error if heap and stack don't fit into RAM */
_Min_Heap_Size = 0x400; /* required amount of heap */
_Min_Stack_Size = 0x400; /* required amount of stack */
_Min_Heap_Size = 0x200; /* required amount of heap */
_Min_Stack_Size = 0x400; /* required amount of stack */
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 4K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 4K
FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 32K
}
/* Define output sections */
@ -33,10 +38,9 @@ SECTIONS
. = ALIGN(4);
*(.text) /* .text sections (code) */
*(.text*) /* .text* sections (code) */
*(.rodata) /* .rodata sections (constants, strings, etc.) */
*(.rodata*) /* .rodata* sections (constants, strings, etc.) */
*(.glue_7) /* glue arm to thumb code */
*(.glue_7t) /* glue thumb to arm code */
*(.eh_frame)
KEEP (*(.init))
KEEP (*(.fini))
@ -45,15 +49,21 @@ SECTIONS
_etext = .; /* define a global symbols at end of code */
} >FLASH
/* Constant data goes into FLASH */
.rodata :
{
. = ALIGN(4);
*(.rodata) /* .rodata sections (constants, strings, etc.) */
*(.rodata*) /* .rodata* sections (constants, strings, etc.) */
. = ALIGN(4);
} >FLASH
.ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
.ARM : {
.ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
.ARM : {
__exidx_start = .;
*(.ARM.exidx*)
__exidx_end = .;
} >FLASH
.ARM.attributes 0 : { *(.ARM.attributes) }
*(.ARM.exidx*)
__exidx_end = .;
} >FLASH
.preinit_array :
{
@ -71,16 +81,16 @@ SECTIONS
.fini_array :
{
PROVIDE_HIDDEN (__fini_array_start = .);
KEEP (*(.fini_array*))
KEEP (*(SORT(.fini_array.*)))
KEEP (*(.fini_array*))
PROVIDE_HIDDEN (__fini_array_end = .);
} >FLASH
/* used by the startup to initialize data */
_sidata = .;
_sidata = LOADADDR(.data);
/* Initialized data sections goes into RAM, load LMA copy after code */
.data : AT ( _sidata )
.data :
{
. = ALIGN(4);
_sdata = .; /* create a global symbol at data start */
@ -89,8 +99,8 @@ SECTIONS
. = ALIGN(4);
_edata = .; /* define a global symbol at data end */
} >RAM
} >RAM AT> FLASH
/* Uninitialized data section */
. = ALIGN(4);
.bss :
@ -107,27 +117,18 @@ SECTIONS
__bss_end__ = _ebss;
} >RAM
PROVIDE ( end = _ebss );
PROVIDE ( _end = _ebss );
/* User_heap_stack section, used to check that there is enough RAM left */
._user_heap_stack :
{
. = ALIGN(4);
PROVIDE ( end = . );
PROVIDE ( _end = . );
. = . + _Min_Heap_Size;
. = . + _Min_Stack_Size;
. = ALIGN(4);
} >RAM
/* MEMORY_bank1 section, code must be located here explicitly */
/* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */
.memory_b1_text :
{
*(.mb1text) /* .mb1text sections (code) */
*(.mb1text*) /* .mb1text* sections (code) */
*(.mb1rodata) /* read-only data (constants) */
*(.mb1rodata*)
} >MEMORY_B1
/* Remove information from the standard libraries */
/DISCARD/ :
@ -136,4 +137,8 @@ SECTIONS
libm.a ( * )
libgcc.a ( * )
}
.ARM.attributes 0 : { *(.ARM.attributes) }
}

1
Inc/config.h
View File

@ -33,6 +33,7 @@
#define MPU_ACCEL_FSR 2 // [g] Set Acceleromenter Full Scale Range: 2g, 4g, 8g, 16g. !! DMP sensor fusion works only with 2g !!
#define MPU_I2C_SPEED 400000 // [bit/s] Define I2C speed for communicating with the MPU6050
#define DELAY_IN_MAIN_LOOP 1 // [ms] Delay in the main loop
// #define PRINTF_FLOAT_SUPPORT // [-] Uncomment this for printf to support float on Serial Debug. It will increase code size! Better to avoid it!
/* =============================================================================================== */

110
Inc/defines.h
View File

@ -26,6 +26,10 @@
#include "gd32f1x0.h"
#include "config.h"
#if defined(PRINTF_FLOAT_SUPPORT) && defined(SERIAL_DEBUG) && defined(__GNUC__)
asm(".global _printf_float"); // this is the magic trick for printf to support float. Warning: It will increase code considerably! Better to avoid!
#endif
/* =========================== Defines General =========================== */
// #define _BV(bit) (1 << (bit))
// #define ARRAYNUM(arr_nanme) (uint32_t)(sizeof(arr_nanme) / sizeof(*(arr_nanme)))
@ -38,23 +42,29 @@
/* =========================== Defines LEDs =========================== */
#define LED1_GPIO_Port GPIOA
#define LED1_Pin GPIO_PIN_0 // RED
#define LED2_GPIO_Port GPIOB
#define LED2_Pin GPIO_PIN_9 // GREEN
#define LED3_GPIO_Port GPIOB
#define LED3_Pin GPIO_PIN_8 // YELLOW
#define LED4_GPIO_Port GPIOB
#define LED4_Pin GPIO_PIN_5 // BLUE1
#define LED5_GPIO_Port GPIOB
#define LED5_Pin GPIO_PIN_4 // BLUE2
#define LED1_GPIO_Port GPIOA
#define LED1_Pin GPIO_PIN_0 // RED
#define LED2_GPIO_Port GPIOB
#define LED2_Pin GPIO_PIN_9 // GREEN
#define LED3_GPIO_Port GPIOB
#define LED3_Pin GPIO_PIN_8 // YELLOW
#define LED4_GPIO_Port GPIOB
#define LED4_Pin GPIO_PIN_5 // BLUE1
#define LED5_GPIO_Port GPIOB
#define LED5_Pin GPIO_PIN_4 // BLUE2
#define LED1_SET (0x01)
#define LED2_SET (0x02)
#define LED3_SET (0x04)
#define LED4_SET (0x08)
#define LED5_SET (0x10)
/* =========================== Defines SENSORS =========================== */
#define SENSOR1_GPIO_Port GPIOA
#define SENSOR1_Pin GPIO_PIN_4
#define SENSOR1_Pin GPIO_PIN_4
#define SENSOR2_GPIO_Port GPIOC
#define SENSOR2_Pin GPIO_PIN_14
#define SENSOR2_Pin GPIO_PIN_14
/* =========================== Defines USART =========================== */
@ -74,66 +84,66 @@
// USART to Mainboard, connected to USART1
#define USART_MAIN USART1
#define USART_MAIN USART1
#define USART_MAIN_CLK RCU_USART1
#define USART_MAIN_TX_PIN GPIO_PIN_2
#define USART_MAIN_RX_PIN GPIO_PIN_3
// USART address for DMA defines
#define USART0_TDATA_ADDRESS ((uint32_t)0x40013828) // USART0: 0x4001 3800 - 0x4001 3BFF
#define USART0_RDATA_ADDRESS ((uint32_t)0x40013824)
#define USART1_TDATA_ADDRESS ((uint32_t)0x40004428) // USART1: 0x4000 4400 - 0x4000 47FF
#define USART1_RDATA_ADDRESS ((uint32_t)0x40004424)
#define USART0_TDATA_ADDRESS ((uint32_t)0x40013828) // USART0: 0x4001 3800 - 0x4001 3BFF
#define USART0_RDATA_ADDRESS ((uint32_t)0x40013824)
#define USART1_TDATA_ADDRESS ((uint32_t)0x40004428) // USART1: 0x4000 4400 - 0x4000 47FF
#define USART1_RDATA_ADDRESS ((uint32_t)0x40004424)
/* =========================== Defines AUX =========================== */
#define AUX1_PU_GPIO_Port GPIOC
#define AUX1_PU_Pin GPIO_PIN_15
#define AUX2_GPIO_Port GPIOA
#define AUX2_Pin GPIO_PIN_1
#define AUX3_GPIO_Port GPIOB
#define AUX3_Pin GPIO_PIN_10
#define AUX1_PU_GPIO_Port GPIOC
#define AUX1_PU_Pin GPIO_PIN_15
#define AUX2_GPIO_Port GPIOA
#define AUX2_Pin GPIO_PIN_1
#define AUX3_GPIO_Port GPIOB
#define AUX3_Pin GPIO_PIN_10
#ifdef AUX45_USE_GPIO
#define AUX4_GPIO_Port GPIOA
#define AUX4_Pin GPIO_PIN_10
#define AUX5_GPIO_Port GPIOA
#define AUX5_Pin GPIO_PIN_9
#define AUX4_GPIO_Port GPIOA
#define AUX4_Pin GPIO_PIN_10
#define AUX5_GPIO_Port GPIOA
#define AUX5_Pin GPIO_PIN_9
#endif
/* =========================== Defines I2C =========================== */
typedef enum {READ = 0, WRITE = !READ} i2c_cmd;
#define MPU_I2C I2C0
#define MPU_RCU_I2C RCU_I2C0
#define MPU_I2C I2C0
#define MPU_RCU_I2C RCU_I2C0
#define MPU_SCL_GPIO_Port GPIOB
#define MPU_SCL_Pin GPIO_PIN_6
#define MPU_SCL_Pin GPIO_PIN_6
#define MPU_SDA_GPIO_Port GPIOB
#define MPU_SDA_Pin GPIO_PIN_7
#define MPU_SDA_Pin GPIO_PIN_7
#define I2C_OWN_ADDRESS7 0x24
#ifdef AUX45_USE_I2C
#define AUX_I2C I2C1
#define AUX_RCU_I2C RCU_I2C1
#define AUX_I2C I2C1
#define AUX_RCU_I2C RCU_I2C1
#define AUX_SCL_GPIO_Port GPIOA
#define AUX_SCL_Pin GPIO_PIN_9
#define AUX_SCL_Pin GPIO_PIN_9
#define AUX_SDA_GPIO_Port GPIOA
#define AUX_SDA_Pin GPIO_PIN_10
#define AUX_SDA_Pin GPIO_PIN_10
#define AUX_I2C_OWN_ADDRESS7 0x34
#endif
/* =========================== Defines MPU-6050 =========================== */
#define log_i printf // redirect the log_i debug function to printf
#define RAD2DEG 57.295779513082323 // RAD2DEG = 180/pi. Example: angle[deg] = angle[rad] * RAD2DEG
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define COMPASS_ON (0x04)
#define log_i printf // redirect the log_i debug function to printf
#define RAD2DEG 57.295779513082323 // RAD2DEG = 180/pi. Example: angle[deg] = angle[rad] * RAD2DEG
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define COMPASS_ON (0x04)
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define PRINT_EULER (0x08)
#define PRINT_TEMP (0x10)
#define PRINT_PEDO (0x20)
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define PRINT_EULER (0x08)
#define PRINT_TEMP (0x10)
#define PRINT_PEDO (0x20)
typedef struct{
int16_t x;
@ -161,11 +171,11 @@ typedef struct{
} Euler;
typedef struct {
Gyro gyro;
Accel accel;
Gyro gyro;
Accel accel;
Quaternion quat;
Euler euler;
int16_t temp;
Euler euler;
int16_t temp;
} MPU_Data;
#endif

0
Drivers/GD32F1x0_standard_peripheral/Include/gd32f1x0_libopt.h → Inc/gd32f1x0_libopt.h
View File

37
Inc/util.h
View File

@ -26,33 +26,34 @@
#include "defines.h"
extern volatile int8_t i2c_status;
extern volatile i2c_cmd i2c_ReadWriteCmd;
extern volatile uint8_t i2c_slaveAddress;
extern volatile uint8_t i2c_regAddress;
extern volatile uint8_t* i2c_txbuffer;
extern volatile uint8_t* i2c_rxbuffer;
extern volatile uint8_t i2c_nDABytes;
extern volatile int8_t i2c_nRABytes;
extern volatile int8_t i2c_status;
extern volatile i2c_cmd i2c_ReadWriteCmd;
extern volatile uint8_t i2c_slaveAddress;
extern volatile uint8_t i2c_regAddress;
extern volatile uint8_t* i2c_txbuffer;
extern volatile uint8_t* i2c_rxbuffer;
extern volatile uint8_t i2c_nDABytes;
extern volatile int8_t i2c_nRABytes;
#ifdef AUX45_USE_I2C
extern volatile int8_t i2c_aux_status;
extern volatile i2c_cmd i2c_aux_ReadWriteCmd;
extern volatile uint8_t i2c_aux_slaveAddress;
extern volatile uint8_t i2c_aux_regAddress;
extern volatile uint8_t* i2c_aux_txbuffer;
extern volatile uint8_t* i2c_aux_rxbuffer;
extern volatile uint8_t i2c_aux_nDABytes;
extern volatile int8_t i2c_aux_nRABytes;
extern volatile int8_t i2c_aux_status;
extern volatile i2c_cmd i2c_aux_ReadWriteCmd;
extern volatile uint8_t i2c_aux_slaveAddress;
extern volatile uint8_t i2c_aux_regAddress;
extern volatile uint8_t* i2c_aux_txbuffer;
extern volatile uint8_t* i2c_aux_rxbuffer;
extern volatile uint8_t i2c_aux_nDABytes;
extern volatile int8_t i2c_aux_nRABytes;
#endif
/* general functions */
void consoleLog(char *message);
void introDemoLED(uint32_t tDelay);
void toggle_led(uint32_t gpio_periph, uint32_t pin);
void intro_demo_led(uint32_t tDelay);
/* i2c write/read functions */
int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
int8_t i2c_writeByte (uint8_t slaveAddr, uint8_t regAddr, uint8_t data);
int8_t i2c_writeByte (uint8_t slaveAddr, uint8_t regAddr, uint8_t data);
int8_t i2c_writeBit (uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
int8_t i2c_readBytes (uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
int8_t i2c_readByte (uint8_t slaveAddr, uint8_t regAddr, uint8_t *data);

7
MDK-ARM/sideboard-hack.uvopt
View File

@ -29,7 +29,7 @@
<CLKADS>8000000</CLKADS>
<OPTTT>
<gFlags>1</gFlags>
<BeepAtEnd>1</BeepAtEnd>
<BeepAtEnd>0</BeepAtEnd>
<RunSim>1</RunSim>
<RunTarget>0</RunTarget>
<RunAbUc>0</RunAbUc>
@ -115,6 +115,11 @@
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U -O207 -S0 -C0 -A0 -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0GD32F1x0_32 -FS08000000 -FL08000</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>UL2CM3</Key>

4
MDK-ARM/sideboard-hack.uvproj
View File

@ -77,9 +77,9 @@
<nStopB2X>0</nStopB2X>
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg1>1</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg1Name>$K\ARM\ARMCC\bin\fromelf.exe --bin --output=.\Objects\@L.bin !L</UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>

2
Makefile
View File

@ -150,7 +150,7 @@ CFLAGS += -MMD -MP -MF"$(@:%.o=%.d)"
# LDFLAGS
#######################################
# link script
LDSCRIPT = GD32F130C6_FLASH.ld
LDSCRIPT = GD32F130C6T_FLASH.ld
# libraries
LIBS = -lc -lm -lnosys

32
README.md
View File

@ -14,6 +14,9 @@ This repository implements the firmware for the hoveboard sideboards. The hovebo
The original sideboard hardware supports one 4-pin cable that originally was connected to the hoveboard mainboard. It breaks out GND, 12/15V and USART. Additionally, two ports are used to connect to the LED boards. On the back of the board, two Photo Interrupter Optical Switches can be found, originally used to detect if a human is standing on the hoverboard.
![sideboard](/docs/pictures/sideboard_pinout.png)
The LED boards consist of colored LEDs (blue, red, green, orange) used for design and to inform the user about the current hoverboard state.
![ledboard](/docs/pictures/ledboard_pinout.png)
The heart of the sideboard is a [GD32F130C6T6](/docs/GD32F130xx-Datasheet_Rev3.3.pdf) with the pinout shown in the follwing figure:
![MCU_pinout](/docs/pictures/MCU_pinout.png)
@ -40,13 +43,34 @@ If you have never flashed your sideboard before, the MCU is probably locked. To
To build and flash choose one of the following methods:
### Method 1: Using Keil uVision (recommended)
### Method 1: Using PlatformIO
- in [Keil uVision](https://www.keil.com/download/product/), open the [sideboard-hack.uvproj](/MDK-ARM/)
Because this board is not yet supported by [PlatformIO](https://platformio.org/), we have to make two extra steps. These steps can be skipped once the board is supported (see [this thread](https://community.platformio.org/t/build-gd32-project-with-platformio/11944)).
- go to your PlatformIO home folder (Windows: `C:\Users\<user>\.platformio`, Unix/Max: `/home/<user>/.platformio`). Then go into `packages`. If the folder `framework-spl` exists, delete it.
- unpack the `framework-spl.zip` in the `packages` folder so that the directory structure is now:
```
packages/
| - framework-spl/
| |-- gd32/
| |-- platformio/
| |-- stm32/
| |-- package.json
```
(This folder contains the `GD32F1x0_Firmware_Library_v3.1.0` files)
- open the project folder in the IDE of choice (vscode or Atom)
- press the 'PlatformIO:Build' or the 'PlatformIO:Upload' button (bottom left in vscode).
### Method 2: Using Keil uVision
- in [Keil uVision](https://www.keil.com/download/product/), open the [sideboard-hack.uvproj](/MDK-ARM/)
- if you are asked to install missing package, click `Yes`
- click Build Target (or press F7) to build the firmware
- click Load Code (or press F8) to flash the firmware.
### Method 2: Using Ubuntu
### Method 3: Using Ubuntu
- prerequisites: install [ST-Flash utility](https://github.com/texane/stlink).
@ -59,8 +83,6 @@ make
make flash
```
*Note: If someone finds a way to build and flash the GD32 MCU via [Platformio](https://platformio.org/) let me know.*
---
## Example Variants

260
Src/i2c_it.c
View File

@ -24,43 +24,52 @@
#include "systick.h"
#include "config.h"
void I2C0_EventIRQ_Handler(void)
{
uint16_t k;
if (i2c_ReadWriteCmd == WRITE) { // check for WRITE command
// ======================================== WRITE ========================================
// --------------------------------------------------------------------
// | Master | S | AD+W | | RA | | DATA | | DATA | | P |
// | Slave | | | ACK | | ACK | | ACK | | ACK | |
// --------------------------------------------------------------------
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
i2c_nRABytes--;
} else {
if (i2c_nDABytes > 0) {
i2c_data_transmit(I2C0, *i2c_txbuffer++); // the master sends a data byte
i2c_nDABytes--;
for(k=0; k<500; k++); // make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
} else {
i2c_stop_on_bus(I2C0); // the master sends a stop condition to I2C bus
i2c_status = 0; // 0 = Success
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
}
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
i2c_nRABytes--;
} else {
if (i2c_nDABytes > 0) {
i2c_data_transmit(I2C0, *i2c_txbuffer++); // the master sends a data byte
i2c_nDABytes--;
if (0 == i2c_nDABytes) {
i2c_status = 0; // 0 = Success
}
for(k=0; k<500; k++) {
#ifdef __GNUC__
asm volatile ("nop"); // unoptimizable NOP loop, 500 times to make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
#else
__asm volatile ("nop");
#endif
}
} else {
i2c_stop_on_bus(I2C0); // the master sends a stop condition to I2C bus
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
}
} else if (i2c_ReadWriteCmd == READ) { // check for READ command
@ -74,55 +83,55 @@ void I2C0_EventIRQ_Handler(void)
// Phase 1 - send the Register Address
if (i2c_nRABytes >= 0) {
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C0); // send start condition
}
i2c_nRABytes--;
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C0); // send start condition
}
i2c_nRABytes--;
}
// Phase 2 - read Data
} else {
if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
if((1 == i2c_nDABytes) || (2 == i2c_nDABytes)){
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
if((1 == i2c_nDABytes) || (2 == i2c_nDABytes)){
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_nDABytes > 0){
if(3 == i2c_nDABytes){
while(!i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_rxbuffer++ = i2c_data_receive(I2C0); // read a data byte from I2C_DATA
i2c_nDABytes--;
if(0 == i2c_nDABytes){
i2c_stop_on_bus(I2C0); // send a stop condition
i2c_status = 0; // 0 = Success
i2c_ack_config(I2C0, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_nDABytes > 0){
if(3 == i2c_nDABytes){
while(!i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_rxbuffer++ = i2c_data_receive(I2C0); // read a data byte from I2C_DATA
i2c_nDABytes--;
if(0 == i2c_nDABytes){
i2c_stop_on_bus(I2C0); // send a stop condition
i2c_status = 0; // 0 = Success
i2c_ack_config(I2C0, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}
}
@ -192,32 +201,39 @@ void I2C1_EventIRQ_Handler(void)
// | Slave | | | ACK | | ACK | | ACK | | ACK | |
// --------------------------------------------------------------------
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
i2c_aux_nRABytes--;
} else {
if (i2c_aux_nDABytes > 0) {
i2c_data_transmit(I2C1, *i2c_aux_txbuffer++); // the master sends a data byte
i2c_aux_nDABytes--;
for(k=0; k<500; k++); // make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
} else {
i2c_stop_on_bus(I2C1); // the master sends a stop condition to I2C bus
i2c_aux_status = 0; // 0 = Success
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
}
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
i2c_aux_nRABytes--;
} else {
if (i2c_aux_nDABytes > 0) {
i2c_data_transmit(I2C1, *i2c_aux_txbuffer++); // the master sends a data byte
i2c_aux_nDABytes--;
for(k=0; k<500; k++);
if (0 == i2c_aux_nDABytes) {
i2c_aux_status = 0; // 0 = Success
}
#ifdef __GNUC__
asm volatile ("nop"); // unoptimizable NOP loop, 500 times to make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
#else
__asm volatile ("nop");
#endif
} else {
i2c_stop_on_bus(I2C1); // the master sends a stop condition to I2C bus
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
}
} else if (i2c_aux_ReadWriteCmd == READ) { // check for READ command
@ -231,55 +247,55 @@ void I2C1_EventIRQ_Handler(void)
// Phase 1 - send the Register Address
if (i2c_aux_nRABytes >= 0) {
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C1); // send start condition
}
i2c_aux_nRABytes--;
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C1); // send start condition
}
i2c_aux_nRABytes--;
}
// Phase 2 - read Data
} else {
if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
if((1 == i2c_aux_nDABytes) || (2 == i2c_aux_nDABytes)){
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
if((1 == i2c_aux_nDABytes) || (2 == i2c_aux_nDABytes)){
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_aux_nDABytes > 0){
if(3 == i2c_aux_nDABytes){
while(!i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_aux_rxbuffer++ = i2c_data_receive(I2C1); // read a data byte from I2C_DATA
i2c_aux_nDABytes--;
if(0 == i2c_aux_nDABytes){
i2c_stop_on_bus(I2C1); // send a stop condition
i2c_aux_status = 0; // 0 = Success
i2c_ack_config(I2C1, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C1, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_aux_nDABytes > 0){
if(3 == i2c_aux_nDABytes){
while(!i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_aux_rxbuffer++ = i2c_data_receive(I2C1); // read a data byte from I2C_DATA
i2c_aux_nDABytes--;
if(0 == i2c_aux_nDABytes){
i2c_stop_on_bus(I2C1); // send a stop condition
i2c_aux_status = 0; // 0 = Success
i2c_ack_config(I2C1, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C1, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}
}

176
Src/main.c
View File

@ -31,28 +31,29 @@
#ifdef SERIAL_CONTROL
typedef struct{
uint16_t start;
int16_t roll;
int16_t pitch;
uint16_t start;
int16_t roll;
int16_t pitch;
int16_t yaw;
uint16_t sensors;
uint16_t checksum;
uint16_t sensors;
uint16_t checksum;
} SerialSideboard;
SerialSideboard Sideboard;
#endif
#ifdef SERIAL_FEEDBACK
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t checksum;
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;
@ -61,22 +62,23 @@ static int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial com
static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif
extern MPU_Data mpu; // holds the MPU-6050 data
ErrStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR
extern MPU_Data mpu; // holds the MPU-6050 data
ErrStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR
uint8_t userCommand; // holds the user command input
uint8_t sensor1, sensor2; // holds the sensor1 and sensor 2 values
uint8_t userCommand; // holds the user command input
FlagStatus sensor1, sensor2; // holds the sensor1 and sensor 2 values
FlagStatus sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
int main(void)
{
systick_config(); // SysTick config
gpio_config(); // GPIO config
usart_config(USART_MAIN, USART_MAIN_BAUD); // USART config
i2c_config(); // I2C config
i2c_nvic_config(); // NVIC peripheral config
systick_config(); // SysTick config
gpio_config(); // GPIO config
usart_config(USART_MAIN, USART_MAIN_BAUD); // USART config
i2c_config(); // I2C config
i2c_nvic_config(); // NVIC peripheral config
#ifdef SERIAL_CONTROL
usart_Tx_DMA_config(USART_MAIN, (uint8_t *)&Sideboard, sizeof(Sideboard));
@ -85,30 +87,39 @@ int main(void)
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
#endif
introDemoLED(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to initialize.
if(mpu_config()) { // IMU MPU-6050 config
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED
}
mpu_handle_input('h'); // Print the User Help commands to serial
else {
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED
}
mpu_handle_input('h'); // Print the User Help commands to serial
while(1){
while(1) {
delay_1ms(DELAY_IN_MAIN_LOOP);
// ==================================== LEDs Handling ====================================
// gpio_bit_write(LED4_GPIO_Port, LED4_Pin, (bit_status)(1-gpio_input_bit_get(LED4_GPIO_Port, LED4_Pin))); // Toggle BLUE1 LED
if (SUCCESS == mpuStatus) {
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED
} else {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED
}
// toggle_led(LED4_GPIO_Port, LED4_Pin); // Toggle BLUE1 LED
#ifdef SERIAL_FEEDBACK
if (!timeoutFlagSerial) {
if (Feedback.cmdLed & LED1_SET) { gpio_bit_set(LED1_GPIO_Port, LED1_Pin); } else { gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); }
if (Feedback.cmdLed & LED2_SET) { gpio_bit_set(LED2_GPIO_Port, LED2_Pin); } else { gpio_bit_reset(LED2_GPIO_Port, LED2_Pin); }
if (Feedback.cmdLed & LED3_SET) { gpio_bit_set(LED3_GPIO_Port, LED3_Pin); } else { gpio_bit_reset(LED3_GPIO_Port, LED3_Pin); }
if (Feedback.cmdLed & LED4_SET) { gpio_bit_set(LED4_GPIO_Port, LED4_Pin); } else { gpio_bit_reset(LED4_GPIO_Port, LED4_Pin); }
if (Feedback.cmdLed & LED5_SET) { gpio_bit_set(LED5_GPIO_Port, LED5_Pin); } else { gpio_bit_reset(LED5_GPIO_Port, LED5_Pin); }
}
#endif
// ==================================== USER Handling ====================================
#ifdef SERIAL_DEBUG
// Get the user Input as one character from Serial
if(SET == usart_flag_get(USART_MAIN, USART_FLAG_RBNE)) { // Check if Read Buffer Not Empty meanind Serial data is available
if(SET == usart_flag_get(USART_MAIN, USART_FLAG_RBNE)) { // Check if Read Buffer Not Empty meanind Serial data is available
userCommand = usart_data_receive(USART_MAIN);
if (userCommand != 10 && userCommand != 13) { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
if (userCommand != 10 && userCommand != 13) { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
log_i("Command = %c\n", userCommand);
mpu_handle_input(userCommand);
}
@ -122,35 +133,42 @@ int main(void)
mpu_get_data();
}
// Print MPU data to Console
if (main_loop_counter % 50 == 0 && SUCCESS == mpuStatus) {
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
// ==================================== SENSORS Handling ====================================
sensor1_read = gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin);
// SENSOR1
if (gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin)) {
sensor1 = 1;
// Sensor ACTIVE: Do something here
if (sensor1 == RESET && sensor1_read == SET) {
sensor1 = SET;
// Sensor ACTIVE: Do something here (one time task on activation)
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
consoleLog("-- SENSOR 1 Active --\n");
delay_1ms(50);
} else {
sensor1 = 0;
consoleLog("-- SENSOR 1 Active --\n");
} else if(sensor1 == SET && sensor1_read == RESET) {
sensor1 = RESET;
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
}
// SENSOR2
if (gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin)) {
sensor2 = 1;
// Sensor ACTIVE: Do something here
if (sensor2 == RESET && sensor2_read == SET) {
sensor2 = SET;
// Sensor ACTIVE: Do something here (one time task on activation)
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
consoleLog("-- SENSOR 2 Active --\n");
delay_1ms(50);
} else {
sensor2 = 0;
} else if (sensor2 == SET && sensor2_read == RESET) {
sensor2 = RESET;
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
}
if (sensor1 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
// ==================================== SERIAL Tx/Rx Handling ====================================
@ -161,8 +179,8 @@ int main(void)
Sideboard.start = (uint16_t)SERIAL_START_FRAME;
Sideboard.roll = (int16_t)mpu.euler.roll;
Sideboard.pitch = (int16_t)mpu.euler.pitch;
Sideboard.yaw = (int16_t)mpu.euler.yaw;
Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1));
Sideboard.yaw = (int16_t)mpu.euler.yaw;
Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1));
Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.roll ^ Sideboard.pitch ^ Sideboard.yaw ^ Sideboard.sensors);
dma_channel_disable(DMA_CH3);
@ -175,33 +193,33 @@ int main(void)
#ifdef SERIAL_FEEDBACK
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp);
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed);
if (NewFeedback.start == SERIAL_START_FRAME && NewFeedback.checksum == checksum) {
if (timeoutFlagSerial) { // Check for previous timeout flag
if (timeoutCntSerial-- <= 0) // Timeout de-qualification
timeoutFlagSerial = 0; // Timeout flag cleared
} else {
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Copy the new data
NewFeedback.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCntSerial = 0; // Reset the timeout counter
}
} else {
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial = 1; // Timeout detected
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
}
// Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA
if (main_loop_counter % 50 == 0 && NewFeedback.start != SERIAL_START_FRAME && NewFeedback.start != 0xFFFF) {
dma_channel_disable(DMA_CH4);
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
}
if (timeoutFlagSerial) { // Check for previous timeout flag
if (timeoutCntSerial-- <= 0) // Timeout de-qualification
timeoutFlagSerial = 0; // Timeout flag cleared
} else {
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Copy the new data
NewFeedback.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCntSerial = 0; // Reset the timeout counter
}
} else {
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial = 1; // Timeout detected
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
}
if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State and indicate error if desired
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on Red LED
} else {
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); // Follow the Normal behavior
}
// Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA
if (main_loop_counter % 50 == 0 && NewFeedback.start != SERIAL_START_FRAME && NewFeedback.start != 0xFFFF) {
dma_channel_disable(DMA_CH4);
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
}
}
if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State and indicate error if desired
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on Red LED
} else {
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); // Follow the Normal behavior
}
#endif

352
Src/mpu6050.c
View File

@ -522,13 +522,13 @@ const struct hw_s hw = {
const struct test_s test = {
.gyro_sens = 32768/250,
.accel_sens = 32768/2, // FSR = +-2G = 16384 LSB/G
.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
.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
@ -2183,17 +2183,17 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
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);
log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n",
(long)st_shift_cust[i], bias_regular[i],
bias_st[i]);
log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]);
}
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);
log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i],
(long)test.max_accel_var);
if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
if(debug)
@ -2209,15 +2209,15 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
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);
log_i("Min MG: %ld\r\n", (long)accel_st_al_min);
log_i("Max MG: %ld\r\n", (long)accel_st_al_max);
}
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);
log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]);
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);
@ -2230,7 +2230,7 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
/* 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);
log_i("Accel:CRITERIA C: bias less than %ld\n", (long)accel_offset_max);
for (i = 0; i < 3; i++) {
if(fabs(bias_regular[i]) > accel_offset_max) {
if(debug)
@ -2279,17 +2279,17 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
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);
log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n",
(long)st_shift_cust[i], bias_regular[i],
bias_st[i]);
log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]);
}
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);
log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i],
(long)test.max_gyro_var);
if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
if(debug)
@ -2304,14 +2304,14 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
if(debug) {
log_i("GYRO:CRITERIA B\r\n");
log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f);
log_i("Max DPS: %ld\r\n", (long)gyro_st_al_max);
}
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);
log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]);
if(st_shift_cust[i] < gyro_st_al_max) {
if(debug)
log_i("GYRO FAIL axis:%d greater than 60dps\n", i);
@ -2324,7 +2324,7 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
/* 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);
log_i("Gyro:CRITERIA C: bias less than %ld\n", (long)gyro_offset_max);
for (i = 0; i < 3; i++) {
if(fabs(bias_regular[i]) > gyro_offset_max) {
if(debug)
@ -2456,8 +2456,8 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in
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);
log_i("Accel offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, accel[0], accel[1], accel[2]);
log_i("Gyro offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, gyro[0], gyro[1], gyro[2]);
}
return 0;
@ -2872,9 +2872,9 @@ static int setup_compass(void)
if (akm_addr > 0x0F) {
/* TODO: Handle this case in all compass-related functions. */
#ifdef SERIAL_DEBUG
log_i("Compass not found.\n");
#endif
#ifdef SERIAL_DEBUG
log_i("Compass not found.\n");
#endif
return -1;
}
@ -3227,18 +3227,18 @@ void mpu_start_self_test(void)
result = mpu_run_self_test(gyro, accel);
#endif
if (result == 0x7) {
#ifdef SERIAL_DEBUG
consoleLog("Passed!\n");
log_i("accel: %7.4f %7.4f %7.4f\n",
accel[0]/65536.f,
accel[1]/65536.f,
accel[2]/65536.f);
log_i("gyro: %7.4f %7.4f %7.4f\n",
gyro[0]/65536.f,
gyro[1]/65536.f,
gyro[2]/65536.f);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#endif
#ifdef SERIAL_DEBUG
consoleLog("Passed!\n");
log_i("accel: %ld %ld %ld\n",
accel[0],
accel[1],
accel[2]);
log_i("gyro: %ld %ld %ld\n",
gyro[0],
gyro[1],
gyro[2]);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#endif
#ifdef USE_CAL_HW_REGISTERS
/*
@ -3302,17 +3302,17 @@ void mpu_setup_gyro(void)
unsigned char mask = 0, lp_accel_was_on = 0;
if (hal.sensors & ACCEL_ON) {
mask |= INV_XYZ_ACCEL;
consoleLog("Accel sensor On.\n");
consoleLog("Accel sensor On.\n");
} else {
consoleLog("Accel sensor Off.\n");
}
consoleLog("Accel sensor Off.\n");
}
if (hal.sensors & GYRO_ON) {
mask |= INV_XYZ_GYRO;
lp_accel_was_on |= hal.lp_accel_mode;
consoleLog("Gyro sensor On.\n");
consoleLog("Gyro sensor On.\n");
} else {
consoleLog("Gyro sensor Off.\n");
}
consoleLog("Gyro sensor Off.\n");
}
#ifdef COMPASS_ENABLED
if (hal.sensors & COMPASS_ON) {
mask |= INV_XYZ_COMPASS;
@ -3338,35 +3338,35 @@ void mpu_setup_gyro(void)
/* =========================== MPU-6050 Configuration =========================== */
int mpu_config(void)
{
consoleLog("-- Configuring MPU6050... ");
if(mpu_init()) {
consoleLog("FAIL (MPU).\n");
return -1;
}
consoleLog("-- Configuring MPU6050... ");
if(mpu_init()) {
consoleLog("FAIL (MPU).\n");
return -1;
}
/* Get/set hardware configuration. Start gyro. */
/* Get/set hardware configuration. Start gyro. */
/* Wake up all sensors. */
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
/* Push both gyro and accel data into the FIFO. */
/* Push both gyro and accel data into the FIFO. */
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(MPU_DEFAULT_HZ);
/* Read back configuration in case it was set improperly. */
/* Read back configuration in case it was set improperly. */
// mpu_get_sample_rate(&gyro_rate);
// mpu_get_gyro_fsr(&gyro_fsr);
// mpu_get_accel_fsr(&accel_fsr);
/* Initialize HAL state variables. */
hal.sensors = ACCEL_ON | GYRO_ON;
hal.dmp_on = 0;
hal.report = 0;
/* Initialize HAL state variables. */
hal.sensors = ACCEL_ON | GYRO_ON;
hal.dmp_on = 0;
hal.report = 0;
hal.next_pedo_ms = 0;
hal.next_temp_ms = 0;
#ifdef MPU_DMP_ENABLE
/* To initialize the DMP:
/* To initialize the DMP:
* 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
* inv_mpu_dmp_motion_driver.h into the MPU memory.
* 2. Push the gyro and accel orientation matrix to the DMP.
@ -3396,11 +3396,11 @@ int mpu_config(void)
* DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
* be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
*/
consoleLog(" writing DMP... ");
if (dmp_load_motion_driver_firmware()) {
consoleLog(" FAIL (DMP) --\r\n");
return -1;
}
consoleLog(" writing DMP... ");
if (dmp_load_motion_driver_firmware()) {
consoleLog(" FAIL (DMP) --\r\n");
return -1;
}
// dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
dmp_register_tap_cb(mpu_tap_func);
dmp_register_android_orient_cb(mpu_android_orient_func);
@ -3425,8 +3425,8 @@ int mpu_config(void)
hal.dmp_on = 1;
#endif
consoleLog(" OK --\r\n");
return 0;
consoleLog(" OK --\r\n");
return 0;
}
@ -3453,93 +3453,93 @@ void mpu_get_data(void)
* Let's make them timer-based.
*/
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel[3], sensors;
static long quat[4], temperature;
unsigned char more;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
mpu.quat.w = quat[0];
mpu.quat.x = quat[1];
mpu.quat.y = quat[2];
mpu.quat.z = quat[3];
mpu_calc_euler_angles(); // Calculate Euler angles
new_data = 1;
}
short gyro[3], accel[3], sensors;
static long quat[4], temperature;
unsigned char more;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
mpu.quat.w = quat[0];
mpu.quat.x = quat[1];
mpu.quat.y = quat[2];
mpu.quat.z = quat[3];
mpu_calc_euler_angles(); // Calculate Euler angles
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel[3];
long temperature;
unsigned char sensors, more;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
short gyro[3], accel[3];
long temperature;
unsigned char sensors, more;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
}
if (new_data) {
// do something if needed
// do something if needed
}
}
@ -3609,7 +3609,7 @@ void mpu_calc_euler_angles(void) {
// Calculate Euler angles: source <https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles>
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // roll (x-axis rotation)
pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation)
pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation)
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // yaw (z-axis rotation)
// Convert [rad] to [deg*100]
@ -3678,30 +3678,30 @@ void mpu_handle_input(char c)
switch (c) {
/* This command prints the Help text. */
case 'h':
consoleLog("=================== HELP COMMANDS ===================\n");
consoleLog("h: Print Help commands\n");
consoleLog("8: Set Accelerometer sensor on/off\n");
consoleLog("9: Set Gyroscope sensor on/off\n");
consoleLog("r: Print Registers value\n");
consoleLog("a: Print Accelerometer data\n");
consoleLog("g: Print Gyroscope data\n");
consoleLog("q: Print Quaternion data\n");
consoleLog("e: Print Euler angles in degree * 100\n");
consoleLog("t: Print Temperature in degC * 100\n");
consoleLog("p: Print Pedometer data\n");
consoleLog("0: Reset Pedometer\n");
consoleLog("1: Set DMP/MPU frequency 10 Hz\n");
consoleLog("2: Set DMP/MPU frequency 20 Hz\n");
consoleLog("3: Set DMP/MPU frequency 40 Hz\n");
consoleLog("4: Set DMP/MPU frequency 50 Hz\n");
consoleLog("5: Set DMP/MPU frequency 100 Hz\n");
consoleLog(",: Set DMP interrupt to gesture event only\n");
consoleLog(".: Set DMP interrupt to continuous\n");
consoleLog("f: Set DMP on/off\n");
consoleLog("v: Set Quaternion on/off\n");
consoleLog("w: Test out low-power accel mode\n");
consoleLog("s: Run self-test (device must be facing up or down)\n");
consoleLog("=====================================================\n");
consoleLog("=================== HELP COMMANDS ===================\n");
consoleLog("h: Print Help commands\n");
consoleLog("8: Set Accelerometer sensor on/off\n");
consoleLog("9: Set Gyroscope sensor on/off\n");
consoleLog("r: Print Registers value\n");
consoleLog("a: Print Accelerometer data\n");
consoleLog("g: Print Gyroscope data\n");
consoleLog("q: Print Quaternion data\n");
consoleLog("e: Print Euler angles in degree * 100\n");
consoleLog("t: Print Temperature in degC * 100\n");
consoleLog("p: Print Pedometer data\n");
consoleLog("0: Reset Pedometer\n");
consoleLog("1: Set DMP/MPU frequency 10 Hz\n");
consoleLog("2: Set DMP/MPU frequency 20 Hz\n");
consoleLog("3: Set DMP/MPU frequency 40 Hz\n");
consoleLog("4: Set DMP/MPU frequency 50 Hz\n");
consoleLog("5: Set DMP/MPU frequency 100 Hz\n");
consoleLog(",: Set DMP interrupt to gesture event only\n");
consoleLog(".: Set DMP interrupt to continuous\n");
consoleLog("f: Set DMP on/off\n");
consoleLog("v: Set Quaternion on/off\n");
consoleLog("w: Test out low-power accel mode\n");
consoleLog("s: Run self-test (device must be facing up or down)\n");
consoleLog("=====================================================\n");
break;
/* These commands turn off individual sensors. */
@ -3714,14 +3714,14 @@ void mpu_handle_input(char c)
mpu_setup_gyro();
break;
/* This command prints out the value of each gyro register for debugging.
/* This command prints out the value of each gyro register for debugging.
* If logging is disabled, this function has no effect.
*/
case 'r':
mpu_reg_dump();
break;
/* These commands print individual sensor data. */
/* These commands print individual sensor data. */
case 'a':
hal.report ^= PRINT_ACCEL;
break;
@ -3850,9 +3850,9 @@ void mpu_handle_input(char c)
/* Test out low-power accel mode. */
case 'w':
if (hal.dmp_on) {
consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n");
consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n");
break; /* LP accel is not compatible with the DMP. */
}
}
mpu_lp_accel_mode(20);
/* When LP accel mode is enabled, the driver automatically configures
* the hardware for latched interrupts. However, the MCU sometimes
@ -3869,8 +3869,8 @@ void mpu_handle_input(char c)
break;
/* The hardware self test is completely localized in the gyro driver.
* Logging is assumed to be enabled; otherwise, a couple LEDs could
* probably be used here to display the test results.
* Logging is assumed to be enabled; otherwise, a couple LEDs could
* probably be used here to display the test results.
*/
case 's':
mpu_start_self_test();

38
Src/setup.c
View File

@ -29,17 +29,17 @@
extern volatile ErrStatus status;
// Private variables
static rcu_periph_enum USART_CLK[USARTn] = {USART_AUX_CLK,
USART_MAIN_CLK
};
static rcu_periph_enum USART_CLK[USARTn] = { USART_AUX_CLK,
USART_MAIN_CLK
};
static uint32_t USART_TX_PIN[USARTn] = {USART_AUX_TX_PIN,
USART_MAIN_TX_PIN
};
static uint32_t USART_TX_PIN[USARTn] = { USART_AUX_TX_PIN,
USART_MAIN_TX_PIN
};
static uint32_t USART_RX_PIN[USARTn] = {USART_AUX_RX_PIN,
USART_MAIN_RX_PIN
};
static uint32_t USART_RX_PIN[USARTn] = { USART_AUX_RX_PIN,
USART_MAIN_RX_PIN
};
void gpio_config(void) {
@ -82,11 +82,11 @@ void gpio_config(void) {
/* =========================== Configure I2C GPIOs =========================== */
/* enable I2C clock */
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(MPU_RCU_I2C);
rcu_periph_clock_enable(MPU_RCU_I2C);
/* connect PB6 to I2C_SCL and PB7 to I2C_SDA */
gpio_af_set(MPU_SCL_GPIO_Port, GPIO_AF_1, MPU_SCL_Pin);
gpio_af_set(MPU_SDA_GPIO_Port, GPIO_AF_1, MPU_SDA_Pin);
gpio_af_set(MPU_SCL_GPIO_Port, GPIO_AF_1, MPU_SCL_Pin);
gpio_af_set(MPU_SDA_GPIO_Port, GPIO_AF_1, MPU_SDA_Pin);
/* configure GPIO port */
gpio_mode_set(MPU_SCL_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, MPU_SCL_Pin);
@ -195,7 +195,7 @@ void usart_Tx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t Size) {
// --------------------------- TX Channel ---------------------------
/* enable DMA clock */
rcu_periph_clock_enable(RCU_DMA);
rcu_periph_clock_enable(RCU_DMA);
/* deinitialize DMA channel2 */
dma_deinit(DMA_CH3);
@ -203,11 +203,11 @@ void usart_Tx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t Size) {
dma_init_struct.memory_addr = (uint32_t)pData;
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
dma_init_struct.number = Size;
dma_init_struct.number = Size;
dma_init_struct.periph_addr = USART1_TDATA_ADDRESS;
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init(DMA_CH3, dma_init_struct);
/* configure DMA mode */
@ -237,11 +237,11 @@ void usart_Rx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t Size) {
dma_init_struct.memory_addr = (uint32_t)pData;
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
dma_init_struct.number = Size;
dma_init_struct.number = Size;
dma_init_struct.periph_addr = USART1_RDATA_ADDRESS;
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init(DMA_CH4, dma_init_struct);
/* configure DMA mode */
@ -271,8 +271,8 @@ void i2c_config(void) {
#ifdef AUX45_USE_I2C
/* I2C clock configure */
//i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_2); // I2C duty cycle in fast mode
i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_16_9); // I2C duty cycle in fast mode plus
//i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_2); // I2C duty cycle in fast mode
i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_16_9); // I2C duty cycle in fast mode plus
/* I2C address configure */
i2c_mode_addr_config(AUX_I2C, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, AUX_I2C_OWN_ADDRESS7);
/* enable I2C */

8
Src/systick.c
View File

@ -88,21 +88,21 @@ void delay_decrement(void)
/*!
\brief tick count increment in ms
\param[in] none
\param[out] none
\param[out] none
\retval none
*/
void tick_count_increment()
{
tick_count_ms++;
tick_count_ms++;
}
/*!
\brief get tick count in ms
\param[in] *count: pointer to count
\param[out] none
\param[out] none
\retval none
*/
void get_tick_count_ms(unsigned long *count)
{
*count = tick_count_ms;
*count = tick_count_ms;
}

151
Src/util.c
View File

@ -27,26 +27,26 @@
// MAIN I2C variables
volatile int8_t i2c_status;
volatile i2c_cmd i2c_ReadWriteCmd;
volatile uint8_t i2c_regAddress;
volatile uint8_t i2c_slaveAddress;
volatile uint8_t* i2c_txbuffer;
volatile uint8_t* i2c_rxbuffer;
volatile uint8_t i2c_nDABytes;
volatile int8_t i2c_nRABytes;
volatile uint8_t buffer[14];
volatile int8_t i2c_status;
volatile i2c_cmd i2c_ReadWriteCmd;
volatile uint8_t i2c_regAddress;
volatile uint8_t i2c_slaveAddress;
volatile uint8_t* i2c_txbuffer;
volatile uint8_t* i2c_rxbuffer;
volatile uint8_t i2c_nDABytes;
volatile int8_t i2c_nRABytes;
volatile uint8_t buffer[14];
#ifdef AUX45_USE_I2C
// AUX I2C variables
volatile int8_t i2c_aux_status;
volatile i2c_cmd i2c_aux_ReadWriteCmd;
volatile uint8_t i2c_aux_regAddress;
volatile uint8_t i2c_aux_slaveAddress;
volatile uint8_t* i2c_aux_txbuffer;
volatile uint8_t* i2c_aux_rxbuffer;
volatile uint8_t i2c_aux_nDABytes;
volatile int8_t i2c_aux_nRABytes;
volatile int8_t i2c_aux_status;
volatile i2c_cmd i2c_aux_ReadWriteCmd;
volatile uint8_t i2c_aux_regAddress;
volatile uint8_t i2c_aux_slaveAddress;
volatile uint8_t* i2c_aux_txbuffer;
volatile uint8_t* i2c_aux_rxbuffer;
volatile uint8_t i2c_aux_nDABytes;
volatile int8_t i2c_aux_nRABytes;
#endif
@ -55,49 +55,68 @@ volatile int8_t i2c_aux_nRABytes;
void consoleLog(char *message)
{
#ifdef SERIAL_DEBUG
log_i("%s", message);
log_i("%s", message);
#endif
}
/* retarget the C library printf function to the USART */
int fputc(int ch, FILE *f)
#ifdef SERIAL_DEBUG
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE {
usart_data_transmit(USART_MAIN, (uint8_t)ch);
while(RESET == usart_flag_get(USART_MAIN, USART_FLAG_TBE));
return ch;
}
#ifdef __GNUC__
int _write(int file, char *data, int len) {
int i;
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
return len;
}
#endif
#endif
void toggle_led(uint32_t gpio_periph, uint32_t pin)
{
usart_data_transmit(USART_MAIN, (uint8_t)ch);
while(RESET == usart_flag_get(USART_MAIN, USART_FLAG_TBE));
return ch;
GPIO_OCTL(gpio_periph) ^= pin;
}
void introDemoLED(uint32_t tDelay)
void intro_demo_led(uint32_t tDelay)
{
int i;
int i;
for (i = 0; i < 6; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
delay_1ms(tDelay);
}
for (i = 0; i < 6; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
delay_1ms(tDelay);
}
for (i = 0; i < 2; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
delay_1ms(tDelay);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
}
for (i = 0; i < 2; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
delay_1ms(tDelay);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
}
}
@ -113,12 +132,12 @@ int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_
i2c_ReadWriteCmd = WRITE;
// assign inputs
i2c_status = -1;
i2c_status = -1;
i2c_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_regAddress = regAddr;
i2c_txbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
i2c_txbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
@ -170,12 +189,12 @@ int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t
i2c_ReadWriteCmd = READ;
// assign inputs
i2c_status = -1;
i2c_status = -1;
i2c_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_regAddress = regAddr;
i2c_rxbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
i2c_rxbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
@ -231,12 +250,12 @@ int8_t i2c_aux_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, ui
i2c_aux_ReadWriteCmd = WRITE;
// assign inputs
i2c_aux_status = -1;
i2c_aux_status = -1;
i2c_aux_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_aux_regAddress = regAddr;
i2c_aux_txbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
i2c_aux_txbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
@ -264,12 +283,12 @@ int8_t i2c_aux_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uin
i2c_aux_ReadWriteCmd = READ;
// assign inputs
i2c_aux_status = -1;
i2c_aux_status = -1;
i2c_aux_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_aux_regAddress = regAddr;
i2c_aux_rxbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
i2c_aux_rxbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);

2
add_nanolib.py Normal file
View File

@ -0,0 +1,2 @@
Import("env")
env.Append(LINKFLAGS=["--specs=nano.specs"])

40
boards/gd32f130c6.json Normal file
View File

@ -0,0 +1,40 @@
{
"build": {
"core": "gd32",
"cpu": "cortex-m3",
"extra_flags": "-DGD32F1x0 -DGD32F130_150 -D__GD32F130_SUBFAMILY -D__GD32F1x0_FAMILY",
"f_cpu": "72000000L",
"mcu": "gd32f130c6t6"
},
"connectivity": [
],
"debug": {
"default_tools": [
"stlink"
],
"jlink_device": "GD32F130C6",
"onboard_tools": [
"stlink"
],
"openocd_target": "stm32f1x",
"svd_path": "STM32F10x.svd"
},
"frameworks": [
"stm32cube",
"spl"
],
"name": "Generic GD32F130C6T6",
"upload": {
"maximum_ram_size": 4096,
"maximum_size": 32768,
"protocol": "stlink",
"protocols": [
"jlink",
"stlink",
"blackmagic",
"mbed"
]
},
"url": "https://www.gigadevice.com/microcontroller/gd32f130c6t6/",
"vendor": "GigaDevices"
}

BIN
build/VARIANT_DEBUG/firmware.axf
View File

Binary file not shown.

BIN
build/VARIANT_DEBUG/firmware.bin
View File

Binary file not shown.

1584
build/VARIANT_DEBUG/firmware.hex
View File

File diff suppressed because it is too large Load Diff

BIN
build/VARIANT_HOVERBOARD/firmware.axf
View File

Binary file not shown.

1436
build/VARIANT_HOVERBOARD/firmware.hex
View File

File diff suppressed because it is too large Load Diff

BIN
docs/pictures/GD_pics.pptx
View File

Binary file not shown.

BIN
docs/pictures/back.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.5 MiB

BIN
docs/pictures/front.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 2.9 MiB

BIN
docs/pictures/ledboard_pinout.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 361 KiB

BIN
framework-spl.zip Normal file
View File

Binary file not shown.

69
platformio.ini Normal file
View File

@ -0,0 +1,69 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[platformio]
include_dir = Inc
src_dir = Src
;=================== VARIANT SELECTION ==========================
;default_envs = VARIANT_DEBUG ; DEFAULT Variant
;default_envs = VARIANT_HOVERBOARD ; HOVERBOARD Variant
;================================================================
;================================================================
[env:VARIANT_DEBUG]
platform = ststm32
board = gd32f130c6
debug_tool = stlink
upload_protocol = stlink
framework = spl
extra_scripts = add_nanolib.py ; adds nanolib to reduce printf memory foot print
; Serial Port settings (make sure the COM port is correct)
monitor_port = COM5
monitor_speed = 38400
build_flags =
-IInc
-ISrc
-DUSE_STDPERIPH_DRIVER
-DGD32F130_150
-Wl,-T./GD32F130C6T_FLASH.ld
-Wl,-lc
-Wl,-lm
-g -ggdb
-D VARIANT_DEBUG
;================================================================
[env:VARIANT_HOVERBOARD]
platform = ststm32
board = gd32f130c6
debug_tool = stlink
upload_protocol = stlink
framework = spl
extra_scripts = add_nanolib.py
build_flags =
-IInc
-ISrc
-DUSE_STDPERIPH_DRIVER
-DGD32F130_150
-Wl,-T./GD32F130C6T_FLASH.ld
-Wl,-lc
-Wl,-lm
-g -ggdb
-D VARIANT_HOVERBOARD
;================================================================

3
startup_gd32f1x0.s
View File

@ -77,7 +77,8 @@ LoopFillZerobss:
/* Call the clock system intitialization function.*/
bl SystemInit
/* Call static constructors */
bl __libc_init_array
// nope no C++ code today
// bl __libc_init_array
/* Call the application's entry point.*/
bl main
bx lr