diff --git a/.travis.yml b/.travis.yml index 790f441..815c5bc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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" \ No newline at end of file + - name: platformio + script: platformio run + language: python + python: + - "2.7" + install: + - pip install -U platformio + - platformio update + cache: + - directories: "~/.platformio" \ No newline at end of file diff --git a/Inc/config.h b/Inc/config.h index 93033c9..c93b9fc 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -78,6 +78,13 @@ /* ==================================== 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 @@ -85,11 +92,11 @@ #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_TX // [-] Use AUX4, AUX5 as USART 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_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 @@ -112,7 +119,8 @@ #error SERIAL_DEBUG and SERIAL_FEEDBACK not allowed. It is on the same cable. #endif -#if defined(AUX45_USE_GPIO) && (defined(SERIAL_AUX_RX) || defined(AUX45_USE_I2C)) || (defined(SERIAL_AUX_RX) && 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 diff --git a/Inc/defines.h b/Inc/defines.h index 0914584..096e851 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -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) +#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 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 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 diff --git a/Inc/i2c_it.h b/Inc/i2c_it.h index d484752..19d06b7 100644 --- a/Inc/i2c_it.h +++ b/Inc/i2c_it.h @@ -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 diff --git a/Inc/mpu6050.h b/Inc/mpu6050.h index 12c63ee..2c8c53c 100644 --- a/Inc/mpu6050.h +++ b/Inc/mpu6050.h @@ -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 */ @@ -134,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 + diff --git a/Inc/util.h b/Inc/util.h index 3b5ea61..e23c753 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -27,10 +27,11 @@ #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 @@ -75,12 +76,18 @@ typedef struct{ 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 usart0_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); @@ -88,10 +95,16 @@ void usart_process_debug(uint8_t *userCommand, uint32_t len); #ifdef SERIAL_FEEDBACK 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); diff --git a/MDK-ARM/sideboard-hack.uvopt b/MDK-ARM/sideboard-hack.uvopt index e233441..a9e3334 100644 --- a/MDK-ARM/sideboard-hack.uvopt +++ b/MDK-ARM/sideboard-hack.uvopt @@ -168,6 +168,153 @@ + + VARIANT_HOVERCAR + 0x4 + ARM-ADS + + 8000000 + + 1 + 0 + 1 + 0 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + .\Listings\ + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 1 + 0 + 0 + + 0 + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 5 + + + + + + + + + + + STLink\ST-LINKIII-KEIL_SWO.dll + + + + 0 + ST-LINKIII-KEIL_SWO + -U -O207 -S0 -C0 -A0 -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0GD32F1x0_32 -FS08000000 -FL08000 + + + 0 + UL2CM3 + UL2CM3(-O207 -S0 -C0 -FO7 -FD20000000 -FC800 -FN1 -FF0GD32F1x0_32 -FS08000000 -FL08000) + + + + + 0 + + + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + 0 + 0 + 0 + + + + + + + VARIANT_HOVERBOARD 0x4 diff --git a/MDK-ARM/sideboard-hack.uvproj b/MDK-ARM/sideboard-hack.uvproj index 6556711..c34f7b6 100644 --- a/MDK-ARM/sideboard-hack.uvproj +++ b/MDK-ARM/sideboard-hack.uvproj @@ -622,6 +622,622 @@ + + VARIANT_HOVERCAR + 0x4 + ARM-ADS + 5060422::V5.06 update 4 (build 422)::ARMCC + + + GD32F130C6 + GigaDevice + IRAM(0x20000000-0x20000FFF) IROM(0x08000000-0x08007FFF) CLOCK(8000000) CPUTYPE("Cortex-M3") + + "Startup\GD\GD32F1x0\startup_gd32f1x0.s" ("GD32F1x0 Startup Code") + UL2CM3(-O207 -S0 -C0 -FO7 -FD20000000 -FC800 -FN1 -FF0GD32F1x0_32 -FS08000000 -FL08000) + 7715 + gd32f1x0.h + + + + + + + + + + SFD\GD\GD32F1x0\GD32F1x0.SFR + 0 + 0 + + + + GD\GD32F1x0\ + GD\GD32F1x0\ + + 0 + 0 + 0 + 0 + 1 + + .\Objects\ + firmware + 1 + 0 + 1 + 1 + 1 + .\Listings\ + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 1 + 0 + $K\ARM\ARMCC\bin\fromelf.exe --bin --output=.\Objects\@L.bin !L + + 0 + 0 + 0 + 0 + + 0 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 1 + + + SARMCM3.DLL + -REMAP + DCM.DLL + -pCM3 + SARMCM3.DLL + + TCM.DLL + -pCM3 + + + + 1 + 0 + 0 + 0 + 16 + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + + + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 1 + + 0 + 5 + + + + + + + + + + + + + + STLink\ST-LINKIII-KEIL_SWO.dll + + + + + 1 + 0 + 0 + 1 + 1 + 4096 + + 1 + BIN\UL2CM3.DLL + "" () + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M3" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 0 + 0 + 0 + 8 + 1 + 0 + 0 + 0 + 3 + 3 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x1000 + + + 1 + 0x8000000 + 0x8000 + + + 0 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8000000 + 0x8000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x1000 + + + 0 + 0x0 + 0x0 + + + + + + 1 + 4 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 2 + 0 + 0 + 1 + 0 + 1 + 1 + 1 + 1 + 0 + 0 + 0 + + + USE_STDPERIPH_DRIVER,GD32F130_150,VARIANT_HOVERCAR + + ..\Inc;..\Drivers\CMSIS\Include;..\Drivers\CMSIS;..\Drivers\GD32F1x0_standard_peripheral\Include + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + 1 + 0 + 0 + 0 + 1 + 0 + 0x08000000 + 0x20000000 + + .\Objects\sideboard-hack.sct + + + + + + + + + + + Startup + + + startup_gd32f1x0.s + 2 + .\startup_gd32f1x0.s + + + + + CMSIS + + + system_gd32f1x0.c + 1 + ..\Drivers\CMSIS\Source\system_gd32f1x0.c + + + + + Peripheral + + + gd32f1x0_adc.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_adc.c + + + gd32f1x0_can.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_can.c + + + gd32f1x0_cec.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_cec.c + + + gd32f1x0_cmp.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_cmp.c + + + gd32f1x0_crc.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_crc.c + + + gd32f1x0_dac.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_dac.c + + + gd32f1x0_dbg.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_dbg.c + + + gd32f1x0_dma.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_dma.c + + + gd32f1x0_exti.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_exti.c + + + gd32f1x0_fmc.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_fmc.c + + + gd32f1x0_fwdgt.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_fwdgt.c + + + gd32f1x0_gpio.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_gpio.c + + + gd32f1x0_i2c.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_i2c.c + + + gd32f1x0_ivref.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_ivref.c + + + gd32f1x0_misc.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_misc.c + + + gd32f1x0_opa.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_opa.c + + + gd32f1x0_pmu.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_pmu.c + + + gd32f1x0_rcu.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_rcu.c + + + gd32f1x0_rtc.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_rtc.c + + + gd32f1x0_slcd.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_slcd.c + + + gd32f1x0_spi.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_spi.c + + + gd32f1x0_syscfg.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_syscfg.c + + + gd32f1x0_timer.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_timer.c + + + gd32f1x0_tsi.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_tsi.c + + + gd32f1x0_usart.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_usart.c + + + gd32f1x0_wwdgt.c + 1 + ..\Drivers\GD32F1x0_standard_peripheral\Source\gd32f1x0_wwdgt.c + + + + + Src + + + gd32f1x0_it.c + 1 + ..\Src\gd32f1x0_it.c + + + main.c + 1 + ..\Src\main.c + + + setup.c + 1 + ..\Src\setup.c + + + systick.c + 1 + ..\Src\systick.c + + + util.c + 1 + ..\Src\util.c + + + i2c_it.c + 1 + ..\Src\i2c_it.c + + + mpu6050.c + 1 + ..\Src\mpu6050.c + + + mpu6050_dmp.c + 1 + ..\Src\mpu6050_dmp.c + + + config.h + 5 + ..\Inc\config.h + + + + + VARIANT_HOVERBOARD 0x4 diff --git a/Src/gd32f1x0_it.c b/Src/gd32f1x0_it.c index ea756ab..4ce60ce 100644 --- a/Src/gd32f1x0_it.c +++ b/Src/gd32f1x0_it.c @@ -118,8 +118,8 @@ void PendSV_Handler(void) */ void SysTick_Handler(void) { - tick_count_increment(); - delay_decrement(); + tick_count_increment(); + delay_decrement(); } /*! @@ -129,7 +129,7 @@ void SysTick_Handler(void) \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 @@ -143,7 +143,7 @@ void USART0_IRQHandler(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) usart1_rx_check(); // Check for data to process diff --git a/Src/i2c_it.c b/Src/i2c_it.c index 7b474a9..dbfa270 100644 --- a/Src/i2c_it.c +++ b/Src/i2c_it.c @@ -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); + } + } + + } + + } + } - } - } - } @@ -212,115 +212,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); + } + } + + } + + } + } } /*! diff --git a/Src/main.c b/Src/main.c index 6be65a1..ba14616 100644 --- a/Src/main.c +++ b/Src/main.c @@ -29,47 +29,19 @@ #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 UART1 Rx Serial -extern uint8_t timeoutFlagSerial; // Timeout Flag for UART1 Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) -#endif - -#ifdef SERIAL_AUX_TX -extern SerialAuxTx AuxTx; -#endif - -#ifdef SERIAL_AUX_RX -extern SerialCommand command; -extern uint16_t timeoutCntSerial0; // Timeout counter for UART0 Rx Serial -extern uint8_t timeoutFlagSerial0; // Timeout Flag for UART0Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) - #ifdef CONTROL_IBUS - uint16_t ibus_captured_value[IBUS_NUM_CHANNELS]; - #endif -#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 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 + i2c_config(); // I2C config i2c_nvic_config(); // I2C interrupt configuration input_init(); // Input initialization @@ -77,133 +49,11 @@ int main(void) 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 - - // ==================================== 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 - - // ==================================== SENSORS Handling ==================================== - sensor1_read = gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin); - sensor2_read = gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin); - - // SENSOR1 - if (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"); - } - - if (sensor1 == SET) { - // Sensor ACTIVE: Do something here (continuous task) - } - if (sensor2 == SET) { - // Sensor ACTIVE: Do something here (continuous task) - } - - - // ==================================== SERIAL Tx/Rx Handling ==================================== - // 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.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(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); - } - #endif - // Rx USART MAIN - #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 - - // 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 - for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { - ibus_captured_value[(i/2)] = command.channels[i] + (command.channels[i+1] << 8) - 1000; // 1000-2000 -> 0-1000 - } - //ch1 = (ibus_captured_value[0] - 500) * 2; - //ch2 = (ibus_captured_value[1] - 500) * 2; - log_i( "CH1: %d \t CH2: %d\n", (ibus_captured_value[0] - 500) * 2, (ibus_captured_value[1] - 500) * 2); - #endif - if (timeoutCntSerial0++ >= SERIAL_TIMEOUT) { // Timeout qualification - timeoutFlagSerial0 = 1; // Timeout detected - timeoutCntSerial0 = SERIAL_TIMEOUT; // Limit timout counter value - } - 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 - } - #endif + 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++; diff --git a/Src/mpu6050.c b/Src/mpu6050.c index e356e3a..f018ebd 100644 --- a/Src/mpu6050.c +++ b/Src/mpu6050.c @@ -1,9 +1,9 @@ /** * This file was taken from InvenSense MotionApps v6.12 library and - * refactored for the hoverboard-sideboard-hack project. + * refactored for the hoverboard-sideboard-hack project. * * Copyright (C) 2020-2021 Emanuel FERU - * Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + * Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -46,6 +46,10 @@ MPU_Data mpu; // holds the MPU-6050 data +#ifdef SERIAL_AUX_RX +uint8_t print_aux = 0; // print AUX serial data +#endif + #ifdef MPU_SENSOR_ENABLE static signed char MPU_ORIENTATION[9] = {1, 0, 0, // [-] MPU Sensor orientation matrix: set this according to the sensor installation @@ -566,8 +570,7 @@ static int setup_compass(void); /** * @brief Enable/disable data ready interrupt. - * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready - * interrupt is used. + * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready interrupt is used. * @param[in] enable 1 to enable interrupt. * @return 0 if successful. */ @@ -613,9 +616,9 @@ int mpu_reg_dump(void) continue; if (i2c_read(st.hw->addr, ii, 1, &data)) return -1; - #ifdef SERIAL_DEBUG - log_i("%#5x: %#5x\r\n", ii, data); - #endif + #ifdef SERIAL_DEBUG + log_i("%#5x: %#5x\r\n", ii, data); + #endif } return 0; } @@ -895,17 +898,17 @@ int mpu_get_temperature(long *data, unsigned long *timestamp) * @return 0 if successful. */ int mpu_read_6500_accel_bias(long *accel_bias) { - unsigned char data[6]; - if (i2c_read(st.hw->addr, 0x77, 2, &data[0])) - return -1; - if (i2c_read(st.hw->addr, 0x7A, 2, &data[2])) - return -1; - if (i2c_read(st.hw->addr, 0x7D, 2, &data[4])) - return -1; - accel_bias[0] = ((long)data[0]<<8) | data[1]; - accel_bias[1] = ((long)data[2]<<8) | data[3]; - accel_bias[2] = ((long)data[4]<<8) | data[5]; - return 0; + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x77, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x7A, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x7D, 2, &data[4])) + return -1; + accel_bias[0] = ((long)data[0]<<8) | data[1]; + accel_bias[1] = ((long)data[2]<<8) | data[3]; + accel_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; } /** @@ -917,31 +920,31 @@ int mpu_read_6500_accel_bias(long *accel_bias) { * @return 0 if successful. */ int mpu_read_6050_accel_bias(long *accel_bias) { - unsigned char data[6]; - if (i2c_read(st.hw->addr, 0x06, 2, &data[0])) - return -1; - if (i2c_read(st.hw->addr, 0x08, 2, &data[2])) - return -1; - if (i2c_read(st.hw->addr, 0x0A, 2, &data[4])) - return -1; - accel_bias[0] = ((long)data[0]<<8) | data[1]; - accel_bias[1] = ((long)data[2]<<8) | data[3]; - accel_bias[2] = ((long)data[4]<<8) | data[5]; - return 0; + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x06, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x08, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x0A, 2, &data[4])) + return -1; + accel_bias[0] = ((long)data[0]<<8) | data[1]; + accel_bias[1] = ((long)data[2]<<8) | data[3]; + accel_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; } int mpu_read_6500_gyro_bias(long *gyro_bias) { - unsigned char data[6]; - if (i2c_read(st.hw->addr, 0x13, 2, &data[0])) - return -1; - if (i2c_read(st.hw->addr, 0x15, 2, &data[2])) - return -1; - if (i2c_read(st.hw->addr, 0x17, 2, &data[4])) - return -1; - gyro_bias[0] = ((long)data[0]<<8) | data[1]; - gyro_bias[1] = ((long)data[2]<<8) | data[3]; - gyro_bias[2] = ((long)data[4]<<8) | data[5]; - return 0; + unsigned char data[6]; + if (i2c_read(st.hw->addr, 0x13, 2, &data[0])) + return -1; + if (i2c_read(st.hw->addr, 0x15, 2, &data[2])) + return -1; + if (i2c_read(st.hw->addr, 0x17, 2, &data[4])) + return -1; + gyro_bias[0] = ((long)data[0]<<8) | data[1]; + gyro_bias[1] = ((long)data[2]<<8) | data[3]; + gyro_bias[2] = ((long)data[4]<<8) | data[5]; + return 0; } /** @@ -957,7 +960,7 @@ int mpu_set_gyro_bias_reg(long *gyro_bias) unsigned char data[6] = {0, 0, 0, 0, 0, 0}; int i=0; for(i=0;i<3;i++) { - gyro_bias[i]= (-gyro_bias[i]); + gyro_bias[i]= (-gyro_bias[i]); } data[0] = (gyro_bias[0] >> 8) & 0xff; data[1] = (gyro_bias[0]) & 0xff; @@ -1691,9 +1694,9 @@ int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, fifo_count = (data[0] << 8) | data[1]; if (fifo_count < packet_size) return 0; - // #ifdef SERIAL_DEBUG - // log_i("FIFO count: %hd\n", fifo_count); - // #endif + // #ifdef SERIAL_DEBUG + // log_i("FIFO count: %hd\r\n", fifo_count); + // #endif if (fifo_count > (st.hw->max_fifo >> 1)) { /* FIFO is 50% full, better check overflow bit. */ if (i2c_read(st.hw->addr, st.reg->int_status, 1, data)) @@ -2128,38 +2131,38 @@ static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) #define REG_6500_XG_ST_DATA 0x0 #define REG_6500_XA_ST_DATA 0xD static const unsigned short mpu_6500_st_tb[256] = { - 2620,2646,2672,2699,2726,2753,2781,2808, //7 - 2837,2865,2894,2923,2952,2981,3011,3041, //15 - 3072,3102,3133,3165,3196,3228,3261,3293, //23 - 3326,3359,3393,3427,3461,3496,3531,3566, //31 - 3602,3638,3674,3711,3748,3786,3823,3862, //39 - 3900,3939,3979,4019,4059,4099,4140,4182, //47 - 4224,4266,4308,4352,4395,4439,4483,4528, //55 - 4574,4619,4665,4712,4759,4807,4855,4903, //63 - 4953,5002,5052,5103,5154,5205,5257,5310, //71 - 5363,5417,5471,5525,5581,5636,5693,5750, //79 - 5807,5865,5924,5983,6043,6104,6165,6226, //87 - 6289,6351,6415,6479,6544,6609,6675,6742, //95 - 6810,6878,6946,7016,7086,7157,7229,7301, //103 - 7374,7448,7522,7597,7673,7750,7828,7906, //111 - 7985,8065,8145,8227,8309,8392,8476,8561, //119 - 8647,8733,8820,8909,8998,9088,9178,9270, - 9363,9457,9551,9647,9743,9841,9939,10038, - 10139,10240,10343,10446,10550,10656,10763,10870, - 10979,11089,11200,11312,11425,11539,11654,11771, - 11889,12008,12128,12249,12371,12495,12620,12746, - 12874,13002,13132,13264,13396,13530,13666,13802, - 13940,14080,14221,14363,14506,14652,14798,14946, - 15096,15247,15399,15553,15709,15866,16024,16184, - 16346,16510,16675,16842,17010,17180,17352,17526, - 17701,17878,18057,18237,18420,18604,18790,18978, - 19167,19359,19553,19748,19946,20145,20347,20550, - 20756,20963,21173,21385,21598,21814,22033,22253, - 22475,22700,22927,23156,23388,23622,23858,24097, - 24338,24581,24827,25075,25326,25579,25835,26093, - 26354,26618,26884,27153,27424,27699,27976,28255, - 28538,28823,29112,29403,29697,29994,30294,30597, - 30903,31212,31524,31839,32157,32479,32804,33132 + 2620,2646,2672,2699,2726,2753,2781,2808, //7 + 2837,2865,2894,2923,2952,2981,3011,3041, //15 + 3072,3102,3133,3165,3196,3228,3261,3293, //23 + 3326,3359,3393,3427,3461,3496,3531,3566, //31 + 3602,3638,3674,3711,3748,3786,3823,3862, //39 + 3900,3939,3979,4019,4059,4099,4140,4182, //47 + 4224,4266,4308,4352,4395,4439,4483,4528, //55 + 4574,4619,4665,4712,4759,4807,4855,4903, //63 + 4953,5002,5052,5103,5154,5205,5257,5310, //71 + 5363,5417,5471,5525,5581,5636,5693,5750, //79 + 5807,5865,5924,5983,6043,6104,6165,6226, //87 + 6289,6351,6415,6479,6544,6609,6675,6742, //95 + 6810,6878,6946,7016,7086,7157,7229,7301, //103 + 7374,7448,7522,7597,7673,7750,7828,7906, //111 + 7985,8065,8145,8227,8309,8392,8476,8561, //119 + 8647,8733,8820,8909,8998,9088,9178,9270, + 9363,9457,9551,9647,9743,9841,9939,10038, + 10139,10240,10343,10446,10550,10656,10763,10870, + 10979,11089,11200,11312,11425,11539,11654,11771, + 11889,12008,12128,12249,12371,12495,12620,12746, + 12874,13002,13132,13264,13396,13530,13666,13802, + 13940,14080,14221,14363,14506,14652,14798,14946, + 15096,15247,15399,15553,15709,15866,16024,16184, + 16346,16510,16675,16842,17010,17180,17352,17526, + 17701,17878,18057,18237,18420,18604,18790,18978, + 19167,19359,19553,19748,19946,20145,20347,20550, + 20756,20963,21173,21385,21598,21814,22033,22253, + 22475,22700,22927,23156,23388,23622,23858,24097, + 24338,24581,24827,25075,25326,25579,25835,26093, + 26354,26618,26884,27153,27424,27699,27976,28255, + 28538,28823,29112,29403,29697,29994,30294,30597, + 30903,31212,31524,31839,32157,32479,32804,33132 }; static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug) { @@ -2168,85 +2171,85 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug) float st_shift_cust[3], st_shift_ratio[3], ct_shift_prod[3], accel_offset_max; unsigned char regs[3]; if (i2c_read(st.hw->addr, REG_6500_XA_ST_DATA, 3, regs)) { - if(debug) - log_i("Reading OTP Register Error.\n"); - return 0x07; + if(debug) + log_i("Reading OTP Register Error\r\n"); + return 0x07; } if(debug) - log_i("Accel OTP:%d, %d, %d\n", regs[0], regs[1], regs[2]); - for (i = 0; i < 3; i++) { - if (regs[i] != 0) { - ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; - ct_shift_prod[i] *= 65536.f; - ct_shift_prod[i] /= test.accel_sens; - } - else { - ct_shift_prod[i] = 0; - otp_value_zero = 1; - } - } - if(otp_value_zero == 0) { - if(debug) - log_i("ACCEL:CRITERIA A\n"); - for (i = 0; i < 3; i++) { - st_shift_cust[i] = bias_st[i] - bias_regular[i]; - if(debug) { - log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n", - (long)st_shift_cust[i], bias_regular[i], - bias_st[i]); - log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]); - } + log_i("Accel OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]); + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + ct_shift_prod[i] *= 65536.f; + ct_shift_prod[i] /= test.accel_sens; + } + else { + ct_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + if(otp_value_zero == 0) { + if(debug) + log_i("ACCEL:CRITERIA A\r\n"); + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; + if(debug) { + log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n", + (long)st_shift_cust[i], bias_regular[i], + bias_st[i]); + log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]); + } - st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f; + st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f; - if(debug) - log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i], - (long)test.max_accel_var); + if(debug) + log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i], + (long)test.max_accel_var); - if (fabs(st_shift_ratio[i]) > test.max_accel_var) { - if(debug) - log_i("ACCEL Fail Axis = %d\n", i); - result |= 1 << i; //Error condition - } - } - } - else { - /* Self Test Pass/Fail Criteria B */ - accel_st_al_min = test.min_g * 65536.f; - accel_st_al_max = test.max_g * 65536.f; + if (fabs(st_shift_ratio[i]) > test.max_accel_var) { + if(debug) + log_i("ACCEL Fail Axis = %d\r\n", i); + result |= 1 << i; //Error condition + } + } + } + else { + /* Self Test Pass/Fail Criteria B */ + accel_st_al_min = test.min_g * 65536.f; + accel_st_al_max = test.max_g * 65536.f; - if(debug) { - log_i("ACCEL:CRITERIA B\r\n"); - log_i("Min MG: %ld\r\n", (long)accel_st_al_min); - log_i("Max MG: %ld\r\n", (long)accel_st_al_max); - } + if(debug) { + log_i("ACCEL:CRITERIA B\r\n"); + log_i("Min MG: %ld\r\n", (long)accel_st_al_min); + log_i("Max MG: %ld\r\n", (long)accel_st_al_max); + } - for (i = 0; i < 3; i++) { - st_shift_cust[i] = bias_st[i] - bias_regular[i]; + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; - if(debug) - log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]); - if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) { - if(debug) - log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i); - result |= 1 << i; //Error condition - } - } - } + if(debug) + log_i("Bias_shift=%ld, st=%ld, reg=%ld\r\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]); + if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) { + if(debug) + log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\r\n", i); + result |= 1 << i; //Error condition + } + } + } - if(result == 0) { - /* Self Test Pass/Fail Criteria C */ - accel_offset_max = test.max_g_offset * 65536.f; - if(debug) - log_i("Accel:CRITERIA C: bias less than %ld\n", (long)accel_offset_max); - for (i = 0; i < 3; i++) { - if(fabs(bias_regular[i]) > accel_offset_max) { - if(debug) - log_i("FAILED: Accel axis:%d = %ld > 500mg\n", i, bias_regular[i]); - result |= 1 << i; //Error condition - } - } - } + if(result == 0) { + /* Self Test Pass/Fail Criteria C */ + accel_offset_max = test.max_g_offset * 65536.f; + if(debug) + log_i("Accel:CRITERIA C: bias less than %ld\n", (long)accel_offset_max); + for (i = 0; i < 3; i++) { + if(fabs(bias_regular[i]) > accel_offset_max) { + if(debug) + log_i("FAILED: Accel axis:%d = %ld > 500mg\n", i, bias_regular[i]); + result |= 1 << i; //Error condition + } + } + } return result; } @@ -2259,88 +2262,88 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug) unsigned char regs[3]; if (i2c_read(st.hw->addr, REG_6500_XG_ST_DATA, 3, regs)) { - if(debug) - log_i("Reading OTP Register Error.\n"); + if(debug) + log_i("Reading OTP Register Error.\n"); return 0x07; } if(debug) - log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]); + log_i("Gyro OTP:%d, %d, %d\r\n", regs[0], regs[1], regs[2]); - for (i = 0; i < 3; i++) { - if (regs[i] != 0) { - ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; - ct_shift_prod[i] *= 65536.f; - ct_shift_prod[i] /= test.gyro_sens; - } - else { - ct_shift_prod[i] = 0; - otp_value_zero = 1; - } - } + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + ct_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + ct_shift_prod[i] *= 65536.f; + ct_shift_prod[i] /= test.gyro_sens; + } + else { + ct_shift_prod[i] = 0; + otp_value_zero = 1; + } + } - if(otp_value_zero == 0) { - if(debug) - log_i("GYRO:CRITERIA A\n"); - /* Self Test Pass/Fail Criteria A */ - for (i = 0; i < 3; i++) { - st_shift_cust[i] = bias_st[i] - bias_regular[i]; + if(otp_value_zero == 0) { + if(debug) + log_i("GYRO:CRITERIA A\n"); + /* Self Test Pass/Fail Criteria A */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; - if(debug) { - log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n", - (long)st_shift_cust[i], bias_regular[i], - bias_st[i]); - log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]); - } + if(debug) { + log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n", + (long)st_shift_cust[i], bias_regular[i], + bias_st[i]); + log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]); + } - st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i]; + st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i]; - if(debug) - log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i], - (long)test.max_gyro_var); + if(debug) + log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i], + (long)test.max_gyro_var); - if (fabs(st_shift_ratio[i]) < test.max_gyro_var) { - if(debug) - log_i("Gyro Fail Axis = %d\n", i); - result |= 1 << i; //Error condition - } - } - } - else { - /* Self Test Pass/Fail Criteria B */ - gyro_st_al_max = test.max_dps * 65536.f; + if (fabs(st_shift_ratio[i]) < test.max_gyro_var) { + if(debug) + log_i("Gyro Fail Axis = %d\n", i); + result |= 1 << i; //Error condition + } + } + } + else { + /* Self Test Pass/Fail Criteria B */ + gyro_st_al_max = test.max_dps * 65536.f; - if(debug) { - log_i("GYRO:CRITERIA B\r\n"); - log_i("Max DPS: %ld\r\n", (long)gyro_st_al_max); - } + if(debug) { + log_i("GYRO:CRITERIA B\r\n"); + log_i("Max DPS: %ld\r\n", (long)gyro_st_al_max); + } - for (i = 0; i < 3; i++) { - st_shift_cust[i] = bias_st[i] - bias_regular[i]; + for (i = 0; i < 3; i++) { + st_shift_cust[i] = bias_st[i] - bias_regular[i]; - if(debug) - log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]); - if(st_shift_cust[i] < gyro_st_al_max) { - if(debug) - log_i("GYRO FAIL axis:%d greater than 60dps\n", i); - result |= 1 << i; //Error condition - } - } - } + if(debug) + log_i("Bias_shift=%ld, st=%ld, reg=%ld\r\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]); + if(st_shift_cust[i] < gyro_st_al_max) { + if(debug) + log_i("GYRO FAIL axis:%d greater than 60dps\r\n", i); + result |= 1 << i; //Error condition + } + } + } - if(result == 0) { - /* Self Test Pass/Fail Criteria C */ - gyro_offset_max = test.min_dps * 65536.f; - if(debug) - log_i("Gyro:CRITERIA C: bias less than %ld\n", (long)gyro_offset_max); - for (i = 0; i < 3; i++) { - if(fabs(bias_regular[i]) > gyro_offset_max) { - if(debug) - log_i("FAILED: Gyro axis:%d = %ld > 20dps\n", i, bias_regular[i]); - result |= 1 << i; //Error condition - } - } - } + if(result == 0) { + /* Self Test Pass/Fail Criteria C */ + gyro_offset_max = test.min_dps * 65536.f; + if(debug) + log_i("Gyro:CRITERIA C: bias less than %ld\r\n", (long)gyro_offset_max); + for (i = 0; i < 3; i++) { + if(fabs(bias_regular[i]) > gyro_offset_max) { + if(debug) + log_i("FAILED: Gyro axis:%d = %ld > 20dps\r\n", i, bias_regular[i]); + result |= 1 << i; //Error condition + } + } + } return result; } @@ -2406,44 +2409,44 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in accel[0] = accel[1] = accel[2] = 0; if(debug) - log_i("Starting Bias Loop Reads\n"); + log_i("Starting Bias Loop Reads\r\n"); //start reading samples while (s < test.packet_thresh) { - delay_ms(test.sample_wait_ms); //wait 10ms to fill FIFO - if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) - return -1; - fifo_count = (data[0] << 8) | data[1]; - packet_count = fifo_count / MAX_PACKET_LENGTH; - if ((test.packet_thresh - s) < packet_count) - packet_count = test.packet_thresh - s; - read_size = packet_count * MAX_PACKET_LENGTH; + delay_ms(test.sample_wait_ms); //wait 10ms to fill FIFO + if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) + return -1; + fifo_count = (data[0] << 8) | data[1]; + packet_count = fifo_count / MAX_PACKET_LENGTH; + if ((test.packet_thresh - s) < packet_count) + packet_count = test.packet_thresh - s; + read_size = packet_count * MAX_PACKET_LENGTH; - //burst read from FIFO - if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data)) - return -1; - ind = 0; - for (ii = 0; ii < packet_count; ii++) { - short accel_cur[3], gyro_cur[3]; - accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1]; - accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3]; - accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5]; - accel[0] += (long)accel_cur[0]; - accel[1] += (long)accel_cur[1]; - accel[2] += (long)accel_cur[2]; - gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]); - gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]); - gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]); - gyro[0] += (long)gyro_cur[0]; - gyro[1] += (long)gyro_cur[1]; - gyro[2] += (long)gyro_cur[2]; - ind += MAX_PACKET_LENGTH; - } - s += packet_count; + //burst read from FIFO + if (i2c_read(st.hw->addr, st.reg->fifo_r_w, read_size, data)) + return -1; + ind = 0; + for (ii = 0; ii < packet_count; ii++) { + short accel_cur[3], gyro_cur[3]; + accel_cur[0] = ((short)data[ind + 0] << 8) | data[ind + 1]; + accel_cur[1] = ((short)data[ind + 2] << 8) | data[ind + 3]; + accel_cur[2] = ((short)data[ind + 4] << 8) | data[ind + 5]; + accel[0] += (long)accel_cur[0]; + accel[1] += (long)accel_cur[1]; + accel[2] += (long)accel_cur[2]; + gyro_cur[0] = (((short)data[ind + 6] << 8) | data[ind + 7]); + gyro_cur[1] = (((short)data[ind + 8] << 8) | data[ind + 9]); + gyro_cur[2] = (((short)data[ind + 10] << 8) | data[ind + 11]); + gyro[0] += (long)gyro_cur[0]; + gyro[1] += (long)gyro_cur[1]; + gyro[2] += (long)gyro_cur[2]; + ind += MAX_PACKET_LENGTH; + } + s += packet_count; } if(debug) - log_i("Samples: %d\n", s); + log_i("Samples: %d\r\n", s); //stop FIFO data[0] = 0; @@ -2464,8 +2467,8 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in if(debug) { - log_i("Accel offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, accel[0], accel[1], accel[2]); - log_i("Gyro offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, gyro[0], gyro[1], gyro[2]); + log_i("Accel offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, accel[0], accel[1], accel[2]); + log_i("Gyro offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, gyro[0], gyro[1], gyro[2]); } return 0; @@ -2504,7 +2507,7 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) if(debug) - log_i("Starting MPU6500 HWST!\r\n"); + log_i("Starting MPU6500 HWST!\r\n"); if (st.chip_cfg.dmp_on) { mpu_set_dmp_state(0); @@ -2521,7 +2524,7 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) mpu_get_fifo_config(&fifo_sensors); if(debug) - log_i("Retrieving Biases\r\n"); + log_i("Retrieving Biases\r\n"); for (ii = 0; ii < tries; ii++) if (!get_st_6500_biases(gyro, accel, 0, debug)) @@ -2531,14 +2534,14 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) * We'll just report an error for all three sensors. */ if(debug) - log_i("Retrieving Biases Error - possible I2C error\n"); + log_i("Retrieving Biases Error - possible I2C error\r\n"); result = 0; goto restore; } if(debug) - log_i("Retrieving ST Biases\n"); + log_i("Retrieving ST Biases\r\n"); for (ii = 0; ii < tries; ii++) if (!get_st_6500_biases(gyro_st, accel_st, 1, debug)) @@ -2546,7 +2549,7 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) if (ii == tries) { if(debug) - log_i("Retrieving ST Biases Error - possible I2C error\n"); + log_i("Retrieving ST Biases Error - possible I2C error\r\n"); /* Again, probably an I2C error. */ result = 0; @@ -2555,11 +2558,11 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) accel_result = accel_6500_self_test(accel, accel_st, debug); if(debug) - log_i("Accel Self Test Results: %d\n", accel_result); + log_i("Accel Self Test Results: %d\r\n", accel_result); gyro_result = gyro_6500_self_test(gyro, gyro_st, debug); if(debug) - log_i("Gyro Self Test Results: %d\n", gyro_result); + log_i("Gyro Self Test Results: %d\r\n", gyro_result); result = 0; if (!gyro_result) @@ -2570,34 +2573,34 @@ int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug) #ifdef AK89xx_SECONDARY compass_result = compass_self_test(); if(debug) - log_i("Compass Self Test Results: %d\n", compass_result); + log_i("Compass Self Test Results: %d\r\n", compass_result); if (!compass_result) result |= 0x04; #else result |= 0x04; #endif restore: - if(debug) - log_i("Exiting HWST\n"); - /* Set to invalid values to ensure no I2C writes are skipped. */ - st.chip_cfg.gyro_fsr = 0xFF; - st.chip_cfg.accel_fsr = 0xFF; - st.chip_cfg.lpf = 0xFF; - st.chip_cfg.sample_rate = 0xFFFF; - st.chip_cfg.sensors = 0xFF; - st.chip_cfg.fifo_enable = 0xFF; - st.chip_cfg.clk_src = INV_CLK_PLL; - mpu_set_gyro_fsr(gyro_fsr); - mpu_set_accel_fsr(accel_fsr); - mpu_set_lpf(lpf); - mpu_set_sample_rate(sample_rate); - mpu_set_sensors(sensors_on); - mpu_configure_fifo(fifo_sensors); + if(debug) + log_i("Exiting HWST\r\n"); + /* Set to invalid values to ensure no I2C writes are skipped. */ + st.chip_cfg.gyro_fsr = 0xFF; + st.chip_cfg.accel_fsr = 0xFF; + st.chip_cfg.lpf = 0xFF; + st.chip_cfg.sample_rate = 0xFFFF; + st.chip_cfg.sensors = 0xFF; + st.chip_cfg.fifo_enable = 0xFF; + st.chip_cfg.clk_src = INV_CLK_PLL; + mpu_set_gyro_fsr(gyro_fsr); + mpu_set_accel_fsr(accel_fsr); + mpu_set_lpf(lpf); + mpu_set_sample_rate(sample_rate); + mpu_set_sensors(sensors_on); + mpu_configure_fifo(fifo_sensors); - if (dmp_was_on) - mpu_set_dmp_state(1); + if (dmp_was_on) + mpu_set_dmp_state(1); - return result; + return result; } #endif /* @@ -2881,7 +2884,7 @@ static int setup_compass(void) if (akm_addr > 0x0F) { /* TODO: Handle this case in all compass-related functions. */ #ifdef SERIAL_DEBUG - log_i("Compass not found.\n"); + log_i("Compass not found\r\n"); #endif return -1; } @@ -3083,7 +3086,7 @@ int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, unsigned #endif if (lpa_freq) { #if defined MPU6500 - unsigned char thresh_hw; + unsigned char thresh_hw; /* 1LSb = 4mg. */ if (thresh > 1020) @@ -3235,17 +3238,17 @@ void mpu_start_self_test(void) result = mpu_run_self_test(gyro, accel); #endif #ifdef SERIAL_DEBUG - log_i("accel: %ld %ld %ld\n", + log_i("accel: %ld %ld %ld\r\n", accel[0], accel[1], accel[2]); - log_i("gyro: %ld %ld %ld\n", + log_i("gyro: %ld %ld %ld\r\n", gyro[0], gyro[1], gyro[2]); #endif if (result == 0x7) { - consoleLog("Passed!\n"); + consoleLog("Passed!\r\n"); /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/ #ifdef USE_CAL_HW_REGISTERS @@ -3256,10 +3259,10 @@ void mpu_start_self_test(void) unsigned char i = 0; for(i = 0; i<3; i++) { - gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps - accel[i] *= 2048.f; //convert to +-16G - accel[i] = accel[i] >> 16; - gyro[i] = (long)(gyro[i] >> 16); + gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps + accel[i] *= 2048.f; //convert to +-16G + accel[i] = accel[i] >> 16; + gyro[i] = (long)(gyro[i] >> 16); } mpu_set_gyro_bias_reg(gyro); @@ -3272,12 +3275,12 @@ void mpu_start_self_test(void) #endif } else { - if (!(result & 0x1)) - consoleLog("Gyro failed.\n"); - if (!(result & 0x2)) - consoleLog("Accel failed.\n"); - if (!(result & 0x4)) - consoleLog("Compass failed.\n"); + if (!(result & 0x1)) + consoleLog("Gyro failed\r\n"); + if (!(result & 0x2)) + consoleLog("Accel failed\r\n"); + if (!(result & 0x4)) + consoleLog("Compass failed\r\n"); } } @@ -3304,18 +3307,18 @@ static struct hal_s hal = {0}; void mpu_setup_gyro(void) { unsigned char mask = 0, lp_accel_was_on = 0; - if (hal.sensors & ACCEL_ON) { + if (hal.sensors & ACCEL_ON) { mask |= INV_XYZ_ACCEL; - consoleLog("Accel sensor On.\n"); - } else { - consoleLog("Accel sensor Off.\n"); + consoleLog("Accel ON\r\n"); + } else { + consoleLog("Accel OFF\r\n"); } if (hal.sensors & GYRO_ON) { mask |= INV_XYZ_GYRO; lp_accel_was_on |= hal.lp_accel_mode; - consoleLog("Gyro sensor On.\n"); + consoleLog("Gyro ON\r\n"); } else { - consoleLog("Gyro sensor Off.\n"); + consoleLog("Gyro OFF\r\n"); } #ifdef COMPASS_ENABLED if (hal.sensors & COMPASS_ON) { @@ -3379,33 +3382,33 @@ unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) /* =========================== MPU-6050 Configuration =========================== */ int mpu_config(void) { - consoleLog("-- Configuring MPU6050... "); + consoleLog("Configuring MPU6050... "); if(mpu_init()) { - consoleLog("FAIL (MPU).\n"); + consoleLog("FAIL (MPU)\r\n"); return -1; - } - + } + /* Get/set hardware configuration. Start gyro. */ /* Wake up all sensors. */ mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); - + /* Push both gyro and accel data into the FIFO. */ mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(MPU_DEFAULT_HZ); - + /* Read back configuration in case it was set improperly. */ // mpu_get_sample_rate(&gyro_rate); // mpu_get_gyro_fsr(&gyro_fsr); // mpu_get_accel_fsr(&accel_fsr); - + /* Initialize HAL state variables. */ - hal.sensors = ACCEL_ON | GYRO_ON; - hal.dmp_on = 0; - hal.report = 0; - hal.next_pedo_ms = 0; - hal.next_temp_ms = 0; - + hal.sensors = ACCEL_ON | GYRO_ON; + hal.dmp_on = 0; + hal.report = 0; + hal.next_pedo_ms = 0; + hal.next_temp_ms = 0; + #ifdef MPU_DMP_ENABLE /* To initialize the DMP: * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in @@ -3437,9 +3440,9 @@ int mpu_config(void) * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot * be used in combination with DMP_FEATURE_SEND_RAW_GYRO. */ - consoleLog(" writing DMP... "); + consoleLog(" writing DMP... "); if (dmp_load_motion_driver_firmware()) { - consoleLog(" FAIL (DMP) --\r\n"); + consoleLog(" FAIL (DMP)\r\n"); return -1; } dmp_set_orientation(inv_orientation_matrix_to_scalar(MPU_ORIENTATION)); @@ -3459,14 +3462,14 @@ int mpu_config(void) * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G */ hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | - DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_RAW_GYRO | DMP_FEATURE_GYRO_CAL; + DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_RAW_GYRO | DMP_FEATURE_GYRO_CAL; dmp_enable_feature(hal.dmp_features); dmp_set_fifo_rate(MPU_DEFAULT_HZ); mpu_set_dmp_state(1); - hal.dmp_on = 1; + hal.dmp_on = 1; #endif - consoleLog(" OK --\r\n"); + consoleLog(" OK\r\n"); return 0; } @@ -3475,32 +3478,32 @@ int mpu_config(void) void mpu_get_data(void) { - - unsigned long sensor_timestamp; - unsigned long timestamp; - unsigned char new_data = 0, new_temp = 0; - uint8_t mpu_int_status; // holds actual interrupt status byte from MPU - - // check for DMP interrupt bit or Data Ready interrupt bit (in case DMP is disabled) -> this interrupt should happen frequently - i2c_readByte(st.hw->addr, st.reg->int_status, &mpu_int_status); - if (mpu_int_status & MPU_INT_STATUS_DMP || mpu_int_status & MPU_INT_STATUS_DATA_READY) { - hal.new_gyro = 1; - } - - get_tick_count_ms(×tamp); - /* Temperature data doesn't need to be read with every gyro sample. - * Let's make them timer-based. - */ - if (timestamp > hal.next_temp_ms) { + + unsigned long sensor_timestamp; + unsigned long timestamp; + unsigned char new_data = 0, new_temp = 0; + uint8_t mpu_int_status; // holds actual interrupt status byte from MPU + + // check for DMP interrupt bit or Data Ready interrupt bit (in case DMP is disabled) -> this interrupt should happen frequently + i2c_readByte(st.hw->addr, st.reg->int_status, &mpu_int_status); + if (mpu_int_status & MPU_INT_STATUS_DMP || mpu_int_status & MPU_INT_STATUS_DATA_READY) { + hal.new_gyro = 1; + } + + get_tick_count_ms(×tamp); + /* Temperature data doesn't need to be read with every gyro sample. + * Let's make them timer-based. + */ + if (timestamp > hal.next_temp_ms) { hal.next_temp_ms = timestamp + TEMP_READ_MS; new_temp = 1; - } - - - if (hal.new_gyro && hal.dmp_on) { + } + + + if (hal.new_gyro && hal.dmp_on) { short gyro[3], accel[3], sensors; - static long quat[4], temperature; - unsigned char more; + static long quat[4], temperature; + unsigned char more; /* This function gets new data from the FIFO when the DMP is in * use. The FIFO can contain any combination of gyro, accel, * quaternion, and gesture data. The sensors parameter tells the @@ -3516,7 +3519,7 @@ void mpu_get_data(void) dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); if (!more) hal.new_gyro = 0; - if (sensors & INV_XYZ_GYRO) { + if (sensors & INV_XYZ_GYRO) { mpu.gyro.x = gyro[0]; mpu.gyro.y = gyro[1]; mpu.gyro.z = gyro[2]; @@ -3524,27 +3527,27 @@ void mpu_get_data(void) if (new_temp) { new_temp = 0; mpu_get_temperature(&temperature, &sensor_timestamp); - mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC] - } + mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC] + } } if (sensors & INV_XYZ_ACCEL) { mpu.accel.x = accel[0]; mpu.accel.y = accel[1]; mpu.accel.z = accel[2]; - new_data = 1; + new_data = 1; } if (sensors & INV_WXYZ_QUAT) { mpu.quat.w = quat[0]; mpu.quat.x = quat[1]; mpu.quat.y = quat[2]; mpu.quat.z = quat[3]; - mpu_calc_euler_angles(); // Calculate Euler angles - new_data = 1; + mpu_calc_euler_angles(); // Calculate Euler angles + new_data = 1; } - } else if (hal.new_gyro) { + } else if (hal.new_gyro) { short gyro[3], accel[3]; long temperature; - unsigned char sensors, more; + unsigned char sensors, more; /* This function gets new data from the FIFO. The FIFO can contain * gyro, accel, both, or neither. The sensors parameter tells the * caller which data fields were actually populated with new data. @@ -3558,7 +3561,7 @@ void mpu_get_data(void) mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more); if (more) hal.new_gyro = 1; - if (sensors & INV_XYZ_GYRO) { + if (sensors & INV_XYZ_GYRO) { mpu.gyro.x = gyro[0]; mpu.gyro.y = gyro[1]; mpu.gyro.z = gyro[2]; @@ -3566,7 +3569,7 @@ void mpu_get_data(void) if (new_temp) { new_temp = 0; mpu_get_temperature(&temperature, &sensor_timestamp); - mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC] + mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC] } } if (sensors & INV_XYZ_ACCEL) { @@ -3575,57 +3578,57 @@ void mpu_get_data(void) mpu.accel.z = accel[2]; new_data = 1; } - } + } - if (new_data) { + if (new_data) { // do something if needed - } - + } + } /* =========================== MPU-6050 Post-processing Functions =========================== */ void mpu_read_gyro_raw(void) -{ - uint8_t buffer[6]; - - // Read 6 BYTES of data starting from GYRO_XOUT_H register (the MPU-6050 automatically increments the register address) - i2c_readBytes(st.hw->addr, st.reg->raw_accel, 6, buffer); - - mpu.gyro.x = (int16_t)(buffer[0] << 8 | buffer[1]); - mpu.gyro.y = (int16_t)(buffer[2] << 8 | buffer[3]); - mpu.gyro.z = (int16_t)(buffer[4] << 8 | buffer[5]); +{ + uint8_t buffer[6]; - /*** convert the RAW hardware units values into dps (�/s) - we have to divide according to the Full scale value set in FS_SEL, - configured to 2000�/s (check MPU_GYRO_FSR). So we need to divide by 16.4 LSB/�/s - for more details check GYRO_CONFIG Register ****/ - //Gx = mpu.gyro.x / 16.4; - //Gy = mpu.gyro.y / 16.4; - //Gz = mpu.gyro.z / 16.4; + // Read 6 BYTES of data starting from GYRO_XOUT_H register (the MPU-6050 automatically increments the register address) + i2c_readBytes(st.hw->addr, st.reg->raw_accel, 6, buffer); + + mpu.gyro.x = (int16_t)(buffer[0] << 8 | buffer[1]); + mpu.gyro.y = (int16_t)(buffer[2] << 8 | buffer[3]); + mpu.gyro.z = (int16_t)(buffer[4] << 8 | buffer[5]); + + /*** convert the RAW hardware units values into dps (�/s) + we have to divide according to the Full scale value set in FS_SEL, + configured to 2000�/s (check MPU_GYRO_FSR). So we need to divide by 16.4 LSB/�/s + for more details check GYRO_CONFIG Register ****/ + //Gx = mpu.gyro.x / 16.4; + //Gy = mpu.gyro.y / 16.4; + //Gz = mpu.gyro.z / 16.4; } void mpu_read_accel_raw(void) { - uint8_t buffer[6]; + uint8_t buffer[6]; - // Read 6 BYTES of data starting from ACCEL_XOUT_H register (the MPU-6050 automatically increments the register address) - i2c_readBytes(st.hw->addr, st.reg->raw_gyro, 6, buffer); - - mpu.accel.x = (int16_t)(buffer[0] << 8 | buffer[1]); - mpu.accel.y = (int16_t)(buffer[2] << 8 | buffer[3]); - mpu.accel.z = (int16_t)(buffer[4] << 8 | buffer[5]); + // Read 6 BYTES of data starting from ACCEL_XOUT_H register (the MPU-6050 automatically increments the register address) + i2c_readBytes(st.hw->addr, st.reg->raw_gyro, 6, buffer); + + mpu.accel.x = (int16_t)(buffer[0] << 8 | buffer[1]); + mpu.accel.y = (int16_t)(buffer[2] << 8 | buffer[3]); + mpu.accel.z = (int16_t)(buffer[4] << 8 | buffer[5]); - /*** convert the RAW hardware units into acceleration in 'g' - we have to divide according to the Full scale value set in FS_SEL, - configured to 2g (check MPU_ACCEL_FSR). So we need to divide by 16384.0 LSB/g - for more details check ACCEL_CONFIG Register ****/ - //Ax = mpu.accel.x / 16384.0; - //Ay = mpu.accel.y / 16384.0; - //Az = mpu.accel.z / 16384.0; + /*** convert the RAW hardware units into acceleration in 'g' + we have to divide according to the Full scale value set in FS_SEL, + configured to 2g (check MPU_ACCEL_FSR). So we need to divide by 16384.0 LSB/g + for more details check ACCEL_CONFIG Register ****/ + //Ax = mpu.accel.x / 16384.0; + //Ay = mpu.accel.y / 16384.0; + //Az = mpu.accel.z / 16384.0; } /* @@ -3636,26 +3639,26 @@ void mpu_read_accel_raw(void) * 3. yaw (z-axis rotation) */ void mpu_calc_euler_angles(void) { - - float w, x, y, z; - float yaw, pitch, roll; - - // Convert quaternions[q30] to quaternion[float] - w = (float)mpu.quat.w / q30; // q30 = 2^30 - x = (float)mpu.quat.x / q30; - y = (float)mpu.quat.y / q30; - z = (float)mpu.quat.z / q30; - - // Calculate Euler angles: source - roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // roll (x-axis rotation) - pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation) - yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // yaw (z-axis rotation) - - // Convert [rad] to [deg*100] - mpu.euler.roll = (int16_t)(roll * RAD2DEG * 100); - mpu.euler.pitch = (int16_t)(pitch * RAD2DEG * 100); - mpu.euler.yaw = (int16_t)(yaw * RAD2DEG * 100); - + + float w, x, y, z; + float yaw, pitch, roll; + + // Convert quaternions[q30] to quaternion[float] + w = (float)mpu.quat.w / q30; // q30 = 2^30 + x = (float)mpu.quat.x / q30; + y = (float)mpu.quat.y / q30; + z = (float)mpu.quat.z / q30; + + // Calculate Euler angles: source + roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // roll (x-axis rotation) + pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation) + yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // yaw (z-axis rotation) + + // Convert [rad] to [deg*100] + mpu.euler.roll = (int16_t)(roll * RAD2DEG * 100); + mpu.euler.pitch = (int16_t)(pitch * RAD2DEG * 100); + mpu.euler.yaw = (int16_t)(yaw * RAD2DEG * 100); + } @@ -3683,263 +3686,282 @@ void mpu_tap_func(unsigned char direction, unsigned char count) default: return; } - #ifdef SERIAL_DEBUG - log_i("x %d\n", count); - #endif + #ifdef SERIAL_DEBUG + log_i("x %d\r\n", count); + #endif return; } void mpu_android_orient_func(unsigned char orientation) { - switch (orientation) { - case ANDROID_ORIENT_PORTRAIT: - consoleLog("Portrait\n"); + switch (orientation) { + case ANDROID_ORIENT_PORTRAIT: + consoleLog("Portrait\r\n"); break; - case ANDROID_ORIENT_LANDSCAPE: - consoleLog("Landscape\n"); + case ANDROID_ORIENT_LANDSCAPE: + consoleLog("Landscape\r\n"); break; - case ANDROID_ORIENT_REVERSE_PORTRAIT: - consoleLog("Reverse Portrait\n"); + case ANDROID_ORIENT_REVERSE_PORTRAIT: + consoleLog("Rev. Portrait\r\n"); break; - case ANDROID_ORIENT_REVERSE_LANDSCAPE: - consoleLog("Reverse Landscape\n"); + case ANDROID_ORIENT_REVERSE_LANDSCAPE: + consoleLog("Rev. Landscape\r\n"); break; - default: - return; - } + default: + return; + } } +/* =========================== MPU Print data =========================== */ + +void mpu_print_to_console(void) +{ +#ifdef SERIAL_DEBUG + if (hal.report & PRINT_ACCEL) { + log_i( "Accel[XYZ]: %d %d %d\r\n", mpu.accel.x, mpu.accel.y, mpu.accel.z); + } + if (hal.report & PRINT_GYRO) { + log_i( "Gyro[XYZ]: %d %d %d\r\n", mpu.gyro.x, mpu.gyro.y, mpu.gyro.z); + } + if (hal.report & PRINT_QUAT) { + log_i( "Quat[WXYZ]: %ld %ld %ld %ld\r\n", (long)mpu.quat.w, (long)mpu.quat.x, (long)mpu.quat.y, (long)mpu.quat.z); + } + if (hal.report & PRINT_EULER) { + log_i( "Euler[RPY]: %d %d %d\r\n", mpu.euler.roll, mpu.euler.pitch, mpu.euler.yaw); + } + if (hal.report & PRINT_TEMP) { + log_i( "Temp: %d\r\n", mpu.temp); + } + if (hal.report & PRINT_PEDO) { + unsigned long timestamp; + get_tick_count_ms(×tamp); + if (timestamp > hal.next_pedo_ms) { + hal.next_pedo_ms = timestamp + PEDO_READ_MS; + unsigned long step_count, walk_time; + dmp_get_pedometer_step_count(&step_count); + dmp_get_pedometer_walk_time(&walk_time); + log_i( "Walked %ld steps in %ld sec\r\n", step_count, walk_time/1000); + } + } +#endif +} + +#endif // MPU_SENSOR_ENABLE + + /* =========================== User Input Handling =========================== */ + void mpu_handle_input(char c) { #ifdef SERIAL_DEBUG switch (c) { - /* This command prints the Help text. */ - case 'h': - consoleLog("====== HELP COMMANDS ======\n"); - consoleLog("h: Print Help commands\n"); - consoleLog("8: Set Accelerometer sensor on/off\n"); - consoleLog("9: Set Gyroscope sensor on/off\n"); - consoleLog("r: Print Registers value\n"); - consoleLog("a: Print Accelerometer data\n"); - consoleLog("g: Print Gyroscope data\n"); - consoleLog("q: Print Quaternion data\n"); - consoleLog("e: Print Euler angles in degree * 100\n"); - consoleLog("t: Print Temperature in degC * 100\n"); - consoleLog("p: Print Pedometer data\n"); - consoleLog("0: Reset Pedometer\n"); - consoleLog("1: Set DMP/MPU frequency 10 Hz\n"); - consoleLog("2: Set DMP/MPU frequency 50 Hz\n"); - consoleLog("3: Set DMP/MPU frequency 100 Hz\n"); - consoleLog(",: Set DMP interrupt to gesture event only\n"); - consoleLog(".: Set DMP interrupt to continuous\n"); - consoleLog("f: Set DMP on/off\n"); - consoleLog("v: Set Quaternion on/off\n"); - consoleLog("w: Test low-power accel mode\n"); - consoleLog("s: Run self-test (device must be facing up or down)\n"); - consoleLog("===========================\n"); - break; - - /* These commands turn off individual sensors. */ - case '8': - hal.sensors ^= ACCEL_ON; - mpu_setup_gyro(); - break; - case '9': - hal.sensors ^= GYRO_ON; - mpu_setup_gyro(); - break; - - /* This command prints out the value of each gyro register for debugging. - * If logging is disabled, this function has no effect. - */ - case 'r': - mpu_reg_dump(); - break; - - /* These commands print individual sensor data. */ - case 'a': - hal.report ^= PRINT_ACCEL; - break; - case 'g': - hal.report ^= PRINT_GYRO; - break; - case 'q': - hal.report ^= PRINT_QUAT; - break; - case 'e': - hal.report ^= PRINT_EULER; - break; - case 't': - hal.report ^= PRINT_TEMP; - break; - case 'p': - /* Toggle pedometer display. */ - hal.report ^= PRINT_PEDO; - break; - case '0': - /* Reset pedometer. */ - dmp_set_pedometer_step_count(0); - dmp_set_pedometer_walk_time(0); - consoleLog("Pedometer reset done.\n"); - break; - - /* Depending on your application, sensor data may be needed at a faster or - * slower rate. These commands can speed up or slow down the rate at which - * the sensor data is received. - */ - case '1': - if (hal.dmp_on) { - if (0 == dmp_set_fifo_rate(10)) {consoleLog("DMP: 10 Hz\n");} - } else - if (0 == mpu_set_sample_rate(10)) {consoleLog("MPU: 10 Hz\n");} - break; - case '2': - if (hal.dmp_on) { - if (0 == dmp_set_fifo_rate(50)) {consoleLog("DMP: 50 Hz\n");} - } else - if (0 == mpu_set_sample_rate(50)) {consoleLog("MPU: 50 Hz\n");} - break; - case '3': - if (hal.dmp_on) { - if (0 == dmp_set_fifo_rate(100)) {consoleLog("DMP: 100 Hz\n");} - } else - if (0 == mpu_set_sample_rate(100)) {consoleLog("MPU: 100 Hz\n");} - break; - - /* Set hardware to interrupt on gesture event only. This feature is - * useful for keeping the MCU asleep until the DMP detects as a tap or - * orientation event. - */ - case ',': - dmp_set_interrupt_mode(DMP_INT_GESTURE); - break; - case '.': - /* Set hardware to interrupt periodically. */ - dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); - break; + /* This command prints the Help text. */ + case 'h': + consoleLog("=== HELP ===\r\n"); + consoleLog("h: Print Help\r\n"); + consoleLog("x: Print Serial AUX\r\n"); + #ifdef MPU_SENSOR_ENABLE + consoleLog("8: Set Accelerometer on/off\r\n"); + consoleLog("9: Set Gyroscope on/off\r\n"); + consoleLog("r: Print Registers\r\n"); + consoleLog("a: Print Accelerometer\r\n"); + consoleLog("g: Print Gyroscope\r\n"); + consoleLog("q: Print Quaternion\r\n"); + consoleLog("e: Print Euler angles in deg*100\r\n"); + consoleLog("t: Print Temperature in degC*100\r\n"); + consoleLog("p: Print Pedometer\r\n"); + consoleLog("0: Reset Pedometer\r\n"); + consoleLog("1: Set DMP/MPU freq 10 Hz\r\n"); + consoleLog("2: Set DMP/MPU freq 50 Hz\r\n"); + consoleLog("3: Set DMP/MPU freq 100 Hz\r\n"); + consoleLog(",: Set DMP interrupt to gesture\r\n"); + consoleLog(".: Set DMP interrupt to continuous\r\n"); + consoleLog("f: Set DMP on/off\r\n"); + consoleLog("v: Set Quaternion on/off\r\n"); + consoleLog("w: Test low-power accel mode\r\n"); + consoleLog("s: Run self-test (device must be facing up or down)\r\n"); + #endif // MPU_SENSOR_ENABLE + consoleLog("============\r\n"); + break; - /* Toggle DMP. */ - case 'f': - if (hal.lp_accel_mode) /* LP accel is not compatible with the DMP. */ - return; - if (hal.dmp_on) { - unsigned short dmp_rate; - unsigned char mask = 0; - hal.dmp_on = 0; - mpu_set_dmp_state(0); - /* Restore FIFO settings. */ - if (hal.sensors & ACCEL_ON) - mask |= INV_XYZ_ACCEL; - if (hal.sensors & GYRO_ON) - mask |= INV_XYZ_GYRO; - if (hal.sensors & COMPASS_ON) - mask |= INV_XYZ_COMPASS; - mpu_configure_fifo(mask); - /* When the DMP is used, the hardware sampling rate is fixed at - * 200Hz, and the DMP is configured to downsample the FIFO output - * using the function dmp_set_fifo_rate. However, when the DMP is - * turned off, the sampling rate remains at 200Hz. This could be - * handled in mpu6050.c, but it would need to know that - * mpu6050_dmp.c exists. To avoid this, we'll just - * put the extra logic in the application layer. - */ - dmp_get_fifo_rate(&dmp_rate); - mpu_set_sample_rate(dmp_rate); - consoleLog("DMP disabled.\n"); - } else { - unsigned short sample_rate; - hal.dmp_on = 1; - /* Preserve current FIFO rate. */ - mpu_get_sample_rate(&sample_rate); - dmp_set_fifo_rate(sample_rate); - mpu_set_dmp_state(1); - consoleLog("DMP enabled.\n"); - } - break; + /* These commands print individual sensor data. */ + case 'x': + #ifdef SERIAL_AUX_RX + print_aux ^= PRINT_AUX; + #else + consoleLog("AUX serial NOT enabled\r\n"); + #endif + break; - case 'v': - /* Toggle LP quaternion. - * The DMP features can be enabled/disabled at runtime. Use this same - * approach for other features. - */ - hal.dmp_features ^= DMP_FEATURE_6X_LP_QUAT; - dmp_enable_feature(hal.dmp_features); - if (!(hal.dmp_features & DMP_FEATURE_6X_LP_QUAT)) { - consoleLog("LP quaternion disabled.\n"); - } else - consoleLog("LP quaternion enabled.\n"); - break; - - /* Test out low-power accel mode. */ - case 'w': - if (hal.dmp_on) { - consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n"); - break; /* LP accel is not compatible with the DMP. */ - } - mpu_lp_accel_mode(20); - /* When LP accel mode is enabled, the driver automatically configures - * the hardware for latched interrupts. However, the MCU sometimes - * misses the rising/falling edge, and the hal.new_gyro flag is never - * set. To avoid getting locked in this state, we're overriding the - * driver's configuration and sticking to unlatched interrupt mode. - * - * TODO: The MCU supports level-triggered interrupts. - */ - mpu_set_int_latched(0); - hal.sensors &= ~(GYRO_ON|COMPASS_ON); - hal.sensors |= ACCEL_ON; - hal.lp_accel_mode = 1; - break; - - /* The hardware self test is completely localized in the gyro driver. - * Logging is assumed to be enabled; otherwise, a couple LEDs could - * probably be used here to display the test results. - */ - case 's': - mpu_start_self_test(); - break; - - default: - break; + #ifdef MPU_SENSOR_ENABLE + /* These commands turn off individual sensors. */ + case '8': + hal.sensors ^= ACCEL_ON; + mpu_setup_gyro(); + break; + case '9': + hal.sensors ^= GYRO_ON; + mpu_setup_gyro(); + break; + + /* This command prints out the value of each gyro register for debugging. + * If logging is disabled, this function has no effect. + */ + case 'r': + mpu_reg_dump(); + break; + + /* These commands print individual sensor data. */ + case 'a': + hal.report ^= PRINT_ACCEL; + break; + case 'g': + hal.report ^= PRINT_GYRO; + break; + case 'q': + hal.report ^= PRINT_QUAT; + break; + case 'e': + hal.report ^= PRINT_EULER; + break; + case 't': + hal.report ^= PRINT_TEMP; + break; + case 'p': + /* Toggle pedometer display. */ + hal.report ^= PRINT_PEDO; + break; + case '0': + /* Reset pedometer. */ + dmp_set_pedometer_step_count(0); + dmp_set_pedometer_walk_time(0); + consoleLog("Pedometer reset OK\r\n"); + break; + + /* Depending on your application, sensor data may be needed at a faster or + * slower rate. These commands can speed up or slow down the rate at which + * the sensor data is received. + */ + case '1': + if (hal.dmp_on) { + if (0 == dmp_set_fifo_rate(10)) {consoleLog("DMP 10 Hz\r\n");} + } else + if (0 == mpu_set_sample_rate(10)) {consoleLog("MPU 10 Hz\r\n");} + break; + case '2': + if (hal.dmp_on) { + if (0 == dmp_set_fifo_rate(50)) {consoleLog("DMP 50 Hz\r\n");} + } else + if (0 == mpu_set_sample_rate(50)) {consoleLog("MPU 50 Hz\r\n");} + break; + case '3': + if (hal.dmp_on) { + if (0 == dmp_set_fifo_rate(100)) {consoleLog("DMP 100 Hz\r\n");} + } else + if (0 == mpu_set_sample_rate(100)) {consoleLog("MPU 100 Hz\r\n");} + break; + + /* Set hardware to interrupt on gesture event only. This feature is + * useful for keeping the MCU asleep until the DMP detects as a tap or + * orientation event. + */ + case ',': + dmp_set_interrupt_mode(DMP_INT_GESTURE); + break; + case '.': + /* Set hardware to interrupt periodically. */ + dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); + break; + + /* Toggle DMP. */ + case 'f': + if (hal.lp_accel_mode) /* LP accel is not compatible with the DMP. */ + return; + if (hal.dmp_on) { + unsigned short dmp_rate; + unsigned char mask = 0; + hal.dmp_on = 0; + mpu_set_dmp_state(0); + /* Restore FIFO settings. */ + if (hal.sensors & ACCEL_ON) + mask |= INV_XYZ_ACCEL; + if (hal.sensors & GYRO_ON) + mask |= INV_XYZ_GYRO; + if (hal.sensors & COMPASS_ON) + mask |= INV_XYZ_COMPASS; + mpu_configure_fifo(mask); + /* When the DMP is used, the hardware sampling rate is fixed at + * 200Hz, and the DMP is configured to downsample the FIFO output + * using the function dmp_set_fifo_rate. However, when the DMP is + * turned off, the sampling rate remains at 200Hz. This could be + * handled in mpu6050.c, but it would need to know that + * mpu6050_dmp.c exists. To avoid this, we'll just + * put the extra logic in the application layer. + */ + dmp_get_fifo_rate(&dmp_rate); + mpu_set_sample_rate(dmp_rate); + consoleLog("DMP OFF\r\n"); + } else { + unsigned short sample_rate; + hal.dmp_on = 1; + /* Preserve current FIFO rate. */ + mpu_get_sample_rate(&sample_rate); + dmp_set_fifo_rate(sample_rate); + mpu_set_dmp_state(1); + consoleLog("DMP ON\r\n"); + } + break; + + case 'v': + /* Toggle LP quaternion. + * The DMP features can be enabled/disabled at runtime. Use this same + * approach for other features. + */ + hal.dmp_features ^= DMP_FEATURE_6X_LP_QUAT; + dmp_enable_feature(hal.dmp_features); + if (!(hal.dmp_features & DMP_FEATURE_6X_LP_QUAT)) { + consoleLog("Quat OFF\n"); + } else + consoleLog("Quat ON\n"); + break; + + /* Test out low-power accel mode. */ + case 'w': + if (hal.dmp_on) { + consoleLog("Low-power mode needs DMP to be off!\r\n"); + break; /* LP accel is not compatible with the DMP. */ + } + mpu_lp_accel_mode(20); + /* When LP accel mode is enabled, the driver automatically configures + * the hardware for latched interrupts. However, the MCU sometimes + * misses the rising/falling edge, and the hal.new_gyro flag is never + * set. To avoid getting locked in this state, we're overriding the + * driver's configuration and sticking to unlatched interrupt mode. + * + * TODO: The MCU supports level-triggered interrupts. + */ + mpu_set_int_latched(0); + hal.sensors &= ~(GYRO_ON|COMPASS_ON); + hal.sensors |= ACCEL_ON; + hal.lp_accel_mode = 1; + break; + + /* The hardware self test is completely localized in the gyro driver. + * Logging is assumed to be enabled; otherwise, a couple LEDs could + * probably be used here to display the test results. + */ + case 's': + mpu_start_self_test(); + break; + #endif // MPU_SENSOR_ENABLE + + default: + break; } #endif // SERIAL_DEBUG } -void mpu_print_to_console(void) -{ - #ifdef SERIAL_DEBUG - if (hal.report & PRINT_ACCEL) { - log_i( "Accel[XYZ]: \t %d \t %d \t %d \n", mpu.accel.x, mpu.accel.y, mpu.accel.z); - } - if (hal.report & PRINT_GYRO) { - log_i( "Gyro[XYZ]: \t %d \t %d \t %d \n", mpu.gyro.x, mpu.gyro.y, mpu.gyro.z); - } - if (hal.report & PRINT_QUAT) { - log_i( "Quat[WXYZ]: \t %ld \t %ld \t %ld \t %ld \n", (long)mpu.quat.w, (long)mpu.quat.x, (long)mpu.quat.y, (long)mpu.quat.z); - } - if (hal.report & PRINT_EULER) { - log_i( "Euler[RPY]: \t %d \t %d \t %d \n", mpu.euler.roll, mpu.euler.pitch, mpu.euler.yaw); - } - if (hal.report & PRINT_TEMP) { - log_i( "Temperature: %d \n", mpu.temp); - } - if (hal.report & PRINT_PEDO) { - unsigned long timestamp; - get_tick_count_ms(×tamp); - if (timestamp > hal.next_pedo_ms) { - hal.next_pedo_ms = timestamp + PEDO_READ_MS; - unsigned long step_count, walk_time; - dmp_get_pedometer_step_count(&step_count); - dmp_get_pedometer_walk_time(&walk_time); - log_i( "Walked %ld steps in %ld seconds..\n", step_count, walk_time/1000); - } - } - #endif -} - -#endif // MPU_SENSOR_ENABLE diff --git a/Src/setup.c b/Src/setup.c index f142524..f758c85 100644 --- a/Src/setup.c +++ b/Src/setup.c @@ -29,323 +29,326 @@ // Private variables -static rcu_periph_enum USART_CLK[USARTn] = { USART0_CLK, - USART1_CLK - }; +static rcu_periph_enum USART_CLK[USARTn] = { USART0_CLK, + USART1_CLK + }; -static uint32_t USART_TX_PIN[USARTn] = { USART0_TX_PIN, - USART1_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 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 - }; + USART1_TX_DMA_CH + }; static dma_channel_enum USART_RX_DMA_CH[USARTn] = { USART0_RX_DMA_CH, - USART1_RX_DMA_CH - }; + USART1_RX_DMA_CH + }; -static uint32_t USART_TDATA_ADDRESS[USARTn] = { USART0_TDATA_ADDRESS, - USART1_TDATA_ADDRESS - }; +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_RDATA_ADDRESS[USARTn] = { USART0_RDATA_ADDRESS, + USART1_RDATA_ADDRESS + }; 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) { - - uint8_t USART_ID = 0U; - if(selUSART == USART0){ - USART_ID = 0U; - } - if(selUSART == USART1){ - USART_ID = 1U; - } - /* enable GPIO clock */ - rcu_periph_clock_enable(USART_GPIO_CLK); + uint8_t USART_ID = 0U; + if(selUSART == USART0){ + USART_ID = 0U; + } + if(selUSART == USART1){ + USART_ID = 1U; + } - /* enable USART clock */ - rcu_periph_clock_enable(USART_CLK[USART_ID]); + /* enable GPIO clock */ + rcu_periph_clock_enable(USART_GPIO_CLK); - /* connect port to USARTx_Tx */ - gpio_af_set(USART_GPIO_PORT, USART_AF, USART_TX_PIN[USART_ID]); + /* enable USART clock */ + rcu_periph_clock_enable(USART_CLK[USART_ID]); - /* connect port to USARTx_Rx */ - gpio_af_set(USART_GPIO_PORT, USART_AF, USART_RX_PIN[USART_ID]); + /* connect port to USARTx_Tx */ + gpio_af_set(USART_GPIO_PORT, USART_AF, USART_TX_PIN[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_Rx */ + gpio_af_set(USART_GPIO_PORT, USART_AF, USART_RX_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]); + /* 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]); - /* 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 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 --------------------------- - uint8_t USART_ID = 0U; - if(selUSART == USART0){ - USART_ID = 0U; - } - if(selUSART == USART1){ - USART_ID = 1U; - } + dma_parameter_struct dma_init_struct; - /* 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); + // --------------------------- 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(USART_TX_DMA_CH[USART_ID]); - - /* wait DMA channel transfer complete */ - // while (RESET == dma_flag_get(USART_TX_DMA[USART_ID], 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 --------------------------- - uint8_t USART_ID = 0U; - if(selUSART == USART0){ - USART_ID = 0U; - } - if(selUSART == USART1){ - USART_ID = 1U; - } +void usart_Rx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t dSize) { - /* enable DMA clock */ - rcu_periph_clock_enable(RCU_DMA); + dma_parameter_struct dma_init_struct; - /* 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]); + // --------------------------- RX Channel --------------------------- - /* USART DMA enable for reception */ - usart_dma_receive_config(selUSART, USART_DENR_ENABLE); + uint8_t USART_ID = 0U; + if(selUSART == USART0){ + USART_ID = 0U; + } + if(selUSART == USART1){ + USART_ID = 1U; + } - /* enable DMA channel */ - dma_channel_enable(USART_RX_DMA_CH[USART_ID]); + /* 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(USART_RX_DMA[USART_ID], 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, 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 } diff --git a/Src/systick.c b/Src/systick.c index 5ea1821..09c977b 100644 --- a/Src/systick.c +++ b/Src/systick.c @@ -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) diff --git a/Src/util.c b/Src/util.c index 3fca171..594f34c 100644 --- a/Src/util.c +++ b/Src/util.c @@ -28,65 +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 rx1_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular 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 UART1 Rx Serial -uint8_t timeoutFlagSerial = 0; // Timeout Flag for UART1 Rx Serial: 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 -SerialAuxTx AuxTx; +static SerialAuxTx AuxTx; #endif #ifdef SERIAL_AUX_RX -static uint8_t rx0_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer +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 -SerialCommand command; -SerialCommand command_raw; -uint16_t timeoutCntSerial0 = 0; // Timeout counter for UART0 Rx Serial -uint8_t timeoutFlagSerial0 = 0; // Timeout Flag for UART0 Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) +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; - uint16_t ibus_captured_value[IBUS_NUM_CHANNELS]; + 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; @@ -99,186 +115,331 @@ volatile int8_t i2c_aux_nRABytes; void consoleLog(char *message) { #ifdef SERIAL_DEBUG - log_i("%s", message); + log_i("%s", message); #endif } /* retarget the C library printf function to the USART */ #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 position switch + if (ch > 850) return 2; // switch in position 2 + else if (ch > 250) return 1; // switch in position 1 + else return 0; // switch in position 0 + } 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 *)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 + #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 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 - } +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 } - 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; + // Print MPU data to Console + #ifdef SERIAL_DEBUG + if (main_loop_counter % 50 == 0) { + mpu_print_to_console(); } - #endif // SERIAL_AUX_RX + #endif +#endif } +/* + * 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); + } + #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 + + // 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; + #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 - } - } - } - 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_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 + 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 == rx1_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 } /* @@ -287,13 +448,11 @@ void usart1_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 @@ -305,19 +464,55 @@ 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 @@ -341,39 +536,54 @@ void usart_process_command(SerialCommand *command_in, SerialCommand *command_out } #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; + + // 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)); + + // 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 i2c_status; - // Wait until all data bytes are sent/received - while(i2c_nDABytes > 0); - - return i2c_status; - } @@ -382,7 +592,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); } @@ -405,37 +615,37 @@ 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; + + // 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; + } @@ -444,7 +654,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); } @@ -465,33 +675,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; - } /* @@ -499,37 +709,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 diff --git a/platformio.ini b/platformio.ini index a9aed61..a05475e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,7 +14,7 @@ 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 ;================================================================