Merge pull request #948 from mlyle/tantohf2

Tanto Hotfix 2 changes
d-ronin · Apr 28, 2016 · b04a62e · b04a62e
2 parents 5ad1de1 + 736621b
commit b04a62e
Show file tree

Hide file tree

Showing 6 changed files with 108 additions and 23 deletions.
diff --git a/flight/PiOS/Common/pios_mpu.c b/flight/PiOS/Common/pios_mpu.c
@@ -423,7 +423,8 @@ static int32_t PIOS_MPU_Common_Init(void)
 
 	/* Wait 20 ms for data ready interrupt and make sure it happens twice */
 	if ((PIOS_Semaphore_Take(mpu_dev->data_ready_sema, 20) != true) ||
-		(PIOS_Semaphore_Take(mpu_dev->data_ready_sema, 20) != true)) {
+			(PIOS_Semaphore_Take(mpu_dev->data_ready_sema, 20) != true)) {
+		PIOS_EXTI_DeInit(mpu_dev->cfg->exti_cfg);
 		return -PIOS_MPU_ERROR_NOIRQ;
 	}
 

diff --git a/flight/PiOS/STM32F10x/pios_exti.c b/flight/PiOS/STM32F10x/pios_exti.c
@@ -153,6 +153,32 @@ int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg)
 	return -1;
 }
 
+void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg)
+{
+	PIOS_Assert(cfg);
+	PIOS_Assert(&__start__exti);
+	PIOS_Assert(cfg >= &__start__exti);
+	PIOS_Assert(cfg < &__stop__exti);
+
+	/* Disconnect this config from the requested vector */
+	uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line);
+	pios_exti_line_to_cfg_map[line_index] = PIOS_EXTI_INVALID;
+
+	/* Disconnect EXTI */
+	uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin);
+	// sadly std-periph doesn't provide a way to disable the exti line
+	uint32_t tmp = ((uint32_t)0x0F) << (0x04 * (exti_source_pin & (uint8_t)0x03));
+	AFIO->EXTICR[exti_source_pin >> 0x02] &= ~tmp;
+
+	/* Disable EXTI */
+	EXTI_InitTypeDef init = {
+		.EXTI_LineCmd = DISABLE,
+		.EXTI_Mode = cfg->exti.init.EXTI_Mode,
+		.EXTI_Line = cfg->exti.init.EXTI_Line,
+	};
+	EXTI_Init(&init);
+}
+
 static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index)
 {
 	uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index];

diff --git a/flight/PiOS/STM32F30x/pios_exti.c b/flight/PiOS/STM32F30x/pios_exti.c
@@ -150,6 +150,32 @@ int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg)
 	return -1;
 }
 
+void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg)
+{
+	PIOS_Assert(cfg);
+	PIOS_Assert(&__start__exti);
+	PIOS_Assert(cfg >= &__start__exti);
+	PIOS_Assert(cfg < &__stop__exti);
+
+	/* Disconnect this config from the requested vector */
+	uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line);
+	pios_exti_line_to_cfg_map[line_index] = PIOS_EXTI_INVALID;
+
+	/* Disconnect EXTI */
+	uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin);
+	// sadly std-periph doesn't provide a way to disable the exti line
+	uint32_t tmp = ((uint32_t)0x0F) << (0x04 * (exti_source_pin & (uint8_t)0x03));
+	SYSCFG->EXTICR[exti_source_pin >> 0x02] &= ~tmp;
+
+	/* Disable EXTI */
+	EXTI_InitTypeDef init = {
+		.EXTI_LineCmd = DISABLE,
+		.EXTI_Mode = cfg->exti.init.EXTI_Mode,
+		.EXTI_Line = cfg->exti.init.EXTI_Line,
+	};
+	EXTI_Init(&init);
+}
+
 static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index)
 {
 	uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index];

diff --git a/flight/PiOS/STM32F4xx/pios_exti.c b/flight/PiOS/STM32F4xx/pios_exti.c
@@ -150,6 +150,32 @@ int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg)
 	return -1;
 }
 
+void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg)
+{
+	PIOS_Assert(cfg);
+	PIOS_Assert(&__start__exti);
+	PIOS_Assert(cfg >= &__start__exti);
+	PIOS_Assert(cfg < &__stop__exti);
+
+	/* Disconnect this config from the requested vector */
+	uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line);
+	pios_exti_line_to_cfg_map[line_index] = PIOS_EXTI_INVALID;
+
+	/* Disconnect EXTI */
+	uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin);
+	// sadly std-periph doesn't provide a way to disable the exti line
+	uint32_t tmp = ((uint32_t)0x0F) << (0x04 * (exti_source_pin & (uint8_t)0x03));
+	SYSCFG->EXTICR[exti_source_pin >> 0x02] &= ~tmp;
+
+	/* Disable EXTI */
+	EXTI_InitTypeDef init = {
+		.EXTI_LineCmd = DISABLE,
+		.EXTI_Mode = cfg->exti.init.EXTI_Mode,
+		.EXTI_Line = cfg->exti.init.EXTI_Line,
+	};
+	EXTI_Init(&init);
+}
+
 static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index)
 {
 	uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index];

diff --git a/flight/PiOS/inc/pios_exti.h b/flight/PiOS/inc/pios_exti.h
@@ -46,7 +46,8 @@ struct pios_exti_cfg {
 /* must be added to any pios_exti_cfg definition for it to be valid */
 #define __exti_config	__attribute__((section("_exti")))
 
-extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg);
+extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg);
+extern void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg);
 
 #endif /* PIOS_EXTI_H */
 

diff --git a/flight/targets/naze32/fw/pios_board.c b/flight/targets/naze32/fw/pios_board.c
@@ -209,27 +209,6 @@ void PIOS_Board_Init(void) {
 	uint8_t hw_rcvrport;
 	HwNazeRcvrPortGet(&hw_rcvrport);
 
-#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
-#ifdef PIOS_INCLUDE_SERVO
-	switch (hw_rcvrport) {
-		case HWNAZE_RCVRPORT_DISABLED:
-		case HWNAZE_RCVRPORT_PWM:
-		case HWNAZE_RCVRPORT_PPM:
-		case HWNAZE_RCVRPORT_PPMPWM:
-		case HWNAZE_RCVRPORT_PPMSERIAL:
-		case HWNAZE_RCVRPORT_SERIAL:
-			PIOS_Servo_Init(&pios_servo_cfg);
-			break;
-		case HWNAZE_RCVRPORT_PPMOUTPUTS:
-		case HWNAZE_RCVRPORT_OUTPUTS:
-			PIOS_Servo_Init(&pios_servo_rcvr_cfg);
-			break;
-	}
-#endif
-#else
-	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
-#endif
-
 #if defined(PIOS_INCLUDE_ADC)
 	{
 		uint16_t number_of_adc_pins = 2; // first two pins are always available
@@ -264,6 +243,11 @@ void PIOS_Board_Init(void) {
 
 	pios_mpu_dev_t mpu_dev = NULL;
 	int retval = PIOS_MPU_I2C_Init(&mpu_dev, pios_i2c_internal_id, &pios_mpu_cfg);
+	if (retval == -PIOS_MPU_ERROR_NOIRQ && mpu_pin == HWNAZE_MPU6050INTPIN_AUTO) {
+		// retry with alternate exti config, needed on afromini
+		pios_mpu_cfg.exti_cfg = (pios_mpu_cfg.exti_cfg == &pios_exti_mpu_cfg_v5) ? &pios_exti_mpu_cfg : &pios_exti_mpu_cfg_v5;
+		retval = PIOS_MPU_I2C_Init(&mpu_dev, pios_i2c_internal_id, &pios_mpu_cfg);
+	}
 	if (retval != 0)
 		PIOS_HAL_Panic(PIOS_LED_ALARM, PIOS_HAL_PANIC_IMU);
 
@@ -439,6 +423,27 @@ void PIOS_Board_Init(void) {
 		break;
 	}
 
+#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
+#ifdef PIOS_INCLUDE_SERVO
+	switch (hw_rcvrport) {
+		case HWNAZE_RCVRPORT_DISABLED:
+		case HWNAZE_RCVRPORT_PWM:
+		case HWNAZE_RCVRPORT_PPM:
+		case HWNAZE_RCVRPORT_PPMPWM:
+		case HWNAZE_RCVRPORT_PPMSERIAL:
+		case HWNAZE_RCVRPORT_SERIAL:
+			PIOS_Servo_Init(&pios_servo_cfg);
+			break;
+		case HWNAZE_RCVRPORT_PPMOUTPUTS:
+		case HWNAZE_RCVRPORT_OUTPUTS:
+			PIOS_Servo_Init(&pios_servo_rcvr_cfg);
+			break;
+	}
+#endif
+#else
+	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
+#endif
+
 #if defined(PIOS_INCLUDE_MS5611)
 	if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_internal_id) != 0)
 		PIOS_HAL_Panic(PIOS_LED_ALARM, PIOS_HAL_PANIC_BARO);