Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/development' into sirinfpv-dev

Browse source
This commit is contained in:
Evgeny Sychov 2016-06-16 01:37:56 -07:00
commit 8d41a07ae7
21 changed files with 296 additions and 260 deletions
Download patch file Download diff file Expand all files Collapse all files

286
src/main/drivers/io.c
View file

@ -8,115 +8,115 @@
// io ports defs are stored in array by index now // io ports defs are stored in array by index now
struct ioPortDef_s { struct ioPortDef_s {
rccPeriphTag_t rcc; rccPeriphTag_t rcc;
}; };
#if defined(STM32F1) #if defined(STM32F1)
const struct ioPortDef_s ioPortDefs[] = { const struct ioPortDef_s ioPortDefs[] = {
{ RCC_APB2(IOPA) }, { RCC_APB2(IOPA) },
{ RCC_APB2(IOPB) }, { RCC_APB2(IOPB) },
{ RCC_APB2(IOPC) }, { RCC_APB2(IOPC) },
{ RCC_APB2(IOPD) }, { RCC_APB2(IOPD) },
{ RCC_APB2(IOPE) }, { RCC_APB2(IOPE) },
{ {
#if defined (STM32F10X_HD) || defined (STM32F10X_XL) || defined (STM32F10X_HD_VL) #if defined (STM32F10X_HD) || defined (STM32F10X_XL) || defined (STM32F10X_HD_VL)
RCC_APB2(IOPF), RCC_APB2(IOPF),
#else #else
0, 0,
#endif #endif
}, },
{ {
#if defined (STM32F10X_HD) || defined (STM32F10X_XL) || defined (STM32F10X_HD_VL) #if defined (STM32F10X_HD) || defined (STM32F10X_XL) || defined (STM32F10X_HD_VL)
RCC_APB2(IOPG), RCC_APB2(IOPG),
#else #else
0, 0,
#endif #endif
}, },
}; };
#elif defined(STM32F3) #elif defined(STM32F3)
const struct ioPortDef_s ioPortDefs[] = { const struct ioPortDef_s ioPortDefs[] = {
{ RCC_AHB(GPIOA) }, { RCC_AHB(GPIOA) },
{ RCC_AHB(GPIOB) }, { RCC_AHB(GPIOB) },
{ RCC_AHB(GPIOC) }, { RCC_AHB(GPIOC) },
{ RCC_AHB(GPIOD) }, { RCC_AHB(GPIOD) },
{ RCC_AHB(GPIOE) }, { RCC_AHB(GPIOE) },
{ RCC_AHB(GPIOF) }, { RCC_AHB(GPIOF) },
}; };
#elif defined(STM32F4) #elif defined(STM32F4)
const struct ioPortDef_s ioPortDefs[] = { const struct ioPortDef_s ioPortDefs[] = {
{ RCC_AHB1(GPIOA) }, { RCC_AHB1(GPIOA) },
{ RCC_AHB1(GPIOB) }, { RCC_AHB1(GPIOB) },
{ RCC_AHB1(GPIOC) }, { RCC_AHB1(GPIOC) },
{ RCC_AHB1(GPIOD) }, { RCC_AHB1(GPIOD) },
{ RCC_AHB1(GPIOE) }, { RCC_AHB1(GPIOE) },
{ RCC_AHB1(GPIOF) }, { RCC_AHB1(GPIOF) },
}; };
# endif # endif
ioRec_t* IO_Rec(IO_t io) ioRec_t* IO_Rec(IO_t io)
{ {
return io; return io;
} }
GPIO_TypeDef* IO_GPIO(IO_t io) GPIO_TypeDef* IO_GPIO(IO_t io)
{ {
ioRec_t *ioRec = IO_Rec(io); ioRec_t *ioRec = IO_Rec(io);
return ioRec->gpio; return ioRec->gpio;
} }
uint16_t IO_Pin(IO_t io) uint16_t IO_Pin(IO_t io)
{ {
ioRec_t *ioRec = IO_Rec(io); ioRec_t *ioRec = IO_Rec(io);
return ioRec->pin; return ioRec->pin;
} }
// port index, GPIOA == 0 // port index, GPIOA == 0
int IO_GPIOPortIdx(IO_t io) int IO_GPIOPortIdx(IO_t io)
{ {
if (!io) if (!io)
return -1; return -1;
return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10); // ports are 0x400 apart return (((size_t)IO_GPIO(io) - GPIOA_BASE) >> 10); // ports are 0x400 apart
} }
int IO_EXTI_PortSourceGPIO(IO_t io) int IO_EXTI_PortSourceGPIO(IO_t io)
{ {
return IO_GPIOPortIdx(io); return IO_GPIOPortIdx(io);
} }
int IO_GPIO_PortSource(IO_t io) int IO_GPIO_PortSource(IO_t io)
{ {
return IO_GPIOPortIdx(io); return IO_GPIOPortIdx(io);
} }
// zero based pin index // zero based pin index
int IO_GPIOPinIdx(IO_t io) int IO_GPIOPinIdx(IO_t io)
{ {
if (!io) if (!io)
return -1; return -1;
return 31 - __builtin_clz(IO_Pin(io)); // CLZ is a bit faster than FFS return 31 - __builtin_clz(IO_Pin(io)); // CLZ is a bit faster than FFS
} }
int IO_EXTI_PinSource(IO_t io) int IO_EXTI_PinSource(IO_t io)
{ {
return IO_GPIOPinIdx(io); return IO_GPIOPinIdx(io);
} }
int IO_GPIO_PinSource(IO_t io) int IO_GPIO_PinSource(IO_t io)
{ {
return IO_GPIOPinIdx(io); return IO_GPIOPinIdx(io);
} }
// mask on stm32f103, bit index on stm32f303 // mask on stm32f103, bit index on stm32f303
uint32_t IO_EXTI_Line(IO_t io) uint32_t IO_EXTI_Line(IO_t io)
{ {
if (!io) if (!io)
return 0; return 0;
#if defined(STM32F1) #if defined(STM32F1)
return 1 << IO_GPIOPinIdx(io); return 1 << IO_GPIOPinIdx(io);
#elif defined(STM32F3) #elif defined(STM32F3)
return IO_GPIOPinIdx(io); return IO_GPIOPinIdx(io);
#elif defined(STM32F4) #elif defined(STM32F4)
return 1 << IO_GPIOPinIdx(io); return 1 << IO_GPIOPinIdx(io);
#else #else
# error "Unknown target type" # error "Unknown target type"
#endif #endif
@ -124,151 +124,151 @@ uint32_t IO_EXTI_Line(IO_t io)
bool IORead(IO_t io) bool IORead(IO_t io)
{ {
if (!io) if (!io)
return false; return false;
return !! (IO_GPIO(io)->IDR & IO_Pin(io)); return !! (IO_GPIO(io)->IDR & IO_Pin(io));
} }
void IOWrite(IO_t io, bool hi) void IOWrite(IO_t io, bool hi)
{ {
if (!io) if (!io)
return; return;
#ifdef STM32F4 #ifdef STM32F4
if (hi) { if (hi) {
IO_GPIO(io)->BSRRL = IO_Pin(io); IO_GPIO(io)->BSRRL = IO_Pin(io);
} }
else { else {
IO_GPIO(io)->BSRRH = IO_Pin(io); IO_GPIO(io)->BSRRH = IO_Pin(io);
} }
#else #else
IO_GPIO(io)->BSRR = IO_Pin(io) << (hi ? 0 : 16); IO_GPIO(io)->BSRR = IO_Pin(io) << (hi ? 0 : 16);
#endif #endif
} }
void IOHi(IO_t io) void IOHi(IO_t io)
{ {
if (!io) if (!io)
return; return;
#ifdef STM32F4 #ifdef STM32F4
IO_GPIO(io)->BSRRL = IO_Pin(io); IO_GPIO(io)->BSRRL = IO_Pin(io);
#else #else
IO_GPIO(io)->BSRR = IO_Pin(io); IO_GPIO(io)->BSRR = IO_Pin(io);
#endif #endif
} }
void IOLo(IO_t io) void IOLo(IO_t io)
{ {
if (!io) if (!io)
return; return;
#ifdef STM32F4 #ifdef STM32F4
IO_GPIO(io)->BSRRH = IO_Pin(io); IO_GPIO(io)->BSRRH = IO_Pin(io);
#else #else
IO_GPIO(io)->BRR = IO_Pin(io); IO_GPIO(io)->BRR = IO_Pin(io);
#endif #endif
} }
void IOToggle(IO_t io) void IOToggle(IO_t io)
{ {
if (!io) if (!io)
return; return;
uint32_t mask = IO_Pin(io); uint32_t mask = IO_Pin(io);
// Read pin state from ODR but write to BSRR because it only changes the pins // Read pin state from ODR but write to BSRR because it only changes the pins
// high in the mask value rather than all pins. XORing ODR directly risks // high in the mask value rather than all pins. XORing ODR directly risks
// setting other pins incorrectly because it change all pins' state. // setting other pins incorrectly because it change all pins' state.
#ifdef STM32F4 #ifdef STM32F4
if (IO_GPIO(io)->ODR & mask) { if (IO_GPIO(io)->ODR & mask) {
IO_GPIO(io)->BSRRH = mask; IO_GPIO(io)->BSRRH = mask;
} else { } else {
IO_GPIO(io)->BSRRL = mask; IO_GPIO(io)->BSRRL = mask;
} }
#else #else
if (IO_GPIO(io)->ODR & mask) if (IO_GPIO(io)->ODR & mask)
mask <<= 16; // bit is set, shift mask to reset half mask <<= 16; // bit is set, shift mask to reset half
IO_GPIO(io)->BSRR = mask; IO_GPIO(io)->BSRR = mask;
#endif #endif
} }
// claim IO pin, set owner and resources // claim IO pin, set owner and resources
void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resources) void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resources)
{ {
ioRec_t *ioRec = IO_Rec(io); ioRec_t *ioRec = IO_Rec(io);
if (owner != OWNER_FREE) // pass OWNER_FREE to keep old owner if (owner != OWNER_FREE) // pass OWNER_FREE to keep old owner
ioRec->owner = owner; ioRec->owner = owner;
ioRec->resourcesUsed |= resources; ioRec->resourcesUsed |= resources;
} }
void IORelease(IO_t io) void IORelease(IO_t io)
{ {
ioRec_t *ioRec = IO_Rec(io); ioRec_t *ioRec = IO_Rec(io);
ioRec->owner = OWNER_FREE; ioRec->owner = OWNER_FREE;
} }
resourceOwner_t IOGetOwner(IO_t io) resourceOwner_t IOGetOwner(IO_t io)
{ {
ioRec_t *ioRec = IO_Rec(io); ioRec_t *ioRec = IO_Rec(io);
return ioRec->owner; return ioRec->owner;
} }
resourceType_t IOGetResources(IO_t io) resourceType_t IOGetResources(IO_t io)
{ {
ioRec_t *ioRec = IO_Rec(io); ioRec_t *ioRec = IO_Rec(io);
return ioRec->resourcesUsed; return ioRec->resourcesUsed;
} }
#if defined(STM32F1) #if defined(STM32F1)
void IOConfigGPIO(IO_t io, ioConfig_t cfg) void IOConfigGPIO(IO_t io, ioConfig_t cfg)
{ {
if (!io) if (!io)
return; return;
rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
RCC_ClockCmd(rcc, ENABLE); RCC_ClockCmd(rcc, ENABLE);
GPIO_InitTypeDef init = { GPIO_InitTypeDef init = {
.GPIO_Pin = IO_Pin(io), .GPIO_Pin = IO_Pin(io),
.GPIO_Speed = cfg & 0x03, .GPIO_Speed = cfg & 0x03,
.GPIO_Mode = cfg & 0x7c, .GPIO_Mode = cfg & 0x7c,
}; };
GPIO_Init(IO_GPIO(io), &init); GPIO_Init(IO_GPIO(io), &init);
} }
#elif defined(STM32F3) || defined(STM32F4) #elif defined(STM32F3) || defined(STM32F4)
void IOConfigGPIO(IO_t io, ioConfig_t cfg) void IOConfigGPIO(IO_t io, ioConfig_t cfg)
{ {
if (!io) if (!io)
return; return;
rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
RCC_ClockCmd(rcc, ENABLE); RCC_ClockCmd(rcc, ENABLE);
GPIO_InitTypeDef init = { GPIO_InitTypeDef init = {
.GPIO_Pin = IO_Pin(io), .GPIO_Pin = IO_Pin(io),
.GPIO_Mode = (cfg >> 0) & 0x03, .GPIO_Mode = (cfg >> 0) & 0x03,
.GPIO_Speed = (cfg >> 2) & 0x03, .GPIO_Speed = (cfg >> 2) & 0x03,
.GPIO_OType = (cfg >> 4) & 0x01, .GPIO_OType = (cfg >> 4) & 0x01,
.GPIO_PuPd = (cfg >> 5) & 0x03, .GPIO_PuPd = (cfg >> 5) & 0x03,
}; };
GPIO_Init(IO_GPIO(io), &init); GPIO_Init(IO_GPIO(io), &init);
} }
void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af) void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af)
{ {
if (!io) if (!io)
return; return;
rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc; rccPeriphTag_t rcc = ioPortDefs[IO_GPIOPortIdx(io)].rcc;
RCC_ClockCmd(rcc, ENABLE); RCC_ClockCmd(rcc, ENABLE);
GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af); GPIO_PinAFConfig(IO_GPIO(io), IO_GPIO_PinSource(io), af);
GPIO_InitTypeDef init = { GPIO_InitTypeDef init = {
.GPIO_Pin = IO_Pin(io), .GPIO_Pin = IO_Pin(io),
.GPIO_Mode = (cfg >> 0) & 0x03, .GPIO_Mode = (cfg >> 0) & 0x03,
.GPIO_Speed = (cfg >> 2) & 0x03, .GPIO_Speed = (cfg >> 2) & 0x03,
.GPIO_OType = (cfg >> 4) & 0x01, .GPIO_OType = (cfg >> 4) & 0x01,
.GPIO_PuPd = (cfg >> 5) & 0x03, .GPIO_PuPd = (cfg >> 5) & 0x03,
}; };
GPIO_Init(IO_GPIO(io), &init); GPIO_Init(IO_GPIO(io), &init);
} }
#endif #endif
@ -279,32 +279,32 @@ ioRec_t ioRecs[DEFIO_IO_USED_COUNT];
// initialize all ioRec_t structures from ROM // initialize all ioRec_t structures from ROM
// currently only bitmask is used, this may change in future // currently only bitmask is used, this may change in future
void IOInitGlobal(void) { void IOInitGlobal(void) {
ioRec_t *ioRec = ioRecs; ioRec_t *ioRec = ioRecs;
for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) { for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) {
for (unsigned pin = 0; pin < sizeof(ioDefUsedMask[0]) * 8; pin++) { for (unsigned pin = 0; pin < sizeof(ioDefUsedMask[0]) * 8; pin++) {
if (ioDefUsedMask[port] & (1 << pin)) { if (ioDefUsedMask[port] & (1 << pin)) {
ioRec->gpio = (GPIO_TypeDef *)(GPIOA_BASE + (port << 10)); // ports are 0x400 apart ioRec->gpio = (GPIO_TypeDef *)(GPIOA_BASE + (port << 10)); // ports are 0x400 apart
ioRec->pin = 1 << pin; ioRec->pin = 1 << pin;
ioRec++; ioRec++;
} }
} }
} }
} }
IO_t IOGetByTag(ioTag_t tag) IO_t IOGetByTag(ioTag_t tag)
{ {
int portIdx = DEFIO_TAG_GPIOID(tag); int portIdx = DEFIO_TAG_GPIOID(tag);
int pinIdx = DEFIO_TAG_PIN(tag); int pinIdx = DEFIO_TAG_PIN(tag);
if (portIdx >= DEFIO_PORT_USED_COUNT) if (portIdx >= DEFIO_PORT_USED_COUNT)
return NULL; return NULL;
// check if pin exists // check if pin exists
if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) if (!(ioDefUsedMask[portIdx] & (1 << pinIdx)))
return NULL; return NULL;
// count bits before this pin on single port // count bits before this pin on single port
int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]); int offset = __builtin_popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]);
// and add port offset // and add port offset
offset += ioDefUsedOffset[portIdx]; offset += ioDefUsedOffset[portIdx];
return ioRecs + offset; return ioRecs + offset;
} }

10
src/main/drivers/io_impl.h
View file

@ -5,14 +5,14 @@
#include "platform.h" #include "platform.h"
typedef struct ioDef_s { typedef struct ioDef_s {
ioTag_t tag; ioTag_t tag;
} ioDef_t; } ioDef_t;
typedef struct ioRec_s { typedef struct ioRec_s {
GPIO_TypeDef *gpio; GPIO_TypeDef *gpio;
uint16_t pin; uint16_t pin;
resourceOwner_t owner; resourceOwner_t owner;
resourceType_t resourcesUsed; // TODO! resourceType_t resourcesUsed; // TODO!
} ioRec_t; } ioRec_t;
extern ioRec_t ioRecs[DEFIO_IO_USED_COUNT]; extern ioRec_t ioRecs[DEFIO_IO_USED_COUNT];

9
src/main/drivers/pwm_mapping.c
View file

@ -189,12 +189,11 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
#endif #endif
#ifdef SONAR #ifdef SONAR
if (init->sonarGPIOConfig && if (init->sonarConfig &&
( (
CheckGPIOPin(timerHardwarePtr->pin, init->sonarGPIOConfig->gpio, init->sonarGPIOConfig->triggerPin) || timerHardwarePtr->pin == init->sonarConfig->triggerPin ||
CheckGPIOPin(timerHardwarePtr->pin, init->sonarGPIOConfig->gpio, init->sonarGPIOConfig->echoPin) timerHardwarePtr->pin == init->sonarConfig->echoPin
) )) {
) {
continue; continue;
} }
#endif #endif

13
src/main/drivers/pwm_mapping.h
View file

@ -16,7 +16,7 @@
*/ */
#pragma once #pragma once
#include "gpio.h"
#include "timer.h" #include "timer.h"
#define MAX_PWM_MOTORS 12 #define MAX_PWM_MOTORS 12
@ -42,11 +42,10 @@
#define ONESHOT42_TIMER_MHZ 24 #define ONESHOT42_TIMER_MHZ 24
#define ONESHOT125_TIMER_MHZ 8 #define ONESHOT125_TIMER_MHZ 8
typedef struct sonarGPIOConfig_s { typedef struct sonarIOConfig_s {
GPIO_TypeDef *gpio; ioTag_t triggerPin;
uint16_t triggerPin; ioTag_t echoPin;
uint16_t echoPin; } sonarIOConfig_t;
} sonarGPIOConfig_t;
typedef struct drv_pwm_config_s { typedef struct drv_pwm_config_s {
bool useParallelPWM; bool useParallelPWM;
@ -78,7 +77,7 @@ typedef struct drv_pwm_config_s {
uint16_t motorPwmRate; uint16_t motorPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
// some higher value (used by 3d mode), or 0, for brushed pwm drivers. // some higher value (used by 3d mode), or 0, for brushed pwm drivers.
sonarGPIOConfig_t *sonarGPIOConfig; sonarIOConfig_t *sonarConfig;
} drv_pwm_config_t; } drv_pwm_config_t;
enum { enum {

29
src/main/drivers/sonar_hcsr04.c
View file

@ -22,7 +22,6 @@
#include "build_config.h" #include "build_config.h"
#include "system.h" #include "system.h"
#include "gpio.h"
#include "nvic.h" #include "nvic.h"
#include "io.h" #include "io.h"
#include "exti.h" #include "exti.h"
@ -43,8 +42,9 @@ static uint32_t lastMeasurementAt;
static sonarHardware_t const *sonarHardware; static sonarHardware_t const *sonarHardware;
extiCallbackRec_t hcsr04_extiCallbackRec; extiCallbackRec_t hcsr04_extiCallbackRec;
static IO_t echoIO; static IO_t echoIO;
//static IO_t triggerIO; static IO_t triggerIO;
void hcsr04_extiHandler(extiCallbackRec_t* cb) void hcsr04_extiHandler(extiCallbackRec_t* cb)
{ {
@ -52,7 +52,7 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb)
uint32_t timing_stop; uint32_t timing_stop;
UNUSED(cb); UNUSED(cb);
if (digitalIn(sonarHardware->echo_gpio, sonarHardware->echo_pin) != 0) { if (IORead(echoIO) != 0) {
timing_start = micros(); timing_start = micros();
} }
else { else {
@ -71,32 +71,27 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sona
sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
#if !defined(UNIT_TEST) #if !defined(UNIT_TEST)
gpio_config_t gpio;
#ifdef STM32F10X #ifdef STM32F10X
// enable AFIO for EXTI support // enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif #endif
#ifdef STM32F303xC #if defined(STM32F3) || defined(STM32F4)
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif #endif
// trigger pin // trigger pin
gpio.pin = sonarHardware->trigger_pin; triggerIO = IOGetByTag(sonarHardware->triggerIO);
gpio.mode = Mode_Out_PP; IOInit(triggerIO, OWNER_SONAR, RESOURCE_INPUT);
gpio.speed = Speed_2MHz; IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
gpioInit(sonarHardware->trigger_gpio, &gpio);
// echo pin // echo pin
gpio.pin = sonarHardware->echo_pin; echoIO = IOGetByTag(sonarHardware->echoIO);
gpio.mode = Mode_IN_FLOATING; IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT);
gpioInit(sonarHardware->echo_gpio, &gpio); IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
echoIO = IOGetByTag(sonarHardware->echoIO);
#ifdef USE_EXTI #ifdef USE_EXTI
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
@ -123,10 +118,10 @@ void hcsr04_start_reading(void)
lastMeasurementAt = now; lastMeasurementAt = now;
digitalHi(sonarHardware->trigger_gpio, sonarHardware->trigger_pin); IOHi(triggerIO);
// The width of trig signal must be greater than 10us // The width of trig signal must be greater than 10us
delayMicroseconds(11); delayMicroseconds(11);
digitalLo(sonarHardware->trigger_gpio, sonarHardware->trigger_pin); IOLo(triggerIO);
#endif #endif
} }

4
src/main/drivers/sonar_hcsr04.h
View file

@ -21,10 +21,6 @@
#include "io.h" #include "io.h"
typedef struct sonarHardware_s { typedef struct sonarHardware_s {
uint16_t trigger_pin;
GPIO_TypeDef* trigger_gpio;
uint16_t echo_pin;
GPIO_TypeDef* echo_gpio;
ioTag_t triggerIO; ioTag_t triggerIO;
ioTag_t echoIO; ioTag_t echoIO;
} sonarHardware_t; } sonarHardware_t;

9
src/main/main.c
View file

@ -267,12 +267,11 @@ void init(void)
if (feature(FEATURE_SONAR)) { if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = { sonarIOConfig_t sonarConfig = {
.gpio = SONAR_GPIO, .triggerPin = sonarHardware->triggerIO,
.triggerPin = sonarHardware->echo_pin, .echoPin = sonarHardware->echoIO
.echoPin = sonarHardware->trigger_pin,
}; };
pwm_params.sonarGPIOConfig = &sonarGPIOConfig; pwm_params.sonarConfig = &sonarConfig;
} }
#endif #endif

78
src/main/sensors/sonar.c
View file

@ -26,7 +26,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/sonar_hcsr04.h" #include "drivers/sonar_hcsr04.h"
#include "drivers/gpio.h" #include "drivers/io.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
#include "config/config.h" #include "config/config.h"
@ -48,79 +48,21 @@ float sonarMaxTiltCos;
static int32_t calculatedAltitude; static int32_t calculatedAltitude;
#ifdef SONAR_CUSTOM_CONFIG
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig);
#endif
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
{ {
#if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R) #if defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN)
static const sonarHardware_t const sonarPWM56 = {
.trigger_pin = Pin_8, // PWM5 (PB8) - 5v tolerant
.trigger_gpio = GPIOB,
.echo_pin = Pin_9, // PWM6 (PB9) - 5v tolerant
.echo_gpio = GPIOB,
.triggerIO = IO_TAG(PB8),
.echoIO = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.triggerIO = IO_TAG(PB0),
.echoIO = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
#elif defined(OLIMEXINO)
UNUSED(batteryConfig); UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = { static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) .triggerIO = IO_TAG(SONAR_TRIGGER_PIN),
.trigger_gpio = GPIOB, .echoIO = IO_TAG(SONAR_ECHO_PIN),
.echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.triggerIO = IO_TAG(PB0),
.echoIO = IO_TAG(PB1),
};
return &sonarHardware;
#elif defined(CC3D)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_5, // (PB5)
.trigger_gpio = GPIOB,
.echo_pin = Pin_0, // (PB0) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.triggerIO = IO_TAG(PB5),
.echoIO = IO_TAG(PB0),
};
return &sonarHardware;
// TODO - move sonar pin selection to CLI
#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(FURYF3) || defined(RMDO)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_0, // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.triggerIO = IO_TAG(PB0),
.echoIO = IO_TAG(PB1),
};
return &sonarHardware;
#elif defined(SPARKY)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_2, // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOA,
.echo_pin = Pin_1, // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.triggerIO = IO_TAG(PA2),
.echoIO = IO_TAG(PB1),
}; };
return &sonarHardware; return &sonarHardware;
#elif defined(SONAR_CUSTOM_CONFIG)
return sonarGetTargetHardwareConfiguration(batteryConfig);
#elif defined(UNIT_TEST) #elif defined(UNIT_TEST)
UNUSED(batteryConfig); UNUSED(batteryConfig);
return 0; return 0;

3
src/main/target/CC3D/target.h
View file

@ -118,6 +118,9 @@
#define DISPLAY #define DISPLAY
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB0
#define SONAR_TRIGGER_PIN PB5
#undef GPS #undef GPS
#undef BARO #undef BARO

28
src/main/target/EUSTM32F103RC/sonar_config.c Normal file
View file

@ -0,0 +1,28 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
static const sonarHardware_t const sonarPWM56 = {
.triggerIO = IO_TAG(PB8),
.echoIO = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.triggerIO = IO_TAG(PB0),
.echoIO = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
}

2
src/main/target/EUSTM32F103RC/target.h
View file

@ -69,6 +69,8 @@
#define MAG_AK8975_ALIGN CW180_DEG_FLIP #define MAG_AK8975_ALIGN CW180_DEG_FLIP
#define SONAR #define SONAR
#define SONAR_CUSTOM_CONFIG
#define DISPLAY #define DISPLAY
#define USE_USART1 #define USE_USART1

3
src/main/target/FURYF3/target.h
View file

@ -184,6 +184,9 @@
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_FEATURES FEATURE_BLACKBOX

28
src/main/target/NAZE/sonar_config.c Normal file
View file

@ -0,0 +1,28 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
static const sonarHardware_t const sonarPWM56 = {
.triggerIO = IO_TAG(PB8),
.echoIO = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.triggerIO = IO_TAG(PB0),
.echoIO = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
}

1
src/main/target/NAZE/target.h
View file

@ -111,6 +111,7 @@
#define MAG_HMC5883_ALIGN CW180_DEG #define MAG_HMC5883_ALIGN CW180_DEG
#define SONAR #define SONAR
#define SONAR_CUSTOM_CONFIG
#define DISPLAY #define DISPLAY
#define USE_USART1 #define USE_USART1

2
src/main/target/OLIMEXINO/target.h
View file

@ -59,6 +59,8 @@
#define USE_MAG_HMC5883 #define USE_MAG_HMC5883
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define USE_USART1 #define USE_USART1
#define USE_USART2 #define USE_USART2

28
src/main/target/PORT103R/sonar_config.c Normal file
View file

@ -0,0 +1,28 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/io.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
static const sonarHardware_t const sonarPWM56 = {
.triggerIO = IO_TAG(PB8),
.echoIO = IO_TAG(PB9),
};
static const sonarHardware_t sonarRC78 = {
.triggerIO = IO_TAG(PB0),
.echoIO = IO_TAG(PB1),
};
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
}
}

2
src/main/target/PORT103R/target.h
View file

@ -87,6 +87,8 @@
#define USE_FLASH_M25P16 #define USE_FLASH_M25P16
#define SONAR #define SONAR
#define SONAR_CUSTOM_CONFIG
#define DISPLAY #define DISPLAY
#define USE_USART1 #define USE_USART1

2
src/main/target/RMDO/target.h
View file

@ -52,6 +52,8 @@
#define USE_FLASH_M25P16 #define USE_FLASH_M25P16
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define USE_USART1 #define USE_USART1
#define USE_USART2 #define USE_USART2

3
src/main/target/SPARKY/target.h
View file

@ -156,6 +156,9 @@
// USART2, PA3 // USART2, PA3
#define BIND_PIN PA3 #define BIND_PIN PA3
//#define SONAR
//#define SONAR_ECHO_PIN PB1
//#define SONAR_TRIGGER_PIN PA2
// available IO pins (from schematics) // available IO pins (from schematics)
//#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) //#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15))

2
src/main/target/SPRACINGF3/target.h
View file

@ -61,6 +61,8 @@
#define USE_FLASH_M25P16 #define USE_FLASH_M25P16
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define USE_USART1 #define USE_USART1
#define USE_USART2 #define USE_USART2

10
src/main/target/SPRACINGF3MINI/target.h
View file

@ -24,9 +24,9 @@
// early prototype had slightly different pin mappings. // early prototype had slightly different pin mappings.
//#define SPRACINGF3MINI_MKII_REVA //#define SPRACINGF3MINI_MKII_REVA
#define LED0 PB3 #define LED0 PB3
#define BEEPER PC15 #define BEEPER PC15
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. #define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins.
@ -50,7 +50,7 @@
//#define USE_FAKE_ACC //#define USE_FAKE_ACC
#define USE_ACC_MPU6500 #define USE_ACC_MPU6500
#define ACC_MPU6500_ALIGN CW180_DEG #define ACC_MPU6500_ALIGN CW180_DEG
#define GYRO_MPU6500_ALIGN CW180_DEG #define GYRO_MPU6500_ALIGN CW180_DEG
#define BARO #define BARO
@ -64,11 +64,13 @@
#define MAG_AK8975_ALIGN CW90_DEG_FLIP #define MAG_AK8975_ALIGN CW90_DEG_FLIP
#define SONAR #define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define USB_IO #define USB_IO
#define USB_CABLE_DETECTION #define USB_CABLE_DETECTION
#define USB_DETECT_PIN PB5 #define USB_DETECT_PIN PB5
#define USE_VCP #define USE_VCP
#define USE_USART1 #define USE_USART1