Code clean and readme file commit
This commit is contained in:
parent
a9b1a7598a
commit
5e9b6df1ad
164
CMakeLists.txt
164
CMakeLists.txt
|
@ -1,11 +1,6 @@
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
project(rosbuzz)
|
project(rosbuzz)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## Find catkin macros and libraries
|
## Find catkin macros and libraries
|
||||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
|
||||||
## is used, also find other catkin packages
|
|
||||||
find_package(catkin REQUIRED COMPONENTS
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
roscpp
|
roscpp
|
||||||
std_msgs
|
std_msgs
|
||||||
|
@ -13,186 +8,33 @@ find_package(catkin REQUIRED COMPONENTS
|
||||||
sensor_msgs
|
sensor_msgs
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
## System dependencies are found with CMake's conventions
|
|
||||||
# find_package(Boost REQUIRED COMPONENTS system)
|
|
||||||
|
|
||||||
|
|
||||||
################################################
|
|
||||||
## Declare ROS messages, services and actions ##
|
|
||||||
################################################
|
|
||||||
|
|
||||||
## To declare and build messages, services or actions from within this
|
|
||||||
## package, follow these steps:
|
|
||||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
|
||||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
|
||||||
## * In the file package.xml:
|
|
||||||
## * add a build_depend tag for "message_generation"
|
|
||||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
|
||||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
|
||||||
## but can be declared for certainty nonetheless:
|
|
||||||
## * add a run_depend tag for "message_runtime"
|
|
||||||
## * In this file (CMakeLists.txt):
|
|
||||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
|
||||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
|
||||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
|
||||||
## catkin_package(CATKIN_DEPENDS ...)
|
|
||||||
## * uncomment the add_*_files sections below as needed
|
|
||||||
## and list every .msg/.srv/.action file to be processed
|
|
||||||
## * uncomment the generate_messages entry below
|
|
||||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
|
||||||
|
|
||||||
## Generate messages in the 'msg' folder
|
|
||||||
# add_message_files(
|
|
||||||
# DIRECTORY msg
|
|
||||||
# FILES
|
|
||||||
# neighbour_pos.msg
|
|
||||||
# Message2.msg
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Generate services in the 'srv' folder
|
|
||||||
# add_service_files(
|
|
||||||
# FILES
|
|
||||||
# Service1.srv
|
|
||||||
# Service2.srv
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Generate actions in the 'action' folder
|
|
||||||
# add_action_files(
|
|
||||||
# FILES
|
|
||||||
# Action1.action
|
|
||||||
# Action2.action
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Generate added messages and services with any dependencies listed here
|
|
||||||
# generate_messages(
|
|
||||||
# DEPENDENCIES
|
|
||||||
# sensor_msgs
|
|
||||||
# )
|
|
||||||
|
|
||||||
################################################
|
|
||||||
## Declare ROS dynamic reconfigure parameters ##
|
|
||||||
################################################
|
|
||||||
|
|
||||||
## To declare and build dynamic reconfigure parameters within this
|
|
||||||
## package, follow these steps:
|
|
||||||
## * In the file package.xml:
|
|
||||||
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
|
|
||||||
## * In this file (CMakeLists.txt):
|
|
||||||
## * add "dynamic_reconfigure" to
|
|
||||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
|
||||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
|
||||||
## and list every .cfg file to be processed
|
|
||||||
|
|
||||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
|
||||||
# generate_dynamic_reconfigure_options(
|
|
||||||
# cfg/DynReconf1.cfg
|
|
||||||
# cfg/DynReconf2.cfg
|
|
||||||
# )
|
|
||||||
|
|
||||||
###################################
|
###################################
|
||||||
## catkin specific configuration ##
|
## catkin specific configuration ##
|
||||||
###################################
|
###################################
|
||||||
## The catkin_package macro generates cmake config files for your package
|
|
||||||
## Declare things to be passed to dependent projects
|
|
||||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
|
||||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
|
||||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
|
||||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
|
||||||
catkin_package(
|
catkin_package(
|
||||||
#CATKIN_DEPENDS message_runtime
|
#CATKIN_DEPENDS message_runtime
|
||||||
# INCLUDE_DIRS include
|
|
||||||
# LIBRARIES buzzros
|
|
||||||
# CATKIN_DEPENDS Buzz roscpp st_msgs
|
|
||||||
# DEPENDS system_lib
|
|
||||||
)
|
)
|
||||||
|
|
||||||
###########
|
###########
|
||||||
## Build ##
|
## Build ##
|
||||||
###########
|
###########
|
||||||
|
|
||||||
## Specify additional locations of header files
|
|
||||||
## Your package locations should be listed before other locations
|
|
||||||
# include_directories(include)
|
|
||||||
include_directories(
|
include_directories(
|
||||||
${catkin_INCLUDE_DIRS}
|
${catkin_INCLUDE_DIRS}
|
||||||
)
|
)
|
||||||
|
|
||||||
## Declare a C++ library
|
# Executables
|
||||||
# add_library(buzzros
|
|
||||||
# src/${PROJECT_NAME}/buzzros.cpp
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Add cmake target dependencies of the library
|
|
||||||
## as an example, code may need to be generated before libraries
|
|
||||||
## either from message generation or dynamic reconfigure
|
|
||||||
# add_dependencies(buzzros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
## Declare a C++ executable
|
|
||||||
# add_executable(buzzros_node src/buzzros_node.cpp)
|
|
||||||
|
|
||||||
## Add cmake target dependencies of the executable
|
|
||||||
## same as for the library above
|
|
||||||
# add_dependencies(buzzros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
## Specify libraries to link a library or executable target against
|
|
||||||
# target_link_libraries(buzzros_node
|
|
||||||
# ${catkin_LIBRARIES}
|
|
||||||
# )
|
|
||||||
|
|
||||||
add_executable(rosbuzz_node src/rosbuzz.cpp
|
add_executable(rosbuzz_node src/rosbuzz.cpp
|
||||||
src/buzz_utility.cpp
|
src/buzz_utility.cpp
|
||||||
src/buzzuav_closures.cpp
|
src/buzzuav_closures.cpp
|
||||||
src/uav_utility.cpp)
|
src/uav_utility.cpp)
|
||||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
||||||
#add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
|
|
||||||
#############
|
|
||||||
## Install ##
|
|
||||||
#############
|
|
||||||
|
|
||||||
# all install targets should use catkin DESTINATION variables
|
# Executables and libraries for installation to do
|
||||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
|
||||||
|
|
||||||
## Mark executable scripts (Python etc.) for installation
|
|
||||||
## in contrast to setup.py, you can choose the destination
|
|
||||||
# install(PROGRAMS
|
|
||||||
# scripts/my_python_script
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark executables and/or libraries for installation
|
|
||||||
install(TARGETS rosbuzz_node
|
install(TARGETS rosbuzz_node
|
||||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||||
)
|
)
|
||||||
|
|
||||||
## Mark cpp header files for installation
|
|
||||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
|
||||||
# FILES_MATCHING PATTERN "*.h"
|
|
||||||
# PATTERN ".svn" EXCLUDE
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
|
||||||
# install(FILES
|
|
||||||
# # myfile1
|
|
||||||
# # myfile2
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
#############
|
|
||||||
## Testing ##
|
|
||||||
#############
|
|
||||||
|
|
||||||
## Add gtest based cpp test target and link libraries
|
|
||||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_buzzros.cpp)
|
|
||||||
# if(TARGET ${PROJECT_NAME}-test)
|
|
||||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
|
||||||
# endif()
|
|
||||||
|
|
||||||
## Add folders to be run by python nosetests
|
|
||||||
# catkin_add_nosetests(test)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1,19 +1,19 @@
|
||||||
<?xml version="1.0"?>
|
<?xml version="1.0"?>
|
||||||
<package>
|
<package>
|
||||||
<name>rosbuzz</name>
|
<name>rosbuzz</name>
|
||||||
<version>0.0.0</version>
|
<version>0.1.0</version>
|
||||||
<description>The rosbuzz package</description>
|
<description>The rosbuzz package</description>
|
||||||
|
|
||||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||||
<!-- Example: -->
|
<!-- Example: -->
|
||||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||||
<maintainer email="vivek@todo.todo">vivek</maintainer>
|
<maintainer email="vivek-shankar.varadharajan@polymtl.ca">vivek</maintainer>
|
||||||
|
|
||||||
|
|
||||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||||
<!-- Commonly used license strings: -->
|
<!-- Commonly used license strings: -->
|
||||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||||
<license>TODO</license>
|
<license>MIT</license>
|
||||||
|
|
||||||
|
|
||||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||||
|
@ -25,7 +25,7 @@
|
||||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||||
<!-- Authors do not have to be maintianers, but could be -->
|
<!-- Authors do not have to be maintianers, but could be -->
|
||||||
<!-- Example: -->
|
<!-- Example: -->
|
||||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
<author email="vivek-shankar.varadharajan@polymtl.ca">vivek</author>
|
||||||
|
|
||||||
|
|
||||||
<!-- The *_depend tags are used to specify dependencies -->
|
<!-- The *_depend tags are used to specify dependencies -->
|
||||||
|
|
|
@ -0,0 +1,85 @@
|
||||||
|
ROS Implemenation of Buzz
|
||||||
|
=========================
|
||||||
|
|
||||||
|
What is Buzz?
|
||||||
|
=============
|
||||||
|
|
||||||
|
Buzz is a novel programming language for heterogeneous robots swarms.
|
||||||
|
|
||||||
|
Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion.
|
||||||
|
|
||||||
|
Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations.
|
||||||
|
|
||||||
|
Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm.
|
||||||
|
|
||||||
|
Self-organization results from the fact that the Buzz run-time platform is purely distributed.
|
||||||
|
|
||||||
|
The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS.
|
||||||
|
|
||||||
|
More information is available at http://the.swarming.buzz/wiki/doku.php?id=start.
|
||||||
|
|
||||||
|
Downloading ROS Package
|
||||||
|
=======================
|
||||||
|
|
||||||
|
$ git clone
|
||||||
|
|
||||||
|
Requirements
|
||||||
|
------------
|
||||||
|
* Buzz : You can download the development sources through git:
|
||||||
|
|
||||||
|
$ git clone https://github.com/MISTLab/Buzz.git buzz
|
||||||
|
|
||||||
|
* ROS binary distribution (Indigo or higher) with catkin
|
||||||
|
|
||||||
|
You need the following package:
|
||||||
|
|
||||||
|
* mavros_msgs : You can install using apt-get:
|
||||||
|
|
||||||
|
$ sudo apt-get install ros-"distro"-mavros ros-"distro"-mavros-extras
|
||||||
|
|
||||||
|
Compilation
|
||||||
|
-----------
|
||||||
|
|
||||||
|
To compile the ros package, execute the following:
|
||||||
|
|
||||||
|
$ cd catkin_ws
|
||||||
|
$ catkin_make
|
||||||
|
$ source devel/setup.bash
|
||||||
|
|
||||||
|
Run
|
||||||
|
===
|
||||||
|
|
||||||
|
$ rosrun rosbuzz rosbuzz_node
|
||||||
|
|
||||||
|
Publisher
|
||||||
|
----------
|
||||||
|
|
||||||
|
* Messages from Buzz (BVM):
|
||||||
|
The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink".
|
||||||
|
|
||||||
|
Subscribers
|
||||||
|
-----------
|
||||||
|
|
||||||
|
* Current position of the Robot:
|
||||||
|
The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos".
|
||||||
|
|
||||||
|
* Messages to Buzz (BVM):
|
||||||
|
The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink".
|
||||||
|
|
||||||
|
* Battery status:
|
||||||
|
The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
|
||||||
|
|
||||||
|
Service
|
||||||
|
-------
|
||||||
|
|
||||||
|
* Remote Controller command:
|
||||||
|
The package offers service using mavros_msgs/CommandInt service with name "djicmd_rc".
|
||||||
|
|
||||||
|
Client
|
||||||
|
------
|
||||||
|
|
||||||
|
* Flight controller client:
|
||||||
|
This package is a client of mavros_msgs/CommandInt service with name "djicmd".
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1,23 +1,19 @@
|
||||||
|
/** @file buzz_utility.cpp
|
||||||
|
* @version 1.0
|
||||||
|
* @date 27.09.2016
|
||||||
|
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||||
|
* @author Vivek Shankar Varadharajan
|
||||||
|
* @copyright 2016 MistLab. All rights reserved.
|
||||||
|
*/
|
||||||
#define _GNU_SOURCE
|
#define _GNU_SOURCE
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
#include "buzz_utility.h"
|
#include "buzz_utility.h"
|
||||||
#include "buzzuav_closures.h"
|
#include "buzzuav_closures.h"
|
||||||
#include <buzz/buzzdebug.h>
|
#include <buzz/buzzdebug.h>
|
||||||
|
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <netdb.h>
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <pthread.h>
|
|
||||||
#include <vector>
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <bitset>
|
|
||||||
#include <map>
|
#include <map>
|
||||||
using namespace std;
|
using namespace std;
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
@ -33,10 +29,8 @@ static int TCP_COMM_STREAM = -1;
|
||||||
static uint8_t* STREAM_SEND_BUF = NULL;
|
static uint8_t* STREAM_SEND_BUF = NULL;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*adds neighbours position*/
|
||||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
|
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
|
||||||
/* Reset neighbor information */
|
/* Reset neighbor information */
|
||||||
buzzneighbors_reset(VM);
|
buzzneighbors_reset(VM);
|
||||||
|
@ -49,10 +43,10 @@ map< int, Pos_struct >::iterator it;
|
||||||
(it->second).y,
|
(it->second).y,
|
||||||
(it->second).z);
|
(it->second).z);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
/***************************************/
|
||||||
|
/*Reinterprets uint64_t into 4 uint16_t*/
|
||||||
|
/***************************************/
|
||||||
uint16_t* u64_cvt_u16(uint64_t u64){
|
uint16_t* u64_cvt_u16(uint64_t u64){
|
||||||
uint16_t* out = new uint16_t[4];
|
uint16_t* out = new uint16_t[4];
|
||||||
uint32_t int32_1 = u64 & 0xFFFFFFFF;
|
uint32_t int32_1 = u64 & 0xFFFFFFFF;
|
||||||
|
@ -63,9 +57,12 @@ uint16_t* u64_cvt_u16(uint64_t u64){
|
||||||
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
|
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
|
||||||
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
|
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
|
||||||
return out;
|
return out;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/***************************************************/
|
||||||
|
/*Appends obtained messages to buzz in message Queue*/
|
||||||
|
/***************************************************/
|
||||||
|
|
||||||
void in_msg_process(uint64_t* payload){
|
void in_msg_process(uint64_t* payload){
|
||||||
|
|
||||||
/* Go through messages and add them to the FIFO */
|
/* Go through messages and add them to the FIFO */
|
||||||
|
@ -96,6 +93,9 @@ void in_msg_process(uint64_t* payload){
|
||||||
/* Process messages */
|
/* Process messages */
|
||||||
buzzvm_process_inmsgs(VM);
|
buzzvm_process_inmsgs(VM);
|
||||||
}
|
}
|
||||||
|
/***************************************************/
|
||||||
|
/*Obtains messages from buzz out message Queue*/
|
||||||
|
/***************************************************/
|
||||||
|
|
||||||
uint64_t* out_msg_process(){
|
uint64_t* out_msg_process(){
|
||||||
buzzvm_process_outmsgs(VM);
|
buzzvm_process_outmsgs(VM);
|
||||||
|
@ -139,12 +139,14 @@ uint64_t* out_msg_process(){
|
||||||
uint64_t* payload_64 = new uint64_t[total_size];
|
uint64_t* payload_64 = new uint64_t[total_size];
|
||||||
|
|
||||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||||
|
/*for(int i=0;i<total_size;i++){
|
||||||
|
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
|
||||||
|
}*/
|
||||||
/* Send message */
|
/* Send message */
|
||||||
return payload_64;
|
return payload_64;
|
||||||
}
|
}
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*Buzz script not able to load*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
static const char* buzz_error_info() {
|
static const char* buzz_error_info() {
|
||||||
|
@ -170,6 +172,7 @@ static const char* buzz_error_info() {
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*Buzz hooks that can be used inside .bzz file*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
static int buzz_register_hooks() {
|
static int buzz_register_hooks() {
|
||||||
|
@ -192,7 +195,7 @@ static int buzz_register_hooks() {
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*Sets the .bzz and .bdbg file*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
int buzz_script_set(const char* bo_filename,
|
int buzz_script_set(const char* bo_filename,
|
||||||
|
@ -202,7 +205,7 @@ int buzz_script_set(const char* bo_filename,
|
||||||
gethostname(hstnm, 30);
|
gethostname(hstnm, 30);
|
||||||
/* Make numeric id from hostname */
|
/* Make numeric id from hostname */
|
||||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||||
int id =0; // strtol(hstnm + 1, NULL, 10);
|
int id = strtol(hstnm + 1, NULL, 10);
|
||||||
cout << " Robot ID" << id<< endl;
|
cout << " Robot ID" << id<< endl;
|
||||||
/* Reset the Buzz VM */
|
/* Reset the Buzz VM */
|
||||||
if(VM) buzzvm_destroy(&VM);
|
if(VM) buzzvm_destroy(&VM);
|
||||||
|
@ -260,6 +263,7 @@ int buzz_script_set(const char* bo_filename,
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*Swarm struct*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
struct buzzswarm_elem_s {
|
struct buzzswarm_elem_s {
|
||||||
|
@ -297,11 +301,9 @@ void check_swarm_members(const void* key, void* data, void* params) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
/*Step through the buzz script*/
|
||||||
|
|
||||||
void buzz_script_step() {
|
void buzz_script_step() {
|
||||||
|
|
||||||
|
|
||||||
//buzzvm_process_inmsgs(VM);
|
|
||||||
/*
|
/*
|
||||||
* Update sensors
|
* Update sensors
|
||||||
*/
|
*/
|
||||||
|
@ -330,13 +332,10 @@ void buzz_script_step() {
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*Destroy the bvm and other resorces*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
void buzz_script_destroy() {
|
void buzz_script_destroy() {
|
||||||
|
|
||||||
/* Get rid of stream buffer */
|
|
||||||
//free(STREAM_SEND_BUF);
|
|
||||||
/* Get rid of virtual machine */
|
|
||||||
if(VM) {
|
if(VM) {
|
||||||
if(VM->state != BUZZVM_STATE_READY) {
|
if(VM->state != BUZZVM_STATE_READY) {
|
||||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||||
|
@ -353,6 +352,7 @@ void buzz_script_destroy() {
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*Execution completed*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
int buzz_script_done() {
|
int buzz_script_done() {
|
||||||
|
|
|
@ -1,6 +1,5 @@
|
||||||
#ifndef BUZZ_UTILITY_H
|
#ifndef BUZZ_UTILITY_H
|
||||||
#define BUZZ_UTILITY_H
|
#define BUZZ_UTILITY_H
|
||||||
#include <vector>
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <map>
|
#include <map>
|
||||||
struct pos_struct
|
struct pos_struct
|
||||||
|
@ -13,19 +12,21 @@ typedef struct pos_struct Pos_struct ;
|
||||||
|
|
||||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||||
|
|
||||||
extern int buzz_listen(const char* type,
|
int buzz_listen(const char* type,
|
||||||
int msg_size);
|
int msg_size);
|
||||||
|
|
||||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
||||||
|
|
||||||
void in_msg_process(uint64_t* payload);
|
void in_msg_process(uint64_t* payload);
|
||||||
|
|
||||||
uint64_t* out_msg_process();
|
uint64_t* out_msg_process();
|
||||||
|
|
||||||
extern int buzz_script_set(const char* bo_filename,
|
int buzz_script_set(const char* bo_filename,
|
||||||
const char* bdbg_filename);
|
const char* bdbg_filename);
|
||||||
extern void buzz_script_step();
|
void buzz_script_step();
|
||||||
|
|
||||||
extern void buzz_script_destroy();
|
void buzz_script_destroy();
|
||||||
|
|
||||||
extern int buzz_script_done();
|
int buzz_script_done();
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -1,4 +1,11 @@
|
||||||
#define _GNU_SOURCE
|
/** @file buzzuav_closures.cpp
|
||||||
|
* @version 1.0
|
||||||
|
* @date 27.09.2016
|
||||||
|
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||||
|
* @author Vivek Shankar Varadharajan
|
||||||
|
* @copyright 2016 MistLab. All rights reserved.
|
||||||
|
*/
|
||||||
|
//#define _GNU_SOURCE
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include "buzzuav_closures.h"
|
#include "buzzuav_closures.h"
|
||||||
#include "uav_utility.h"
|
#include "uav_utility.h"
|
||||||
|
@ -56,16 +63,17 @@ int buzzros_print(buzzvm_t vm) {
|
||||||
|
|
||||||
int buzzuav_goto(buzzvm_t vm) {
|
int buzzuav_goto(buzzvm_t vm) {
|
||||||
buzzvm_lnum_assert(vm, 3);
|
buzzvm_lnum_assert(vm, 3);
|
||||||
buzzvm_lload(vm, 1); /* Lattitude */
|
buzzvm_lload(vm, 1); /* Altitude */
|
||||||
buzzvm_lload(vm, 2); /* Longitude */
|
buzzvm_lload(vm, 2); /* Longitude */
|
||||||
buzzvm_lload(vm, 3); /* Altitude */
|
buzzvm_lload(vm, 3); /* Latitude */
|
||||||
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||||
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value * 10.0f;
|
goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
|
||||||
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f;
|
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value;
|
||||||
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value * 10.0f;
|
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
|
||||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||||
|
printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
|
||||||
return buzzvm_ret0(vm);
|
return buzzvm_ret0(vm);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -95,16 +103,19 @@ cur_cmd=rc_cmd;
|
||||||
|
|
||||||
int buzzuav_takeoff(buzzvm_t vm) {
|
int buzzuav_takeoff(buzzvm_t vm) {
|
||||||
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||||
|
printf(" Buzz requested Take off !!! \n");
|
||||||
return buzzvm_ret0(vm);
|
return buzzvm_ret0(vm);
|
||||||
}
|
}
|
||||||
|
|
||||||
int buzzuav_land(buzzvm_t vm) {
|
int buzzuav_land(buzzvm_t vm) {
|
||||||
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||||
|
printf(" Buzz requested Land !!! \n");
|
||||||
return buzzvm_ret0(vm);
|
return buzzvm_ret0(vm);
|
||||||
}
|
}
|
||||||
|
|
||||||
int buzzuav_gohome(buzzvm_t vm) {
|
int buzzuav_gohome(buzzvm_t vm) {
|
||||||
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
|
||||||
|
printf(" Buzz requested gohome !!! \n");
|
||||||
return buzzvm_ret0(vm);
|
return buzzvm_ret0(vm);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -137,10 +148,11 @@ int buzzuav_update_battery(buzzvm_t vm) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*To do !!!!!*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
int buzzuav_update_prox(buzzvm_t vm) {
|
int buzzuav_update_prox(buzzvm_t vm) {
|
||||||
static char PROXIMITY_BUF[256];
|
/* static char PROXIMITY_BUF[256];
|
||||||
int i;
|
int i;
|
||||||
//kh4_proximity_ir(PROXIMITY_BUF, DSPIC);
|
//kh4_proximity_ir(PROXIMITY_BUF, DSPIC);
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
|
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
|
||||||
|
@ -161,7 +173,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
|
||||||
buzzvm_tput(vm);
|
buzzvm_tput(vm);
|
||||||
}
|
}
|
||||||
buzzvm_gstore(vm);
|
buzzvm_gstore(vm);
|
||||||
return vm->state;
|
return vm->state;*/
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
|
@ -17,15 +17,13 @@ int buzzros_print(buzzvm_t vm);
|
||||||
int buzzuav_goto(buzzvm_t vm);
|
int buzzuav_goto(buzzvm_t vm);
|
||||||
/* Returns the current command from local variable*/
|
/* Returns the current command from local variable*/
|
||||||
int getcmd();
|
int getcmd();
|
||||||
|
/*Sets goto position could be used for bypassing*/
|
||||||
void set_goto(double pos[]);
|
void set_goto(double pos[]);
|
||||||
|
/*sets rc requested command */
|
||||||
void rc_call(int rc_cmd);
|
void rc_call(int rc_cmd);
|
||||||
|
/* sets the battery state */
|
||||||
/* sets the battery state to the local variable
|
|
||||||
*/
|
|
||||||
void set_battery(float voltage,float current,float remaining);
|
void set_battery(float voltage,float current,float remaining);
|
||||||
/*retuns the current go to position from local variable*/
|
/*retuns the current go to position */
|
||||||
double* getgoto();
|
double* getgoto();
|
||||||
/*
|
/*
|
||||||
* Commands the UAV to takeoff
|
* Commands the UAV to takeoff
|
||||||
|
@ -47,7 +45,7 @@ int buzzuav_update_battery(buzzvm_t vm);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Updates IR information in Buzz
|
* Updates IR information in Buzz
|
||||||
* Proximity and ground sensors
|
* Proximity and ground sensors to do !!!!
|
||||||
*/
|
*/
|
||||||
int buzzuav_update_prox(buzzvm_t vm);
|
int buzzuav_update_prox(buzzvm_t vm);
|
||||||
|
|
||||||
|
|
165
src/out.basm
165
src/out.basm
|
@ -1,13 +1,11 @@
|
||||||
!23
|
!24
|
||||||
'init
|
'init
|
||||||
'i
|
'i
|
||||||
'v
|
|
||||||
'stigmergy
|
|
||||||
'create
|
|
||||||
'step
|
|
||||||
's
|
's
|
||||||
'swarm
|
'swarm
|
||||||
|
'create
|
||||||
'join
|
'join
|
||||||
|
'step
|
||||||
'neighbors
|
'neighbors
|
||||||
'foreach
|
'foreach
|
||||||
'print
|
'print
|
||||||
|
@ -20,20 +18,23 @@
|
||||||
'azimuth
|
'azimuth
|
||||||
'elevation =
|
'elevation =
|
||||||
'elevation
|
'elevation
|
||||||
|
'exec
|
||||||
|
'uav_takeoff
|
||||||
|
'uav_goto
|
||||||
'reset
|
'reset
|
||||||
'destroy
|
'destroy
|
||||||
|
|
||||||
pushs 0
|
pushs 0
|
||||||
pushcn @__label_1
|
pushcn @__label_1
|
||||||
gstore
|
gstore
|
||||||
pushs 5
|
pushs 6
|
||||||
pushcn @__label_2
|
pushcn @__label_2
|
||||||
gstore
|
gstore
|
||||||
pushs 21
|
|
||||||
pushcn @__label_4
|
|
||||||
gstore
|
|
||||||
pushs 22
|
pushs 22
|
||||||
pushcn @__label_5
|
pushcn @__label_8
|
||||||
|
gstore
|
||||||
|
pushs 23
|
||||||
|
pushcn @__label_9
|
||||||
gstore
|
gstore
|
||||||
nop
|
nop
|
||||||
|
|
||||||
|
@ -47,67 +48,103 @@
|
||||||
pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 3 |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 3 |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gload |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gload |4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 4 |4,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 4 |4,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
tget |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
tget |4,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushi 1 |4,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 10 |4,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushi 1 |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 1 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
callc |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
callc |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gstore |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gstore |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
ret0 |6,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 2 |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
gload |5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 5 |5,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
tget |5,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushi 0 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
callc |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
ret0 |7,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
|
||||||
@__label_2
|
@__label_2
|
||||||
pushs 6 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 7 |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 7 |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gload |12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gload |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 8 |12,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 4 |10,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
tget |12,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
tget |10,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushl @__label_3 |13,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushi 10 |10,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 1 |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushi 1 |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
callc |17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
callc |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 1 |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gstore |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gload |18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 6 |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 1 |18,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gload |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
eq |18,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 8 |11,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
jumpz @__label_4 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
tget |11,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 2 |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushi 0 |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gload |19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
callc |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 19 |19,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 9 |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
tget |19,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gload |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushl @__label_6 |19,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 10 |13,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 1 |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
tget |13,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
callc |19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushl @__label_3 |14,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 1 |20,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushi 1 |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 0 |20,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
callc |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gstore |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
ret0 |30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
jump @__label_5 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
@__label_4 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
gload |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 19 |23,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
tget |23,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushl @__label_7 |23,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushi 1 |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
callc |23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 1 |24,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushi 1 |24,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
gstore |24,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
@__label_5 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
ret0 |35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
|
||||||
@__label_3
|
@__label_3
|
||||||
pushs 11 |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 9 |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
gload |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
gload |14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 12 |15,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 10 |14,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
lload 1 |15,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
lload 1 |14,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 13 |15,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 11 |14,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 14 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 12 |15,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
lload 2 |15,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 13 |15,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
tget |15,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 14 |15,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushs 15 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 15 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 16 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
tget |16,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
tget |16,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 16 |16,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 14 |16,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
tget |17,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
tget |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 16 |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushi 11 |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 19 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
callc |17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
lload 2 |18,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
ret0 |17,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
pushs 20 |18,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
tget |18,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
pushi 11 |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
callc |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
ret0 |18,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
|
||||||
|
|
||||||
@__label_4
|
@__label_6
|
||||||
ret0 |34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 20 |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
gload |19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushi 0 |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
callc |19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
ret0 |19,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
|
||||||
@__label_5
|
@__label_7
|
||||||
ret0 |38,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
pushs 21 |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
gload |23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushf 1.1234 |23,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushf 2.2345 |23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushf 3.3456 |23,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
pushi 3 |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
callc |23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
ret0 |23,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
|
||||||
|
@__label_8
|
||||||
|
ret0 |39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
|
||||||
|
@__label_9
|
||||||
|
ret0 |43,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||||
|
|
BIN
src/out.bdbg
BIN
src/out.bdbg
Binary file not shown.
BIN
src/out.bo
BIN
src/out.bo
Binary file not shown.
|
@ -1,7 +1,8 @@
|
||||||
/** @file rosbuzz.cpp
|
/** @file rosbuzz.cpp
|
||||||
* @version 1
|
* @version 1.0
|
||||||
* @date 27.09.2016
|
* @date 27.09.2016
|
||||||
* @brief Buzz Implementation as a node in ROS.
|
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||||
|
* @author Vivek Shankar Varadharajan
|
||||||
* @copyright 2016 MistLab. All rights reserved.
|
* @copyright 2016 MistLab. All rights reserved.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
@ -27,21 +28,28 @@
|
||||||
#include <map>
|
#include <map>
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This program implements Buzz node in ros using mavros_msgs, Developed for Dji M100.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
static int done = 0;
|
static int done = 0;
|
||||||
static double cur_pos[3];
|
static double cur_pos[3];
|
||||||
static uint64_t payload;
|
static uint64_t payload;
|
||||||
static std::map< int, Pos_struct> neighbours_pos_map;
|
static std::map< int, Pos_struct> neighbours_pos_map;
|
||||||
static int timer_step=0;
|
static int timer_step=0;
|
||||||
/**
|
|
||||||
* This program implements Buzz node in ros.
|
/*Refresh neighbours Position for every ten step*/
|
||||||
*/
|
|
||||||
void maintain_pos(int tim_step){
|
void maintain_pos(int tim_step){
|
||||||
if(timer_step >=10){
|
if(timer_step >=10){
|
||||||
neighbours_pos_map.clear();
|
neighbours_pos_map.clear();
|
||||||
timer_step=0;
|
timer_step=0;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*Maintain neighbours position*/
|
||||||
void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
|
void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
|
||||||
map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
||||||
if(it!=neighbours_pos_map.end())
|
if(it!=neighbours_pos_map.end())
|
||||||
|
@ -49,22 +57,15 @@ neighbours_pos_map.erase(it);
|
||||||
neighbours_pos_map.insert(make_pair(id, pos_arr));
|
neighbours_pos_map.insert(make_pair(id, pos_arr));
|
||||||
}
|
}
|
||||||
|
|
||||||
/*usage*/
|
/*print usage information not needed at the moment*/
|
||||||
void usage(const char* path, int status) {
|
void usage(const char* path, int status) {
|
||||||
fprintf(stderr, "Usage:\n");
|
|
||||||
fprintf(stderr, "\t%s <stream> <msg_size> <file.bo> <file.bdb>\n\n", path);
|
|
||||||
fprintf(stderr, "== Options ==\n\n");
|
|
||||||
fprintf(stderr, " stream The stream type: tcp or bt\n");
|
|
||||||
fprintf(stderr, " msg_size The message size in bytes\n");
|
|
||||||
fprintf(stderr, " file.bo The Buzz bytecode file\n");
|
|
||||||
fprintf(stderr, " file.bdbg The Buzz debug file\n\n");
|
|
||||||
exit(status);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*Set the current position of the robot callback*/
|
/*Set the current position of the robot callback*/
|
||||||
void set_cur_pos(double latitude,
|
void set_cur_pos(double latitude,
|
||||||
double longitude,
|
double longitude,
|
||||||
double altitude){
|
double altitude){
|
||||||
/* set the current position of the robot*/
|
|
||||||
cur_pos [0] =latitude;
|
cur_pos [0] =latitude;
|
||||||
cur_pos [1] =longitude;
|
cur_pos [1] =longitude;
|
||||||
cur_pos [2] =altitude;
|
cur_pos [2] =altitude;
|
||||||
|
@ -73,8 +74,8 @@ cur_pos [2] =altitude;
|
||||||
|
|
||||||
/*convert from catresian to spherical coordinate system callback */
|
/*convert from catresian to spherical coordinate system callback */
|
||||||
double* cvt_spherical_coordinates(double neighbours_pos_payload []){
|
double* cvt_spherical_coordinates(double neighbours_pos_payload []){
|
||||||
|
|
||||||
double latitude,longitude,altitude;
|
double latitude,longitude,altitude;
|
||||||
//for(int i=0;i<pos_vect.size();i++){
|
|
||||||
latitude=neighbours_pos_payload[0];
|
latitude=neighbours_pos_payload[0];
|
||||||
longitude = neighbours_pos_payload[1];
|
longitude = neighbours_pos_payload[1];
|
||||||
altitude=neighbours_pos_payload[2];
|
altitude=neighbours_pos_payload[2];
|
||||||
|
@ -99,39 +100,35 @@ set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
||||||
/*payload callback callback*/
|
/*payload callback callback*/
|
||||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
||||||
{
|
{
|
||||||
uint64_t message_obt[(msg->payload64.end())-(msg->payload64.begin())];
|
uint64_t message_obt[msg->payload64.size()];
|
||||||
int i = 0;
|
int i = 0;
|
||||||
|
|
||||||
// print all the remaining numbers
|
/* Go throught the obtained payload*/
|
||||||
for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
|
for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
|
||||||
{
|
{
|
||||||
message_obt[i] =(uint64_t) *it;
|
message_obt[i] =(uint64_t) *it;
|
||||||
cout<<" obtaind message "<<message_obt[i]<<endl;
|
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
||||||
i++;
|
i++;
|
||||||
}
|
}
|
||||||
|
/* Extract neighbours position from payload*/
|
||||||
double neighbours_pos_payload[3];
|
double neighbours_pos_payload[3];
|
||||||
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
|
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
|
||||||
//cout<<"neighbours pos recived "<<" lat " << (double)neighbours_pos_payload[0]<<" long "<<(double)neighbours_pos_payload[1] <<" alti "<<(double)neighbours_pos_payload[2]<<endl;
|
/*Convert obtained position to relative position*/
|
||||||
|
|
||||||
for(i=0;i<3;i++){
|
for(i=0;i<3;i++){
|
||||||
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
|
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
|
||||||
}
|
}
|
||||||
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
|
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
|
||||||
//cout<<" neighbours pos cvt " <<" lat " << cvt_neighbours_pos_payload[0]<<" long "<<cvt_neighbours_pos_payload[1] <<" alti "<<cvt_neighbours_pos_payload[2]<<endl;
|
/*Extract robot id of the neighbour*/
|
||||||
|
|
||||||
uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));
|
uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||||
//cout << " values " <<out[0] <<" "<<(int)out[1] <<" "<<out[2] <<" "<<out[3] <<" "<<endl;
|
/*pass neighbour position to local maintaner*/
|
||||||
struct pos_struct n_pos;
|
Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
|
||||||
n_pos.x=cvt_neighbours_pos_payload[0];
|
|
||||||
n_pos.y=cvt_neighbours_pos_payload[1];
|
|
||||||
n_pos.z=cvt_neighbours_pos_payload[2];
|
|
||||||
neighbours_pos_maintain((int)out[1],n_pos);
|
neighbours_pos_maintain((int)out[1],n_pos);
|
||||||
delete[] out;
|
delete[] out;
|
||||||
in_msg_process((message_obt+3));
|
in_msg_process((message_obt+3));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*Dji RC commands service */
|
||||||
int oldcmdID=0;
|
int oldcmdID=0;
|
||||||
int rc_cmd;
|
int rc_cmd;
|
||||||
bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
bool rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||||
|
@ -227,9 +224,9 @@ int main(int argc, char **argv)
|
||||||
|
|
||||||
|
|
||||||
/* The bytecode filename */
|
/* The bytecode filename */
|
||||||
char* bcfname = "../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||||
/* The debugging information file name */
|
/* The debugging information file name */
|
||||||
char* dbgfname = "../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||||
/* Set CTRL-C handler */
|
/* Set CTRL-C handler */
|
||||||
signal(SIGTERM, ctrlc_handler);
|
signal(SIGTERM, ctrlc_handler);
|
||||||
signal(SIGINT, ctrlc_handler);
|
signal(SIGINT, ctrlc_handler);
|
||||||
|
@ -239,8 +236,9 @@ int main(int argc, char **argv)
|
||||||
if(buzz_script_set(bcfname, dbgfname)) {
|
if(buzz_script_set(bcfname, dbgfname)) {
|
||||||
fprintf(stdout, "Bytecode file found and set\n");
|
fprintf(stdout, "Bytecode file found and set\n");
|
||||||
|
|
||||||
|
/*Commands for dji flight controller*/
|
||||||
mavros_msgs::CommandInt cmd_srv;
|
mavros_msgs::CommandInt cmd_srv;
|
||||||
|
|
||||||
int count = 0;
|
int count = 0;
|
||||||
while (ros::ok() && !done && !buzz_script_done())
|
while (ros::ok() && !done && !buzz_script_done())
|
||||||
{
|
{
|
||||||
|
@ -262,19 +260,27 @@ int main(int argc, char **argv)
|
||||||
/* diji client call*/
|
/* diji client call*/
|
||||||
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
||||||
else{ ROS_ERROR("Failed to call service 'djicmd'"); }
|
else{ ROS_ERROR("Failed to call service 'djicmd'"); }
|
||||||
/*Prepare Pay load to be sent*/
|
/*obtain Pay load to be sent*/
|
||||||
uint64_t* payload_out_ptr= out_msg_process();
|
uint64_t* payload_out_ptr= out_msg_process();
|
||||||
uint64_t position[3];
|
uint64_t position[3];
|
||||||
|
/*Appened current position to message*/
|
||||||
memcpy(position, cur_pos, 3*sizeof(uint64_t));
|
memcpy(position, cur_pos, 3*sizeof(uint64_t));
|
||||||
mavros_msgs::Mavlink payload_out;
|
mavros_msgs::Mavlink payload_out;
|
||||||
payload_out.payload64.push_back(position[0]);
|
payload_out.payload64.push_back(position[0]);
|
||||||
payload_out.payload64.push_back(position[1]);
|
payload_out.payload64.push_back(position[1]);
|
||||||
payload_out.payload64.push_back(position[2]);
|
payload_out.payload64.push_back(position[2]);
|
||||||
|
/*Append Buzz message*/
|
||||||
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
|
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
|
||||||
for(int i=0;i<out[0];i++){
|
for(int i=0;i<out[0];i++){
|
||||||
payload_out.payload64.push_back(payload_out_ptr[i]);
|
payload_out.payload64.push_back(payload_out_ptr[i]);
|
||||||
}
|
}
|
||||||
|
int i=0;
|
||||||
|
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin(); it != payload_out.payload64.end(); ++it)
|
||||||
|
{
|
||||||
|
//message_obt[i] =(uint64_t) *it;
|
||||||
|
//cout<<" [Debug:] sent message "<<*it<<endl;
|
||||||
|
i++;
|
||||||
|
}
|
||||||
/*publish prepared messages in respective topic*/
|
/*publish prepared messages in respective topic*/
|
||||||
payload_pub.publish(payload_out);
|
payload_pub.publish(payload_out);
|
||||||
delete[] out;
|
delete[] out;
|
||||||
|
@ -286,12 +292,6 @@ int main(int argc, char **argv)
|
||||||
++count;
|
++count;
|
||||||
timer_step+=1;
|
timer_step+=1;
|
||||||
maintain_pos(timer_step);
|
maintain_pos(timer_step);
|
||||||
/* cout<<"time step"<<timer_step<<endl;
|
|
||||||
map< int, Pos_struct >::iterator it;
|
|
||||||
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
|
|
||||||
std::cout << it->first << " => " << (it->second).x <<" " << (it->second).y <<" " << (it->second).z<<endl;
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
|
|
||||||
}
|
}
|
||||||
/* Cleanup */
|
/* Cleanup */
|
||||||
|
|
23
src/test.bzz
23
src/test.bzz
|
@ -1,31 +1,36 @@
|
||||||
# Executed once at init time.
|
# Executed once at init time.
|
||||||
function init() {
|
function init() {
|
||||||
i=0
|
i=0
|
||||||
v = stigmergy.create(1)
|
s = swarm.create(10)
|
||||||
#v.put("a", 6)
|
s.join()
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
function step() {
|
function step() {
|
||||||
s = swarm.create(10)
|
# print all the neighbours position
|
||||||
s.join()
|
|
||||||
|
|
||||||
neighbors.foreach(
|
neighbors.foreach(
|
||||||
function(rid, data) {
|
function(rid, data) {
|
||||||
print("robot ", rid, ": ",
|
print("robot ", rid, ": ",
|
||||||
"distance = ", data.distance, ", ",
|
"distance = ", data.distance, ", ",
|
||||||
"azimuth = ", data.azimuth, ", ",
|
"azimuth = ", data.azimuth, ", ",
|
||||||
"elevation = ", data.elevation) })
|
"elevation = ", data.elevation) })
|
||||||
#if(i==1){
|
if(i==1){
|
||||||
|
s.exec(function() {uav_takeoff()})
|
||||||
|
i=0
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
|
||||||
|
i=1
|
||||||
|
}
|
||||||
|
#if(i==5){
|
||||||
#s.leave()
|
#s.leave()
|
||||||
#i=0
|
#i=0
|
||||||
#}
|
#}
|
||||||
#else{
|
#else{
|
||||||
#i=1
|
#i=1
|
||||||
#}
|
#}
|
||||||
#s.select(uav_takeoff)
|
#i = i + 1
|
||||||
#print("Distance to source:", mydist, neighbors.count())
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -4,50 +4,20 @@
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
//knet_dev_t* DSPIC;
|
|
||||||
static const int ACC_INC = 3;
|
|
||||||
static const int ACC_DIV = 0;
|
|
||||||
static const int MIN_SPEED_ACC = 20;
|
|
||||||
static const int MIN_SPEED_DEC = 1;
|
|
||||||
static const int MAX_SPEED = 400; /* mm/sec */
|
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*To do !*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
void uav_setup() {
|
void uav_setup() {
|
||||||
/* Set the libkhepera debug level */
|
|
||||||
/*kb_set_debug_level(2);
|
|
||||||
// initiate libkhepera and robot access
|
|
||||||
if(kh4_init(0, NULL) !=0)
|
|
||||||
{
|
|
||||||
printf("\nERROR: could not initialize the khepera!\n\n");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
/* open robot socket and store the handle in their respective pointers
|
|
||||||
DSPIC = knet_open("Khepera4:dsPic", KNET_BUS_I2C, 0, NULL);
|
|
||||||
/* Set speed profile
|
|
||||||
kh4_SetSpeedProfile(ACC_INC, /* Acceleration increment
|
|
||||||
ACC_DIV, /* Acceleration divider
|
|
||||||
MIN_SPEED_ACC, /* Minimum speed acc
|
|
||||||
MIN_SPEED_DEC, /* Minimum speed dec
|
|
||||||
MAX_SPEED, /* Maximum speed
|
|
||||||
DSPIC
|
|
||||||
);
|
|
||||||
kh4_SetMode(kh4RegSpeedProfile, DSPIC);
|
|
||||||
/* Mute ultrasonic sensor
|
|
||||||
kh4_activate_us(0, DSPIC);*/
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
/*To do !*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
void uav_done() {
|
void uav_done() {
|
||||||
/* /* Stop wheels
|
|
||||||
kh4_set_speed(0, 0, DSPIC);
|
|
||||||
/* Set motors to idle
|
|
||||||
kh4_SetMode(kh4RegIdle, DSPIC);
|
|
||||||
/* Clear rgb leds because they consume energy
|
|
||||||
kh4_SetRGBLeds(0, 0, 0, 0, 0, 0, 0, 0, 0, DSPIC); */
|
|
||||||
fprintf(stdout, "Robot stopped.\n");
|
fprintf(stdout, "Robot stopped.\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,12 +1,9 @@
|
||||||
#ifndef UAV_UTILITY_H
|
#ifndef UAV_UTILITY_H
|
||||||
#define UAV_UTILITY_H
|
#define UAV_UTILITY_H
|
||||||
|
|
||||||
//#include <khepera/khepera.h>
|
|
||||||
|
|
||||||
extern void uav_setup();
|
extern void uav_setup();
|
||||||
|
|
||||||
extern void uav_done();
|
extern void uav_done();
|
||||||
|
|
||||||
//extern knet_dev_t* DSPIC;
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue