diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index b21db79..d9e0a48 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/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 -} \ No newline at end of file +} diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index a01a302..19227e0 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/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 -} \ No newline at end of file +} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 8d86360..3cd99d3 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/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 - } - } + } \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/bidding.bzz b/buzz_scripts/include/taskallocate/bidding.bzz index 82101c1..b54c2c3 100644 --- a/buzz_scripts/include/taskallocate/bidding.bzz +++ b/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" -OUTPUT_FILENAME_AND_PATH = "/home/amber/bidding_output/output-" # automatically completed with 'ID.csv' +CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.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 diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index 61a8acb..e14c1d7 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/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 -} \ No newline at end of file +} diff --git a/include/roscontroller.h b/include/roscontroller.h index 0f2bc2f..6edb678 100644 --- a/include/roscontroller.h +++ b/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" diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 4686ffd..458ddfd 100644 --- a/src/buzz_utility.cpp +++ b/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 IN_MSG; std::map users_map; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cd8714c..50be762 100644 --- a/src/roscontroller.cpp +++ b/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);