From 959e339914a6e53f0854fc7f1eaca0a3109a938c Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 26 Jan 2017 23:19:16 -0500 Subject: [PATCH 1/6] fixed goto uav --- include/roscontroller.h | 3 ++ src/buzzuav_closures.cpp | 48 +++++++++++++++--- src/roscontroller.cpp | 106 +++++++++++++++++++++++++++++++++++++-- 3 files changed, 145 insertions(+), 12 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index ee44f0b..90fdc42 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -90,6 +90,9 @@ private: /*convert from spherical to cartesian coordinate system callback */ void cvt_cartesian_coordinates(double neighbours_pos_payload [], double out[]); + + /*convert from spherical to cartesian coordinate system callback */ + void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[]); /*battery status callback*/ void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d561133..8df9100 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -61,7 +61,34 @@ int buzzros_print(buzzvm_t vm) { /****************************************/ /****************************************/ - +/*convert from spherical to cartesian coordinate system callback */ + void cartesian_coordinates(double spherical_pos_payload [], double out[]){ + double latitude, longitude, rho; + latitude = spherical_pos_payload[0] / 180.0 * M_PI; + longitude = spherical_pos_payload[1] / 180.0 * M_PI; + rho = spherical_pos_payload[2]+6371000; //centered on Earth + try { + out[0] = cos(latitude) * cos(longitude) * rho; + out[1] = cos(latitude) * sin(longitude) * rho; + out[2] = sin(latitude) * rho; // z is 'up' + } catch (std::overflow_error e) { +// std::cout << e.what() << " Error in convertion to cartesian coordinate system "<f.value; - float dx = buzzvm_stack_at(vm, 2)->f.value; + float dx = buzzvm_stack_at(vm, 1)->f.value; + float dy = buzzvm_stack_at(vm, 2)->f.value; //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; - double dlat=dx/(6371000+cur_pos[2]); - double dlon=dy/cos(cur_pos[0]/180*3.1416)/(6371000+cur_pos[2]); - goto_pos[0]=cur_pos[0]+dlat*180/3.1416; - goto_pos[1]=cur_pos[1]+dlon*180/3.1416; - goto_pos[2]=cur_pos[2]; + 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] = 6371000 + cur_pos_cartesian[2]; + spherical_coordinates(goto_spheric, 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]; cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); buzz_cmd=2; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 71ead2b..7d2cab2 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -249,8 +249,8 @@ namespace rosbzz_node{ /*convert from spherical to cartesian coordinate system callback */ void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){ double latitude, longitude, rho; - latitude = spherical_pos_payload[0] / 180.0 * 3.1416; - longitude = spherical_pos_payload[1] / 180.0 * 3.1416; + latitude = spherical_pos_payload[0] / 180.0 * M_PI; + longitude = spherical_pos_payload[1] / 180.0 * M_PI; rho = spherical_pos_payload[2]+6371000; //centered on Earth try { out[0] = cos(latitude) * cos(longitude) * rho; @@ -261,6 +261,101 @@ namespace rosbzz_node{ } } + /*convert from GPS delta to Range and Bearing */ + void roscontroller::cvt_rangebearing_coordinates(double spherical_pos[], double out[]){ + //Earth ellipse parameteres + double f = 1.0/298.257223563; + double a = 6378137; + double b = 6356752.314245; + double a2b2b2 = (a*a-b*b)/(b*b); + // constants to enhance the computation + double omega = (spherical_pos[1]-cur_pos[1])/180*M_PI; + double A, lambda0, sigma, deltasigma, lambda=omega; + double cosU2 = cos(atan((1-f)*tan(spherical_pos[0]/180*M_PI))); + double sinU2 = sin(atan((1-f)*tan(spherical_pos[0]/180*M_PI))); + double cosU1 = cos(atan((1-f)*tan(cur_pos[0]/180*M_PI))); + double sinU1 = sin(atan((1-f)*tan(cur_pos[0]/180*M_PI))); + try { + bool converged = 0; + for (int i = 0; i < 20; i++) { + lambda0=lambda; + + // eq. 14 + double sin2sigma = (cosU2 * sin(lambda) * cosU2 * sin(lambda)) + pow(cosU1*sinU2 - sinU1*cosU2 * cos(lambda), 2.0); + double sinsigma = sqrt(sin2sigma); + + // eq. 15 + double cossigma = sinU1*sinU2 + (cosU1*cosU2 * cos(lambda)); + + // eq. 16 + sigma = atan2(sinsigma, cossigma); + + // eq. 17 Careful! sin2sigma might be almost 0! + double sinalpha = (sin2sigma == 0) ? 0.0 : cosU1*cosU2 * sin(lambda) / sinsigma; + double alpha = asin(sinalpha); + double cos2alpha = cos(alpha) * cos(alpha); + + // eq. 18 Careful! cos2alpha might be almost 0! + double cos2sigmam = cos2alpha == 0.0 ? 0.0 : cossigma - 2 * sinU1*sinU2 / cos2alpha; + double u2 = cos2alpha * a2b2b2; + double cos2sigmam2 = cos2sigmam * cos2sigmam; + + // eq. 3 + A = 1.0 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2))); + + // eq. 4 + double B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2))); + + // eq. 6 + deltasigma = B * sinsigma * (cos2sigmam + B / 4 * (cossigma * (-1 + 2 * cos2sigmam2) - B / 6 * cos2sigmam * (-3 + 4 * sin2sigma) * (-3 + 4 * cos2sigmam2))); + + // eq. 10 + double C = f / 16 * cos2alpha * (4 + f * (4 - 3 * cos2alpha)); + + // eq. 11 (modified) + lambda = omega + (1 - C) * f * sinalpha * (sigma + C * sinsigma * (cos2sigmam + C * cossigma * (-1 + 2 * cos2sigmam2))); + + // see how much improvement we got + double change = fabs((lambda - lambda0) / lambda); + + if ((i > 1) && (change < 0.0000000000001)) { + //cout << "CONVERSION CONVERGED at " << i << " !!!!" << endl; + converged = 1; + break; + } + } + + // eq. 19 + out[0] = b * A * (sigma - deltasigma); + + // didn't converge? must be N/S + if (!converged) { + if (cur_pos[0] > spherical_pos[0]) { + out[1] = M_PI/2; + out[2] = 0; + } else if (cur_pos[0] < spherical_pos[0]) { + out[1] = 0; + out[2] = M_PI/2; + } + } else { // else, it converged, so do the math + + // eq. 20 + out[1] = atan2(cosU2 * sin(lambda), (cosU1*sinU2 - sinU1*cosU2 * cos(lambda))); + if (out[1] < 0.0) out[1] += 2*M_PI; + + // eq. 21 + out[2] = atan2(cosU1 * sin(lambda), (-sinU1*cosU2 + cosU1*sinU2 * cos(lambda))) + 3.1416; + if (out[2] < 0.0) out[2] += 2*M_PI; + } + + if (out[1] >= M_PI) out[1] -= M_PI; + if (out[2] >= M_PI) out[2] -= M_PI; + + } catch (std::overflow_error e) { + std::cout << e.what() << " Error in convertion to range and bearing "< Date: Thu, 26 Jan 2017 23:37:08 -0500 Subject: [PATCH 2/6] goto test --- src/buzzuav_closures.cpp | 3 ++- src/test1.bzz | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 8df9100..618a317 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -99,13 +99,14 @@ 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); //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] = 6371000 + cur_pos_cartesian[2]; + goto_spheric[2] = cur_pos_cartesian[2]; spherical_coordinates(goto_spheric, goto_pos); // goto_pos[0]=cur_pos[0]+dlat*180/M_PI; // goto_pos[1]=cur_pos[1]+dlon*180/M_PI; diff --git a/src/test1.bzz b/src/test1.bzz index ca6b7ab..55fd63a 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -34,7 +34,7 @@ function hexagon() { if(neighbors.count() > 0) math.vec2.scale(accum, 1.0 / neighbors.count()) # Move according to vector - print("Robot ", id, "must push ",accum.x, "; ", accum.y) +# print("Robot ", id, "must push ",accum.x, "; ", accum.y) uav_goto(accum.x, accum.y) } From ab368990d9acc6332f3d2d8100947d8528be55e6 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 26 Jan 2017 23:49:53 -0500 Subject: [PATCH 3/6] 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]; From 37cf9aea6cf715b424cca79ece9dca2b1e28f4b0 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 27 Jan 2017 00:00:19 -0500 Subject: [PATCH 4/6] goto fixes --- src/buzzuav_closures.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 40b5711..16c4638 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -83,8 +83,8 @@ int buzzros_print(buzzvm_t vm) { z = cartesian_pos_payload[2]; try { out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-6371000; - out[1]=atan(y/x); - out[0]=atan((sqrt(pow(x,2.0)+pow(y,2.0)))/z); + out[1]=atan2(y,x)*180/M_PI; + out[0]=atan2((sqrt(pow(x,2.0)+pow(y,2.0))),z)*180/M_PI; } catch (std::overflow_error e) { // std::cout << e.what() << " Error in convertion to spherical coordinate system "< Date: Fri, 27 Jan 2017 00:12:20 -0500 Subject: [PATCH 5/6] goto fixes --- src/buzzuav_closures.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 16c4638..0f7a185 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -83,7 +83,7 @@ int buzzros_print(buzzvm_t vm) { z = cartesian_pos_payload[2]; try { out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-6371000; - out[1]=atan2(y,x)*180/M_PI; + out[1]=atan2(x,y)*180/M_PI; out[0]=atan2((sqrt(pow(x,2.0)+pow(y,2.0))),z)*180/M_PI; } catch (std::overflow_error e) { // std::cout << e.what() << " Error in convertion to spherical coordinate system "< Date: Fri, 27 Jan 2017 00:18:47 -0500 Subject: [PATCH 6/6] goto fixes --- src/buzzuav_closures.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 0f7a185..bc49964 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -83,8 +83,8 @@ int buzzros_print(buzzvm_t vm) { z = cartesian_pos_payload[2]; try { out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-6371000; - out[1]=atan2(x,y)*180/M_PI; - out[0]=atan2((sqrt(pow(x,2.0)+pow(y,2.0))),z)*180/M_PI; + out[1]=atan2(y,x)*180/M_PI; + out[0]=atan2(z,(sqrt(pow(x,2.0)+pow(y,2.0))))*180/M_PI; } catch (std::overflow_error e) { // std::cout << e.what() << " Error in convertion to spherical coordinate system "<