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

merging dji_mav_mist

This commit is contained in:
dave 2017-07-26 10:10:15 -04:00
parent b9c0dfb117
commit 74846c5df8
3 changed files with 56 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
buzz_scripts/graphform.bzz
View File

@ -6,6 +6,7 @@ include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
include "graphs/shapes_L.bzz"
@ -14,7 +15,7 @@ ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
# max velocity in cm/step
ROBOT_MAXVEL = 500.0
ROBOT_MAXVEL = 200.0
#
# Global variables
@ -808,6 +809,7 @@ function init() {
#
uav_initswarm()
v_tag = stigmergy.create(m_lockstig)
uav_initstig()
Reset()
TARGET_ALTITUDE = 2.5 + id * 1.5
}
@ -818,6 +820,7 @@ function init() {
function step(){
uav_rccmd()
uav_neicmd()
uav_updatestig()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
@ -846,8 +849,10 @@ function step(){
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
#navigation
if(id==0)
stattab_print()
#navigation
#broadcast messag
neighbors.broadcast("m",packmessage(m_selfMessage))

63
buzz_scripts/include/vstigenv.bzz
View File

@ -3,6 +3,7 @@ GROUND_VSTIG = 11
WAIT4STEP = 10
v_status = {}
v_ground = {}
b_updating = 0
function uav_initstig() {
v_status = stigmergy.create(STATUS_VSTIG)
@ -11,29 +12,48 @@ function uav_initstig() {
counter=WAIT4STEP
function uav_updatestig() {
# TODO: Push values on update only.
if(counter<=0) {
var ls={.g=0,.b=battery.capacity,.x=xbee_status.rssi,.f=flight.status}
b_updating=1
#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
log("Pushing ", ls, "on vstig with id:", id)
v_status.put(id, ls)
counter=WAIT4STEP
} else
} else {
b_updating=0
counter=counter-1
}
}
function unpackstatus(recv_value){
gps=(recv_value-recv_value%1000000)/1000000
recv_value=recv_value-gps*1000000
batt=(recv_value-recv_value%1000)/1000
recv_value=recv_value-batt*1000
xbee=(recv_value-recv_value%10)/10
recv_value=recv_value-xbee*10
fc=recv_value
log("- GPS ", gps)
log("- Battery ", batt)
log("- Xbee ", xbee)
log("- Status ", fc)
}
function checkusers() {
# Read a value from the structure
if(size(users)>0)
log("Got a user!")
# Read a value from the structure
if(size(users)>0)
log("Got a user!")
# log(users)
# users_print(users.dataG)
# if(size(users.dataG)>0)
# vt.put("p", users.dataG)
# log(users)
# users_print(users.dataG)
# if(size(users.dataG)>0)
# vt.put("p", users.dataG)
# Get the number of keys in the structure
#log("The vstig has ", vt.size(), " elements")
# users_save(vt.get("p"))
# table_print(users.dataL)
# Get the number of keys in the structure
# log("The vstig has ", vt.size(), " elements")
# users_save(vt.get("p"))
# table_print(users.dataL)
}
function users_save(t) {
@ -56,17 +76,14 @@ function usertab_print(t) {
function stattab_print() {
if(v_status.size()>0) {
u=0
while(u<8){
tab = v_status.get(u)
if(tab!=nil) {
log("id: ", u)
log("- GPS ", tab.g)
log("- Battery ", tab.b)
log("- Xbee ", tab.x)
log("- Status ", tab.f)
if(b_updating==0) {
u=0
while(u<8){
tab = v_status.get(u)
if(tab!=nil)
unpackstatus(tab)
u=u+1
}
u=u+1
}
}
}

16
src/buzzuav_closures.cpp
View File

@ -25,9 +25,9 @@ namespace buzzuav_closures{
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
static float rssi = 0.0;
static int rssi = 0;
static float raw_packet_loss = 0.0;
static float filtered_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
std::map< int, buzz_utility::RB_struct> targets_map;
@ -247,7 +247,7 @@ namespace buzzuav_closures{
}
/*---------------------------------------/
/ Buss closure for basic UAV commands
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 1);
@ -334,7 +334,7 @@ namespace buzzuav_closures{
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
buzzvm_pushf(vm, batt[2]);
buzzvm_pushi(vm, (int)batt[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
@ -347,7 +347,7 @@ namespace buzzuav_closures{
void set_rssi(float value)
{
rssi = value;
rssi = round(value);
}
void set_raw_packet_loss(float value)
@ -357,7 +357,7 @@ namespace buzzuav_closures{
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = value;
filtered_packet_loss = round(100*value);
}
void set_api_rssi(float value)
@ -374,7 +374,7 @@ namespace buzzuav_closures{
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
buzzvm_pushf(vm, rssi);
buzzvm_pushi(vm, rssi);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
@ -382,7 +382,7 @@ namespace buzzuav_closures{
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
buzzvm_pushf(vm, filtered_packet_loss);
buzzvm_pushi(vm, filtered_packet_loss);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));