Merge pull request #260 from sdegeorgio/master

Reworked the Wii nunchuck driver to make it much more robust.

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);
+nunchuk_state Nunchuk_Read(void);
-void Nunchuk_Read(void);
-uint8_t Nunchuk_Ping(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
 

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) {
+uint8_t Nunchuk_tx(uint8_t i2cBuffer[], uint8_t i2cBufferLength) {
-  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 1, 10) == HAL_OK) {
+  if(HAL_I2C_Master_Transmit(&hi2c2,NUNCHUK_I2C_ADDRESS,(uint8_t*)i2cBuffer, i2cBufferLength, 100) == HAL_OK) {
-    return 1;
+    return true;
   }
-  return 0;
+  return false;
 }
 
-void Nunchuk_Init(void) {
+uint8_t Nunchuk_rx(uint8_t i2cBuffer[], uint8_t i2cBufferLength) {
-    //-- START -- init WiiNunchuk
+  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) {
+uint8_t Nunchuk_Connect() {
-  i2cBuffer[0] = 0x00;
+  /* Initialise / re-initialise I2C peripheral */
-  HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 10);
+  I2C_Init();
-  HAL_Delay(3);
+  
-  if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuk_data, 6, 10) == HAL_OK) {
+  /* 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;
   }
-
+  return nunchukState;
-  #ifndef TRANSPOTTER
-    if (timeoutCntGen > 3) {
-      HAL_Delay(50);
-      Nunchuk_Init();
-    }
-  #endif
-
   //setScopeChannel(0, (int)nunchuk_data[0]);
   //setScopeChannel(1, (int)nunchuk_data[1]);
   //setScopeChannel(2, (int)nunchuk_data[5] & 1);

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()) {
+              if(Nunchuk_Read() == NUNCHUK_CONNECTED) {
-              HAL_Delay(500);
+                #ifdef SUPPORT_LCD
-              Nunchuk_Init();
+                  LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control");
-              #ifdef SUPPORT_LCD
+                #endif
-                LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control");
+                nunchuk_connected = 1;
-              #endif
+	      }
-              timeoutCntGen = 0;
+	    } else {
-              timeoutFlgGen = 0;
+              nunchuk_connected = 0;
-              HAL_Delay(1000);
+	    }
-              nunchuk_connected = 1;
-            }
           }
         }   
       #endif

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();
+/*  hdma_i2c2_rx.Instance = DMA1_Channel5;
-    __HAL_RCC_GPIOB_CLK_ENABLE();
+  hdma_i2c2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
-  /* USER CODE BEGIN I2C2_MspInit 0 */
+  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 */
+  __HAL_LINKDMA(&hi2c2,hdmarx,hdma_i2c2_rx);
-
+*/
-    /**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 */
 
+/*  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) {

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
+      if (((input1[inIdx].cmd > 50 || input2[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
+          || (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) {
+    if (Nunchuk_Read() == NUNCHUK_CONNECTED) {
-      Nunchuk_Read();
       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