Skip to content

Commit b04a62e

Browse files
mlylemlyle
committed
Merge pull request #948 from mlyle/tantohf2
Tanto Hotfix 2 changes
2 parents 5ad1de1 + 736621b commit b04a62e

File tree

6 files changed

+108
-23
lines changed

6 files changed

+108
-23
lines changed

‎flight/PiOS/Common/pios_mpu.c

+2-1
Original file line numberDiff line numberDiff line change
@@ -423,7 +423,8 @@ static int32_t PIOS_MPU_Common_Init(void)
423423

424424
/* Wait 20 ms for data ready interrupt and make sure it happens twice */
425425
if ((PIOS_Semaphore_Take(mpu_dev->data_ready_sema, 20) != true) ||
426-
(PIOS_Semaphore_Take(mpu_dev->data_ready_sema, 20) != true)) {
426+
(PIOS_Semaphore_Take(mpu_dev->data_ready_sema, 20) != true)) {
427+
PIOS_EXTI_DeInit(mpu_dev->cfg->exti_cfg);
427428
return -PIOS_MPU_ERROR_NOIRQ;
428429
}
429430

‎flight/PiOS/STM32F10x/pios_exti.c

+26
Original file line numberDiff line numberDiff line change
@@ -153,6 +153,32 @@ int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg)
153153
return -1;
154154
}
155155

156+
void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg)
157+
{
158+
PIOS_Assert(cfg);
159+
PIOS_Assert(&__start__exti);
160+
PIOS_Assert(cfg >= &__start__exti);
161+
PIOS_Assert(cfg < &__stop__exti);
162+
163+
/* Disconnect this config from the requested vector */
164+
uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line);
165+
pios_exti_line_to_cfg_map[line_index] = PIOS_EXTI_INVALID;
166+
167+
/* Disconnect EXTI */
168+
uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin);
169+
// sadly std-periph doesn't provide a way to disable the exti line
170+
uint32_t tmp = ((uint32_t)0x0F) << (0x04 * (exti_source_pin & (uint8_t)0x03));
171+
AFIO->EXTICR[exti_source_pin >> 0x02] &= ~tmp;
172+
173+
/* Disable EXTI */
174+
EXTI_InitTypeDef init = {
175+
.EXTI_LineCmd = DISABLE,
176+
.EXTI_Mode = cfg->exti.init.EXTI_Mode,
177+
.EXTI_Line = cfg->exti.init.EXTI_Line,
178+
};
179+
EXTI_Init(&init);
180+
}
181+
156182
static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index)
157183
{
158184
uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index];

‎flight/PiOS/STM32F30x/pios_exti.c

+26
Original file line numberDiff line numberDiff line change
@@ -150,6 +150,32 @@ int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg)
150150
return -1;
151151
}
152152

153+
void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg)
154+
{
155+
PIOS_Assert(cfg);
156+
PIOS_Assert(&__start__exti);
157+
PIOS_Assert(cfg >= &__start__exti);
158+
PIOS_Assert(cfg < &__stop__exti);
159+
160+
/* Disconnect this config from the requested vector */
161+
uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line);
162+
pios_exti_line_to_cfg_map[line_index] = PIOS_EXTI_INVALID;
163+
164+
/* Disconnect EXTI */
165+
uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin);
166+
// sadly std-periph doesn't provide a way to disable the exti line
167+
uint32_t tmp = ((uint32_t)0x0F) << (0x04 * (exti_source_pin & (uint8_t)0x03));
168+
SYSCFG->EXTICR[exti_source_pin >> 0x02] &= ~tmp;
169+
170+
/* Disable EXTI */
171+
EXTI_InitTypeDef init = {
172+
.EXTI_LineCmd = DISABLE,
173+
.EXTI_Mode = cfg->exti.init.EXTI_Mode,
174+
.EXTI_Line = cfg->exti.init.EXTI_Line,
175+
};
176+
EXTI_Init(&init);
177+
}
178+
153179
static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index)
154180
{
155181
uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index];

‎flight/PiOS/STM32F4xx/pios_exti.c

+26
Original file line numberDiff line numberDiff line change
@@ -150,6 +150,32 @@ int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg)
150150
return -1;
151151
}
152152

153+
void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg)
154+
{
155+
PIOS_Assert(cfg);
156+
PIOS_Assert(&__start__exti);
157+
PIOS_Assert(cfg >= &__start__exti);
158+
PIOS_Assert(cfg < &__stop__exti);
159+
160+
/* Disconnect this config from the requested vector */
161+
uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line);
162+
pios_exti_line_to_cfg_map[line_index] = PIOS_EXTI_INVALID;
163+
164+
/* Disconnect EXTI */
165+
uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin);
166+
// sadly std-periph doesn't provide a way to disable the exti line
167+
uint32_t tmp = ((uint32_t)0x0F) << (0x04 * (exti_source_pin & (uint8_t)0x03));
168+
SYSCFG->EXTICR[exti_source_pin >> 0x02] &= ~tmp;
169+
170+
/* Disable EXTI */
171+
EXTI_InitTypeDef init = {
172+
.EXTI_LineCmd = DISABLE,
173+
.EXTI_Mode = cfg->exti.init.EXTI_Mode,
174+
.EXTI_Line = cfg->exti.init.EXTI_Line,
175+
};
176+
EXTI_Init(&init);
177+
}
178+
153179
static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index)
154180
{
155181
uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index];

‎flight/PiOS/inc/pios_exti.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,8 @@ struct pios_exti_cfg {
4646
/* must be added to any pios_exti_cfg definition for it to be valid */
4747
#define __exti_config __attribute__((section("_exti")))
4848

49-
extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg);
49+
extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg *cfg);
50+
extern void PIOS_EXTI_DeInit(const struct pios_exti_cfg *cfg);
5051

5152
#endif /* PIOS_EXTI_H */
5253

‎flight/targets/naze32/fw/pios_board.c

+26-21
Original file line numberDiff line numberDiff line change
@@ -209,27 +209,6 @@ void PIOS_Board_Init(void) {
209209
uint8_t hw_rcvrport;
210210
HwNazeRcvrPortGet(&hw_rcvrport);
211211

212-
#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
213-
#ifdef PIOS_INCLUDE_SERVO
214-
switch (hw_rcvrport) {
215-
case HWNAZE_RCVRPORT_DISABLED:
216-
case HWNAZE_RCVRPORT_PWM:
217-
case HWNAZE_RCVRPORT_PPM:
218-
case HWNAZE_RCVRPORT_PPMPWM:
219-
case HWNAZE_RCVRPORT_PPMSERIAL:
220-
case HWNAZE_RCVRPORT_SERIAL:
221-
PIOS_Servo_Init(&pios_servo_cfg);
222-
break;
223-
case HWNAZE_RCVRPORT_PPMOUTPUTS:
224-
case HWNAZE_RCVRPORT_OUTPUTS:
225-
PIOS_Servo_Init(&pios_servo_rcvr_cfg);
226-
break;
227-
}
228-
#endif
229-
#else
230-
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
231-
#endif
232-
233212
#if defined(PIOS_INCLUDE_ADC)
234213
{
235214
uint16_t number_of_adc_pins = 2; // first two pins are always available
@@ -264,6 +243,11 @@ void PIOS_Board_Init(void) {
264243

265244
pios_mpu_dev_t mpu_dev = NULL;
266245
int retval = PIOS_MPU_I2C_Init(&mpu_dev, pios_i2c_internal_id, &pios_mpu_cfg);
246+
if (retval == -PIOS_MPU_ERROR_NOIRQ && mpu_pin == HWNAZE_MPU6050INTPIN_AUTO) {
247+
// retry with alternate exti config, needed on afromini
248+
pios_mpu_cfg.exti_cfg = (pios_mpu_cfg.exti_cfg == &pios_exti_mpu_cfg_v5) ? &pios_exti_mpu_cfg : &pios_exti_mpu_cfg_v5;
249+
retval = PIOS_MPU_I2C_Init(&mpu_dev, pios_i2c_internal_id, &pios_mpu_cfg);
250+
}
267251
if (retval != 0)
268252
PIOS_HAL_Panic(PIOS_LED_ALARM, PIOS_HAL_PANIC_IMU);
269253

@@ -439,6 +423,27 @@ void PIOS_Board_Init(void) {
439423
break;
440424
}
441425

426+
#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
427+
#ifdef PIOS_INCLUDE_SERVO
428+
switch (hw_rcvrport) {
429+
case HWNAZE_RCVRPORT_DISABLED:
430+
case HWNAZE_RCVRPORT_PWM:
431+
case HWNAZE_RCVRPORT_PPM:
432+
case HWNAZE_RCVRPORT_PPMPWM:
433+
case HWNAZE_RCVRPORT_PPMSERIAL:
434+
case HWNAZE_RCVRPORT_SERIAL:
435+
PIOS_Servo_Init(&pios_servo_cfg);
436+
break;
437+
case HWNAZE_RCVRPORT_PPMOUTPUTS:
438+
case HWNAZE_RCVRPORT_OUTPUTS:
439+
PIOS_Servo_Init(&pios_servo_rcvr_cfg);
440+
break;
441+
}
442+
#endif
443+
#else
444+
PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
445+
#endif
446+
442447
#if defined(PIOS_INCLUDE_MS5611)
443448
if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_internal_id) != 0)
444449
PIOS_HAL_Panic(PIOS_LED_ALARM, PIOS_HAL_PANIC_BARO);

0 commit comments

Comments
 (0)