Spiri/ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

goto fixes

Browse Source
This commit is contained in:
David St-Onge 2017-01-26 23:49:53 -05:00
parent 597ce839c4
commit ab368990d9
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/buzzuav_closures.cpp
View File

@ -82,9 +82,9 @@ int buzzros_print(buzzvm_t vm) {
y = cartesian_pos_payload[1]; y = cartesian_pos_payload[1];
z = cartesian_pos_payload[2]; z = cartesian_pos_payload[2];
try { 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[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) { } catch (std::overflow_error e) {
// std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl; // 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); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dx = buzzvm_stack_at(vm, 1)->f.value; float dx = buzzvm_stack_at(vm, 1)->f.value;
float dy = buzzvm_stack_at(vm, 2)->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; //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
double cur_pos_cartesian[3]; double cur_pos_cartesian[3];
cartesian_coordinates(cur_pos,cur_pos_cartesian); cartesian_coordinates(cur_pos,cur_pos_cartesian);
double goto_spheric[3]; double goto_cartesian[3];
goto_spheric[0] = dx + cur_pos_cartesian[0]; goto_cartesian[0] = dx + cur_pos_cartesian[0];
goto_spheric[1] = dy + cur_pos_cartesian[1]; goto_cartesian[1] = dy + cur_pos_cartesian[1];
goto_spheric[2] = cur_pos_cartesian[2]; goto_cartesian[2] = cur_pos_cartesian[2];
spherical_coordinates(goto_spheric, goto_pos); spherical_coordinates(goto_cartesian, goto_pos);
// goto_pos[0]=cur_pos[0]+dlat*180/M_PI; // goto_pos[0]=cur_pos[0]+dlat*180/M_PI;
// goto_pos[1]=cur_pos[1]+dlon*180/M_PI; // goto_pos[1]=cur_pos[1]+dlon*180/M_PI;
// goto_pos[2]=cur_pos[2]; // goto_pos[2]=cur_pos[2];