// Initialise the EXT driver and register the buttons events to the current // thread. void exti_init(void) { chEvtRegisterMask(&pc0_sw_evt, &pc0_sw_listener, PC0_SW_MSK); chEvtRegisterMask(&pc1_sw_evt, &pc1_sw_listener, PC1_SW_MSK); chEvtRegisterMask(&pc2_sw_evt, &pc2_sw_listener, PC2_SW_MSK); chEvtRegisterMask(&pc3_sw_evt, &pc3_sw_listener, PC3_SW_MSK); extStart(&EXTD1, &extcfg); }
static void cmdIr(BaseSequentialStream *chp, int argc, char *argv[]) { (void)argv; (void)argc; chEvtRegisterMask(&esSensorEvents , &el, SENSOR_EVENT_IR_END); chprintf(chp, "Pinging with IR...\r\n"); chEvtBroadcastFlags(&esSensorEvents, SENSOR_EVENT_IR_START); chEvtWaitOne(SENSOR_EVENT_IR_END); chThdSleepMilliseconds(50); ir_index_t i; ir_angle_t angle; chSysLock(); for (i = 0; i < IR_RX_COUNT; i++) { angle = irDetections[i]; chprintf(chp, "IR Angle: %D\r\n", angle); } chSysUnlock(); chEvtUnregister(&esSensorEvents, &el); }
static THD_FUNCTION(ThreadTestEvents, arg) { (void)arg; event_listener_t el; eventmask_t events; int i = 0; chEvtRegisterMask( &hmc5983cfg.data_holder->es, &el, HMC5983_DATA_AVAILABLE_EVENTMASK); while(1) { events = chEvtWaitOne(HMC5983_DATA_AVAILABLE_EVENTMASK); if (events == HMC5983_DATA_AVAILABLE_EVENTMASK) { if (i++ > 10) { palTogglePad(GPIOC, GPIOC_LED_USR); i = 0; } } } return MSG_OK; }
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); } } }
CCM_FUNC static THD_FUNCTION(ThreadBDU, arg) { event_listener_t el1; eventmask_t flags; (void)arg; chRegSetThreadName("BDU"); chEvtRegisterMask(chnGetEventSource(&SDU2), &el1, ALL_EVENTS); while(USBD1.state != USB_READY) chThdSleepMilliseconds(10); while(SDU2.state != SDU_READY) chThdSleepMilliseconds(10); while (TRUE) { chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE); flags = chEvtGetAndClearFlags(&el1); pwmEnableChannel(&PWMD_LED2, CHN_LED2, PWM_PERCENTAGE_TO_WIDTH(&PWMD_LED2, 8000)); if (flags & CHN_INPUT_AVAILABLE) { pwmEnableChannel(&PWMD_LED2, CHN_LED2, PWM_PERCENTAGE_TO_WIDTH(&PWMD_LED2, 1000)); readCommand((BaseChannel *)&SDU2); } else chThdSleepMilliseconds(2); } return; }
void startConsole(Logging *sharedLogger, CommandHandler console_line_callback_p) { logger = sharedLogger; console_line_callback = console_line_callback_p; #if (defined(EFI_CONSOLE_UART_DEVICE) && ! EFI_SIMULATOR) || defined(__DOXYGEN__) palSetPadMode(CONSOLE_MODE_SWITCH_PORT, CONSOLE_MODE_SWITCH_PIN, PAL_MODE_INPUT_PULLUP); b_isCommandLineConsoleOverTTL = GET_CONSOLE_MODE_VALUE() == EFI_USE_UART_FOR_CONSOLE; if (isCommandLineConsoleOverTTL()) { /* * Activates the serial * it is important to set 'NONE' as flow control! in terminal application on the PC */ serialConfig.speed = engineConfiguration->uartConsoleSerialSpeed; sdStart(EFI_CONSOLE_UART_DEVICE, &serialConfig); // cannot use pin repository here because pin repository prints to console palSetPadMode(EFI_CONSOLE_RX_PORT, EFI_CONSOLE_RX_PIN, PAL_MODE_ALTERNATE(EFI_CONSOLE_AF)); palSetPadMode(EFI_CONSOLE_TX_PORT, EFI_CONSOLE_TX_PIN, PAL_MODE_ALTERNATE(EFI_CONSOLE_AF)); isSerialConsoleStarted = true; chEvtRegisterMask((event_source_t *) chnGetEventSource(EFI_CONSOLE_UART_DEVICE), &consoleEventListener, 1); } #else b_isCommandLineConsoleOverTTL = false; #endif /* EFI_PROD_CODE */ chThdCreateStatic(consoleThreadStack, sizeof(consoleThreadStack), NORMALPRIO, (tfunc_t)consoleThreadThreadEntryPoint, NULL); addConsoleAction(SWITCH_TO_BINARY_COMMAND, switchToBinaryProtocol); }
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; }
/** * @brief Thread for the flash save operation. * * @param[in] arg Unused. * @return Unused. */ static THD_FUNCTION(ThreadSysInfoFlashSave, arg) { (void)arg; /* Event registration for new estimation */ event_listener_t el; /* Set thread name */ chRegSetThreadName("SysInfo FlashSave"); /* Register to new estimation */ chEvtRegisterMask(ptrGetFlashSaveEventSource(), &el, FLASHSAVE_SAVE_EVENTMASK); while (1) { /* Wait for new estimation */ chEvtWaitOne(FLASHSAVE_SAVE_EVENTMASK); /* Save settings to flash */ FlashSave_Write(FlashSave_STR2ID("SIVN"), true, (uint8_t *)system_strings.vehicle_name, VEHICLE_NAME_SIZE); FlashSave_Write(FlashSave_STR2ID("SIVT"), true, (uint8_t *)system_strings.vehicle_type, VEHICLE_TYPE_SIZE); } }
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; }
static void evt1_execute(void) { event_listener_t el1, el2; /* * Testing chEvtRegisterMask() and chEvtUnregister(). */ chEvtObjectInit(&es1); chEvtRegisterMask(&es1, &el1, 1); chEvtRegisterMask(&es1, &el2, 2); test_assert(1, chEvtIsListeningI(&es1), "no listener"); chEvtUnregister(&es1, &el1); test_assert(2, chEvtIsListeningI(&es1), "no listener"); chEvtUnregister(&es1, &el2); test_assert(3, !chEvtIsListeningI(&es1), "stuck listener"); /* * Testing chEvtDispatch(). */ chEvtDispatch(evhndl, 7); test_assert_sequence(4, "ABC"); }
void vexAudioPlaySoundWait( int freq, int amplitude, int timems ) { EventListener el; // register event chEvtRegisterMask(&sound_done, &el, 1); // play sound vexAudioPlaySound( freq, amplitude, timems ); // wait for event, 10 second timeout chEvtWaitAnyTimeout( ALL_EVENTS, MS2ST(10000)); // unregister chEvtUnregister( &sound_done, &el ); }
static msg_t fnet_receive_thread(void *arg) { (void) arg; EventListener el1; chRegSetThreadName("FNET receive thread"); chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID); chEvtAddEvents(FRAME_RECEIVED_ID); while (TRUE) { eventmask_t mask = chEvtWaitAny(ALL_EVENTS ); if (mask & FRAME_RECEIVED_ID) { fnet_stm32_input(); } } return RDY_OK; }
static msg_t fnet_timer_thread(void *period_ms) { EventListener el0; chRegSetThreadName("FNET timer thread"); evtInit(&fnetEventTimer, MS2ST(period_ms) ); evtStart(&fnetEventTimer); chEvtRegisterMask(&fnetEventTimer.et_es, &el0, PERIODIC_TIMER_ID); chEvtAddEvents(PERIODIC_TIMER_ID); while (TRUE) { eventmask_t mask = chEvtWaitAny(ALL_EVENTS ); if (mask & PERIODIC_TIMER_ID) { fnet_timer_ticks_inc(); fnet_timer_handler_bottom(NULL ); } } return RDY_OK; }
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; }
static void evt2_execute(void) { eventmask_t m; event_listener_t el1, el2; systime_t target_time; /* * Test on chEvtWaitOne() without wait. */ chEvtAddEvents(5); m = chEvtWaitOne(ALL_EVENTS); test_assert(1, m == 1, "single event error"); m = chEvtWaitOne(ALL_EVENTS); test_assert(2, m == 4, "single event error"); m = chEvtGetAndClearEvents(ALL_EVENTS); test_assert(3, m == 0, "stuck event"); /* * Test on chEvtWaitOne() with wait. */ test_wait_tick(); target_time = chVTGetSystemTime() + MS2ST(50); threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1, thread1, chThdGetSelfX()); m = chEvtWaitOne(ALL_EVENTS); test_assert_time_window(4, target_time, target_time + ALLOWED_DELAY); test_assert(5, m == 1, "single event error"); m = chEvtGetAndClearEvents(ALL_EVENTS); test_assert(6, m == 0, "stuck event"); test_wait_threads(); /* * Test on chEvtWaitAny() without wait. */ chEvtAddEvents(5); m = chEvtWaitAny(ALL_EVENTS); test_assert(7, m == 5, "unexpected pending bit"); m = chEvtGetAndClearEvents(ALL_EVENTS); test_assert(8, m == 0, "stuck event"); /* * Test on chEvtWaitAny() with wait. */ test_wait_tick(); target_time = chVTGetSystemTime() + MS2ST(50); threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1, thread1, chThdGetSelfX()); m = chEvtWaitAny(ALL_EVENTS); test_assert_time_window(9, target_time, target_time + ALLOWED_DELAY); test_assert(10, m == 1, "single event error"); m = chEvtGetAndClearEvents(ALL_EVENTS); test_assert(11, m == 0, "stuck event"); test_wait_threads(); /* * Test on chEvtWaitAll(). */ chEvtObjectInit(&es1); chEvtObjectInit(&es2); chEvtRegisterMask(&es1, &el1, 1); chEvtRegisterMask(&es2, &el2, 4); test_wait_tick(); target_time = chVTGetSystemTime() + MS2ST(50); threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1, thread2, "A"); m = chEvtWaitAll(5); test_assert_time_window(12, target_time, target_time + ALLOWED_DELAY); m = chEvtGetAndClearEvents(ALL_EVENTS); test_assert(13, m == 0, "stuck event"); test_wait_threads(); chEvtUnregister(&es1, &el1); chEvtUnregister(&es2, &el2); test_assert(14, !chEvtIsListeningI(&es1), "stuck listener"); test_assert(15, !chEvtIsListeningI(&es2), "stuck listener"); }
/** * @brief LWIP handling thread. * * @param[in] p pointer to a @p lwipthread_opts structure or @p NULL * @return The function does not return. */ msg_t lwip_thread(void *p) { event_timer_t evt; event_listener_t el0, el1; struct ip_addr ip, gateway, netmask; static struct netif thisif; static const MACConfig mac_config = {thisif.hwaddr}; chRegSetThreadName("lwipthread"); /* Initializes the thing.*/ tcpip_init(NULL, NULL); /* TCP/IP parameters, runtime or compile time.*/ if (p) { struct lwipthread_opts *opts = p; unsigned i; for (i = 0; i < 6; i++) thisif.hwaddr[i] = opts->macaddress[i]; ip.addr = opts->address; gateway.addr = opts->gateway; netmask.addr = opts->netmask; } else { thisif.hwaddr[0] = LWIP_ETHADDR_0; thisif.hwaddr[1] = LWIP_ETHADDR_1; thisif.hwaddr[2] = LWIP_ETHADDR_2; thisif.hwaddr[3] = LWIP_ETHADDR_3; thisif.hwaddr[4] = LWIP_ETHADDR_4; thisif.hwaddr[5] = LWIP_ETHADDR_5; LWIP_IPADDR(&ip); LWIP_GATEWAY(&gateway); LWIP_NETMASK(&netmask); } macStart(ÐD1, &mac_config); netif_add(&thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input); netif_set_default(&thisif); netif_set_up(&thisif); /* Setup event sources.*/ evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL); evtStart(&evt); chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID); chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID); chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID); /* Goes to the final priority after initialization.*/ chThdSetPriority(LWIP_THREAD_PRIORITY); while (TRUE) { eventmask_t mask = chEvtWaitAny(ALL_EVENTS); if (mask & PERIODIC_TIMER_ID) { bool_t current_link_status = macPollLinkStatus(ÐD1); if (current_link_status != netif_is_link_up(&thisif)) { if (current_link_status) tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up, &thisif, 0); else tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down, &thisif, 0); } } if (mask & FRAME_RECEIVED_ID) { struct pbuf *p; while ((p = low_level_input(&thisif)) != NULL) { struct eth_hdr *ethhdr = p->payload; switch (htons(ethhdr->type)) { /* IP or ARP packet? */ case ETHTYPE_IP: case ETHTYPE_ARP: #if PPPOE_SUPPORT /* PPPoE packet? */ case ETHTYPE_PPPOEDISC: case ETHTYPE_PPPOE: #endif /* PPPOE_SUPPORT */ /* full packet send to tcpip_thread to process */ if (thisif.input(p, &thisif) == ERR_OK) break; LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n")); default: pbuf_free(p); } } } } return 0; }
void PillRegisterEvtChange(EventListener *PEvtLstnr, uint8_t EvtMask) { chEvtRegisterMask(&IEvtPillChange, PEvtLstnr, EvtMask); }
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; }
/** * @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; }
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; }
void EvtSource::registerMask(chibios_rt::EvtListener *elp, eventmask_t emask) { chEvtRegisterMask(&ev_source, &elp->ev_listener, emask); }
void Event::RegisterMask(EventListener *elp, eventmask_t emask) { chEvtRegisterMask(&event,elp, emask); }
int main(void) { // copy vector table memcpy((char *)0x20000000, (const char *)&_vectors, 0x200); // remap SRAM1 to 0x00000000 SYSCFG->MEMRMP |= 0x03; /* system & hardware initialization */ halInit(); // float usb inputs, hope the host notices detach... palSetPadMode(GPIOA, 11, PAL_MODE_INPUT); palSetPadMode(GPIOA, 12, PAL_MODE_INPUT); // setup LEDs palSetPadMode(LED1_PORT,LED1_PIN,PAL_MODE_OUTPUT_PUSHPULL); palSetPadMode(LED2_PORT, LED2_PIN, PAL_MODE_OUTPUT_PUSHPULL); palClearPad(LED1_PORT,LED1_PIN); palSetPad(LED2_PORT,LED2_PIN); chSysInit(); palSetPadMode(GPIOA, 11, PAL_MODE_ALTERNATE(10)); palSetPadMode(GPIOA, 12, PAL_MODE_ALTERNATE(10)); palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); palSetPadMode(GPIOC, 10, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); palSetPadMode(GPIOC, 11, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); palSetPadMode(GPIOC, 12, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); palSetPadMode(GPIOD, 2, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST); chThdSleepMilliseconds(50); /* initialize the SD card */ sdcStart(&SDCD1, NULL); sdcConnect(&SDCD1); /* initialize the USB mass storage driver */ msdInit(&UMSD1); /* turn off green LED, turn on red LED */ palSetPadMode(LED1_PORT, LED1_PIN, PAL_MODE_OUTPUT_PUSHPULL); palClearPad(LED1_PORT, LED1_PIN); palSetPadMode(LED2_PORT, LED2_PIN, PAL_MODE_OUTPUT_PUSHPULL); palSetPad(LED2_PORT, LED2_PIN); /* start the USB mass storage service */ int ret = msdStart(&UMSD1, &msdConfig); if (ret != 0) { /* no media found : bye bye !*/ usbDisconnectBus(&USBD1); chThdSleepMilliseconds(1000); NVIC_SystemReset(); } /* watch the mass storage events */ EventListener connected; EventListener ejected; chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1)); chEvtRegisterMask(&UMSD1.evt_ejected, &ejected, EVENT_MASK(2)); /* start the USB driver */ usbDisconnectBus(&USBD1); chThdSleepMilliseconds(1000); usbStart(&USBD1, &usbConfig); usbConnectBus(&USBD1); while (TRUE) { eventmask_t event = chEvtWaitOne(EVENT_MASK(1) | EVENT_MASK(2)); if (event == EVENT_MASK(1)) { /* media connected */ } else if (event == EVENT_MASK(2)) { /* media ejected : bye bye !*/ usbDisconnectBus(&USBD1); chThdSleepMilliseconds(1000); NVIC_SystemReset(); } } return 0; }
static msg_t thdUsbStorage(void *arg) { (void) arg; // unused chRegSetThreadName("UsbStorage:polling"); uint antiBounce=5; EventListener connected; // Should use EXTI interrupt instead of active polling, // but in the chibios_opencm3 implementation, since EXTI is // used via libopencm3, ISR are routed on pprz/opencm3 and cannot // be used concurrently by chibios api // Should be fixed when using chibios-rt branch while (!chThdShouldTerminate() && antiBounce) { const bool_t usbConnected = palReadPad (GPIOA, GPIOA_OTG_FS_VBUS); if (usbConnected) antiBounce--; else antiBounce=5; chThdSleepMilliseconds(20); } isRunning = true; chRegSetThreadName("UsbStorage:connected"); /* Stop the logs*/ chibios_logFinish (true); /* connect sdcard sdc interface sdio */ if (sdioConnect () == false) chThdExit (RDY_TIMEOUT); /* initialize the USB mass storage driver */ msdInit(&UMSD1); /* start the USB mass storage service */ msdStart(&UMSD1, &msdConfig); /* start the USB driver */ usbDisconnectBus(&USBD1); chThdSleepMilliseconds(1000); usbStart(&USBD1, &usbConfig); usbConnectBus(&USBD1); /* wait for a real usb storage connexion before shutting down autopilot */ chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1)); chEvtWaitOne(EVENT_MASK(1)); /* stop autopilot */ if (pprzThdPtr != NULL) { chThdTerminate (pprzThdPtr); chThdWait (pprzThdPtr); pprzThdPtr = NULL; } /* wait until usb-storage is unmount and usb cable is unplugged*/ while (!chThdShouldTerminate() && palReadPad (GPIOA, GPIOA_OTG_FS_VBUS)) { chThdSleepMilliseconds(10); } /* then close open descriptors and reboot autopilot */ usbDisconnectBus(&USBD1); chThdSleepMilliseconds(500); msdStop(&UMSD1); sdioDisconnect (); MCU_RESTART(); return RDY_OK; }
static msg_t control_thread(void* arg) { (void)arg; chRegSetThreadName("motor"); EventListener listener; chEvtRegisterMask(&_setpoint_update_event, &listener, ALL_EVENTS); uint64_t timestamp_hnsec = motor_rtctl_timestamp_hnsec(); while (1) { /* * Control loop period adapts to comm period. */ const uint32_t comm_period = motor_rtctl_get_comm_period_hnsec(); unsigned control_period_ms = IDLE_CONTROL_PERIOD_MSEC; if (comm_period > 0) { control_period_ms = comm_period / HNSEC_PER_MSEC; } if (control_period_ms < 1) { control_period_ms = 1; } else if (control_period_ms > IDLE_CONTROL_PERIOD_MSEC) { control_period_ms = IDLE_CONTROL_PERIOD_MSEC; } /* * Thread priority - maximum if the motor is running, normal otherwise */ const tprio_t desired_thread_priority = (comm_period > 0) ? HIGHPRIO : NORMALPRIO; if (desired_thread_priority != chThdGetPriority()) { const tprio_t old = chThdSetPriority(desired_thread_priority); lowsyslog("Motor: Priority changed: %i --> %i\n", (int)old, (int)desired_thread_priority); } /* * The event must be set only when the mutex is unlocked. * Otherwise this thread will take control, stumble upon the locked mutex, return the control * to the thread that holds the mutex, unlock the mutex, then proceed. */ chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(control_period_ms)); chMtxLock(&_mutex); const uint64_t new_timestamp_hnsec = motor_rtctl_timestamp_hnsec(); const uint32_t dt_hnsec = new_timestamp_hnsec - timestamp_hnsec; const float dt = dt_hnsec / (float)HNSEC_PER_SEC; timestamp_hnsec = new_timestamp_hnsec; assert(dt > 0); update_filters(dt); update_setpoint_ttl(dt_hnsec / HNSEC_PER_MSEC); update_control(comm_period, dt); poll_beep(); chMtxUnlock(); watchdog_reset(_watchdog_id); } assert_always(0); return 0; }