merging dji_mav_mist
This commit is contained in:
dave
parent
b9c0dfb117
commit
74846c5df8
|
|
@ -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))
|
||||
|
||||
|
|
|
|||
|
|
@ -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,14 +12,33 @@ 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
|
||||
|
|
@ -56,17 +76,14 @@ function usertab_print(t) {
|
|||
|
||||
function stattab_print() {
|
||||
if(v_status.size()>0) {
|
||||
if(b_updating==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(tab!=nil)
|
||||
unpackstatus(tab)
|
||||
u=u+1
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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));
|
||||
|
|
|
|||
Loading…
Reference in New Issue