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

fix gps precision

Browse Source
This commit is contained in:
David St-Onge 2017-02-08 11:23:12 -05:00
parent a01a947d86
commit b9b845802b
5 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/buzzuav_closures.h
View File

@ -20,6 +20,7 @@ int buzzros_print(buzzvm_t vm);
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
* commands the UAV to go to a position supplied
*/
int buzzuav_moveto(buzzvm_t vm);
int buzzuav_goto(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd();

3
src/buzz_utility.cpp
View File

@ -259,6 +259,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);

10
src/buzzuav_closures.cpp
View File

@ -107,7 +107,8 @@ void gps_from_rb(double range, double bearing, double out[3]) {
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
int buzzuav_goto(buzzvm_t vm) {
int buzzuav_moveto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
@ -160,6 +161,13 @@ int buzzuav_goto(buzzvm_t vm) {
return buzzvm_ret0(vm);
}
int buzzuav_goto(buzzvm_t vm) {
set_goto(rc_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;
return buzzvm_ret0(vm);
}
/******************************/
double* getgoto(){

4
src/roscontroller.cpp
View File

@ -609,8 +609,8 @@ namespace rosbzz_node{
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
double rc_goto[3];
rc_goto[0]=req.x;
rc_goto[1]=req.y;
rc_goto[0]=req.x/pow(10,7);
rc_goto[1]=req.y/pow(10,7);
rc_goto[2]=req.z;
buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;

11
src/test1.bzz
View File

@ -16,7 +16,7 @@ TARGET_ALTITUDE = 10.0
# Lennard-Jones parameters
TARGET = 50.0 #0.000001001
EPSILON = 200.0 #0.001
EPSILON = 0.5 #0.001
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
@ -41,8 +41,8 @@ function hexagon() {
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_goto(accum.x, accum.y)
# print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
# uav_moveto(accum.x, accum.y)
}
########################################
@ -55,7 +55,7 @@ function hexagon() {
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 3 # number of robots in the swarm
ROBOTS = 1 # number of robots in the swarm
#
# Sets a barrier
@ -142,6 +142,9 @@ function step() {
flight.rc_cmd=0
statef=land
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16 and flight.status==2) {
flight.rc_cmd=0
uav_goto()
}
statef()
}