From 796bb78dd8c864738b2fdd014630fb5990ec9ca2 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 6 Sep 2017 18:55:12 -0400 Subject: [PATCH] onboard fix and gimbal ctr --- buzz_scripts/include/uavstates.bzz | 50 +++++++++++++----------------- buzz_scripts/testaloneWP.bzz | 2 +- src/buzzuav_closures.cpp | 35 +++++++++++---------- src/roscontroller.cpp | 9 ++++++ 4 files changed, 50 insertions(+), 46 deletions(-) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index b2d4d03..2ae665a 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -27,27 +27,20 @@ function idle() { UAVSTATE = "IDLE" } -firstpassT = 1 function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - if(firstpassT) { - barrier_create(TAKEOFF_VSTIG) - firstpassT = 0 - } - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } } -firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land @@ -56,26 +49,26 @@ function land() { uav_land() if(flight.status != 2 and flight.status != 3){ - barrier_set(ROBOTS,turnedoff,land, 21) + barrier_set(ROBOTS,turnedoff,land, 21) barrier_ready() } } function set_goto(transf) { - UAVSTATE = "GOTO" + UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf); - } + gotoWP(transf); + } - if(rc_goto.id==id){ - if(s!=nil){ - if(s.in()) - s.leave() - } - } else { - neighbors.broadcast("cmd", 16) - neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - } + #if(rc_goto.id==id){ + # if(s!=nil){ + # if(s.in()) + # s.leave() + #} + #} else { + # neighbors.broadcast("cmd", 16) + # neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) + #} } ptime=0 @@ -93,7 +86,7 @@ function picture() { } function gotoWP(transf) { - # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) print(" has to move ", goal.range) m_navigation = math.vec2.newp(goal.range, goal.bearing) @@ -141,7 +134,7 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "GOTO" + UAVSTATE = "GOTOGPS" statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 @@ -190,7 +183,8 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { -# print("gps2rb: ",lat,lon,position.latitude,position.longitude) +# print("gps2rb d: ",lat,lon) +# print("gps2rb h: ",position.latitude,position.longitude) d_lon = lon - position.longitude d_lat = lat - position.latitude ned_x = d_lat/180*math.pi * 6371000.0; @@ -213,4 +207,4 @@ function gps_from_vec(vec) { #goal.latitude = d_lat + position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; #goal.longitude = d_lon + position.longitude; -} \ No newline at end of file +} diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 97b92bf..892b5a6 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -23,7 +23,7 @@ function step() { statef() - log("Current state: ", UAVSTATE) +# log("Current state: ", UAVSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d1f6804..d0ae31a 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -35,6 +35,7 @@ namespace buzzuav_closures{ string WPlistname = ""; std::map< int, buzz_utility::RB_struct> targets_map; + std::map< int, buzz_utility::RB_struct> wplist_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; std::map< int, buzz_utility::neighbors_status> neighbors_status_map; @@ -158,13 +159,13 @@ namespace buzzuav_closures{ RB_arr.longitude=lon; RB_arr.altitude=alt; // Insert elements. - map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid); - if(it!=targets_map.end()) - targets_map.erase(it); - targets_map.insert(make_pair(tid, RB_arr)); + map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid); + if(it!=wplist_map.end()) + wplist_map.erase(it); + wplist_map.insert(make_pair(tid, RB_arr)); } - ROS_INFO("----->Saved %i waypoints.", targets_map.size()); + ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); // Close the file: fin.close(); @@ -213,23 +214,23 @@ namespace buzzuav_closures{ rb_from_gps(tmp, rb, cur_pos); //ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); buzzvm_push(vm, targettbl); - /* When we get here, the "targets" table is on top of the stack */ + // When we get here, the "targets" table is on top of the stack //ROS_INFO("Buzz_utility will save user %i.", it->first); - /* Push user id */ + // Push user id buzzvm_pushi(vm, it->first); - /* Create entry table */ + // Create entry table buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - /* Insert range */ + // Insert range buzzvm_push(vm, entry); buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); buzzvm_pushf(vm, rb[0]); buzzvm_tput(vm); - /* Insert longitude */ + // Insert longitude buzzvm_push(vm, entry); buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); buzzvm_pushf(vm, rb[1]); buzzvm_tput(vm); - /* Save entry into data table */ + // Save entry into data table buzzvm_push(vm, entry); buzzvm_tput(vm); } @@ -341,12 +342,12 @@ namespace buzzuav_closures{ goal[1] = buzzvm_stack_at(vm, 2)->f.value; goal[2] = buzzvm_stack_at(vm, 1)->f.value; if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ - if(targets_map.size()<=0) + if(wplist_map.size()<=0) parse_gpslist(); - goal[0] = targets_map.begin()->second.latitude; - goal[1] = targets_map.begin()->second.longitude; - goal[2] = targets_map.begin()->second.altitude; - targets_map.erase(targets_map.begin()->first); + goal[0] = wplist_map.begin()->second.latitude; + goal[1] = wplist_map.begin()->second.longitude; + goal[2] = wplist_map.begin()->second.altitude; + wplist_map.erase(wplist_map.begin()->first); } double rb[3]; @@ -355,7 +356,7 @@ namespace buzzuav_closures{ if(fabs(rb[0])<150.0) rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); + ROS_WARN("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index f4e0162..3383569 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -761,6 +761,15 @@ void roscontroller::flight_controller_service_call() roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ ROS_INFO("TAKING A PICTURE HERE!! --------------"); + cmd_srv.request.param1 = 90; + cmd_srv.request.param2 = 0; + cmd_srv.request.param3 = 0; + cmd_srv.request.param4 = 0; + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); mavros_msgs::CommandBool capture_command; capture_srv.call(capture_command); }