int main(int argc, char **argv)
{
time_t begin = time(NULL);
autopilot_initialize();
serial_start("/dev/ttyUSB0");
printf("Opened serial connection.\n");
read_messages(); // first read to make sure that there is a connection
printf("Acquired initial read\n");
autopilot_start();
printf("locked data from read_messages()\n");
send_pre_arm_void_commands();
// arm_sequence();
usleep(200000); // 200ms = 5Hz
offboard_control_sequence();
usleep(200000);
mavlink_set_position_target_local_ned_t set_point;
for(int i = 0; i < 30; i++){
// printf("Current Initial position : x = %f , y = %f , z = %f\n", ip.x, ip.y, ip.z);
printf("%d: Current set point : x = %f, y = %f z=%f\n", i, ip.x, ip.y, ip.z - 0.25);
set_yaw (ip.yaw, &set_point);
set__(- 1.0 + ip.x , ip.y, ip.z - 2, &set_point);
usleep(100000);
}
// disarm_sequence();
usleep(200000);
disable_offboard_control_sequence();
usleep(200000);
return 0;
}
void toolbar2___::with__(window___* w, void*shangji,void*ce,deque<string>* p, int addr_fmt, char*buf) {
toolbar2_item___* item;
if((*p)[1] == toolbar2_tag2_) {
init__(w->box2__(), left_icon_item_);
item = left_icon_item_;
} else if((*p)[1] == toolbar2_tag3_) {
init__(w->box2__(), tree_item_);
item = tree_item_;
} else if((*p)[1] == toolbar2_tag4_) {
init__(w->box3__(), right_icon_item_);
item = right_icon_item_;
} else if((*p)[1] == toolbar2_tag5_) {
init__(w->box4__(), bottom_icon_item_);
item = bottom_icon_item_;
} else if((*p)[1] == toolbar2_tag6_) {
init__(w->box3__(), right_tree_item_);
item = right_tree_item_;
} else {
init__(w->box__(), icon_item_);
item = icon_item_;
}
for(size_t i2=2;i2<p->size();i2++){
const string& p2=(*p)[i2];
if(p2 == "选择") {
GtkTreeModel *m = item->model__();
GtkTreeIter i;
GtkTreePath *path;
for(size_t i3 = i2;i3 < p->size() - 1;) {
int id2 = 0;
if(!set__(i3, p, id2)) return;
if(!iter_by_id__(m, id2, &i)) {
buzhichi__(i3, p);
return;
}
switch(item->tag_) {
case 'i':
path = gtk_tree_model_get_path (m, &i);
gtk_icon_view_select_path (item->icon_view__(), path);
gtk_tree_path_free (path);
break;
}
}
return;
}
if(p2 == "遍历") {
if(buzu__(++i2, p)) return;
item->for__((*p)[i2], ce, shangji);
continue;
}
if(p2 == "设") {
int id = 0;
if(!set__(i2, p, id)) return;
callback_item___* cbi = callback_item__(id);
if(!cbi) {
wufa__(p->size(), p);
return;
}
GtkTreeModel *m = item->model__();
GtkTreeIter i;
for(i2++; i2 < p->size(); i2++) {
const string& p3 = (*p)[i2];
if(p3 == "名") {
if(set__(i2, p, cbi->title_)) {
if(!iter_by_id__(m, cbi->id_, &i)) {
buzhichi__(i2, p);
return;
}
switch(item->tag_) {
case 'i':
gtk_list_store_set(((icon_item___*)item)->store_, &i, TITLE_COLUMN, cbi->title_.c_str(), -1);
continue;
}
}
return;
}
if(p3 == "提示") {if(set__(i2, p, cbi->tooltip_)) continue; return;}
if(p3 == "图标") {
string name;
if(!set__(i2, p, name)) return;
GdkPixbuf *pixbuf = item->get_icon__(name);
switch(item->tag_) {
case 'i':
gtk_list_store_set(((icon_item___*)item)->store_, &i, PIXBUF_COLUMN, pixbuf, -1);
break;
}
g_object_unref (pixbuf);
continue;
}
buzhichi__(i2, p);
}
break;
}
if(p2 == "获取") {
string code3;
if(!set__(i2, p, code3)) return;
switch(item->tag_) {
case 'i':
icon_view_select__(item->icon_view__(), code3.c_str(), shangji, ce);
break;
}
continue;
}
#ifdef ver_gtk3_
if(p2 == "宽") {
if(buzu__(++i2, p)) return;
gtk_scrolled_window_set_min_content_width(item->scrolled__(), s2i__((*p)[i2]));
continue;
}
if(p2 == "高") {
if(buzu__(++i2, p)) return;
gtk_scrolled_window_set_min_content_height(item->scrolled__(), s2i__((*p)[i2]));
continue;
}
#endif
if(p2 == "名") {
if(buzu__(++i2, p)) return;
if(item->label_) {
gtk_label_set_markup(GTK_LABEL(item->label_), (*p)[i2].c_str());
}
continue;
}
if(p2 == "钮宽") {if(set__(i2, p, item->btn_width_)) continue; return;}
if(p2 == "钮高") {if(set__(i2, p, item->btn_height_)) continue; return;}
if(p2 == "名提示") {item->title_tooltip_ = true; continue;}
if(p2 == "可拖动") {
switch(item->tag_) {
case 'i':
gtk_icon_view_set_reorderable(item->icon_view__(), true);
break;
case 't':
gtk_tree_view_set_reorderable(item->tree_view__(), true);
break;
}
continue;
}
if(p2 == "可选择") {
int i;
if(!set__(i2, p, i)) return;
switch(item->tag_) {
case 'i':
gtk_icon_view_set_selection_mode(item->icon_view__(), (GtkSelectionMode)i);
break;
}
continue;
}
if(p2 == "无标题") {
if(item->tag_ == 'i') {
//gtk_icon_view_set_text_column (item->icon_view__(), -1);
gtk_icon_view_set_markup_column (item->icon_view__(), -1);
continue;
}
buzhichi__(i2, p);
return;
}
//gtk_icon_view_set_selection_mode (iconview__(), GTK_SELECTION_MULTIPLE);
if(p2 == "列头") {
if(buzu__(++i2, p)) return;
if(item->tag_ == 't') {
tree_item___* ti = (tree_item___*)item;
gtk_tree_view_column_set_title(ti->col_, (*p)[i2].c_str());
continue;
}
buzhichi__(i2, p);
return;
}
if(p2 == "删钮") {
GtkTreeModel *m = item->model__();
GtkTreeIter i;
for(size_t i3 = i2;i3 < p->size() - 1;) {
int id2 = 0;
if(!set__(i3, p, id2)) return;
if(!iter_by_id__(m, id2, &i)) {
buzhichi__(i3, p);
return;
}
item->store_del__(&i);
}
break;
}
if(p2 == "加钮") {
string title, tooltip, code, arg, code2, arg2, code3_2;
string set_up;
int posi = -1;
for(i2++;i2<p->size();i2++){
const string& p3=(*p)[i2];
if(p3 == "名") {if(set__(i2, p, title)) continue; return;}
if(p3 == "提示") {if(set__(i2, p, tooltip)) continue; return;}
if(p3 == "代码") {if(set__(i2, p, code)) continue; return;}
if(p3 == "附") {if(set__(i2, p, arg)) continue; return;}
if(p3 == "位置") {if(set__(i2, p, posi)) continue; return;}
if(p3 == "缺省代码") {if(set__(i2, p, item->def_code_)) continue; return;}
if(p3 == "缺省附") {if(set__(i2, p, item->def_arg_)) continue; return;}
if(p3 == "双击代码") {if(set__(i2, p, code2)) continue; return;}
if(p3 == "双击附") {if(set__(i2, p, arg2)) continue; return;}
if(p3 == "缺省双击代码") {if(set__(i2, p, item->def_code2_)) continue; return;}
if(p3 == "缺省双击附") {if(set__(i2, p, item->def_arg2_)) continue; return;}
if(p3 == "右击代码") {if(set__(i2, p, code3_2)) continue; return;}
if(p3 == "缺省右击代码") {if(set__(i2, p, item->def_code3_2_)) continue; return;}
if(p3 == "设置上") {if(set__(i2, p, set_up)) continue; return;}
if(p3 == "恢复上") {
if(item->tag_ == 't') {
string restore_up;
if(set__(i2, p, restore_up)) {
int err;
const char* ret = call4__(ce, restore_up, shangji, 0, NULL, &err);
if(err) return;
((tree_item___*)item)->up_ = ret;
}
} else {
buzhichi__(i2, p);
}
return;
}
if(p3 == "大小") {
if(buzu__(++i2, p)) return;
item->btn_width_ = item->btn_height_ = s2i__((*p)[i2]) * 8;
continue;
}
if(p3 == "-") continue;
GdkPixbuf *pixbuf = item->get_icon__(p3);
callback_item___* si = new callback_item___(title,
!code.empty() ? code : item->def_code_,
!arg.empty() ? arg : item->def_arg_,
tooltip.empty() && item->title_tooltip_ ? title : tooltip);
si->add_code__(!code2.empty() ? code2 : item->def_code2_);
si->add_arg__(!arg2.empty() ? arg2 : item->def_arg2_);
si->add_code__(!code3_2.empty() ? code3_2 : item->def_code3_2_);
item->store_add__(si, pixbuf, posi);
g_object_unref (pixbuf);
if(!set_up.empty()) {
if(item->tag_ == 't') {
tree_item___* ti = (tree_item___*)item;
const char* argv[] = {ti->up_.c_str()};
int err;
call4__(ce, set_up, shangji, 1, argv, &err);
if(err)
return;
ti->up_ = ti->last_;
} else {
buzhichi__(i2, p);
return;
}
set_up.clear();
}
code.clear();
arg.clear();
tooltip.clear();
code2.clear();
arg2.clear();
code3_2.clear();
posi = -1;
}
break;
}
if(p2 == "清空") {
switch(item->tag_) {
case 'i':
gtk_list_store_clear (((icon_item___*)item)->store_);
continue;
}
}
if(p2 == "地址") {
if(addr_fmt == 16)
l2x__((long)item->scrolled_, buf);
else
l2s__((long)item->scrolled_, buf);
continue;
}
buzhichi__(i2, p);
return;
}
}
