From a01a947d868da1a6dcc56389ad4ef5c1074ede75 Mon Sep 17 00:00:00 2001 From: Mario Alexis Labrana Date: Wed, 1 Feb 2017 14:08:17 -0500 Subject: [PATCH] lj test --- src/buzzuav_closures.cpp | 10 +- src/buzzuav_closures.cpp.save | 355 ++++++++++++++++++++++++++++++++++ src/test1.bzz | 2 +- 3 files changed, 361 insertions(+), 6 deletions(-) create mode 100644 src/buzzuav_closures.cpp.save diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 15d0b0f..867e8d5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -117,8 +117,8 @@ int buzzuav_goto(buzzvm_t vm) { buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); float dy = buzzvm_stack_at(vm, 1)->f.value; float dx = buzzvm_stack_at(vm, 2)->f.value; -// float d = sqrt(dx*dx+dy*dy); -// float b = atan2(dy,dx); + double d = sqrt(dx*dx+dy*dy); //range + double b = atan2(dy,dx); //bearing printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; //double cur_pos_cartesian[3]; @@ -131,8 +131,8 @@ int buzzuav_goto(buzzvm_t vm) { // goto_pos[0]=dx; // goto_pos[1]=dy; //goto_pos[2]=height; // force a constant altitude to avoid loop increases -// gps_from_rb(d, b, goto_pos); - if(dx == 1.0){ + gps_from_rb(d, b, goto_pos); +/* if(dx == 1.0){ if((int)buzz_utility::get_robotid()==1){ goto_pos[0]=hcpos1[0];goto_pos[1]=hcpos1[1];goto_pos[2]=height; } @@ -153,7 +153,7 @@ int buzzuav_goto(buzzvm_t vm) { if((int)buzz_utility::get_robotid()==3){ goto_pos[0]=hcpos3[2];goto_pos[1]=hcpos3[3];goto_pos[2]=height; } - } + }*/ 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/buzzuav_closures.cpp.save b/src/buzzuav_closures.cpp.save new file mode 100644 index 0000000..9fe53eb --- /dev/null +++ b/src/buzzuav_closures.cpp.save @@ -0,0 +1,355 @@ +/** @file buzzuav_closures.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. + * @author Vivek Shankar Varadharajan + * @copyright 2016 MistLab. All rights reserved. + */ +//#define _GNU_SOURCE +#include "buzzuav_closures.h" +#include "buzz_utility.h" +namespace buzzuav_closures{ +static double goto_pos[3]; +static double rc_goto_pos[3]; +static float batt[3]; +static double cur_pos[3]; +static uint8_t status; +static int cur_cmd = 0; +static int rc_cmd=0; +static int buzz_cmd=0; +static float height=0; +/****************************************/ +/****************************************/ + +int buzzros_print(buzzvm_t vm) { + int i; + for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { + buzzvm_lload(vm, i); + buzzobj_t o = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + switch(o->o.type) { + case BUZZTYPE_NIL: + ROS_INFO("BUZZ - [nil]"); + break; + case BUZZTYPE_INT: + ROS_INFO("%d", o->i.value); + //fprintf(stdout, "%d", o->i.value); + break; + case BUZZTYPE_FLOAT: + ROS_INFO("%f", o->f.value); + break; + case BUZZTYPE_TABLE: + ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value))); + break; + case BUZZTYPE_CLOSURE: + if(o->c.value.isnative) + ROS_INFO("[n-closure @%d]", o->c.value.ref); + else + ROS_INFO("[c-closure @%d]", o->c.value.ref); + break; + case BUZZTYPE_STRING: + ROS_INFO("%s", o->s.value.str); + break; + case BUZZTYPE_USERDATA: + ROS_INFO("[userdata @%p]", o->u.value); + break; + default: + break; + } + } + //fprintf(stdout, "\n"); +45.564489-73.56253745.56472945.564140-73.56233645.564362 return buzzvm_ret0(vm); +} + +/****************************************/ +/****************************************/ +#define EARTH_RADIUS 6371000.0 +/*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]+EARTH_RADIUS; //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 d = sqrt(dx*dx+dy*dy); +// float b = atan2(dy,dx); + 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_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 +// gps_from_rb(d, b, goto_pos); + if(dx == 1.0){ + if((int)buzz_utility::get_robotid()==1){ + goto_pos[0]=hcpos1[0];goto_pos[1]=hcpos1[1];goto_pos[2]=height; + } + if((int)buzz_utility::get_robotid()==2){ + goto_pos[0]=hcpos2[0];goto_pos[1]=hcpos2[1];goto_pos[2]=height; + } + if((int)buzz_utility::get_robotid()==3){ + goto_pos[0]=hcpos3[0];goto_pos[1]=hcpos3[1];goto_pos[2]=height; + } + } + if(dx == 2.0){ + if((int)buzz_utility::get_robotid()==1){ + goto_pos[0]=hcpos1[2];goto_pos[1]=hcpos1[3];goto_pos[2]=height; + } + if((int)buzz_utility::get_robotid()==2){ + goto_pos[0]=hcpos2[2];goto_pos[1]=hcpos2[3];goto_pos[2]=height; + } + if((int)buzz_utility::get_robotid()==3){ + goto_pos[0]=hcpos3[2];goto_pos[1]=hcpos3[3];goto_pos[2]=height; + } + } + 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(){ +return goto_pos; +} +/******************************/ +int getcmd(){ + return cur_cmd; +} + +void set_goto(double pos[]){ +goto_pos[0]=pos[0]; +goto_pos[1]=pos[1]; +goto_pos[2]=pos[2]; + +} + +int bzz_cmd(){ +int cmd = buzz_cmd; +buzz_cmd=0; +return cmd; +} + +void rc_set_goto(double pos[]){ +rc_goto_pos[0]=pos[0]; +rc_goto_pos[1]=pos[1]; +rc_goto_pos[2]=pos[2]; + +} +void rc_call(int rc_cmd_in){ +rc_cmd=rc_cmd_in; +} + +/****************************************/ +/****************************************/ + +int buzzuav_takeoff(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); /* Altitude */ + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value; + height = goto_pos[2]; + cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; + printf(" Buzz requested Take off !!! \n"); + buzz_cmd=1; + return buzzvm_ret0(vm); +} + +int buzzuav_land(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_LAND; + printf(" Buzz requested Land !!! \n"); + buzz_cmd=1; + return buzzvm_ret0(vm); +} + +int buzzuav_gohome(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + printf(" Buzz requested gohome !!! \n"); + buzz_cmd=1; + return buzzvm_ret0(vm); +} + +/****************************************/ +void set_battery(float voltage,float current,float remaining){ + batt[0]=voltage; + batt[1]=current; + batt[2]=remaining; +} +/****************************************/ + +int buzzuav_update_battery(buzzvm_t vm) { + //static char BATTERY_BUF[256]; + buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); + buzzvm_pushf(vm, batt[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); + buzzvm_pushf(vm, batt[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); + buzzvm_pushf(vm, batt[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} +/****************************************/ +/*current pos update*/ +/***************************************/ +void set_currentpos(double latitude, double longitude, double altitude){ + cur_pos[0]=latitude; + cur_pos[1]=longitude; + cur_pos[2]=altitude; +} +/****************************************/ +int buzzuav_update_currentpos(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, cur_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, cur_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, cur_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} +/****************************************/ +/*flight status update*/ +/***************************************/ +void flight_status_update(uint8_t state){ + status=state; +} +/****************************************/ +int buzzuav_update_flight_status(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); + buzzvm_pushi(vm, rc_cmd); + buzzvm_tput(vm); + buzzvm_dup(vm); + rc_cmd=0; + buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); + buzzvm_pushi(vm, status); + buzzvm_tput(vm); + buzzvm_gstore(vm); + //also set rc_controllers goto + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + + + +/****************************************/ +/*To do !!!!!*/ +/****************************************/ + +int buzzuav_update_prox(buzzvm_t vm) { + /* static char PROXIMITY_BUF[256]; + int i; + //kh4_proximity_ir(PROXIMITY_BUF, DSPIC); + buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1)); + buzzvm_pusht(vm); + for(i = 0; i < 8; i++) { + buzzvm_dup(vm); + buzzvm_pushi(vm, i+1); + buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8)); + buzzvm_tput(vm); + } + buzzvm_gstore(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "ground_ir", 1)); + buzzvm_pusht(vm); + for(i = 8; i < 12; i++) { + buzzvm_dup(vm); + buzzvm_pushi(vm, i-7); + buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8)); + buzzvm_tput(vm); + } + buzzvm_gstore(vm);*/ + return vm->state; +} + +/****************************************/ +/****************************************/ + +int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} + +} + diff --git a/src/test1.bzz b/src/test1.bzz index 8742b9e..7ae18bd 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -42,7 +42,7 @@ function hexagon() { math.vec2.scale(accum, 1.0 / neighbors.count()) # Move according to vector print("Robot ", id, "must push ",accum.length, "; ", accum.angle) - uav_goto(1.0, 0.0) #accum.x, accum.y) + uav_goto(accum.x, accum.y) } ########################################