Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

individual waypoint ctl, map-based waypoint in webctl in formation

This commit is contained in:
dave 2018-10-19 02:34:18 -04:00
parent 6d53409aab
commit 4dec1ca100
7 changed files with 108 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
buzz_scripts/include/act/naviguation.bzz
View File

@ -1,15 +1,21 @@
GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.4 # m.
GOTOANG_TOL = 0.1 # rad.
# Core naviguation function to travel to a GPS target location.
function goto_gps(transf) {
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf()
else {
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
if(transf!=nil)
transf()
} else {
m_navigation = LimitSpeed(m_navigation, 1.0)
#m_navigation = LCA(m_navigation)
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
}
}

23
buzz_scripts/include/act/neighborcomm.bzz
View File

@ -18,9 +18,9 @@ function rc_cmd_listen() {
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
barrier_ready(20)
neighbors.broadcast("cmd", 20)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
BVMSTATE = "PATHPLAN"
# } else if(flight.rc_cmd==16) {
# flight.rc_cmd=0
# BVMSTATE = "PATHPLAN"
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
arm()
@ -110,3 +110,20 @@ function nei_cmd_listen() {
#}
})
}
# broadcast GPS goals
function bd_goal() {
neighbors.broadcast("goal", {.id=rc_goto.id, .la=rc_goto.latitude, .lo=rc_goto.longitude, .al=rc_goto.altitude})
}
# listens to neighbors broadcasting gps goals
function nei_goal_listen() {
neighbors.listen("goal",
function(vid, value, rid) {
print("Got (", vid, ",", value.id, value.la, value.lo, ") #", rid)
if(value.id==id) {
print("Got (", vid, ",", value, ") #", rid)
storegoal(value.la, value.lo, pose.position.altitude)
}
})
}

19
buzz_scripts/include/act/states.bzz
View File

@ -14,10 +14,6 @@ TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.4 # m.
GOTOANG_TOL = 0.1 # rad.
path_it = 0
pic_time = 0
g_it = 0
@ -100,8 +96,23 @@ function stop() {
}
# State function for individual waypoint control
firsttimeinwp = 1
function indiWP() {
if(firsttimeinwp) {
nei_goal_listen()
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0
}
BVMSTATE = "INDIWP"
if(rc_goto.id != -1) {
log(rc_goto.id)
if(rc_goto.id == id) {
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
} else
bd_goal()
}
goto_gps(nil)
}

3
buzz_scripts/include/utils/takeoff_heights.bzz
View File

@ -2,10 +2,11 @@ takeoff_heights ={
.1 = 21.0,
.2 = 18.0,
.3 = 12.0,
.5 = 24.0,
.5 = 12.0,
.6 = 3.0,
.9 = 15.0,
.11 = 6.0,
.14 = 18.0,
.16 = 9.0,
.17 = 3.0,
.18 = 6.0,

4
include/buzzuav_closures.h
View File

@ -50,6 +50,10 @@ int buzzuav_takepicture(buzzvm_t vm);
* Returns the current command from local variable
*/
int getcmd();
/*
* update GPS goal value
*/
void set_gpsgoal(double goal[3]);
/*
* Sets goto position from rc client
*/

67
src/buzzuav_closures.cpp
View File

@ -14,23 +14,29 @@ namespace buzzuav_closures
// TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr;
static double goto_pos[4];
static double rc_goto_pos[3];
static float rc_gimbal[4];
static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 };
static double goto_gpsgoal[3];
static double cur_pos[4];
static double cur_NEDpos[2];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd = 0;
static int rc_id = -1;
static int rc_cmd = 0;
static double rc_gpsgoal[3];
static float rc_gimbal[4];
static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 };
static uint8_t status;
static int cur_cmd = 0;
static int buzz_cmd = 0;
static float height = 0;
static bool deque_full = false;
static int rssi = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
std::map<int, buzz_utility::RB_struct> targets_map;
@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm)
// DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros?
buzz_cmd = NAV_SPLINE_WAYPOINT;
return buzzvm_ret0(vm);
}
@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm)
wplist_map.erase(wplist_map.begin()->first);
}
double rb[3];
set_gpsgoal(goal);
// prevent an overwrite
rc_id = -1;
return buzzvm_ret0(vm);
}
void set_gpsgoal(double goal[3])
/*
/ update GPS goal value
-----------------------------------*/
{
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0) {
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
ROS_INFO("Set GPS GOAL TO ---- %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);
}
int buzzuav_arm(buzzvm_t vm)
@ -504,9 +520,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
-----------------------------------*/
{
rc_id = id;
rc_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude;
rc_goto_pos[2] = altitude;
rc_gpsgoal[0] = latitude;
rc_gpsgoal[1] = longitude;
rc_gpsgoal[2] = altitude;
}
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
@ -773,15 +789,30 @@ int buzzuav_update_flight_status(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_pushf(vm, rc_gpsgoal[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, rc_goto_pos[1]);
buzzvm_pushf(vm, rc_gpsgoal[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, rc_goto_pos[2]);
buzzvm_pushf(vm, rc_gpsgoal[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "cur_goal", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushf(vm, goto_gpsgoal[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, goto_gpsgoal[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, goto_gpsgoal[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;

11
src/roscontroller.cpp
View File

@ -762,7 +762,10 @@ script
Arm();
// Registering HOME POINT.
home = cur_pos;
BClpose = true;
// Initialize GPS goal for safety.
double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude};
buzzuav_closures::set_gpsgoal(gpspgoal);
BClpose = true;
}
if (current_mode != "GUIDED" && setmode)
SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo
@ -792,7 +795,7 @@ script
{
armstate = 0;
Arm();
BClpose = false;
BClpose = false;
}
if (mav_client.call(cmd_srv))
{
@ -944,7 +947,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
/*
/ Get GPS from NED and a reference GPS point (struct input)
/ Get NED coordinates from a reference GPS point (struct input)
----------------------------------------------------------- */
{
gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude);
@ -953,7 +956,7 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat)
/*
/ Get GPS from NED and a reference GPS point
/ Get NED coordinates from a reference GPS point and current GPS location
----------------------------------------------------------- */
{
double d_lon = gps_t_lon - gps_r_lon;