void initTelemetry(bool State)
{
if (State != telemetryEnabled) {
if (State)
serialInit(9600);
else
serialInit(cfg.serial_baudrate);
telemetryEnabled = State;
}
}
void initMinimOSDTelemetry(bool State)
{
if (State != telemetryEnabled)
{
if (State) serialInit(57600);
else serialInit(cfg.serial_baudrate);
telemetryEnabled = State;
reset_mavlink();
}
}
void updateTelemetryState(void)
{
bool State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : f.ARMED;
if (State != telemetryEnabled) {
if (mcfg.telemetry_softserial == TELEMETRY_UART) {
if (State)
serialInit(9600);
else
serialInit(mcfg.serial_baudrate);
}
telemetryEnabled = State;
}
}
/*---------------------------------------------------------------------------
TITLE : cmd_bluetooth_check
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
void cmd_bluetooth_check( void )
{
uint32_t time_out;
uint8_t ch;
uint8_t ch_array[2];
uint8_t ch_i;
osStatus ret;
ret = osMutexWait( Mutex_Loop, 1000 );
if( ret != osOK )
{
_menu_printf("Fail to osMutexWait\r\n");
return;
}
core.blueport = uartOpen(USART2, NULL, 115200, MODE_RXTX);
_menu_printf("\r\n");
_menu_printf("AT -> ");
serialPrint(core.blueport, "AT");
ch_array[0] = 0;
ch_array[1] = 0;
ch_i = 0;
//-- 응답이 올때까지 기다림
time_out = 1000;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
_menu_putch(ch);
ch_array[ch_i++] = ch;
if( ch_i >= 2 ) break;
}
osDelay(1);
}
if( ch_array[0] == 'O' && ch_array[1] == 'K' )
{
_menu_printf("\r\nBluetooth OK");
}
else
{
_menu_printf("\r\nBluetooth Fail");
}
_menu_printf("\r\n");
serialInit(mcfg.serial_baudrate);
osMutexRelease( Mutex_Loop );
}
void main(void) {
rccInit();
timerInit();
configInit();
adcInit();
fetInit();
serialInit();
runInit();
cliInit();
owInit();
runDisarm(REASON_STARTUP);
inputMode = ESC_INPUT_PWM;
fetSetDutyCycle(0);
fetBeep(200, 100);
fetBeep(300, 100);
fetBeep(400, 100);
fetBeep(500, 100);
fetBeep(400, 100);
fetBeep(300, 100);
fetBeep(200, 100);
pwmInit();
statusLed = digitalInit(GPIO_STATUS_LED_PORT, GPIO_STATUS_LED_PIN);
digitalHi(statusLed);
errorLed = digitalInit(GPIO_ERROR_LED_PORT, GPIO_ERROR_LED_PIN);
digitalHi(errorLed);
#ifdef ESC_DEBUG
tp = digitalInit(GPIO_TP_PORT, GPIO_TP_PIN);
digitalLo(tp);
#endif
// self calibrating idle timer loop
{
volatile unsigned long cycles;
volatile unsigned int *DWT_CYCCNT = (int *)0xE0001004;
volatile unsigned int *DWT_CONTROL = (int *)0xE0001000;
volatile unsigned int *SCB_DEMCR = (int *)0xE000EDFC;
*SCB_DEMCR = *SCB_DEMCR | 0x01000000;
*DWT_CONTROL = *DWT_CONTROL | 1; // enable the counter
minCycles = 0xffff;
while (1) {
idleCounter++;
NOPS_4;
cycles = *DWT_CYCCNT;
*DWT_CYCCNT = 0; // reset the counter
// record shortest number of instructions for loop
totalCycles += cycles;
if (cycles < minCycles)
minCycles = cycles;
}
}
}
void main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
if (CALBC1_1MHZ==0xFF) // If calibration constant erased
{
while(1); // do not load, trap CPU!!
}
BCSCTL1 = CALBC1_1MHZ;
DCOCTL = CALDCO_1MHZ;
lcdInit(); // Initialize LCD
serialInit(); // initalize serial communication
//Button Configurations
P1DIR &= ~(BIT5 +BIT7);
P1REN |= BIT5 + BIT7;
//PWM Initialization
P1DIR |= BIT6;
P1SEL |= BIT6;
TA0CTL = TASSEL_2 + MC_1; //SMCLK, up mode
TA0CCR0 = period;
TA0CCR1 = 0;
TA0CCTL1 = OUTMOD_6;
WDTCTL = WDT_MDLY_8; //watchdog interrupt every 8 ms
IE1 |= WDTIE; // ENable Watchdog Tiemr interrupt
lcdSetText("No Alarm Set.", 0,0);
alarm_state = 0;
_bis_SR_register(LPM0_bits+GIE);
}
void hardwareInit(void)
{
PLLConfig();
SET_ADPCFG;
buttonsInit();
LED_SET_OUT;
LED_OFF;
sensorsInit();
sensorsOn();
readBatteryVoltage();
servoInit();
motorsInit();
//motorsOff();
profilerInit();
readButtons();
if( key_UP&&key_DN ) //if ICSP is connected
{
displayEnabled = TRUE;
serialInit();
//_TRISB10 = 0;
//_TRISB11 = 0;
}
CC2500_init();
systemTimerInit();
}
/* UART_init initializes the UART hardware
* Argument :
* device : /dev/tty???# (linux only)
* option : baudrate (arduino only) | E_115200_8N2, E_115200_8N1 (linux only)
* Return value :
* >=0 : success
* <0 : error
*/
int UART_init(const char* device, uint32_t option){
#ifdef ARCH_X86_LINUX
if(option & E_FRAMEBASED){
framebased = 1;
}
#endif
#if defined(ARCH_328P_ARDUINO) || defined(ARCH_LPC21XX)
return serialInit(option);
#elif defined(ARCH_X86_LINUX)
return serialInit(device,option);
#elif defined(ARCH_LM4FXX)
return serialInit(option);
#endif
return -1;
}
void main(void)
{
uint16_t i;
serialInit(9600);
vw_setup(600);
puts("PIC Receiver Demo\n");
vw_rx_start();
while(1)
{
if (vw_have_message())
{
uint8_t len = VW_MAX_MESSAGE_LEN;
if (vw_recv(text, &len))
{
for (i = 0; i < len; i++)
putchar(text[i]);
putchar('\n');
}
}
}
}
void amb8420InitSerial(void)
{
serialInit(AMB8420_UART_ID, AMB8420_SERIAL_BAUDRATE, 0);
serialEnableTX(AMB8420_UART_ID);
serialSetReceiveHandle(AMB8420_UART_ID, serialReceive);
serial[AMB8420_UART_ID].function = SERIAL_FUNCTION_RADIO;
}
bool Arduino::init(const char* serialPort) {
if (fd == -1) {
fd = serialInit(serialPort, BAUDRATE);
sleep(2); // let the Arduinos setup
flush();
if (fd == -1)
return false;
}
return true;
}
int Camera::restart(){
if ((Fg_AcquireEx(fg, camPort, nrOfPicturesToGrab, ACQ_STANDARD, memHandle)) < 0) {
fprintf(stderr, "Fg_AcquireEx() failed: %s\n", Fg_getLastErrorDescription(fg));
Fg_FreeMemEx(fg, memHandle);
Fg_FreeGrabber(fg);
return FG_ERROR;
}
serialInit(ComNr);
return FG_OK;
}
void init_touch(void) {
serialInit(TOUCHSCREEN, Baud_9600); //Initialize control and baud rate.
serialSendChar(TOUCHSCREEN, 0x13); //Disable
serialSendChar(TOUCHSCREEN, 0x55); //Sync
serialSendChar(TOUCHSCREEN, 0x01); //Size
serialSendChar(TOUCHSCREEN, 0x12); //Enable
prevPoint.x = 0;
prevPoint.y = 0;
printf("< Touch Screen Initialized >\n");
}
int main()
{
///////// System Clock Initialization /////////
// Fosc = 12MHz
// MSEL=4, PSEL=1
// PLLCFG=0100100=0x24=36
// cclk = 60MHz
// Fcco = 240MHz
PLLCFG = 0x24;
// Enable PLL
PLLCON = 0x1;
PLLFEED = 0xAA;
PLLFEED = 0x55;
// Wait for PLL to achieve lock
while(!(PLLSTAT & 0x400));
// Connect PLL
PLLCON=0x3;
PLLFEED=0xAA;
PLLFEED=0x55;
///////// Peripheral Clock Initialization /////////
// Set periphery divider
// 0x00 => /4
// 0x01 => /1
// 0x02 => /2
APBDIV_bit.APBDIV = 0x02;
serialInit(UART0, 115200);
if (initOF() == false) {
serialPuts(UART0, "SPI Init Error\n");
}
while (1) {
updateOF();
if (_motion == false)
serialPuts(UART0, "no ");
serialPuts(UART0, "motion\n");
if (_overflow == true)
serialPuts(UART0, "overflow\n");
serialPuts(UART0, "squal: ");
serialPuti(UART0, surface_quality);
serialPutc(UART0, '\n');
serialPuti(UART0, raw_dx);
serialPutc(UART0, '\n');
serialPuti(UART0, raw_dx);
serialPutc(UART0, '\n');
delay(1000);
}
}
int main(void)
{
uint8_t ch;
/* This code makes the following assumptions:
* No interrupts will execute
* SP points to RAMEND
* r1 contains zero
* If not, uncomment the following instructions. */
//cli();
asm volatile("clr __zero_reg__");
#if defined(__AVR_ATmega8__)
SP = RAMEND; // This is done by hardware reset
#endif
/* Disable the watchdog timer to prevent
* eternal reset loop of doom and despair */
ch = MCUSR;
MCUSR = 0;
if(ch & (_BV(WDRF) | _BV(BORF) | _BV(PORF)))
if(eeprom_read_byte(EEPROM_IMG_STAT) == EEPROM_IMG_OK_VALUE)
appStart();
wdt_enable(WDTO_8S);
// Wait to ensure startup of W5100
_delay_ms(200);
// Prescaler=0, ClkIO Period = 62,5ns
// TCCR1B values:
// 0x01 -> ClkIO/1 -> 62,5ns period, 4ms max
// 0x02 -> ClkIO/8 -> 500ns period, 32ms max
// 0X03 -> ClkIO/64 -> 4us period, 256ms max
// 0x04 -> ClkIO/256 -> 16us period, 1024ms max
// 0x05 -> ClkIO/1024 -> 64us period, 4096ms max
// Set up Timer 1 as timekeeper for LED flashing
TCCR1B = _BV(CS12); // Same thing as TCCR1B = 0x04;
/* Write version information in the EEPROM */
if(eeprom_read_byte(EEPROM_MAJVER) != ARIADNE_MAJVER)
eeprom_write_byte(EEPROM_MAJVER, ARIADNE_MAJVER);
if(eeprom_read_byte(EEPROM_MINVER) != ARIADNE_MINVER)
eeprom_write_byte(EEPROM_MINVER, ARIADNE_MINVER);
/* Initialize UART communication */
serialInit();
DBG_MAIN(tracePGMlnMain(mDebugMain_TITLE);)
int Camera::init(const char* file) {
int isSlave =0;
if ((fg = Fg_InitEx("FullAreaGray8", m_boardNr, isSlave)) == NULL) {
fprintf(stderr, "error in Fg_InitEx: %s\n", Fg_getLastErrorDescription(NULL));
exit(EXIT_FAILURE);
}
m_file = file;
if(Fg_loadConfig(fg,file)!=FG_OK){
printf("\nFile config loading failed\n");
exit(EXIT_FAILURE);
}
ComNr=m_boardNr*2;
serialInit(ComNr);
if(Fg_setParameter(fg,FG_TRIGGERMODE,&TriggerMode,camPort)==FG_OK){
printf("\nTrig config succeed\n");
}
size_t totalBufferSize = m_width * m_height * samplePerPixel * bytePerSample * nbBuffers;
memHandle = Fg_AllocMemEx(fg, totalBufferSize, nbBuffers);
if (memHandle == NULL) {
fprintf(stderr, "error in Fg_AllocMemEx: %s\n", Fg_getLastErrorDescription(fg));
Fg_FreeGrabber(fg);
exit(EXIT_FAILURE);
}
try{
// if(setExposure(m_exposure)!=FG_OK){throw 0;}
// setWidth(m_width);
// setHeight(m_height);
// setYoffset(m_yoffset);
// setXoffset(m_xoffset);
int bitAlignment = FG_LEFT_ALIGNED;
CHECK(FG_BITALIGNMENT, "FG_BITALIGNMENT", &bitAlignment);
CHECK(FG_FRAMESPERSEC, "FG_FRAMESPERSEC", &m_framespersec);
CHECK(FG_TIMEOUT, "FG_TIMEOUT", &timeout);
// startAcquire();
cout << "test" << endl;
}catch(string const& error){
cout << "ERROR: " << error <<endl;
close();
init(file);
}catch(int e){
close();
init(file);
}
return FG_OK;
}
void init(void)
{
cli(); // Unset interrupts while configuring
#if defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)
// Set Linduino DC590 connector mux to I2C
DDRB |= _BV(PB0);
PORTB |= _BV(PB0);
#endif
// Generate I2C command lookup table
uint8_t i = 0;
int i2cNumCommands = sizeof(i2c_registerMap) / sizeof(i2cCommand);
do {
uint8_t j;
i2c_registerIndex[i] = I2C_UNSUPPORTED; // default
for(j=0; j<i2cNumCommands; j++)
{
if(i == i2c_registerMap[j].cmdCode)
{
i2c_registerIndex[i] = j;
}
}
i++;
} while (i != 0);
// Set defaults
I2C_PAGE[0] = 0;
I2C_STATUS_CML[0] = 0;
I2C_STATUS[0] = 0;
I2C_CONFIG_ALL[0] = 0;
I2C_REV_ID[0] = 0xA000;
for(i=0; i<I2C_NUMPAGES; i++)
{
// FIXME: reset counter registers
}
serialInit();
// Initialize I2C last - after all registers are configured
i2c_slave_init(I2C_ADDRESS, 0);
sei();
}
void main(void)
{
unsigned char i;
serialInit();
i=0;
for(;;)
{
if (serialTxBusy != 1)
{
serialPutChar(i++);
delay(50000);
}
}
}
void productionDebug(void)
{
gpio_config_t gpio;
// remap PB6 to USART1_TX
gpio.pin = Pin_6;
gpio.mode = Mode_AF_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true);
serialInit(mcfg.serial_baudrate);
delay(25);
serialPrint(core.mainport, "DBG ");
printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2);
serialPrint(core.mainport, "EOF");
delay(25);
gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false);
}
static int serialRestart (WVIEW_MEDIUM *med)
{
MEDIUM_SERIAL *work = (MEDIUM_SERIAL*)med->workData;
serialExit(med);
radMsgLog (PRI_HIGH, "serialRestart: attempting restart");
while ((!wviewdIsExiting()) && (serialInit(med, work->device) == ERROR))
{
radMsgLog (PRI_HIGH, "serialRestart: restart failed");
radUtilsSleep(5000);
radMsgLog (PRI_HIGH, "serialRestart: retrying restart");
}
if (!wviewdIsExiting())
{
radMsgLog (PRI_HIGH, "serialRestart: recovered");
}
return OK;
}
int main(void)
{
serialInit(SPEED9600);
sei();
serialWrite("HEllo world!!!\n");
while(1)
{
uint8_t a,b,c;
serialWaitUntil('\r');
a = serialParseInt();
b = serialParseInt();
c = serialParseInt();
serialPrintIntLn(a*b*c);
serialClearBuffer();
}
serialEnd();
return 0;
}
int main(void)
{
DDRC = 0x00;//PORTC на вход
PORTC = 0x3;
ButtonsInit();
ButtonsSet(&PINC, PIN0, 1, &OnClick0);
ButtonsSet(&PINC, PIN1, 1, &OnClick1);
ButtonsActivate();
serialInit(SPEED9600);
sei();
serialWrite("Hello! AVR Mega16 Boot OK\n");
while(1)
{
}
return 0;
}
int main(void)
{
uint8_t buf[10];
DDRD |= ((1 << PD5) | (1 << PD6));//PD5 и PD6 на выход
serialInit(SPEED9600);
mirf_init();
_delay_ms(50);
sei();
mirf_config();
while(1)
{
while(!mirf_data_ready()) ;
mirf_get_data(buf);
if(buf[0] == 1)
{
PORTD |= (1 << PD5);
}
else
{
PORTD &= ~(1 << PD5);
}
if(buf[1] == 1)
{
PORTD |= (1 << PD6);
}
else
{
PORTD &= ~(1 << PD6);
}
}
return 0;
}
/*
* @brief Initialize the minimal amount of hardware for the bootloader to function
* @returns void
*/
void systemInit(void) {
uint32 i, j;
MAP_IntMasterDisable();
// increase LDO voltage so that PLL operates properly
MAP_SysCtlLDOSet(SYSCTL_LDO_2_75V);
#ifdef PART_LM3S8962
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);
#endif
systemIDInit();
blinkyLedInit();
buttonsInit();
srand(roneID);
serialInit();
#if defined(RONE_V9) || defined(RONE_V12)
SPIInit();
radioInit();
#endif // defined(RONE_V9) || defined(RONE_V12)
// Triple blink blinky, startup signal
for (i = 0; i < 3; i++) {
blinkyLedSet(1);
for (j = 0; j < 150000;) {
j++;
}
blinkyLedSet(0);
for (j = 0; j < 250000;) {
j++;
}
}
// Initialize 24-bit Systick
SysTickPeriodSet(0xffffff);
SysTickEnable();
}
void ourtosInit(uint8_t maxPriority, freq_t freq) {
int i;
/* Initialize globals.
*
* Note that we don't initialize _mainLoopStackPointer, since the timer ISR
* will always run at least once and initialize it before we try to use it.
*/
_maxPriority = maxPriority;
_started = false;
_debug = false;
_mutexesEnabled = true;
_currentTask = MAIN_LOOP_PRIORITY;
/* Initialize task array.
* Mark that all priorities are not yet devoted to either mutexes or tasks.
*/
for (i = 0; i < _maxPriority; i++) {
taskArray[i].usage = USAGE_NONE;
}
timerInit(freq);
serialInit(BAUD_9600);
}
/*---------------------------------------------------------------------------
TITLE : cmd_bluetooth_change_name
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
void cmd_bluetooth_change_name( void )
{
uint32_t time_out;
uint8_t ch;
uint8_t ch_array[20] = "AT+NAME";
uint8_t ch_i;
uint8_t ch_offset;
osStatus ret;
ret = osMutexWait( Mutex_Loop, 1000 );
if( ret != osOK )
{
_menu_printf("Fail to osMutexWait\r\n");
return;
}
core.blueport = uartOpen(USART2, NULL, 115200, MODE_RXTX);
//-- 이름 입력
//
_menu_printf("Enter Name : ");
ch_i = 0;
ch_offset = 7;
while(1)
{
if( _menu_received() > 0 )
{
ch = _menu_getch();
if( ch_i < 12 && ch != 0x0D )
{
ch_array[ch_offset+ch_i++] = ch;
}
else
{
break;
}
_menu_printf("%c", ch);
if( ch == 0x0D )
{
_menu_printf("\r\n");
break;
}
}
}
ch_array[ch_offset+ch_i] = 0;
//-- 블루투스 설정
//
//serialPrint(core.blueport, "AT+NAMEbaram1");
serialPrint(core.blueport, (const char *)ch_array);
ch_array[0] = 0;
ch_array[1] = 0;
ch_i = 0;
//-- 응답이 올때까지 기다림
time_out = 1000;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
//_menu_putch(ch);
ch_array[ch_i++] = ch;
if( ch_i >= 2 ) break;
}
osDelay(1);
}
if( ch_array[0] == 'O' && ch_array[1] == 'K' )
{
_menu_printf("\r\nBluetooth OK");
}
else
{
_menu_printf("\r\nBluetooth Fail");
}
_menu_printf("\r\nEnd\r\n");
serialInit(mcfg.serial_baudrate);
osMutexRelease( Mutex_Loop );
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef STM32F40_41xxx
SetSysClock();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_USART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_USART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
spiInit(SPI3);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE_INT);
#if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(SPARKY2)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
#ifdef I2C_DEVICE_EXT
i2cInit(I2C_DEVICE_EXT);
#endif
}
#endif
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
#ifdef COLIBRI
if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) {
ledStripEnable();
}
#else
ledStripEnable();
#endif
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredState) == requiredState) {
stopMotors();
}
while (1);
}
void init(void)
{
#ifdef USE_HAL_DRIVER
HAL_Init();
#endif
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false);
#endif
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
#if !defined(USE_HAL_DRIVER)
dmaInit();
#endif
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
#endif
uint16_t idlePulse = masterConfig.motorConfig.mincommand;
if (feature(FEATURE_3D)) {
idlePulse = masterConfig.flight3DConfig.neutral3d;
}
if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
idlePulse = 0; // brushed motors
}
#ifdef USE_QUAD_MIXER_ONLY
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig);
}
#endif
#ifndef SKIP_RX_PWM_PPM
if (feature(FEATURE_RX_PPM)) {
ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(&masterConfig.pwmConfig);
}
pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode);
#endif
mixerConfigureOutput();
#ifdef USE_SERVOS
servoConfigureOutput();
#endif
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
/* temp until PGs are implemented. */
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#ifdef USE_SPI
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#ifdef USE_SPI_DEVICE_4
spiInit(SPIDEV_4);
#endif
#endif
#ifdef VTX
vtxInit();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
#ifdef USE_RTC6705
if (feature(FEATURE_VTX)) {
rtc6705_soft_spi_init();
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
}
#endif
#ifdef OSD
if (feature(FEATURE_OSD)) {
osdInit();
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
LED2_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspFcInit();
mspSerialInit();
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(masterConfig.transponderData);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init(IO_TAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IO_TAG_NONE);
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
#if defined(STM32F4) || defined(STM32F7)
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
fcTasksInit();
systemState |= SYSTEM_STATE_READY;
}
/*---------------------------------------------------------------------------
TITLE : cmd_bluetooth_setup
WORK :
ARG : void
RET : void
---------------------------------------------------------------------------*/
void cmd_bluetooth_setup( void )
{
uint32_t time_out;
uint8_t ch;
osStatus ret;
//-- MW의 메인 Loop를 중지 시킨다.
//
ret = osMutexWait( Mutex_Loop, 1000 );
if( ret != osOK )
{
_menu_printf("Fail to osMutexWait\r\n");
return;
}
_menu_printf("\r\nUart2 Open 9600BPS \r\n");
core.blueport = uartOpen(USART2, NULL, 9600, MODE_RXTX);
_menu_printf("\r\nAT -> ");
serialPrint(core.blueport, "AT");
//-- 응답이 올때까지 기다림
time_out = 100;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
_menu_putch(ch);
}
osDelay(1);
}
_menu_printf("\r\nAT+BAUD4 -> ");
serialPrint(core.blueport, "AT+BAUD4");
//-- 응답이 올때까지 기다림
time_out = 500;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
_menu_putch(ch);
}
osDelay(1);
}
_menu_printf("\r\nAT+RESET -> ");
serialPrint(core.blueport, "AT+RESET");
//-- 응답이 올때까지 기다림
time_out = 500;
while(time_out--)
{
if( serialTotalBytesWaiting(core.blueport) )
{
ch = serialRead(core.blueport);
_menu_putch(ch);
}
osDelay(1);
}
_menu_printf("\r\n");
serialInit(mcfg.serial_baudrate);
osMutexRelease( Mutex_Loop );
}
void main()
{
//temp variable useful for loops
int i;
//temp variables for serial communication
int c;
unsigned int bytesRead;
//variables for wifi communication
int newMode;
int interfacePacketRecieved;
int network;
#GLOBAL_INIT
{
setMode(ROBOT_ENABLE);
rx.state = WAIT;
/***********************
Initialization of Packets
***********************/
// Initialize software version
aPacket[1] = SOFTWARE_VERSION_ID >> 8;
aPacket[2] = SOFTWARE_VERSION_ID & 0xFF;
// Initialize UDP response packets
gPacket[0] = 'G';
gPacket[1] = ROBOTMODE;
gPacket[2] = SOFTWARE_VERSION_ID >> 8;
gPacket[3] = SOFTWARE_VERSION_ID & 0xFF;
gPacket[4] = 0; // user version id
gPacket[5] = 0;
gPacket[6] = 0; // battery voltage
gPacket[7] = 0;
// Initialize J packet values to 0
memset(jPacket, 0, J_PACKET_LENGTH);
// Initialize X default packet (autonomous mode dummy packet) to 0
memset(xPacketDefaultValues, 0, X_PACKET_LENGTH);
}
/////////////////////////////////////////////////////////////////////////
// Configure Port D -- Leave PD4-PD7 untouched (used for other purposes)
/////////////////////////////////////////////////////////////////////////
WrPortI(PDCR, &PDCRShadow, RdPortI(PDCR) & 0xF0); // clear bits to pclk/2
WrPortI(PDFR, &PDFRShadow, RdPortI(PDFR) & 0xF0); // no special functions
WrPortI(PDDCR, &PDDCRShadow, RdPortI(PDDCR) & 0xF0); // clear bits to drive
// high and low
WrPortI(PDDR, &PDDRShadow, RdPortI(PDDR) | 0x0D); // set outputs high
WrPortI(PDDDR, &PDDDRShadow, RdPortI(PDDDR) | 0x0D); // set inputs and
// outputs
//Initialize serial communication
serialInit();
//Initialize controls as neutral
//memset(interface.axis, 127, sizeof(interface.axis));
//memset(interface.btn, 0, sizeof(interface.btn));
//Decide which router to connect to
if (BitRdPortI(PDDR, 1) == 1) {
//USER settings
BitWrPortI(PDDR, &PDDRShadow, 1, 0); // turn off user LED
network = USER_ROUTER;
printf("USER\n");
} else {
//COMPETITION Settings
BitWrPortI(PDDR, &PDDRShadow, 0, 0); // turn on user LED
network = COMP_ROUTER;
setMode(ROBOT_DISABLE);
printf("COMPETITION\n");
}
// */
printf("Robot Mode: %d \n", ROBOTMODE);
// Wait for X2 handshake (to learn team #) before attempting connection
printf("Waiting for X2...\n");
while (x2Connection != X2_CONN_ESTABLISHED) {
if ((c = serCgetc()) != -1) {
byteReceived(c);
}
}
printf("X2 connection established\n");
sendPacket('F', F_PACKET_LENGTH, fPacket);
ConnectToNetwork(network, teamNo);
//Main Loop
while(1) {
// Receive field, interface & X2 reply packets as they come
costate {
//Check to see if we have new field communication
if (udpRecieveFieldPacket()) {
newMode = ProcessFieldPacket();
if (newMode != -1) {
// Set robot mode flags
setMode(newMode);
// If disable flag set: zero motor values
if (newMode & ROBOT_DISABLE)
disableRobot();
// Send X2 packet with new mode flags
sendPacket('F', F_PACKET_LENGTH, fPacket);
// Send back response packet with our robot mode flags
// and code versions
sendFieldSocket(gPacket, G_PACKET_LENGTH);
//printf("Robot Mode: %d \n", ROBOTMODE);
}
}
//Check to see if we have new field commuication, and we can use it
if (udpRecieveInterfacePacket()) {
if (interBuf[0] == 'I') {
udpNewData = TRUE;
}
}
// Receive X2 serial data in bursts of up to X2_RX_BURST_LENGTH
bytesRead = 0;
while ((c = serCgetc()) != -1 &&
bytesRead <= X2_RX_BURST_LENGTH) {
byteReceived((unsigned char)c);
bytesRead += 1;
}
}
// */
// Time out if no UDP packets received after 500 ms
// TODO: Update the interface to send an enable packet. In the meantime
// this feature is only activated in competition mode
costate {
if (network == COMP_ROUTER) {
waitfor( udpNewData || DelayMs(500) );
if (!udpNewData)
disableRobot();
}
}
// Send X2 packets
costate {
// FOR TESTING
// udpNewData = TRUE;
waitfor( udpNewData );
// Check that disable bit is not set
if (!(ROBOTMODE & ROBOT_DISABLE)) {
// If in autonomous mode, send a dummy 'X' packet to keep the
// motor refreshing, but do not send actual joystick values
if (ROBOTMODE & ROBOT_AUTON)
sendPacket('X', X_PACKET_LENGTH, xPacketDefaultValues);
// Otherwise in enable mode, send the received joystick values
else
sendPacket('X', X_PACKET_LENGTH, interBuf+1);
x2OkToSend = FALSE;
udpNewData = FALSE;
// Wait for reply before sending another packet
waitfor( x2OkToSend || DelayMs(500) );
if (!x2OkToSend) { // no reply came within 500ms timeout
//printf("disable");
disableRobot();
}
}
}
costate {
// If auto bit is set, blink the LED fast
if (ROBOTMODE & ROBOT_AUTON) {
BitWrPortI(PDDR, &PDDRShadow, 0, 0); // turn on user LED
waitfor (DelayMs(100));
BitWrPortI(PDDR, &PDDRShadow, 1, 0); // turn off user LED
waitfor (DelayMs(100));
// Otherwise if disable bit is set, blink the LED slowly
} else if (ROBOTMODE & ROBOT_DISABLE) {
BitWrPortI(PDDR, &PDDRShadow, 0, 0); // turn on user LED
waitfor (DelayMs(500));
BitWrPortI(PDDR, &PDDRShadow, 1, 0); // turn off user LED
waitfor (DelayMs(500));
}
}
} // while(1)
} // main
