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

initialization

This commit is contained in:
isvogor 2017-04-17 11:51:07 -04:00
parent 3af2a03ffc
commit e01e6e6c5e
3 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/buzzuav_closures.cpp
View File

@ -109,6 +109,7 @@ namespace buzzuav_closures{
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos); gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm); return buzzvm_ret0(vm);

16
src/roscontroller.cpp
View File

@ -26,6 +26,9 @@ namespace rosbzz_node{
GetRobotId(); GetRobotId();
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
home[0] = 0.0;
home[1] = 0.0;
home[2] = 0.0;
} }
/*--------------------- /*---------------------
@ -43,6 +46,7 @@ namespace rosbzz_node{
void roscontroller::GetRobotId() void roscontroller::GetRobotId()
{ {
/*
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
mavros_msgs::ParamGet::Response robot_id_srv_response; mavros_msgs::ParamGet::Response robot_id_srv_response;
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@ -51,8 +55,8 @@ namespace rosbzz_node{
} }
robot_id=robot_id_srv_response.value.integer; robot_id=robot_id_srv_response.value.integer;
*/
//robot_id = 8; robot_id = 8;
} }
/*------------------------------------------------- /*-------------------------------------------------
@ -512,9 +516,9 @@ namespace rosbzz_node{
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1]; double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[2] = cur[2]; out[2] = cur[2];
} }
@ -796,6 +800,8 @@ namespace rosbzz_node{
moveMsg.pose.orientation.z = 0; moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1; moveMsg.pose.orientation.w = 1;
std::cout<<"Home (0,1,2): " << home[0] <<", " << home[1] << ", " << home[2] <<std::endl;
localsetpoint_pub.publish(moveMsg); localsetpoint_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!"); ROS_INFO("Sent local NON RAW position message!");

2
src/testflockfev.bzz
View File

@ -50,7 +50,7 @@ function hexagon() {
statef=land statef=land
} else { } else {
timeW = timeW+1 timeW = timeW+1
uav_moveto(0.0,0.0) uav_moveto(0.2,0.0)
} }
} }