From 7dd7ce2afc8ee8a4995a29f585cad21b3bd83ffb Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Wed, 13 Dec 2017 21:44:30 -0500
Subject: [PATCH] Implement index pin handling

---
 Firmware/MotorControl/low_level.c |  9 +++++++++
 Firmware/MotorControl/low_level.h |  1 +
 Firmware/Src/gpio.c               | 19 +++++++++++++++++++
 Firmware/Src/stm32f4xx_it.c       | 16 ++++++++++++++++
 4 files changed, 45 insertions(+)

diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c
index 06e3e1ad..4a0eb7e0 100644
--- a/Firmware/MotorControl/low_level.c
+++ b/Firmware/MotorControl/low_level.c
@@ -35,6 +35,7 @@ float vbus_voltage = 12.0f;
 
 // TODO stick parameter into struct
 #define ENCODER_CPR (600 * 4)
+#define ENC_USE_INDEX_PIN false
 #define POLE_PAIRS 7
 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
 
@@ -113,6 +114,7 @@ Motor_t motors[] = {
         .rotor_mode = ROTOR_MODE_ENCODER,
         .encoder = {
             .encoder_timer = &htim3,
+            .index_found = !(ENC_USE_INDEX_PIN),
             .encoder_cpr = ENCODER_CPR,
             .encoder_offset = 0,
             .encoder_state = 0,
@@ -207,6 +209,7 @@ Motor_t motors[] = {
         .rotor_mode = ROTOR_MODE_ENCODER,
         .encoder = {
             .encoder_timer = &htim4,
+            .index_found = !(ENC_USE_INDEX_PIN),
             .encoder_cpr = ENCODER_CPR,
             .encoder_offset = 0,
             .encoder_state = 0,
@@ -527,6 +530,12 @@ void step_cb(uint16_t GPIO_Pin) {
     }
 }
 
+void enc_index_cb(uint16_t GPIO_Pin, int index){
+    setEncoderCount(&motors[index], 0);
+    motors[index].IndexFound = true;
+}
+
+
 void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
     static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12);
     // Only one conversion in sequence, so only rank1
diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h
index 342c19ec..d2a73fac 100644
--- a/Firmware/MotorControl/low_level.h
+++ b/Firmware/MotorControl/low_level.h
@@ -102,6 +102,7 @@ typedef struct {
 
 typedef struct {
     TIM_HandleTypeDef* encoder_timer;
+    bool index_found;
     int encoder_cpr;
     int32_t encoder_offset;
     int32_t encoder_state;
diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c
index e751fb61..6af27891 100644
--- a/Firmware/Src/gpio.c
+++ b/Firmware/Src/gpio.c
@@ -195,11 +195,30 @@ void SetGPIO12toStepDir() {
   HAL_NVIC_EnableIRQ(EXTI0_IRQn);
 }
 
+void SetupENCIndexGPIO(){
+  /*Configure GPIO pins : PAPin PAPin */
+  GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : PBPin PBPin */
+  GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+}
+
+
 //Dispatch processing of external interrupts based on source
 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
   //Step signals for M0 and M1
   if (GPIO_Pin & GPIO_1_Pin || GPIO_Pin & GPIO_3_Pin) {
     step_cb(GPIO_Pin);
+  } else if(GPIO_Pin & M0_ENC_Z_Pin){
+    enc_index_cb(GPIO_Pin, 0);
+  } else if(GPIO_Pin & M1_ENC_Z_Pin){
+    enc_index_cb(GPIO_Pin, 1);
   }
 }
 
diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c
index 8f6d6d42..5e97f1f4 100644
--- a/Firmware/Src/stm32f4xx_it.c
+++ b/Firmware/Src/stm32f4xx_it.c
@@ -313,6 +313,14 @@ void EXTI2_IRQHandler(void)
   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
 }
 
+/**
+* @brief This function handles EXTI line4 interrupt.
+*/
+void EXTI3_IRQHandler(void)
+{
+  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3);
+}
+
 /**
 * @brief This function handles EXTI line4 interrupt.
 */
@@ -321,5 +329,13 @@ void EXTI4_IRQHandler(void)
   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);
 }
 
+/**
+* @brief This function handles EXTI line4 interrupt.
+*/
+void EXTI15_IRQHandler(void)
+{
+  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
+}
+
 /* USER CODE END 1 */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/