Merge branch 'feature/index_pin' of git://github.com/Wetmelon/ODrive into Wetmelon-feature/index_pin

Firmware/Inc/gpio.h

@@ -72,6 +72,7 @@ void MX_GPIO_Init(void);
 
 void SetGPIO12toUART();
 void SetGPIO12toStepDir();
+void SetupENCIndexGPIO();
 
 /* USER CODE END Prototypes */
 

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 true
 #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,
@@ -347,6 +350,7 @@ void init_motor_control() {
     // Start Encoders
     HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
     HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
+    SetupENCIndexGPIO();
 
     // Wait for current sense calibration to converge
     // TODO make timing a function of calibration filter tau
@@ -527,6 +531,19 @@ void step_cb(uint16_t GPIO_Pin) {
     }
 }
 
+// Triggered when an encoder passes over the "Index" pin
+void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) {
+    if (!motors[motor_index].encoder.index_found) {
+        setEncoderCount(&motors[motor_index], 0);
+        motors[motor_index].encoder.index_found = true;
+    }
+    if(GPIO_Pin == M0_ENC_Z_Pin){
+        HAL_NVIC_DisableIRQ(EXTI3_IRQn);
+    } else {
+        HAL_NVIC_DisableIRQ(EXTI15_10_IRQn);
+    }
+}
+
 void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
     static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12);
     // Only one conversion in sequence, so only rank1

Firmware/MotorControl/low_level.h

@@ -103,6 +103,7 @@ typedef struct {
 
 typedef struct {
     TIM_HandleTypeDef* encoder_timer;
+    bool index_found;
     int encoder_cpr;
     int32_t encoder_offset;
     int32_t encoder_state;
@@ -193,6 +194,7 @@ void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_for
 void set_current_setpoint(Motor_t* motor, float current_setpoint);
 
 void step_cb(uint16_t GPIO_Pin);
+void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index);
 void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
 void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
 

Firmware/Src/gpio.c

@@ -195,11 +195,38 @@ void SetGPIO12toStepDir() {
   HAL_NVIC_EnableIRQ(EXTI0_IRQn);
 }
 
+void SetupENCIndexGPIO(){
+  GPIO_InitTypeDef GPIO_InitStruct;
+
+  /*Configure GPIO pins : PAPin PAPin */
+  GPIO_InitStruct.Pin = M0_ENC_Z_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
+
+  /*Configure GPIO pins : PBPin PBPin */
+  GPIO_InitStruct.Pin = M1_ENC_Z_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(EXTI3_IRQn);
+}
+
+
 //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);
   }
 }
 

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 line3 interrupt.
+*/
+void EXTI3_IRQHandler(void)
+{
+  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3);
+}
+
 /**
 * @brief This function handles EXTI line4 interrupt.
 */
@@ -321,5 +329,15 @@ void EXTI4_IRQHandler(void)
   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);
 }
 
+/**
+* @brief This function handles EXTI lines 10-15 interrupt.
+*/
+void EXTI15_10_IRQHandler(void)
+{
+  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
+}
+
+
+
 /* USER CODE END 1 */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/