diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 618a317..40b5711 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -82,9 +82,9 @@ int buzzros_print(buzzvm_t vm) {
                 y = cartesian_pos_payload[1];
                 z = cartesian_pos_payload[2];
                 try {
-                out[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
+                out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-6371000;
                 out[1]=atan(y/x);
-                out[2]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
+                out[0]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z);
                 } catch (std::overflow_error e) {
 //                std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
                 }
@@ -99,15 +99,15 @@ int buzzuav_goto(buzzvm_t vm) {
    buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
    float dx = buzzvm_stack_at(vm, 1)->f.value;
    float dy = buzzvm_stack_at(vm, 2)->f.value;
-   printf(" Vector for Goto: %f,%f\n",dx,dy);
+   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
    //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
    double cur_pos_cartesian[3];
    cartesian_coordinates(cur_pos,cur_pos_cartesian);
-   double goto_spheric[3];
-   goto_spheric[0] = dx + cur_pos_cartesian[0];
-   goto_spheric[1] = dy + cur_pos_cartesian[1];
-   goto_spheric[2] = cur_pos_cartesian[2];
-   spherical_coordinates(goto_spheric, goto_pos);
+   double goto_cartesian[3];
+   goto_cartesian[0] = dx + cur_pos_cartesian[0];
+   goto_cartesian[1] = dy + cur_pos_cartesian[1];
+   goto_cartesian[2] = cur_pos_cartesian[2];
+   spherical_coordinates(goto_cartesian, goto_pos);
 //   goto_pos[0]=cur_pos[0]+dlat*180/M_PI;
 //   goto_pos[1]=cur_pos[1]+dlon*180/M_PI;
 //   goto_pos[2]=cur_pos[2];