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

new gps transform

This commit is contained in:
David St-Onge 2017-01-30 23:52:56 -05:00
parent db1fd60616
commit 6ab4e35b61
4 changed files with 33 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/roscontroller.h
View File

@ -99,6 +99,9 @@ private:
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearingGB_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing2D_coordinates(double neighbours_pos_payload [], double out[], double pos[]);

30
src/buzzuav_closures.cpp
View File

@ -91,6 +91,13 @@ int buzzros_print(buzzvm_t vm) {
// std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
}
}
void gps_from_rb(double range, double bearing, double out[3]) {
out[0] = asin(sin(cur_pos[0]) * cos(range/EARTH_RADIUS) + cos(cur_pos[0]) * sin(range/EARTH_RADIUS) * cos(bearing));
out[1] = cur_pos[1] + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(cur_pos[0]), cos(bearing/EARTH_RADIUS) - sin(cur_pos[0])*sin(out[0]));
out[3] = height; //constant height.
}
int buzzuav_goto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */
@ -99,20 +106,21 @@ int buzzuav_goto(buzzvm_t vm) {
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
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: %.7f,%.7f\n",dx,dy);
float d = buzzvm_stack_at(vm, 1)->f.value;
float b = buzzvm_stack_at(vm, 2)->f.value;
printf(" Vector for Goto: %.7f,%.7f\n",d,b);
//goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
double cur_pos_cartesian[3];
cartesian_coordinates(cur_pos,cur_pos_cartesian);
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);
//double cur_pos_cartesian[3];
//cartesian_coordinates(cur_pos,cur_pos_cartesian);
//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]=dx;
// goto_pos[1]=dy;
goto_pos[2]=height; // force a constant altitude to avoid loop increases
//goto_pos[2]=height; // force a constant altitude to avoid loop increases
gps_from_rb(d, b, goto_pos);
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;

11
src/roscontroller.cpp
View File

@ -320,6 +320,15 @@ namespace rosbzz_node{
#define EARTH_RADIUS 6371000.0
#define LAT_AVERAGE 45.564898
/*rectangular projection range and bearing coordinate system callback */
void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){
out[1] = acos(sin(cur[0]) * sin(nei[0]) +cos(nei[0]) * cos(cur[0])*cos(nei[1]-cur[1]))*EARTH_RADIUS;
double y = sin(cur[1]-nei[1])*cos(cur[0]);
double x = cos(cur[0])*sin(nei[0]) - sin(nei[0])*cos(cur[0])*cos(cur[1]-nei[1]);
out[1] = atan2(y,x)/M_PI*180+180;
out[2] = 0.0;
}
/*rectangular projection range and bearing coordinate system callback */
void roscontroller::cvt_rangebearing2D_coordinates(double spherical_pos_payload [], double out[], double cur[]){
double nei_lat = spherical_pos_payload[0] / 180.0 * M_PI;
@ -534,7 +543,7 @@ namespace rosbzz_node{
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
// cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
double cvt_neighbours_pos_test[3];
cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos);
cvt_rangebearingGB_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos);
/*Convert obtained position to relative CARTESIAN position*/
double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);

2
src/test1.bzz
View File

@ -16,7 +16,7 @@ TARGET_ALTITUDE = 10.01
# Lennard-Jones parameters
TARGET = 50.0 #0.000001001
EPSILON = 80.0 #0.001
EPSILON = 200.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {