static int set_shortterm(vtimer_t *timer) { DEBUG("set_shortterm(): Absolute: %" PRIu32 " %" PRIu32 "\n", timer->absolute.seconds, timer->absolute.microseconds); timer->priority_queue_entry.priority = timer->absolute.microseconds; /* *** UGLY FIX BEGINS *** */ /* Workaround for a bug in a so far undiscovered location which causes the * vtimer to add the same timer twice, locking the system in an infinite * loop inside priority_queue_add. */ priority_queue_remove(&shortterm_priority_queue_root, timer_get_node(timer)); /* *** UGLY FIX ENDS *** */ priority_queue_add(&shortterm_priority_queue_root, timer_get_node(timer)); return 1; }
void condition_variable::wait(unique_lock<mutex>& lock) noexcept { if (!lock.owns_lock()) { throw std::system_error( std::make_error_code(std::errc::operation_not_permitted), "Mutex not locked."); } priority_queue_node_t n; n.priority = sched_active_thread->priority; n.data = sched_active_pid; n.next = NULL; // the signaling thread may not hold the mutex, the queue is not thread safe unsigned old_state = disableIRQ(); priority_queue_add(&m_queue, &n); restoreIRQ(old_state); mutex_unlock_and_sleep(lock.mutex()->native_handle()); if (n.data != -1u) { // on signaling n.data is set to -1u // if it isn't set, then the wakeup is either spurious or a timer wakeup old_state = disableIRQ(); priority_queue_remove(&m_queue, &n); restoreIRQ(old_state); } mutex_lock(lock.mutex()->native_handle()); }
void carmen_localize_ackerman_initialize_particles_uniform(carmen_localize_ackerman_particle_filter_p filter, carmen_robot_ackerman_laser_message *laser, carmen_localize_ackerman_map_p map) { priority_queue_p queue = priority_queue_init(filter->param->num_particles); double *laser_x, *laser_y; int i, j, x_l, y_l; float angle, prob, ctheta, stheta; carmen_point_t point; queue_node_p mark; int *beam_valid; /* compute the correct laser_skip */ if (filter->param->laser_skip <= 0) { filter->param->laser_skip = floor(filter->param->integrate_angle / laser->config.angular_resolution); } fprintf(stderr, "\rDoing global localization... (%.1f%% complete)", 0.0); filter->initialized = 0; /* copy laser scan into temporary memory */ laser_x = (double *)calloc(laser->num_readings, sizeof(double)); carmen_test_alloc(laser_x); laser_y = (double *)calloc(laser->num_readings, sizeof(double)); carmen_test_alloc(laser_y); beam_valid = (int *)calloc(laser->num_readings, sizeof(int)); carmen_test_alloc(beam_valid); for(i = 0; i < laser->num_readings; i++) { if (laser->range[i] < laser->config.maximum_range && laser->range[i] < filter->param->max_range) beam_valid[i] = 1; else beam_valid[i] = 0; } /* do all calculations in map coordinates */ for(i = 0; i < laser->num_readings; i++) { angle = laser->config.start_angle + i * laser->config.angular_resolution; laser_x[i] = (filter->param->front_laser_offset + laser->range[i] * cos(angle)) / map->config.resolution; laser_y[i] = (laser->range[i] * sin(angle)) / map->config.resolution; } for(i = 0; i < filter->param->global_test_samples; i++) { if(i % 10000 == 0) { fprintf(stderr, "\rDoing global localization... (%.1f%% complete)", i / (double)filter->param->global_test_samples * 100.0); carmen_ipc_sleep(0.001); } do { point.x = carmen_uniform_random(0, map->config.x_size - 1); point.y = carmen_uniform_random(0, map->config.y_size - 1); } while(map->carmen_map.map[(int)point.x][(int)point.y] > filter->param->occupied_prob || map->carmen_map.map[(int)point.x][(int)point.y] == -1); point.theta = carmen_uniform_random(-M_PI, M_PI); prob = 0.0; ctheta = cos(point.theta); stheta = sin(point.theta); for(j = 0; j < laser->num_readings && (queue->last == NULL || prob > queue->last->prob); j += filter->param->laser_skip) { if (beam_valid[j]) { x_l = point.x + laser_x[j] * ctheta - laser_y[j] * stheta; y_l = point.y + laser_x[j] * stheta + laser_y[j] * ctheta; if(x_l >= 0 && y_l >= 0 && x_l < map->config.x_size && y_l < map->config.y_size) prob += map->gprob[x_l][y_l]; else prob -= 100; } } priority_queue_add(queue, point, prob); } /* transfer samples from priority queue back into particles */ mark = queue->first; for(i = 0; i < queue->num_elements; i++) { filter->particles[i].x = (mark->point.x * map->config.resolution) + map->config.x_origin; filter->particles[i].y = (mark->point.y * map->config.resolution) + map->config.y_origin; filter->particles[i].theta = mark->point.theta; mark = mark->next; } priority_queue_free(queue); free(laser_x); free(laser_y); free(beam_valid); if(filter->param->do_scanmatching) { for(i = 0; i < filter->param->num_particles; i++) { point.x = filter->particles[i].x; point.y = filter->particles[i].y; point.theta = filter->particles[i].theta; carmen_localize_ackerman_laser_scan_gd(laser->num_readings, laser->range, laser->config.angular_resolution, laser->config.start_angle, &point, filter->param->front_laser_offset, map, filter->param->laser_skip); filter->particles[i].x = point.x; filter->particles[i].y = point.y; filter->particles[i].theta = point.theta; filter->particles[i].weight = 0.0; } } filter->initialized = 1; filter->first_odometry = 1; filter->global_mode = 1; filter->distance_travelled = 0; fprintf(stderr, "\rDoing global localization... (%.1f%% complete)\n\n", 100.0); }
static int set_longterm(vtimer_t *timer) { timer->priority_queue_entry.priority = timer->absolute.seconds; priority_queue_add(&longterm_priority_queue_root, timer_get_node(timer)); return 0; }
static int _msg_send(msg_t *m, kernel_pid_t target_pid, bool block, unsigned state) { #ifdef DEVELHELP if (!pid_is_valid(target_pid)) { DEBUG("msg_send(): target_pid is invalid, continuing anyways\n"); } #endif /* DEVELHELP */ tcb_t *target = (tcb_t*) sched_threads[target_pid]; m->sender_pid = sched_active_pid; if (target == NULL) { DEBUG("msg_send(): target thread does not exist\n"); restoreIRQ(state); return -1; } DEBUG("msg_send() %s:%i: Sending from %" PRIkernel_pid " to %" PRIkernel_pid ". block=%i src->state=%i target->state=%i\n", RIOT_FILE_RELATIVE, __LINE__, sched_active_pid, target_pid, block, sched_active_thread->status, target->status); if (target->status != STATUS_RECEIVE_BLOCKED) { DEBUG("msg_send() %s:%i: Target %" PRIkernel_pid " is not RECEIVE_BLOCKED.\n", RIOT_FILE_RELATIVE, __LINE__, target_pid); if (queue_msg(target, m)) { DEBUG("msg_send() %s:%i: Target %" PRIkernel_pid " has a msg_queue. Queueing message.\n", RIOT_FILE_RELATIVE, __LINE__, target_pid); restoreIRQ(state); if (sched_active_thread->status == STATUS_REPLY_BLOCKED) { thread_yield_higher(); } return 1; } if (!block) { DEBUG("msg_send: %" PRIkernel_pid ": Receiver not waiting, block=%u\n", sched_active_thread->pid, block); restoreIRQ(state); return 0; } DEBUG("msg_send: %" PRIkernel_pid ": send_blocked.\n", sched_active_thread->pid); priority_queue_node_t n; n.priority = sched_active_thread->priority; n.data = (unsigned int) sched_active_thread; n.next = NULL; DEBUG("msg_send: %" PRIkernel_pid ": Adding node to msg_waiters:\n", sched_active_thread->pid); priority_queue_add(&(target->msg_waiters), &n); sched_active_thread->wait_data = (void*) m; int newstatus; if (sched_active_thread->status == STATUS_REPLY_BLOCKED) { newstatus = STATUS_REPLY_BLOCKED; } else { newstatus = STATUS_SEND_BLOCKED; } sched_set_status((tcb_t*) sched_active_thread, newstatus); DEBUG("msg_send: %" PRIkernel_pid ": Back from send block.\n", sched_active_thread->pid); restoreIRQ(state); thread_yield_higher(); } else { DEBUG("msg_send: %" PRIkernel_pid ": Direct msg copy from %" PRIkernel_pid " to %" PRIkernel_pid ".\n", sched_active_thread->pid, thread_getpid(), target_pid); /* copy msg to target */ msg_t *target_message = (msg_t*) target->wait_data; *target_message = *m; sched_set_status(target, STATUS_PENDING); restoreIRQ(state); thread_yield_higher(); } return 1; }
static void recursive_insert(lm_trie_t *trie, ngram_raw_t **raw_ngrams, uint32 *counts, int order) { uint32 unigram_idx = 0; uint32 *words; float *probs; const uint32 unigram_count = (uint32)counts[0]; priority_queue_t *ngrams = priority_queue_create(order, &ngram_ord_comparator); ngram_raw_ord_t *ngram; uint32 *raw_ngrams_ptr; int i; words = (uint32 *)ckd_calloc(order, sizeof(*words)); //for blanks catching probs = (float *)ckd_calloc(order - 1, sizeof(*probs)); //for blanks prob generating ngram = (ngram_raw_ord_t *)ckd_calloc(1, sizeof(*ngram)); ngram->order = 1; ngram->instance.words = &unigram_idx; priority_queue_add(ngrams, ngram); raw_ngrams_ptr = (uint32 *)ckd_calloc(order - 1, sizeof(*raw_ngrams_ptr)); for (i = 2; i <= order; ++i) { ngram_raw_ord_t *tmp_ngram = (ngram_raw_ord_t *)ckd_calloc(1, sizeof(*tmp_ngram)); tmp_ngram->order = i; raw_ngrams_ptr[i-2] = 0; tmp_ngram->instance = raw_ngrams[i - 2][raw_ngrams_ptr[i-2]]; priority_queue_add(ngrams, tmp_ngram); } for (;;) { ngram_raw_ord_t *top = (ngram_raw_ord_t *)priority_queue_poll(ngrams); if (top->order == 1) { trie->unigrams[unigram_idx].next = unigram_next(trie, order); words[0] = unigram_idx; probs[0] = trie->unigrams[unigram_idx].prob; if (++unigram_idx == unigram_count + 1) { ckd_free(top); break; } priority_queue_add(ngrams, top); } else { for (i = 0; i < top->order - 1; i++) { if (words[i] != top->instance.words[i]) { //need to insert dummy suffixes to make ngram of higher order reachable int j; assert(i > 0); //unigrams are not pruned without removing ngrams that contains them for (j = i; j < top->order - 1; j++) { middle_t *middle = &trie->middle_begin[j - 1]; bitarr_address_t address = middle_insert(middle, top->instance.words[j], j + 1, order); //calculate prob for blank float calc_prob = probs[j - 1] + trie->unigrams[top->instance.words[j]].bo; probs[j] = calc_prob; lm_trie_quant_mwrite(trie->quant, address, j - 1, calc_prob, 0.0f); } } } memcpy(words, top->instance.words, top->order * sizeof(*words)); if (top->order == order) { float *weights = top->instance.weights; bitarr_address_t address = longest_insert(trie->longest, top->instance.words[top->order - 1]); lm_trie_quant_lwrite(trie->quant, address, weights[0]); } else { float *weights = top->instance.weights; middle_t *middle = &trie->middle_begin[top->order - 2]; bitarr_address_t address = middle_insert(middle, top->instance.words[top->order - 1], top->order, order); //write prob and backoff probs[top->order - 1] = weights[0]; lm_trie_quant_mwrite(trie->quant, address, top->order - 2, weights[0], weights[1]); } raw_ngrams_ptr[top->order - 2]++; if (raw_ngrams_ptr[top->order - 2] < counts[top->order - 1]) { top->instance = raw_ngrams[top->order-2][raw_ngrams_ptr[top->order - 2]]; priority_queue_add(ngrams, top); } else { ckd_free(top); } } } assert(priority_queue_size(ngrams) == 0); priority_queue_free(ngrams, NULL); ckd_free(raw_ngrams_ptr); ckd_free(words); ckd_free(probs); }