SyncManager.cpp 10 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355
  1. #include "stdafx.h"
  2. #include"SyncManager.h"
  3. #include <deque>
  4. #include <Eigen/Dense>
  5. #include <fstream>
  6. #include <iostream>
  7. #include <direct.h>
  8. #include <io.h>
  9. #include "../ProcessRemodule.h"
  10. #include "../log_process_module.h"
  11. #include "./../system_basic_info/SystemAnalysis.h"
  12. #include "Functions/Functions.h"
  13. #pragma warning(disable: 4244)
  14. HostServer::SyncManager::SyncManager()
  15. {
  16. init();
  17. }
  18. void HostServer::SyncManager::init()
  19. {
  20. InitializeCriticalSectionAndSpinCount(&m_csSyncTime, 4000);
  21. _anchors.swap(unordered_map<unsigned long long, Position>());
  22. _distance.swap(unordered_map<unsigned long long,unordered_map<unsigned long long, double>>());
  23. std::string dir = "";
  24. TCHAR chpath[MAX_PATH];
  25. ::GetModuleFileName(NULL,chpath, MAX_PATH);
  26. (_tcsrchr(chpath, _T('\\')))[1] = 0;
  27. dir = CFunctions::TCHAR2string(chpath);
  28. logDir = dir + "\\synclog\\";
  29. if(0 != _access(logDir.c_str(), 0)){
  30. _mkdir(logDir.c_str());
  31. }
  32. isOutputLog = false;
  33. }
  34. void HostServer::SyncManager::analyzeSyncMsg(SyncTimeMsg &msg)
  35. {
  36. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_10);
  37. EnterCriticalSection(&m_csSyncTime);
  38. int idx = FindSyncTimeMsg(msg.RootIdCode(), msg.SyncNum());
  39. if( -1 == idx){ // 没找到
  40. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_72);
  41. // 如果时间同步消息的版本数量超过最大限制,则删除最早添加的消息
  42. if(_syncTimeMsgs[msg.RootIdCode()].size() >= MAX_SYNCTIME_NUM){ // 删除第一个
  43. // 可能有泄露
  44. _syncTimeMsgs[msg.RootIdCode()].pop_front();
  45. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_12);
  46. }
  47. SyncTimeMsgItem it;
  48. it.SyncNum = msg.SyncNum();
  49. it.SyncTimeMsgs[msg.LocalIdCode()] = msg;
  50. _syncTimeMsgs[msg.RootIdCode()].push_back(it);
  51. idx = _syncTimeMsgs[msg.RootIdCode()].size() - 1;
  52. }else{
  53. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_73);
  54. _syncTimeMsgs[msg.RootIdCode()][idx].SyncTimeMsgs[msg.LocalIdCode()] = msg;
  55. }
  56. //
  57. while(_historySync[msg.RootIdCode()].size() > MAX_SYNCTIME_NUM){
  58. _historySync[msg.RootIdCode()].pop_front();
  59. }
  60. // 更新时间同步并计算
  61. for(auto it :_syncTimeMsgs[msg.RootIdCode()][idx].SyncTimeMsgs)
  62. {
  63. updateSync(msg.RootIdCode(), idx, msg.SyncNum(), it.first);
  64. }
  65. LeaveCriticalSection(&m_csSyncTime);
  66. }
  67. bool HostServer::SyncManager::updateSync(unsigned long long rootIdCode, int idx, unsigned short SyncNum, unsigned long long localIdCode)
  68. {
  69. if(-1 == idx){
  70. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_74);
  71. return false;
  72. }
  73. // 如果当前版本的时间同步消息未收到则返回false
  74. unordered_map<unsigned long long, SyncTimeMsg>::iterator itSyncTime = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(localIdCode);
  75. if(itSyncTime == _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.end()){
  76. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_75);
  77. return false;
  78. }
  79. SyncTimeMsg &msg = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[localIdCode];
  80. // 如果当前的为root,则返回true
  81. if(msg.SyncLevel() == 0){
  82. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_76);
  83. return true;
  84. }
  85. int idx_synctime = FindHisSyncTime(rootIdCode, SyncNum);
  86. // 如果时间同步已经计算过,则返回true
  87. if(-1 != idx_synctime){
  88. unordered_map<unsigned long long, SyncTime>::iterator itHistSync = _historySync[rootIdCode][idx_synctime].HistSync.find(localIdCode);
  89. if(itHistSync != _historySync[rootIdCode][idx_synctime].HistSync.end()){
  90. if(itHistSync->second.TimeDelay())
  91. return true;
  92. }
  93. }
  94. // 如果已经收到了上一级的同步消息
  95. if(_syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.count(msg.UpperIdCode())){
  96. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_77);
  97. if(!updateSync(rootIdCode, idx, SyncNum, msg.UpperIdCode())){
  98. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_78);
  99. return false;
  100. }
  101. SyncTimeMsg &upperMsg = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[msg.UpperIdCode()];
  102. SyncTime* s = NULL;
  103. for(auto it(_historySync[rootIdCode].rbegin()); it != _historySync[rootIdCode].rend(); ++it)
  104. {
  105. if(it->SyncNum != msg.SyncNum() && it->HistSync.count(localIdCode))
  106. {
  107. s = &(it->HistSync.find(localIdCode)->second);
  108. break;
  109. }
  110. }
  111. idx_synctime = FindHisSyncTime(rootIdCode, SyncNum);
  112. if(-1 == idx_synctime){
  113. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_79);
  114. SyncTimeItem it;
  115. it.SyncNum = msg.SyncNum();
  116. it.HistSync[localIdCode] = SyncTime(msg, upperMsg, s);
  117. _historySync[rootIdCode].push_back(it);
  118. idx_synctime = _historySync[rootIdCode].size() - 1;
  119. }else{
  120. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_80);
  121. _historySync[rootIdCode][idx_synctime].HistSync[localIdCode] = SyncTime(msg, upperMsg, s);
  122. }
  123. // 计算时间同步
  124. long long upperTimeDelay = 0;
  125. if(_syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[msg.UpperIdCode()].SyncLevel() != 0)
  126. {
  127. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_81);
  128. upperTimeDelay = _historySync[rootIdCode][idx_synctime].HistSync[msg.UpperIdCode()].TimeDelay();
  129. }
  130. long long sendTime = _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].SendTime();
  131. long long receiveTime = _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].ReceiveTime();
  132. long long timeDelay = receiveTime - sendTime - _distance[localIdCode][msg.UpperIdCode()];
  133. timeDelay += upperTimeDelay;
  134. // 从不同的Upper来的同步数据,跨周期判断可能不正确,将时间差控制在一个周期内
  135. while(timeDelay > TIME_MAX){
  136. timeDelay -= TIME_MAX;
  137. }
  138. while(timeDelay + TIME_MAX < 0 ){
  139. timeDelay += TIME_MAX;
  140. }
  141. _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].TimeDelay(timeDelay);
  142. if(isOutputLog){
  143. char filename[100];
  144. char temp[200];
  145. long long aa = timeDelay;
  146. if(aa < 0){
  147. aa += TIME_MAX;
  148. }
  149. int bb = (receiveTime > TIME_MAX) ? 1 : 0;
  150. int cc = (sendTime > TIME_MAX) ? 1 : 0;
  151. int dd = msg.UpperIdCode() >> 8;
  152. sprintf_s(filename, "%s\\%d.log", logDir.c_str(), localIdCode>>8);
  153. ofstream outfile(filename, ofstream::app);
  154. sprintf_s(temp, "sn:%d, up:%d, rec:%I64d, snd:%I64d, del:%I64d, crt:%I64d, r_crs:%d, s_crs:%d\r", SyncNum, dd, receiveTime, sendTime, timeDelay, aa, bb, cc);
  155. outfile << temp;
  156. }
  157. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_69);
  158. return true;
  159. }else{
  160. LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_110);
  161. }
  162. return false;
  163. }
  164. unsigned long long HostServer::SyncManager::calTimeByLinar(TagMsg &tag)
  165. {
  166. EnterCriticalSection(&m_csSyncTime);
  167. // 获取历史记录中与此tag最近的两条
  168. deque<SyncTime> hisSync;
  169. unsigned long long rootIdCode = tag.SyncRootIdCode;
  170. int i = 0;
  171. int idx = FindSyncTimeMsg(rootIdCode, tag.SyncNum);
  172. if(-1 != idx){
  173. unordered_map<unsigned long long, SyncTimeMsg>::iterator it = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(tag.StationIdCode);
  174. if(it != _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.end()){
  175. if(it->second.SyncLevel() == 0){
  176. LeaveCriticalSection(&m_csSyncTime);
  177. return tag.ReceiveTime;
  178. }
  179. }
  180. }
  181. int idx_sync = -1;
  182. while(hisSync.size() < 2 && i < MAX_CALCLINER_NUM)
  183. {
  184. auto syncNum = tag.SyncNum - i;
  185. idx_sync = FindHisSyncTime(rootIdCode, syncNum);
  186. if(-1 != idx_sync){
  187. if(_historySync[rootIdCode][idx_sync].HistSync.count(tag.StationIdCode)){
  188. hisSync.push_front(_historySync[rootIdCode][idx_sync].HistSync[tag.StationIdCode]);
  189. }
  190. }
  191. i++;
  192. }
  193. // 如果满足条件的历史记录不足两个则返回
  194. if(hisSync.size() < 2){
  195. LeaveCriticalSection(&m_csSyncTime);
  196. return LLONG_MAX;
  197. }
  198. // 计算预估值
  199. long long y1(hisSync.at(0).ReceiveTime() - hisSync.at(0).TimeDelay()),y2(hisSync.at(1).ReceiveTime() - hisSync.at(1).TimeDelay());
  200. long long x1(hisSync.at(0).RealReceiveTime()), x2(hisSync.at(1).RealReceiveTime()), x3(tag.ReceiveTime);
  201. LeaveCriticalSection(&m_csSyncTime);
  202. unsigned long long res;
  203. if(x1 > x2)
  204. {
  205. x2 += TIME_MAX;
  206. }
  207. if(x2 > x3)
  208. {
  209. x3 += TIME_MAX;
  210. }
  211. if(y1 < 0){ // 理论y值不能小于0
  212. y1 += TIME_MAX;
  213. }
  214. if(y2 < 0 ){
  215. y2 += TIME_MAX;
  216. }
  217. if(y1 > y2){
  218. y2 += TIME_MAX;
  219. }else if(y2-y1 > TIME_MAX){
  220. y2 -=TIME_MAX;
  221. }
  222. Eigen::Matrix3d a;
  223. a << x1 ,1 , 0,
  224. x2 ,1 , 0,
  225. x3, 1, -1;
  226. Eigen::Vector3d b(y1, y2, 0);
  227. Eigen::Vector3d X = a.colPivHouseholderQr().solve(b);
  228. res = X(2);
  229. res &= TIME_MAX;
  230. return res;
  231. }
  232. void HostServer::SyncManager::updateDistance(unsigned int localId, unsigned char localAntNum, unsigned int upperId, unsigned char uppderAntNum, double d)
  233. {
  234. unsigned long long lId = SyncHelper::parseId(localId, localAntNum);
  235. unsigned long long uId = SyncHelper::parseId(upperId, uppderAntNum);
  236. if(_anchors.count(lId) == 0)
  237. {
  238. _anchors[lId] = Position();
  239. }
  240. if(_anchors.count(uId) == 0)
  241. {
  242. _anchors[uId] = Position();
  243. }
  244. _distance[lId][uId] = d;
  245. _distance[uId][lId] = d;
  246. // 删除所有版本的消息记录
  247. _historySync.clear();
  248. _syncTimeMsgs.clear();
  249. }
  250. void HostServer::SyncManager::updateAnchor(unsigned int localId, unsigned char localAntNum, double x, double y, double z)
  251. {
  252. deleteAnchor(localId, localAntNum);
  253. unsigned long long lId = SyncHelper::parseId(localId, localAntNum);
  254. _anchors[lId] = Position( x, y, z);
  255. }
  256. void HostServer::SyncManager::deleteAnchor(unsigned int localId, unsigned char localAntNum)
  257. {
  258. unsigned long long lId = SyncHelper::parseId(localId, localAntNum);
  259. auto it = _anchors.find(lId);
  260. if(it == _anchors.end()) return;
  261. // 删除anchor
  262. _anchors.erase(it);
  263. // 删除与此anchor相关的距离信息
  264. _distance.erase(_distance.find(lId));
  265. for(auto it(_distance.begin()); it != _distance.end(); it++)
  266. {
  267. it->second.erase(it->second.find(lId));
  268. if(it->second.size() == 0)
  269. {
  270. it = _distance.erase(it);
  271. }
  272. }
  273. // 删除所有版本的消息记录
  274. _historySync.clear();
  275. _syncTimeMsgs.clear();
  276. }
  277. HostServer::SyncManager::~SyncManager()
  278. {
  279. DeleteCriticalSection(&m_csSyncTime);
  280. }
  281. int HostServer::SyncManager::FindSyncTimeMsg(unsigned long long rootIdCode, unsigned short SyncNum )
  282. {
  283. int idx = -1;
  284. for(int i = _syncTimeMsgs[rootIdCode].size() - 1; i >= 0; i--){
  285. if(_syncTimeMsgs[rootIdCode][i].SyncNum == SyncNum ){
  286. return i;
  287. }
  288. }
  289. return idx;
  290. }
  291. int HostServer::SyncManager::FindHisSyncTime(unsigned long long rootIdCode, unsigned short SyncNum )
  292. {
  293. int idx = -1;
  294. for(int i = _historySync[rootIdCode].size() - 1; i >= 0; i--){
  295. if(_historySync[rootIdCode][i].SyncNum == SyncNum ){
  296. return i;
  297. }
  298. }
  299. return idx;
  300. }