Untabification and Added get functions for Xbee_status
This commit is contained in:
pyhs
parent
1d24de28a5
commit
8e5acd264d
|
|
@ -14,8 +14,8 @@
|
|||
|
||||
namespace buzzuav_closures{
|
||||
typedef enum {
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_LAND,
|
||||
COMMAND_GOHOME,
|
||||
COMMAND_ARM,
|
||||
|
|
|
|||
|
|
@ -42,197 +42,206 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
namespace rosbzz_node{
|
||||
namespace rosbzz_node
|
||||
{
|
||||
|
||||
class roscontroller{
|
||||
class roscontroller
|
||||
{
|
||||
|
||||
public:
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
||||
private:
|
||||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index=0;
|
||||
uint8_t current=0;
|
||||
num_robot_count(){}
|
||||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index = 0;
|
||||
uint8_t current = 0;
|
||||
num_robot_count(){}
|
||||
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
|
||||
|
||||
}; typedef struct num_robot_count Num_robot_count ;
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ; // not useful in cpp
|
||||
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ;
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step=0;
|
||||
int robot_id=0;
|
||||
std::string robot_name = "";
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step=0;
|
||||
int robot_id=0;
|
||||
std::string robot_name = "";
|
||||
//int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int message_number=0;
|
||||
uint8_t no_of_robots=0;
|
||||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=0;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
|
||||
std::string stream_client_name;
|
||||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
Num_robot_count count_robots;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
ros::Subscriber current_position_sub;
|
||||
ros::Subscriber users_sub;
|
||||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
ros::Subscriber relative_altitude_sub;
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int message_number=0;
|
||||
uint8_t no_of_robots=0;
|
||||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=0 ;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
|
||||
std::string stream_client_name;
|
||||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
Num_robot_count count_robots;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
ros::Subscriber current_position_sub;
|
||||
ros::Subscriber users_sub;
|
||||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
ros::Subscriber relative_altitude_sub;
|
||||
|
||||
std::string local_pos_sub_name;
|
||||
ros::Subscriber local_pos_sub;
|
||||
double local_pos_new[3];
|
||||
std::string local_pos_sub_name;
|
||||
ros::Subscriber local_pos_sub;
|
||||
double local_pos_new[3];
|
||||
|
||||
|
||||
ros::ServiceClient stream_client;
|
||||
ros::ServiceClient stream_client;
|
||||
|
||||
int setpoint_counter;
|
||||
double my_x = 0, my_y = 0;
|
||||
int setpoint_counter;
|
||||
double my_x = 0, my_y = 0;
|
||||
|
||||
std::ofstream log;
|
||||
std::ofstream log;
|
||||
|
||||
/*Commands for flight controller*/
|
||||
//mavros_msgs::CommandInt cmd_srv;
|
||||
mavros_msgs::CommandLong cmd_srv;
|
||||
std::vector<std::string> m_sMySubscriptions;
|
||||
std::map<std::string, std::string> m_smTopic_infos;
|
||||
/*Commands for flight controller*/
|
||||
//mavros_msgs::CommandInt cmd_srv;
|
||||
mavros_msgs::CommandLong cmd_srv;
|
||||
std::vector<std::string> m_sMySubscriptions;
|
||||
std::map<std::string, std::string> m_smTopic_infos;
|
||||
|
||||
mavros_msgs::CommandBool m_cmdBool;
|
||||
ros::ServiceClient arm_client;
|
||||
mavros_msgs::CommandBool m_cmdBool;
|
||||
ros::ServiceClient arm_client;
|
||||
|
||||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
ros::ServiceClient mode_client;
|
||||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
ros::ServiceClient mode_client;
|
||||
|
||||
/*Initialize publisher and subscriber, done in the constructor*/
|
||||
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
||||
/*Initialize publisher and subscriber, done in the constructor*/
|
||||
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
||||
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||
|
||||
/*compiles buzz script from the specified .bzz file*/
|
||||
std::string Compile_bzz(std::string bzzfile_name);
|
||||
/*compiles buzz script from the specified .bzz file*/
|
||||
std::string Compile_bzz(std::string bzzfile_name);
|
||||
|
||||
/*Flight controller service call*/
|
||||
void flight_controller_service_call();
|
||||
/*Flight controller service call*/
|
||||
void flight_controller_service_call();
|
||||
|
||||
/*Neighbours pos publisher*/
|
||||
void neighbours_pos_publisher();
|
||||
/*Neighbours pos publisher*/
|
||||
void neighbours_pos_publisher();
|
||||
|
||||
/*Prepare messages and publish*/
|
||||
void prepare_msg_and_publish();
|
||||
/*Prepare messages and publish*/
|
||||
void prepare_msg_and_publish();
|
||||
|
||||
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
void maintain_pos(int tim_step);
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
void maintain_pos(int tim_step);
|
||||
|
||||
/*Puts neighbours position inside neigbours_pos_map*/
|
||||
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
/*Puts neighbours position inside neigbours_pos_map*/
|
||||
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
|
||||
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
void gps_rb(GPS nei_pos, double out[]);
|
||||
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
|
||||
void gps_ned_home(float &ned_x, float &ned_y);
|
||||
void gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
void gps_rb(GPS nei_pos, double out[]);
|
||||
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
|
||||
void gps_ned_home(float &ned_x, float &ned_y);
|
||||
void gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
|
||||
/*battery status callback*/
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
/*battery status callback */
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
||||
/*flight extended status callback*/
|
||||
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
/*flight extended status callback*/
|
||||
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
|
||||
/*flight status callback*/
|
||||
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
||||
/*flight status callback*/
|
||||
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
||||
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
void users_pos(const rosbuzz::neigh_pos msg);
|
||||
|
||||
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
|
||||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
|
||||
/* RC commands service */
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
||||
/* RC commands service */
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
||||
|
||||
/*robot id sub callback*/
|
||||
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
||||
/*robot id sub callback*/
|
||||
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
||||
|
||||
/*Obstacle distance table callback*/
|
||||
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
/*Obstacle distance table callback*/
|
||||
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
|
||||
/*Get publisher and subscriber from YML file*/
|
||||
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
||||
/*Get publisher and subscriber from YML file*/
|
||||
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
||||
|
||||
/*Arm/disarm method that can be called from buzz*/
|
||||
void Arm();
|
||||
/*Arm/disarm method that can be called from buzz*/
|
||||
void Arm();
|
||||
|
||||
/*set mode like guided for solo*/
|
||||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
/*set mode like guided for solo*/
|
||||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
|
||||
/*Robot independent subscribers*/
|
||||
void Subscribe(ros::NodeHandle& n_c);
|
||||
/*Robot independent subscribers*/
|
||||
void Subscribe(ros::NodeHandle& n_c);
|
||||
|
||||
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
||||
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
||||
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
|
||||
void fc_command_setup();
|
||||
void fc_command_setup();
|
||||
|
||||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
|
||||
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
|
||||
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
|
||||
|
||||
void SetStreamRate(int id, int rate, int on_off);
|
||||
void SetStreamRate(int id, int rate, int on_off);
|
||||
|
||||
void get_number_of_robots();
|
||||
|
||||
void GetRobotId();
|
||||
bool GetDequeFull(bool &result);
|
||||
bool GetRssi(float &result);
|
||||
bool TriggerAPIRssi(const uint8_t short_id);
|
||||
bool GetAPIRssi(const uint8_t short_id, float &result);
|
||||
bool GetRawPacketLoss(const uint8_t short_id, float &result);
|
||||
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
|
||||
|
||||
void get_number_of_robots();
|
||||
void GetRobotId();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -414,7 +414,7 @@ int create_stig_tables() {
|
|||
//buzzvm_gstore(VM);
|
||||
//buzzvm_dump(VM);
|
||||
|
||||
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||
buzzvm_tget(VM);
|
||||
|
|
@ -785,5 +785,3 @@ int create_stig_tables() {
|
|||
buzzvm_gstore(VM);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -11,423 +11,428 @@
|
|||
|
||||
namespace buzzuav_closures{
|
||||
|
||||
// TODO: Minimize the required global variables and put them in the header
|
||||
//static const rosbzz_node::roscontroller* roscontroller_ptr;
|
||||
/*forward declaration of ros controller ptr storing function*/
|
||||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
|
||||
static double goto_pos[3];
|
||||
static double rc_goto_pos[3];
|
||||
static float batt[3];
|
||||
static float obst[5]={0,0,0,0,0};
|
||||
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;
|
||||
// TODO: Minimize the required global variables and put them in the header
|
||||
//static const rosbzz_node::roscontroller* roscontroller_ptr;
|
||||
/*forward declaration of ros controller ptr storing function*/
|
||||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
|
||||
static double goto_pos[3];
|
||||
static double rc_goto_pos[3];
|
||||
static float batt[3];
|
||||
static float obst[5] = {0,0,0,0,0};
|
||||
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;
|
||||
|
||||
|
||||
std::map< int, buzz_utility::Pos_struct> neighbors_map;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbors_map;
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
int buzzros_print(buzzvm_t vm) {
|
||||
int i;
|
||||
char buffer [50] = "";
|
||||
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
|
||||
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:
|
||||
sprintf(buffer,"%s BUZZ - [nil]", buffer);
|
||||
break;
|
||||
case BUZZTYPE_INT:
|
||||
sprintf(buffer,"%s %d", buffer, o->i.value);
|
||||
//fprintf(stdout, "%d", o->i.value);
|
||||
break;
|
||||
case BUZZTYPE_FLOAT:
|
||||
sprintf(buffer,"%s %f", buffer, o->f.value);
|
||||
break;
|
||||
case BUZZTYPE_TABLE:
|
||||
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
|
||||
break;
|
||||
case BUZZTYPE_CLOSURE:
|
||||
if(o->c.value.isnative)
|
||||
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
|
||||
else
|
||||
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
|
||||
break;
|
||||
case BUZZTYPE_STRING:
|
||||
sprintf(buffer,"%s %s", buffer, o->s.value.str);
|
||||
break;
|
||||
case BUZZTYPE_USERDATA:
|
||||
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
ROS_INFO(buffer);
|
||||
//fprintf(stdout, "\n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
int buzzros_print(buzzvm_t vm)
|
||||
{
|
||||
int i;
|
||||
char buffer [50] = "";
|
||||
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
|
||||
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:
|
||||
sprintf(buffer,"%s BUZZ - [nil]", buffer);
|
||||
break;
|
||||
case BUZZTYPE_INT:
|
||||
sprintf(buffer,"%s %d", buffer, o->i.value);
|
||||
//fprintf(stdout, "%d", o->i.value);
|
||||
break;
|
||||
case BUZZTYPE_FLOAT:
|
||||
sprintf(buffer,"%s %f", buffer, o->f.value);
|
||||
break;
|
||||
case BUZZTYPE_TABLE:
|
||||
sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
|
||||
break;
|
||||
case BUZZTYPE_CLOSURE:
|
||||
if(o->c.value.isnative)
|
||||
sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
|
||||
else
|
||||
sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
|
||||
break;
|
||||
case BUZZTYPE_STRING:
|
||||
sprintf(buffer,"%s %s", buffer, o->s.value.str);
|
||||
break;
|
||||
case BUZZTYPE_USERDATA:
|
||||
sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
ROS_INFO("%s",buffer);
|
||||
//fprintf(stdout, "\n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
/ Compute GPS destination from current position and desired Range and Bearing
|
||||
/----------------------------------------*/
|
||||
/*----------------------------------------/
|
||||
/ Compute GPS destination from current position and desired Range and Bearing
|
||||
/----------------------------------------*/
|
||||
|
||||
void gps_from_rb(double range, double bearing, double out[3]) {
|
||||
double lat = cur_pos[0]*M_PI/180.0;
|
||||
double lon = cur_pos[1]*M_PI/180.0;
|
||||
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
|
||||
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
|
||||
out[0] = out[0]*180.0/M_PI;
|
||||
out[1] = out[1]*180.0/M_PI;
|
||||
out[2] = height; //constant height.
|
||||
}
|
||||
void gps_from_rb(double range, double bearing, double out[3]) {
|
||||
double lat = cur_pos[0]*M_PI/180.0;
|
||||
double lon = cur_pos[1]*M_PI/180.0;
|
||||
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
|
||||
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
|
||||
out[0] = out[0]*180.0/M_PI;
|
||||
out[1] = out[1]*180.0/M_PI;
|
||||
out[2] = height; //constant height.
|
||||
}
|
||||
|
||||
void rb_from_gps(double nei[], double out[], double cur[]){
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
|
||||
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
out[1] = atan2(ned_y,ned_x);
|
||||
out[2] = 0.0;
|
||||
}
|
||||
void rb_from_gps(double nei[], double out[], double cur[]){
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
|
||||
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
out[1] = atan2(ned_y,ned_x);
|
||||
out[2] = 0.0;
|
||||
}
|
||||
|
||||
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
|
||||
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};
|
||||
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
|
||||
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};
|
||||
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
int buzzuav_moveto(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 2);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
//buzzvm_lload(vm, 3); /* Latitude */
|
||||
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
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;
|
||||
double d = sqrt(dx*dx+dy*dy); //range
|
||||
goto_pos[0]=dx;
|
||||
goto_pos[1]=dy;
|
||||
goto_pos[2]=height;
|
||||
/*double b = atan2(dy,dx); //bearing
|
||||
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
|
||||
gps_from_rb(d, b, goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
int buzzuav_moveto(buzzvm_t vm)
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 2);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
//buzzvm_lload(vm, 3); /* Latitude */
|
||||
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
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;
|
||||
// The compiler warnings are useful!
|
||||
// double d = sqrt(dx*dx+dy*dy); //range
|
||||
goto_pos[0]=dx;
|
||||
goto_pos[1]=dy;
|
||||
goto_pos[2]=height;
|
||||
/*double b = atan2(dy,dx); //bearing
|
||||
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
|
||||
gps_from_rb(d, b, goto_pos);
|
||||
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]);
|
||||
buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
//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
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
|
||||
if(vm->state != BUZZVM_STATE_READY) return vm->state;
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
|
||||
buzzvm_gload(vm);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
|
||||
buzzobj_t nbr = buzzvm_stack_at(vm, 1);
|
||||
/* Get "data" field */
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
|
||||
buzzvm_tget(vm);
|
||||
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
|
||||
//ROS_INFO("Empty data, create a new table");
|
||||
buzzvm_pop(vm);
|
||||
buzzvm_push(vm, nbr);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzobj_t data = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, data);
|
||||
}
|
||||
/* When we get here, the "data" table is on top of the stack */
|
||||
/* Push user id */
|
||||
buzzvm_pushi(vm, id);
|
||||
/* Create entry table */
|
||||
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
|
||||
/* Insert range */
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
|
||||
buzzvm_pushf(vm, range);
|
||||
buzzvm_tput(vm);
|
||||
/* Insert longitude */
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
|
||||
buzzvm_pushf(vm, bearing);
|
||||
buzzvm_tput(vm);
|
||||
/* Save entry into data table */
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_tput(vm);
|
||||
//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
|
||||
return vm->state;
|
||||
}
|
||||
int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
|
||||
if(vm->state != BUZZVM_STATE_READY) return vm->state;
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
|
||||
buzzvm_gload(vm);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
|
||||
buzzobj_t nbr = buzzvm_stack_at(vm, 1);
|
||||
/* Get "data" field */
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
|
||||
buzzvm_tget(vm);
|
||||
if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
|
||||
//ROS_INFO("Empty data, create a new table");
|
||||
buzzvm_pop(vm);
|
||||
buzzvm_push(vm, nbr);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzobj_t data = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, data);
|
||||
}
|
||||
/* When we get here, the "data" table is on top of the stack */
|
||||
/* Push user id */
|
||||
buzzvm_pushi(vm, id);
|
||||
/* Create entry table */
|
||||
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
|
||||
/* Insert range */
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
|
||||
buzzvm_pushf(vm, range);
|
||||
buzzvm_tput(vm);
|
||||
/* Insert longitude */
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
|
||||
buzzvm_pushf(vm, bearing);
|
||||
buzzvm_tput(vm);
|
||||
/* Save entry into data table */
|
||||
buzzvm_push(vm, entry);
|
||||
buzzvm_tput(vm);
|
||||
//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
|
||||
return vm->state;
|
||||
}
|
||||
|
||||
int buzzuav_adduserRB(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); /* longitude */
|
||||
buzzvm_lload(vm, 2); /* latitude */
|
||||
buzzvm_lload(vm, 3); /* id */
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
double tmp[3];
|
||||
tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
|
||||
tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
|
||||
tmp[2] = 0.0;
|
||||
int uid = buzzvm_stack_at(vm, 3)->i.value;
|
||||
double rb[3];
|
||||
int buzzuav_adduserRB(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); /* longitude */
|
||||
buzzvm_lload(vm, 2); /* latitude */
|
||||
buzzvm_lload(vm, 3); /* id */
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
double tmp[3];
|
||||
tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
|
||||
tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
|
||||
tmp[2] = 0.0;
|
||||
int uid = buzzvm_stack_at(vm, 3)->i.value;
|
||||
double rb[3];
|
||||
|
||||
rb_from_gps(tmp, rb, cur_pos);
|
||||
if(fabs(rb[0])<100.0) {
|
||||
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
|
||||
return users_add2localtable(vm, uid, rb[0], rb[1]);
|
||||
} else
|
||||
printf(" ---------- User too far %f\n",rb[0]);
|
||||
rb_from_gps(tmp, rb, cur_pos);
|
||||
if(fabs(rb[0])<100.0) {
|
||||
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
|
||||
return users_add2localtable(vm, uid, rb[0], rb[1]);
|
||||
} else
|
||||
printf(" ---------- User too far %f\n",rb[0]);
|
||||
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to go directly to a GPS destination from the Mission Planner
|
||||
/----------------------------------------*/
|
||||
int buzzuav_goto(buzzvm_t vm) {
|
||||
rc_goto_pos[2]=height;
|
||||
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=COMMAND_GOTO;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to go directly to a GPS destination from the Mission Planner
|
||||
/----------------------------------------*/
|
||||
int buzzuav_goto(buzzvm_t vm) {
|
||||
rc_goto_pos[2]=height;
|
||||
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=COMMAND_GOTO;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
|
||||
/----------------------------------------*/
|
||||
int buzzuav_arm(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
|
||||
printf(" Buzz requested Arm \n");
|
||||
buzz_cmd=COMMAND_ARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
int buzzuav_disarm(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
|
||||
printf(" Buzz requested Disarm \n");
|
||||
buzz_cmd=COMMAND_DISARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
|
||||
/----------------------------------------*/
|
||||
int buzzuav_arm(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
|
||||
printf(" Buzz requested Arm \n");
|
||||
buzz_cmd=COMMAND_ARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
int buzzuav_disarm(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
|
||||
printf(" Buzz requested Disarm \n");
|
||||
buzz_cmd=COMMAND_DISARM;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
/*---------------------------------------/
|
||||
/ Buss closure for basic UAV commands
|
||||
/---------------------------------------*/
|
||||
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 = COMMAND_TAKEOFF;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
/*---------------------------------------/
|
||||
/ Buss closure for basic UAV commands
|
||||
/---------------------------------------*/
|
||||
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 = COMMAND_TAKEOFF;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
int buzzuav_land(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||
printf(" Buzz requested Land !!! \n");
|
||||
buzz_cmd = COMMAND_LAND;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
int buzzuav_land(buzzvm_t vm) {
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||
printf(" Buzz requested Land !!! \n");
|
||||
buzz_cmd = COMMAND_LAND;
|
||||
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 = COMMAND_GOHOME;
|
||||
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 = COMMAND_GOHOME;
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
||||
/*-------------------------------
|
||||
/ Get/Set to transfer variable from Roscontroller to buzzuav
|
||||
/------------------------------------*/
|
||||
double* getgoto() {
|
||||
return goto_pos;
|
||||
}
|
||||
/*-------------------------------
|
||||
/ Get/Set to transfer variable from Roscontroller to buzzuav
|
||||
/------------------------------------*/
|
||||
double* getgoto() {
|
||||
return goto_pos;
|
||||
}
|
||||
|
||||
int getcmd() {
|
||||
return cur_cmd;
|
||||
}
|
||||
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];
|
||||
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;
|
||||
}
|
||||
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_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;
|
||||
}
|
||||
}
|
||||
void rc_call(int rc_cmd_in) {
|
||||
rc_cmd = rc_cmd_in;
|
||||
}
|
||||
|
||||
void set_obstacle_dist(float dist[]) {
|
||||
for (int i = 0; i < 5; i++)
|
||||
obst[i] = dist[i];
|
||||
}
|
||||
void set_obstacle_dist(float dist[]) {
|
||||
for (int i = 0; i < 5; i++)
|
||||
obst[i] = dist[i];
|
||||
}
|
||||
|
||||
void set_battery(float voltage,float current,float remaining){
|
||||
batt[0]=voltage;
|
||||
batt[1]=current;
|
||||
batt[2]=remaining;
|
||||
}
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
/*adds neighbours position*/
|
||||
void neighbour_pos_callback(int id, float range, float bearing, float elevation){
|
||||
buzz_utility::Pos_struct pos_arr;
|
||||
pos_arr.x=range;
|
||||
pos_arr.y=bearing;
|
||||
pos_arr.z=elevation;
|
||||
map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
|
||||
if(it!=neighbors_map.end())
|
||||
neighbors_map.erase(it);
|
||||
neighbors_map.insert(make_pair(id, pos_arr));
|
||||
}
|
||||
int buzzuav_update_battery(buzzvm_t vm) {
|
||||
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;
|
||||
}
|
||||
/*adds neighbours position*/
|
||||
void neighbour_pos_callback(int id, float range, float bearing, float elevation){
|
||||
buzz_utility::Pos_struct pos_arr;
|
||||
pos_arr.x=range;
|
||||
pos_arr.y=bearing;
|
||||
pos_arr.z=elevation;
|
||||
map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
|
||||
if(it!=neighbors_map.end())
|
||||
neighbors_map.erase(it);
|
||||
neighbors_map.insert(make_pair(id, pos_arr));
|
||||
}
|
||||
|
||||
/* update at each step the VM table */
|
||||
void update_neighbors(buzzvm_t vm){
|
||||
/* Reset neighbor information */
|
||||
buzzneighbors_reset(vm);
|
||||
/* Get robot id and update neighbor information */
|
||||
map< int, buzz_utility::Pos_struct >::iterator it;
|
||||
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
|
||||
buzzneighbors_add(vm,
|
||||
it->first,
|
||||
(it->second).x,
|
||||
(it->second).y,
|
||||
(it->second).z);
|
||||
}
|
||||
}
|
||||
/* update at each step the VM table */
|
||||
void update_neighbors(buzzvm_t vm){
|
||||
/* Reset neighbor information */
|
||||
buzzneighbors_reset(vm);
|
||||
/* Get robot id and update neighbor information */
|
||||
map< int, buzz_utility::Pos_struct >::iterator it;
|
||||
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
|
||||
buzzneighbors_add(vm,
|
||||
it->first,
|
||||
(it->second).x,
|
||||
(it->second).y,
|
||||
(it->second).z);
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
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;
|
||||
}
|
||||
/****************************************/
|
||||
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;
|
||||
}
|
||||
|
||||
void flight_status_update(uint8_t state){
|
||||
status=state;
|
||||
}
|
||||
void flight_status_update(uint8_t state){
|
||||
status=state;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------
|
||||
/ Create the generic robot table with status, remote controller current comand and destination
|
||||
/ and current position of the robot
|
||||
/ TODO: change global name for robot
|
||||
/------------------------------------------------------*/
|
||||
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;
|
||||
}
|
||||
/*----------------------------------------------------
|
||||
/ Create the generic robot table with status, remote controller current comand and destination
|
||||
/ and current position of the robot
|
||||
/ TODO: change global name for robot
|
||||
/------------------------------------------------------*/
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/******************************************************/
|
||||
/*Create an obstacle Buzz table from proximity sensors*/
|
||||
/* Acessing proximity in buzz script
|
||||
proximity[0].angle and proximity[0].value - front
|
||||
"" "" "" - right and back
|
||||
proximity[3].angle and proximity[3].value - left
|
||||
proximity[4].angle = -1 and proximity[4].value -bottom */
|
||||
/******************************************************/
|
||||
/******************************************************/
|
||||
/*Create an obstacle Buzz table from proximity sensors*/
|
||||
/* Acessing proximity in buzz script
|
||||
proximity[0].angle and proximity[0].value - front
|
||||
"" "" "" - right and back
|
||||
proximity[3].angle and proximity[3].value - left
|
||||
proximity[4].angle = -1 and proximity[4].value -bottom */
|
||||
/******************************************************/
|
||||
|
||||
int buzzuav_update_prox(buzzvm_t vm) {
|
||||
int buzzuav_update_prox(buzzvm_t vm) {
|
||||
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_gstore(vm);
|
||||
|
|
@ -441,80 +446,80 @@ namespace buzzuav_closures{
|
|||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[i+1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[i+1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/* Store read table in the proximity table */
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
angle+=1.5708;
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
angle+=1.5708;
|
||||
}
|
||||
/* Create table for bottom read */
|
||||
angle =-1;
|
||||
/* Create table for bottom read */
|
||||
angle =-1;
|
||||
buzzvm_pusht(vm);
|
||||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/*Store read table in the proximity table*/
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, 4);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/*Store read table in the proximity table*/
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, 4);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
|
||||
/*
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
|
||||
buzzvm_pushf(vm, obst[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
|
||||
buzzvm_pushf(vm, obst[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
|
||||
buzzvm_pushf(vm, obst[3]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
|
||||
buzzvm_pushf(vm, obst[4]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);*/
|
||||
return vm->state;
|
||||
}
|
||||
/*
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
|
||||
buzzvm_pushf(vm, obst[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
|
||||
buzzvm_pushf(vm, obst[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
|
||||
buzzvm_pushf(vm, obst[3]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
|
||||
buzzvm_pushf(vm, obst[4]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);*/
|
||||
return vm->state;
|
||||
}
|
||||
|
||||
/**********************************************/
|
||||
/*Dummy closure for use during update testing */
|
||||
/**********************************************/
|
||||
/**********************************************/
|
||||
/*Dummy closure for use during update testing */
|
||||
/**********************************************/
|
||||
|
||||
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
|
||||
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
|
||||
|
||||
/***********************************************/
|
||||
/* Store Ros controller object pointer */
|
||||
/***********************************************/
|
||||
/***********************************************/
|
||||
/* Store Ros controller object pointer */
|
||||
/***********************************************/
|
||||
|
||||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
|
||||
//roscontroller_ptr = roscontroller_ptrin;
|
||||
//}
|
||||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
|
||||
//roscontroller_ptr = roscontroller_ptrin;
|
||||
//}
|
||||
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue