merge changes from field

buzz_scripts/include/act/barrier.bzz

@@ -7,7 +7,7 @@
 #
 # Constants
 #
-BARRIER_TIMEOUT = 200 # in steps
+BARRIER_TIMEOUT = 600 # in steps
 BARRIER_VSTIG = 80
 timeW = 1
 barrier = nil
@@ -108,4 +108,4 @@ function barrier_allgood(barrier, bc) {
         barriergood = 0
     })
   return barriergood
-}
+}

buzz_scripts/include/act/naviguation.bzz

@@ -33,4 +33,4 @@ function LimitSpeed(vel_vec, factor){
   if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
   	vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
 	return vel_vec
-}
+}

buzz_scripts/include/act/states.bzz

@@ -319,62 +319,5 @@ function voronoicentroid_done() {
 # Custom state function for the developer to play with
 function cusfun(){
 	BVMSTATE="CUSFUN"
-	#log("cusfun: yay!!!")
+	
-	#LOCAL_TARGET = math.vec2.new(5 ,0 )
-	#local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
-    #if(math.vec2.length(local_set_point) > GOTODIST_TOL){
-	#	local_set_point = LimitSpeed(local_set_point, 1.0)
-	#	goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
-    #}
-    #else{
-    #	log("TARGET REACHED")
-    #}
-
-	initialPoint = 0
-
-	# Get waypoint list
-	if (id == 1) {
-		waypoints = {
-			.0 = math.vec2.new(-5.0, -5.0),
-			.1 = math.vec2.new(5.0, 5.0)
-		}
-	} else if (id == 2 ) {
-		waypoints = {
-			.0 = math.vec2.new(5.0, 5.0),
-			.1 = math.vec2.new(-5.0, -5.0)
-		}
-	}
-
-	# Current waypoint
-	target_point = waypoints[point]
-
-	# Get drone position and transform to global frame - depends on lauch file
-	offset_x = 0.0
-	offset_y = 0.0
-	if (id == 1) {
-		offset_x = 5.0
-		offset_y = 5.0
-	} else if (id == 2 ) {
-		offset_x = -5.0
-		offset_y = -5.0
-	}
-	pos_x = pose.position.x + offset_x;
-	pos_y = pose.position.y + offset_y;
-
-	# Get a vector pointing to target and target distance
-	vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y))
-	distance_to_target = math.vec2.length(vector_to_target)
-
-	# Run LCA and increment waypoint
-	if(distance_to_target > 0.2){
-		vector_to_target = LCA(vector_to_target)
-		vector_to_target = LimitSpeed(vector_to_target, 0.5)
-		goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0)
-	} else {
-		if(point < size(waypoints) - 1){
-			point = point + 1
-		} else {
-			point = 0
-		}
-	}
 }

buzz_scripts/include/taskallocate/bidding.bzz

@@ -6,8 +6,8 @@
 #
 ########################################
 
-CSV_FILENAME_AND_PATH = "/home/amber/ROS_WS/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv"
+CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv"
-OUTPUT_FILENAME_AND_PATH = "/home/amber/bidding_output/output-" # automatically completed with 'ID.csv'
+OUTPUT_FILENAME_AND_PATH = "/home/ubuntu/bidding_output/output-" # automatically completed with 'ID.csv'
 BID_WAIT = 40
 PICTURE_WAIT = 40
 BASE_ALTITUDE = 5.0

buzz_scripts/include/utils/takeoff_heights.bzz

@@ -8,7 +8,7 @@ takeoff_heights ={
     .6 = 12.0,
     .9 = 8.0,
     .11 = 6.0,
-    .14 = 18.0,
+    .14 = 9.0,
     .16 = 9.0,
     .17 = 3.0,
     .18 = 6.0,
@@ -22,4 +22,4 @@ takeoff_heights ={
     .26 = 10.0,
     .27 = 28.0,
     .28 = 14.0
-}
+}

include/roscontroller.h

@@ -11,7 +11,7 @@
 #include "mavros_msgs/ExtendedState.h"
 #include "mavros_msgs/SetMode.h"
 #include "mavros_msgs/State.h"
-#include "mavros_msgs/BatteryStatus.h"
+//#include "mavros_msgs/BatteryStatus.h"
 #include "sensor_msgs/BatteryState.h"
 #include "mavros_msgs/Mavlink.h"
 #include "mavros_msgs/PositionTarget.h"

src/buzz_utility.cpp

@@ -17,7 +17,7 @@ static buzzvm_t VM = 0;
 static char* BO_FNAME = 0;
 static uint8_t* BO_BUF = 0;
 static buzzdebug_t DBG_INFO = 0;
-static uint32_t MAX_MSG_SIZE = 210;//250;  // Maximum Msg size for sending update packets
+static uint32_t MAX_MSG_SIZE = 190;//250;  // Maximum Msg size for sending update packets
 static uint8_t Robot_id = 0;
 static std::vector<uint8_t*> IN_MSG;
 std::map<int, Pos_struct> users_map;

src/roscontroller.cpp

@@ -839,7 +839,7 @@ script
 	      BClpose = true;
       }
       if (current_mode != "GUIDED" && setmode)
-        SetMode("GUIDED", 2000);  // added for compatibility with 3DR Solo
+        SetMode("GUIDED", 3000);  // added for compatibility with 3DR Solo
       if(setmode)
       {
         if (mav_client.call(cmd_srv))
@@ -1040,7 +1040,7 @@ void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg)
 / Update battery status into BVM from subscriber
 /------------------------------------------------------*/
 {
-  buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage);
+  buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage * 100.0);
   //  DEBUG
   // ROS_INFO("voltage : %d  current : %d  remaining : %d",msg->voltage,
   // msg->current, msg ->remaining);