/*! \brief sdlog thread. * * Test logging to microSD card on e407. * */ msg_t sdlog_thread(void *p) { void * arg __attribute__ ((unused)) = p; static const evhandler_t evhndl_sdclog[] = { sdc_log_data, sdc_log_data, sdc_log_data, sdc_log_data, sdc_log_data }; struct EventListener el0, el1, el2, el3, el4; chRegSetThreadName("sdlog_thread"); #ifdef DEBUG_SDCLOG /*chThdSleepMilliseconds(1000);*/ #endif SDCLOGDBG("Start sdlog thread\r\n"); // init structure datafile_state.log_sequence = 0; datafile_state.write_errors = 0; datafile_state.sd_log_opened = false; sdc_reset_fp_index(); sdc_init_eod((uint8_t)0xa5); // Assert data is halfword aligned if(((sizeof(GENERIC_message)*8) % 16) != 0) { SDCLOGDBG("%s: GENERIC message is not halfword aligned.\r\n", __func__); return (SDC_ASSERT_ERROR); } // Assert we will not overflow Payload if( (sizeof(MPU9150_read_data) > (sizeof(datafile_state.log_data.data)-1)) || (sizeof(MPL3115A2_read_data) > (sizeof(datafile_state.log_data.data)-1)) || (sizeof(ADIS16405_burst_data) > (sizeof(datafile_state.log_data.data)-1))) { SDCLOGDBG("%s: DATA size is too large\r\n"); return (SDC_ASSERT_ERROR); } chEvtRegister(&mpl3115a2_data_event , &el0, MPL3115A2); chEvtRegister(&adis_spi_burst_data_captured, &el1, ADIS16405); chEvtRegister(&mpu9150_data_event , &el2, MPU9150); chEvtRegister(&sdc_halt_event , &el3, SENSOR_LOG_HALT); chEvtRegister(&sdc_start_event , &el4, SENSOR_LOG_START); while(1) { if(!fs_stop) { chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(50))); } else { chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout((1<<SENSOR_LOG_START), MS2ST(50))); } } return -1; }
static THD_FUNCTION(thSerEcho, arg) { (void)arg; chRegSetThreadName("SerEcho"); event_listener_t elSerData; eventflags_t flags; chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1)); while (!chThdShouldTerminateX()) { chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10)); flags = chEvtGetAndClearFlags(&elSerData); if (flags & CHN_INPUT_AVAILABLE) { msg_t charbuf; do { charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE); if ( charbuf != Q_TIMEOUT ) { chSequentialStreamPut(&SD1, charbuf); } } while (charbuf != Q_TIMEOUT); } } }
int main(void) { halInit(); chSysInit(); static const evhandler_t evhndl[] = { }; // CDC. sduObjectInit(&SDU1); sduStart(&SDU1, &serusbcfg); usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1500); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); chThdSleepMilliseconds(4000); uart_init(); chThdCreateStatic(uart_thread_wa, sizeof(uart_thread_wa), NORMALPRIO, uart_thread, NULL); while (true) { palTogglePad(GPIOC, GPIOC_LED); chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100))); } }
static void cmd_gps_passthrough(BaseSequentialStream *chp, int argc, char *argv[]) { (void)argc; (void)argv; static const SerialConfig sc = { 9600, 0, USART_CR2_STOP1_BITS | USART_CR2_LINEN, 0}; sdStart(&SD1, &sc); EventListener elSerData; flagsmask_t flags; chEvtRegisterMask(chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1)); while (TRUE) { chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10)); flags = chEvtGetAndClearFlags(&elSerData); if (flags & CHN_INPUT_AVAILABLE) { msg_t charbuf; do { charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE); if ( charbuf != Q_TIMEOUT ) { chSequentialStreamPut(chp, charbuf); } } while (charbuf != Q_TIMEOUT); } } }
/** * Command processing thread. */ msg_t CmdExecutor::main(void){ this->setName("CmdExecutor"); eventmask_t evt = 0; struct EventListener el_command_long; chEvtRegisterMask(&event_mavlink_in_command_long, &el_command_long, EVMSK_MAVLINK_IN_COMMAND_LONG); /* wait modems */ while(GlobalFlags.messaging_ready == 0) chThdSleepMilliseconds(50); /* main loop */ while(!chThdShouldTerminate()){ evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_COMMAND_LONG, MS2ST(50)); switch (evt){ case EVMSK_MAVLINK_IN_COMMAND_LONG: executeCmd(&mavlink_in_command_long_struct); break; default: break; } } chEvtUnregister(&event_mavlink_in_command_long, &el_command_long); chThdExit(0); return 0; }
static void evt3_execute(void) { eventmask_t m; /* * Tests various timeout situations. */ m = chEvtWaitOneTimeout(ALL_EVENTS, TIME_IMMEDIATE); test_assert(1, m == 0, "spurious event"); m = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE); test_assert(2, m == 0, "spurious event"); m = chEvtWaitAllTimeout(ALL_EVENTS, TIME_IMMEDIATE); test_assert(3, m == 0, "spurious event"); m = chEvtWaitOneTimeout(ALL_EVENTS, 10); test_assert(4, m == 0, "spurious event"); m = chEvtWaitAnyTimeout(ALL_EVENTS, 10); test_assert(5, m == 0, "spurious event"); m = chEvtWaitAllTimeout(ALL_EVENTS, 10); test_assert(6, m == 0, "spurious event"); }
static msg_t Thread_mpu9150_int(void* arg) { (void) arg; static const evhandler_t evhndl_mpu9150[] = { mpu9150_int_event_handler }; struct EventListener evl_mpu9150; chRegSetThreadName("mpu9150_int"); chEvtRegister(&mpu9150_int_event, &evl_mpu9150, 0); while (TRUE) { chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50))); } return -1; }
/*! \brief ADIS Newdata Thread */ static msg_t Thread_adis_newdata(void *arg) { (void)arg; chRegSetThreadName("adis_newdata"); static const evhandler_t evhndl_newdata[] = { adis_newdata_handler }; struct EventListener evl_spi_cb2; chEvtRegister(&adis_spi_cb_newdata, &evl_spi_cb2, 0); while (TRUE) { chEvtDispatch(evhndl_newdata, chEvtWaitOneTimeout((eventmask_t)1, US2ST(50))); } return -1; }
static msg_t MmcReaderDmThread(void *sdp){ chRegSetThreadName("MmcReaderDm"); struct EventListener el_storage_request_count_dm; struct EventListener el_storage_request_dm; chEvtRegisterMask(&event_mavlink_oblique_storage_request_count_dm, &el_storage_request_count_dm, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM); chEvtRegisterMask(&event_mavlink_oblique_storage_request_dm, &el_storage_request_dm, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM); eventmask_t evt = 0; while (!chThdShouldTerminate()){ evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM | EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM, MS2ST(50)); if (!mmcIsCardInserted(&MMCD1)) continue; else{ switch (evt){ case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM): bnapStorageAcquire(&Storage); mavlink_oblique_storage_count_struct.count = Storage.used; bnapStorageRelease(&Storage); chEvtBroadcastFlags(&event_mavlink_oblique_storage_count, EVMSK_MAVLINK_OBLIQUE_STORAGE_COUNT); break; case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM): _oblique_storage_request_handler_dm(sdp); break; default: break; } } } chEvtUnregister(&event_mavlink_oblique_storage_request_count_dm, &el_storage_request_count_dm); chEvtUnregister(&event_mavlink_oblique_storage_request_dm, &el_storage_request_dm); return 0; }
msg_t data_udp_send_thread(void *p) { void * arg __attribute__ ((unused)) = p; static const evhandler_t evhndl_mpu9150[] = { data_udp_send_mpu9150_data }; struct EventListener evl_mpu9150; err_t err; ip_addr_t ip_addr_sensor; ip_addr_t ip_addr_fc; chRegSetThreadName("data_udp_send_thread"); chEvtRegister(&mpu9150_data_event, &evl_mpu9150, 0); IMU_A_IP_ADDR(&ip_addr_sensor); IP_PSAS_FC(&ip_addr_fc); mpu9150_mac_info.conn = netconn_new( NETCONN_UDP ); /* Bind to the local address, or to ANY address */ // netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY) err = netconn_bind(mpu9150_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port if (err == ERR_OK) { /* Connect to specific address or a broadcast address */ /* * \todo Understand why a UDP needs a connect... * This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally. * */ // netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT ); err = netconn_connect(mpu9150_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A ); if(err == ERR_OK) { while (TRUE) { chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50))); } } return RDY_RESET; } return RDY_RESET; }
/*! \brief ADIS DIO1 thread * * For burst mode transactions t_readrate is 1uS * */ static msg_t Thread_adis_dio1(void *arg) { (void)arg; static const evhandler_t evhndl_dio1[] = { adis_burst_read_handler, //adis_read_id_handler, adis_spi_cb_txdone_handler, adis_release_bus }; struct EventListener evl_dio; struct EventListener evl_spi_ev; struct EventListener evl_spi_release; chRegSetThreadName("adis_dio"); chEvtRegister(&adis_dio1_event, &evl_dio, 0); chEvtRegister(&adis_spi_cb_txdone_event, &evl_spi_ev, 1); chEvtRegister(&adis_spi_cb_releasebus, &evl_spi_release, 2); while (TRUE) { chEvtDispatch(evhndl_dio1, chEvtWaitOneTimeout((EVENT_MASK(2)|EVENT_MASK(1)|EVENT_MASK(0)), US2ST(50))); } return -1; }
static msg_t MmcWriterThread(void *arg){ chRegSetThreadName("MmcWriter"); (void)arg; struct EventListener el_gps_raw_int; chEvtRegisterMask(&event_mavlink_gps_raw_int, &el_gps_raw_int, EVMSK_MAVLINK_GPS_RAW_INT); eventmask_t evt = 0; /* wait until card not ready */ NOT_READY: while (!mmcIsCardInserted(&MMCD1)) chThdSleep(SDC_POLLING_INTERVAL); chThdSleep(SDC_POLLING_INTERVAL); if (!mmcIsCardInserted(&MMCD1)) goto NOT_READY; else _insert_handler(); /* main work cycle */ while (!chThdShouldTerminate()){ evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_GPS_RAW_INT, WRITE_TMO); if (!mmcIsCardInserted(&MMCD1)){ _remove_handler(); goto NOT_READY; } else{ bnapStorageAcquire(&Storage); bnapStorageDoRecord(&Storage); bnapStorageRelease(&Storage); } } chEvtUnregister(&event_mavlink_gps_raw_int, &el_gps_raw_int); chThdExit(0); (void)evt; return 0; }
eventmask_t BaseThread::waitOneEventTimeout(eventmask_t ewmask, systime_t time) { return chEvtWaitOneTimeout(ewmask, time); }
int main(void) { static Thread *shelltp = NULL; static const evhandler_t evhndl[] = { InsertHandler, RemoveHandler, packetReceivedHandler }; struct EventListener el0, el1, el2; chEvtRegister(&packet_event, &el2, 0); chEvtInit(&packet_event); /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&SDU1); sduStart(&SDU1, &serusbcfg); /* * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1000); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); /* * Shell manager initialization. */ shellInit(); /* * Activates the serial driver 6 and SDC driver 1 using default * configuration. */ sdStart(&SD6, NULL); sdcStart(&SDCD1, NULL); /* * Activates the card insertion monitor. */ tmr_init(&SDCD1); /* * Creates the blinker thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Creates the LWIP threads (it changes priority internally). */ chThdCreateStatic(wa_lwip_thread, LWIP_THREAD_STACK_SIZE, NORMALPRIO + 2, lwip_thread, NULL); /* * Creates the HTTP thread (it changes priority internally). */ /* chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1, http_server, NULL); */ WORKING_AREA(wa_udp_receive_server, PWM_REC_STACK_SIZE); chThdCreateStatic(wa_udp_receive_server, sizeof(wa_udp_receive_server), NORMALPRIO +1, udp_receive_server_init, NULL); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and listen for events. */ chEvtRegister(&inserted_event, &el0, 0); chEvtRegister(&removed_event, &el1, 1); while (TRUE) { if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE)) shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO); else if (chThdTerminated(shelltp)) { chThdRelease(shelltp); /* Recovers memory of the previous shell. */ shelltp = NULL; /* Triggers spawning of a new shell. */ } if (palReadPad(GPIOA, GPIOA_BUTTON_WKUP) != 0) { } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500))); } }
int Mp3Decode(const char* pszFile) { int nResult = 0; BYTE* pInData = g_Mp3InBuffer; UINT unInDataLeft = 0; FIL fIn; UINT bEof = FALSE; UINT bOutOfData = FALSE; MP3FrameInfo mp3FrameInfo; uint32_t unDmaBufMode = 0; g_pMp3DmaBufferPtr = g_pMp3DmaBuffer; g_pMp3DecoderThread = chThdSelf(); FRESULT errFS = f_open(&fIn, pszFile, FA_READ); if(errFS != FR_OK) { chprintf((BaseChannel*)&SD2, "Mp3Decode: Failed to open file \"%s\" for reading, err=%d\r\n", pszFile, errFS); return -1; } HMP3Decoder hMP3Decoder = MP3InitDecoder(); if(hMP3Decoder == NULL) { chprintf((BaseChannel*)&SD2, "Mp3Decode: Failed to initialize mp3 decoder engine\r\n"); return -2; } chprintf((BaseChannel*)&SD2, "Mp3Decode: Start decoding \"%s\"\r\n", pszFile); char szArtist[80]; char szTitle[80]; palSetPad(GPIOD, 12); // green LED Mp3ReadId3V2Tag(&fIn, szArtist, sizeof(szArtist), szTitle, sizeof(szTitle)); palClearPad(GPIOD, 12); // green LED if(szArtist[0] != 0 || szTitle[0] != 0) { chprintf((BaseChannel*)&SD2, "Mp3Decode: Now playing (ID3v2): %s - %s\r\n", szArtist, szTitle); } int nDecodeRes = ERR_MP3_NONE; UINT unFramesDecoded = 0; do { if(unInDataLeft < (2 * MAINBUF_SIZE) && (!bEof)) { UINT unRead = Mp3FillReadBuffer(pInData, unInDataLeft, &fIn); unInDataLeft += unRead; pInData = g_Mp3InBuffer; if(unRead == 0) { bEof = 1; } } // find start of next MP3 frame - assume EOF if no sync found int nOffset = MP3FindSyncWord(pInData, unInDataLeft); if(nOffset < 0) { bOutOfData = TRUE; break; } pInData += nOffset; unInDataLeft -= nOffset; // decode one MP3 frame - if offset < 0 then bytesLeft was less than a full frame nDecodeRes = MP3Decode(hMP3Decoder, &pInData, (int*)&unInDataLeft, (short*)g_pMp3OutBuffer, 0); switch(nDecodeRes) { case ERR_MP3_NONE: { MP3GetLastFrameInfo(hMP3Decoder, &mp3FrameInfo); if(unFramesDecoded == 0) { chprintf((BaseChannel*)&SD2, "Mp3Decode: %d Hz %d Bit %d Channels\r\n", mp3FrameInfo.samprate, mp3FrameInfo.bitsPerSample, mp3FrameInfo.nChans); if((mp3FrameInfo.samprate > 48000) || (mp3FrameInfo.bitsPerSample != 16) || (mp3FrameInfo.nChans != 2)) { chprintf((BaseChannel*)&SD2, "Mp3Decode: incompatible MP3 file.\r\n"); nResult = -5; break; } } if((unFramesDecoded) % 100 == 0) { chprintf((BaseChannel*)&SD2, "Mp3Decode: frame %u, bitrate=%d\r\n", unFramesDecoded, mp3FrameInfo.bitrate); } unFramesDecoded++; g_pMp3OutBufferPtr = g_pMp3OutBuffer; uint32_t unOutBufferAvail= mp3FrameInfo.outputSamps; while(unOutBufferAvail > 0) { // fill up the whole dma buffer uint32_t unDmaBufferSpace = 0; if(unDmaBufMode == 0) { // fill the whole buffer // dma buf ptr was reset to beginning of the buffer unDmaBufferSpace = g_pMp3DmaBuffer + MP3_DMA_BUFFER_SIZE - g_pMp3DmaBufferPtr; } else if(unDmaBufMode == 1) { // fill the first half of the buffer // dma buf ptr was reset to beginning of the buffer unDmaBufferSpace = g_pMp3DmaBuffer + (MP3_DMA_BUFFER_SIZE / 2) - g_pMp3DmaBufferPtr; } else { // fill the last half of the buffer // dma buf ptr was reset to middle of the buffer unDmaBufferSpace = g_pMp3DmaBuffer + MP3_DMA_BUFFER_SIZE - g_pMp3DmaBufferPtr; } uint32_t unCopy = unDmaBufferSpace > unOutBufferAvail ? unOutBufferAvail : unDmaBufferSpace; if(unCopy > 0) { memcpy(g_pMp3DmaBufferPtr, g_pMp3OutBufferPtr, unCopy * sizeof(uint16_t)); unOutBufferAvail -= unCopy; g_pMp3OutBufferPtr += unCopy; unDmaBufferSpace -= unCopy; g_pMp3DmaBufferPtr += unCopy; } if(unDmaBufferSpace == 0) { // dma buffer full // see if this was the first run if(unDmaBufMode == 0) { // on the first buffer fill up, // start the dma transfer if(EVAL_AUDIO_Init(OUTPUT_DEVICE_HEADPHONE, 80, (uint32_t)mp3FrameInfo.samprate)) { chprintf((BaseChannel *) &SD2, "Mp3Decode: audio init failed\r\n"); nResult = -4; break; } EVAL_AUDIO_Play(g_pMp3DmaBuffer, MP3_DMA_BUFFER_SIZE * sizeof(uint16_t)); } // we must wait for the dma stream tx interrupt here eventmask_t em = chEvtWaitAny((eventmask_t)2 | 4 | 8); if(em & 8) { // stop requested chprintf((BaseChannel*)&SD2, "Mp3Decode: Stop requested\r\n"); nResult = 1; break; } if((em & 2) && (em & 4)) { chprintf((BaseChannel*)&SD2, "Mp3Decode: DMA out of sync (HT and TC both set)\r\n"); nResult = -3; break; } if(unDmaBufMode == 0 || unDmaBufMode == 2) { // the dma event we expect is "half transfer" (=2) if(em & 2) { // set up first half mode unDmaBufMode = 1; g_pMp3DmaBufferPtr = g_pMp3DmaBuffer; } else { chprintf((BaseChannel*)&SD2, "Mp3Decode: DMA out of sync (expected HT, got TC)\r\n"); nResult = -3; break; } } else { // the dma event we expect is "transfer complete" (=4) if(em & 4) { // set up last half mode unDmaBufMode = 2; g_pMp3DmaBufferPtr = g_pMp3DmaBuffer + (MP3_DMA_BUFFER_SIZE / 2); } else { chprintf((BaseChannel*)&SD2, "Mp3Decode: DMA out of sync (expected TC, got HT)\r\n"); nResult = -3; } } } } break; } case ERR_MP3_MAINDATA_UNDERFLOW: { // do nothing - next call to decode will provide more mainData break; } case ERR_MP3_FREE_BITRATE_SYNC: { break; } case ERR_MP3_INDATA_UNDERFLOW: { chprintf((BaseChannel*)&SD2, "Mp3Decode: Decoding error ERR_MP3_INDATA_UNDERFLOW\r\n"); bOutOfData = TRUE; break; } default: { chprintf((BaseChannel*)&SD2, "Mp3Decode: Decoding error %d\r\n", nDecodeRes); bOutOfData = TRUE; break; } } } while((!bOutOfData) && (nResult == 0)); chprintf((BaseChannel*)&SD2, "Mp3Decode: Finished decoding\r\n"); MP3FreeDecoder(hMP3Decoder); if(EVAL_AUDIO_Stop(CODEC_PDWN_HW)) { chprintf((BaseChannel*)&SD2, "Mp3Decode: Failed to stop audio\r\n"); } EVAL_AUDIO_DeInit(); f_close(&fIn); // this is the only legit way I know // to remvove still pending event flags // from the thread chEvtWaitOneTimeout(2, 50); chEvtWaitOneTimeout(4, 50); return nResult; }
eventmask_t Event::WaitOneTimeout(eventmask_t ewmask, systime_t time) { return chEvtWaitOneTimeout(ewmask, time); }
/* * Application entry point. */ int main(void) { static const evhandler_t evhndl[] = { InsertHandler, RemoveHandler }; Thread *shelltp = NULL; struct EventListener el0, el1; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Activates the serial driver 1 using the driver default configuration. */ sdStart(&SD1, NULL); /* * Shell manager initialization. */ shellInit(); /* * Initializes the MMC driver to work with SPI. */ palSetPadMode(IOPORT1, PIOA_CS_MMC, PAL_MODE_OUTPUT_PUSHPULL); palSetPad(IOPORT1, PIOA_CS_MMC); mmcObjectInit(&MMCD1); mmcStart(&MMCD1, &mmccfg); /* * Activates the card insertion monitor. */ tmr_init(&MMCD1); /* * Creates the blinker threads. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and listen for events. */ chEvtRegister(&inserted_event, &el0, 0); chEvtRegister(&removed_event, &el1, 1); while (TRUE) { if (!shelltp) shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO); else if (chThdTerminated(shelltp)) { chThdRelease(shelltp); /* Recovers memory of the previous shell. */ shelltp = NULL; /* Triggers spawning of a new shell. */ } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500))); } return 0; }
/* * Application entry point. */ int main(void) { enum led_status lstat = LST_INIT; EventListener el0; alert_status_t proto_st = ALST_INIT; alert_status_t bmp085_st = ALST_INIT; alert_status_t mpu6050_st = ALST_INIT; alert_status_t hmc5883_st = ALST_INIT; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); #ifdef BOARD_IMU_AHRF /* Clear DRDY pad */ palClearPad(GPIOA, GPIOA_DRDY); /* Activates serial */ sdStart(&SD1, NULL); sdStart(&SD2, NULL); /* Activate pwm */ pwmStart(&PWMD1, &pwm1cfg); /* Activate i2c */ i2cStart(&I2CD1, &i2c1cfg); /* Activate exti */ extStart(&EXTD1, &extcfg); #endif /* BOARD_IMU_AHRF */ #ifdef BOARD_CAPTAIN_PRO2 /* Activates serial */ sdStart(&SD3, NULL); sdStart(&SD4, NULL); /* Activate pwm */ pwmStart(&PWMD3, &pwm3cfg); pwmStart(&PWMD4, &pwm4cfg); pwmStart(&PWMD5, &pwm5cfg); /* Activate i2c */ i2cStart(&I2CD1, &i2c1cfg); /* Activate exti */ extStart(&EXTD1, &extcfg); /* Activate adc */ adcStart(&ADCD1, NULL); #endif /* BOARD_CAPTAIN_PRO2 */ /* alert subsys */ chEvtInit(&alert_event_source); chEvtRegister(&alert_event_source, &el0, 0); /* init devices */ pt_init(); chThdSleepMilliseconds(10); /* power on delay */ #ifdef HAS_DEV_BMP085 bmp085_init(); chThdSleepMilliseconds(50); /* init delay */ #endif #ifdef HAS_DEV_MS5611 ms5611_init(&ms5611cfg); chThdSleepMilliseconds(50); /* init delay */ #endif #ifdef HAS_DEV_MPU6050 mpu6050_init(&mpu6050cfg); chThdSleepMilliseconds(250); /* give some time for mpu6050 configuration */ #endif #ifdef HAS_DEV_HMC5883 hmc5883_init(&hmc5883cfg); #endif #ifdef HAS_DEV_SERVOPWM servopwm_init(&servopwmcfg); #endif #ifdef HAS_DEV_NTC10K ntc10k_init(); #endif #ifdef HAS_DEV_RPM rpm_init(); #endif #ifdef BOARD_IMU_AHRF /* Set DRDY pad */ palSetPad(GPIOA, GPIOA_DRDY); #endif while (TRUE) { eventmask_t msk = chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100)); if (msk & EVENT_MASK(0)) { flagsmask_t fl = chEvtGetAndClearFlags(&el0); if (fl & ALERT_FLAG_PROTO) proto_st = pt_get_status(); #ifdef HAS_DEV_MPU6050 if (fl & ALERT_FLAG_MPU6050) mpu6050_st = mpu6050_get_status(); #endif #ifdef HAS_DEV_HMC5883 if (fl & ALERT_FLAG_HMC5883) hmc5883_st = hmc5883_get_status(); #endif #ifdef HAS_DEV_BMP085 if (fl & ALERT_FLAG_BMP085) bmp085_st = bmp085_get_status(); #endif #ifdef HAS_DEV_MS5611 if (fl & ALERT_FLAG_BMP085) bmp085_st = ms5611_get_status(); #endif pt_set_sens_state(mpu6050_st, hmc5883_st, bmp085_st); } if (proto_st == ALST_FAIL || mpu6050_st == ALST_FAIL || hmc5883_st == ALST_FAIL || bmp085_st == ALST_FAIL) lstat = LST_FAIL; else if (proto_st == ALST_INIT || mpu6050_st == ALST_INIT || hmc5883_st == ALST_INIT || bmp085_st == ALST_INIT) lstat = LST_INIT; else if (proto_st == ALST_NORMAL && mpu6050_st == ALST_NORMAL && hmc5883_st == ALST_NORMAL && bmp085_st == ALST_NORMAL) lstat = LST_NORMAL; led_update(lstat); } }
static msg_t PlannerThread(void* arg){ chRegSetThreadName("Planner"); (void)arg; while(GlobalFlags.messaging_ready == 0) chThdSleepMilliseconds(50); struct EventListener el_mission_request_list; struct EventListener el_mission_count; struct EventListener el_mission_clear_all; struct EventListener el_mission_item; chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST); chEvtRegisterMask(&event_mavlink_in_mission_count, &el_mission_count, EVMSK_MAVLINK_IN_MISSION_COUNT); chEvtRegisterMask(&event_mavlink_in_mission_clear_all, &el_mission_clear_all, EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL); chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM); eventmask_t evt = 0; while (!chThdShouldTerminate()) { evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST | EVMSK_MAVLINK_IN_MISSION_COUNT | EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL | EVMSK_MAVLINK_IN_MISSION_ITEM, MS2ST(100)); switch (evt){ /* ground want to know how many items we have */ case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST: chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list); mav2gcs(); chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST); break; /* ground says how many items it wants to send here */ case EVMSK_MAVLINK_IN_MISSION_COUNT: /* this event now will be handled inside write loop */ chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item); gcs2mav(mavlink_in_mission_count_struct.count); /* register event back to main cycle */ chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM); break; /* ground wants erase all wps */ case EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL: mission_clear_all(); break; case EVMSK_MAVLINK_IN_MISSION_ITEM: /* If a waypoint planner component receives WAYPOINT messages outside * of transactions it answers with a WAYPOINT_ACK message. */ send_ack(MAV_MISSION_DENIED); break; default: //chDbgPanic("unimplemented"); break; } } chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list); chEvtUnregister(&event_mavlink_in_mission_count, &el_mission_count); chEvtUnregister(&event_mavlink_in_mission_clear_all, &el_mission_clear_all); chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item); chThdExit(0); return 0; }
/* * Application entry point. */ int main(void) { static thread_t *shelltp = NULL; static const evhandler_t evhndl[] = { InsertHandler, RemoveHandler }; event_listener_t el0, el1; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. * - lwIP subsystem initialization using the default configuration. */ halInit(); chSysInit(); lwipInit(NULL); /* * Initialize board LED. */ palSetLineMode(LINE_ARD_D13, PAL_MODE_OUTPUT_PUSHPULL); /* * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&SDU2); sduStart(&SDU2, &serusbcfg); /* * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1500); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); /* * Shell manager initialization. */ shellInit(); /* * Activates the serial driver 1 and SDC driver 1 using default * configuration. */ sdStart(&SD1, NULL); sdcStart(&SDCD1, NULL); /* * Activates the card insertion monitor. */ tmr_init(&SDCD1); /* * Creates the blinker thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Creates the HTTP thread (it changes priority internally). */ chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1, http_server, NULL); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and listen for events. */ chEvtRegister(&inserted_event, &el0, 0); chEvtRegister(&removed_event, &el1, 1); while (true) { if (!shelltp && (SDU2.config->usbp->state == USB_ACTIVE)) shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO); else if (chThdTerminatedX(shelltp)) { chThdRelease(shelltp); /* Recovers memory of the previous shell. */ shelltp = NULL; /* Triggers spawning of a new shell. */ } if (palReadPad(GPIOI, GPIOI_BUTTON_USER) != 0) { } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500))); } }
/* * Application entry point. */ int main(void) { static const evhandler_t evhndl[] = { InsertHandler, RemoveHandler, ShellHandler }; event_listener_t el0, el1, el2; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&SDU1); sduStart(&SDU1, &serusbcfg); /* * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1500); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); /* * Shell manager initialization. */ shellInit(); /* * Activates the card insertion monitor. */ tmr_init(&SDCD1); /* * Creates the blinker thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Normal main() thread activity, handling SD card events and shell * start/exit. */ chEvtRegister(&inserted_event, &el0, 0); chEvtRegister(&removed_event, &el1, 1); chEvtRegister(&shell_terminated, &el2, 2); while (true) { if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE)) { shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE, "shell", NORMALPRIO + 1, shellThread, (void *)&shell_cfg1); } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500))); } }
int main(void) { static Thread *shelltp = NULL; static const evhandler_t evhndl_main[] = { extdetail_WKUP_button_handler }; struct EventListener el0; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); extdetail_init(); palSetPad(GPIOC, GPIOC_LED); palSetPad(GPIOA, GPIOA_SPI1_SCK); palSetPad(GPIOA, GPIOA_SPI1_NSS); /* * SPI1 I/O pins setup. */ palSetPadMode(adis_connections.spi_sck_port, adis_connections.spi_sck_pad, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); palSetPadMode(adis_connections.spi_miso_port, adis_connections.spi_miso_pad, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST| PAL_STM32_PUDR_FLOATING); palSetPadMode(adis_connections.spi_mosi_port, adis_connections.spi_mosi_pad, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST ); palSetPadMode(adis_connections.spi_cs_port, adis_connections.spi_cs_pad, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); palSetPad(GPIOA, GPIOA_SPI1_SCK); palSetPad(GPIOA, GPIOA_SPI1_NSS); /*! * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&SDU_PSAS); sduStart(&SDU_PSAS, &serusbcfg); /*! * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1000); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); shellInit(); iwdg_begin(); /*! * Activates the serial driver 6 and SDC driver 1 using default * configuration. */ sdStart(&SD6, NULL); spiStart(&SPID1, &adis_spicfg); /* Set transfer parameters. */ chThdSleepMilliseconds(300); adis_init(); adis_reset(); /*! Activates the EXT driver 1. */ extStart(&EXTD1, &extcfg); chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL); chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL); chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL); chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL); chEvtRegister(&extdetail_wkup_event, &el0, 0); while (TRUE) { if (!shelltp && (SDU_PSAS.config->usbp->state == USB_ACTIVE)) shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO); else if (chThdTerminated(shelltp)) { chThdRelease(shelltp); /* Recovers memory of the previous shell. */ shelltp = NULL; /* Triggers spawning of a new shell. */ } chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500))); } }
/* * Application entry point. */ int main(void) { static thread_t *shelltp = NULL; static const evhandler_t evhndl[] = { InsertHandler, RemoveHandler }; event_listener_t el0, el1; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&SDU1); sduStart(&SDU1, &serusbcfg); /* * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1500); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); /* * Shell manager initialization. */ shellInit(); /* * Activates the card insertion monitor. */ tmr_init(&SDCD1); /* * Creates the blinker thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and listen for events. */ chEvtRegister(&inserted_event, &el0, 0); chEvtRegister(&removed_event, &el1, 1); while (true) { if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE)) shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO); else if (chThdTerminatedX(shelltp)) { chThdRelease(shelltp); /* Recovers memory of the previous shell. */ shelltp = NULL; /* Triggers spawning of a new shell. */ } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500))); } }
int main(void) { static const evhandler_t evhndl_main[] = { extdetail_WKUP_button_handler }; struct EventListener el0; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); extdetail_init(); palSetPad(GPIOC, GPIOC_LED); /*! * GPIO Pins for generating pulses at data input detect and data output send. * Used for measuring latency timing of data * * \sa board.h */ palClearPad( TIMEOUTPUT_PORT, TIMEOUTPUT_PIN); palSetPadMode(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL); palSetPad( TIMEINPUT_PORT, TIMEINPUT_PIN); palSetPadMode(TIMEINPUT_PORT, TIMEINPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL ); /* * I2C2 I/O pins setup */ palSetPadMode(si_i2c_connections.i2c_sda_port , si_i2c_connections.i2c_sda_pad, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST |PAL_STM32_PUDR_FLOATING ); palSetPadMode(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad, PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_FLOATING); palSetPad(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad ); static const ShellCommand commands[] = { {"mem", cmd_mem}, {"threads", cmd_threads}, {NULL, NULL} }; usbSerialShellStart(commands); mpu9150_start(&I2CD2); i2cStart(mpu9150_driver.i2c_instance, &si_i2c_config); mpu9150_init(mpu9150_driver.i2c_instance); /* Administrative threads */ chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL); chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL); /* MAC */ /*! * Use a locally administered MAC address second LSbit of MSB of MAC should be 1 * Use unicast address LSbit of MSB of MAC should be 0 */ data_udp_init(); chThdCreateStatic(wa_lwip_thread , sizeof(wa_lwip_thread) , NORMALPRIO + 2, lwip_thread , SENSOR_LWIP); chThdCreateStatic(wa_data_udp_send_thread , sizeof(wa_data_udp_send_thread) , NORMALPRIO , data_udp_send_thread , NULL); /* i2c MPU9150 */ chThdCreateStatic(waThread_mpu9150_int, sizeof(waThread_mpu9150_int) , NORMALPRIO , Thread_mpu9150_int, NULL); /* SPI ADIS */ //chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL); //chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL); /*! Activates the EXT driver 1. */ extStart(&EXTD1, &extcfg); chEvtRegister(&extdetail_wkup_event, &el0, 0); while (TRUE) { chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500))); } }
static bool gcs2mav(uint16_t N){ uint32_t seq = 0; uint32_t retry_cnt = PLANNER_RETRY_CNT; eventmask_t evt = 0; mavlink_mission_item_t mi; /* local copy */ uint8_t status = MAV_MISSION_ERROR; /* check available space */ if (N > wpdb.getCapacity()){ send_ack(MAV_MISSION_NO_SPACE); return CH_FAILED; } /* prepare to harvest waypoints */ struct EventListener el_mission_item; chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM); chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, 1);/* fake wait to clear "queue" */ if (CH_FAILED == wpdb.clear()) chDbgPanic(""); for (seq=0; seq<N; seq++){ /* prepare request to ground */ mavlink_out_mission_request_struct.target_component = MAV_COMP_ID_MISSIONPLANNER; mavlink_out_mission_request_struct.target_system = GROUND_STATION_ID; mavlink_out_mission_request_struct.seq = seq; retry_cnt = PLANNER_RETRY_CNT; chThdSleep(PLANNER_ADDITIONAL_TMO); do{ /* drop message */ chEvtBroadcastFlags(&event_mavlink_out_mission_request, EVMSK_MAVLINK_OUT_MISSION_REQUEST); /* wait answer */ evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, PLANNER_RETRY_TMO); if (EVMSK_MAVLINK_IN_MISSION_ITEM == evt){ chSysLock(); mi = mavlink_in_mission_item_struct; chSysUnlock(); /* check waypoint cosherness and write it if cosher */ status = check_wp(&mi, seq); if (status != MAV_MISSION_ACCEPTED) goto EXIT; else{ if (CH_FAILED == wpdb.save(&mi, seq)) chDbgPanic(""); break; /* do-while */ } } retry_cnt--; if(0 == retry_cnt) goto EXIT; }while(retry_cnt); } /* save waypoint count in eeprom only in the very end of transaction */ if (CH_FAILED == wpdb.finalize()) chDbgPanic(""); /* final stuff */ EXIT: chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item); send_ack(status); if (0 == retry_cnt) return CH_FAILED; else return CH_SUCCESS; }
int main(void) { uint32_t i; FRDMSlave * slave; FRDMSlave::data_t * data_iter; FRDMSlave::data_t * buffer_end; halInit(); chSysInit(); main_thread = chThdSelf(); Platform::early_init(); for(slave = slaves; slave < slaves + num_slaves; slave++){ slave->init(); } timer_config(); button.set_press_handler((button_t::button_handler)button_cb, nullptr); term.launch(); while(true){ // Wait for a button press chEvtGetAndClearEvents(ALL_EVENTS); chEvtWaitOne(MAIN_SIG_BUTTON); /* for(slave = slaves; slave < slaves + num_slaves; slave++){ if(!slave->ping()) break; } if(slave != slaves + num_slaves){ // One of the slaves didn't respond... for(i = 0; i < 10; i++){ chThdSleep(MS2ST(100)); leds[0].toggle(); leds[1].toggle(); leds[2].toggle(); leds[3].toggle(); } } */ // Flash lights 3 times for(i = 0; i < 5; i++){ chThdSleep(MS2ST(500)); leds[0].toggle(); leds[1].toggle(); leds[2].toggle(); leds[3].toggle(); } // Let the last one go for a whole second chThdSleep(MS2ST(500)); leds[0].clear(); leds[1].clear(); leds[2].clear(); leds[3].clear(); // Trigger first sample sync_pin.toggle(); timer_start(); chEvtGetAndClearEvents(MAIN_SIG_BUTTON); sample_count = 0; data_iter = acc_buffer1; buffer_end = acc_buffer1 + sizeof(acc_buffer1)/sizeof(*acc_buffer1); while(!chEvtWaitOneTimeout(MAIN_SIG_BUTTON, TIME_IMMEDIATE)){ chEvtWaitOne(MAIN_SIG_TICK); for(slave = slaves; slave < slaves + num_slaves; slave++){ slave->start_read(); } for(slave = slaves; slave < slaves + num_slaves; slave++){ slave->finish_read(data_iter); data_iter += FRDMSlave::samples_per_transfer; sample_count += FRDMSlave::samples_per_transfer; if(data_iter == acc_buffer1 + (sizeof(acc_buffer1)/sizeof(*acc_buffer1))){ data_iter = acc_buffer2; } else if(data_iter == acc_buffer2 + (sizeof(acc_buffer2)/sizeof(*acc_buffer2))){ break; } } if(slave != slaves + num_slaves){ break; } // Trigger next sample sync_pin.toggle(); #if 0 for(slave = slaves; slave < slaves + num_slaves; slave++){ slave->finish_read(data_iter); data_iter += FRDMSlave::samples_per_transfer; if((uint32_t)(buffer_end - data_iter) < FRDMSlave::samples_per_transfer * sizeof(FRDMSlave::data_t)){ if(data_iter < acc_buffer1){ // Buffer is full! break; } data_iter = acc_buffer2; buffer_end = acc_buffer2 + sizeof(acc_buffer2)/sizeof(*acc_buffer2); } } if(slave != slaves + num_slaves){ for(i = 0; i < 10; i++){ chThdSleep(MS2ST(100)); leds[0].toggle(); leds[1].toggle(); leds[2].toggle(); leds[3].toggle(); } break; } #endif leds[0].toggle(); } timer_stop(); for(slave = slaves; slave < slaves + num_slaves; slave++){ slave->stop(); } leds[0].clear(); leds[1].clear(); leds[2].clear(); leds[3].clear(); chThdSleep(MS2ST(250)); } }
/** * @details When the last waypoint was successfully received * the requesting component sends a WAYPOINT_ACK message * to the targeted component. This finishes the transaction. * Notice that the targeted component has to listen to * WAYPOINT_REQUEST messages of the last waypoint until it * gets the WAYPOINT_ACK or another message that starts * a different transaction or a timeout happens. */ static bool mav2gcs(void){ eventmask_t evt = 0; bool status = CH_FAILED; uint32_t retry_cnt = PLANNER_RETRY_CNT; struct EventListener el_mission_request; struct EventListener el_mission_ack; struct EventListener el_mission_request_list; chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST); chEvtRegisterMask(&event_mavlink_in_mission_request, &el_mission_request, EVMSK_MAVLINK_IN_MISSION_REQUEST); chEvtRegisterMask(&event_mavlink_in_mission_ack, &el_mission_ack, EVMSK_MAVLINK_IN_MISSION_ACK); START: mavlink_out_mission_count_struct.target_component = MAV_COMP_ID_MISSIONPLANNER; mavlink_out_mission_count_struct.target_system = GROUND_STATION_ID; mavlink_out_mission_count_struct.count = wpdb.getCount(); chEvtBroadcastFlags(&event_mavlink_out_mission_count, EVMSK_MAVLINK_OUT_MISSION_COUNT); if (0 == mavlink_out_mission_count_struct.count){ status = CH_SUCCESS; goto EXIT; } /* теперь нам надо понять, дошло сообщение с количеством вейпоинтов, или нет. * Если нет - земля пришле повторный запрос MISSION_REQUEST_LIST */ evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST | EVMSK_MAVLINK_IN_MISSION_REQUEST | EVMSK_MAVLINK_IN_MISSION_ACK, PLANNER_RETRY_TMO * PLANNER_RETRY_CNT); switch(evt){ case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST: /* надобно повторить */ goto START; /* maint loooong loop */ case EVMSK_MAVLINK_IN_MISSION_REQUEST: do{ wpdb.load(&mavlink_out_mission_item_struct, mavlink_in_mission_request_struct.seq); mavlink_out_mission_item_struct.target_component = MAV_COMP_ID_MISSIONPLANNER; mavlink_out_mission_item_struct.target_system = GROUND_STATION_ID; chEvtBroadcastFlags(&event_mavlink_out_mission_item, EVMSK_MAVLINK_OUT_MISSION_ITEM); /* wait next request or ack */ evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST | EVMSK_MAVLINK_IN_MISSION_ACK, PLANNER_RETRY_TMO); switch(evt){ case EVMSK_MAVLINK_IN_MISSION_REQUEST: continue; case EVMSK_MAVLINK_IN_MISSION_ACK: status = CH_SUCCESS; goto EXIT; break; default: retry_cnt--; status = CH_FAILED; break; } }while(retry_cnt); break; /* case EVMSK_MAVLINK_IN_MISSION_REQUEST */ case EVMSK_MAVLINK_IN_MISSION_ACK: status = CH_SUCCESS; break; default: status = CH_FAILED; break; } EXIT: chEvtUnregister(&event_mavlink_in_mission_request, &el_mission_request); chEvtUnregister(&event_mavlink_in_mission_ack, &el_mission_ack); chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list); return status; }
/* * Application entry point. */ int main(void) { static const evhandler_t evhndl[] = { InsertHandler, RemoveHandler, ShellHandler }; event_listener_t el0, el1, el2; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. * - lwIP subsystem initialization using the default configuration. */ halInit(); chSysInit(); /* * Initialize RNG */ rccEnableAHB2(RCC_AHB2ENR_RNGEN, 0); RNG->CR |= RNG_CR_IE; RNG->CR |= RNG_CR_RNGEN; /* lwip */ lwipInit(NULL); /* * Target-dependent setup code. */ portab_setup(); /* * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&PORTAB_SDU1); sduStart(&PORTAB_SDU1, &serusbcfg); /* * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1500); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); /* * Shell manager initialization. */ shellInit(); /* * Activates the SDC driver 1 using default configuration. */ sdcStart(&SDCD1, NULL); /* * Activates the card insertion monitor. */ tmr_init(&SDCD1); /* * Creates the blinker thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Creates the HTTPS thread (it changes priority internally). */ chThdCreateStatic(wa_https_server, sizeof(wa_https_server), NORMALPRIO + 1, https_server, NULL); /* * Normal main() thread activity, handling SD card events and shell * start/exit. */ chEvtRegister(&inserted_event, &el0, 0); chEvtRegister(&removed_event, &el1, 1); chEvtRegister(&shell_terminated, &el2, 2); while (true) { if (!shelltp && (PORTAB_SDU1.config->usbp->state == USB_ACTIVE)) { shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE, "shell", NORMALPRIO + 1, shellThread, (void *)&shell_cfg1); } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, TIME_MS2I(500))); } }
/* * Application entry point. */ int main(void) { static const evhandler_t evhndl[] = { WKUP_button_handler }; struct EventListener el0; /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Initialize event structures BEFORE using them */ chEvtInit(&wkup_event); /* * Shell manager initialization. */ usbSerialShellStart(commands); // Enable Continuous GPT for 1ms Interval gptStart(&GPTD1, &gpt1cfg); gptStartContinuous(&GPTD1,10000); // Configure pins for Feedback ADC's palSetPadMode(GPIOA, GPIOA_PIN4, PAL_MODE_INPUT_ANALOG); palSetPadMode(GPIOA, GPIOA_PIN5, PAL_MODE_INPUT_ANALOG); // Configure pins for Input ADC's palSetPadMode(GPIOF, GPIOF_PIN6, PAL_MODE_INPUT_ANALOG); palSetPadMode(GPIOF, GPIOF_PIN7, PAL_MODE_INPUT_ANALOG); // Configure pins for LED's // PD3: Vertical Axis LED palSetPadMode(GPIOD, GPIOD_PIN3, PAL_MODE_OUTPUT_PUSHPULL); // PD4: Lateral Axis LED palSetPadMode(GPIOD, GPIOD_PIN4, PAL_MODE_OUTPUT_PUSHPULL); // Configure pins for switches // PD8: Drive Enable Switch palSetPadMode(GPIOD, GPIOD_PIN8, PAL_MODE_INPUT); // PD9: Mode Select Switch palSetPadMode(GPIOD, GPIOD_PIN9, PAL_MODE_INPUT); // Configure pins for long lead GMD Enable/Watchdog // PD5: Enable out to GMD palSetPadMode(GPIOD, GPIOD_PIN5, PAL_MODE_OUTPUT_PUSHPULL); // PD6: Watchdog out to GMD palSetPadMode(GPIOD, GPIOD_PIN6, PAL_MODE_OUTPUT_PUSHPULL); // Configure pins for short lead GMD Enable/Watchdog // PD10: Enable out to GMD palSetPadMode(GPIOD, GPIOD_PIN10, PAL_MODE_OUTPUT_PUSHPULL); // PD11: Watchdog out to GMD palSetPadMode(GPIOD, GPIOD_PIN11, PAL_MODE_OUTPUT_PUSHPULL); // Configure pins for PWM output (D12-D15: TIM4, channel 1-4) palSetPadMode(GPIOD, GPIOD_PIN12, PAL_MODE_ALTERNATE(2)); //U-pole, short lead palSetPadMode(GPIOD, GPIOD_PIN13, PAL_MODE_ALTERNATE(2)); //V-pole, short lead palSetPadMode(GPIOD, GPIOD_PIN14, PAL_MODE_ALTERNATE(2)); //U-pole, long lead palSetPadMode(GPIOD, GPIOD_PIN15, PAL_MODE_ALTERNATE(2)); //V-pole, long lead adcStart(&ADCD1, NULL); adcStart(&ADCD2, NULL); adcStart(&ADCD3, NULL); pwmStart(&PWMD4, &pwmcfg); // Enable TIM4 PWM channel 1-4 with initial DC=0% /* @param[in] pwmp pointer to a @p PWMDriver object * @param[in] channel PWM channel identifier (0...PWM_CHANNELS-1) * @param[in] width PWM pulse width as clock pulses number */ pwmEnableChannel(&PWMD4, 0, 0); pwmEnableChannel(&PWMD4, 1, 0); pwmEnableChannel(&PWMD4, 2, 0); pwmEnableChannel(&PWMD4, 3, 0); // Set axis control gain and limit values // Set Vertical Axis Gains vertAxisStruct.U16PositionPGain = 4; vertAxisStruct.U16PositionIGain = 1; vertAxisStruct.U16PositionDGain = 0; // Set vertical axis limits vertAxisStruct.U16CommandLimit = VERTICAL_COMMAND_LIMIT; vertAxisStruct.U16HighPosnLimit = 5100; vertAxisStruct.U16LowPosnLimit = 2480; // Set Lateral Axis Gains latAxisStruct.U16PositionPGain = 2; latAxisStruct.U16PositionIGain = 0; latAxisStruct.U16PositionDGain = 0; // Set lateral axis limits latAxisStruct.U16CommandLimit = LATERAL_COMMAND_LIMIT; latAxisStruct.U16HighPosnLimit = 5300; latAxisStruct.U16LowPosnLimit = 3100; /* * Activates the serial driver 6 and SDC driver 1 using default * configuration. */ sdStart(&SD6, NULL); /* * Activates the EXT driver 1. * This is for the external interrupt */ extStart(&EXTD1, &extcfg); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and listen for events. */ chEvtRegister(&wkup_event, &el0, 0); while (TRUE) { //Cycle motordrive if timer fails if(U32DelayCount++ > 2500){ motordrive(&GPTD1); } chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500))); } }
void fatfs_dispatch_event( void ) { chEvtDispatch( evhndl, chEvtWaitOneTimeout( ALL_EVENTS, MS2ST( 500 ) ) ); }