Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'multiwii/upstream' into upstream

Browse source
This commit is contained in:
Dominic Clifton 2013-08-24 11:31:23 +01:00
commit 92a68041a8
19 changed files with 460 additions and 377 deletions
Download patch file Download diff file Expand all files Collapse all files

102
src/cli.c
View file

@ -140,6 +140,7 @@ const clivalue_t valueTable[] = {
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
{ "throttle_angle_correction", VAR_UINT8, &cfg.throttle_angle_correction, 0, 100 },
{ "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },
@ -215,6 +216,8 @@ const clivalue_t valueTable[] = {
static void cliSetVar(const clivalue_t *var, const int32_t value);
static void cliPrintVar(const clivalue_t *var, uint32_t full);
static void cliPrint(const char *str);
static void cliWrite(uint8_t ch);
#ifndef HAVE_ITOA_FUNCTION
@ -395,7 +398,7 @@ static char *ftoa(float x, char *floatString)
static void cliPrompt(void)
{
uartPrint("\r\n# ");
cliPrint("\r\n# ");
}
static int cliCompare(const void *a, const void *b)
@ -440,7 +443,7 @@ static void cliCMix(char *cmdline)
len = strlen(cmdline);
if (len == 0) {
uartPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f)
break;
@ -457,10 +460,10 @@ static void cliCMix(char *cmdline)
mixsum[1] += mcfg.customMixer[i].pitch;
mixsum[2] += mcfg.customMixer[i].yaw;
}
uartPrint("Sanity check:\t");
cliPrint("Sanity check:\t");
for (i = 0; i < 3; i++)
uartPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
uartPrint("\r\n");
cliPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
cliPrint("\r\n");
return;
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
@ -472,7 +475,7 @@ static void cliCMix(char *cmdline)
len = strlen(++ptr);
for (i = 0; ; i++) {
if (mixerNames[i] == NULL) {
uartPrint("Invalid mixer type...\r\n");
cliPrint("Invalid mixer type...\r\n");
break;
}
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
@ -508,7 +511,7 @@ static void cliCMix(char *cmdline)
check++;
}
if (check != 4) {
uartPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
} else {
cliCMix("");
}
@ -520,9 +523,9 @@ static void cliCMix(char *cmdline)
static void cliDefaults(char *cmdline)
{
uartPrint("Resetting to defaults...\r\n");
cliPrint("Resetting to defaults...\r\n");
checkFirstTime(true);
uartPrint("Rebooting...");
cliPrint("Rebooting...");
delay(10);
systemReset(false);
}
@ -595,13 +598,13 @@ static void cliDump(char *cmdline)
setval = &valueTable[i];
printf("set %s = ", valueTable[i].name);
cliPrintVar(setval, 0);
uartPrint("\r\n");
cliPrint("\r\n");
}
}
static void cliExit(char *cmdline)
{
uartPrint("\r\nLeaving CLI mode...\r\n");
cliPrint("\r\nLeaving CLI mode...\r\n");
*cliBuffer = '\0';
bufferIndex = 0;
cliMode = 0;
@ -619,22 +622,22 @@ static void cliFeature(char *cmdline)
mask = featureMask();
if (len == 0) {
uartPrint("Enabled features: ");
cliPrint("Enabled features: ");
for (i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("%s ", featureNames[i]);
}
uartPrint("\r\n");
cliPrint("\r\n");
} else if (strncasecmp(cmdline, "list", len) == 0) {
uartPrint("Available features: ");
cliPrint("Available features: ");
for (i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
printf("%s ", featureNames[i]);
}
uartPrint("\r\n");
cliPrint("\r\n");
return;
} else {
bool remove = false;
@ -647,16 +650,16 @@ static void cliFeature(char *cmdline)
for (i = 0; ; i++) {
if (featureNames[i] == NULL) {
uartPrint("Invalid feature name...\r\n");
cliPrint("Invalid feature name...\r\n");
break;
}
if (strncasecmp(cmdline, featureNames[i], len) == 0) {
if (remove) {
featureClear(1 << i);
uartPrint("Disabled ");
cliPrint("Disabled ");
} else {
featureSet(1 << i);
uartPrint("Enabled ");
cliPrint("Enabled ");
}
printf("%s\r\n", featureNames[i]);
break;
@ -669,7 +672,7 @@ static void cliHelp(char *cmdline)
{
uint32_t i = 0;
uartPrint("Available commands:\r\n");
cliPrint("Available commands:\r\n");
for (i = 0; i < CMD_COUNT; i++)
printf("%s\t%s\r\n", cmdTable[i].name, cmdTable[i].param);
}
@ -689,12 +692,12 @@ static void cliMap(char *cmdline)
for (i = 0; i < 8; i++) {
if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
continue;
uartPrint("Must be any order of AETR1234\r\n");
cliPrint("Must be any order of AETR1234\r\n");
return;
}
parseRcChannels(cmdline);
}
uartPrint("Current assignment: ");
cliPrint("Current assignment: ");
for (i = 0; i < 8; i++)
out[mcfg.rcmap[i]] = rcChannelLetters[i];
out[i] = '\0';
@ -712,19 +715,19 @@ static void cliMixer(char *cmdline)
printf("Current mixer: %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
return;
} else if (strncasecmp(cmdline, "list", len) == 0) {
uartPrint("Available mixers: ");
cliPrint("Available mixers: ");
for (i = 0; ; i++) {
if (mixerNames[i] == NULL)
break;
printf("%s ", mixerNames[i]);
}
uartPrint("\r\n");
cliPrint("\r\n");
return;
}
for (i = 0; ; i++) {
if (mixerNames[i] == NULL) {
uartPrint("Invalid mixer type...\r\n");
cliPrint("Invalid mixer type...\r\n");
break;
}
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
@ -756,13 +759,24 @@ static void cliProfile(char *cmdline)
static void cliSave(char *cmdline)
{
uartPrint("Saving...");
cliPrint("Saving...");
writeEEPROM(0, true);
uartPrint("\r\nRebooting...");
cliPrint("\r\nRebooting...");
delay(10);
systemReset(false);
}
static void cliPrint(const char *str)
{
while (*str)
uartWrite(core.mainport, *(str++));
}
static void cliWrite(uint8_t ch)
{
uartWrite(core.mainport, ch);
}
static void cliPrintVar(const clivalue_t *var, uint32_t full)
{
int32_t value = 0;
@ -837,12 +851,12 @@ static void cliSet(char *cmdline)
len = strlen(cmdline);
if (len == 0 || (len == 1 && cmdline[0] == '*')) {
uartPrint("Current settings: \r\n");
cliPrint("Current settings: \r\n");
for (i = 0; i < VALUE_COUNT; i++) {
val = &valueTable[i];
printf("%s = ", valueTable[i].name);
cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
uartPrint("\r\n");
cliPrint("\r\n");
}
} else if ((eqptr = strstr(cmdline, "="))) {
// has equal, set var
@ -858,12 +872,12 @@ static void cliSet(char *cmdline)
printf("%s set to ", valueTable[i].name);
cliPrintVar(val, 0);
} else {
uartPrint("ERR: Value assignment out of range\r\n");
cliPrint("ERR: Value assignment out of range\r\n");
}
return;
}
}
uartPrint("ERR: Unknown variable name\r\n");
cliPrint("ERR: Unknown variable name\r\n");
} else {
// no equals, check for matching variables.
for (i = 0; i < VALUE_COUNT; i++) {
@ -898,26 +912,26 @@ static void cliStatus(char *cmdline)
if (accHardware == ACC_MPU6050)
printf(".%c", mcfg.mpu6050_scale ? 'o' : 'n');
}
uartPrint("\r\n");
cliPrint("\r\n");
printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cGetErrorCounter(), sizeof(master_t));
}
static void cliVersion(char *cmdline)
{
uartPrint("Afro32 CLI version 2.1 " __DATE__ " / " __TIME__);
cliPrint("Afro32 CLI version 2.2 " __DATE__ " / " __TIME__);
}
void cliProcess(void)
{
if (!cliMode) {
cliMode = 1;
uartPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrompt();
}
while (isUartAvailable()) {
uint8_t c = uartRead();
while (isUartAvailable(core.mainport)) {
uint8_t c = uartRead(core.mainport);
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
@ -944,28 +958,28 @@ void cliProcess(void)
}
if (!bufferIndex || pstart != pend) {
/* Print list of ambiguous matches */
uartPrint("\r\033[K");
cliPrint("\r\033[K");
for (cmd = pstart; cmd <= pend; cmd++) {
uartPrint(cmd->name);
uartWrite('\t');
cliPrint(cmd->name);
cliWrite('\t');
}
cliPrompt();
i = 0; /* Redraw prompt */
}
for (; i < bufferIndex; i++)
uartWrite(cliBuffer[i]);
cliWrite(cliBuffer[i]);
} else if (!bufferIndex && c == 4) {
cliExit(cliBuffer);
return;
} else if (c == 12) {
// clear screen
uartPrint("\033[2J\033[1;1H");
cliPrint("\033[2J\033[1;1H");
cliPrompt();
} else if (bufferIndex && (c == '\n' || c == '\r')) {
// enter pressed
clicmd_t *cmd = NULL;
clicmd_t target;
uartPrint("\r\n");
cliPrint("\r\n");
cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
@ -975,7 +989,7 @@ void cliProcess(void)
if (cmd)
cmd->func(cliBuffer + strlen(cmd->name) + 1);
else
uartPrint("ERR: Unknown command, try 'help'");
cliPrint("ERR: Unknown command, try 'help'");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
@ -988,13 +1002,13 @@ void cliProcess(void)
// backspace
if (bufferIndex) {
cliBuffer[--bufferIndex] = 0;
uartPrint("\010 \010");
cliPrint("\010 \010");
}
} else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
if (!bufferIndex && c == 32)
continue;
cliBuffer[bufferIndex++] = c;
uartWrite(c);
cliWrite(c);
}
}
}

3
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
static uint8_t EEPROM_CONF_VERSION = 48;
static const uint8_t EEPROM_CONF_VERSION = 48;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -260,6 +260,7 @@ static void resetConf(void)
cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 40;
cfg.alt_hold_fast_change = 1;
cfg.throttle_angle_correction = 0; // could be 40
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec

4
src/drv_gpio.h
View file

@ -21,8 +21,8 @@ typedef enum
typedef enum
{
Pin_0 = 0x0000,
Pin_1 = 0x0001,
Pin_0 = 0x0001,
Pin_1 = 0x0002,
Pin_2 = 0x0004,
Pin_3 = 0x0008,
Pin_4 = 0x0010,

4
src/drv_hcsr04.c
View file

@ -19,7 +19,7 @@ static uint8_t exti_pin_source;
static IRQn_Type exti_irqn;
static uint32_t last_measurement;
static volatile int16_t *distance_ptr;
static volatile int32_t *distance_ptr;
void ECHO_EXTI_IRQHandler(void)
{
@ -106,7 +106,7 @@ void hcsr04_init(sonar_config_t config)
}
// distance calculation is done asynchronously, using interrupt
void hcsr04_get_distance(volatile int16_t *distance)
void hcsr04_get_distance(volatile int32_t *distance)
{
uint32_t current_time = millis();

2
src/drv_hcsr04.h
View file

@ -6,4 +6,4 @@ typedef enum {
} sonar_config_t;
void hcsr04_init(sonar_config_t config);
void hcsr04_get_distance(volatile int16_t* distance);
void hcsr04_get_distance(volatile int32_t *distance);

11
src/drv_pwm.c
View file

@ -264,15 +264,12 @@ static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
TIM_ICInit(tim, &TIM_ICInitStructure);
}
static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, uint8_t input)
static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
{
gpio_config_t cfg;
cfg.pin = pin;
if (input)
cfg.mode = Mode_IPD;
else
cfg.mode = Mode_AF_PP;
cfg.mode = mode;
cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
@ -281,7 +278,7 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
{
pwmPortData_t *p = &pwmPorts[port];
pwmTimeBase(timerHardware[port].tim, period);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, 0);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP);
pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
// Needed only on TIM1
if (timerHardware[port].outputEnable)
@ -309,7 +306,7 @@ static pwmPortData_t *pwmInConfig(uint8_t port, pwmCallbackPtr callback, uint8_t
{
pwmPortData_t *p = &pwmPorts[port];
pwmTimeBase(timerHardware[port].tim, 0xFFFF);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, 1);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_IPD);
pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
TIM_Cmd(timerHardware[port].tim, ENABLE);
pwmNVICConfig(timerHardware[port].irq);

5
src/drv_system.c
View file

@ -88,8 +88,11 @@ void systemInit(void)
LED1_OFF;
BEEP_OFF;
for (i = 0; i < gpio_count; i++)
for (i = 0; i < gpio_count; i++) {
if (hse_value == 12000000 && gpio_setup[i].cfg.mode == Mode_Out_OD)
gpio_setup[i].cfg.mode = Mode_Out_PP;
gpioInit(gpio_setup[i].gpio, &gpio_setup[i].cfg);
}
// Init cycle counter
cycleCounterInit();

429
src/drv_uart.c
View file

@ -1,59 +1,40 @@
#include "board.h"
/*
DMA UART routines idea lifted from AutoQuad
Copyright © 2011 Bill Nesbitt
*/
#define UART_BUFFER_SIZE 256
// Receive buffer, circular DMA
volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
volatile uint32_t rxDMAPos = 0;
volatile uint8_t txBuffer[UART_BUFFER_SIZE];
volatile uint32_t txBufferTail = 0;
volatile uint32_t txBufferHead = 0;
volatile bool txDMAEmpty = false;
static serialPort_t serialPort1;
static serialPort_t serialPort2;
static void uartTxDMA(void)
{
DMA1_Channel4->CMAR = (uint32_t)&txBuffer[txBufferTail];
if (txBufferHead > txBufferTail) {
DMA1_Channel4->CNDTR = txBufferHead - txBufferTail;
txBufferTail = txBufferHead;
} else {
DMA1_Channel4->CNDTR = UART_BUFFER_SIZE - txBufferTail;
txBufferTail = 0;
}
txDMAEmpty = false;
DMA_Cmd(DMA1_Channel4, ENABLE);
}
void DMA1_Channel4_IRQHandler(void)
{
DMA_ClearITPendingBit(DMA1_IT_TC4);
DMA_Cmd(DMA1_Channel4, DISABLE);
if (txBufferHead != txBufferTail)
uartTxDMA();
else
txDMAEmpty = true;
}
void uartInit(uint32_t speed)
// USART1 - Telemetry (RX/TX by DMA)
serialPort_t *serialUSART1(uint32_t baudRate, portmode_t mode)
{
serialPort_t *s;
static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
static volatile uint8_t tx1Buffer[UART1_TX_BUFFER_SIZE];
gpio_config_t gpio;
USART_InitTypeDef USART_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
s = &serialPort1;
s->rxBufferSize = UART1_RX_BUFFER_SIZE;
s->txBufferSize = UART1_TX_BUFFER_SIZE;
s->rxBuffer = rx1Buffer;
s->txBuffer = tx1Buffer;
s->rxDMAChannel = DMA1_Channel5;
s->txDMAChannel = DMA1_Channel4;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
// USART1_TX PA9
// USART1_RX PA10
gpio.pin = Pin_9;
gpio.speed = Speed_2MHz;
gpio.pin = Pin_9;
gpio.mode = Mode_AF_PP;
if (mode & MODE_TX)
gpioInit(GPIOA, &gpio);
gpio.pin = Pin_10;
gpio.mode = Mode_IPU;
if (mode & MODE_RX)
gpioInit(GPIOA, &gpio);
// DMA TX Interrupt
@ -63,191 +44,255 @@ void uartInit(uint32_t speed)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
USART_InitStructure.USART_BaudRate = speed;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
// Receive DMA into a circular buffer
DMA_DeInit(DMA1_Channel5);
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)rxBuffer;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_BufferSize = UART_BUFFER_SIZE;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_Init(DMA1_Channel5, &DMA_InitStructure);
DMA_Cmd(DMA1_Channel5, ENABLE);
USART_DMACmd(USART1, USART_DMAReq_Rx, ENABLE);
rxDMAPos = DMA_GetCurrDataCounter(DMA1_Channel5);
// Transmit DMA
DMA_DeInit(DMA1_Channel4);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USART1->DR;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_Init(DMA1_Channel4, &DMA_InitStructure);
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
DMA1_Channel4->CNDTR = 0;
USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
USART_Cmd(USART1, ENABLE);
return s;
}
bool isUartAvailable(void)
// USART2 - GPS or Spektrum or ?? (RX + TX by IRQ)
serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
{
return (DMA_GetCurrDataCounter(DMA1_Channel5) != rxDMAPos) ? true : false;
}
bool isUartTransmitDMAEmpty(void)
{
return txDMAEmpty;
}
bool isUartTransmitEmpty(void)
{
return (txBufferTail == txBufferHead);
}
uint8_t uartRead(void)
{
uint8_t ch;
ch = rxBuffer[UART_BUFFER_SIZE - rxDMAPos];
// go back around the buffer
if (--rxDMAPos == 0)
rxDMAPos = UART_BUFFER_SIZE;
return ch;
}
uint8_t uartReadPoll(void)
{
while (!isUartAvailable()); // wait for some bytes
return uartRead();
}
void uartWrite(uint8_t ch)
{
txBuffer[txBufferHead] = ch;
txBufferHead = (txBufferHead + 1) % UART_BUFFER_SIZE;
// if DMA wasn't enabled, fire it up
if (!(DMA1_Channel4->CCR & 1))
uartTxDMA();
}
void uartPrint(char *str)
{
while (*str)
uartWrite(*(str++));
}
/* -------------------------- UART2 (Spektrum, GPS) ----------------------------- */
uartReceiveCallbackPtr uart2Callback = NULL;
#define UART2_BUFFER_SIZE 128
volatile uint8_t tx2Buffer[UART2_BUFFER_SIZE];
uint32_t tx2BufferTail = 0;
uint32_t tx2BufferHead = 0;
bool uart2RxOnly = false;
static void uart2Open(uint32_t speed)
{
USART_InitTypeDef USART_InitStructure;
USART_StructInit(&USART_InitStructure);
USART_InitStructure.USART_BaudRate = speed;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_Mode = USART_Mode_Rx | (uart2RxOnly ? 0 : USART_Mode_Tx);
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_Init(USART2, &USART_InitStructure);
USART_Cmd(USART2, ENABLE);
}
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
{
NVIC_InitTypeDef NVIC_InitStructure;
serialPort_t *s;
static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
static volatile uint8_t tx2Buffer[UART2_TX_BUFFER_SIZE];
gpio_config_t gpio;
NVIC_InitTypeDef NVIC_InitStructure;
s = &serialPort2;
s->baudRate = baudRate;
s->rxBufferSize = UART2_RX_BUFFER_SIZE;
s->txBufferSize = UART2_TX_BUFFER_SIZE;
s->rxBuffer = rx2Buffer;
s->txBuffer = tx2Buffer;
s->USARTx = USART2;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
// USART2_TX PA2
// USART2_RX PA3
gpio.speed = Speed_2MHz;
gpio.pin = GPIO_Pin_2;
gpio.mode = Mode_AF_PP;
if (mode & MODE_TX)
gpioInit(GPIOA, &gpio);
gpio.pin = Pin_3;
gpio.mode = Mode_IPU;
if (mode & MODE_RX)
gpioInit(GPIOA, &gpio);
uart2RxOnly = rxOnly;
// RX/TX Interrupt
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// USART2_TX PA2
// USART2_RX PA3
gpio.pin = GPIO_Pin_2;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_AF_PP;
if (!rxOnly)
gpioInit(GPIOA, &gpio);
gpio.pin = Pin_3;
gpio.mode = Mode_IPU;
gpioInit(GPIOA, &gpio);
uart2Open(speed);
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
if (!rxOnly)
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
uart2Callback = func;
return s;
}
void uart2ChangeBaud(uint32_t speed)
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode)
{
uart2Open(speed);
DMA_InitTypeDef DMA_InitStructure;
USART_InitTypeDef USART_InitStructure;
serialPort_t *s = NULL;
if (USARTx == USART1)
s = serialUSART1(baudRate, mode);
if (USARTx == USART2)
s = serialUSART2(baudRate, mode);
s->USARTx = USARTx;
s->rxBufferHead = s->rxBufferTail = 0;
s->txBufferHead = s->txBufferTail = 0;
// callback for IRQ-based RX ONLY
s->callback = callback;
s->mode = mode;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(USARTx, &USART_InitStructure);
USART_Cmd(USARTx, ENABLE);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&USARTx->DR;
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
// Receive DMA or IRQ
if (mode & MODE_RX) {
if (s->rxDMAChannel) {
DMA_InitStructure.DMA_BufferSize = s->rxBufferSize;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->rxBuffer;
DMA_DeInit(s->rxDMAChannel);
DMA_Init(s->rxDMAChannel, &DMA_InitStructure);
DMA_Cmd(s->rxDMAChannel, ENABLE);
USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE);
s->rxDMAPos = DMA_GetCurrDataCounter(s->rxDMAChannel);
} else {
USART_ITConfig(USARTx, USART_IT_RXNE, ENABLE);
}
}
// Transmit DMA or IRQ
if (mode & MODE_TX) {
if (s->txDMAChannel) {
DMA_InitStructure.DMA_BufferSize = s->txBufferSize;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_DeInit(s->txDMAChannel);
DMA_Init(s->txDMAChannel, &DMA_InitStructure);
DMA_ITConfig(s->txDMAChannel, DMA_IT_TC, ENABLE);
DMA_SetCurrDataCounter(s->txDMAChannel, 0);
s->txDMAChannel->CNDTR = 0;
USART_DMACmd(USARTx, USART_DMAReq_Tx, ENABLE);
} else {
USART_ITConfig(USARTx, USART_IT_TXE, ENABLE);
}
}
return s;
}
void uart2Write(uint8_t ch)
void uartChangeBaud(serialPort_t *s, uint32_t baudRate)
{
if (uart2RxOnly)
return;
USART_InitTypeDef USART_InitStructure;
tx2Buffer[tx2BufferHead] = ch;
tx2BufferHead = (tx2BufferHead + 1) % UART2_BUFFER_SIZE;
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (s->mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (s->mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(s->USARTx, &USART_InitStructure);
}
bool isUart2TransmitEmpty(void)
static void uartStartTxDMA(serialPort_t *s)
{
return tx2BufferTail == tx2BufferHead;
s->txDMAChannel->CMAR = (uint32_t)&s->txBuffer[s->txBufferTail];
if (s->txBufferHead > s->txBufferTail) {
s->txDMAChannel->CNDTR = s->txBufferHead - s->txBufferTail;
s->txBufferTail = s->txBufferHead;
} else {
s->txDMAChannel->CNDTR = s->txBufferSize - s->txBufferTail;
s->txBufferTail = 0;
}
s->txDMAEmpty = false;
DMA_Cmd(s->txDMAChannel, ENABLE);
}
bool isUartAvailable(serialPort_t *s)
{
if (s->rxDMAChannel)
return s->rxDMAChannel->CNDTR != s->rxDMAPos;
else
return s->rxBufferTail != s->rxBufferHead;
}
// BUGBUG TODO TODO FIXME
bool isUartTransmitEmpty(serialPort_t *s)
{
if (s->txDMAChannel)
return s->txDMAEmpty;
else
return s->txBufferTail == s->txBufferHead;
}
uint8_t uartRead(serialPort_t *s)
{
uint8_t ch;
if (s->rxDMAChannel) {
ch = s->rxBuffer[s->rxBufferSize - s->rxDMAPos];
if (--s->rxDMAPos == 0)
s->rxDMAPos = s->rxBufferSize;
} else {
ch = s->rxBuffer[s->rxBufferTail];
s->rxBufferTail = (s->rxBufferTail + 1) % s->rxBufferSize;
}
return ch;
}
void uartWrite(serialPort_t *s, uint8_t ch)
{
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
if (s->txDMAChannel) {
if (!(s->txDMAChannel->CCR & 1))
uartStartTxDMA(s);
} else {
USART_ITConfig(s->USARTx, USART_IT_TXE, ENABLE);
}
}
// Handlers
// USART1 Tx DMA Handler
void DMA1_Channel4_IRQHandler(void)
{
serialPort_t *s = &serialPort1;
DMA_ClearITPendingBit(DMA1_IT_TC4);
DMA_Cmd(s->txDMAChannel, DISABLE);
if (s->txBufferHead != s->txBufferTail)
uartStartTxDMA(s);
else
s->txDMAEmpty = true;
}
// USART1 Tx IRQ Handler
void USART1_IRQHandler(void)
{
serialPort_t *s = &serialPort1;
uint16_t SR = s->USARTx->SR;
if (SR & USART_FLAG_TXE) {
if (s->txBufferTail != s->txBufferHead) {
s->USARTx->DR = s->txBuffer[s->txBufferTail];
s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
} else {
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
}
}
}
// USART2 Rx/Tx IRQ Handler
void USART2_IRQHandler(void)
{
uint16_t SR = USART2->SR;
serialPort_t *s = &serialPort2;
uint16_t SR = s->USARTx->SR;
if (SR & USART_IT_RXNE) {
if (uart2Callback)
uart2Callback(USART_ReceiveData(USART2));
if (SR & USART_FLAG_RXNE) {
// If we registered a callback, pass crap there
if (s->callback) {
s->callback(s->USARTx->DR);
} else {
s->rxBuffer[s->rxBufferHead] = s->USARTx->DR;
s->rxBufferHead = (s->rxBufferHead + 1) % s->rxBufferSize;
}
}
if (SR & USART_FLAG_TXE) {
if (tx2BufferTail != tx2BufferHead) {
USART2->DR = tx2Buffer[tx2BufferTail];
tx2BufferTail = (tx2BufferTail + 1) % UART2_BUFFER_SIZE;
if (s->txBufferTail != s->txBufferHead) {
s->USARTx->DR = s->txBuffer[s->txBufferTail];
s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
} else {
USART_ITConfig(USART2, USART_IT_TXE, DISABLE);
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
}
}
}

58
src/drv_uart.h
View file

@ -1,17 +1,47 @@
#pragma once
// USART1
void uartInit(uint32_t speed);
bool isUartAvailable(void);
bool isUartTransmitEmpty(void);
bool isUartTransmitDMAEmpty(void);
uint8_t uartRead(void);
uint8_t uartReadPoll(void);
void uartWrite(uint8_t ch);
void uartPrint(char *str);
#define UART_BUFFER_SIZE 64
// USART2 (GPS, Spektrum)
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly);
void uart2ChangeBaud(uint32_t speed);
bool isUart2TransmitEmpty(void);
void uart2Write(uint8_t ch);
#define UART1_RX_BUFFER_SIZE 256
#define UART1_TX_BUFFER_SIZE 256
#define UART2_RX_BUFFER_SIZE 128
#define UART2_TX_BUFFER_SIZE 64
#define MAX_SERIAL_PORTS 2
// This is a bitmask
typedef enum portmode_t {
MODE_RX = 1,
MODE_TX = 2,
MODE_RXTX = 3
} portmode_t;
typedef struct {
uint32_t baudRate;
uint32_t rxBufferSize;
uint32_t txBufferSize;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer;
uint32_t rxDMAPos;
uint32_t rxBufferHead;
uint32_t rxBufferTail;
uint32_t txBufferHead;
uint32_t txBufferTail;
DMA_Channel_TypeDef *rxDMAChannel;
DMA_Channel_TypeDef *txDMAChannel;
uint32_t rxDMAIrq;
uint32_t txDMAIrq;
bool txDMAEmpty;
USART_TypeDef *USARTx;
uartReceiveCallbackPtr callback;
portmode_t mode;
} serialPort_t;
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
void uartChangeBaud(serialPort_t *s, uint32_t baudRate);
bool isUartAvailable(serialPort_t *s);
bool isUartTransmitEmpty(serialPort_t *s);
uint8_t uartRead(serialPort_t *s);
void uartWrite(serialPort_t *s, uint8_t ch);
void uartPrint(serialPort_t *s, const char *str);

13
src/gps.c
View file

@ -43,7 +43,7 @@ void gpsInit(uint32_t baudrate)
int offset = 0;
GPS_set_pids();
uart2Init(baudrate, GPS_NewData, false);
core.gpsport = uartOpen(USART2, GPS_NewData, baudrate, MODE_RXTX);
if (mcfg.gps_type == GPS_UBLOX)
offset = 0;
@ -52,7 +52,7 @@ void gpsInit(uint32_t baudrate)
if (mcfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
uart2ChangeBaud(init_speed[i]);
uartChangeBaud(core.gpsport, init_speed[i]);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
@ -71,10 +71,10 @@ void gpsInit(uint32_t baudrate)
}
}
uart2ChangeBaud(baudrate);
uartChangeBaud(core.gpsport, baudrate);
if (mcfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
uart2Write(ubloxInit[i]); // send ubx init binary
uartWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
delay(4);
}
} else if (mcfg.gps_type == GPS_MTK) {
@ -91,13 +91,13 @@ void gpsInit(uint32_t baudrate)
static void gpsPrint(const char *str)
{
while (*str) {
uart2Write(*str);
uartWrite(core.gpsport, *str);
if (mcfg.gps_type == GPS_UBLOX)
delay(4);
str++;
}
// wait to send all
while (!isUart2TransmitEmpty());
while (!isUartTransmitEmpty(core.gpsport));
delay(30);
}
@ -1112,7 +1112,6 @@ static bool UBLOX_parse_gps(void)
f.GPS_FIX = false;
GPS_numSat = _buffer.solution.satellites;
// GPS_hdop = _buffer.solution.position_DOP;
// debug[3] = GPS_hdop;
break;
case MSG_VELNED:
// speed_3d = _buffer.velned.speed_3d; // cm/s

58
src/imu.c
View file

@ -7,12 +7,13 @@ int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t baroPressureSum = 0;
int32_t BaroAlt = 0;
int16_t sonarAlt; // to think about the unit
int32_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int16_t vario = 0; // variometer in cm/s
int32_t errorAltitudeI = 0;
int32_t vario = 0; // variometer in cm/s
int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind,
float magneticDeclination = 0.0f; // calculated at startup from config
float accVelScale;
@ -85,7 +86,7 @@ void computeIMU(void)
Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
@ -115,15 +116,6 @@ void computeIMU(void)
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
// **************************************************
//****** advanced users settings *******************
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#define GYR_CMPFM_FACTOR 200.0f
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
@ -235,7 +227,7 @@ static void getEstimatedAttitude(void)
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (72 < accMag && accMag < 133) {
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
@ -291,13 +283,19 @@ static void getEstimatedAttitude(void)
heading = heading + 360;
}
#endif
if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f;
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
}
}
#ifdef BARO
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
int16_t applyDeadband(int16_t value, int16_t deadband)
int16_t applyDeadband(int32_t value, int32_t deadband)
{
if (abs(value) < deadband) {
value = 0;
@ -315,13 +313,13 @@ int getEstimatedAltitude(void)
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t dTime;
int16_t error16;
int16_t baroVel;
int16_t accZ;
static int16_t accZoffset = 0;
int32_t error;
int32_t baroVel;
int32_t accZ;
int32_t vel_tmp;
static int32_t accZoffset = 0;
static float vel = 0.0f;
static int32_t lastBaroAlt;
int16_t vel_tmp;
dTime = currentT - previousT;
if (dTime < UPDATE_INTERVAL)
@ -337,27 +335,27 @@ int getEstimatedAltitude(void)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise
EstAlt = (EstAlt * 6 + BaroAlt * 2) / 8; // additional LPF to reduce baro noise
//P
error16 = constrain(AltHold - EstAlt, -300, 300);
error16 = applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error16 >> 7), -150, +150);
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain(((cfg.P8[PIDALT] * error) / 128), -150, +150);
//I
errorAltitudeI += cfg.I8[PIDALT] * error16 >> 6;
errorAltitudeI += (cfg.I8[PIDALT] * error) / 64;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI >> 9; // I in range +/-60
BaroPID += errorAltitudeI / 512; // I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G (invG is calculated in getEstimatedAttitude)
accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG;
if (!f.ARMED) {
accZoffset -= accZoffset >> 3;
accZoffset -= accZoffset / 8;
accZoffset += accZ;
}
accZ -= accZoffset >> 3;
accZ -= accZoffset / 8;
accZ = applyDeadband(accZ, cfg.accz_deadband);
// Integrator - velocity, cm/sec
@ -377,7 +375,7 @@ int getEstimatedAltitude(void)
vel_tmp = vel;
vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp;
BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp >> 4, -150, 150);
BaroPID -= constrain((cfg.D8[PIDALT] * vel_tmp) / 16, -150, 150);
return 1;
}

34
src/main.c
View file

@ -1,7 +1,8 @@
#include "board.h"
#include "mw.h"
extern uint8_t useServo;
core_t core;
extern rcReadRawDataPtr rcReadRawFunc;
// two receiver read functions
@ -12,15 +13,15 @@ extern uint16_t spektrumReadRawRC(uint8_t chan);
// gcc/GNU version
static void _putc(void *p, char c)
{
uartWrite(c);
uartWrite(core.mainport, c);
}
#else
// keil/armcc version
int fputc(int c, FILE *f)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!isUartTransmitDMAEmpty());
uartWrite(c);
while (!isUartTransmitEmpty(core.mainport));
uartWrite(core.mainport, c);
return c;
}
#endif
@ -31,27 +32,6 @@ int main(void)
drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params;
#if 0
// PC12, PA15
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
GPIOA->BRR = 0x8000; // set low 15
GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
GPIOC->BRR = 0x1000; // set low 12
#endif
#if 0
// using this to write asm for bootloader :)
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
AFIO->MAPR &= 0xF0FFFFFF;
AFIO->MAPR = 0x02000000;
GPIOB->BRR = 0x18; // set low 4 & 3
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif
systemInit();
#ifdef USE_LAME_PRINTF
init_printf(NULL, _putc);
@ -73,7 +53,7 @@ int main(void)
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
mixerInit(); // this will set useServo var depending on mixer type
mixerInit(); // this will set core.useServo var depending on mixer type
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true;
@ -82,7 +62,7 @@ int main(void)
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo;
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;

7
src/mixer.c
View file

@ -2,7 +2,6 @@
#include "mw.h"
static uint8_t numberMotor = 0;
uint8_t useServo = 0;
int16_t motor[MAX_MOTORS];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
@ -144,10 +143,10 @@ void mixerInit(void)
int i;
// enable servos for mixes that require them. note, this shifts motor counts.
useServo = mixers[mcfg.mixerConfiguration].useServo;
core.useServo = mixers[mcfg.mixerConfiguration].useServo;
// if we want camstab/trig, that also enables servos, even if mixer doesn't
if (feature(FEATURE_SERVO_TILT))
useServo = 1;
core.useServo = 1;
if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) {
// load custom mixer into currentMixer
@ -198,7 +197,7 @@ void mixerLoadMix(int index)
void writeServos(void)
{
if (!useServo)
if (!core.useServo)
return;
switch (mcfg.mixerConfiguration) {

5
src/mw.c
View file

@ -762,7 +762,6 @@ void loop(void)
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
Sonar_update();
debug[2] = sonarAlt;
}
#endif
if (feature(FEATURE_VARIO) && f.VARIO_MODE)
@ -838,6 +837,10 @@ void loop(void)
}
#endif
if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE]+= throttleAngleCorrection;
}
if (sensors(SENSOR_GPS)) {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
float sin_yaw_y = sinf(heading * 0.0174532925f);

22
src/mw.h
View file

@ -176,6 +176,7 @@ typedef struct config_t {
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
uint8_t throttle_angle_correction; //
// Failsafe related configuration
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
@ -285,6 +286,16 @@ typedef struct master_t {
uint8_t chk; // XOR checksum
} master_t;
// Core runtime settings
typedef struct core_t {
serialPort_t *mainport;
serialPort_t *gpsport;
serialPort_t *telemport;
serialPort_t *rcvrport;
bool useServo;
} core_t;
typedef struct flags_t {
uint8_t OK_TO_ARM;
uint8_t ARMED;
@ -327,14 +338,14 @@ extern int32_t baroPressure;
extern int32_t baroTemperature;
extern int32_t baroPressureSum;
extern int32_t BaroAlt;
extern int16_t sonarAlt;
extern int32_t sonarAlt;
extern int32_t EstAlt;
extern int32_t AltHold;
extern int16_t errorAltitudeI;
extern int16_t BaroPID;
extern int16_t vario;
extern int32_t errorAltitudeI;
extern int32_t BaroPID;
extern int32_t vario;
extern int16_t throttleAngleCorrection;
extern int16_t headFreeModeHold;
extern int16_t zVelocity;
extern int16_t heading, magHold;
extern int16_t motor[MAX_MOTORS];
extern int16_t servo[8];
@ -368,6 +379,7 @@ extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern core_t core;
extern master_t mcfg;
extern config_t cfg;
extern flags_t f;

5
src/printf.c
View file

@ -30,6 +30,7 @@
*/
#include "board.h"
#include "mw.h"
#ifdef USE_LAME_PRINTF
#define PRINTF_LONG_SUPPORT
@ -227,7 +228,7 @@ void tfp_printf(char *fmt, ...)
va_start(va, fmt);
tfp_format(stdout_putp, stdout_putf, fmt, va);
va_end(va);
while (!isUartTransmitEmpty());
while (!isUartTransmitEmpty(core.mainport));
}
static void putcp(void *p, char c)
@ -235,8 +236,6 @@ static void putcp(void *p, char c)
*(*((char **) p))++ = c;
}
void tfp_sprintf(char *s, char *fmt, ...)
{
va_list va;

26
src/serial.c
View file

@ -157,16 +157,16 @@ void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
uartWrite(t);
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 8;
uartWrite(t);
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 16;
uartWrite(t);
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 24;
uartWrite(t);
uartWrite(core.mainport, t);
checksum ^= t;
}
@ -174,16 +174,16 @@ void serialize16(int16_t a)
{
static uint8_t t;
t = a;
uartWrite(t);
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 8 & 0xff;
uartWrite(t);
uartWrite(core.mainport, t);
checksum ^= t;
}
void serialize8(uint8_t a)
{
uartWrite(a);
uartWrite(core.mainport, a);
checksum ^= a;
}
@ -261,7 +261,9 @@ void serialInit(uint32_t baudrate)
{
int idx;
uartInit(baudrate);
core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
// TODO fix/hax
core.telemport = core.mainport;
// calculate used boxes based on features and fill availableBoxes[] array
memset(availableBoxes, 0xFF, sizeof(availableBoxes));
@ -602,6 +604,8 @@ static void evaluateCommand(void)
break;
case MSP_DEBUG:
headSerialReply(8);
// make use of this crap, output some useful QA statistics
debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
for (i = 0; i < 4; i++)
serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
@ -668,8 +672,8 @@ void serialCom(void)
return;
}
while (isUartAvailable()) {
c = uartRead();
while (isUartAvailable(core.mainport)) {
c = uartRead(core.mainport);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
@ -705,7 +709,7 @@ void serialCom(void)
c_state = IDLE;
}
}
if (!cliMode && !isUartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
if (!cliMode && !isUartAvailable(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry();
return;
}

4
src/spektrum.c
View file

@ -27,10 +27,10 @@ void spektrumInit(void)
spek_chan_mask = 0x03;
}
uart2Init(115200, spektrumDataReceive, true);
core.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX);
}
// UART2 Receive ISR callback
// Receive ISR callback
static void spektrumDataReceive(uint16_t c)
{
uint32_t spekTime;

19
src/telemetry.c
View file

@ -48,29 +48,28 @@
// from sensors.c
extern uint8_t batteryCellCount;
static void sendDataHead(uint8_t id)
{
uartWrite(PROTOCOL_HEADER);
uartWrite(id);
uartWrite(core.telemport, PROTOCOL_HEADER);
uartWrite(core.telemport, id);
}
static void sendTelemetryTail(void)
{
uartWrite(PROTOCOL_TAIL);
uartWrite(core.telemport, PROTOCOL_TAIL);
}
static void serializeFrsky(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
uartWrite(0x5d);
uartWrite(0x3e);
uartWrite(core.telemport, 0x5d);
uartWrite(core.telemport, 0x3e);
} else if (data == 0x5d) {
uartWrite(0x5d);
uartWrite(0x3d);
uartWrite(core.telemport, 0x5d);
uartWrite(core.telemport, 0x3d);
} else
uartWrite(data);
uartWrite(core.telemport, data);
}
static void serialize16(int16_t a)
@ -183,7 +182,7 @@ static void sendVoltage(void)
/*
* Send voltage with ID_VOLTAGE_AMP
*/
static void sendVoltageAmp()
static void sendVoltageAmp(void)
{
uint16_t voltage = (vbat * 110) / 21;