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

fixed local setpoint

This commit is contained in:
David St-Onge 2017-05-10 18:39:28 -04:00
parent b095536db6
commit cab2cc03a1
3 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/roscontroller.h
View File

@ -69,6 +69,7 @@ private:
double DEFAULT_REFERENCE_LATITUDE = 45.457817;
double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
double target[3];
double cur_pos[3];
double home[3];
double cur_rel_altitude;

9
script/testflockfev.bzz
View File

@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 18.0 #0.001
TARGET = 12.0 #0.000001001
EPSILON = 6.0 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
@ -48,9 +48,12 @@ function hexagon() {
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# timeW = timeW+1
# uav_moveto(0.06,0.0)
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.0)
# uav_moveto(0.0,0.06)
# }
}

10
src/roscontroller.cpp
View File

@ -31,8 +31,9 @@ namespace rosbzz_node{
robot_id= strtol(robot_name.c_str() + 5, NULL, 10);
setpoint_counter = 0;
fcu_timeout = TIMEOUT;
home[0]=0.0;home[1]=0.0;home[2]=0.0;
target[0]=0.0;target[1]=0.0;target[2]=0.0;
}
/*---------------------
@ -755,8 +756,11 @@ namespace rosbzz_node{
ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
moveMsg.pose.position.x = local_pos[1]+y;
moveMsg.pose.position.y = local_pos[0]+x;
target[0] += y;
target[1] += x;
moveMsg.pose.position.x = target[0];
moveMsg.pose.position.y = target[1];
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;