SyncManager.cpp 10 KB

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