diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index a4746af..c708df9 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2.0 # m/steps +GOTO_MAXVEL = 0.2 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. diff --git a/include/roscontroller.h b/include/roscontroller.h index 9713a9d..f1604f0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -104,8 +104,8 @@ private: uint16_t nid; uint16_t size; double sent_time; - double received_time; - MsgData(int mi, uint16_t ni, uint16_t s, double st, double rt): + uint64_t received_time; + MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; MsgData(){}; }; @@ -122,7 +122,7 @@ private: ros::Time logical_clock; ros::Time previous_step_time; std::vector inmsgdata; - double out_msg_time; + uint64_t out_msg_time; double logical_time_rate; bool time_sync_jumped; std::string robot_name = ""; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ba440e6..2441720 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -176,25 +176,25 @@ void roscontroller::RosControllerRun() log< 0)log<<","; + // if(neighbours_pos_map.size() > 0)log<<","; map::iterator it = neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { log<< it->first<<","; - log << (double)it->second.x << "," << (double)it->second.y + log <<","<< (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ @@ -654,7 +654,7 @@ with size......... | / } //Store out msg time get_logical_time(); - out_msg_time = logical_clock.toSec(); + out_msg_time = logical_clock.toNSec(); // publish prepared messages in respective topic payload_pub.publish(payload_out); delete[] out; @@ -1171,7 +1171,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); get_logical_time(); // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toNSec()); inmsgdata.push_back(inm); if (debug)