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

iBUS on AUX Serial is working

Browse Source 
- implemented iBUS
- updated the Tabs to spaces
This commit is contained in:
EmanuelFeru
2020-12-06 15:21:39 +01:00
parent 6420960287
commit 570e11574e
16 changed files with 2566 additions and 1693 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
Src/gd32f1x0_it.c
View File

@@ -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

428
Src/i2c_it.c
View File

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

168
Src/main.c
View File

@@ -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++;

1290
Src/mpu6050.c
View File

File diff suppressed because it is too large Load Diff

523
Src/setup.c
View File

@@ -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
}

4
Src/systick.c
View File

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

790
Src/util.c
View File

@@ -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