made NED pos available in buzz

include/buzzuav_closures.h

@@ -89,6 +89,8 @@ void set_filtered_packet_loss(float value);
 /*
  * sets current position
  */
+void set_currentNEDpos(double x, double y);
+
 void set_currentpos(double latitude, double longitude, float altitude, float yaw);
 /*
  * returns the current go to position

src/buzzuav_closures.cpp

@@ -19,6 +19,7 @@ static float rc_gimbal[4];
 static float batt[3];
 static float obst[5] = { 0, 0, 0, 0, 0 };
 static double cur_pos[4];
+static double cur_NEDpos[2];
 static uint8_t status;
 static int cur_cmd = 0;
 static int rc_cmd = 0;
@@ -629,6 +630,15 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
   return vm->state;
 }
 
+void set_currentNEDpos(double x, double y)
+/*
+/ update interface position array
+-----------------------------------*/
+{
+  cur_NEDpos[0] = x;
+  cur_NEDpos[1] = y;
+}
+
 void set_currentpos(double latitude, double longitude, float altitude, float yaw)
 /*
 / update interface position array
@@ -697,6 +707,14 @@ int buzzuav_update_currentpos(buzzvm_t vm)
   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0));
   buzzvm_pushf(vm, cur_pos[2]);
   buzzvm_tput(vm);
+  buzzvm_push(vm, tPosition);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 0));
+  buzzvm_pushf(vm, cur_NEDpos[0]);
+  buzzvm_tput(vm);
+  buzzvm_push(vm, tPosition);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 0));
+  buzzvm_pushf(vm, cur_NEDpos[1]);
+  buzzvm_tput(vm);
   //  Store read table in the proximity table
   buzzvm_push(vm, tPoseTable);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0));

src/roscontroller.cpp

@@ -987,6 +987,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
 {
   cur_pos.x = msg->pose.position.x;
   cur_pos.y = msg->pose.position.y;
+  set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
   //  cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
   tf::Quaternion q(
     msg->pose.orientation.x,