From ab368990d9acc6332f3d2d8100947d8528be55e6 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 26 Jan 2017 23:49:53 -0500 Subject: [PATCH] goto fixes --- src/buzzuav_closures.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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 "<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];