onboard fix and gimbal ctr

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;
-}
+}

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.

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);
 	}
 

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);
   }