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

goto fixes

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];
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];