battery test changes

src/out.basm

@@ -1,24 +1,27 @@
-!8
+!11
 'init
+'i
+'uav_takeoff
 'step
 'print
-'bzz print: voltage: 
+'bzz print: remaining: 
 'battery
-'voltage
+'capacity
+'uav_land
 'reset
 'destroy
 
 	pushs 0
 	pushcn @__label_1
 	gstore
-	pushs 1
+	pushs 3
 	pushcn @__label_2
 	gstore
-	pushs 6
-	pushcn @__label_3
+	pushs 9
+	pushcn @__label_5
 	gstore
-	pushs 7
-	pushcn @__label_4
+	pushs 10
+	pushcn @__label_6
 	gstore
 	nop
 
@@ -28,22 +31,45 @@
 	done
 
 @__label_1
-	ret0	|4,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 1	|3,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 0	|3,2,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gstore	|3,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 2	|4,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|4,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 0	|4,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	callc	|4,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	ret0	|5,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 
 @__label_2
-	pushs 2	|9,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gload	|9,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 3	|9,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 4	|9,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	gload	|9,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushs 5	|9,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	tget	|9,45,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	pushi 2	|9,46,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	callc	|9,46,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
-	ret0	|11,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 4	|10,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|10,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 5	|10,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 6	|10,39,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|10,39,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 7	|10,40,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	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
+	gload	|11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 20	|11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gt	|11,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	jumpz @__label_3	|11,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 8	|11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 0	|11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	callc	|11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+@__label_3	|12,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 1	|12,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushs 1	|12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gload	|12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	pushi 1	|12,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	add	|12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	gstore	|12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+	ret0	|14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 
-@__label_3
-	ret0	|15,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+@__label_5
+	ret0	|18,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
 
-@__label_4
-	ret0	|19,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz
+@__label_6
+	ret0	|22,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz

src/roscontroller.cpp

@@ -80,7 +80,7 @@ namespace rosbzz_node{
 	void roscontroller::Initialize_pub_sub(){
 		/*subscribers*/
   		current_position_sub = n_c.subscribe("/dji_sdk/global_position", 1000, &roscontroller::current_pos,this);
-  		battery_sub = n_c.subscribe("/dji_sdk/power_status", 1000, &roscontroller::battery,this);
+  		battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this);
   		payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
@@ -189,6 +189,7 @@ namespace rosbzz_node{
 	/*battery status callback*/ 
 	void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
 		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
+		//ROS_INFO("voltage : %d  current : %d  remaining : %d",msg->voltage, msg->current, msg ->remaining);
 	}
 
 	/*current position callback*/

src/test.bzz

@@ -1,12 +1,15 @@
 # Executed once at init time.
 function init() {
-
+i=0
+uav_takeoff()
 }
  
 # Executed at each time step.
 function step() {
 
-print("bzz print: voltage: ", battery.voltage)
+print("bzz print: remaining: ", battery.capacity)
+if(i>20){uav_land()}
+i=i+1
 
 }