diff --git a/Inc/defines.h b/Inc/defines.h index a9b28e3..ecc10dc 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -229,11 +229,16 @@ typedef struct { uint16_t l_rx2; } adc_buf_t; +typedef enum { + NUNCHUK_CONNECTING, + NUNCHUK_DISCONNECTED, + NUNCHUK_RECONNECTING, + NUNCHUK_CONNECTED +} nunchuk_state; + // Define I2C, Nunchuk, PPM, PWM functions void I2C_Init(void); -void Nunchuk_Init(void); -void Nunchuk_Read(void); -uint8_t Nunchuk_Ping(void); +nunchuk_state Nunchuk_Read(void); void PPM_Init(void); void PPM_ISR_Callback(void); void PWM_Init(void); @@ -256,6 +261,5 @@ void PWM_ISR_CH2_Callback(void); #define SWC_SET (0x1800) // 0001 1000 0000 0000 #define SWD_SET (0x2000) // 0010 0000 0000 0000 - #endif // DEFINES_H diff --git a/Src/control.c b/Src/control.c index 81dc179..dd73094 100644 --- a/Src/control.c +++ b/Src/control.c @@ -6,6 +6,8 @@ #include "setup.h" #include "config.h" +#define NUNCHUK_I2C_ADDRESS 0xA4 + TIM_HandleTypeDef TimHandle; TIM_HandleTypeDef TimHandle2; uint8_t ppm_count = 0; @@ -15,6 +17,7 @@ uint8_t timeoutFlgGen = 0; uint8_t nunchuk_data[6] = {0}; uint8_t i2cBuffer[2]; +nunchuk_state nunchukState = NUNCHUK_CONNECTING; extern I2C_HandleTypeDef hi2c2; extern DMA_HandleTypeDef hdma_i2c2_rx; @@ -214,44 +217,127 @@ void PWM_Init(void) { } #endif -uint8_t Nunchuk_Ping(void) { - if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 1, 10) == HAL_OK) { - return 1; +uint8_t Nunchuk_tx(uint8_t i2cBuffer[], uint8_t i2cBufferLength) { + if(HAL_I2C_Master_Transmit(&hi2c2,NUNCHUK_I2C_ADDRESS,(uint8_t*)i2cBuffer, i2cBufferLength, 100) == HAL_OK) { + return true; } - return 0; + return false; } -void Nunchuk_Init(void) { - //-- START -- init WiiNunchuk +uint8_t Nunchuk_rx(uint8_t i2cBuffer[], uint8_t i2cBufferLength) { + if(HAL_I2C_Master_Receive(&hi2c2,NUNCHUK_I2C_ADDRESS,(uint8_t*)i2cBuffer, i2cBufferLength, 100) == HAL_OK) { + return true; + } + return false; +} + +uint8_t Nunchuk_Init(void) { + //-- START -- init WiiNunchuk i2cBuffer[0] = 0xF0; i2cBuffer[1] = 0x55; - HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 2, 100); + if(Nunchuk_tx(i2cBuffer, 2) == false) { + return false; + } HAL_Delay(10); i2cBuffer[0] = 0xFB; i2cBuffer[1] = 0x00; - HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 2, 100); + if(Nunchuk_tx(i2cBuffer, 2) == false) { + return false; + } HAL_Delay(10); + + return true; } -void Nunchuk_Read(void) { - i2cBuffer[0] = 0x00; - HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10); - HAL_Delay(3); - if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 6, 10) == HAL_OK) { +uint8_t Nunchuk_Connect() { + /* Initialise / re-initialise I2C peripheral */ + I2C_Init(); + + /* Initialise / re-initialise nunchuk */ + if(Nunchuk_Init() == true) { + nunchukState = NUNCHUK_CONNECTED; + return true; + } else { + return false; + } +} + +nunchuk_state Nunchuk_Read(void) { + static uint8_t delay_counter = 0; + uint16_t checksum = 0; + uint8_t success = true; + uint8_t i = 0; + + switch(nunchukState) { + case NUNCHUK_DISCONNECTED: + success = false; + /* Delay a bit before reconnecting */ + if(delay_counter++ > 100) { + success = Nunchuk_Connect(); + delay_counter = 0; + } + break; + + case NUNCHUK_CONNECTING: + case NUNCHUK_RECONNECTING: + /* Try to reconnect once, if fails again fall back to disconnected state */ + success = Nunchuk_Connect(); + if(!success) { + nunchukState = NUNCHUK_DISCONNECTED; + } + break; + + case NUNCHUK_CONNECTED: + /* Send read address of 0x00 to the Nunchuk */ + i2cBuffer[0] = 0x00; + if(!Nunchuk_tx(i2cBuffer, 1)) { + success = false; + } + HAL_Delay(3); + + /* Clear the receive data buffer */ + for(i = 0; i<6; i++) { + nunchuk_data[i] = 0; + } + + /* Read back 6 bytes from the Nunchuk */ + if(!Nunchuk_rx(nunchuk_data, 6)) { + success = false; + } + HAL_Delay(3); + + /* Checksum the receive buffer to ensure it is not in an error condition, i.e. all 0x00 or 0xFF */ + for(i = 0; i<6; i++) { + checksum += nunchuk_data[i]; + } + if(checksum == 0 || checksum == 0x5FA) { + success = false; + } + + /* Comms failure or timeout counter reached timeout limit */ + if(success == false || timeoutCntGen > 3) { + /* Clear the receive data buffer */ + for(i = 0; i<6; i++) { + nunchuk_data[i] = 0; + } + /* Brings motors to safe stop */ + /* Expected values from nunchuk for stopped (mid) position */ + nunchuk_data[0] = 127; + nunchuk_data[1] = 128; + timeoutFlgGen = 1; + nunchukState = NUNCHUK_RECONNECTING; + } + break; + } + /* Reset the timeout flag and counter if successful communication */ + if(success == true) { timeoutCntGen = 0; timeoutFlgGen = 0; } - - #ifndef TRANSPOTTER - if (timeoutCntGen > 3) { - HAL_Delay(50); - Nunchuk_Init(); - } - #endif - + return nunchukState; //setScopeChannel(0, (int)nunchuk_data[0]); //setScopeChannel(1, (int)nunchuk_data[1]); //setScopeChannel(2, (int)nunchuk_data[5] & 1); diff --git a/Src/main.c b/Src/main.c index eda4056..2f665da 100644 --- a/Src/main.c +++ b/Src/main.c @@ -138,7 +138,7 @@ static uint8_t sideboard_leds_R; #endif #ifdef VARIANT_TRANSPOTTER - extern uint8_t nunchuk_connected; + uint8_t nunchuk_connected; extern float setDistance; static uint8_t checkRemote = 0; @@ -376,17 +376,15 @@ int main(void) { #ifdef SUPPORT_NUNCHUK if (transpotter_counter % 500 == 0) { if (nunchuk_connected == 0 && enable == 0) { - if (Nunchuk_Ping()) { - HAL_Delay(500); - Nunchuk_Init(); - #ifdef SUPPORT_LCD - LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control"); - #endif - timeoutCntGen = 0; - timeoutFlgGen = 0; - HAL_Delay(1000); - nunchuk_connected = 1; - } + if(Nunchuk_Read() == NUNCHUK_CONNECTED) { + #ifdef SUPPORT_LCD + LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control"); + #endif + nunchuk_connected = 1; + } + } else { + nunchuk_connected = 0; + } } } #endif diff --git a/Src/setup.c b/Src/setup.c index 66ee441..3a8829f 100644 --- a/Src/setup.c +++ b/Src/setup.c @@ -283,17 +283,21 @@ DMA_HandleTypeDef hdma_i2c2_tx; void I2C_Init(void) { + /* Initialise I2C2 GPIO pins + * I2C2 GPIO Configuration + * PB10 ------> I2C2_SCL + * PB11 ------> I2C2_SDA + */ + GPIO_InitTypeDef GPIO_InitStruct; + __HAL_RCC_GPIOB_CLK_ENABLE(); + GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11; + GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + /* Initialise I2C peripheral */ __HAL_RCC_I2C2_CLK_ENABLE(); - __HAL_RCC_DMA1_CLK_ENABLE(); - - /* DMA1_Channel4_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 1, 4); - HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn); - /* DMA1_Channel5_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 1, 3); - HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn); - hi2c2.Instance = I2C2; hi2c2.Init.ClockSpeed = 200000; hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2; @@ -303,65 +307,53 @@ void I2C_Init(void) hi2c2.Init.OwnAddress2 = 0; hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + __HAL_RCC_I2C2_FORCE_RESET(); + __HAL_RCC_I2C2_RELEASE_RESET(); HAL_I2C_Init(&hi2c2); - GPIO_InitTypeDef GPIO_InitStruct; + /* Peripheral DMA init*/ +/* __HAL_RCC_DMA1_CLK_ENABLE(); +*/ + /* DMA1_Channel4_IRQn interrupt configuration */ +/* HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 1, 4); + HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn); +*/ + /* DMA1_Channel5_IRQn interrupt configuration */ +/* HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 1, 3); + HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn); +*/ - __HAL_RCC_DMA1_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - /* USER CODE BEGIN I2C2_MspInit 0 */ +/* hdma_i2c2_rx.Instance = DMA1_Channel5; + hdma_i2c2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_i2c2_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_i2c2_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_i2c2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_i2c2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_i2c2_rx.Init.Mode = DMA_NORMAL; + hdma_i2c2_rx.Init.Priority = DMA_PRIORITY_MEDIUM; + HAL_DMA_Init(&hdma_i2c2_rx); - /* USER CODE END I2C2_MspInit 0 */ - - /**I2C2 GPIO Configuration - PB10 ------> I2C2_SCL - PB11 ------> I2C2_SDA - */ - GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11; - GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; - GPIO_InitStruct.Pull = GPIO_PULLUP; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - - /* Peripheral clock enable */ - __HAL_RCC_I2C2_CLK_ENABLE(); - - /* Peripheral DMA init*/ - - hdma_i2c2_rx.Instance = DMA1_Channel5; - hdma_i2c2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; - hdma_i2c2_rx.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_i2c2_rx.Init.MemInc = DMA_MINC_ENABLE; - hdma_i2c2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_i2c2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - hdma_i2c2_rx.Init.Mode = DMA_NORMAL; - hdma_i2c2_rx.Init.Priority = DMA_PRIORITY_MEDIUM; - HAL_DMA_Init(&hdma_i2c2_rx); - - __HAL_LINKDMA(&hi2c2,hdmarx,hdma_i2c2_rx); - - hdma_i2c2_tx.Instance = DMA1_Channel4; - hdma_i2c2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; - hdma_i2c2_tx.Init.PeriphInc = DMA_PINC_DISABLE; - hdma_i2c2_tx.Init.MemInc = DMA_MINC_ENABLE; - hdma_i2c2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; - hdma_i2c2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; - hdma_i2c2_tx.Init.Mode = DMA_NORMAL; - hdma_i2c2_tx.Init.Priority = DMA_PRIORITY_MEDIUM; - HAL_DMA_Init(&hdma_i2c2_tx); - - __HAL_LINKDMA(&hi2c2,hdmatx,hdma_i2c2_tx); - - /* Peripheral interrupt init */ - HAL_NVIC_SetPriority(I2C2_EV_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(I2C2_EV_IRQn); - HAL_NVIC_SetPriority(I2C2_ER_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(I2C2_ER_IRQn); - /* USER CODE BEGIN I2C2_MspInit 1 */ - - /* USER CODE END I2C2_MspInit 1 */ + __HAL_LINKDMA(&hi2c2,hdmarx,hdma_i2c2_rx); +*/ +/* hdma_i2c2_tx.Instance = DMA1_Channel4; + hdma_i2c2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; + hdma_i2c2_tx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_i2c2_tx.Init.MemInc = DMA_MINC_ENABLE; + hdma_i2c2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_i2c2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_i2c2_tx.Init.Mode = DMA_NORMAL; + hdma_i2c2_tx.Init.Priority = DMA_PRIORITY_MEDIUM; + HAL_DMA_Init(&hdma_i2c2_tx); + __HAL_LINKDMA(&hi2c2,hdmatx,hdma_i2c2_tx); +*/ + /* Peripheral interrupt init */ +/* HAL_NVIC_SetPriority(I2C2_EV_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(I2C2_EV_IRQn); + HAL_NVIC_SetPriority(I2C2_ER_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(I2C2_ER_IRQn); +*/ } void MX_GPIO_Init(void) { diff --git a/Src/util.c b/Src/util.c index 67002a9..7403cd4 100644 --- a/Src/util.c +++ b/Src/util.c @@ -111,12 +111,6 @@ uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request LCD_PCF8574_HandleTypeDef lcd; #endif -#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) -uint8_t nunchuk_connected = 1; -#else -uint8_t nunchuk_connected = 0; -#endif - #ifdef VARIANT_TRANSPOTTER float setDistance; uint16_t VirtAddVarTab[NB_OF_VAR] = {1337}; // Virtual address defined by the user: 0xFFFF value is prohibited @@ -288,11 +282,6 @@ void Input_Init(void) { PWM_Init(); #endif - #ifdef CONTROL_NUNCHUK - I2C_Init(); - Nunchuk_Init(); - #endif - #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) UART2_Init(); #endif @@ -643,8 +632,8 @@ void updateCurSpdLim(void) { void standstillHold(void) { #if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE) if (!rtP_Left.b_cruiseCtrlEna) { // If Stanstill in NOT Active -> try Activation - if ((input1[inIdx].cmd > 50 && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small - || (abs(input2[inIdx].cmd) < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small + if (((input1[inIdx].cmd > 50 || input2[inIdx].cmd < -50) && speedAvgAbs < 30) // Check if Brake is pressed AND measured speed is small + || (input2[inIdx].cmd < 20 && speedAvgAbs < 5)) { // OR Throttle is small AND measured speed is very small rtP_Left.n_cruiseMotTgt = 0; rtP_Right.n_cruiseMotTgt = 0; rtP_Left.b_cruiseCtrlEna = 1; @@ -653,7 +642,7 @@ void standstillHold(void) { } } else { // If Stanstill is Active -> try Deactivation - if (input1[inIdx].cmd < 20 && abs(input2[inIdx].cmd) > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control + if (input1[inIdx].cmd < 20 && input2[inIdx].cmd > 50 && !cruiseCtrlAcv) { // Check if Brake is released AND Throttle is pressed AND no Cruise Control rtP_Left.b_cruiseCtrlEna = 0; rtP_Right.b_cruiseCtrlEna = 0; standstillAcv = 0; @@ -813,8 +802,7 @@ void readInputRaw(void) { #endif #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) - if (nunchuk_connected) { - Nunchuk_Read(); + if (Nunchuk_Read() == NUNCHUK_CONNECTED) { if (inIdx == CONTROL_NUNCHUK) { input1[inIdx].raw = (nunchuk_data[0] - 127) * 8; // X axis 0-255 input2[inIdx].raw = (nunchuk_data[1] - 128) * 8; // Y axis 0-255