static void test_002_005_execute(void) {
systime_t time;
eventmask_t events;
/* A set of event flags are set on the current thread then the
function chVTGetSystemTimeX() is invoked, the function is supposed to
return immediately because the event flags are already pending,
after return the events mask is tested.*/
test_set_step(1);
{
time = chVTGetSystemTimeX();
chEvtSignalI(chThdGetSelfX(), 0x55);
events = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
test_assert((eventmask_t)0 != events,
"timed out");
test_assert((eventmask_t)0x55 == events,
"wrong events mask");
}
/* The pending event flags mask is cleared then the function
chVTGetSystemTimeX() is invoked, after return the events
mask is tested. The thread is signaled by another thread.*/
test_set_step(2);
{
time = chVTGetSystemTimeX();
chThdGetSelfX()->epmask = 0;
events = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
test_assert((eventmask_t)0 != events,
"timed out");
test_assert((eventmask_t)0x55 == events,
"wrong events mask");
}
/* The function chVTGetSystemTimeX() is invoked, no event can
wakeup the thread, the function must return because timeout.*/
test_set_step(3);
{
chSysLock();
time = chVTGetSystemTimeX();
events = chEvtWaitAnyTimeoutS(0, MS2ST(1000));
chSysUnlock();
test_assert_time_window(time + MS2ST(1000),
time + MS2ST(1000) + 1,
"out of time window");
test_assert((eventmask_t)0 == events,
"wrong events mask");
}
}
void Node::spin(systime_t timeout) {
eventmask_t mask;
eventid_t eid;
LocalSubscriber * sub;
mask = chEvtWaitAnyTimeout(ALL_EVENTS, timeout);
if (mask == 0) {
return;
}
eid = 0;
sub = _subscribers;
while (sub && mask) {
if (mask & EVENT_MASK(eid)) {
mask &= ~EVENT_MASK(eid);
if (sub->callback()) {
BaseMessage * msg;
while ((msg = sub->get()) != NULL) {
sub->callback(msg);
sub->release(msg);
}
}
}
eid++;
sub = sub->next();
}
}
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;
}
static msg_t can_rx(void *p) {
struct can_instance *cip = p;
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
#if SPC5_CAN_USE_FILTERS
rxFlag = chEvtGetAndClearFlagsI(&el);
#endif
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
#if !SPC5_CAN_USE_FILTERS
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#else
while (canReceive(cip->canp, rxFlag,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#endif
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
static THD_FUNCTION(cancom_thread, arg) {
(void)arg;
chRegSetThreadName("CAN");
event_listener_t el;
CANRxFrame rxmsg;
uint8_t buffer[9];
chEvtRegister(&CANDx.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0) {
continue;
}
while (canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
buffer[0] = rxmsg.SID;
for (int i = 0;i < rxmsg.DLC;i++) {
buffer[i + 1] = rxmsg.data8[i];
}
packet_handler_int_process_packet(buffer, rxmsg.DLC + 1);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
inline
SpinEvent_::Mask SpinEvent_::wait(const Time &timeout) {
systime_t ticks;
if (timeout == Time::IMMEDIATE) ticks = TIME_IMMEDIATE;
else if (timeout == Time::INFINITE) ticks = TIME_INFINITE;
else if (timeout.to_us_raw() > 100000) ticks = MS2ST(timeout.to_ms_raw());
else ticks = US2ST(timeout.to_us_raw());
return chEvtWaitAnyTimeout(ALL_EVENTS, ticks);
}
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");
}
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 THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palTogglePad(GPIOE, GPIOE_LED3_RED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
/**
* @brief Wait for signals.
*/
osEvent osSignalWait(int32_t signals, uint32_t millisec) {
osEvent event;
if (signals == 0)
event.value.signals = (uint32_t)chEvtWaitAnyTimeout((eventmask_t)signals,
(systime_t)millisec);
else
event.value.signals = (uint32_t)chEvtWaitAllTimeout((eventmask_t)signals,
(systime_t)millisec);
/* Type of event.*/
if (event.value.signals == 0)
event.status = osEventTimeout;
else
event.status = osEventSignal;
return event;
}
static msg_t can_rx(void *p) {
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(IOPORT3, GPIOC_LED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
/**
* @brief Wait for signals.
*/
osEvent osSignalWait(int32_t signals, uint32_t millisec) {
osEvent event;
systime_t timeout = ((millisec == 0) || (millisec == osWaitForever)) ?
TIME_INFINITE : MS2ST(millisec);
if (signals == 0)
event.value.signals = (uint32_t)chEvtWaitAnyTimeout(ALL_EVENTS, timeout);
else
event.value.signals = (uint32_t)chEvtWaitAllTimeout((eventmask_t)signals,
timeout);
/* Type of event.*/
if (event.value.signals == 0)
event.status = osEventTimeout;
else
event.status = osEventSignal;
return event;
}
static THD_FUNCTION(can_rx, p) {
struct can_instance *cip = p;
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
continue;
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palToggleLine(cip->led);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static THD_FUNCTION(uart_thread, arg)
{
(void)arg;
chRegSetThreadName("UART thread");
// KPA.
event_listener_t kpa_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD1), &kpa_event_listener, EVENT_MASK(1), CHN_INPUT_AVAILABLE);
struct writer kpa_writer;
writer_init(&kpa_writer, &SD1, uart_write);
// FBV.
event_listener_t fbv_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD6), &fbv_event_listener, EVENT_MASK(2), CHN_INPUT_AVAILABLE);
struct writer fbv_writer;
writer_init(&fbv_writer, &SD6, uart_write);
// Bridge-
struct bridge bridge;
bridge_init(&bridge, &kpa_writer, &fbv_writer);
uint8_t byte = 0;
while (true)
{
eventflags_t evt = chEvtWaitAnyTimeout(EVENT_MASK(1) | EVENT_MASK(2), MS2ST(1));
if (evt & EVENT_MASK(1))
{
chEvtGetAndClearFlags(&kpa_event_listener);
while (readByte(&SD1, &byte))
bridge_update_kpa(&bridge, byte);
}
if (evt & EVENT_MASK(2))
{
chEvtGetAndClearFlags(&fbv_event_listener);
while (readByte(&SD6, &byte))
bridge_update_fbv(&bridge, byte);
}
bridge_update_time(&bridge, clock_get_ms());
}
}
static THD_FUNCTION(serialThread, arg) {
(void)arg;
event_listener_t new_data_listener;
event_listener_t sd1_listener;
event_listener_t sd2_listener;
chEvtRegister(&new_data_event, &new_data_listener, 0);
eventflags_t events = CHN_INPUT_AVAILABLE
| SD_PARITY_ERROR | SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR | SD_BREAK_DETECTED;
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD1),
&sd1_listener,
EVENT_MASK(1),
events);
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD2),
&sd2_listener,
EVENT_MASK(2),
events);
bool need_wait = false;
while(true) {
eventflags_t flags1 = 0;
eventflags_t flags2 = 0;
if (need_wait) {
eventmask_t mask = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
if (mask & EVENT_MASK(1)) {
flags1 = chEvtGetAndClearFlags(&sd1_listener);
print_error("DOWNLINK", flags1, &SD1);
}
if (mask & EVENT_MASK(2)) {
flags2 = chEvtGetAndClearFlags(&sd2_listener);
print_error("UPLINK", flags2, &SD2);
}
}
// Always stay as master, even if the USB goes into sleep mode
is_master |= usbGetDriverStateI(&USBD1) == USB_ACTIVE;
router_set_master(is_master);
need_wait = true;
need_wait &= read_from_serial(&SD2, UP_LINK) == 0;
need_wait &= read_from_serial(&SD1, DOWN_LINK) == 0;
update_transport();
}
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while (true) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
oca_led_toggle(oca_led_act);
CanMessage UsbMessage = CanMessage_init_default;
UsbMessage.DLC = rxmsg.DLC;
UsbMessage.RTR = rxmsg.DLC;
UsbMessage.IDE = rxmsg.IDE;
UsbMessage.ID = rxmsg.EID;
UsbMessage.Data1 = rxmsg.data32[0];
UsbMessage.Data1 = rxmsg.data32[1];
uint8_t buffer[30];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
pb_encode(&stream, CanMessage_fields, &UsbMessage);
/*if(serusbcfg.usbp->state == USB_ACTIVE)
{
if(SDU1.state == SDU_READY)
chnWrite(&SDU1, buffer, stream.bytes_written);
}*/
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
void AP_IOMCU_FW::update()
{
eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(1));
if (do_reboot && (AP_HAL::millis() > reboot_time)) {
hal.scheduler->reboot(true);
while (true) {}
}
if ((mask & EVENT_MASK(IOEVENT_PWM)) ||
(last_safety_off != reg_status.flag_safety_off)) {
last_safety_off = reg_status.flag_safety_off;
pwm_out_update();
}
uint32_t now = AP_HAL::millis();
// output SBUS if enabled
if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) &&
reg_status.flag_safety_off &&
now - sbus_last_ms >= sbus_interval_ms) {
// output a new SBUS frame
sbus_last_ms = now;
sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS);
}
// run remaining functions at 1kHz
if (now != last_loop_ms) {
last_loop_ms = now;
heater_update();
rcin_update();
safety_update();
rcout_mode_update();
hal.rcout->timer_tick();
}
}
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;
}
msg_t LwIpThreadFunc(void* arg)
{
(void) arg;
bool started_phy_read = false;
bool link_status = false;
rcc::enable_gpio<pins::red, pins::yellow, pins::green, pins::button>();
gpio::configure<pins::red, pins::yellow, pins::green>(gpio::output());
gpio::configure<pins::button>(gpio::input, gpio::pull_up());
netif_init();
etharp_init();
udp_init();
tcpip_init(NULL,NULL);
std::copy(mac_addr, mac_addr+6, interface.hwaddr);
std::copy(mac_addr, mac_addr+6, cfg.mac_addr.begin());
interface.hwaddr_len = 6;
netif_add(&interface, &ip.addr, &netmask.addr, &gateway.addr, NULL, lwip_funcs::ethernetif_init, ::ethernet_input );
netif_set_default(&interface);
netif_set_up(&interface);
bool dhcp_bound = false;
unsigned int history = 0;
bool enabled = false;
while(true) {
auto eventmask = chEvtWaitAnyTimeout( RxAvailableMask | TxCompletedMask, 100 );
if( eventmask & RxAvailableMask ) {
rx_walk_descriptors(&interface);
}
if( eventmask & TxCompletedMask ) {
tx_walk_descriptors();
}
unsigned int val = pins::button::get();
history = (history << 1) | val;
if((history&0xF) == 12 /*~3&0xF*/ ) {
if(enabled) {
ptp_port.post_thread_command(uptp::SystemPort::ThreadCommands::DisableClock);
pins::green::reset();
} else {
ptp_port.post_thread_command(uptp::SystemPort::ThreadCommands::EnableClock);
pins::green::set();
}
enabled = !enabled;
}
if(started_phy_read) {
auto phystats = eth::phy_read_register_finish();
if(!link_status && (phystats & 1)) {
start_mac_dma(phystats, interface);
netif_set_link_up(&interface);
dhcp_start(&interface);
ptp_port.start();
link_status = true;
} else if(link_status && !(phystats&1)){
//link_status = false;
//netif_set_link_down(&interface);
}
started_phy_read = false;
} else {
eth::phy_read_register_start(0x10, 1);
started_phy_read = true;
}
if(link_status && interface.dhcp->state == DHCP_STATE_BOUND && !dhcp_bound) {
ptp_port.ip_addr_changed(interface.ip_addr);
dhcp_bound = true;
}
}
return 0;
}
eventmask_t BaseThread::waitAnyEventTimeout(eventmask_t ewmask,
systime_t time) {
return chEvtWaitAnyTimeout(ewmask, time);
}
CCM_FUNC static THD_FUNCTION(ThreadCAN, arg)
{
event_listener_t el;
CANTxFrame txmsg;
CANRxFrame rxmsg;
uint16_t i;
bool pidserved;
(void)arg;
chRegSetThreadName("CAN Bus");
while(CAND1.state != CAN_READY) chThdSleepMilliseconds(10);
while(SDU1.state != SDU_READY) chThdSleepMilliseconds(10);
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(TRUE) {
// Are we using coms for sensor data? If not just sleep.
if (settings.sensorsInput != SENSORS_INPUT_COM && (settings.functions & FUNC_OBD_SERVER) == 0) {
chThdSleepMilliseconds(100);
continue;
}
//checkCanFilters(&CAND1, &cancfg);
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
continue;
}
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
if (dbg_can) {
chprintf(DBG_STREAM, "->[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), rxmsg.SID, rxmsg.RTR);
for(i = 0; i < rxmsg.DLC; i++) {
chprintf(DBG_STREAM, ":%02x", rxmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
if (settings.functions & FUNC_OBD_SERVER) {
pidserved = serveCanOBDPidRequest(&CAND1, &txmsg, &rxmsg);
if (dbg_can && pidserved) {
chprintf(DBG_STREAM, "<-[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), txmsg.SID, txmsg.RTR);
for(i = 0; i < txmsg.DLC; i++) {
chprintf(DBG_STREAM, ":%02x", txmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
else if (dbg_can) {
chprintf(DBG_STREAM, "!![CANBUS][%08x][ignored sender %03x]\r\n", chSysGetRealtimeCounterX(), rxmsg.SID);
}
}
if ((settings.functions & FUNC_OBD_SERVER) == 0) {
if (settings.sensorsInput == SENSORS_INPUT_OBD_CAN) {
readCanOBDPidResponse(&rxmsg);
}
else if (settings.sensorsInput == SENSORS_INPUT_YAMAHA_CAN) {
readCanYamahaPid(&rxmsg);
}
}
}
if (settings.sensorsInput == SENSORS_INPUT_OBD_CAN
&& (settings.functions & FUNC_OBD_SERVER) == 0) {
// Request OBD PIDs
sendCanOBDFrames(&CAND1, &txmsg);
if (dbg_can) {
chprintf(DBG_STREAM, "<-[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), txmsg.SID, txmsg.RTR);
for(i = 0; i < 8; i++) {
chprintf(DBG_STREAM, ":%02x", txmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
chThdSleepMilliseconds(100); // ~10Hz
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return;
}
eventmask_t Event::WaitAnyTimeout(eventmask_t ewmask, systime_t time) {
return chEvtWaitAnyTimeout(ewmask, time);
}
