addition of some additional buzz hooks

include/buzzuav_closures.h

@@ -28,8 +28,13 @@ void set_goto(double pos[]);
 void rc_call(int rc_cmd);
 /* sets the battery state */
 void set_battery(float voltage,float current,float remaining);
+/* sets current position */
+void set_currentpos(double latitude, double longitude, double altitude);
 /*retuns the current go to position */
 double* getgoto();
+/* updates flight status*/
+void flight_status_update(uint8_t state);
+
 /*
  * Commands the UAV to takeoff
  */
@@ -47,6 +52,14 @@ int buzzuav_gohome(buzzvm_t vm);
  * Updates battery information in Buzz
  */
 int buzzuav_update_battery(buzzvm_t vm);
+/*
+ * Updates current position in Buzz
+ */
+int buzzuav_update_currentpos(buzzvm_t vm);
+/*
+ * Updates flight status in Buzz
+ */
+int buzzuav_update_flight_status(buzzvm_t vm);
 
 /*
  * Updates IR information in Buzz

include/roscontroller.h

@@ -4,6 +4,7 @@
 #include "mavros_msgs/GlobalPositionTarget.h"
 #include "mavros_msgs/CommandCode.h"
 #include "mavros_msgs/CommandInt.h"
+#include "mavros_msgs/ExtendedState.h"
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
@@ -50,6 +51,7 @@ private:
 	ros::Subscriber current_position_sub;
 	ros::Subscriber battery_sub;
 	ros::Subscriber payload_sub;
+	ros::Subscriber flight_status_sub;
 	/*Create node Handler*/
 		ros::NodeHandle n_c;
 	/*Commands for flight controller*/
@@ -61,36 +63,40 @@ private:
 	/*Obtain data from ros parameter server*/	
 	void Rosparameters_get();
 
+	/*compiles buzz script from the specified .bzz file*/
 	void Compile_bzz();
 
 	/*Prepare messages and publish*/
-	inline void prepare_msg_and_publish();
+	void prepare_msg_and_publish();
 
 	  	
 	/*Refresh neighbours Position for every ten step*/
-	inline void maintain_pos(int tim_step);
+	void maintain_pos(int tim_step);
 
 	/*Maintain neighbours position*/
-	inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
+	void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
 
         /*Set the current position of the robot callback*/
-	inline void set_cur_pos(double latitude,
+	void set_cur_pos(double latitude,
 			 double longitude,
 			 double altitude);
 	/*convert from catresian to spherical coordinate system callback */
-	inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
+	double* cvt_spherical_coordinates(double neighbours_pos_payload []);
 	
 	/*battery status callback*/ 
-	inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
+	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
-
+	
+	/*flight status callback*/
+	void flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
+	
 	/*current position callback*/
-	inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
+	void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
 
 	/*payload callback callback*/
-	inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
+	void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
 
 	/* RC commands service */
-	inline bool rc_callback(mavros_msgs::CommandInt::Request  &req,
+	bool rc_callback(mavros_msgs::CommandInt::Request  &req,
 		         mavros_msgs::CommandInt::Response &res);
 
 

include/roscontroller.h.save

@@ -0,0 +1,100 @@
+ #pragma once
+#include "ros/ros.h"
+#include "sensor_msgs/NavSatFix.h"
+#include "mavros_msgs/GlobalPositionTarget.h"
+#include "mavros_msgs/CommandCode.h"
+#include "mavros_msgs/CommandInt.h"
+#include "mavros_msgs/State.h"
+#include "mavros_msgs/BatteryStatus.h"
+#include "mavros_msgs/Mavlink.h"
+#include "sensor_msgs/NavSatStatus.h"
+#include <sstream>
+#include <buzz/buzzasm.h>
+#include "buzzuav_closures.h"
+#include "buzz_utility.h"
+#include "uav_utility.h"
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <signal.h>
+#include <ostream>
+#include <map>
+
+using namespace std;
+
+namespace rosbzz_node{
+
+
+class roscontroller{
+
+public:
+	roscontroller();
+	~roscontroller();
+	//void RosControllerInit();
+	void RosControllerRun();
+	
+private:
+        
+ 	double cur_pos[3];
+	uint64_t payload;
+	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
+	int timer_step=0;
+	int robot_id=0;
+        int oldcmdID=0;
+	int rc_cmd;
+	std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname; //, rcclient;
+	bool rcclient;
+	ros::ServiceClient mav_client;
+	ros::Publisher payload_pub;
+	ros::ServiceServer service;
+	ros::Subscriber current_position_sub;
+	ros::Subscriber battery_sub;
+	ros::Subscriber payload_sub;
+	/*Create node Handler*/
+		ros::NodeHandle n_c;
+	/*Commands for flight controller*/
+  		mavros_msgs::CommandInt cmd_srv;	
+  	
+
+	void Initialize_pub_sub();
+
+	/*Obtain data from ros parameter server*/	
+	void Rosparameters_get();
+
+	void Compile_bzz();
+
+	/*Prepare messages and publish*/
+	inline void prepare_msg_and_publish();
+
+	  	
+	/*Refresh neighbours Position for every ten step*/
+	inline void maintain_pos(int tim_step);
+
+	/*Maintain neighbours position*/
+	inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
+
+        /*Set the current position of the robot callback*/
+	inline void set_cur_pos(double latitude,
+			 double longitude,
+			 double altitude);
+	/*convert from catresian to spherical coordinate system callback */
+	inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
+	
+	/*battery status callback*/ 
+	inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
+
+	/*current position callback*/
+	inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
+
+	/*payload callback callback*/
+	inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
+
+	/* RC commands service */
+	inline bool rc_callback(mavros_msgs::CommandInt::Request  &req,
+		         mavros_msgs::CommandInt::Response &res);
+
+
+
+};
+
+}

src/buzz_utility.cpp

@@ -301,7 +301,9 @@ void buzz_script_step() {
     */
    buzzuav_closures::buzzuav_update_battery(VM);
    buzzuav_closures::buzzuav_update_prox(VM);
-   /*
+   buzzuav_closures::buzzuav_update_currentpos(VM);
+   buzzuav_closures::buzzuav_update_flight_status(VM);
+    /*
     * Call Buzz step() function
     */
    if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {

src/buzzuav_closures.cpp

@@ -10,6 +10,8 @@
 namespace buzzuav_closures{
 static double goto_pos[3];
 static float batt[3];
+static double cur_pos[3];
+static uint8_t status;
 static int cur_cmd;
 static int rc_cmd=0;
 /****************************************/
@@ -148,6 +150,51 @@ int buzzuav_update_battery(buzzvm_t vm) {
    buzzvm_gstore(vm);
    return vm->state;
 }
+/****************************************/
+/*current pos update*/
+/***************************************/
+void set_currentpos(double latitude, double longitude, double altitude){
+   cur_pos[0]=latitude;
+   cur_pos[1]=longitude;
+   cur_pos[2]=altitude;
+}
+/****************************************/
+int buzzuav_update_currentpos(buzzvm_t vm) {
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
+   buzzvm_pusht(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
+   buzzvm_pushf(vm, cur_pos[0]);
+   buzzvm_tput(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
+   buzzvm_pushf(vm, cur_pos[1]);
+   buzzvm_tput(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
+   buzzvm_pushf(vm, cur_pos[2]);
+   buzzvm_tput(vm);
+   buzzvm_gstore(vm);
+   return vm->state;
+}
+/****************************************/
+/*flight status update*/
+/***************************************/
+void flight_status_update(uint8_t state){
+   status=state;
+}
+/****************************************/
+int buzzuav_update_flight_status(buzzvm_t vm) {
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
+   buzzvm_pusht(vm);
+   buzzvm_dup(vm);
+   buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
+   buzzvm_pushf(vm, status);
+   buzzvm_tput(vm); 
+   buzzvm_gstore(vm);
+   return vm->state;
+}
+
 
 /****************************************/
 /*To do !!!!!*/

src/out.basm

@@ -1,4 +1,4 @@
-!11
+!21
 'init
 'i
 'uav_takeoff
@@ -7,6 +7,16 @@
 'bzz print: remaining: 
 'battery
 'capacity
+'latitude : 
+'position
+'latitude
+' longitude: 
+'longitude
+' altitude : 
+'altitude
+'flight status: 
+'flight
+'status
 'uav_land
 'reset
 'destroy
@@ -17,10 +27,10 @@
 	pushs 3
 	pushcn @__label_2
 	gstore
-	pushs 9
+	pushs 19
 	pushcn @__label_5
 	gstore
-	pushs 10
+	pushs 20
 	pushcn @__label_6
 	gstore
 	nop
@@ -50,26 +60,54 @@
 	tget	|10,48,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 	pushi 2	|10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 	callc	|10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 1	|11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 4	|11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gload	|11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushi 20	|11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 8	|11,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gt	|11,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 9	|11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	jumpz @__label_3	|11,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 8	|11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 10	|11,29,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gload	|11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	tget	|11,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushi 0	|11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 11	|11,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	callc	|11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 9	|11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-@__label_3	|12,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 1	|12,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 12	|11,62,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 1	|12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	tget	|11,71,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gload	|12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 13	|11,72,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushi 1	|12,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 9	|11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	add	|12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gstore	|12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 14	|11,97,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	ret0	|14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	tget	|11,105,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 6	|11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	callc	|11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 4	|12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 15	|12,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 16	|12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 17	|12,31,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	tget	|12,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 2	|12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	callc	|12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 1	|13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 10	|13,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	eq	|13,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	jumpz @__label_3	|13,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 18	|13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 0	|13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	callc	|13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+@__label_3	|14,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 1	|14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 1	|14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 1	|14,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	add	|14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gstore	|14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	ret0	|16,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 
 @__label_5
-	ret0	|18,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	ret0	|20,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 
 @__label_6
-	ret0	|22,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	ret0	|24,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz

src/roscontroller.cpp

@@ -79,9 +79,10 @@ namespace rosbzz_node{
 
 	void roscontroller::Initialize_pub_sub(){
 		/*subscribers*/
-  		current_position_sub = n_c.subscribe("/dji_sdk/global_position", 1000, &roscontroller::current_pos,this);
+  		current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this);
   		battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this);
   		payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this);
+		flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
 		/* Clients*/
@@ -140,7 +141,7 @@ namespace rosbzz_node{
          		cout<<" [Debug:] sent message "<<*it<<endl;
 			i++;
         	}*/
-    		/*publish prepared messages in respective topic*/
+     		/*publish prepared messages in respective topic*/
     		payload_pub.publish(payload_out);
     		delete[] out;
     		delete[] payload_out_ptr;
@@ -191,10 +192,14 @@ namespace rosbzz_node{
 		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
 		//ROS_INFO("voltage : %d  current : %d  remaining : %d",msg->voltage, msg->current, msg ->remaining);
 	}
-
+	/*flight status update*/
+	void roscontroller::flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
+		buzzuav_closures::flight_status_update(msg->landed_state);
+	}
 	/*current position callback*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
 		set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
+		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
 	}
 
 	/*payload callback callback*/

src/test.bzz

@@ -8,7 +8,9 @@ uav_takeoff()
 function step() {
 
 print("bzz print: remaining: ", battery.capacity)
-if(i>20){uav_land()}
+print("latitude : ",position.latitude," longitude: ",position.longitude," altitude : ", position.altitude)
+print("flight status: ",flight.status)
+if(i==10){uav_land()}
 i=i+1
 
 }