mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Fixed whitespace
This commit is contained in:
parent
786343e2b2
commit
8dd4a584c1
53 changed files with 311 additions and 311 deletions
|
@ -1644,7 +1644,7 @@ void blackboxUpdate(timeUs_t currentTimeUs)
|
||||||
// Did we run out of room on the device? Stop!
|
// Did we run out of room on the device? Stop!
|
||||||
if (isBlackboxDeviceFull()) {
|
if (isBlackboxDeviceFull()) {
|
||||||
#ifdef USE_FLASHFS
|
#ifdef USE_FLASHFS
|
||||||
if (blackboxState != BLACKBOX_STATE_ERASING
|
if (blackboxState != BLACKBOX_STATE_ERASING
|
||||||
&& blackboxState != BLACKBOX_STATE_START_ERASE
|
&& blackboxState != BLACKBOX_STATE_START_ERASE
|
||||||
&& blackboxState != BLACKBOX_STATE_ERASED) {
|
&& blackboxState != BLACKBOX_STATE_ERASED) {
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -610,7 +610,7 @@ static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
|
||||||
}
|
}
|
||||||
|
|
||||||
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
|
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
|
||||||
{
|
{
|
||||||
int exitType = (int)ptr;
|
int exitType = (int)ptr;
|
||||||
switch (exitType) {
|
switch (exitType) {
|
||||||
case CMS_EXIT_SAVE:
|
case CMS_EXIT_SAVE:
|
||||||
|
|
|
@ -32,7 +32,7 @@ int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int i
|
||||||
uint8_t *outByte = outBuf;
|
uint8_t *outByte = outBuf;
|
||||||
*outByte = 0;
|
*outByte = 0;
|
||||||
uint8_t outBit = 0x80;
|
uint8_t outBit = 0x80;
|
||||||
|
|
||||||
for (int ii = 0; ii < inLen; ++ii) {
|
for (int ii = 0; ii < inLen; ++ii) {
|
||||||
const int huffCodeLen = huffmanTable[*inBuf].codeLen;
|
const int huffCodeLen = huffmanTable[*inBuf].codeLen;
|
||||||
const uint16_t huffCode = huffmanTable[*inBuf].code;
|
const uint16_t huffCode = huffmanTable[*inBuf].code;
|
||||||
|
|
|
@ -85,7 +85,7 @@ static inline void mma8451ConfigureInterrupt(void)
|
||||||
#ifdef MMA8451_INT_PIN
|
#ifdef MMA8451_INT_PIN
|
||||||
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
|
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
|
||||||
// TODO - maybe pullup / pulldown ?
|
// TODO - maybe pullup / pulldown ?
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);
|
IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
|
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
|
||||||
|
|
|
@ -65,9 +65,9 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
|
||||||
},
|
},
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_I2C_DEVICE_3
|
#ifdef USE_I2C_DEVICE_3
|
||||||
{
|
{
|
||||||
.device = I2CDEV_3,
|
.device = I2CDEV_3,
|
||||||
.reg = I2C3,
|
.reg = I2C3,
|
||||||
.sclPins = { DEFIO_TAG_E(PA8) },
|
.sclPins = { DEFIO_TAG_E(PA8) },
|
||||||
.sdaPins = { DEFIO_TAG_E(PC9) },
|
.sdaPins = { DEFIO_TAG_E(PC9) },
|
||||||
.rcc = RCC_APB1(I2C3),
|
.rcc = RCC_APB1(I2C3),
|
||||||
|
|
|
@ -366,7 +366,7 @@ void i2cInit(I2CDevice device)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
i2cDevice_t *pDev = &i2cDevice[device];
|
i2cDevice_t *pDev = &i2cDevice[device];
|
||||||
const i2cHardware_t *hw = pDev->hardware;
|
const i2cHardware_t *hw = pDev->hardware;
|
||||||
|
|
||||||
if (!hw) {
|
if (!hw) {
|
||||||
return;
|
return;
|
||||||
|
|
|
@ -84,7 +84,7 @@ void i2cInit(I2CDevice device)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
i2cDevice_t *pDev = &i2cDevice[device];
|
i2cDevice_t *pDev = &i2cDevice[device];
|
||||||
const i2cHardware_t *hw = pDev->hardware;
|
const i2cHardware_t *hw = pDev->hardware;
|
||||||
|
|
||||||
if (!hw) {
|
if (!hw) {
|
||||||
|
|
|
@ -138,7 +138,7 @@ void spiInitDevice(SPIDevice device)
|
||||||
LL_SPI_Disable(spi->dev);
|
LL_SPI_Disable(spi->dev);
|
||||||
LL_SPI_DeInit(spi->dev);
|
LL_SPI_DeInit(spi->dev);
|
||||||
|
|
||||||
LL_SPI_InitTypeDef init =
|
LL_SPI_InitTypeDef init =
|
||||||
{
|
{
|
||||||
.TransferDirection = SPI_DIRECTION_2LINES,
|
.TransferDirection = SPI_DIRECTION_2LINES,
|
||||||
.Mode = SPI_MODE_MASTER,
|
.Mode = SPI_MODE_MASTER,
|
||||||
|
@ -152,7 +152,7 @@ void spiInitDevice(SPIDevice device)
|
||||||
.CRCCalculation = SPI_CRCCALCULATION_DISABLE,
|
.CRCCalculation = SPI_CRCCALCULATION_DISABLE,
|
||||||
};
|
};
|
||||||
LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF);
|
LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF);
|
||||||
|
|
||||||
LL_SPI_Init(spi->dev, &init);
|
LL_SPI_Init(spi->dev, &init);
|
||||||
LL_SPI_Enable(spi->dev);
|
LL_SPI_Enable(spi->dev);
|
||||||
}
|
}
|
||||||
|
@ -227,7 +227,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
|
||||||
*/
|
*/
|
||||||
bool spiIsBusBusy(SPI_TypeDef *instance)
|
bool spiIsBusBusy(SPI_TypeDef *instance)
|
||||||
{
|
{
|
||||||
return LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY
|
return LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY
|
||||||
|| LL_SPI_IsActiveFlag_BSY(instance);
|
|| LL_SPI_IsActiveFlag_BSY(instance);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -216,7 +216,7 @@ void i2c_OLED_clear_display_quick(busDevice_t *bus)
|
||||||
}
|
}
|
||||||
|
|
||||||
void i2c_OLED_clear_display(busDevice_t *bus)
|
void i2c_OLED_clear_display(busDevice_t *bus)
|
||||||
{
|
{
|
||||||
static const uint8_t i2c_OLED_cmd_clear_display_pre[] = {
|
static const uint8_t i2c_OLED_cmd_clear_display_pre[] = {
|
||||||
0xa6, // Set Normal Display
|
0xa6, // Set Normal Display
|
||||||
0xae, // Display OFF
|
0xae, // Display OFF
|
||||||
|
|
|
@ -104,8 +104,8 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
configTimeBase(timerHardware->tim, period, hz);
|
configTimeBase(timerHardware->tim, period, hz);
|
||||||
pwmOCConfig(timerHardware->tim,
|
pwmOCConfig(timerHardware->tim,
|
||||||
timerHardware->channel,
|
timerHardware->channel,
|
||||||
value,
|
value,
|
||||||
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
|
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
|
||||||
);
|
);
|
||||||
|
@ -233,7 +233,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
default:
|
default:
|
||||||
case PWM_TYPE_ONESHOT125:
|
case PWM_TYPE_ONESHOT125:
|
||||||
sMin = 125e-6f;
|
sMin = 125e-6f;
|
||||||
sLen = 125e-6f;
|
sLen = 125e-6f;
|
||||||
break;
|
break;
|
||||||
case PWM_TYPE_ONESHOT42:
|
case PWM_TYPE_ONESHOT42:
|
||||||
sMin = 42e-6f;
|
sMin = 42e-6f;
|
||||||
|
@ -241,7 +241,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
break;
|
break;
|
||||||
case PWM_TYPE_MULTISHOT:
|
case PWM_TYPE_MULTISHOT:
|
||||||
sMin = 5e-6f;
|
sMin = 5e-6f;
|
||||||
sLen = 20e-6f;
|
sLen = 20e-6f;
|
||||||
break;
|
break;
|
||||||
case PWM_TYPE_BRUSHED:
|
case PWM_TYPE_BRUSHED:
|
||||||
sMin = 0;
|
sMin = 0;
|
||||||
|
@ -250,7 +250,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
break;
|
break;
|
||||||
case PWM_TYPE_STANDARD:
|
case PWM_TYPE_STANDARD:
|
||||||
sMin = 1e-3f;
|
sMin = 1e-3f;
|
||||||
sLen = 1e-3f;
|
sLen = 1e-3f;
|
||||||
useUnsyncedPwm = true;
|
useUnsyncedPwm = true;
|
||||||
idlePulse = 0;
|
idlePulse = 0;
|
||||||
break;
|
break;
|
||||||
|
@ -294,8 +294,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
if (isDshot) {
|
if (isDshot) {
|
||||||
pwmDshotMotorHardwareConfig(timerHardware,
|
pwmDshotMotorHardwareConfig(timerHardware,
|
||||||
motorIndex,
|
motorIndex,
|
||||||
motorConfig->motorPwmProtocol,
|
motorConfig->motorPwmProtocol,
|
||||||
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
|
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
|
||||||
motors[motorIndex].enabled = true;
|
motors[motorIndex].enabled = true;
|
||||||
|
@ -320,14 +320,14 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
const uint32_t hz = clock / prescaler;
|
const uint32_t hz = clock / prescaler;
|
||||||
const unsigned period = useUnsyncedPwm ? hz / pwmRateHz : 0xffff;
|
const unsigned period = useUnsyncedPwm ? hz / pwmRateHz : 0xffff;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
if brushed then it is the entire length of the period.
|
if brushed then it is the entire length of the period.
|
||||||
TODO: this can be moved back to periodMin and periodLen
|
TODO: this can be moved back to periodMin and periodLen
|
||||||
once mixer outputs a 0..1 float value.
|
once mixer outputs a 0..1 float value.
|
||||||
*/
|
*/
|
||||||
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
|
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
|
||||||
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
|
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
|
||||||
|
|
||||||
pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
|
pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
|
||||||
|
|
||||||
bool timerAlreadyUsed = false;
|
bool timerAlreadyUsed = false;
|
||||||
|
|
|
@ -46,9 +46,9 @@ typedef enum {
|
||||||
DSHOT_CMD_SPIN_DIRECTION_1,
|
DSHOT_CMD_SPIN_DIRECTION_1,
|
||||||
DSHOT_CMD_SPIN_DIRECTION_2,
|
DSHOT_CMD_SPIN_DIRECTION_2,
|
||||||
DSHOT_CMD_3D_MODE_OFF,
|
DSHOT_CMD_3D_MODE_OFF,
|
||||||
DSHOT_CMD_3D_MODE_ON,
|
DSHOT_CMD_3D_MODE_ON,
|
||||||
DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
|
DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
|
||||||
DSHOT_CMD_SAVE_SETTINGS,
|
DSHOT_CMD_SAVE_SETTINGS,
|
||||||
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
|
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
|
||||||
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
|
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
|
||||||
DSHOT_CMD_MAX = 47
|
DSHOT_CMD_MAX = 47
|
||||||
|
@ -85,7 +85,7 @@ typedef enum {
|
||||||
#define MOTOR_BIT_1 14
|
#define MOTOR_BIT_1 14
|
||||||
#define MOTOR_BITLENGTH 19
|
#define MOTOR_BITLENGTH 19
|
||||||
|
|
||||||
#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
|
#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
|
||||||
#define PROSHOT_BASE_SYMBOL 24 // 1uS
|
#define PROSHOT_BASE_SYMBOL 24 // 1uS
|
||||||
#define PROSHOT_BIT_WIDTH 3
|
#define PROSHOT_BIT_WIDTH 3
|
||||||
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
|
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
|
||||||
|
|
|
@ -439,7 +439,7 @@ static void sdcard_sendDataBlockBegin(const uint8_t *buffer, bool multiBlockWrit
|
||||||
|
|
||||||
LL_SPI_EnableDMAReq_TX(SDCARD_SPI_INSTANCE);
|
LL_SPI_EnableDMAReq_TX(SDCARD_SPI_INSTANCE);
|
||||||
|
|
||||||
#elif defined(SDCARD_DMA_CHANNEL_TX)
|
#elif defined(SDCARD_DMA_CHANNEL_TX)
|
||||||
|
|
||||||
// Queue the transmission of the sector payload
|
// Queue the transmission of the sector payload
|
||||||
#ifdef SDCARD_DMA_CLK
|
#ifdef SDCARD_DMA_CLK
|
||||||
|
|
|
@ -16,7 +16,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Cleanflight (or Baseflight): original
|
* Cleanflight (or Baseflight): original
|
||||||
* jflyper: Mono-timer and single-wire half-duplex
|
* jflyper: Mono-timer and single-wire half-duplex
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
|
@ -42,73 +42,73 @@ static tcpPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
|
||||||
static bool tcpPortInitialized[SERIAL_PORT_COUNT];
|
static bool tcpPortInitialized[SERIAL_PORT_COUNT];
|
||||||
static bool tcpStart = false;
|
static bool tcpStart = false;
|
||||||
bool tcpIsStart(void) {
|
bool tcpIsStart(void) {
|
||||||
return tcpStart;
|
return tcpStart;
|
||||||
}
|
}
|
||||||
static void onData(dyad_Event *e) {
|
static void onData(dyad_Event *e) {
|
||||||
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||||
tcpDataIn(s, (uint8_t*)e->data, e->size);
|
tcpDataIn(s, (uint8_t*)e->data, e->size);
|
||||||
}
|
}
|
||||||
static void onClose(dyad_Event *e) {
|
static void onClose(dyad_Event *e) {
|
||||||
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||||
s->clientCount--;
|
s->clientCount--;
|
||||||
s->conn = NULL;
|
s->conn = NULL;
|
||||||
fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
||||||
if (s->clientCount == 0) {
|
if (s->clientCount == 0) {
|
||||||
s->connected = false;
|
s->connected = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
static void onAccept(dyad_Event *e) {
|
static void onAccept(dyad_Event *e) {
|
||||||
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||||
fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount);
|
fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount);
|
||||||
|
|
||||||
s->connected = true;
|
s->connected = true;
|
||||||
if (s->clientCount > 0) {
|
if (s->clientCount > 0) {
|
||||||
dyad_close(e->remote);
|
dyad_close(e->remote);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
s->clientCount++;
|
s->clientCount++;
|
||||||
fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
||||||
s->conn = e->remote;
|
s->conn = e->remote;
|
||||||
dyad_setNoDelay(e->remote, 1);
|
dyad_setNoDelay(e->remote, 1);
|
||||||
dyad_setTimeout(e->remote, 120);
|
dyad_setTimeout(e->remote, 120);
|
||||||
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
|
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
|
||||||
dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
|
dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
|
||||||
}
|
}
|
||||||
static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
|
static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
|
||||||
{
|
{
|
||||||
if (tcpPortInitialized[id]) {
|
if (tcpPortInitialized[id]) {
|
||||||
fprintf(stderr, "port is already initialized!\n");
|
fprintf(stderr, "port is already initialized!\n");
|
||||||
return s;
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (pthread_mutex_init(&s->txLock, NULL) != 0) {
|
if (pthread_mutex_init(&s->txLock, NULL) != 0) {
|
||||||
fprintf(stderr, "TX mutex init failed - %d\n", errno);
|
fprintf(stderr, "TX mutex init failed - %d\n", errno);
|
||||||
// TODO: clean up & re-init
|
// TODO: clean up & re-init
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
if (pthread_mutex_init(&s->rxLock, NULL) != 0) {
|
if (pthread_mutex_init(&s->rxLock, NULL) != 0) {
|
||||||
fprintf(stderr, "RX mutex init failed - %d\n", errno);
|
fprintf(stderr, "RX mutex init failed - %d\n", errno);
|
||||||
// TODO: clean up & re-init
|
// TODO: clean up & re-init
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
tcpStart = true;
|
tcpStart = true;
|
||||||
tcpPortInitialized[id] = true;
|
tcpPortInitialized[id] = true;
|
||||||
|
|
||||||
s->connected = false;
|
s->connected = false;
|
||||||
s->clientCount = 0;
|
s->clientCount = 0;
|
||||||
s->id = id;
|
s->id = id;
|
||||||
s->conn = NULL;
|
s->conn = NULL;
|
||||||
s->serv = dyad_newStream();
|
s->serv = dyad_newStream();
|
||||||
dyad_setNoDelay(s->serv, 1);
|
dyad_setNoDelay(s->serv, 1);
|
||||||
dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s);
|
dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s);
|
||||||
|
|
||||||
if (dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) {
|
if (dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) {
|
||||||
fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
|
fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
|
||||||
} else {
|
} else {
|
||||||
fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
|
fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
|
||||||
}
|
}
|
||||||
return s;
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_e mode, portOptions_e options)
|
serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_e mode, portOptions_e options)
|
||||||
|
@ -117,7 +117,7 @@ serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t b
|
||||||
|
|
||||||
#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
|
#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
|
||||||
if (id >= 0 && id < SERIAL_PORT_COUNT) {
|
if (id >= 0 && id < SERIAL_PORT_COUNT) {
|
||||||
s = tcpReconfigure(&tcpSerialPorts[id], id);
|
s = tcpReconfigure(&tcpSerialPorts[id], id);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
if (!s)
|
if (!s)
|
||||||
|
@ -128,10 +128,10 @@ serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, uint32_t b
|
||||||
// common serial initialisation code should move to serialPort::init()
|
// common serial initialisation code should move to serialPort::init()
|
||||||
s->port.rxBufferHead = s->port.rxBufferTail = 0;
|
s->port.rxBufferHead = s->port.rxBufferTail = 0;
|
||||||
s->port.txBufferHead = s->port.txBufferTail = 0;
|
s->port.txBufferHead = s->port.txBufferTail = 0;
|
||||||
s->port.rxBufferSize = RX_BUFFER_SIZE;
|
s->port.rxBufferSize = RX_BUFFER_SIZE;
|
||||||
s->port.txBufferSize = TX_BUFFER_SIZE;
|
s->port.txBufferSize = TX_BUFFER_SIZE;
|
||||||
s->port.rxBuffer = s->rxBuffer;
|
s->port.rxBuffer = s->rxBuffer;
|
||||||
s->port.txBuffer = s->txBuffer;
|
s->port.txBuffer = s->txBuffer;
|
||||||
|
|
||||||
// callback works for IRQ-based RX ONLY
|
// callback works for IRQ-based RX ONLY
|
||||||
s->port.rxCallback = rxCallback;
|
s->port.rxCallback = rxCallback;
|
||||||
|
@ -239,19 +239,19 @@ void tcpDataOut(tcpPort_t *instance)
|
||||||
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size)
|
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size)
|
||||||
{
|
{
|
||||||
tcpPort_t *s = (tcpPort_t *)instance;
|
tcpPort_t *s = (tcpPort_t *)instance;
|
||||||
pthread_mutex_lock(&s->rxLock);
|
pthread_mutex_lock(&s->rxLock);
|
||||||
|
|
||||||
while (size--) {
|
while (size--) {
|
||||||
// printf("%c", *ch);
|
// printf("%c", *ch);
|
||||||
s->port.rxBuffer[s->port.rxBufferHead] = *(ch++);
|
s->port.rxBuffer[s->port.rxBufferHead] = *(ch++);
|
||||||
if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) {
|
if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) {
|
||||||
s->port.rxBufferHead = 0;
|
s->port.rxBufferHead = 0;
|
||||||
} else {
|
} else {
|
||||||
s->port.rxBufferHead++;
|
s->port.rxBufferHead++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
pthread_mutex_unlock(&s->rxLock);
|
pthread_mutex_unlock(&s->rxLock);
|
||||||
// printf("\n");
|
// printf("\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
static const struct serialPortVTable tcpVTable = {
|
static const struct serialPortVTable tcpVTable = {
|
||||||
|
|
|
@ -259,7 +259,7 @@ void uartWrite(serialPort_t *instance, uint8_t ch)
|
||||||
if (s->txDMAChannel)
|
if (s->txDMAChannel)
|
||||||
#endif
|
#endif
|
||||||
{
|
{
|
||||||
uartTryStartTxDMA(s);
|
uartTryStartTxDMA(s);
|
||||||
} else {
|
} else {
|
||||||
USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
|
USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
|
@ -74,7 +74,7 @@ const uartHardware_t uartHardware[UARTDEV_COUNT] = {
|
||||||
},
|
},
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_UART2
|
#ifdef USE_UART2
|
||||||
{
|
{
|
||||||
.device = UARTDEV_2,
|
.device = UARTDEV_2,
|
||||||
.reg = USART2,
|
.reg = USART2,
|
||||||
.rxDMAChannel = UART2_RX_DMA_CHANNEL,
|
.rxDMAChannel = UART2_RX_DMA_CHANNEL,
|
||||||
|
|
|
@ -50,12 +50,12 @@
|
||||||
/*** ERLT ***/
|
/*** ERLT ***/
|
||||||
#define TRANSPONDER_DATA_LENGTH_ERLT 1
|
#define TRANSPONDER_DATA_LENGTH_ERLT 1
|
||||||
|
|
||||||
#define ERLTBitQuiet 0
|
#define ERLTBitQuiet 0
|
||||||
#define ERLTCyclesForOneBit 25
|
#define ERLTCyclesForOneBit 25
|
||||||
#define ERLTCyclesForZeroBit 10
|
#define ERLTCyclesForZeroBit 10
|
||||||
#define TRANSPONDER_DMA_BUFFER_SIZE_ERLT 200 // actually ERLT is variable length 91-196 depending on the ERLT id
|
#define TRANSPONDER_DMA_BUFFER_SIZE_ERLT 200 // actually ERLT is variable length 91-196 depending on the ERLT id
|
||||||
#define TRANSPONDER_TIMER_MHZ_ERLT 18
|
#define TRANSPONDER_TIMER_MHZ_ERLT 18
|
||||||
#define TRANSPONDER_CARRIER_HZ_ERLT 38000
|
#define TRANSPONDER_CARRIER_HZ_ERLT 38000
|
||||||
#define TRANSPONDER_TRANSMIT_DELAY_ERLT 22500
|
#define TRANSPONDER_TRANSMIT_DELAY_ERLT 22500
|
||||||
#define TRANSPONDER_TRANSMIT_JITTER_ERLT 5000
|
#define TRANSPONDER_TRANSMIT_JITTER_ERLT 5000
|
||||||
/*** ******** ***/
|
/*** ******** ***/
|
||||||
|
|
|
@ -36,40 +36,40 @@ void transponderIrInitERLT(transponder_t *transponder){
|
||||||
|
|
||||||
void addBitToBuffer(transponder_t *transponder, uint8_t cycles, uint8_t pulsewidth)
|
void addBitToBuffer(transponder_t *transponder, uint8_t cycles, uint8_t pulsewidth)
|
||||||
{
|
{
|
||||||
for (int i = 0; i < cycles; i++) {
|
for (int i = 0; i < cycles; i++) {
|
||||||
transponder->transponderIrDMABuffer.erlt[dmaBufferOffset++] = pulsewidth;
|
transponder->transponderIrDMABuffer.erlt[dmaBufferOffset++] = pulsewidth;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void updateTransponderDMABufferERLT(transponder_t *transponder, const uint8_t* transponderData)
|
void updateTransponderDMABufferERLT(transponder_t *transponder, const uint8_t* transponderData)
|
||||||
{
|
{
|
||||||
uint8_t byteToSend = ~(*transponderData); //transponderData is stored inverted, so invert before using
|
uint8_t byteToSend = ~(*transponderData); //transponderData is stored inverted, so invert before using
|
||||||
uint8_t paritysum = 0; //sum of one bits
|
uint8_t paritysum = 0; //sum of one bits
|
||||||
|
|
||||||
dmaBufferOffset = 0; //reset buffer count
|
dmaBufferOffset = 0; //reset buffer count
|
||||||
|
|
||||||
//start bit 1, always pulsed, bit value = 0
|
//start bit 1, always pulsed, bit value = 0
|
||||||
addBitToBuffer(transponder, ERLTCyclesForZeroBit, transponder->bitToggleOne);
|
addBitToBuffer(transponder, ERLTCyclesForZeroBit, transponder->bitToggleOne);
|
||||||
|
|
||||||
//start bit 2, always not pulsed, bit value = 0
|
//start bit 2, always not pulsed, bit value = 0
|
||||||
addBitToBuffer(transponder, ERLTCyclesForZeroBit, ERLTBitQuiet);
|
addBitToBuffer(transponder, ERLTCyclesForZeroBit, ERLTBitQuiet);
|
||||||
|
|
||||||
//add data bits, only the 6 LSB
|
//add data bits, only the 6 LSB
|
||||||
for (int i = 5; i >= 0; i--)
|
for (int i = 5; i >= 0; i--)
|
||||||
{
|
{
|
||||||
uint8_t bv = (byteToSend >> i) & 0x01;
|
uint8_t bv = (byteToSend >> i) & 0x01;
|
||||||
paritysum += bv;
|
paritysum += bv;
|
||||||
addBitToBuffer(transponder, (bv ? ERLTCyclesForOneBit : ERLTCyclesForZeroBit), ((i % 2) ? transponder->bitToggleOne : ERLTBitQuiet));
|
addBitToBuffer(transponder, (bv ? ERLTCyclesForOneBit : ERLTCyclesForZeroBit), ((i % 2) ? transponder->bitToggleOne : ERLTBitQuiet));
|
||||||
}
|
}
|
||||||
|
|
||||||
//parity bit, always pulsed, bit value is zero if sum is even, one if odd
|
//parity bit, always pulsed, bit value is zero if sum is even, one if odd
|
||||||
addBitToBuffer(transponder, ((paritysum % 2) ? ERLTCyclesForOneBit : ERLTCyclesForZeroBit), transponder->bitToggleOne);
|
addBitToBuffer(transponder, ((paritysum % 2) ? ERLTCyclesForOneBit : ERLTCyclesForZeroBit), transponder->bitToggleOne);
|
||||||
|
|
||||||
//add final zero after the pulsed parity bit to stop pulses until the next update
|
//add final zero after the pulsed parity bit to stop pulses until the next update
|
||||||
transponder->transponderIrDMABuffer.erlt[dmaBufferOffset++] = ERLTBitQuiet;
|
transponder->transponderIrDMABuffer.erlt[dmaBufferOffset++] = ERLTBitQuiet;
|
||||||
|
|
||||||
//reset buffer size to that required by this ERLT id
|
//reset buffer size to that required by this ERLT id
|
||||||
transponder->dma_buffer_size = dmaBufferOffset;
|
transponder->dma_buffer_size = dmaBufferOffset;
|
||||||
}
|
}
|
||||||
|
|
||||||
const struct transponderVTable erltTansponderVTable = {
|
const struct transponderVTable erltTansponderVTable = {
|
||||||
|
|
|
@ -2112,8 +2112,8 @@ static void cliBeeper(char *cmdline)
|
||||||
|
|
||||||
#ifdef FRSKY_BIND
|
#ifdef FRSKY_BIND
|
||||||
void cliFrSkyBind(char *cmdline){
|
void cliFrSkyBind(char *cmdline){
|
||||||
UNUSED(cmdline);
|
UNUSED(cmdline);
|
||||||
frSkyDBind();
|
frSkyDBind();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -2391,7 +2391,7 @@ static void cliDshotProg(char *cmdline)
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
writeDshotCommand(escIndex, command);
|
writeDshotCommand(escIndex, command);
|
||||||
}
|
}
|
||||||
|
|
||||||
cliPrintLinef("Command %d written.", command);
|
cliPrintLinef("Command %d written.", command);
|
||||||
|
|
||||||
|
|
|
@ -512,7 +512,7 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
|
||||||
if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) {
|
if((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) {
|
||||||
motorOutputMax = deadbandMotor3dLow;
|
motorOutputMax = deadbandMotor3dLow;
|
||||||
motorOutputMin = motorOutputLow;
|
motorOutputMin = motorOutputLow;
|
||||||
throttlePrevious = rcCommand[THROTTLE]; //3D Mode Throttle Fix #3696
|
throttlePrevious = rcCommand[THROTTLE]; //3D Mode Throttle Fix #3696
|
||||||
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; //3D Mode Throttle Fix #3696
|
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; //3D Mode Throttle Fix #3696
|
||||||
currentThrottleInputRange = rcCommandThrottleRange3dLow;
|
currentThrottleInputRange = rcCommandThrottleRange3dLow;
|
||||||
if(isMotorProtocolDshot()) mixerInversion = true;
|
if(isMotorProtocolDshot()) mixerInversion = true;
|
||||||
|
|
|
@ -468,7 +468,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
||||||
inCrashRecoveryMode = true;
|
inCrashRecoveryMode = true;
|
||||||
crashDetectedAtUs = currentTimeUs;
|
crashDetectedAtUs = currentTimeUs;
|
||||||
}
|
}
|
||||||
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold
|
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (ABS(errorRate) < crashGyroThreshold
|
||||||
|| ABS(getSetpointRate(axis)) > crashSetpointThreshold)) {
|
|| ABS(getSetpointRate(axis)) > crashSetpointThreshold)) {
|
||||||
inCrashRecoveryMode = false;
|
inCrashRecoveryMode = false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -47,18 +47,18 @@ STATIC_UNIT_TESTED rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1
|
||||||
|
|
||||||
static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
|
static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
uint8_t crc=0x00;
|
uint8_t crc=0x00;
|
||||||
while (len--) {
|
while (len--) {
|
||||||
crc ^= *ptr++;
|
crc ^= *ptr++;
|
||||||
for (i=8; i>0; --i) {
|
for (i=8; i>0; --i) {
|
||||||
if (crc & 0x80)
|
if (crc & 0x80)
|
||||||
crc = (crc << 1) ^ 0x31;
|
crc = (crc << 1) ^ 0x31;
|
||||||
else
|
else
|
||||||
crc = (crc << 1);
|
crc = (crc << 1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return (crc);
|
return (crc);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
|
static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
|
||||||
|
@ -83,16 +83,16 @@ static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
|
||||||
serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
|
serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void rcSplitProcessMode()
|
static void rcSplitProcessMode()
|
||||||
{
|
{
|
||||||
// if the device not ready, do not handle any mode change event
|
// if the device not ready, do not handle any mode change event
|
||||||
if (RCSPLIT_STATE_IS_READY != cameraState)
|
if (RCSPLIT_STATE_IS_READY != cameraState)
|
||||||
return ;
|
return ;
|
||||||
|
|
||||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||||
uint8_t switchIndex = i - BOXCAMERA1;
|
uint8_t switchIndex = i - BOXCAMERA1;
|
||||||
if (IS_RC_MODE_ACTIVE(i)) {
|
if (IS_RC_MODE_ACTIVE(i)) {
|
||||||
// check last state of this mode, if it's true, then ignore it.
|
// check last state of this mode, if it's true, then ignore it.
|
||||||
// Here is a logic to make a toggle control for this mode
|
// Here is a logic to make a toggle control for this mode
|
||||||
if (switchStates[switchIndex].isActivated) {
|
if (switchStates[switchIndex].isActivated) {
|
||||||
continue;
|
continue;
|
||||||
|
@ -113,7 +113,7 @@ static void rcSplitProcessMode()
|
||||||
argument = RCSPLIT_CTRL_ARGU_INVALID;
|
argument = RCSPLIT_CTRL_ARGU_INVALID;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
|
if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
|
||||||
sendCtrlCommand(argument);
|
sendCtrlCommand(argument);
|
||||||
switchStates[switchIndex].isActivated = true;
|
switchStates[switchIndex].isActivated = true;
|
||||||
|
@ -140,7 +140,7 @@ bool rcSplitInit(void)
|
||||||
// set init value to true, to avoid the action auto run when the flight board start and the switch is on.
|
// set init value to true, to avoid the action auto run when the flight board start and the switch is on.
|
||||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||||
uint8_t switchIndex = i - BOXCAMERA1;
|
uint8_t switchIndex = i - BOXCAMERA1;
|
||||||
switchStates[switchIndex].isActivated = true;
|
switchStates[switchIndex].isActivated = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
cameraState = RCSPLIT_STATE_IS_READY;
|
cameraState = RCSPLIT_STATE_IS_READY;
|
||||||
|
|
|
@ -237,11 +237,11 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
serialPort = openSerialPort(portConfig->identifier,
|
serialPort = openSerialPort(portConfig->identifier,
|
||||||
FUNCTION_RX_SERIAL,
|
FUNCTION_RX_SERIAL,
|
||||||
crsfDataReceive,
|
crsfDataReceive,
|
||||||
CRSF_BAUDRATE,
|
CRSF_BAUDRATE,
|
||||||
CRSF_PORT_MODE,
|
CRSF_PORT_MODE,
|
||||||
CRSF_PORT_OPTIONS | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
CRSF_PORT_OPTIONS | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||||
);
|
);
|
||||||
|
|
||||||
|
|
|
@ -213,18 +213,18 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
|
|
||||||
|
|
||||||
rxBytesToIgnore = 0;
|
rxBytesToIgnore = 0;
|
||||||
serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
|
serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
|
||||||
FUNCTION_RX_SERIAL,
|
FUNCTION_RX_SERIAL,
|
||||||
ibusDataReceive,
|
ibusDataReceive,
|
||||||
IBUS_BAUDRATE,
|
IBUS_BAUDRATE,
|
||||||
portShared ? MODE_RXTX : MODE_RX,
|
portShared ? MODE_RXTX : MODE_RX,
|
||||||
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0)
|
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0)
|
||||||
);
|
);
|
||||||
|
|
||||||
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
|
||||||
if (portShared) {
|
if (portShared) {
|
||||||
initSharedIbusTelemetry(ibusPort);
|
initSharedIbusTelemetry(ibusPort);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return ibusPort != NULL;
|
return ibusPort != NULL;
|
||||||
|
|
|
@ -602,12 +602,12 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfi
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
jetiExBusPort = openSerialPort(portConfig->identifier,
|
jetiExBusPort = openSerialPort(portConfig->identifier,
|
||||||
FUNCTION_RX_SERIAL,
|
FUNCTION_RX_SERIAL,
|
||||||
jetiExBusDataReceive,
|
jetiExBusDataReceive,
|
||||||
JETIEXBUS_BAUDRATE,
|
JETIEXBUS_BAUDRATE,
|
||||||
MODE_RXTX,
|
MODE_RXTX,
|
||||||
JETIEXBUS_OPTIONS | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
JETIEXBUS_OPTIONS | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||||
);
|
);
|
||||||
serialSetMode(jetiExBusPort, MODE_RX);
|
serialSetMode(jetiExBusPort, MODE_RX);
|
||||||
return jetiExBusPort != NULL;
|
return jetiExBusPort != NULL;
|
||||||
|
|
|
@ -248,11 +248,11 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
bool portShared = false;
|
bool portShared = false;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
|
serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
|
||||||
FUNCTION_RX_SERIAL,
|
FUNCTION_RX_SERIAL,
|
||||||
sbusDataReceive,
|
sbusDataReceive,
|
||||||
SBUS_BAUDRATE,
|
SBUS_BAUDRATE,
|
||||||
portShared ? MODE_RXTX : MODE_RX,
|
portShared ? MODE_RXTX : MODE_RX,
|
||||||
SBUS_PORT_OPTIONS | (rxConfig->sbus_inversion ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
SBUS_PORT_OPTIONS | (rxConfig->sbus_inversion ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||||
);
|
);
|
||||||
|
|
||||||
|
|
|
@ -179,11 +179,11 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
bool portShared = false;
|
bool portShared = false;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
serialPort_t *sumdPort = openSerialPort(portConfig->identifier,
|
serialPort_t *sumdPort = openSerialPort(portConfig->identifier,
|
||||||
FUNCTION_RX_SERIAL,
|
FUNCTION_RX_SERIAL,
|
||||||
sumdDataReceive,
|
sumdDataReceive,
|
||||||
SUMD_BAUDRATE,
|
SUMD_BAUDRATE,
|
||||||
portShared ? MODE_RXTX : MODE_RX,
|
portShared ? MODE_RXTX : MODE_RX,
|
||||||
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||||
);
|
);
|
||||||
|
|
||||||
|
|
|
@ -329,11 +329,11 @@ bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
bool portShared = false;
|
bool portShared = false;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
serialPort_t *xBusPort = openSerialPort(portConfig->identifier,
|
serialPort_t *xBusPort = openSerialPort(portConfig->identifier,
|
||||||
FUNCTION_RX_SERIAL,
|
FUNCTION_RX_SERIAL,
|
||||||
xBusDataReceive,
|
xBusDataReceive,
|
||||||
baudRate,
|
baudRate,
|
||||||
portShared ? MODE_RXTX : MODE_RX,
|
portShared ? MODE_RXTX : MODE_RX,
|
||||||
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
SERIAL_NOT_INVERTED | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
|
||||||
);
|
);
|
||||||
|
|
||||||
|
|
|
@ -20,14 +20,14 @@
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
\ | _ _| __| \ |\ \ /| | _ \ _ \ _ \
|
\ | _ _| __| \ |\ \ /| | _ \ _ \ _ \
|
||||||
_ \ | | _| . | \ \ \ / __ | ( |( |__/
|
_ \ | | _| . | \ \ \ / __ | ( |( |__/
|
||||||
_/ _\____|___|___|_|\_| \_/\_/ _| _|\___/\___/_|
|
_/ _\____|___|___|_|\_| \_/\_/ _| _|\___/\___/_|
|
||||||
|
|
||||||
|
|
||||||
Take me to your leader-board...
|
Take me to your leader-board...
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
*/
|
*/
|
||||||
|
@ -61,8 +61,8 @@ void targetConfiguration(void)
|
||||||
{
|
{
|
||||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||||
motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||||
motorConfigMutable()->minthrottle = 1080;
|
motorConfigMutable()->minthrottle = 1080;
|
||||||
motorConfigMutable()->maxthrottle = 2000;
|
motorConfigMutable()->maxthrottle = 2000;
|
||||||
pidConfigMutable()->pid_process_denom = 1;
|
pidConfigMutable()->pid_process_denom = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -20,14 +20,14 @@
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
\ | _ _| __| \ |\ \ /| | _ \ _ \ _ \
|
\ | _ _| __| \ |\ \ /| | _ \ _ \ _ \
|
||||||
_ \ | | _| . | \ \ \ / __ | ( |( |__/
|
_ \ | | _| . | \ \ \ / __ | ( |( |__/
|
||||||
_/ _\____|___|___|_|\_| \_/\_/ _| _|\___/\___/_|
|
_/ _\____|___|___|_|\_| \_/\_/ _| _|\___/\___/_|
|
||||||
|
|
||||||
|
|
||||||
Take me to your leader-board...
|
Take me to your leader-board...
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -20,14 +20,14 @@
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
\ | _ _| __| \ |\ \ /| | _ \ _ \ _ \
|
\ | _ _| __| \ |\ \ /| | _ \ _ \ _ \
|
||||||
_ \ | | _| . | \ \ \ / __ | ( |( |__/
|
_ \ | | _| . | \ \ \ / __ | ( |( |__/
|
||||||
_/ _\____|___|___|_|\_| \_/\_/ _| _|\___/\___/_|
|
_/ _\____|___|___|_|\_| \_/\_/ _| _|\___/\___/_|
|
||||||
|
|
||||||
|
|
||||||
Take me to your leader-board...
|
Take me to your leader-board...
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
*/
|
*/
|
||||||
|
@ -104,7 +104,7 @@
|
||||||
//#define SPI5_MOSI_PIN
|
//#define SPI5_MOSI_PIN
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* Motion Processing Unit (MPU) - Invensense 6-axis MPU-6500 or 9-axis MPU-9250
|
/* Motion Processing Unit (MPU) - Invensense 6-axis MPU-6500 or 9-axis MPU-9250
|
||||||
*/
|
*/
|
||||||
// Interrupt
|
// Interrupt
|
||||||
#define USE_EXTI
|
#define USE_EXTI
|
||||||
|
@ -129,7 +129,7 @@
|
||||||
#define USE_ACC_SPI_MPU6500
|
#define USE_ACC_SPI_MPU6500
|
||||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||||
|
|
||||||
/* Optional Digital Pressure Sensor (barometer) - Bosch BMP280
|
/* Optional Digital Pressure Sensor (barometer) - Bosch BMP280
|
||||||
* TODO: not implemented on V1 or V2 pcb
|
* TODO: not implemented on V1 or V2 pcb
|
||||||
*/
|
*/
|
||||||
#define BARO
|
#define BARO
|
||||||
|
@ -204,13 +204,13 @@
|
||||||
#define I2C_DEVICE (I2CDEV_1)
|
#define I2C_DEVICE (I2CDEV_1)
|
||||||
#define USE_I2C_PULLUP
|
#define USE_I2C_PULLUP
|
||||||
#define I2C1_SCL PB6
|
#define I2C1_SCL PB6
|
||||||
#define I2C1_SDA PB7
|
#define I2C1_SDA PB7
|
||||||
#else
|
#else
|
||||||
#undef CMS
|
#undef CMS
|
||||||
#undef USE_I2C
|
#undef USE_I2C
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* MCU Pin Mapping - LPFQ64 Flags
|
/* MCU Pin Mapping - LPFQ64 Flags
|
||||||
*/
|
*/
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
|
|
|
@ -62,7 +62,7 @@ void targetConfiguration(void)
|
||||||
// Frsky version
|
// Frsky version
|
||||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
|
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
pidProfilesMutable(0)->pid[PID_ROLL].P = 90;
|
pidProfilesMutable(0)->pid[PID_ROLL].P = 90;
|
||||||
pidProfilesMutable(0)->pid[PID_ROLL].I = 44;
|
pidProfilesMutable(0)->pid[PID_ROLL].I = 44;
|
||||||
pidProfilesMutable(0)->pid[PID_ROLL].D = 60;
|
pidProfilesMutable(0)->pid[PID_ROLL].D = 60;
|
||||||
|
|
|
@ -26,7 +26,7 @@
|
||||||
|
|
||||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
|
|
||||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM
|
DEF_TIM(TIM4, CH3, PB8, TIM_USE_PWM | TIM_USE_PPM, TIMER_OUTPUT_NONE, 0), // PPM
|
||||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S1_OUT D1_ST7
|
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S1_OUT D1_ST7
|
||||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S2_OUT D1_ST2
|
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S2_OUT D1_ST2
|
||||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S3_OUT D1_ST6
|
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S3_OUT D1_ST6
|
||||||
|
@ -35,5 +35,5 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
DEF_TIM(TIM3, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT D1_ST2
|
DEF_TIM(TIM3, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // S6_OUT D1_ST2
|
||||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 TX Softserial Smartport
|
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 TX Softserial Smartport
|
||||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 RX
|
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_STANDARD, 0), // UART6 RX
|
||||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED
|
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // LED
|
||||||
};
|
};
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
#define TARGET_BOARD_IDENTIFIER "KOMB" // Furious FPV KOMBINI
|
#define TARGET_BOARD_IDENTIFIER "KOMB" // Furious FPV KOMBINI
|
||||||
#elif defined(FF_ACROWHOOPSP)
|
#elif defined(FF_ACROWHOOPSP)
|
||||||
#define TARGET_BOARD_IDENTIFIER "AWHS" // Furious FPV ACROWHOOP SPEKTRUM
|
#define TARGET_BOARD_IDENTIFIER "AWHS" // Furious FPV ACROWHOOP SPEKTRUM
|
||||||
#else
|
#else
|
||||||
#define TARGET_BOARD_IDENTIFIER "PIKO" // Furious FPV PIKOBLX
|
#define TARGET_BOARD_IDENTIFIER "PIKO" // Furious FPV PIKOBLX
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -64,11 +64,11 @@
|
||||||
#define UART1_TX_PIN PA9
|
#define UART1_TX_PIN PA9
|
||||||
#define UART1_RX_PIN PA10
|
#define UART1_RX_PIN PA10
|
||||||
|
|
||||||
#define UART2_TX_PIN PA14
|
#define UART2_TX_PIN PA14
|
||||||
#define UART2_RX_PIN PA15
|
#define UART2_RX_PIN PA15
|
||||||
|
|
||||||
#define UART3_TX_PIN PB10
|
#define UART3_TX_PIN PB10
|
||||||
#define UART3_RX_PIN PB11
|
#define UART3_RX_PIN PB11
|
||||||
|
|
||||||
#define USE_I2C
|
#define USE_I2C
|
||||||
#define USE_I2C_DEVICE_1
|
#define USE_I2C_DEVICE_1
|
||||||
|
@ -90,8 +90,8 @@
|
||||||
#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST)
|
#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST)
|
||||||
|
|
||||||
#define USE_SPI
|
#define USE_SPI
|
||||||
#define USE_SPI_DEVICE_2
|
#define USE_SPI_DEVICE_2
|
||||||
#define USE_SPI_DEVICE_1
|
#define USE_SPI_DEVICE_1
|
||||||
|
|
||||||
#define SPI2_NSS_PIN PB12
|
#define SPI2_NSS_PIN PB12
|
||||||
#define SPI2_SCK_PIN PB13
|
#define SPI2_SCK_PIN PB13
|
||||||
|
@ -145,5 +145,5 @@
|
||||||
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
|
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
|
||||||
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
|
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
|
||||||
|
|
||||||
#define USABLE_TIMER_CHANNEL_COUNT 9
|
#define USABLE_TIMER_CHANNEL_COUNT 9
|
||||||
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(17))
|
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(17))
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
#define BEEPER PB4
|
#define BEEPER PB4
|
||||||
#define BEEPER_INVERTED
|
#define BEEPER_INVERTED
|
||||||
|
|
||||||
#define INVERTER_PIN_UART6 PC8
|
#define INVERTER_PIN_UART6 PC8
|
||||||
|
|
||||||
#define MPU6000_CS_PIN PA4
|
#define MPU6000_CS_PIN PA4
|
||||||
#define MPU6000_SPI_INSTANCE SPI1
|
#define MPU6000_SPI_INSTANCE SPI1
|
||||||
|
@ -45,7 +45,7 @@
|
||||||
#define USE_BARO_BMP280
|
#define USE_BARO_BMP280
|
||||||
#define USE_BARO_SPI_BMP280
|
#define USE_BARO_SPI_BMP280
|
||||||
#define BMP280_SPI_INSTANCE SPI3
|
#define BMP280_SPI_INSTANCE SPI3
|
||||||
#define BMP280_CS_PIN PB3
|
#define BMP280_CS_PIN PB3
|
||||||
|
|
||||||
#define OSD
|
#define OSD
|
||||||
#define USE_MAX7456
|
#define USE_MAX7456
|
||||||
|
@ -88,7 +88,7 @@
|
||||||
#define USE_SOFTSERIAL1
|
#define USE_SOFTSERIAL1
|
||||||
#define USE_SOFTSERIAL2
|
#define USE_SOFTSERIAL2
|
||||||
|
|
||||||
#define SERIAL_PORT_COUNT 6
|
#define SERIAL_PORT_COUNT 6
|
||||||
|
|
||||||
#define USE_ESCSERIAL
|
#define USE_ESCSERIAL
|
||||||
#define ESCSERIAL_TIMER_TX_PIN PB8 // (HARDARE=0,PPM)
|
#define ESCSERIAL_TIMER_TX_PIN PB8 // (HARDARE=0,PPM)
|
||||||
|
|
|
@ -142,7 +142,7 @@
|
||||||
|
|
||||||
#define SOFTSERIAL1_RX_PIN PB0
|
#define SOFTSERIAL1_RX_PIN PB0
|
||||||
#define SOFTSERIAL1_TX_PIN PB1
|
#define SOFTSERIAL1_TX_PIN PB1
|
||||||
|
|
||||||
#define SONAR
|
#define SONAR
|
||||||
#define SONAR_ECHO_PIN PB1
|
#define SONAR_ECHO_PIN PB1
|
||||||
#define SONAR_TRIGGER_PIN PB0
|
#define SONAR_TRIGGER_PIN PB0
|
||||||
|
|
|
@ -152,13 +152,13 @@
|
||||||
#define SPI3_MISO_PIN PC11
|
#define SPI3_MISO_PIN PC11
|
||||||
#define SPI3_MOSI_PIN PC12
|
#define SPI3_MOSI_PIN PC12
|
||||||
|
|
||||||
/*
|
/*
|
||||||
#define USE_I2C
|
#define USE_I2C
|
||||||
#define USE_I2C_DEVICE_1
|
#define USE_I2C_DEVICE_1
|
||||||
#define I2C_DEVICE (I2CDEV_1)
|
#define I2C_DEVICE (I2CDEV_1)
|
||||||
#define USE_I2C_PULLUP
|
#define USE_I2C_PULLUP
|
||||||
#define I2C1_SCL PB6
|
#define I2C1_SCL PB6
|
||||||
#define I2C1_SDA PB7
|
#define I2C1_SDA PB7
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define LED_STRIP
|
#define LED_STRIP
|
||||||
|
|
|
@ -26,16 +26,16 @@
|
||||||
|
|
||||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
|
|
||||||
DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM
|
DEF_TIM(TIM2, CH2, PA1, TIM_USE_PPM, TIMER_INPUT_ENABLED), // PPM
|
||||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Rx
|
DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Rx
|
||||||
DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Tx
|
DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, TIMER_OUTPUT_ENABLED), // SS1Tx
|
||||||
|
|
||||||
DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S1
|
DEF_TIM(TIM8, CH3, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S1
|
||||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S2
|
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S2
|
||||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S3
|
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S3
|
||||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S4
|
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S4
|
||||||
DEF_TIM(TIM16, CH1, PB4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S5
|
DEF_TIM(TIM16, CH1, PB4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // S5
|
||||||
|
|
||||||
DEF_TIM(TIM15, CH1, PA2, TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP
|
DEF_TIM(TIM15, CH1, PA2, TIM_USE_LED, TIMER_OUTPUT_ENABLED), // LED_STRIP
|
||||||
DEF_TIM(TIM2, CH1, PA15, TIM_USE_ANY, TIMER_OUTPUT_ENABLED), // CAMERA CONTROL
|
DEF_TIM(TIM2, CH1, PA15, TIM_USE_ANY, TIMER_OUTPUT_ENABLED), // CAMERA CONTROL
|
||||||
};
|
};
|
||||||
|
|
|
@ -75,7 +75,7 @@
|
||||||
|
|
||||||
#define LED_STRIP
|
#define LED_STRIP
|
||||||
|
|
||||||
#define CAMERA_CONTROL_PIN PA15
|
#define CAMERA_CONTROL_PIN PA15
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
|
|
|
@ -18,7 +18,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define TARGET_BOARD_IDENTIFIER "MKF4"
|
#define TARGET_BOARD_IDENTIFIER "MKF4"
|
||||||
//#define CONFIG_START_FLASH_ADDRESS (0x08080000)
|
//#define CONFIG_START_FLASH_ADDRESS (0x08080000)
|
||||||
|
|
||||||
#define USBD_PRODUCT_STRING "MatekF4"
|
#define USBD_PRODUCT_STRING "MatekF4"
|
||||||
|
|
||||||
|
@ -112,7 +112,7 @@
|
||||||
//#define SOFTSERIAL1_RX_PIN PA15 // S5
|
//#define SOFTSERIAL1_RX_PIN PA15 // S5
|
||||||
//#define SOFTSERIAL1_TX_PIN PA8 // S6
|
//#define SOFTSERIAL1_TX_PIN PA8 // S6
|
||||||
|
|
||||||
#define SERIAL_PORT_COUNT 7
|
#define SERIAL_PORT_COUNT 7
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
|
|
|
@ -112,7 +112,7 @@
|
||||||
|
|
||||||
#define USE_SOFTSERIAL1
|
#define USE_SOFTSERIAL1
|
||||||
|
|
||||||
#define SERIAL_PORT_COUNT 7
|
#define SERIAL_PORT_COUNT 7
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
|
|
|
@ -166,6 +166,6 @@
|
||||||
#define USABLE_TIMER_CHANNEL_COUNT 5
|
#define USABLE_TIMER_CHANNEL_COUNT 5
|
||||||
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) )
|
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) )
|
||||||
#else
|
#else
|
||||||
#define USABLE_TIMER_CHANNEL_COUNT 7
|
#define USABLE_TIMER_CHANNEL_COUNT 7
|
||||||
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) )
|
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) )
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -216,7 +216,7 @@ typedef struct
|
||||||
} I2C_TypeDef;
|
} I2C_TypeDef;
|
||||||
|
|
||||||
typedef enum
|
typedef enum
|
||||||
{
|
{
|
||||||
FLASH_BUSY = 1,
|
FLASH_BUSY = 1,
|
||||||
FLASH_ERROR_PG,
|
FLASH_ERROR_PG,
|
||||||
FLASH_ERROR_WRP,
|
FLASH_ERROR_WRP,
|
||||||
|
@ -233,7 +233,7 @@ typedef struct {
|
||||||
double position_xyz[3]; // meters, NED from origin
|
double position_xyz[3]; // meters, NED from origin
|
||||||
} fdm_packet;
|
} fdm_packet;
|
||||||
typedef struct {
|
typedef struct {
|
||||||
float motor_speed[4]; // normal: [0.0, 1.0], 3D: [-1.0, 1.0]
|
float motor_speed[4]; // normal: [0.0, 1.0], 3D: [-1.0, 1.0]
|
||||||
} servo_packet;
|
} servo_packet;
|
||||||
|
|
||||||
void FLASH_Unlock(void);
|
void FLASH_Unlock(void);
|
||||||
|
|
|
@ -20,12 +20,12 @@ extern "C" {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
int fd;
|
int fd;
|
||||||
struct sockaddr_in si;
|
struct sockaddr_in si;
|
||||||
struct sockaddr_in recv;
|
struct sockaddr_in recv;
|
||||||
int port;
|
int port;
|
||||||
char* addr;
|
char* addr;
|
||||||
bool isServer;
|
bool isServer;
|
||||||
} udpLink_t;
|
} udpLink_t;
|
||||||
|
|
||||||
int udpInit(udpLink_t* link, const char* addr, int port, bool isServer);
|
int udpInit(udpLink_t* link, const char* addr, int port, bool isServer);
|
||||||
|
|
|
@ -30,7 +30,7 @@
|
||||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
*
|
*
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||||
#ifndef __STM32F1xx_HAL_CONF_H
|
#ifndef __STM32F1xx_HAL_CONF_H
|
||||||
|
@ -45,7 +45,7 @@
|
||||||
|
|
||||||
/* ########################## Module Selection ############################## */
|
/* ########################## Module Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief This is the list of modules to be used in the HAL driver
|
* @brief This is the list of modules to be used in the HAL driver
|
||||||
*/
|
*/
|
||||||
#define HAL_MODULE_ENABLED
|
#define HAL_MODULE_ENABLED
|
||||||
#define HAL_ADC_MODULE_ENABLED
|
#define HAL_ADC_MODULE_ENABLED
|
||||||
|
@ -85,7 +85,7 @@
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSE is used as system clock source, directly or through the PLL).
|
* (when HSE is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined (HSE_VALUE)
|
#if !defined (HSE_VALUE)
|
||||||
#if defined(USE_STM3210C_EVAL)
|
#if defined(USE_STM3210C_EVAL)
|
||||||
#define HSE_VALUE ((uint32_t)25000000) /*!< Value of the External oscillator in Hz */
|
#define HSE_VALUE ((uint32_t)25000000) /*!< Value of the External oscillator in Hz */
|
||||||
#else
|
#else
|
||||||
|
@ -100,7 +100,7 @@
|
||||||
/**
|
/**
|
||||||
* @brief Internal High Speed oscillator (HSI) value.
|
* @brief Internal High Speed oscillator (HSI) value.
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSI is used as system clock source, directly or through the PLL).
|
* (when HSI is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined (HSI_VALUE)
|
#if !defined (HSI_VALUE)
|
||||||
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
|
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
|
||||||
|
@ -114,30 +114,30 @@
|
||||||
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External oscillator in Hz*/
|
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External oscillator in Hz*/
|
||||||
#endif /* LSE_VALUE */
|
#endif /* LSE_VALUE */
|
||||||
|
|
||||||
|
|
||||||
#if !defined (LSE_STARTUP_TIMEOUT)
|
#if !defined (LSE_STARTUP_TIMEOUT)
|
||||||
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */
|
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */
|
||||||
#endif /* HSE_STARTUP_TIMEOUT */
|
#endif /* HSE_STARTUP_TIMEOUT */
|
||||||
|
|
||||||
|
|
||||||
/* Tip: To avoid modifying this file each time you need to use different HSE,
|
/* Tip: To avoid modifying this file each time you need to use different HSE,
|
||||||
=== you can define the HSE value in your toolchain compiler preprocessor. */
|
=== you can define the HSE value in your toolchain compiler preprocessor. */
|
||||||
|
|
||||||
/* ########################### System Configuration ######################### */
|
/* ########################### System Configuration ######################### */
|
||||||
/**
|
/**
|
||||||
* @brief This is the HAL system configuration section
|
* @brief This is the HAL system configuration section
|
||||||
*/
|
*/
|
||||||
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
|
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
|
||||||
#define TICK_INT_PRIORITY ((uint32_t)0x000F) /*!< tick interrupt priority */
|
#define TICK_INT_PRIORITY ((uint32_t)0x000F) /*!< tick interrupt priority */
|
||||||
#define USE_RTOS 0
|
#define USE_RTOS 0
|
||||||
#define PREFETCH_ENABLE 1
|
#define PREFETCH_ENABLE 1
|
||||||
|
|
||||||
/* ########################## Assert Selection ############################## */
|
/* ########################## Assert Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
||||||
* HAL drivers code
|
* HAL drivers code
|
||||||
*/
|
*/
|
||||||
/*#define USE_FULL_ASSERT 1*/
|
/*#define USE_FULL_ASSERT 1*/
|
||||||
|
|
||||||
|
|
||||||
/* ################## Ethernet peripheral configuration ##################### */
|
/* ################## Ethernet peripheral configuration ##################### */
|
||||||
|
@ -152,7 +152,7 @@
|
||||||
#define MAC_ADDR4 0
|
#define MAC_ADDR4 0
|
||||||
#define MAC_ADDR5 0
|
#define MAC_ADDR5 0
|
||||||
|
|
||||||
/* Definition of the Ethernet driver buffers size and count */
|
/* Definition of the Ethernet driver buffers size and count */
|
||||||
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
|
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
|
||||||
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
|
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
|
||||||
#define ETH_RXBUFNB ((uint32_t)8) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
|
#define ETH_RXBUFNB ((uint32_t)8) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
|
||||||
|
@ -160,9 +160,9 @@
|
||||||
|
|
||||||
/* Section 2: PHY configuration section */
|
/* Section 2: PHY configuration section */
|
||||||
|
|
||||||
/* DP83848 PHY Address*/
|
/* DP83848 PHY Address*/
|
||||||
#define DP83848_PHY_ADDRESS 0x01
|
#define DP83848_PHY_ADDRESS 0x01
|
||||||
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
|
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
|
||||||
#define PHY_RESET_DELAY ((uint32_t)0x000000FF)
|
#define PHY_RESET_DELAY ((uint32_t)0x000000FF)
|
||||||
/* PHY Configuration delay */
|
/* PHY Configuration delay */
|
||||||
#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFF)
|
#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFF)
|
||||||
|
@ -174,7 +174,7 @@
|
||||||
|
|
||||||
#define PHY_BCR ((uint16_t)0x00) /*!< Transceiver Basic Control Register */
|
#define PHY_BCR ((uint16_t)0x00) /*!< Transceiver Basic Control Register */
|
||||||
#define PHY_BSR ((uint16_t)0x01) /*!< Transceiver Basic Status Register */
|
#define PHY_BSR ((uint16_t)0x01) /*!< Transceiver Basic Status Register */
|
||||||
|
|
||||||
#define PHY_RESET ((uint16_t)0x8000) /*!< PHY Reset */
|
#define PHY_RESET ((uint16_t)0x8000) /*!< PHY Reset */
|
||||||
#define PHY_LOOPBACK ((uint16_t)0x4000) /*!< Select loop-back mode */
|
#define PHY_LOOPBACK ((uint16_t)0x4000) /*!< Select loop-back mode */
|
||||||
#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100) /*!< Set the full-duplex mode at 100 Mb/s */
|
#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100) /*!< Set the full-duplex mode at 100 Mb/s */
|
||||||
|
@ -195,7 +195,7 @@
|
||||||
#define PHY_SR ((uint16_t)0x10) /*!< PHY status register Offset */
|
#define PHY_SR ((uint16_t)0x10) /*!< PHY status register Offset */
|
||||||
#define PHY_MICR ((uint16_t)0x11) /*!< MII Interrupt Control Register */
|
#define PHY_MICR ((uint16_t)0x11) /*!< MII Interrupt Control Register */
|
||||||
#define PHY_MISR ((uint16_t)0x12) /*!< MII Interrupt Status and Misc. Control Register */
|
#define PHY_MISR ((uint16_t)0x12) /*!< MII Interrupt Status and Misc. Control Register */
|
||||||
|
|
||||||
#define PHY_LINK_STATUS ((uint16_t)0x0001) /*!< PHY Link mask */
|
#define PHY_LINK_STATUS ((uint16_t)0x0001) /*!< PHY Link mask */
|
||||||
#define PHY_SPEED_STATUS ((uint16_t)0x0002) /*!< PHY Speed mask */
|
#define PHY_SPEED_STATUS ((uint16_t)0x0002) /*!< PHY Speed mask */
|
||||||
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004) /*!< PHY Duplex mask */
|
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004) /*!< PHY Duplex mask */
|
||||||
|
@ -210,7 +210,7 @@
|
||||||
|
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
/**
|
/**
|
||||||
* @brief Include module's header file
|
* @brief Include module's header file
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifdef HAL_RCC_MODULE_ENABLED
|
#ifdef HAL_RCC_MODULE_ENABLED
|
||||||
|
@ -220,15 +220,15 @@
|
||||||
#ifdef HAL_GPIO_MODULE_ENABLED
|
#ifdef HAL_GPIO_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_gpio.h"
|
#include "stm32f1xx_hal_gpio.h"
|
||||||
#endif /* HAL_GPIO_MODULE_ENABLED */
|
#endif /* HAL_GPIO_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_DMA_MODULE_ENABLED
|
#ifdef HAL_DMA_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_dma.h"
|
#include "stm32f1xx_hal_dma.h"
|
||||||
#endif /* HAL_DMA_MODULE_ENABLED */
|
#endif /* HAL_DMA_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_ETH_MODULE_ENABLED
|
#ifdef HAL_ETH_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_eth.h"
|
#include "stm32f1xx_hal_eth.h"
|
||||||
#endif /* HAL_ETH_MODULE_ENABLED */
|
#endif /* HAL_ETH_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_CAN_MODULE_ENABLED
|
#ifdef HAL_CAN_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_can.h"
|
#include "stm32f1xx_hal_can.h"
|
||||||
#endif /* HAL_CAN_MODULE_ENABLED */
|
#endif /* HAL_CAN_MODULE_ENABLED */
|
||||||
|
@ -287,7 +287,7 @@
|
||||||
|
|
||||||
#ifdef HAL_PCCARD_MODULE_ENABLED
|
#ifdef HAL_PCCARD_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_pccard.h"
|
#include "stm32f1xx_hal_pccard.h"
|
||||||
#endif /* HAL_PCCARD_MODULE_ENABLED */
|
#endif /* HAL_PCCARD_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_SD_MODULE_ENABLED
|
#ifdef HAL_SD_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_sd.h"
|
#include "stm32f1xx_hal_sd.h"
|
||||||
|
@ -295,7 +295,7 @@
|
||||||
|
|
||||||
#ifdef HAL_NAND_MODULE_ENABLED
|
#ifdef HAL_NAND_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_nand.h"
|
#include "stm32f1xx_hal_nand.h"
|
||||||
#endif /* HAL_NAND_MODULE_ENABLED */
|
#endif /* HAL_NAND_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_SPI_MODULE_ENABLED
|
#ifdef HAL_SPI_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_spi.h"
|
#include "stm32f1xx_hal_spi.h"
|
||||||
|
@ -332,8 +332,8 @@
|
||||||
|
|
||||||
#ifdef HAL_HCD_MODULE_ENABLED
|
#ifdef HAL_HCD_MODULE_ENABLED
|
||||||
#include "stm32f1xx_hal_hcd.h"
|
#include "stm32f1xx_hal_hcd.h"
|
||||||
#endif /* HAL_HCD_MODULE_ENABLED */
|
#endif /* HAL_HCD_MODULE_ENABLED */
|
||||||
|
|
||||||
|
|
||||||
/* Exported macro ------------------------------------------------------------*/
|
/* Exported macro ------------------------------------------------------------*/
|
||||||
#ifdef USE_FULL_ASSERT
|
#ifdef USE_FULL_ASSERT
|
||||||
|
@ -341,7 +341,7 @@
|
||||||
* @brief The assert_param macro is used for function's parameters check.
|
* @brief The assert_param macro is used for function's parameters check.
|
||||||
* @param expr: If expr is false, it calls assert_failed function
|
* @param expr: If expr is false, it calls assert_failed function
|
||||||
* which reports the name of the source file and the source
|
* which reports the name of the source file and the source
|
||||||
* line number of the call that failed.
|
* line number of the call that failed.
|
||||||
* If expr is true, it returns no value.
|
* If expr is true, it returns no value.
|
||||||
* @retval None
|
* @retval None
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -30,7 +30,7 @@
|
||||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
*
|
*
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||||
#ifndef __STM32F3xx_HAL_CONF_H
|
#ifndef __STM32F3xx_HAL_CONF_H
|
||||||
|
@ -45,7 +45,7 @@
|
||||||
|
|
||||||
/* ########################## Module Selection ############################## */
|
/* ########################## Module Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief This is the list of modules to be used in the HAL driver
|
* @brief This is the list of modules to be used in the HAL driver
|
||||||
*/
|
*/
|
||||||
#define HAL_MODULE_ENABLED
|
#define HAL_MODULE_ENABLED
|
||||||
#define HAL_ADC_MODULE_ENABLED
|
#define HAL_ADC_MODULE_ENABLED
|
||||||
|
@ -88,13 +88,13 @@
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSE is used as system clock source, directly or through the PLL).
|
* (when HSE is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined (HSE_VALUE)
|
#if !defined (HSE_VALUE)
|
||||||
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
|
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
|
||||||
#endif /* HSE_VALUE */
|
#endif /* HSE_VALUE */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief In the following line adjust the External High Speed oscillator (HSE) Startup
|
* @brief In the following line adjust the External High Speed oscillator (HSE) Startup
|
||||||
* Timeout value
|
* Timeout value
|
||||||
*/
|
*/
|
||||||
#if !defined (HSE_STARTUP_TIMEOUT)
|
#if !defined (HSE_STARTUP_TIMEOUT)
|
||||||
#define HSE_STARTUP_TIMEOUT ((uint32_t)100) /*!< Time out for HSE start up, in ms */
|
#define HSE_STARTUP_TIMEOUT ((uint32_t)100) /*!< Time out for HSE start up, in ms */
|
||||||
|
@ -103,24 +103,24 @@
|
||||||
/**
|
/**
|
||||||
* @brief Internal High Speed oscillator (HSI) value.
|
* @brief Internal High Speed oscillator (HSI) value.
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSI is used as system clock source, directly or through the PLL).
|
* (when HSI is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined (HSI_VALUE)
|
#if !defined (HSI_VALUE)
|
||||||
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
|
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
|
||||||
#endif /* HSI_VALUE */
|
#endif /* HSI_VALUE */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief In the following line adjust the Internal High Speed oscillator (HSI) Startup
|
* @brief In the following line adjust the Internal High Speed oscillator (HSI) Startup
|
||||||
* Timeout value
|
* Timeout value
|
||||||
*/
|
*/
|
||||||
#if !defined (HSI_STARTUP_TIMEOUT)
|
#if !defined (HSI_STARTUP_TIMEOUT)
|
||||||
#define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */
|
#define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */
|
||||||
#endif /* HSI_STARTUP_TIMEOUT */
|
#endif /* HSI_STARTUP_TIMEOUT */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Internal Low Speed oscillator (LSI) value.
|
* @brief Internal Low Speed oscillator (LSI) value.
|
||||||
*/
|
*/
|
||||||
#if !defined (LSI_VALUE)
|
#if !defined (LSI_VALUE)
|
||||||
#define LSI_VALUE ((uint32_t)40000)
|
#define LSI_VALUE ((uint32_t)40000)
|
||||||
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
|
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
|
||||||
The real value may vary depending on the variations
|
The real value may vary depending on the variations
|
||||||
|
@ -130,7 +130,7 @@
|
||||||
*/
|
*/
|
||||||
#if !defined (LSE_VALUE)
|
#if !defined (LSE_VALUE)
|
||||||
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */
|
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */
|
||||||
#endif /* LSE_VALUE */
|
#endif /* LSE_VALUE */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Time out for LSE start up value in ms.
|
* @brief Time out for LSE start up value in ms.
|
||||||
|
@ -141,7 +141,7 @@
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief External clock source for I2S peripheral
|
* @brief External clock source for I2S peripheral
|
||||||
* This value is used by the I2S HAL module to compute the I2S clock source
|
* This value is used by the I2S HAL module to compute the I2S clock source
|
||||||
* frequency, this source is inserted directly through I2S_CKIN pad.
|
* frequency, this source is inserted directly through I2S_CKIN pad.
|
||||||
* - External clock generated through external PLL component on EVAL 303 (based on MCO or crystal)
|
* - External clock generated through external PLL component on EVAL 303 (based on MCO or crystal)
|
||||||
* - External clock not generated on EVAL 373
|
* - External clock not generated on EVAL 373
|
||||||
|
@ -156,7 +156,7 @@
|
||||||
/* ########################### System Configuration ######################### */
|
/* ########################### System Configuration ######################### */
|
||||||
/**
|
/**
|
||||||
* @brief This is the HAL system configuration section
|
* @brief This is the HAL system configuration section
|
||||||
*/
|
*/
|
||||||
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
|
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
|
||||||
#define TICK_INT_PRIORITY ((uint32_t)(1<<__NVIC_PRIO_BITS) - 1) /*!< tick interrupt priority (lowest by default) */
|
#define TICK_INT_PRIORITY ((uint32_t)(1<<__NVIC_PRIO_BITS) - 1) /*!< tick interrupt priority (lowest by default) */
|
||||||
#define USE_RTOS 0
|
#define USE_RTOS 0
|
||||||
|
@ -166,14 +166,14 @@
|
||||||
|
|
||||||
/* ########################## Assert Selection ############################## */
|
/* ########################## Assert Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
||||||
* HAL drivers code
|
* HAL drivers code
|
||||||
*/
|
*/
|
||||||
/*#define USE_FULL_ASSERT 1*/
|
/*#define USE_FULL_ASSERT 1*/
|
||||||
|
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
/**
|
/**
|
||||||
* @brief Include module's header file
|
* @brief Include module's header file
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifdef HAL_RCC_MODULE_ENABLED
|
#ifdef HAL_RCC_MODULE_ENABLED
|
||||||
|
@ -187,7 +187,7 @@
|
||||||
#ifdef HAL_DMA_MODULE_ENABLED
|
#ifdef HAL_DMA_MODULE_ENABLED
|
||||||
#include "stm32f3xx_hal_dma.h"
|
#include "stm32f3xx_hal_dma.h"
|
||||||
#endif /* HAL_DMA_MODULE_ENABLED */
|
#endif /* HAL_DMA_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_CORTEX_MODULE_ENABLED
|
#ifdef HAL_CORTEX_MODULE_ENABLED
|
||||||
#include "stm32f3xx_hal_cortex.h"
|
#include "stm32f3xx_hal_cortex.h"
|
||||||
#endif /* HAL_CORTEX_MODULE_ENABLED */
|
#endif /* HAL_CORTEX_MODULE_ENABLED */
|
||||||
|
@ -234,7 +234,7 @@
|
||||||
|
|
||||||
#ifdef HAL_PCCARD_MODULE_ENABLED
|
#ifdef HAL_PCCARD_MODULE_ENABLED
|
||||||
#include "stm32f3xx_hal_pccard.h"
|
#include "stm32f3xx_hal_pccard.h"
|
||||||
#endif /* HAL_PCCARD_MODULE_ENABLED */
|
#endif /* HAL_PCCARD_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_HRTIM_MODULE_ENABLED
|
#ifdef HAL_HRTIM_MODULE_ENABLED
|
||||||
#include "stm32f3xx_hal_hrtim.h"
|
#include "stm32f3xx_hal_hrtim.h"
|
||||||
|
@ -314,7 +314,7 @@
|
||||||
* @brief The assert_param macro is used for function's parameters check.
|
* @brief The assert_param macro is used for function's parameters check.
|
||||||
* @param expr: If expr is false, it calls assert_failed function
|
* @param expr: If expr is false, it calls assert_failed function
|
||||||
* which reports the name of the source file and the source
|
* which reports the name of the source file and the source
|
||||||
* line number of the call that failed.
|
* line number of the call that failed.
|
||||||
* If expr is true, it returns no value.
|
* If expr is true, it returns no value.
|
||||||
* @retval None
|
* @retval None
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @file stm32f4xx_hal_conf.h
|
* @file stm32f4xx_hal_conf.h
|
||||||
* @brief HAL configuration template file.
|
* @brief HAL configuration template file.
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @attention
|
* @attention
|
||||||
*
|
*
|
||||||
|
@ -30,7 +30,7 @@
|
||||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
*
|
*
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||||
#ifndef __STM32F4xx_HAL_CONF_H
|
#ifndef __STM32F4xx_HAL_CONF_H
|
||||||
|
@ -45,7 +45,7 @@
|
||||||
|
|
||||||
/* ########################## Module Selection ############################## */
|
/* ########################## Module Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief This is the list of modules to be used in the HAL driver
|
* @brief This is the list of modules to be used in the HAL driver
|
||||||
*/
|
*/
|
||||||
#define HAL_MODULE_ENABLED
|
#define HAL_MODULE_ENABLED
|
||||||
#define HAL_ADC_MODULE_ENABLED
|
#define HAL_ADC_MODULE_ENABLED
|
||||||
|
@ -99,7 +99,7 @@
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSE is used as system clock source, directly or through the PLL).
|
* (when HSE is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined (HSE_VALUE)
|
#if !defined (HSE_VALUE)
|
||||||
#define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */
|
#define HSE_VALUE ((uint32_t)8000000U) /*!< Value of the External oscillator in Hz */
|
||||||
#endif /* HSE_VALUE */
|
#endif /* HSE_VALUE */
|
||||||
|
|
||||||
|
@ -110,7 +110,7 @@
|
||||||
/**
|
/**
|
||||||
* @brief Internal High Speed oscillator (HSI) value.
|
* @brief Internal High Speed oscillator (HSI) value.
|
||||||
* This value is used by the RCC HAL module to compute the system frequency
|
* This value is used by the RCC HAL module to compute the system frequency
|
||||||
* (when HSI is used as system clock source, directly or through the PLL).
|
* (when HSI is used as system clock source, directly or through the PLL).
|
||||||
*/
|
*/
|
||||||
#if !defined (HSI_VALUE)
|
#if !defined (HSI_VALUE)
|
||||||
#define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/
|
#define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/
|
||||||
|
@ -119,7 +119,7 @@
|
||||||
/**
|
/**
|
||||||
* @brief Internal Low Speed oscillator (LSI) value.
|
* @brief Internal Low Speed oscillator (LSI) value.
|
||||||
*/
|
*/
|
||||||
#if !defined (LSI_VALUE)
|
#if !defined (LSI_VALUE)
|
||||||
#define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/
|
#define LSI_VALUE ((uint32_t)32000U) /*!< LSI Typical Value in Hz*/
|
||||||
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
|
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
|
||||||
The real value may vary depending on the variations
|
The real value may vary depending on the variations
|
||||||
|
@ -137,8 +137,8 @@
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief External clock source for I2S peripheral
|
* @brief External clock source for I2S peripheral
|
||||||
* This value is used by the I2S HAL module to compute the I2S clock source
|
* This value is used by the I2S HAL module to compute the I2S clock source
|
||||||
* frequency, this source is inserted directly through I2S_CKIN pad.
|
* frequency, this source is inserted directly through I2S_CKIN pad.
|
||||||
*/
|
*/
|
||||||
#if !defined (EXTERNAL_CLOCK_VALUE)
|
#if !defined (EXTERNAL_CLOCK_VALUE)
|
||||||
#define EXTERNAL_CLOCK_VALUE ((uint32_t)12288000U) /*!< Value of the Internal oscillator in Hz*/
|
#define EXTERNAL_CLOCK_VALUE ((uint32_t)12288000U) /*!< Value of the Internal oscillator in Hz*/
|
||||||
|
@ -150,7 +150,7 @@
|
||||||
/* ########################### System Configuration ######################### */
|
/* ########################### System Configuration ######################### */
|
||||||
/**
|
/**
|
||||||
* @brief This is the HAL system configuration section
|
* @brief This is the HAL system configuration section
|
||||||
*/
|
*/
|
||||||
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
|
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
|
||||||
#define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */
|
#define TICK_INT_PRIORITY ((uint32_t)0x0FU) /*!< tick interrupt priority */
|
||||||
#define USE_RTOS 0U
|
#define USE_RTOS 0U
|
||||||
|
@ -160,7 +160,7 @@
|
||||||
|
|
||||||
/* ########################## Assert Selection ############################## */
|
/* ########################## Assert Selection ############################## */
|
||||||
/**
|
/**
|
||||||
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
* @brief Uncomment the line below to expanse the "assert_param" macro in the
|
||||||
* HAL drivers code
|
* HAL drivers code
|
||||||
*/
|
*/
|
||||||
/* #define USE_FULL_ASSERT 1U */
|
/* #define USE_FULL_ASSERT 1U */
|
||||||
|
@ -177,7 +177,7 @@
|
||||||
#define MAC_ADDR4 0U
|
#define MAC_ADDR4 0U
|
||||||
#define MAC_ADDR5 0U
|
#define MAC_ADDR5 0U
|
||||||
|
|
||||||
/* Definition of the Ethernet driver buffers size and count */
|
/* Definition of the Ethernet driver buffers size and count */
|
||||||
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
|
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
|
||||||
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
|
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
|
||||||
#define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
|
#define ETH_RXBUFNB ((uint32_t)4U) /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
|
||||||
|
@ -185,9 +185,9 @@
|
||||||
|
|
||||||
/* Section 2: PHY configuration section */
|
/* Section 2: PHY configuration section */
|
||||||
|
|
||||||
/* DP83848 PHY Address*/
|
/* DP83848 PHY Address*/
|
||||||
#define DP83848_PHY_ADDRESS 0x01U
|
#define DP83848_PHY_ADDRESS 0x01U
|
||||||
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
|
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
|
||||||
#define PHY_RESET_DELAY ((uint32_t)0x000000FFU)
|
#define PHY_RESET_DELAY ((uint32_t)0x000000FFU)
|
||||||
/* PHY Configuration delay */
|
/* PHY Configuration delay */
|
||||||
#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFFU)
|
#define PHY_CONFIG_DELAY ((uint32_t)0x00000FFFU)
|
||||||
|
@ -199,7 +199,7 @@
|
||||||
|
|
||||||
#define PHY_BCR ((uint16_t)0x0000U) /*!< Transceiver Basic Control Register */
|
#define PHY_BCR ((uint16_t)0x0000U) /*!< Transceiver Basic Control Register */
|
||||||
#define PHY_BSR ((uint16_t)0x0001U) /*!< Transceiver Basic Status Register */
|
#define PHY_BSR ((uint16_t)0x0001U) /*!< Transceiver Basic Status Register */
|
||||||
|
|
||||||
#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */
|
#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */
|
||||||
#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */
|
#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */
|
||||||
#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */
|
#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */
|
||||||
|
@ -220,7 +220,7 @@
|
||||||
#define PHY_SR ((uint16_t)0x0010U) /*!< PHY status register Offset */
|
#define PHY_SR ((uint16_t)0x0010U) /*!< PHY status register Offset */
|
||||||
#define PHY_MICR ((uint16_t)0x0011U) /*!< MII Interrupt Control Register */
|
#define PHY_MICR ((uint16_t)0x0011U) /*!< MII Interrupt Control Register */
|
||||||
#define PHY_MISR ((uint16_t)0x0012U) /*!< MII Interrupt Status and Misc. Control Register */
|
#define PHY_MISR ((uint16_t)0x0012U) /*!< MII Interrupt Status and Misc. Control Register */
|
||||||
|
|
||||||
#define PHY_LINK_STATUS ((uint16_t)0x0001U) /*!< PHY Link mask */
|
#define PHY_LINK_STATUS ((uint16_t)0x0001U) /*!< PHY Link mask */
|
||||||
#define PHY_SPEED_STATUS ((uint16_t)0x0002U) /*!< PHY Speed mask */
|
#define PHY_SPEED_STATUS ((uint16_t)0x0002U) /*!< PHY Speed mask */
|
||||||
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004U) /*!< PHY Duplex mask */
|
#define PHY_DUPLEX_STATUS ((uint16_t)0x0004U) /*!< PHY Duplex mask */
|
||||||
|
@ -242,7 +242,7 @@
|
||||||
|
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
/**
|
/**
|
||||||
* @brief Include module's header file
|
* @brief Include module's header file
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifdef HAL_RCC_MODULE_ENABLED
|
#ifdef HAL_RCC_MODULE_ENABLED
|
||||||
|
@ -256,7 +256,7 @@
|
||||||
#ifdef HAL_DMA_MODULE_ENABLED
|
#ifdef HAL_DMA_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_dma.h"
|
#include "stm32f4xx_hal_dma.h"
|
||||||
#endif /* HAL_DMA_MODULE_ENABLED */
|
#endif /* HAL_DMA_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_CORTEX_MODULE_ENABLED
|
#ifdef HAL_CORTEX_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_cortex.h"
|
#include "stm32f4xx_hal_cortex.h"
|
||||||
#endif /* HAL_CORTEX_MODULE_ENABLED */
|
#endif /* HAL_CORTEX_MODULE_ENABLED */
|
||||||
|
@ -274,7 +274,7 @@
|
||||||
#endif /* HAL_CRC_MODULE_ENABLED */
|
#endif /* HAL_CRC_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_CRYP_MODULE_ENABLED
|
#ifdef HAL_CRYP_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_cryp.h"
|
#include "stm32f4xx_hal_cryp.h"
|
||||||
#endif /* HAL_CRYP_MODULE_ENABLED */
|
#endif /* HAL_CRYP_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_DMA2D_MODULE_ENABLED
|
#ifdef HAL_DMA2D_MODULE_ENABLED
|
||||||
|
@ -296,7 +296,7 @@
|
||||||
#ifdef HAL_FLASH_MODULE_ENABLED
|
#ifdef HAL_FLASH_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_flash.h"
|
#include "stm32f4xx_hal_flash.h"
|
||||||
#endif /* HAL_FLASH_MODULE_ENABLED */
|
#endif /* HAL_FLASH_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_SRAM_MODULE_ENABLED
|
#ifdef HAL_SRAM_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_sram.h"
|
#include "stm32f4xx_hal_sram.h"
|
||||||
#endif /* HAL_SRAM_MODULE_ENABLED */
|
#endif /* HAL_SRAM_MODULE_ENABLED */
|
||||||
|
@ -311,7 +311,7 @@
|
||||||
|
|
||||||
#ifdef HAL_PCCARD_MODULE_ENABLED
|
#ifdef HAL_PCCARD_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_pccard.h"
|
#include "stm32f4xx_hal_pccard.h"
|
||||||
#endif /* HAL_PCCARD_MODULE_ENABLED */
|
#endif /* HAL_PCCARD_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_SDRAM_MODULE_ENABLED
|
#ifdef HAL_SDRAM_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_sdram.h"
|
#include "stm32f4xx_hal_sdram.h"
|
||||||
|
@ -392,7 +392,7 @@
|
||||||
#ifdef HAL_HCD_MODULE_ENABLED
|
#ifdef HAL_HCD_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_hcd.h"
|
#include "stm32f4xx_hal_hcd.h"
|
||||||
#endif /* HAL_HCD_MODULE_ENABLED */
|
#endif /* HAL_HCD_MODULE_ENABLED */
|
||||||
|
|
||||||
#ifdef HAL_DSI_MODULE_ENABLED
|
#ifdef HAL_DSI_MODULE_ENABLED
|
||||||
#include "stm32f4xx_hal_dsi.h"
|
#include "stm32f4xx_hal_dsi.h"
|
||||||
#endif /* HAL_DSI_MODULE_ENABLED */
|
#endif /* HAL_DSI_MODULE_ENABLED */
|
||||||
|
@ -427,7 +427,7 @@
|
||||||
* @brief The assert_param macro is used for function's parameters check.
|
* @brief The assert_param macro is used for function's parameters check.
|
||||||
* @param expr: If expr is false, it calls assert_failed function
|
* @param expr: If expr is false, it calls assert_failed function
|
||||||
* which reports the name of the source file and the source
|
* which reports the name of the source file and the source
|
||||||
* line number of the call that failed.
|
* line number of the call that failed.
|
||||||
* If expr is true, it returns no value.
|
* If expr is true, it returns no value.
|
||||||
* @retval None
|
* @retval None
|
||||||
*/
|
*/
|
||||||
|
@ -444,6 +444,6 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#endif /* __STM32F4xx_HAL_CONF_H */
|
#endif /* __STM32F4xx_HAL_CONF_H */
|
||||||
|
|
||||||
|
|
||||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
||||||
|
|
|
@ -283,7 +283,7 @@ void SystemInit(void)
|
||||||
if (DATA_CACHE_ENABLE) {
|
if (DATA_CACHE_ENABLE) {
|
||||||
SCB_EnableDCache();
|
SCB_EnableDCache();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Configure the system clock to 216 MHz */
|
/* Configure the system clock to 216 MHz */
|
||||||
SystemClock_Config();
|
SystemClock_Config();
|
||||||
|
|
||||||
|
|
|
@ -134,7 +134,7 @@ static uint8_t cycleNum = 0;
|
||||||
static void sendDataHead(uint8_t id)
|
static void sendDataHead(uint8_t id)
|
||||||
{
|
{
|
||||||
frSkyTelemetryWrite(PROTOCOL_HEADER);
|
frSkyTelemetryWrite(PROTOCOL_HEADER);
|
||||||
frSkyTelemetryWrite(id);
|
frSkyTelemetryWrite(id);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void sendTelemetryTail(void)
|
static void sendTelemetryTail(void)
|
||||||
|
@ -147,7 +147,7 @@ static void serializeFrsky(uint8_t data)
|
||||||
// take care of byte stuffing
|
// take care of byte stuffing
|
||||||
if (data == 0x5e) {
|
if (data == 0x5e) {
|
||||||
frSkyTelemetryWrite(0x5d);
|
frSkyTelemetryWrite(0x5d);
|
||||||
frSkyTelemetryWrite(0x3e);
|
frSkyTelemetryWrite(0x3e);
|
||||||
} else if (data == 0x5d) {
|
} else if (data == 0x5d) {
|
||||||
frSkyTelemetryWrite(0x5d);
|
frSkyTelemetryWrite(0x5d);
|
||||||
frSkyTelemetryWrite(0x3d);
|
frSkyTelemetryWrite(0x3d);
|
||||||
|
@ -463,7 +463,7 @@ void frSkyTelemetryWriteSerial(uint8_t ch)
|
||||||
{
|
{
|
||||||
serialWrite(frskyPort, ch);
|
serialWrite(frskyPort, ch);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void initFrSkyTelemetry(void)
|
void initFrSkyTelemetry(void)
|
||||||
{
|
{
|
||||||
|
|
|
@ -61,7 +61,7 @@ typedef enum {
|
||||||
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03
|
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03
|
||||||
} ibusSensorType_e;
|
} ibusSensorType_e;
|
||||||
|
|
||||||
/* Address lookup relative to the sensor base address which is the lowest address seen by the FC
|
/* Address lookup relative to the sensor base address which is the lowest address seen by the FC
|
||||||
The actual lowest value is likely to change when sensors are daisy chained */
|
The actual lowest value is likely to change when sensors are daisy chained */
|
||||||
static const uint8_t sensorAddressTypeLookup[] = {
|
static const uint8_t sensorAddressTypeLookup[] = {
|
||||||
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE,
|
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE,
|
||||||
|
|
|
@ -19,8 +19,8 @@
|
||||||
* The ibus_shared module implements the ibus telemetry packet handling
|
* The ibus_shared module implements the ibus telemetry packet handling
|
||||||
* which is shared between the ibus serial rx and the ibus telemetry.
|
* which is shared between the ibus serial rx and the ibus telemetry.
|
||||||
*
|
*
|
||||||
* There is only one 'user' active at any time, serial rx will open the
|
* There is only one 'user' active at any time, serial rx will open the
|
||||||
* serial port if both functions are enabled at the same time
|
* serial port if both functions are enabled at the same time
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
|
@ -183,15 +183,15 @@ void srxlFrameRpm(sbuf_t *dst)
|
||||||
/*
|
/*
|
||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
UINT8 identifier; // Source device = 0x34
|
UINT8 identifier; // Source device = 0x34
|
||||||
UINT8 sID; // Secondary ID
|
UINT8 sID; // Secondary ID
|
||||||
INT16 current_A; // Instantaneous current, 0.1A (0-3276.8A)
|
INT16 current_A; // Instantaneous current, 0.1A (0-3276.8A)
|
||||||
INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah)
|
INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah)
|
||||||
UINT16 temp_A; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
|
UINT16 temp_A; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
|
||||||
INT16 current_B; // Instantaneous current, 0.1A (0-3276.8A)
|
INT16 current_B; // Instantaneous current, 0.1A (0-3276.8A)
|
||||||
INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah)
|
INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah)
|
||||||
UINT16 temp_B; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
|
UINT16 temp_B; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
|
||||||
UINT16 spare; // Not used
|
UINT16 spare; // Not used
|
||||||
} STRU_TELE_FP_MAH;
|
} STRU_TELE_FP_MAH;
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue