Merge pull request #10 from EmanuelFeru/hovercar-variant

Hovercar variant
This commit is contained in:
EmanuelFeru 2020-12-07 20:22:50 +01:00 committed by GitHub
commit b5de1a626d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
26 changed files with 2775 additions and 1653 deletions

View File

@ -45,14 +45,13 @@ 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"
- name: platformio
script: platformio run
language: python
python:
- "2.7"
install:
- pip install -U platformio
- platformio update
cache:
- directories: "~/.platformio"

View File

@ -27,8 +27,9 @@
// Ubuntu: define the desired build variant here if you want to use make in console
// or use VARIANT environment variable for example like "make -e VARIANT=VARIANT_DEBUG". Select only one at a time.
#if !defined(PLATFORMIO)
// #define VARIANT_DEBUG // Variant for debugging and checking the capabilities of the side-board
// #define VARIANT_HOVERBOARD // Variant for using the side-boards connected to the Hoverboard mainboard
// #define VARIANT_DEBUG // Variant for debugging and checking the capabilities of the side-board
// #define VARIANT_HOVERCAR // Variant for using the side-boards connected to the Hoverboard mainboard
// #define VARIANT_HOVERBOARD // Variant for using the side-boards connected to the Hoverboard mainboard
#endif
/* ==================================== DO NOT TOUCH SETTINGS ==================================== */
@ -40,7 +41,6 @@
// #define PRINTF_FLOAT_SUPPORT // [-] Uncomment this for printf to support float on Serial Debug. It will increase code size! Better to avoid it!
/* =============================================================================================== */
/* ==================================== SETTINGS MPU-6050 ==================================== */
#define MPU_SENSOR_ENABLE // [-] Enable flag for MPU-6050 sensor. Comment-out this flag to Disable the MPU sensor and reduce code size.
#define MPU_DMP_ENABLE // [-] Enable flag for MPU-6050 DMP (Digital Motion Processing) functionality.
@ -59,32 +59,57 @@
#define DMP_SHAKE_REJECT_TIME 40 // [ms] Set shake rejection time. Sets the length of time that the gyro must be outside of the DMP_SHAKE_REJECT_THRESH before taps are rejected. A mandatory 60 ms is added to this parameter.
#define DMP_SHAKE_REJECT_TIMEOUT 10 // [ms] 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.
/* ==================================== SETTINGS USART ==================================== */
#if defined(VARIANT_DEBUG)
#define SERIAL_DEBUG // [-] Define for Serial Debug via the serial port
#elif defined(VARIANT_HOVERBOARD)
#define SERIAL_CONTROL // [-] Define for Serial Control via the serial port
#define SERIAL_FEEDBACK // [-] Define for Serial Feedback via the serial port
#endif
#define USART_MAIN_BAUD 38400 // [bit/s] MAIN Serial Tx/Rx baud rate
#define SERIAL_START_FRAME 0xABCD // [-] Start frame definition for reliable serial communication
#define SERIAL_BUFFER_SIZE 64 // [bytes] Size of Serial Rx buffer. Make sure it is always larger than the 'Feedback' structure size
#define SERIAL_TIMEOUT 600 // [-] Number of wrong received data for Serial timeout detection. Depends on DELAY_IN_MAIN_LOOP
#define USART_MAIN_BAUD 115200 // [bit/s] MAIN Serial Tx/Rx baud rate
#define USART_AUX_BAUD 115200 // [bit/s] AUX Serial Tx/Rx baud rate
/* ==================================== SETTINGS AUX ==================================== */
// #define AUX45_USE_GPIO // [-] Use AUX4, AUX5 as GPIO ports
// #define AUX45_USE_I2C // [-] Use AUX4, AUX5 as I2C port
#define AUX45_USE_USART // [-] Use AUX4, AUX5 as USART port
#ifdef AUX45_USE_USART
#define USART_AUX_BAUD 38400 // [bit/s] AUX Serial Tx/Rx baud rate
#endif
#ifdef AUX45_USE_I2C
#define AUX_I2C_SPEED 100000 // [bit/s] Define I2C speed for communicating via AUX45 wires
#endif
/* ==================================== VARIANT DEBUG ==================================== */
#ifdef VARIANT_DEBUG
#define SERIAL_DEBUG // [-] Define for Serial Debug via the serial port
#define SERIAL_AUX_RX // [-] Use AUX4, AUX5 as USART port
// #define SERIAL_AUX_TX // [-] Use AUX4, AUX5 as USART port
#define CONTROL_IBUS
#define IBUS_NUM_CHANNELS 14 // Total number of IBUS channels to receive, even if they are not used.
#define IBUS_LENGTH 0x20
#define IBUS_COMMAND 0x40
#endif
/* ==================================== VARIANT HOVERCAR ==================================== */
#ifdef VARIANT_HOVERCAR
#define SERIAL_CONTROL // [-] Define for Serial Control via the serial port
#define SERIAL_FEEDBACK // [-] Define for Serial Feedback via the serial port
#define SERIAL_AUX_RX // [-] Use AUX4, AUX5 as USART port
// #define SERIAL_AUX_TX // [-] Use AUX4, AUX5 as USART port
#define CONTROL_IBUS
#define IBUS_NUM_CHANNELS 14 // Total number of IBUS channels to receive, even if they are not used.
#define IBUS_LENGTH 0x20
#define IBUS_COMMAND 0x40
#endif
/* ==================================== VARIANT HOVERBOARD ==================================== */
#ifdef VARIANT_HOVERBOARD
#define SERIAL_CONTROL // [-] Define for Serial Control via the serial port
#define SERIAL_FEEDBACK // [-] Define for Serial Feedback via the serial port
#endif
/* ==================================== VALIDATE SETTINGS ==================================== */
#if defined(SERIAL_DEBUG) && defined(SERIAL_CONTROL)
#error SERIAL_DEBUG and SERIAL_CONTROL not allowed. It is on the same cable.
@ -94,7 +119,8 @@
#error SERIAL_DEBUG and SERIAL_FEEDBACK not allowed. It is on the same cable.
#endif
#if defined(AUX45_USE_GPIO) && (defined(AUX45_USE_USART) || defined(AUX45_USE_I2C)) || (defined(AUX45_USE_USART) && defined(AUX45_USE_I2C))
#if defined(AUX45_USE_GPIO) && (defined(SERIAL_AUX_RX) || defined(SERIAL_AUX_TX) || defined(AUX45_USE_I2C)) \
|| ((defined(SERIAL_AUX_RX) || defined(SERIAL_AUX_TX)) && defined(AUX45_USE_I2C))
#error AUX45_USE_(GPIO,USART,I2C) not allowed in the same time. It is on the same cable.
#endif

View File

@ -28,156 +28,158 @@
#include "util.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!
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 ARRAY_LEN(x) (uint32_t)(sizeof(x) / sizeof(*(x)))
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define max(a, b) (((a) > (b)) ? (a) : (b))
#define i2c_write i2c_writeBytes
#define i2c_read i2c_readBytes
#define delay_ms delay_1ms
#define get_ms get_tick_count_ms
#define log_i printf // redirect the log_i debug function to printf
// #define _BV(bit) (1 << (bit))
#define ARRAY_LEN(x) (uint32_t)(sizeof(x) / sizeof(*(x)))
#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define max(a, b) (((a) > (b)) ? (a) : (b))
#define i2c_write i2c_writeBytes
#define i2c_read i2c_readBytes
#define delay_ms delay_1ms
#define get_ms get_tick_count_ms
#define log_i printf // redirect the log_i debug function to printf
/* =========================== 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)
#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 SENSOR2_GPIO_Port GPIOC
#define SENSOR2_Pin GPIO_PIN_14
#define SENSOR1_GPIO_Port GPIOA
#define SENSOR1_Pin GPIO_PIN_4
#define SENSOR2_GPIO_Port GPIOC
#define SENSOR2_Pin GPIO_PIN_14
/* =========================== Defines USART =========================== */
#define USART_GPIO_PORT GPIOA
#define USART_GPIO_CLK RCU_GPIOA
#define USART_AF GPIO_AF_1
#define USART_GPIO_PORT GPIOA
#define USART_GPIO_CLK RCU_GPIOA
#define USART_AF GPIO_AF_1
// USART ports number
#define USARTn 2
#define USARTn 2
// USART AUX, connected to USART0
#define USART_AUX USART0
#define USART0_CLK RCU_USART0
#define USART0_TX_PIN GPIO_PIN_9
#define USART0_RX_PIN GPIO_PIN_10
#define USART0_TX_DMA_CH DMA_CH1
#define USART0_RX_DMA_CH DMA_CH2
#define USART0_TDATA_ADDRESS ((uint32_t)0x40013828) // USART0: 0x4001 3800 - 0x4001 3BFF, Rx offset: 0x28, Tx offset: 0x24
#define USART0_RDATA_ADDRESS ((uint32_t)0x40013824)
// USART to Auxiliary, connected to USART0
#define USART_AUX USART0
#define USART_AUX_CLK RCU_USART0
#define USART_AUX_TX_PIN GPIO_PIN_9
#define USART_AUX_RX_PIN GPIO_PIN_10
// USART to Mainboard, connected to 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)
// USART MAIN, connected to USART1
#define USART_MAIN USART1
#define USART1_CLK RCU_USART1
#define USART1_TX_PIN GPIO_PIN_2
#define USART1_RX_PIN GPIO_PIN_3
#define USART1_TX_DMA_CH DMA_CH3
#define USART1_RX_DMA_CH DMA_CH4
#define USART1_TDATA_ADDRESS ((uint32_t)0x40004428) // USART1: 0x4000 4400 - 0x4000 47FF, Rx offset: 0x28, Tx offset: 0x24
#define USART1_RDATA_ADDRESS ((uint32_t)0x40004424)
#define PRINT_AUX (0x01)
/* =========================== 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_SCL_GPIO_Port GPIOB
#define MPU_SCL_Pin GPIO_PIN_6
#define MPU_SDA_GPIO_Port GPIOB
#define MPU_SDA_Pin GPIO_PIN_7
#define I2C_OWN_ADDRESS7 0x24
#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_SDA_GPIO_Port GPIOB
#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_SCL_GPIO_Port GPIOA
#define AUX_SCL_Pin GPIO_PIN_9
#define AUX_SDA_GPIO_Port GPIOA
#define AUX_SDA_Pin GPIO_PIN_10
#define AUX_I2C_OWN_ADDRESS7 0x34
#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_SDA_GPIO_Port GPIOA
#define AUX_SDA_Pin GPIO_PIN_10
#define AUX_I2C_OWN_ADDRESS7 0x34
#endif
/* =========================== Defines MPU-6050 =========================== */
#define RAD2DEG 57.295779513082323 // RAD2DEG = 180/pi. Example: angle[deg] = angle[rad] * RAD2DEG
#define q30 1073741824 // 1073741824 = 2^30
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define COMPASS_ON (0x04)
#define RAD2DEG 57.295779513082323 // RAD2DEG = 180/pi. Example: angle[deg] = angle[rad] * RAD2DEG
#define q30 1073741824 // 1073741824 = 2^30
#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;
int16_t y;
int16_t z;
int16_t x;
int16_t y;
int16_t z;
} Gyro;
typedef struct{
int16_t x;
int16_t y;
int16_t z;
int16_t x;
int16_t y;
int16_t z;
} Accel;
typedef struct{
int32_t w;
int32_t x;
int32_t y;
int32_t z;
int32_t w;
int32_t x;
int32_t y;
int32_t z;
} Quaternion;
typedef struct{
int16_t roll;
int16_t pitch;
int16_t yaw;
int16_t roll;
int16_t pitch;
int16_t yaw;
} Euler;
typedef struct {
Gyro gyro;
Accel accel;
Quaternion quat;
Euler euler;
int16_t temp;
Gyro gyro;
Accel accel;
Quaternion quat;
Euler euler;
int16_t temp;
} MPU_Data;
#endif // DEFINES_H

View File

@ -38,6 +38,8 @@ void PendSV_Handler(void);
/* SysTick handle function */
void SysTick_Handler(void);
/* USART0 handle function */
void USART0_IRQHandler(void);
/* USART1 handle function */
void USART1_IRQHandler(void);
/* I2C0 event handle function */
void I2C0_EV_IRQHandler(void);

View File

@ -24,12 +24,12 @@
#include "config.h"
/* Interrupt function declarations */
void I2C0_EventIRQ_Handler(void); // handle I2C0 event interrupt request
void I2C0_ErrorIRQ_Handler(void); // handle I2C0 error interrupt request
void I2C0_EventIRQ_Handler(void); // handle I2C0 event interrupt request
void I2C0_ErrorIRQ_Handler(void); // handle I2C0 error interrupt request
#ifdef AUX45_USE_I2C
void I2C1_EventIRQ_Handler(void); // handle I2C1 event interrupt request
void I2C1_ErrorIRQ_Handler(void); // handle I2C1 error interrupt request
void I2C1_EventIRQ_Handler(void); // handle I2C1 event interrupt request
void I2C1_ErrorIRQ_Handler(void); // handle I2C1 error interrupt request
#endif

View File

@ -54,7 +54,6 @@
/* Set up APIs */
int mpu_init(void);
int mpu_init_slave(void);
int mpu_set_bypass(unsigned char bypass_on);
/* Configuration APIs */
@ -104,18 +103,13 @@ 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_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_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);
@ -139,9 +133,13 @@ void mpu_calc_euler_angles(void);
void mpu_tap_func(unsigned char direction, unsigned char count);
void mpu_android_orient_func(unsigned char orientation);
/* Handle user input commands */
void mpu_handle_input(char c);
/* MPU Print data */
void mpu_print_to_console(void);
#endif // MPU_SENSOR_ENABLE
/* Handle user input commands */
void mpu_handle_input(char c);
#endif // MPU6050_H

View File

@ -23,18 +23,19 @@
#include <stdint.h>
/* Rx Structures USART */
/* Tx structure USART MAIN */
#ifdef SERIAL_CONTROL
typedef struct{
uint16_t start;
int16_t roll;
int16_t pitch;
int16_t yaw;
uint16_t sensors;
int16_t pitch; // Angle
int16_t dPitch; // Angle derivative
int16_t cmd1; // RC Channel 1
int16_t cmd2; // RC Channel 2
uint16_t sensors; // RC Switches and Optical sideboard sensors
uint16_t checksum;
} SerialSideboard;
#endif
/* Rx structure USART MAIN */
#ifdef SERIAL_FEEDBACK
typedef struct{
uint16_t start;
@ -49,16 +50,45 @@ typedef struct{
} SerialFeedback;
#endif
/* Tx structure USART AUX */
#ifdef SERIAL_AUX_TX
typedef struct{
uint16_t start;
int16_t signal1;
int16_t signal2;
uint16_t checksum;
} SerialAuxTx;
#endif
/* Rx structure USART AUX */
#ifdef SERIAL_AUX_RX
#ifdef CONTROL_IBUS
typedef struct{
uint8_t start;
uint8_t type;
uint8_t channels[IBUS_NUM_CHANNELS*2];
uint8_t checksuml;
uint8_t checksumh;
} SerialCommand;
#endif
#endif
/* general functions */
void consoleLog(char *message);
void toggle_led(uint32_t gpio_periph, uint32_t pin);
void intro_demo_led(uint32_t tDelay);
uint8_t switch_check(uint16_t ch, uint8_t type);
/* input initialization function */
void input_init(void);
/* usart read functions */
void usart_rx_check(void);
/* handle functions */
void handle_mpu6050(void);
void handle_sensors(void);
void handle_usart(void);
void handle_leds(void);
/* usart1 read functions */
void usart1_rx_check(void);
#ifdef SERIAL_DEBUG
void usart_process_debug(uint8_t *userCommand, uint32_t len);
#endif
@ -66,6 +96,15 @@ void usart_process_debug(uint8_t *userCommand, uint32_t len);
void usart_process_data(SerialFeedback *Feedback_in, SerialFeedback *Feedback_out);
#endif
/* usart0 read functions */
void usart0_rx_check(void);
#ifdef SERIAL_AUX_RX
void usart_process_command(SerialCommand *command_in, SerialCommand *command_out);
#endif
/* AUX Serial Print data */
void aux_print_to_console(void);
/* 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);

View File

@ -168,6 +168,153 @@
</TargetOption>
</Target>
<Target>
<TargetName>VARIANT_HOVERCAR</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
<CLKADS>8000000</CLKADS>
<OPTTT>
<gFlags>1</gFlags>
<BeepAtEnd>0</BeepAtEnd>
<RunSim>1</RunSim>
<RunTarget>0</RunTarget>
<RunAbUc>0</RunAbUc>
</OPTTT>
<OPTHX>
<HexSelection>1</HexSelection>
<FlashByte>65535</FlashByte>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
</OPTHX>
<OPTLEX>
<PageWidth>79</PageWidth>
<PageLength>66</PageLength>
<TabStop>8</TabStop>
<ListingPath>.\Listings\</ListingPath>
</OPTLEX>
<ListingPage>
<CreateCListing>1</CreateCListing>
<CreateAListing>1</CreateAListing>
<CreateLListing>1</CreateLListing>
<CreateIListing>0</CreateIListing>
<AsmCond>1</AsmCond>
<AsmSymb>1</AsmSymb>
<AsmXref>0</AsmXref>
<CCond>1</CCond>
<CCode>0</CCode>
<CListInc>0</CListInc>
<CSymb>0</CSymb>
<LinkerCodeListing>0</LinkerCodeListing>
</ListingPage>
<OPTXL>
<LMap>1</LMap>
<LComments>1</LComments>
<LGenerateSymbols>1</LGenerateSymbols>
<LLibSym>1</LLibSym>
<LLines>1</LLines>
<LLocSym>1</LLocSym>
<LPubSym>1</LPubSym>
<LXref>0</LXref>
<LExpSel>0</LExpSel>
</OPTXL>
<OPTFL>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<IsCurrentTarget>0</IsCurrentTarget>
</OPTFL>
<CpuCode>0</CpuCode>
<DebugOpt>
<uSim>0</uSim>
<uTrg>1</uTrg>
<sLdApp>1</sLdApp>
<sGomain>1</sGomain>
<sRbreak>1</sRbreak>
<sRwatch>1</sRwatch>
<sRmem>1</sRmem>
<sRfunc>1</sRfunc>
<sRbox>1</sRbox>
<tLdApp>1</tLdApp>
<tGomain>1</tGomain>
<tRbreak>1</tRbreak>
<tRwatch>1</tRwatch>
<tRmem>1</tRmem>
<tRfunc>0</tRfunc>
<tRbox>1</tRbox>
<tRtrace>1</tRtrace>
<sRSysVw>1</sRSysVw>
<tRSysVw>1</tRSysVw>
<sRunDeb>0</sRunDeb>
<sLrtime>0</sLrtime>
<bEvRecOn>1</bEvRecOn>
<nTsel>5</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
<sDlgPa></sDlgPa>
<sIfile></sIfile>
<tDll></tDll>
<tDllPa></tDllPa>
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<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>
<Name>UL2CM3(-O207 -S0 -C0 -FO7 -FD20000000 -FC800 -FN1 -FF0GD32F1x0_32 -FS08000000 -FL08000)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>0</periodic>
<aLwin>0</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
<aPa>0</aPa>
<viewmode>0</viewmode>
<vrSel>0</vrSel>
<aSym>0</aSym>
<aTbox>0</aTbox>
<AscS1>0</AscS1>
<AscS2>0</AscS2>
<AscS3>0</AscS3>
<aSer3>0</aSer3>
<eProf>0</eProf>
<aLa>0</aLa>
<aPa1>0</aPa1>
<AscS4>0</AscS4>
<aSer4>0</aSer4>
<StkLoc>0</StkLoc>
<TrcWin>0</TrcWin>
<newCpu>0</newCpu>
<uProt>0</uProt>
</DebugFlag>
<LintExecutable></LintExecutable>
<LintConfigFile></LintConfigFile>
<bLintAuto>0</bLintAuto>
<bAutoGenD>0</bAutoGenD>
<LntExFlags>0</LntExFlags>
<pMisraName></pMisraName>
<pszMrule></pszMrule>
<pSingCmds></pSingCmds>
<pMultCmds></pMultCmds>
</TargetOption>
</Target>
<Target>
<TargetName>VARIANT_HOVERBOARD</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>

View File

@ -622,6 +622,622 @@
</Group>
</Groups>
</Target>
<Target>
<TargetName>VARIANT_HOVERCAR</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pCCUsed>5060422::V5.06 update 4 (build 422)::ARMCC</pCCUsed>
<TargetOption>
<TargetCommonOption>
<Device>GD32F130C6</Device>
<Vendor>GigaDevice</Vendor>
<Cpu>IRAM(0x20000000-0x20000FFF) IROM(0x08000000-0x08007FFF) CLOCK(8000000) CPUTYPE("Cortex-M3")</Cpu>
<FlashUtilSpec></FlashUtilSpec>
<StartupFile>"Startup\GD\GD32F1x0\startup_gd32f1x0.s" ("GD32F1x0 Startup Code")</StartupFile>
<FlashDriverDll>UL2CM3(-O207 -S0 -C0 -FO7 -FD20000000 -FC800 -FN1 -FF0GD32F1x0_32 -FS08000000 -FL08000)</FlashDriverDll>
<DeviceId>7715</DeviceId>
<RegisterFile>gd32f1x0.h</RegisterFile>
<MemoryEnv></MemoryEnv>
<Cmp></Cmp>
<Asm></Asm>
<Linker></Linker>
<OHString></OHString>
<InfinionOptionDll></InfinionOptionDll>
<SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>SFD\GD\GD32F1x0\GD32F1x0.SFR</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv>
<BinPath></BinPath>
<IncludePath></IncludePath>
<LibPath></LibPath>
<RegisterFilePath>GD\GD32F1x0\</RegisterFilePath>
<DBRegisterFilePath>GD\GD32F1x0\</DBRegisterFilePath>
<TargetStatus>
<Error>0</Error>
<ExitCodeStop>0</ExitCodeStop>
<ButtonStop>0</ButtonStop>
<NotGenerated>0</NotGenerated>
<InvalidFlash>1</InvalidFlash>
</TargetStatus>
<OutputDirectory>.\Objects\</OutputDirectory>
<OutputName>firmware</OutputName>
<CreateExecutable>1</CreateExecutable>
<CreateLib>0</CreateLib>
<CreateHexFile>1</CreateHexFile>
<DebugInformation>1</DebugInformation>
<BrowseInformation>1</BrowseInformation>
<ListingPath>.\Listings\</ListingPath>
<HexFormatSelection>1</HexFormatSelection>
<Merge32K>0</Merge32K>
<CreateBatchFile>0</CreateBatchFile>
<BeforeCompile>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopU1X>0</nStopU1X>
<nStopU2X>0</nStopU2X>
</BeforeCompile>
<BeforeMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopB1X>0</nStopB1X>
<nStopB2X>0</nStopB2X>
</BeforeMake>
<AfterMake>
<RunUserProg1>1</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name>$K\ARM\ARMCC\bin\fromelf.exe --bin --output=.\Objects\@L.bin !L</UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopA1X>0</nStopA1X>
<nStopA2X>0</nStopA2X>
</AfterMake>
<SelectedForBatchBuild>0</SelectedForBatchBuild>
<SVCSIdString></SVCSIdString>
</TargetCommonOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>1</IncludeInBuild>
<AlwaysBuild>0</AlwaysBuild>
<GenerateAssemblyFile>0</GenerateAssemblyFile>
<AssembleAssemblyFile>0</AssembleAssemblyFile>
<PublicsOnly>0</PublicsOnly>
<StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<DllOption>
<SimDllName>SARMCM3.DLL</SimDllName>
<SimDllArguments> -REMAP</SimDllArguments>
<SimDlgDll>DCM.DLL</SimDlgDll>
<SimDlgDllArguments>-pCM3</SimDlgDllArguments>
<TargetDllName>SARMCM3.DLL</TargetDllName>
<TargetDllArguments></TargetDllArguments>
<TargetDlgDll>TCM.DLL</TargetDlgDll>
<TargetDlgDllArguments>-pCM3</TargetDlgDllArguments>
</DllOption>
<DebugOption>
<OPTHX>
<HexSelection>1</HexSelection>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
<Oh166RecLen>16</Oh166RecLen>
</OPTHX>
<Simulator>
<UseSimulator>0</UseSimulator>
<LoadApplicationAtStartup>1</LoadApplicationAtStartup>
<RunToMain>1</RunToMain>
<RestoreBreakpoints>1</RestoreBreakpoints>
<RestoreWatchpoints>1</RestoreWatchpoints>
<RestoreMemoryDisplay>1</RestoreMemoryDisplay>
<RestoreFunctions>1</RestoreFunctions>
<RestoreToolbox>1</RestoreToolbox>
<LimitSpeedToRealTime>0</LimitSpeedToRealTime>
<RestoreSysVw>1</RestoreSysVw>
</Simulator>
<Target>
<UseTarget>1</UseTarget>
<LoadApplicationAtStartup>1</LoadApplicationAtStartup>
<RunToMain>1</RunToMain>
<RestoreBreakpoints>1</RestoreBreakpoints>
<RestoreWatchpoints>1</RestoreWatchpoints>
<RestoreMemoryDisplay>1</RestoreMemoryDisplay>
<RestoreFunctions>0</RestoreFunctions>
<RestoreToolbox>1</RestoreToolbox>
<RestoreTracepoints>1</RestoreTracepoints>
<RestoreSysVw>1</RestoreSysVw>
</Target>
<RunDebugAfterBuild>0</RunDebugAfterBuild>
<TargetSelection>5</TargetSelection>
<SimDlls>
<CpuDll></CpuDll>
<CpuDllArguments></CpuDllArguments>
<PeripheralDll></PeripheralDll>
<PeripheralDllArguments></PeripheralDllArguments>
<InitializationFile></InitializationFile>
</SimDlls>
<TargetDlls>
<CpuDll></CpuDll>
<CpuDllArguments></CpuDllArguments>
<PeripheralDll></PeripheralDll>
<PeripheralDllArguments></PeripheralDllArguments>
<InitializationFile></InitializationFile>
<Driver>STLink\ST-LINKIII-KEIL_SWO.dll</Driver>
</TargetDlls>
</DebugOption>
<Utilities>
<Flash1>
<UseTargetDll>1</UseTargetDll>
<UseExternalTool>0</UseExternalTool>
<RunIndependent>0</RunIndependent>
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
<Capability>1</Capability>
<DriverSelection>4096</DriverSelection>
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3>
<Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities>
<TargetArmAds>
<ArmAdsMisc>
<GenerateListings>0</GenerateListings>
<asHll>1</asHll>
<asAsm>1</asAsm>
<asMacX>1</asMacX>
<asSyms>1</asSyms>
<asFals>1</asFals>
<asDbgD>1</asDbgD>
<asForm>1</asForm>
<ldLst>0</ldLst>
<ldmm>1</ldmm>
<ldXref>1</ldXref>
<BigEnd>0</BigEnd>
<AdsALst>1</AdsALst>
<AdsACrf>1</AdsACrf>
<AdsANop>0</AdsANop>
<AdsANot>0</AdsANot>
<AdsLLst>1</AdsLLst>
<AdsLmap>1</AdsLmap>
<AdsLcgr>1</AdsLcgr>
<AdsLsym>1</AdsLsym>
<AdsLszi>1</AdsLszi>
<AdsLtoi>1</AdsLtoi>
<AdsLsun>1</AdsLsun>
<AdsLven>1</AdsLven>
<AdsLsxf>1</AdsLsxf>
<RvctClst>0</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M3"</AdsCpuType>
<RvctDeviceName></RvctDeviceName>
<mOS>0</mOS>
<uocRom>0</uocRom>
<uocRam>0</uocRam>
<hadIROM>1</hadIROM>
<hadIRAM>1</hadIRAM>
<hadXRAM>0</hadXRAM>
<uocXRam>0</uocXRam>
<RvdsVP>0</RvdsVP>
<hadIRAM2>0</hadIRAM2>
<hadIROM2>0</hadIROM2>
<StupSel>8</StupSel>
<useUlib>1</useUlib>
<EndSel>0</EndSel>
<uLtcg>0</uLtcg>
<nSecure>0</nSecure>
<RoSelD>3</RoSelD>
<RwSelD>3</RwSelD>
<CodeSel>0</CodeSel>
<OptFeed>0</OptFeed>
<NoZi1>0</NoZi1>
<NoZi2>0</NoZi2>
<NoZi3>0</NoZi3>
<NoZi4>0</NoZi4>
<NoZi5>0</NoZi5>
<Ro1Chk>0</Ro1Chk>
<Ro2Chk>0</Ro2Chk>
<Ro3Chk>0</Ro3Chk>
<Ir1Chk>1</Ir1Chk>
<Ir2Chk>0</Ir2Chk>
<Ra1Chk>0</Ra1Chk>
<Ra2Chk>0</Ra2Chk>
<Ra3Chk>0</Ra3Chk>
<Im1Chk>1</Im1Chk>
<Im2Chk>0</Im2Chk>
<OnChipMemories>
<Ocm1>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm1>
<Ocm2>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm2>
<Ocm3>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm3>
<Ocm4>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm4>
<Ocm5>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm5>
<Ocm6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm6>
<IRAM>
<Type>0</Type>
<StartAddress>0x20000000</StartAddress>
<Size>0x1000</Size>
</IRAM>
<IROM>
<Type>1</Type>
<StartAddress>0x8000000</StartAddress>
<Size>0x8000</Size>
</IROM>
<XRAM>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</XRAM>
<OCR_RVCT1>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT1>
<OCR_RVCT2>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT2>
<OCR_RVCT3>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT3>
<OCR_RVCT4>
<Type>1</Type>
<StartAddress>0x8000000</StartAddress>
<Size>0x8000</Size>
</OCR_RVCT4>
<OCR_RVCT5>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT5>
<OCR_RVCT6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT6>
<OCR_RVCT7>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT7>
<OCR_RVCT8>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT8>
<OCR_RVCT9>
<Type>0</Type>
<StartAddress>0x20000000</StartAddress>
<Size>0x1000</Size>
</OCR_RVCT9>
<OCR_RVCT10>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT10>
</OnChipMemories>
<RvctStartVector></RvctStartVector>
</ArmAdsMisc>
<Cads>
<interw>1</interw>
<Optim>4</Optim>
<oTime>0</oTime>
<SplitLS>0</SplitLS>
<OneElfS>1</OneElfS>
<Strict>0</Strict>
<EnumInt>0</EnumInt>
<PlainCh>0</PlainCh>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<wLevel>2</wLevel>
<uThumb>0</uThumb>
<uSurpInc>0</uSurpInc>
<uC99>1</uC99>
<useXO>0</useXO>
<v6Lang>1</v6Lang>
<v6LangP>1</v6LangP>
<vShortEn>1</vShortEn>
<vShortWch>1</vShortWch>
<v6Lto>0</v6Lto>
<v6WtE>0</v6WtE>
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define>USE_STDPERIPH_DRIVER,GD32F130_150,VARIANT_HOVERCAR</Define>
<Undefine></Undefine>
<IncludePath>..\Inc;..\Drivers\CMSIS\Include;..\Drivers\CMSIS;..\Drivers\GD32F1x0_standard_peripheral\Include</IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>1</interw>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<thumb>0</thumb>
<SplitLS>0</SplitLS>
<SwStkChk>0</SwStkChk>
<NoWarn>0</NoWarn>
<uSurpInc>0</uSurpInc>
<useXO>0</useXO>
<uClangAs>0</uClangAs>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Aads>
<LDads>
<umfTarg>1</umfTarg>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<noStLib>0</noStLib>
<RepFail>1</RepFail>
<useFile>0</useFile>
<TextAddressRange>0x08000000</TextAddressRange>
<DataAddressRange>0x20000000</DataAddressRange>
<pXoBase></pXoBase>
<ScatterFile>.\Objects\sideboard-hack.sct</ScatterFile>
<IncludeLibs></IncludeLibs>
<IncludeLibsPath></IncludeLibsPath>
<Misc></Misc>
<LinkerInputFile></LinkerInputFile>
<DisabledWarnings></DisabledWarnings>
</LDads>
</TargetArmAds>
</TargetOption>
<Groups>
<Group>
<GroupName>Startup</GroupName>
<Files>
<File>
<FileName>startup_gd32f1x0.s</FileName>
<FileType>2</FileType>
<FilePath>.\startup_gd32f1x0.s</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>CMSIS</GroupName>
<Files>
<File>
<FileName>system_gd32f1x0.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\CMSIS\Source\system_gd32f1x0.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Peripheral</GroupName>
<Files>
<File>
<FileName>gd32f1x0_adc.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_adc.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_can.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_can.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_cec.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_cec.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_cmp.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_cmp.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_crc.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_crc.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_dac.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_dac.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_dbg.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_dbg.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_dma.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_dma.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_exti.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_exti.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_fmc.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_fmc.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_fwdgt.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_fwdgt.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_gpio.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_i2c.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_i2c.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_ivref.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_ivref.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_misc.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_misc.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_opa.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_opa.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_pmu.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_pmu.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_rcu.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_rcu.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_rtc.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_rtc.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_slcd.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_slcd.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_spi.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_spi.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_syscfg.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_syscfg.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_timer.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_timer.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_tsi.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_tsi.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_usart.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_usart.c</FilePath>
</File>
<File>
<FileName>gd32f1x0_wwdgt.c</FileName>
<FileType>1</FileType>
<FilePath>..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_wwdgt.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Src</GroupName>
<Files>
<File>
<FileName>gd32f1x0_it.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\gd32f1x0_it.c</FilePath>
</File>
<File>
<FileName>main.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\main.c</FilePath>
</File>
<File>
<FileName>setup.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\setup.c</FilePath>
</File>
<File>
<FileName>systick.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\systick.c</FilePath>
</File>
<File>
<FileName>util.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\util.c</FilePath>
</File>
<File>
<FileName>i2c_it.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\i2c_it.c</FilePath>
</File>
<File>
<FileName>mpu6050.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\mpu6050.c</FilePath>
</File>
<File>
<FileName>mpu6050_dmp.c</FileName>
<FileType>1</FileType>
<FilePath>..\Src\mpu6050_dmp.c</FilePath>
</File>
<File>
<FileName>config.h</FileName>
<FileType>5</FileType>
<FilePath>..\Inc\config.h</FilePath>
</File>
</Files>
</Group>
</Groups>
</Target>
<Target>
<TargetName>VARIANT_HOVERBOARD</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>

View File

@ -16,6 +16,7 @@ Table of Contents
* [Example Variants ](#example-variants)
* [Flashing](#flashing)
* [3D Visualization Demo](#3d-visualization-demo)
* [Wiring iBUS Receiver](#wiring-ibus-receiver)
* [Contributions](#contributions)
---
@ -49,8 +50,9 @@ For more details see the [MPU-6050 datasheet](/docs/1_MPU-6000-Datasheet.pdf) an
## Example Variants
This firmware offers currently these variants (selectable in [platformio.ini](/platformio.ini) or [config.h](/Inc/config.h)):
- **VARIANT_DEBUG**: In this variant the user can interact with sideboard by sending commands via a Serial Monitor to observe and check the capabilities of the sideboard.
- **VARIANT_HOVERBOARD**: In this variant the sideboard is communicating with the mainboard of a hoverboard using the [FOC firmware repository](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC).
- **VARIANT_DEBUG**: In this variant the user can interact with sideboard by sending commands via a Serial Monitor to observe and check the capabilities of the sideboard
- **VARIANT_HOVERCAR**: This variant can be used for Hovercar build. An RC receiver with iBUS protocol can be connected to the AUX serial Rx pin (see [schematic](#wiring-ibus-receiver))
- **VARIANT_HOVERBOARD**: In this variant the sideboard is communicating with the mainboard of a hoverboard using the [FOC firmware repository](https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC)
Of course the firmware can be further customized for other needs or projects.
@ -66,24 +68,11 @@ To build and flash choose one of the following methods:
### Method 1: Using Platformio IDE
Because the [GD32F130C6T6](/docs/GD32F130xx-Datasheet_Rev3.3.pdf) 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).
Note the [GD32F130C6T6](/docs/GD32F130xx-Datasheet_Rev3.3.pdf) is now supported by [PlatformIO](https://platformio.org/), see [https://github.com/maxgerhardt/pio-gd32f130c6](https://github.com/maxgerhardt/pio-gd32f130c6).
### Method 2: Using Keil uVision
- in [Keil uVision](https://www.keil.com/download/product/), open the [sideboard-hack.uvproj](/MDK-ARM/)
@ -116,6 +105,14 @@ By [converting Quaternions to Euler angles](https://en.wikipedia.org/wiki/Conver
![sketch_pic](/docs/pictures/sketch_processing_pic.png)
---
## Wiring iBUS Receiver
An RC transmitter (Flysky [FS-i6S](https://www.banggood.com/custlink/3KvdPnfDPc) or [FS-i6X](https://www.banggood.com/custlink/KmDy5swKPD)) can be connected to the sideboard using an [FS-iA6B](https://www.banggood.com/custlink/KD3RFswKcT) receiver as shown in the following schematic:
![wiring_iBUS_pic](/docs/pictures/wiring_ibus_rc.png)
---
## Contributions

View File

@ -118,8 +118,22 @@ void PendSV_Handler(void)
*/
void SysTick_Handler(void)
{
tick_count_increment();
delay_decrement();
tick_count_increment();
delay_decrement();
}
/*!
\brief this function handles the USART0 interrupt request
\param[in] none
\param[out] none
\retval none
*/
void USART0_IRQHandler(void)
{
if(RESET != usart_interrupt_flag_get(USART0, USART_INT_FLAG_IDLE)) { // Check for IDLE line interrupt
usart_flag_clear(USART0, USART_FLAG_IDLE); // Clear IDLE line flag (otherwise it will continue to enter interrupt)
usart0_rx_check(); // Check for data to process
}
}
/*!
@ -129,10 +143,10 @@ void SysTick_Handler(void)
\retval none
*/
void USART1_IRQHandler(void)
{
{
if(RESET != usart_interrupt_flag_get(USART1, USART_INT_FLAG_IDLE)) { // Check for IDLE line interrupt
usart_flag_clear(USART1, USART_FLAG_IDLE); // Clear IDLE line flag (otherwise it will continue to enter interrupt)
usart_rx_check(); // Check for data to process
usart1_rx_check(); // Check for data to process
}
}

View File

@ -47,117 +47,117 @@ extern volatile int8_t i2c_aux_nRABytes;
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--;
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
uint16_t k;
if (i2c_ReadWriteCmd == WRITE) { // check for WRITE command
// ======================================== READ ========================================
// --------------------------------------------------------------------------------------
// | Master | S | AD+W | | RA | | S | AD+R | | ACK | | NACK | P |
// | Slave | | | ACK | | ACK | | | ACK | DATA | | DATA | | |
// --------------------------------------------------------------------------------------
// <---------- Phase 1 ----------> <---------------- Phase 2 ---------------->
// 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
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 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--;
}
// ======================================== WRITE ========================================
// --------------------------------------------------------------------
// | Master | S | AD+W | | RA | | DATA | | DATA | | P |
// | Slave | | | ACK | | ACK | | ACK | | ACK | |
// --------------------------------------------------------------------
// 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
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
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);
}
}
}
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
// ======================================== READ ========================================
// --------------------------------------------------------------------------------------
// | Master | S | AD+W | | RA | | S | AD+R | | ACK | | NACK | P |
// | Slave | | | ACK | | ACK | | | ACK | DATA | | DATA | | |
// --------------------------------------------------------------------------------------
// <---------- Phase 1 ----------> <---------------- Phase 2 ---------------->
// 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
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 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
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
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);
}
}
}
}
}
}
}
}
@ -198,6 +198,7 @@ void I2C0_ErrorIRQ_Handler(void)
if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_PECERR)){
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_PECERR);
}
/* disable the error interrupt */
i2c_interrupt_disable(I2C0,I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
@ -212,115 +213,115 @@ void I2C0_ErrorIRQ_Handler(void)
*/
void I2C1_EventIRQ_Handler(void)
{
uint16_t k;
if (i2c_aux_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(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
uint16_t k;
if (i2c_aux_ReadWriteCmd == WRITE) { // check for WRITE command
// ======================================== WRITE ========================================
// --------------------------------------------------------------------
// | Master | S | AD+W | | RA | | DATA | | DATA | | P |
// | Slave | | | ACK | | ACK | | ACK | | ACK | |
// --------------------------------------------------------------------
// ======================================== READ ========================================
// --------------------------------------------------------------------------------------
// | Master | S | AD+W | | RA | | S | AD+R | | ACK | | NACK | P |
// | Slave | | | ACK | | ACK | | | ACK | DATA | | DATA | | |
// --------------------------------------------------------------------------------------
// <---------- Phase 1 ----------> <---------------- Phase 2 ---------------->
// 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
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 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--;
}
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
// 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
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
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);
}
}
}
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
// ======================================== READ ========================================
// --------------------------------------------------------------------------------------
// | Master | S | AD+W | | RA | | S | AD+R | | ACK | | NACK | P |
// | Slave | | | ACK | | ACK | | | ACK | DATA | | DATA | | |
// --------------------------------------------------------------------------------------
// <---------- Phase 1 ----------> <---------------- Phase 2 ---------------->
// 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
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 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
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
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);
}
}
}
}
}
}
/*!
@ -365,6 +366,7 @@ void I2C1_ErrorIRQ_Handler(void)
if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_PECERR)){
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_PECERR);
}
/* disable the error interrupt */
i2c_interrupt_disable(I2C1,I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}

View File

@ -29,136 +29,35 @@
#include "mpu6050.h"
#include "mpu6050_dmp.h"
#ifdef SERIAL_CONTROL
extern SerialSideboard Sideboard;
#endif
#ifdef SERIAL_FEEDBACK
extern SerialFeedback Feedback;
extern uint16_t timeoutCntSerial; // Timeout counter for Rx Serial command
extern uint8_t timeoutFlagSerial; // 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
extern ErrStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
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()
uint32_t main_loop_counter; // main loop counter to perform task scheduling inside main()
int main(void)
{
systick_config(); // SysTick config
gpio_config(); // GPIO config
usart_nvic_config(); // USART interrupt configuration
usart_config(USART_MAIN, USART_MAIN_BAUD); // USART config
i2c_config(); // I2C config
i2c_nvic_config(); // I2C interrupt configuration
input_init(); // Input initialization
{
systick_config(); // SysTick config
gpio_config(); // GPIO config
while(1) {
delay_1ms(DELAY_IN_MAIN_LOOP);
// ==================================== LEDs Handling ====================================
// 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); }
if (Feedback.cmdLed & LED4_SET) { gpio_bit_set(AUX3_GPIO_Port, AUX3_Pin); } else { gpio_bit_reset(AUX3_GPIO_Port, AUX3_Pin); }
}
#endif
usart_config(USART_MAIN, USART_MAIN_BAUD); // USART MAIN config
#if defined(SERIAL_AUX_RX) || defined(SERIAL_AUX_TX)
usart_config(USART_AUX, USART_AUX_BAUD); // USART AUX config
#endif
usart_nvic_config(); // USART interrupt configuration
// ==================================== MPU-6050 Handling ====================================
#ifdef MPU_SENSOR_ENABLE
// Get MPU data. Because the MPU-6050 interrupt pin is not wired we have to check DMP data by pooling periodically
if (SUCCESS == mpuStatus) {
mpu_get_data();
} else if (ERROR == mpuStatus && main_loop_counter % 100 == 0) {
toggle_led(LED1_GPIO_Port, LED1_Pin); // Toggle the Red LED every 100 ms
}
// Print MPU data to Console
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
#endif
i2c_config(); // I2C config
i2c_nvic_config(); // I2C interrupt configuration
input_init(); // Input initialization
// ==================================== SENSORS Handling ====================================
sensor1_read = gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin);
while(1) {
// SENSOR1
if (sensor1 == RESET && sensor1_read == SET) {
// Sensor ACTIVE: Do something here (one time task on activation)
sensor1 = SET;
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
consoleLog("-- SENSOR 1 Active --\n");
} else if(sensor1 == SET && sensor1_read == RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor1 = RESET;
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
consoleLog("-- SENSOR 1 Deactive --\n");
}
// SENSOR2
if (sensor2 == RESET && sensor2_read == SET) {
// Sensor ACTIVE: Do something here (one time task on activation)
sensor2 = SET;
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
consoleLog("-- SENSOR 2 Active --\n");
} else if (sensor2 == SET && sensor2_read == RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor2 = RESET;
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
consoleLog("-- SENSOR 2 Deactive --\n");
}
delay_1ms(DELAY_IN_MAIN_LOOP);
if (sensor1 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
// ==================================== SERIAL Tx/Rx Handling ====================================
#ifdef SERIAL_CONTROL
// To transmit on USART
if (main_loop_counter % 5 == 0 && dma_transfer_number_get(DMA_CH3) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
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) | (mpuStatus << 2));
Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.roll ^ Sideboard.pitch ^ Sideboard.yaw ^ Sideboard.sensors);
dma_channel_disable(DMA_CH3);
DMA_CHCNT(DMA_CH3) = sizeof(Sideboard);
DMA_CHMADDR(DMA_CH3) = (uint32_t)&Sideboard;
dma_channel_enable(DMA_CH3);
}
#endif
#ifdef SERIAL_FEEDBACK
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial = 1; // Timeout detected
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
}
if (timeoutFlagSerial && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired
toggle_led(LED3_GPIO_Port, LED3_Pin); // Toggle the Yellow LED every 100 ms
}
#endif
main_loop_counter++;
}
handle_mpu6050(); // Handle of the MPU-6050 IMU sensor
handle_sensors(); // Handle of the optical sensors
handle_usart(); // Handle of the USART data
handle_leds(); // Handle of the sideboard LEDs
main_loop_counter++;
}
}

File diff suppressed because it is too large Load Diff

View File

@ -29,290 +29,326 @@
// Private variables
static rcu_periph_enum USART_CLK[USARTn] = { USART_AUX_CLK,
USART_MAIN_CLK
};
static rcu_periph_enum USART_CLK[USARTn] = { USART0_CLK,
USART1_CLK
};
static uint32_t USART_TX_PIN[USARTn] = { USART_AUX_TX_PIN,
USART_MAIN_TX_PIN
};
static uint32_t USART_TX_PIN[USARTn] = { USART0_TX_PIN,
USART1_TX_PIN
};
static uint32_t USART_RX_PIN[USARTn] = { USART0_RX_PIN,
USART1_RX_PIN
};
static dma_channel_enum USART_TX_DMA_CH[USARTn] = { USART0_TX_DMA_CH,
USART1_TX_DMA_CH
};
static dma_channel_enum USART_RX_DMA_CH[USARTn] = { USART0_RX_DMA_CH,
USART1_RX_DMA_CH
};
static uint32_t USART_TDATA_ADDRESS[USARTn] = { USART0_TDATA_ADDRESS,
USART1_TDATA_ADDRESS
};
static uint32_t USART_RDATA_ADDRESS[USARTn] = { USART0_RDATA_ADDRESS,
USART1_RDATA_ADDRESS
};
static uint32_t USART_RX_PIN[USARTn] = { USART_AUX_RX_PIN,
USART_MAIN_RX_PIN
};
void gpio_config(void) {
/* =========================== Configure LEDs GPIOs =========================== */
/* enable the GPIO clock */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
/* configure GPIO port */
gpio_mode_set(LED1_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED1_Pin);
gpio_mode_set(LED2_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED2_Pin);
gpio_mode_set(LED3_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED3_Pin);
gpio_mode_set(LED4_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED4_Pin);
gpio_mode_set(LED5_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED5_Pin);
gpio_output_options_set(LED1_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED1_Pin);
gpio_output_options_set(LED2_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED2_Pin);
gpio_output_options_set(LED3_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED3_Pin);
gpio_output_options_set(LED4_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED4_Pin);
gpio_output_options_set(LED5_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED5_Pin);
/* reset GPIO pin */
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);
/* =========================== Configure Sensors GPIOs =========================== */
/* enable the GPIO clock */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOC);
/* configure GPIO port */
gpio_mode_set(SENSOR1_GPIO_Port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, SENSOR1_Pin);
gpio_mode_set(SENSOR2_GPIO_Port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, SENSOR2_Pin);
/* =========================== Configure I2C GPIOs =========================== */
/* enable I2C clock */
rcu_periph_clock_enable(RCU_GPIOB);
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);
/* configure GPIO port */
gpio_mode_set(MPU_SCL_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, MPU_SCL_Pin);
gpio_output_options_set(MPU_SCL_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, MPU_SCL_Pin);
gpio_mode_set(MPU_SDA_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, MPU_SDA_Pin);
gpio_output_options_set(MPU_SDA_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, MPU_SDA_Pin);
#ifdef AUX45_USE_I2C
/* enable I2C clock */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(AUX_RCU_I2C);
/* connect PB6 to I2C_SCL and PB7 to I2C_SDA */
gpio_af_set(AUX_SCL_GPIO_Port, GPIO_AF_1, AUX_SCL_Pin);
gpio_af_set(AUX_SDA_GPIO_Port, GPIO_AF_1, AUX_SDA_Pin);
/* configure GPIO port */
gpio_mode_set(AUX_SCL_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, AUX_SCL_Pin);
gpio_output_options_set(AUX_SCL_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, AUX_SCL_Pin);
gpio_mode_set(AUX_SDA_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, AUX_SDA_Pin);
gpio_output_options_set(AUX_SDA_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, AUX_SDA_Pin);
#endif
/* =========================== Configure AUX GPIOs =========================== */
/* configure AUX GPIO port */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_GPIOC);
/* configure GPIO port - inputs */
gpio_mode_set(AUX1_PU_GPIO_Port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, AUX1_PU_Pin);
/* configure GPIO port - outputs */
gpio_mode_set(AUX2_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX2_Pin);
gpio_mode_set(AUX3_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX3_Pin);
gpio_output_options_set(AUX2_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX2_Pin);
gpio_output_options_set(AUX3_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX3_Pin);
/* reset GPIO pin */
gpio_bit_reset(AUX2_GPIO_Port, AUX2_Pin);
gpio_bit_reset(AUX3_GPIO_Port, AUX3_Pin);
#ifdef AUX45_USE_GPIO
/* configure GPIO port - outputs */
gpio_mode_set(AUX4_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX4_Pin);
gpio_mode_set(AUX5_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX5_Pin);
gpio_output_options_set(AUX4_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX4_Pin);
gpio_output_options_set(AUX5_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX5_Pin);
/* reset GPIO pin */
gpio_bit_reset(AUX4_GPIO_Port, AUX4_Pin);
gpio_bit_reset(AUX5_GPIO_Port, AUX5_Pin);
#endif
/* =========================== Configure LEDs GPIOs =========================== */
/* enable the GPIO clock */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
/* configure GPIO port */
gpio_mode_set(LED1_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED1_Pin);
gpio_mode_set(LED2_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED2_Pin);
gpio_mode_set(LED3_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED3_Pin);
gpio_mode_set(LED4_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED4_Pin);
gpio_mode_set(LED5_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, LED5_Pin);
gpio_output_options_set(LED1_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED1_Pin);
gpio_output_options_set(LED2_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED2_Pin);
gpio_output_options_set(LED3_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED3_Pin);
gpio_output_options_set(LED4_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED4_Pin);
gpio_output_options_set(LED5_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, LED5_Pin);
/* reset GPIO pin */
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);
/* =========================== Configure Sensors GPIOs =========================== */
/* enable the GPIO clock */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOC);
/* configure GPIO port */
gpio_mode_set(SENSOR1_GPIO_Port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, SENSOR1_Pin);
gpio_mode_set(SENSOR2_GPIO_Port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, SENSOR2_Pin);
/* =========================== Configure I2C GPIOs =========================== */
/* enable I2C clock */
rcu_periph_clock_enable(RCU_GPIOB);
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);
/* configure GPIO port */
gpio_mode_set(MPU_SCL_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, MPU_SCL_Pin);
gpio_output_options_set(MPU_SCL_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, MPU_SCL_Pin);
gpio_mode_set(MPU_SDA_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, MPU_SDA_Pin);
gpio_output_options_set(MPU_SDA_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, MPU_SDA_Pin);
#ifdef AUX45_USE_I2C
/* enable I2C clock */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(AUX_RCU_I2C);
/* connect PB6 to I2C_SCL and PB7 to I2C_SDA */
gpio_af_set(AUX_SCL_GPIO_Port, GPIO_AF_1, AUX_SCL_Pin);
gpio_af_set(AUX_SDA_GPIO_Port, GPIO_AF_1, AUX_SDA_Pin);
/* configure GPIO port */
gpio_mode_set(AUX_SCL_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, AUX_SCL_Pin);
gpio_output_options_set(AUX_SCL_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, AUX_SCL_Pin);
gpio_mode_set(AUX_SDA_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, AUX_SDA_Pin);
gpio_output_options_set(AUX_SDA_GPIO_Port, GPIO_OTYPE_OD, GPIO_OSPEED_50MHZ, AUX_SDA_Pin);
#endif
/* =========================== Configure AUX GPIOs =========================== */
/* configure AUX GPIO port */
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_GPIOC);
/* configure GPIO port - inputs */
gpio_mode_set(AUX1_PU_GPIO_Port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, AUX1_PU_Pin);
/* configure GPIO port - outputs */
gpio_mode_set(AUX2_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX2_Pin);
gpio_mode_set(AUX3_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX3_Pin);
gpio_output_options_set(AUX2_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX2_Pin);
gpio_output_options_set(AUX3_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX3_Pin);
/* reset GPIO pin */
gpio_bit_reset(AUX2_GPIO_Port, AUX2_Pin);
gpio_bit_reset(AUX3_GPIO_Port, AUX3_Pin);
#ifdef AUX45_USE_GPIO
/* configure GPIO port - outputs */
gpio_mode_set(AUX4_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX4_Pin);
gpio_mode_set(AUX5_GPIO_Port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AUX5_Pin);
gpio_output_options_set(AUX4_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX4_Pin);
gpio_output_options_set(AUX5_GPIO_Port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, AUX5_Pin);
/* reset GPIO pin */
gpio_bit_reset(AUX4_GPIO_Port, AUX4_Pin);
gpio_bit_reset(AUX5_GPIO_Port, AUX5_Pin);
#endif
}
void usart_config(uint32_t selUSART, uint32_t selBaudRate) {
/* enable GPIO clock */
uint32_t USART_ID = 0U;
if(selUSART == USART0){
USART_ID = 0U;
}
if(selUSART == USART1){
USART_ID = 1U;
}
rcu_periph_clock_enable(USART_GPIO_CLK);
void usart_config(uint32_t selUSART, uint32_t selBaudRate) {
/* enable USART clock */
rcu_periph_clock_enable(USART_CLK[USART_ID]);
uint8_t USART_ID = 0U;
if(selUSART == USART0){
USART_ID = 0U;
}
if(selUSART == USART1){
USART_ID = 1U;
}
/* connect port to USARTx_Tx */
gpio_af_set(USART_GPIO_PORT, USART_AF, USART_TX_PIN[USART_ID]);
/* enable GPIO clock */
rcu_periph_clock_enable(USART_GPIO_CLK);
/* connect port to USARTx_Rx */
gpio_af_set(USART_GPIO_PORT, USART_AF, USART_RX_PIN[USART_ID]);
/* enable USART clock */
rcu_periph_clock_enable(USART_CLK[USART_ID]);
/* configure USART Tx as alternate function push-pull */
gpio_mode_set(USART_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, USART_TX_PIN[USART_ID]);
gpio_output_options_set(USART_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, USART_TX_PIN[USART_ID]);
/* connect port to USARTx_Tx */
gpio_af_set(USART_GPIO_PORT, USART_AF, USART_TX_PIN[USART_ID]);
/* configure USART Rx as alternate function push-pull */
gpio_mode_set(USART_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, USART_RX_PIN[USART_ID]);
gpio_output_options_set(USART_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, USART_RX_PIN[USART_ID]);
/* connect port to USARTx_Rx */
gpio_af_set(USART_GPIO_PORT, USART_AF, USART_RX_PIN[USART_ID]);
/* USART configure */
usart_deinit(selUSART);
usart_baudrate_set(selUSART, selBaudRate);
usart_transmit_config(selUSART, USART_TRANSMIT_ENABLE);
usart_receive_config(selUSART, USART_RECEIVE_ENABLE);
usart_oversample_config(selUSART, USART_OVSMOD_16); // oversampling: {USART_OVSMOD_8, USART_OVSMOD_16}
usart_sample_bit_config(selUSART, USART_OSB_3BIT); // sample bit: {USART_OSB_1BIT, USART_OSB_3BIT }
usart_enable(selUSART);
/* configure USART Tx as alternate function push-pull */
gpio_mode_set(USART_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, USART_TX_PIN[USART_ID]);
gpio_output_options_set(USART_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, USART_TX_PIN[USART_ID]);
/* configure USART Rx as alternate function push-pull */
gpio_mode_set(USART_GPIO_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, USART_RX_PIN[USART_ID]);
gpio_output_options_set(USART_GPIO_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, USART_RX_PIN[USART_ID]);
/* USART configure */
usart_deinit(selUSART);
usart_baudrate_set(selUSART, selBaudRate);
usart_transmit_config(selUSART, USART_TRANSMIT_ENABLE);
usart_receive_config(selUSART, USART_RECEIVE_ENABLE);
usart_oversample_config(selUSART, USART_OVSMOD_16); // oversampling: {USART_OVSMOD_8, USART_OVSMOD_16}
usart_sample_bit_config(selUSART, USART_OSB_3BIT); // sample bit: {USART_OSB_1BIT, USART_OSB_3BIT }
usart_enable(selUSART);
/* enable the USART IDLE line detection interrupt */
usart_interrupt_enable(selUSART, USART_INT_IDLE);
/* enable the USART IDLE line detection interrupt */
usart_interrupt_enable(selUSART, USART_INT_IDLE);
}
// DMA_CH1 = USART0_TX
// DMA_CH2 = USART0_RX
// DMA_CH3 = USART1_TX
// DMA_CH4 = USART1_RX
void usart_Tx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t dSize) {
dma_parameter_struct dma_init_struct;
// --------------------------- TX Channel ---------------------------
void usart_Tx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t dSize) {
/* enable DMA clock */
rcu_periph_clock_enable(RCU_DMA);
/* deinitialize DMA channel2 */
dma_deinit(DMA_CH3);
dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
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 = dSize;
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(DMA_CH3, dma_init_struct);
/* configure DMA mode */
dma_circulation_disable(DMA_CH3);
dma_memory_to_memory_disable(DMA_CH3);
/* USART DMA enable for transmission */
usart_dma_transmit_config(selUSART, USART_DENT_ENABLE);
dma_parameter_struct dma_init_struct;
// --------------------------- TX Channel ---------------------------
uint8_t USART_ID = 0U;
if(selUSART == USART0){
USART_ID = 0U;
}
if(selUSART == USART1){
USART_ID = 1U;
}
/* enable DMA clock */
rcu_periph_clock_enable(RCU_DMA);
/* deinitialize DMA channel2 */
dma_deinit(USART_TX_DMA_CH[USART_ID]);
dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
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 = dSize;
dma_init_struct.periph_addr = USART_TDATA_ADDRESS[USART_ID];
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(USART_TX_DMA_CH[USART_ID], dma_init_struct);
/* configure DMA mode */
dma_circulation_disable(USART_TX_DMA_CH[USART_ID]);
dma_memory_to_memory_disable(USART_TX_DMA_CH[USART_ID]);
/* USART DMA enable for transmission */
usart_dma_transmit_config(selUSART, USART_DENT_ENABLE);
/* enable DMA channel1 */
dma_channel_enable(USART_TX_DMA_CH[USART_ID]);
/* wait DMA channel transfer complete */
// while (RESET == dma_flag_get(USART_TX_DMA[USART_ID], DMA_FLAG_FTF));
/* enable DMA channel1 */
dma_channel_enable(DMA_CH3);
/* wait DMA channel transfer complete */
// while (RESET == dma_flag_get(DMA_CH3, DMA_FLAG_FTF));
}
void usart_Rx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t dSize) {
dma_parameter_struct dma_init_struct;
// --------------------------- RX Channel ---------------------------
/* enable DMA clock */
rcu_periph_clock_enable(RCU_DMA);
void usart_Rx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t dSize) {
/* deinitialize DMA channel4 */
dma_deinit(DMA_CH4);
dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
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 = dSize;
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(DMA_CH4, dma_init_struct);
/* configure DMA mode */
dma_circulation_enable(DMA_CH4); // dma_circulation_disable(DMA_CH4);
dma_memory_to_memory_disable(DMA_CH4);
dma_parameter_struct dma_init_struct;
/* USART DMA enable for reception */
usart_dma_receive_config(selUSART, USART_DENR_ENABLE);
// --------------------------- RX Channel ---------------------------
/* enable DMA channel */
dma_channel_enable(DMA_CH4);
uint8_t USART_ID = 0U;
if(selUSART == USART0){
USART_ID = 0U;
}
if(selUSART == USART1){
USART_ID = 1U;
}
/* enable DMA clock */
rcu_periph_clock_enable(RCU_DMA);
/* deinitialize DMA channel4 */
dma_deinit(USART_RX_DMA_CH[USART_ID]);
dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
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 = dSize;
dma_init_struct.periph_addr = USART_RDATA_ADDRESS[USART_ID];
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(USART_RX_DMA_CH[USART_ID], dma_init_struct);
/* configure DMA mode */
dma_circulation_enable(USART_RX_DMA_CH[USART_ID]); // dma_circulation_disable(USART_RX_DMA[USART_ID]);
dma_memory_to_memory_disable(USART_RX_DMA_CH[USART_ID]);
/* USART DMA enable for reception */
usart_dma_receive_config(selUSART, USART_DENR_ENABLE);
/* enable DMA channel */
dma_channel_enable(USART_RX_DMA_CH[USART_ID]);
/* wait DMA channel transfer complete */
// while (RESET == dma_flag_get(USART_RX_DMA[USART_ID], DMA_FLAG_FTF));
/* wait DMA channel transfer complete */
// while (RESET == dma_flag_get(DMA_CH4, DMA_FLAG_FTF));
}
void i2c_config(void) {
/* I2C clock configure */
//i2c_clock_config(MPU_I2C, MPU_I2C_SPEED, I2C_DTCY_2); // I2C duty cycle in fast mode
i2c_clock_config(MPU_I2C, MPU_I2C_SPEED, I2C_DTCY_16_9); // I2C duty cycle in fast mode plus
/* I2C address configure */
i2c_mode_addr_config(MPU_I2C, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, I2C_OWN_ADDRESS7);
/* enable I2C */
i2c_enable(MPU_I2C);
/* enable acknowledge */
i2c_ack_config(MPU_I2C, I2C_ACK_ENABLE);
#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 address configure */
i2c_mode_addr_config(AUX_I2C, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, AUX_I2C_OWN_ADDRESS7);
/* enable I2C */
i2c_enable(AUX_I2C);
/* enable acknowledge */
i2c_ack_config(AUX_I2C, I2C_ACK_ENABLE);
#endif
/* I2C clock configure */
//i2c_clock_config(MPU_I2C, MPU_I2C_SPEED, I2C_DTCY_2); // I2C duty cycle in fast mode
i2c_clock_config(MPU_I2C, MPU_I2C_SPEED, I2C_DTCY_16_9); // I2C duty cycle in fast mode plus
/* I2C address configure */
i2c_mode_addr_config(MPU_I2C, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, I2C_OWN_ADDRESS7);
/* enable I2C */
i2c_enable(MPU_I2C);
/* enable acknowledge */
i2c_ack_config(MPU_I2C, I2C_ACK_ENABLE);
#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 address configure */
i2c_mode_addr_config(AUX_I2C, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, AUX_I2C_OWN_ADDRESS7);
/* enable I2C */
i2c_enable(AUX_I2C);
/* enable acknowledge */
i2c_ack_config(AUX_I2C, I2C_ACK_ENABLE);
#endif
}
void usart_nvic_config(void)
{
nvic_irq_enable(USART1_IRQn, 0, 1);
nvic_irq_enable(USART0_IRQn, 0, 1); // Enable USART0 interrupt
nvic_irq_enable(USART1_IRQn, 0, 1); // Enable USART1 interrupt
}
void i2c_nvic_config(void)
{
/* configure the NVIC peripheral */
nvic_priority_group_set(NVIC_PRIGROUP_PRE1_SUB3);
nvic_irq_enable(I2C0_EV_IRQn, 0, 3);
nvic_irq_enable(I2C0_ER_IRQn, 0, 2);
#ifdef AUX45_USE_I2C
nvic_irq_enable(I2C1_EV_IRQn, 0, 4);
nvic_irq_enable(I2C1_ER_IRQn, 0, 1);
#endif
/* configure the NVIC peripheral */
nvic_priority_group_set(NVIC_PRIGROUP_PRE1_SUB3);
nvic_irq_enable(I2C0_EV_IRQn, 0, 0);
nvic_irq_enable(I2C0_ER_IRQn, 0, 0);
#ifdef AUX45_USE_I2C
nvic_irq_enable(I2C1_EV_IRQn, 0, 2);
nvic_irq_enable(I2C1_ER_IRQn, 0, 2);
#endif
}

View File

@ -88,7 +88,7 @@ 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()
@ -99,7 +99,7 @@ void tick_count_increment()
/*!
\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)

View File

@ -28,44 +28,81 @@
#include "util.h"
#include "mpu6050.h"
// USART variables
// USART1 variables
#ifdef SERIAL_CONTROL
SerialSideboard Sideboard;
static SerialSideboard Sideboard;
#endif
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
static uint8_t rx_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx_buffer_len = ARRAY_LEN(rx_buffer);
static uint8_t rx1_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx1_buffer_len = ARRAY_LEN(rx1_buffer);
#endif
#ifdef SERIAL_FEEDBACK
SerialFeedback Feedback;
SerialFeedback FeedbackRaw;
uint16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command
uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static SerialFeedback Feedback;
static SerialFeedback FeedbackRaw;
static uint16_t timeoutCntSerial1 = 0; // Timeout counter for UART1 Rx Serial
static uint8_t timeoutFlagSerial1 = 0; // Timeout Flag for UART1 Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static uint32_t Feedback_len = sizeof(Feedback);
#endif
// USART0 variables
#ifdef SERIAL_AUX_TX
static SerialAuxTx AuxTx;
#endif
#ifdef SERIAL_AUX_RX
static uint8_t rx0_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx0_buffer_len = ARRAY_LEN(rx0_buffer);
#endif
#ifdef SERIAL_AUX_RX
static SerialCommand command;
static SerialCommand command_raw;
static uint16_t timeoutCntSerial0 = 0; // Timeout counter for UART0 Rx Serial
static uint8_t timeoutFlagSerial0 = 0; // Timeout Flag for UART0 Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static uint32_t command_len = sizeof(command);
extern uint8_t print_aux;
#ifdef CONTROL_IBUS
static uint16_t ibus_chksum;
static uint16_t ibus_captured_value[IBUS_NUM_CHANNELS];
#endif
#endif
#if (defined(SERIAL_AUX_RX) && defined(CONTROL_IBUS)) || defined(SERIAL_CONTROL)
static int16_t cmd1, cmd2;
static uint16_t cmdSwitch;
#endif
// Optical sensors variables
static FlagStatus sensor1, sensor2; // holds the sensor1 and sensor 2 values
static FlagStatus sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2
// MPU variables
ErrStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
extern MPU_Data mpu; // holds the MPU-6050 data
#if defined(MPU_SENSOR_ENABLE) || defined(SERIAL_CONTROL)
static ErrStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
#endif
extern uint32_t main_loop_counter; // main loop counter to perform task scheduling inside main()
// MAIN I2C variables
volatile int8_t i2c_status;
volatile i2c_cmd i2c_ReadWriteCmd;
volatile uint8_t i2c_regAddress;
volatile uint8_t i2c_slaveAddress;
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 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 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;
@ -73,156 +110,337 @@ volatile int8_t i2c_aux_nRABytes;
#endif
/* =========================== General Functions =========================== */
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 */
#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
#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)
{
GPIO_OCTL(gpio_periph) ^= pin;
GPIO_OCTL(gpio_periph) ^= pin;
}
void intro_demo_led(uint32_t tDelay)
{
int i;
int i;
for (i = 0; i < 3; 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 < 3; 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);
}
}
uint8_t switch_check(uint16_t ch, uint8_t type) {
if (type) { // 3 positions switch
if (ch < 250) return 0; // switch in position 0
else if (ch < 850) return 1; // switch in position 1
else return 2; // switch in position 2
} else { // 2 positions switch
return (ch > 850);
}
}
/* =========================== Input Initialization Function =========================== */
void input_init(void) {
#ifdef SERIAL_CONTROL
usart_Tx_DMA_config(USART_MAIN, (uint8_t *)&Sideboard, sizeof(Sideboard));
#endif
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)rx_buffer, sizeof(rx_buffer));
#endif
#ifdef SERIAL_CONTROL
usart_Tx_DMA_config(USART_MAIN, (uint8_t *)&Sideboard, sizeof(Sideboard));
#endif
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)rx1_buffer, sizeof(rx1_buffer));
#endif
#ifdef SERIAL_AUX_TX
usart_Tx_DMA_config(USART_AUX, (uint8_t *)&AuxTx, sizeof(AuxTx));
#endif
#ifdef SERIAL_AUX_RX
usart_Rx_DMA_config(USART_AUX, (uint8_t *)rx0_buffer, sizeof(rx0_buffer));
#endif
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
#ifdef MPU_SENSOR_ENABLE
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED
}
else {
mpuStatus = SUCCESS;
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN 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
#endif
#ifdef MPU_SENSOR_ENABLE
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED - sensor enabled and NOT ok
}
else {
mpuStatus = SUCCESS;
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED - sensor enabled and ok
}
#else
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED - sensor disabled
#endif
#ifdef SERIAL_DEBUG
mpu_handle_input('h'); // Print the User Help commands to serial
#endif
}
/* =========================== USART READ Functions =========================== */
/* =========================== Handle Functions =========================== */
/*
* Check for new data received on USART with DMA: refactored function from https://github.com/MaJerle/stm32-usart-uart-dma-rx-tx
* - this function is called for every USART IDLE line detection, in the USART interrupt handler
* Handle of the MPU-6050 IMU sensor
*/
void usart_rx_check(void)
{
#ifdef SERIAL_DEBUG
static uint32_t old_pos;
uint32_t pos;
void handle_mpu6050(void) {
#ifdef MPU_SENSOR_ENABLE
// Get MPU data. Because the MPU-6050 interrupt pin is not wired we have to check DMP data by pooling periodically
if (SUCCESS == mpuStatus) {
mpu_get_data();
} else if (ERROR == mpuStatus && main_loop_counter % 100 == 0) {
toggle_led(LED1_GPIO_Port, LED1_Pin); // Toggle the Red LED every 100 ms
}
// Print MPU data to Console
#ifdef SERIAL_DEBUG
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
#endif
#endif
}
pos = rx_buffer_len - dma_transfer_number_get(DMA_CH4); // Calculate current position in buffer
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx_buffer[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
usart_process_debug(&rx_buffer[old_pos], rx_buffer_len - old_pos); // First Process data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
usart_process_debug(&rx_buffer[0], pos); // Process remaining data
}
/*
* Handle of the optical sensors
*/
void handle_sensors(void) {
sensor1_read = gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin);
// SENSOR1
if (sensor1 == RESET && sensor1_read == SET) {
// Sensor ACTIVE: Do something here (one time task on activation)
sensor1 = SET;
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
consoleLog("SENSOR 1 ON\r\n");
} else if(sensor1 == SET && sensor1_read == RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor1 = RESET;
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
consoleLog("SENSOR 1 OFF\r\n");
}
// SENSOR2
if (sensor2 == RESET && sensor2_read == SET) {
// Sensor ACTIVE: Do something here (one time task on activation)
sensor2 = SET;
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
consoleLog("SENSOR 2 ON\r\n");
} else if (sensor2 == SET && sensor2_read == RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor2 = RESET;
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
consoleLog("SENSOR 2 OFF\r\n");
}
if (sensor1 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
}
/*
* Handle of the USART data
*/
void handle_usart(void) {
// Tx USART MAIN
#ifdef SERIAL_CONTROL
if (main_loop_counter % 5 == 0 && dma_transfer_number_get(USART1_TX_DMA_CH) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
Sideboard.start = (uint16_t)SERIAL_START_FRAME;
Sideboard.pitch = (int16_t)mpu.euler.pitch;
Sideboard.dPitch = (int16_t)mpu.gyro.y;
Sideboard.cmd1 = (int16_t)cmd1;
Sideboard.cmd2 = (int16_t)cmd2;
Sideboard.sensors = (uint16_t)( (cmdSwitch << 8) | (sensor1 | (sensor2 << 1) | (mpuStatus << 2)) );
Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.pitch ^ Sideboard.dPitch ^ Sideboard.cmd1 ^ Sideboard.cmd2 ^ Sideboard.sensors);
dma_channel_disable(USART1_TX_DMA_CH);
DMA_CHCNT(USART1_TX_DMA_CH) = sizeof(Sideboard);
DMA_CHMADDR(USART1_TX_DMA_CH) = (uint32_t)&Sideboard;
dma_channel_enable(USART1_TX_DMA_CH);
}
}
old_pos = pos; // Update old position
if (old_pos == rx_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_DEBUG
#endif
// Rx USART MAIN
#ifdef SERIAL_FEEDBACK
if (timeoutCntSerial1++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial1 = 1; // Timeout detected
timeoutCntSerial1 = SERIAL_TIMEOUT; // Limit timout counter value
}
if (timeoutFlagSerial1 && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired
toggle_led(LED3_GPIO_Port, LED3_Pin); // Toggle the Yellow LED every 100 ms
}
#endif
#ifdef SERIAL_FEEDBACK
static uint32_t old_pos;
uint32_t pos;
uint8_t *ptr;
pos = rx_buffer_len - dma_transfer_number_get(DMA_CH4); // Calculate current position in buffer
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&FeedbackRaw; // Initialize the pointer with FeedbackRaw address
if (pos > old_pos && (pos - old_pos) == Feedback_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx_buffer[old_pos], Feedback_len); // Copy data. This is possible only if FeedbackRaw is contiguous! (meaning all the structure members have the same size)
usart_process_data(&FeedbackRaw, &Feedback); // Process data
} else if ((rx_buffer_len - old_pos + pos) == Feedback_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx_buffer[old_pos], rx_buffer_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_len - old_pos; // Move to correct position in FeedbackRaw
memcpy(ptr, &rx_buffer[0], pos); // Copy remaining data
// Tx USART AUX
#ifdef SERIAL_AUX_TX
if (main_loop_counter % 5 == 0 && dma_transfer_number_get(USART0_TX_DMA_CH) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
AuxTx.start = (uint16_t)SERIAL_START_FRAME;
AuxTx.signal1 = (int16_t)sensor1;
AuxTx.signal2 = (int16_t)sensor2;
AuxTx.checksum = (uint16_t)(AuxTx.start ^ AuxTx.signal1 ^ AuxTx.signal2);
dma_channel_disable(USART0_TX_DMA_CH);
DMA_CHCNT(USART0_TX_DMA_CH) = sizeof(AuxTx);
DMA_CHMADDR(USART0_TX_DMA_CH) = (uint32_t)&AuxTx;
dma_channel_enable(USART0_TX_DMA_CH);
}
#endif
// Rx USART AUX
#ifdef SERIAL_AUX_RX
#ifdef CONTROL_IBUS
if (!timeoutFlagSerial0) {
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, 1000); // 1000-2000 -> 0-1000
}
cmd1 = (ibus_captured_value[0] - 500) * 2; // Channel 1
cmd2 = (ibus_captured_value[1] - 500) * 2; // Channel 2
cmdSwitch = (uint16_t)(switch_check(ibus_captured_value[6],0) | // Channel 7
switch_check(ibus_captured_value[7],1) << 1 | // Channel 8
switch_check(ibus_captured_value[8],1) << 3 | // Channel 9
switch_check(ibus_captured_value[9],0) << 5); // Channel 10
}
#endif
if (timeoutCntSerial0++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial0 = 1; // Timeout detected
timeoutCntSerial0 = SERIAL_TIMEOUT; // Limit timout counter value
cmd1 = cmd2 = 0; // Set commands to 0
cmdSwitch &= ~(1U << 0); // Clear Bit 0, to switch to default control input
}
// if (timeoutFlagSerial0 && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired
// toggle_led(LED2_GPIO_Port, LED2_Pin); // Toggle the Green LED every 100 ms
// }
#ifdef SERIAL_DEBUG
// Print MPU data to Console
if (main_loop_counter % 50 == 0) {
aux_print_to_console();
}
#endif
#endif
}
/*
* Handle of the sideboard LEDs
*/
void handle_leds(void) {
#ifdef SERIAL_FEEDBACK
if (!timeoutFlagSerial1) {
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); }
if (Feedback.cmdLed & LED4_SET) { gpio_bit_set(AUX3_GPIO_Port, AUX3_Pin); } else { gpio_bit_reset(AUX3_GPIO_Port, AUX3_Pin); }
}
#endif
}
/* =========================== USART1 READ Functions =========================== */
void usart1_rx_check(void)
{
#ifdef SERIAL_DEBUG
static uint32_t old_pos;
uint32_t pos;
pos = rx1_buffer_len - dma_transfer_number_get(USART1_RX_DMA_CH); // Calculate current position in buffer
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx1_buffer[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
usart_process_debug(&rx1_buffer[old_pos], rx1_buffer_len - old_pos);// First Process data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
usart_process_debug(&rx1_buffer[0], pos); // Process remaining data
}
usart_process_data(&FeedbackRaw, &Feedback); // Process data
}
}
old_pos = pos; // Updated old position
if (old_pos == rx_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = pos; // Update old position
if (old_pos == rx1_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_FEEDBACK
#endif // SERIAL_DEBUG
#ifdef SERIAL_FEEDBACK
static uint32_t old_pos;
uint32_t pos;
uint8_t *ptr;
pos = rx1_buffer_len - dma_transfer_number_get(USART1_RX_DMA_CH); // Calculate current position in buffer
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&FeedbackRaw; // Initialize the pointer with FeedbackRaw address
if (pos > old_pos && (pos - old_pos) == Feedback_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx1_buffer[old_pos], Feedback_len); // Copy data. This is possible only if FeedbackRaw is contiguous! (meaning all the structure members have the same size)
usart_process_data(&FeedbackRaw, &Feedback); // Process data
} else if ((rx1_buffer_len - old_pos + pos) == Feedback_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx1_buffer[old_pos], rx1_buffer_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx1_buffer_len - old_pos; // Move to correct position in FeedbackRaw
memcpy(ptr, &rx1_buffer[0], pos); // Copy remaining data
}
usart_process_data(&FeedbackRaw, &Feedback); // Process data
}
}
old_pos = pos; // Updated old position
if (old_pos == rx1_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_FEEDBACK
}
/*
@ -231,13 +449,11 @@ void usart_rx_check(void)
#ifdef SERIAL_DEBUG
void usart_process_debug(uint8_t *userCommand, uint32_t len)
{
for (; len > 0; len--, userCommand++) {
if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands
log_i("Command = %c\n", *userCommand);
#ifdef MPU_SENSOR_ENABLE
mpu_handle_input(*userCommand);
#endif
}
for (; len > 0; len--, userCommand++) {
if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands
log_i("Command = %c\r\n", *userCommand);
mpu_handle_input(*userCommand);
}
}
}
#endif // SERIAL_DEBUG
@ -248,53 +464,130 @@ void usart_process_debug(uint8_t *userCommand, uint32_t len)
*/
#ifdef SERIAL_FEEDBACK
void usart_process_data(SerialFeedback *Feedback_in, SerialFeedback *Feedback_out)
{
uint16_t checksum;
if (Feedback_in->start == SERIAL_START_FRAME) {
checksum = (uint16_t)(Feedback_in->start ^ Feedback_in->cmd1 ^ Feedback_in->cmd2 ^ Feedback_in->speedR_meas ^ Feedback_in->speedL_meas
^ Feedback_in->batVoltage ^ Feedback_in->boardTemp ^ Feedback_in->cmdLed);
if (Feedback_in->checksum == checksum) {
*Feedback_out = *Feedback_in;
timeoutCntSerial = 0; // Reset timeout counter
timeoutFlagSerial = 0; // Clear timeout flag
}
}
{
uint16_t checksum;
if (Feedback_in->start == SERIAL_START_FRAME) {
checksum = (uint16_t)(Feedback_in->start ^ Feedback_in->cmd1 ^ Feedback_in->cmd2 ^ Feedback_in->speedR_meas ^ Feedback_in->speedL_meas
^ Feedback_in->batVoltage ^ Feedback_in->boardTemp ^ Feedback_in->cmdLed);
if (Feedback_in->checksum == checksum) {
*Feedback_out = *Feedback_in;
timeoutCntSerial1 = 0; // Reset timeout counter
timeoutFlagSerial1 = 0; // Clear timeout flag
}
}
}
#endif // SERIAL_FEEDBACK
/* =========================== USART0 READ Functions =========================== */
/*
* Check for new data received on USART with DMA: refactored function from https://github.com/MaJerle/stm32-usart-uart-dma-rx-tx
* - this function is called for every USART IDLE line detection, in the USART interrupt handler
*/
void usart0_rx_check(void)
{
#ifdef SERIAL_AUX_RX
static uint32_t old_pos;
uint32_t pos;
uint8_t *ptr;
pos = rx0_buffer_len - dma_transfer_number_get(USART0_RX_DMA_CH); // Calculate current position in buffer
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&command_raw; // Initialize the pointer with structure address
if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx0_buffer[old_pos], command_len); // Copy data. This is possible only if structure is contiguous! (meaning all the structure members have the same size)
usart_process_command(&command_raw, &command); // Process data
} else if ((rx0_buffer_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx0_buffer[old_pos], rx0_buffer_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx0_buffer_len - old_pos; // Update position
memcpy(ptr, &rx0_buffer[0], pos); // Copy remaining data
}
usart_process_command(&command_raw, &command); // Process data
}
}
old_pos = pos; // Updated old position
if (old_pos == rx0_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_AUX_RX
}
/*
* Process command UART0 Rx data
* - if the command_in data is valid (correct START_FRAME and checksum) copy the command_in to command_out
*/
#ifdef SERIAL_AUX_RX
void usart_process_command(SerialCommand *command_in, SerialCommand *command_out)
{
#ifdef CONTROL_IBUS
if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) {
ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND;
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) {
ibus_chksum -= command_in->channels[i];
}
if (ibus_chksum == (uint16_t)((command_in->checksumh << 8) + command_in->checksuml)) {
*command_out = *command_in;
timeoutCntSerial0 = 0; // Reset timeout counter
timeoutFlagSerial0 = 0; // Clear timeout flag
}
}
#endif
}
#endif
/* =========================== AUX Serial Print data =========================== */
void aux_print_to_console(void)
{
#if defined(SERIAL_DEBUG) && defined(SERIAL_AUX_RX)
#ifdef CONTROL_IBUS
if (print_aux & PRINT_AUX) {
log_i( "Ch1: %d Ch2: %d Sw: %u\r\n", cmd1, cmd2, cmdSwitch);
}
#endif
#endif
}
/* =========================== I2C WRITE Functions =========================== */
/*
* write bytes to chip register
*/
int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// assign WRITE command
i2c_ReadWriteCmd = WRITE;
// assign inputs
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;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
{
// the master waits until the I2C bus is idle
while(i2c_flag_get(MPU_I2C, I2C_FLAG_I2CBSY));
// assign WRITE command
i2c_ReadWriteCmd = WRITE;
// the master sends a start condition to I2C bus
i2c_start_on_bus(MPU_I2C);
// assign inputs
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;
uint16_t i2c_timeout = 0;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
// the master waits until the I2C bus is idle
while(i2c_flag_get(MPU_I2C, I2C_FLAG_I2CBSY) && i2c_timeout++ < 20000);
// the master sends a start condition to I2C bus
i2c_start_on_bus(MPU_I2C);
// Wait until all data bytes are sent/received
i2c_timeout = 0;
while(i2c_nDABytes > 0 && i2c_timeout++ < 20000);
return i2c_status;
// Wait until all data bytes are sent/received
while(i2c_nDABytes > 0);
return i2c_status;
}
@ -303,7 +596,7 @@ int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_
*/
int8_t i2c_writeByte(uint8_t slaveAddr, uint8_t regAddr, uint8_t data)
{
return i2c_writeBytes(slaveAddr, regAddr, 1, &data);
return i2c_writeBytes(slaveAddr, regAddr, 1, &data);
}
@ -326,37 +619,40 @@ int8_t i2c_writeBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t
*/
int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// assign READ command
i2c_ReadWriteCmd = READ;
// assign inputs
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;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
if(2 == i2c_nDABytes){
i2c_ackpos_config(MPU_I2C, I2C_ACKPOS_NEXT); // send ACK for the next byte
}
// the master waits until the I2C bus is idle
while(i2c_flag_get(MPU_I2C, I2C_FLAG_I2CBSY));
// the master sends a start condition to I2C bus
i2c_start_on_bus(MPU_I2C);
// Wait until all data bytes are sent/received
while(i2c_nDABytes > 0);
// Return status
return i2c_status;
// assign READ command
i2c_ReadWriteCmd = READ;
// assign inputs
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;
uint16_t i2c_timeout = 0;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
if(2 == i2c_nDABytes){
i2c_ackpos_config(MPU_I2C, I2C_ACKPOS_NEXT); // send ACK for the next byte
}
// the master waits until the I2C bus is idle
while(i2c_flag_get(MPU_I2C, I2C_FLAG_I2CBSY) && i2c_timeout++ < 20000);
// the master sends a start condition to I2C bus
i2c_start_on_bus(MPU_I2C);
// Wait until all data bytes are sent/received
i2c_timeout = 0;
while(i2c_nDABytes > 0 && i2c_timeout++ < 20000);
// Return status
return i2c_status;
}
@ -365,7 +661,7 @@ int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t
*/
int8_t i2c_readByte(uint8_t slaveAddr, uint8_t regAddr, uint8_t *data)
{
return i2c_readBytes(slaveAddr, regAddr, 1, data);
return i2c_readBytes(slaveAddr, regAddr, 1, data);
}
@ -386,33 +682,33 @@ int8_t i2c_readBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *
* write bytes to chip register
*/
int8_t i2c_aux_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// assign WRITE command
i2c_aux_ReadWriteCmd = WRITE;
// assign inputs
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;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
{
// the master waits until the I2C bus is idle
while(i2c_flag_get(AUX_I2C, I2C_FLAG_I2CBSY));
// assign WRITE command
i2c_aux_ReadWriteCmd = WRITE;
// the master sends a start condition to I2C bus
i2c_start_on_bus(AUX_I2C);
// assign inputs
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;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
// the master waits until the I2C bus is idle
while(i2c_flag_get(AUX_I2C, I2C_FLAG_I2CBSY));
// the master sends a start condition to I2C bus
i2c_start_on_bus(AUX_I2C);
// Wait until all data bytes are sent/received
while(i2c_aux_nDABytes > 0);
return i2c_aux_status;
// Wait until all data bytes are sent/received
while(i2c_aux_nDABytes > 0);
return i2c_aux_status;
}
/*
@ -420,37 +716,37 @@ int8_t i2c_aux_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, ui
*/
int8_t i2c_aux_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// assign READ command
i2c_aux_ReadWriteCmd = READ;
// assign inputs
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;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
if(2 == i2c_aux_nDABytes){
i2c_ackpos_config(AUX_I2C, I2C_ACKPOS_NEXT); // send ACK for the next byte
}
// the master waits until the I2C bus is idle
while(i2c_flag_get(AUX_I2C, I2C_FLAG_I2CBSY));
// the master sends a start condition to I2C bus
i2c_start_on_bus(AUX_I2C);
// Wait until all data bytes are sent/received
while(i2c_aux_nDABytes > 0);
// Return status
return i2c_aux_status;
// assign READ command
i2c_aux_ReadWriteCmd = READ;
// assign inputs
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;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
if(2 == i2c_aux_nDABytes){
i2c_ackpos_config(AUX_I2C, I2C_ACKPOS_NEXT); // send ACK for the next byte
}
// the master waits until the I2C bus is idle
while(i2c_flag_get(AUX_I2C, I2C_FLAG_I2CBSY));
// the master sends a start condition to I2C bus
i2c_start_on_bus(AUX_I2C);
// Wait until all data bytes are sent/received
while(i2c_aux_nDABytes > 0);
// Return status
return i2c_aux_status;
}
#endif

View File

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

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.0 MiB

Binary file not shown.

View File

@ -14,10 +14,14 @@ include_dir = Inc
src_dir = Src
;=================== VARIANT SELECTION ==========================
;default_envs = VARIANT_DEBUG ; DEFAULT Variant
;default_envs = VARIANT_DEBUG ; DEBUG Variant
;default_envs = VARIANT_HOVERCAR ; HOVERCAR Variant
;default_envs = VARIANT_HOVERBOARD ; HOVERBOARD Variant
;================================================================
[env]
platform_packages = maxgerhardt/framework-spl@2.10300.0 ; Add GD32 support package: globally override framework-spl for all environments
;================================================================
[env:VARIANT_DEBUG]
@ -31,19 +35,41 @@ extra_scripts = add_nanolib.py ; adds nanolib to reduce printf memory f
; Serial Port settings (make sure the COM port is correct)
monitor_port = COM5
monitor_speed = 38400
monitor_speed = 115200
build_flags =
-IInc
-ISrc
-DUSE_STDPERIPH_DRIVER
-DGD32F130_150
-Wl,-T./GD32F130C6T_FLASH.ld
-Wl,-lc
-Wl,-lm
-T./GD32F130C6T_FLASH.ld
-lc
-lm
-g -ggdb
-D VARIANT_DEBUG
;================================================================
[env:VARIANT_HOVERCAR]
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
-T./GD32F130C6T_FLASH.ld
-lc
-lm
-g -ggdb
-D VARIANT_HOVERCAR
;================================================================
[env:VARIANT_HOVERBOARD]
@ -59,9 +85,9 @@ build_flags =
-ISrc
-DUSE_STDPERIPH_DRIVER
-DGD32F130_150
-Wl,-T./GD32F130C6T_FLASH.ld
-Wl,-lc
-Wl,-lm
-T./GD32F130C6T_FLASH.ld
-lc
-lm
-g -ggdb
-D VARIANT_HOVERBOARD