static void
cmd_eval(void)
{
struct eval_factors ef = compute_eval_factors(current_position());
printf(" material: ");
print_centipawns(ef.material);
printf("\n basic_mobility: ");
print_centipawns(ef.basic_mobility);
printf("\n center_control: ");
print_centipawns(ef.center_control);
printf("\n threats: ");
print_centipawns(ef.threats);
printf("\n pawn_structure: ");
print_centipawns(ef.pawn_structure);
printf("\n passed_pawns: ");
print_centipawns(ef.passed_pawns);
printf("\n king_safety: ");
print_centipawns(ef.king_safety);
printf("\n rook_placement: ");
print_centipawns(ef.rook_placement);
printf("\n knight_placement: ");
print_centipawns(ef.knight_placement);
printf("\n bishop_placement: ");
print_centipawns(ef.bishop_placement);
printf("\n value: ");
print_centipawns(eval(current_position()));
putchar('\n');
}
static int
try_read_move(const char *cmd)
{
move move;
if (is_end())
return 1;
switch (read_move(current_position(), cmd, &move, turn())) {
case none_move:
return 1;
case illegal_move:
printf("Illegal move: %s\n", cmd);
return 0;
case 0:
if (!is_force_mode && !is_opp_turn()) {
printf("It is not %s's turn\n",
whose_turn[opponent_of(computer_side)]);
return 0;
}
operator_move(move);
return 0;
default:
assert(0);
}
return 0;
}
static void
cmd_poskey(void)
{
uint64_t key[2];
get_position_key(current_position(), key);
printf("%016" PRIx64 " %016" PRIx64 "\n", key[0], key[1]);
}
void motion(int x, int y)
{
if (!current_trackball_state->dragged)
return;
glm::ivec2 current_position(x, y);
if (camera_zoom) {
glm::vec2 v(current_trackball_state->prev_position.x - current_position.x, current_trackball_state->prev_position.y - current_position.y);
float delta = glm::length(v);
if (delta > 0.0f) {
float direction = glm::sign(glm::dot(v, glm::vec2(0.0f, 1.0f)));
float theta = direction * glm::clamp(delta, 0.1f, 0.5f);
camera_fovy = glm::clamp(camera_fovy + theta, 5.0f, 60.0f);
}
} else {
glm::vec3 v0 = map_to_sphere(*current_trackball_state, current_trackball_state->prev_position);
glm::vec3 v1 = map_to_sphere(*current_trackball_state, current_position);
glm::vec3 v2 = glm::cross(v0, v1); // calculate rotation axis
float d = glm::dot(v0, v1);
float s = std::sqrt((1.0f + d) * 2.0f);
glm::quat q(0.5f * s, v2 / s);
current_trackball_state->orientation = q * current_trackball_state->orientation;
current_trackball_state->orientation /= glm::length(current_trackball_state->orientation);
}
current_trackball_state->prev_position.x = x;
current_trackball_state->prev_position.y = y;
}
static void
decide_move(void)
{
mtx_lock(&game_mutex);
if (game_started && !is_end() && !is_force_mode) {
if (game_started && has_single_response()) {
move m = get_single_response();
print_computer_move(m);
add_move(m);
engine_move_count_inc();
}
else {
move m = book_get_move(book, current_position());
if (m != none_move) {
print_computer_move(m);
add_move(m);
engine_move_count_inc();
}
else {
set_thinking_done_cb(computer_move,
++callback_key);
start_thinking();
}
}
}
else {
game_started = false;
}
mtx_unlock(&game_mutex);
}
void drawing_context::curve_to(math::vector2f const& control_point, math::vector2f const& point)
{
const float k = 0.551784f;
auto p1 = lerp(current_position(path_), control_point, k);
auto p2 = lerp(point, control_point, k);
path_.cubicTo(p1.x(), p1.y(), p2.x(), p2.y(), point.x(), point.y());
}
bool hybrid_beg_condition::check(const boost::shared_ptr<mrrocpp::ecp::common::generator::behaviour> & bhvr)
{
std::time_t t;
std::cout<<"hybrid_beg_condition::check\n";
lib::Homog_matrix actual_position_matrix;
lib::Xyz_Angle_Axis_vector tool_vector;
lib::Xyz_Angle_Axis_vector angle_axis_vector;
std::vector<double> current_position(6);
bhvr->the_robot->ecp_command.instruction_type = lib::GET;
bhvr->the_robot->ecp_command.get_type = ARM_DEFINITION;
actual_position_matrix = bhvr->the_robot->reply_package.arm.pf_def.arm_frame;
actual_position_matrix.get_xyz_angle_axis(angle_axis_vector);
angle_axis_vector.to_vector(current_position);
std::cout << "Begin_visible_condition : \n" << current_position[0]<<"\t"<<
current_position[1]<<"\t"<< current_position[2]<<"\n"<< current_position[3]<<"\t"<<
current_position[4]<<"\t"<< current_position[5]<<"\n";
boost::shared_ptr<visual_behaviour> bh = boost::dynamic_pointer_cast<visual_behaviour>(bhvr);
if(!bh->sensor_configured)
{
std::cout<<"Konfiguracja sensora\n";
bh->configure();
bh->sensor_configured=true;
}
lib::Homog_matrix tmp;
float diameter;
bh->vs->get_sensor()->get_reading();
t = std::time(0); // t is an integer type
std::cout << t << " seconds since 01-Jan-1970\n";
bh->vs->get_position_change(tmp, 0.1);
while((diameter = bh->vs->get_objects_diameter())==0 || bh->vs->get_ellipse_factor()<0.6)
{
bh->vs->get_sensor()->get_reading();
bh->vs->get_position_change(tmp, 0.1);
std::cout << "Srednica obiektu to :" << diameter<< "\n";
t = std::time(0); // t is an integer type
std::cout << t << " seconds since 01-Jan-1970\n";
}
std::cout << "Srednica obiektu to :" << diameter<< "\n";
t = std::time(0); // t is an integer type
std::cout << t << " seconds since 01-Jan-1970\n";
if(diameter>0 && current_position[1]>=1.8)
{
std::cout<<"Hybride Begin Condition in left is met!!!\n";
std::cout<<"hybrid_beg_condition::check end\n";
return true;
}
std::cout<<"Hybride Begin Condition in left is not met!!!\n";
std::cout<<"hybrid_beg_condition::check end\n";
return false;
};
static void
cmd_undo(void)
{
if (is_force_mode) {
if (revert() != 0)
general_error();
reset_engine(current_position());
debug_engine_set_player_to_move(turn());
}
}
static void
cmd_perfts(void)
{
unsigned depth;
depth = get_uint(1, 1024);
for (unsigned i = 1; i <= depth; ++i) {
uintmax_t n = perft(current_position(), i);
printf("%s%u : %" PRIuMAX "\n", (i < 10) ? " " : "", i, n);
}
}
static void
cmd_printboard(void)
{
char str[BOARD_BUFFER_LENGTH];
mtx_lock(&game_mutex);
(void) board_print(str, current_position(), turn());
mtx_unlock(&game_mutex);
(void) fputs(str, stdout);
}
static void
cmd_redo(void)
{
mtx_lock(&game_mutex);
if (is_force_mode) {
if (forward() != 0)
general_error();
reset_engine(current_position());
debug_engine_set_player_to_move(turn());
}
mtx_unlock(&game_mutex);
}
static void
cmd_hint(void)
{
move m;
char str[MOVE_STR_BUFFER_LENGTH];
mtx_lock(&game_mutex);
if (engine_get_best_move(&m) == 0) {
print_move(current_position(), m, str, conf->move_not, turn());
printf("Hint: %s\n", str);
}
mtx_unlock(&game_mutex);
}
// ECoG positions are reported line by line in the positions Matrix
// mat is supposed to be filled with zeros
// mat is the linear application which maps x (the unknown vector in symmetric system) -> v (potential at the ECoG electrodes)
// difference with Head2EEG is that it interpolates the inner skull layer instead of the scalp layer.
void assemble_Head2ECoG(SparseMatrix& mat, const Geometry& geo, const Matrix& positions, const Interface& i)
{
mat = SparseMatrix(positions.nlin(), (geo.size()-geo.outermost_interface().nb_triangles()));
Vect3 current_position;
Vect3 current_alphas;
Triangle current_triangle;
for ( unsigned it = 0; it < positions.nlin(); ++it) {
for ( unsigned k = 0; k < 3; ++k) {
current_position(k) = positions(it, k);
}
dist_point_interface(current_position, i, current_alphas, current_triangle);
mat(it, current_triangle.s1().index()) = current_alphas(0);
mat(it, current_triangle.s2().index()) = current_alphas(1);
mat(it, current_triangle.s3().index()) = current_alphas(2);
}
}
void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
{
++messages_received_;
if( !validateFloats( *message ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
return;
}
setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
if( last_used_message_ )
{
Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
(last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() )
{
return;
}
}
Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
transformArrow( message, arrow );
QColor color = color_property_->getColor();
arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
float length = length_property_->getFloat();
Ogre::Vector3 scale( length, length, length );
arrow->setScale( scale );
arrows_.push_back( arrow );
last_used_message_ = message;
context_->queueRender();
}
bool settings_parser_c::parse(int argc, char *argv[]) {
arg_c = argc;
arg_v = argv;
position = 1;
for (int i = 0; i < parsers.size(); ++i) {
parsers[i]->invoke_initialization(*this);
}
while (current_position() < argc && !stopped) {
for (auto parser = parsers.begin(); parser != parsers.end(); parser++) {
fetch_current_position();
if ((*parser)->parse(*this)) {
save_current_position((*parser));
}
restore_position();
}
if (saved_count() == 1) {
try {
pop_saved_parser()->invoke(*this);
} catch (std::string error) {
std::cerr << "Invalid parameter value: " << arg_v[position - 1] << " with error: " << error << std::endl;
return false;
}
} else if (saved_count() > 1) {
//ambiguous arguments
//throw
std::cerr << "ambiguous arguments" << std::endl;
} else {
if (is_program()) {
parse_program();
//parse_program//until separator detected
} else {
//unknown_argument
std::cerr << "unknown argument " << arg_v[position] << std::endl;
//throw "";
return false;
}
}
}
return true;
}
static void
print_computer_move(move m)
{
char str[MOVE_STR_BUFFER_LENGTH];
enum move_notation_type mn;
unsigned move_counter;
bool is_black;
if (is_xboard || is_uci)
mn = mn_coordinate;
else
mn = conf->move_not;
mtx_lock(&game_mutex);
(void) print_move(current_position(), m, str, mn, turn());
move_counter = game_full_move_count(game);
is_black = (turn() == black);
mtx_unlock(&game_mutex);
mtx_lock(&stdout_mutex);
tracef("%s %s", __func__, str);
if (is_xboard) {
printf("move %s\n", str);
}
else if (is_uci) {
printf("bestmove %s\n", str);
}
else {
printf("%u. ", move_counter);
if (is_black)
printf("... ");
puts(str);
}
mtx_unlock(&stdout_mutex);
}
void
CameraTrigger::update_distance()
{
if (_lpos_sub < 0) {
_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
}
if (_turning_on) {
return;
}
if (_trigger_enabled) {
struct vehicle_local_position_s local = {};
orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &local);
if (local.xy_valid) {
// Initialize position if not done yet
math::Vector<2> current_position(local.x, local.y);
if (!_valid_position) {
// First time valid position, take first shot
_last_shoot_position = current_position;
_valid_position = local.xy_valid;
shoot_once();
}
// Check that distance threshold is exceeded
if ((_last_shoot_position - current_position).length() >= _distance) {
shoot_once();
_last_shoot_position = current_position;
}
}
}
}
static void
cmd_new(void)
{
callback_key++;
stop_thinking();
mtx_lock(&game_mutex);
game_destroy(game);
if ((game = game_create()) == NULL)
INTERNAL_ERROR();
reset_engine(current_position());
debug_engine_set_player_to_move(turn());
computer_side = black;
set_thinking_done_cb(computer_move, ++callback_key);
unset_search_depth_limit();
if (!(is_xboard || is_uci))
puts("New game - computer black");
game_started = false;
is_force_mode = false;
mtx_unlock(&game_mutex);
}
static void
cmd_setboard(void)
{
struct game *g;
if (game_started)
return;
g = game_create_fen(xstrtok_r(NULL, "\n\r", &line_lasts));
if (g == NULL) {
(void) fprintf(stderr, "Unable to parse FEN\n");
return;
}
mtx_lock(&game_mutex);
game_destroy(game);
game = g;
reset_engine(current_position());
debug_engine_set_player_to_move(turn());
mtx_unlock(&game_mutex);
}
static void
run_cmd_divide(bool ordered)
{
struct divide_info *dinfo;
const char *line;
mtx_lock(&game_mutex);
dinfo = divide_init(current_position(),
get_uint(0, 1024),
turn(),
ordered);
mtx_unlock(&game_mutex);
mtx_lock(&stdout_mutex);
while ((line = divide(dinfo, conf->move_not)) != NULL)
puts(line);
mtx_unlock(&stdout_mutex);
divide_destruct(dinfo);
}
static void
add_move(move m)
{
mtx_lock(&game_mutex);
char move_str[MOVE_STR_BUFFER_LENGTH];
(void) print_move(current_position(), m, move_str, mn_san, turn());
if (game_append(game, m) == 0) {
engine_process_move(m);
debug_engine_set_player_to_move(turn());
tracef("repro: %s", move_str);
if (is_end()) {
game_started = false;
if (is_checkmate() && turn() == white)
puts("0-1 {Black mates}");
else if (is_checkmate() && turn() == black)
puts("1-0 {White mates}");
else if (is_stalemate())
puts("1/2-1/2 {Stalemate}");
else if (is_draw_by_insufficient_material())
puts("1/2-1/2 {No mating material}");
else if (is_draw_by_repetition())
puts("1/2-1/2 {Draw by repetition}");
else if (is_draw_by_50_move_rule())
puts("1/2-1/2 {Draw by 50 move rule}");
else
abort();
}
}
mtx_unlock(&game_mutex);
}
static void
cmd_perfto(void)
{
printf("%" PRIuMAX "\n", perft_ordered(current_position(),
get_uint(1, 1024)));
}
void MeanShiftDemo( VideoCapture& video, Rect& starting_position, int starting_frame_number, int end_frame)
{
bool half_size = true;
video.set(CV_CAP_PROP_POS_FRAMES,starting_frame_number);
Mat current_frame, hls_image;
std::vector<cv::Mat> hls_planes(3);
video >> current_frame;
Rect current_position(starting_position);
if (half_size)
{
resize(current_frame, current_frame, Size( current_frame.cols/2, current_frame.rows/2 ));
current_position.height /= 2;
current_position.width /= 2;
current_position.x /= 2;
current_position.y /= 2;
}
cvtColor(current_frame, hls_image, CV_BGR2HLS);
split(hls_image,hls_planes);
int chosen_channel = 0; // Hue channel
Mat image1ROI = hls_planes[chosen_channel](current_position);
float channel_range[2] = { 0.0, 255.0 };
int channel_numbers[1] = { 0 };
int number_bins[1] = { 32 };
MatND histogram[1];
const float* channel_ranges = channel_range;
calcHist(&(image1ROI), 1, channel_numbers, Mat(), histogram[0], 1 , number_bins, &channel_ranges);
normalize(histogram[0],histogram[0],1.0);
rectangle(current_frame,current_position,Scalar(0,255,0),2);
Mat starting_frame = current_frame.clone();
int frame_number = starting_frame_number;
while (!current_frame.empty() && (frame_number < end_frame))
{
// Calculate back projection
Mat back_projection_probabilities;
calcBackProject(&(hls_planes[chosen_channel]),1,channel_numbers,*histogram,back_projection_probabilities,&channel_ranges,255.0);
// Remove low saturation points from consideration
Mat saturation_mask;
inRange( hls_image, Scalar(0,10,50,0),Scalar(180,256,256,0), saturation_mask );
bitwise_and( back_projection_probabilities, back_projection_probabilities,back_projection_probabilities, saturation_mask );
// Mean shift
TermCriteria criteria(cv::TermCriteria::MAX_ITER,5,0.01);
meanShift(back_projection_probabilities,current_position,criteria);
// Output to screen
rectangle(current_frame,current_position,Scalar(0,255,0),2);
Mat chosen_channel_image, back_projection_image;
cvtColor(hls_planes[chosen_channel], chosen_channel_image, CV_GRAY2BGR);
cvtColor(back_projection_probabilities, back_projection_image, CV_GRAY2BGR);
Mat row1_output = JoinImagesHorizontally( starting_frame, "Starting position", chosen_channel_image, "Chosen channel (Hue)", 4 );
Mat row2_output = JoinImagesHorizontally( back_projection_image, "Back projection", current_frame, "Current position", 4 );
Mat mean_shift_output = JoinImagesVertically(row1_output,"",row2_output,"", 4);
imshow("Mean Shift Tracking", mean_shift_output );
// Advance to next frame
video >> current_frame;
if (half_size)
resize(current_frame, current_frame, Size( current_frame.cols/2, current_frame.rows/2 ));
cvtColor(current_frame, hls_image, CV_BGR2HLS);
split(hls_image,hls_planes);
frame_number++;
cvWaitKey(1000);
}
char c = cvWaitKey();
cvDestroyAllWindows();
}
void settings_parser_c::save_current_position(abstract_parser_c *associated_parser) {
saved_positions.push_back(std::pair<int, abstract_parser_c*>(current_position(), associated_parser));
}
void
CameraTrigger::cycle_trampoline(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
if (trig->_vcommand_sub < 0) {
trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated;
orb_check(trig->_vcommand_sub, &updated);
// while the trigger is inactive it has to be ready
// to become active instantaneously
int poll_interval_usec = 5000;
if (trig->_mode < 3) {
if (updated) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
// Set trigger rate from command
if (cmd.param2 > 0) {
trig->_interval = cmd.param2;
param_set(trig->_p_interval, &(trig->_interval));
}
if (cmd.param1 < 1.0f) {
trig->control(false);
} else if (cmd.param1 >= 1.0f) {
trig->control(true);
// while the trigger is active there is no
// need to poll at a very high rate
poll_interval_usec = 100000;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
if (cmd.param5 > 0) {
// One-shot trigger, default 1 ms interval
trig->_interval = 1000;
trig->control(true);
}
}
}
} else {
// Set trigger based on covered distance
if (trig->_vlposition_sub < 0) {
trig->_vlposition_sub = orb_subscribe(ORB_ID(vehicle_local_position));
}
struct vehicle_local_position_s pos;
orb_copy(ORB_ID(vehicle_local_position), trig->_vlposition_sub, &pos);
if (pos.xy_valid) {
bool turning_on = false;
if (updated && trig->_mode == 4) {
// Check update from command
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
// Set trigger to disabled if the set distance is not positive
if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
trig->turnOnOff();
trig->keepAlive(true);
// Give the camera time to turn on, before starting to send trigger signals
poll_interval_usec = 5000000;
turning_on = true;
} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
hrt_cancel(&(trig->_engagecall));
hrt_cancel(&(trig->_disengagecall));
trig->keepAlive(false);
trig->turnOnOff();
}
trig->_trigger_enabled = cmd.param1 > 0.0f;
trig->_distance = cmd.param1;
}
}
if ((trig->_trigger_enabled || trig->_mode < 4) && !turning_on) {
// Initialize position if not done yet
math::Vector<2> current_position(pos.x, pos.y);
if (!trig->_valid_position) {
// First time valid position, take first shot
trig->_last_shoot_position = current_position;
trig->_valid_position = pos.xy_valid;
trig->shootOnce();
}
// Check that distance threshold is exceeded and the time between last shot is large enough
if ((trig->_last_shoot_position - current_position).length() >= trig->_distance) {
trig->shootOnce();
trig->_last_shoot_position = current_position;
}
}
} else {
poll_interval_usec = 100000;
}
}
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
void
FixedwingPositionControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
/* abort on a nonzero return value from the parameter init */
if (parameters_update()) {
/* parameter setup went wrong, abort */
warnx("aborting startup due to errors.");
_task_should_exit = true;
}
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
* Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint.
*/
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
/* XXX check if radius makes sense here */
float turn_distance = _l1_control.switch_distance(100.0f);
/* lazily publish navigation capabilities */
if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
navigation_capabilities_publish();
}
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}
static void
cmd_polyglotkey(void)
{
printf("%016" PRIx64 "\n",
position_polyglot_key(current_position(), turn()));
}
static void
cmd_qperft(void)
{
printf("%" PRIuMAX "\n", qperft(current_position(), get_uint(1, 1024)));
}