gpsEnablePassthroughResult_e gpsEnablePassthrough(void) { if (gpsData.state != GPS_RECEIVING_DATA) return GPS_PASSTHROUGH_NO_GPS; serialPort_t *gpsPassthroughPort = findOpenSerialPort(FUNCTION_GPS_PASSTHROUGH); if (gpsPassthroughPort) { waitForSerialPortToFinishTransmitting(gpsPassthroughPort); serialSetBaudRate(gpsPassthroughPort, serialConfig->gps_passthrough_baudrate); } else { gpsPassthroughPort = openSerialPort(FUNCTION_GPS_PASSTHROUGH, NULL, serialConfig->gps_passthrough_baudrate, MODE_RXTX, SERIAL_NOT_INVERTED); if (!gpsPassthroughPort) { return GPS_PASSTHROUGH_NO_SERIAL_PORT; } } LED0_OFF; LED1_OFF; while(1) { if (serialTotalBytesWaiting(gpsPort)) { LED0_ON; serialWrite(gpsPassthroughPort, serialRead(gpsPort)); LED0_OFF; } if (serialTotalBytesWaiting(gpsPassthroughPort)) { LED1_ON; serialWrite(gpsPort, serialRead(gpsPassthroughPort)); LED1_OFF; } } return GPS_PASSTHROUGH_ENABLED; }
void serialCom(void) { uint8_t c; static uint8_t offset; static uint8_t dataSize; static enum _serial_state { IDLE, HEADER_START, HEADER_M, HEADER_ARROW, HEADER_SIZE, HEADER_CMD, } c_state = IDLE; // in cli mode, all serial stuff goes to here. enter cli mode by sending # if (cliMode) { cliProcess(); return; } while (serialTotalBytesWaiting(core.mainport)) { c = serialRead(core.mainport); if (c_state == IDLE) { c_state = (c == '$') ? HEADER_START : IDLE; if (c_state == IDLE) evaluateOtherData(c); // evaluate all other incoming serial data } else if (c_state == HEADER_START) { c_state = (c == 'M') ? HEADER_M : IDLE; } else if (c_state == HEADER_M) { c_state = (c == '<') ? HEADER_ARROW : IDLE; } else if (c_state == HEADER_ARROW) { if (c > INBUF_SIZE) { // now we are expecting the payload size c_state = IDLE; continue; } dataSize = c; offset = 0; checksum = 0; indRX = 0; checksum ^= c; c_state = HEADER_SIZE; // the command is to follow } else if (c_state == HEADER_SIZE) { cmdMSP = c; checksum ^= c; c_state = HEADER_CMD; } else if (c_state == HEADER_CMD && offset < dataSize) { checksum ^= c; inBuf[offset++] = c; } else if (c_state == HEADER_CMD && offset >= dataSize) { if (checksum == c) { // compare calculated and transferred checksum evaluateCommand(); // we got a valid packet, evaluate it } c_state = IDLE; } } if (!cliMode && feature(FEATURE_TELEMETRY)) { // The first condition should never evaluate to true but I'm putting it here anyway - silpstream sendTelemetry(); } }
/*--------------------------------------------------------------------------- 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 serialCom(void) { uint8_t c; static uint8_t offset; static uint8_t dataSize; static enum _serial_state { IDLE, HEADER_START, HEADER_M, HEADER_ARROW, HEADER_SIZE, HEADER_CMD, } c_state = IDLE; // in cli mode, all serial stuff goes to here. enter cli mode by sending # if (cliMode) { cliProcess(); return; } while (serialTotalBytesWaiting(core.mainport)) { c = serialRead(core.mainport); if (c_state == IDLE) { c_state = (c == '$') ? HEADER_START : IDLE; if (c_state == IDLE && !f.ARMED) evaluateOtherData(c); // if not armed evaluate all other incoming serial data } else if (c_state == HEADER_START) { c_state = (c == 'M') ? HEADER_M : IDLE; } else if (c_state == HEADER_M) { c_state = (c == '<') ? HEADER_ARROW : IDLE; } else if (c_state == HEADER_ARROW) { if (c > INBUF_SIZE) { // now we are expecting the payload size c_state = IDLE; continue; } dataSize = c; offset = 0; checksum = 0; indRX = 0; checksum ^= c; c_state = HEADER_SIZE; // the command is to follow } else if (c_state == HEADER_SIZE) { cmdMSP = c; checksum ^= c; c_state = HEADER_CMD; } else if (c_state == HEADER_CMD && offset < dataSize) { checksum ^= c; inBuf[offset++] = c; } else if (c_state == HEADER_CMD && offset >= dataSize) { if (checksum == c) { // compare calculated and transferred checksum evaluateCommand(); // we got a valid packet, evaluate it } c_state = IDLE; } } }
void serialCom(void) { uint8_t c; int i; for (i = 0; i < numTelemetryPorts; i++) { currentPortState = &ports[i]; // in cli mode, all serial stuff goes to here. enter cli mode by sending # if (cliMode) { cliProcess(); return; } if (pendReboot) systemReset(false); // noreturn while (serialTotalBytesWaiting(currentPortState->port)) { c = serialRead(currentPortState->port); if (currentPortState->c_state == IDLE) { currentPortState->c_state = (c == '$') ? HEADER_START : IDLE; if (currentPortState->c_state == IDLE && !f.ARMED) evaluateOtherData(c); // if not armed evaluate all other incoming serial data } else if (currentPortState->c_state == HEADER_START) { currentPortState->c_state = (c == 'M') ? HEADER_M : IDLE; } else if (currentPortState->c_state == HEADER_M) { currentPortState->c_state = (c == '<') ? HEADER_ARROW : IDLE; } else if (currentPortState->c_state == HEADER_ARROW) { if (c > INBUF_SIZE) { // now we are expecting the payload size currentPortState->c_state = IDLE; continue; } currentPortState->dataSize = c; currentPortState->offset = 0; currentPortState->checksum = 0; currentPortState->indRX = 0; currentPortState->checksum ^= c; currentPortState->c_state = HEADER_SIZE; // the command is to follow } else if (currentPortState->c_state == HEADER_SIZE) { currentPortState->cmdMSP = c; currentPortState->checksum ^= c; currentPortState->c_state = HEADER_CMD; } else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset < currentPortState->dataSize) { currentPortState->checksum ^= c; currentPortState->inBuf[currentPortState->offset++] = c; } else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset >= currentPortState->dataSize) { if (currentPortState->checksum == c) { // compare calculated and transferred checksum evaluateCommand(); // we got a valid packet, evaluate it } currentPortState->c_state = IDLE; } } } }
int8_t gpsSetPassthrough(void) { if (gpsData.state != GPS_RECEIVINGDATA) return -1; LED0_OFF; LED1_OFF; while(1) { if (serialTotalBytesWaiting(core.gpsport)) { LED0_ON; serialWrite(core.mainport, serialRead(core.gpsport)); LED0_OFF; } if (serialTotalBytesWaiting(core.mainport)) { LED1_ON; serialWrite(core.gpsport, serialRead(core.mainport)); LED1_OFF; } } }
static void hottCheckSerialData(uint32_t currentMicros) { static bool lookingForRequest = true; uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort); if (bytesWaiting <= 1) { return; } if (bytesWaiting != 2) { flushHottRxBuffer(); lookingForRequest = true; return; } if (lookingForRequest) { lastHoTTRequestCheckAt = currentMicros; lookingForRequest = false; return; } else { bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE; if (!enoughTimePassed) { return; } lookingForRequest = true; } uint8_t requestId = serialRead(hottPort); uint8_t address = serialRead(hottPort); if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) { /* * FIXME the first byte of the HoTT request frame is ONLY either 0x80 (binary mode) or 0x7F (text mode). * The binary mode is read as 0x00 (error reading the upper bit) while the text mode is correctly decoded. * The (requestId == 0) test is a workaround for detecting the binary mode with no ambiguity as there is only * one other valid value (0x7F) for text mode. * The error reading for the upper bit should nevertheless be fixed */ processBinaryModeRequest(address); } }
void sendTelemetry(void) { if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED) return; if (serialTotalBytesWaiting(core.telemport) != 0) return; if (millis() - lastCycleTime >= CYCLETIME) { lastCycleTime = millis(); cycleNum++; // Sent every 125ms sendAccel(); sendTelemetryTail(); if ((cycleNum % 4) == 0) { // Sent every 500ms sendBaro(); sendHeading(); sendTelemetryTail(); } if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); if (feature(FEATURE_VBAT)) { sendVoltage(); sendVoltageAmp(); } if (sensors(SENSOR_GPS)) sendGPS(); sendTelemetryTail(); } if (cycleNum == 40) { //Frame 3: Sent every 5s cycleNum = 0; sendTime(); sendTelemetryTail(); } } }
void gpsThread(void) { // read out available GPS bytes if (gpsPort) { while (serialTotalBytesWaiting(gpsPort)) gpsNewData(serialRead(gpsPort)); } switch (gpsData.state) { case GPS_UNKNOWN: break; case GPS_INITIALIZING: case GPS_CHANGE_BAUD: case GPS_CONFIGURE: gpsInitHardware(); break; case GPS_LOST_COMMUNICATION: gpsData.errors++; // try another rate gpsData.baudrateIndex++; gpsData.baudrateIndex %= GPS_INIT_ENTRIES; gpsData.lastMessage = millis(); // TODO - move some / all of these into gpsData GPS_numSat = 0; DISABLE_STATE(GPS_FIX); gpsSetState(GPS_INITIALIZING); break; case GPS_RECEIVING_DATA: // check for no data/gps timeout/cable disconnection etc if (millis() - gpsData.lastMessage > GPS_TIMEOUT) { // remove GPS from capability sensorsClear(SENSOR_GPS); gpsSetState(GPS_LOST_COMMUNICATION); } break; } }
static void hottCheckSerialData(uint32_t currentMicros) { static bool lookingForRequest = true; uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort); if (bytesWaiting <= 1) { return; } if (bytesWaiting != 2) { flushHottRxBuffer(); lookingForRequest = true; return; } if (lookingForRequest) { lastHoTTRequestCheckAt = currentMicros; lookingForRequest = false; return; } else { bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= HOTT_RX_SCHEDULE; if (!enoughTimePassed) { return; } lookingForRequest = true; } uint8_t requestId = serialRead(hottPort); uint8_t address = serialRead(hottPort); if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) { processBinaryModeRequest(address); } }
/*--------------------------------------------------------------------------- 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 ); }
uint8_t _menu_received(void) { return serialTotalBytesWaiting(core.menuport); }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; serialPort_t* loopbackPort = NULL; systemInit(); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif checkFirstTime(false); readEEPROM(); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; mcfg.power_adc_channel = 0; } adcInit(&adc_params); initBoardAlignment(); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); 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; else pwm_params.airplane = false; //pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too pwm_params.useUART = feature(FEATURE_GPS); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum 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; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = mcfg.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = mcfg.midrc; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; case 9: pwm_params.adcChannel = PWM8; break; default: pwm_params.adcChannel = 0; break; } pwmInit(&pwm_params); // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled for (i = 0; i < RC_CHANS; i++) rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: spektrumInit(&rcReadRawFunc); break; case SERIALRX_SBUS: sbusInit(&rcReadRawFunc); break; case SERIALRX_SUMD: sumdInit(&rcReadRawFunc); break; #if defined(SKYROVER) case SERIALRX_HEXAIRBOT: hexairbotInit(&rcReadRawFunc); break; #endif } } else { // spektrum and GPS are mutually exclusive // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. // gpsInit will return if FEATURE_GPS is not enabled. // Sanity check below - protocols other than NMEA do not support baud rate autodetection if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0) mcfg.gps_baudrate = 0; gpsInit(mcfg.gps_baudrate); } #ifdef SONAR // sonar stuff only works with PPM if (feature(FEATURE_PPM)) { if (feature(FEATURE_SONAR)) Sonar_init(); } #endif 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; serialInit(mcfg.serial_baudrate); DEBUG_PRINT("Booting..\r\n"); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsAutodetect(); imuInit(); // Mag is initialized inside imuInit // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); if (feature(FEATURE_SOFTSERIAL)) { setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted); #ifdef SOFTSERIAL_LOOPBACK loopbackPort = (serialPort_t*)&(softSerialPorts[0]); serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n"); #endif } if (feature(FEATURE_TELEMETRY)) initTelemetry(); previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = CALIBRATING_ACC_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES; calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLES_25 = 1; DEBUG_PRINT("Start\r\n"); // loopy while (1) { loop(); #ifdef SOFTSERIAL_LOOPBACK if (loopbackPort) { while (serialTotalBytesWaiting(loopbackPort)) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); //serialWrite(core.mainport, b); }; } #endif } }
/*--------------------------------------------------------------------------- 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 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) { uint8_t i; drv_pwm_config_t pwm_params; bool sensorsOK = false; initPrintfSupport(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); #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 NAZE detectHardwareRevision(); #endif systemInit(); #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 ledInit(); #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioMode = Mode_Out_OD, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, .isInverted = false }; #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); #endif #ifdef NAZE updateHardwareRevision(); #endif #ifdef USE_I2C #ifdef NAZE if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #else // Configure the rest of the stuff i2cInit(I2C_DEVICE); #endif #endif #if !defined(SPARKY) drv_adc_config_t adc_params; 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 // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination); // if gyro was not detected due to whatever reason, we give up now. if (!sensorsOK) failureMode(3); 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; imuInit(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif serialInit(&masterConfig.serialConfig); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(SERIAL_PORT_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #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); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); failsafe = failsafeInit(&masterConfig.rxConfig); beepcodeInit(failsafe); rxInit(&masterConfig.rxConfig, failsafe); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { Sonar_init(); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) initTelemetry(); #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. // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayEnablePageCycling(); #endif } #endif } #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(); } }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; bool sensorsOK = false; #ifdef SOFTSERIAL_LOOPBACK serialPort_t* loopbackPort1 = NULL; serialPort_t* loopbackPort2 = NULL; #endif initEEPROM(); checkFirstTime(false); readEEPROM(); systemInit(mcfg.emf_avoidance); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif activateConfig(); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; mcfg.power_adc_channel = 0; } adcInit(&adc_params); // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); initBoardAlignment(); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsOK = sensorsAutodetect(); // production debug output #ifdef PROD_DEBUG productionDebug(); #endif // if gyro was not detected due to whatever reason, we give up now. if (!sensorsOK) failureMode(3); 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; imuInit(); // Mag is initialized inside imuInit mixerInit(); // this will set core.useServo var depending on mixer type serialInit(mcfg.serial_baudrate); // when using airplane/wing mixer, servo/motor outputs are remapped if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum 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; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = mcfg.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = mcfg.midrc; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; case 9: pwm_params.adcChannel = PWM8; break; default: pwm_params.adcChannel = 0; break; } pwmInit(&pwm_params); core.numServos = pwm_params.numServos; // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled for (i = 0; i < RC_CHANS; i++) rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: spektrumInit(&rcReadRawFunc); break; case SERIALRX_SBUS: sbusInit(&rcReadRawFunc); break; case SERIALRX_SUMD: sumdInit(&rcReadRawFunc); break; case SERIALRX_MSP: mspInit(&rcReadRawFunc); break; } } else { // spektrum and GPS are mutually exclusive // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. // gpsInit will return if FEATURE_GPS is not enabled. gpsInit(mcfg.gps_baudrate); } #ifdef SONAR // sonar stuff only works with PPM if (feature(FEATURE_PPM)) { if (feature(FEATURE_SONAR)) Sonar_init(); } #endif if (feature(FEATURE_SOFTSERIAL)) { //mcfg.softserial_baudrate = 19200; // Uncomment to override config value setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted); setupSoftSerialSecondary(mcfg.softserial_2_inverted); #ifdef SOFTSERIAL_LOOPBACK loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]); serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n"); loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]); serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n"); #endif //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial. } if (feature(FEATURE_TELEMETRY)) initTelemetry(); previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = CALIBRATING_ACC_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES; calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLE = 1; // loopy while (1) { loop(); #ifdef SOFTSERIAL_LOOPBACK if (loopbackPort1) { while (serialTotalBytesWaiting(loopbackPort1)) { uint8_t b = serialRead(loopbackPort1); serialWrite(loopbackPort1, b); //serialWrite(core.mainport, 0x01); //serialWrite(core.mainport, b); }; } if (loopbackPort2) { while (serialTotalBytesWaiting(loopbackPort2)) { #ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data serialRead(loopbackPort2); #else uint8_t b = serialRead(loopbackPort2); serialWrite(loopbackPort2, b); //serialWrite(core.mainport, 0x02); //serialWrite(core.mainport, b); #endif // OLIMEXINO }; } #endif } }
void cliProcess(void) { if (!cliMode) { cliMode = 1; cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n"); cliPrompt(); } while (serialTotalBytesWaiting(core.mainport)) { uint8_t c = serialRead(core.mainport); if (c == '\t' || c == '?') { // do tab completion const clicmd_t *cmd, *pstart = NULL, *pend = NULL; unsigned int i = bufferIndex; for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0)) continue; if (!pstart) pstart = cmd; pend = cmd; } if (pstart) { /* Buffer matches one or more commands */ for (; ; bufferIndex++) { if (pstart->name[bufferIndex] != pend->name[bufferIndex]) break; if (!pstart->name[bufferIndex] && bufferIndex < sizeof(cliBuffer) - 2) { /* Unambiguous -- append a space */ cliBuffer[bufferIndex++] = ' '; cliBuffer[bufferIndex] = '\0'; break; } cliBuffer[bufferIndex] = pstart->name[bufferIndex]; } } if (!bufferIndex || pstart != pend) { /* Print list of ambiguous matches */ cliPrint("\r\033[K"); for (cmd = pstart; cmd <= pend; cmd++) { cliPrint(cmd->name); cliWrite('\t'); } cliPrompt(); i = 0; /* Redraw prompt */ } for (; i < bufferIndex; i++) cliWrite(cliBuffer[i]); } else if (!bufferIndex && c == 4) { cliExit(cliBuffer); return; } else if (c == 12) { // clear screen cliPrint("\033[2J\033[1;1H"); cliPrompt(); } else if (bufferIndex && (c == '\n' || c == '\r')) { // enter pressed clicmd_t *cmd = NULL; clicmd_t target; cliPrint("\r\n"); cliBuffer[bufferIndex] = 0; // null terminate target.name = cliBuffer; target.param = NULL; cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare); if (cmd) cmd->func(cliBuffer + strlen(cmd->name) + 1); else cliPrint("ERR: Unknown command, try 'help'"); memset(cliBuffer, 0, sizeof(cliBuffer)); bufferIndex = 0; // 'exit' will reset this flag, so we don't need to print prompt again if (!cliMode) return; cliPrompt(); } else if (c == 127) { // backspace if (bufferIndex) { cliBuffer[--bufferIndex] = 0; cliPrint("\010 \010"); } } else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) { if (!bufferIndex && c == 32) continue; cliBuffer[bufferIndex++] = c; cliWrite(c); } } }
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(); 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)); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) 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.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; 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 = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) 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); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, #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); spiInit(SPI4); spiInit(SPI5); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else #if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(STM32F4DISCOVERY) i2cInit(I2C_DEVICE_INT); 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, currentProfile->mag_declination)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(3); } 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); cliInit(&masterConfig.serialConfig); failsafeInit(&masterConfig.rxConfig); 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(&masterConfig.batteryConfig); } #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(); } #endif #if defined(SPRACINGF3) || defined(CC3D) || defined(COLIBRI) || defined(REVO) 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 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 #include <stdio.h> #include "stm32f4xx_rcc.h" #include "stm32f4xx_gpio.h" GPIO_InitTypeDef GPIO_InitStruct; int main(void) { RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_14 | GPIO_Pin_13 | GPIO_Pin_12; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOD, &GPIO_InitStruct); printf("Hello World!\r\n"); hello(); while (1) { static int count = 0; static int i; for (i = 0; i < 10000000; ++i) ; GPIO_ToggleBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15); printf("%d\r\n", ++count); } //init(); /* while (1) { //loop(); int x = 1;//processLoopback(); }*/ }
bool canSendFrSkyTelemetry(void) { return serialTotalBytesWaiting(frskyPort) == 0; }
static void flushHottRxBuffer(void) { while (serialTotalBytesWaiting(hottPort) > 0) { serialRead(hottPort); } }
uint8_t HardwareSerial::available(void) { serialPort_t * port = (serialPort_t *)this->_uart; return serialTotalBytesWaiting(port); }