add solid ball function

card.cpp

@@ -424,6 +424,22 @@ void card_list::on_message(zloop<task*>* loop, message_pdoa_locinfo& loc, bool i
     c->on_message(loop, loc, is_history);
 }
 
+// 实心球项目数据处理
+void card_list::on_message(zloop<task*>* loop, message_ins_locinfo& loc, bool is_history)
+{
+	uint64_t cardid = tool_other::type_id_to_u64(loc.m_card_type, loc.m_card_id);
+	const auto &c = card_list::instance()->get(cardid);
+	if(!c)
+	{
+		log_warn("[pdoa] 数据库中未定义该卡的信息，card_id=%d, card_type=%d, cardid:%lld", loc.m_card_id, loc.m_card_type, cardid);
+		return;
+	}
+
+	// 是否使用原始定位算法，0为否，1为是
+	//loc.m_sync_ct = use_original_algo;
+    c->on_message(loop, loc, is_history);
+}
+
 //获取卡数据  //标识id 人staff_id 车 vehicle_id
 const std::shared_ptr<card_location_base> card_list::get_card_by_cid(int cid)
 {

card.h

@@ -24,6 +24,7 @@ struct card_list:single_base<card_list,uint64_t,std::shared_ptr<card_location_ba
     void on_message(zloop<task*> *loop,message_locinfo&loc,bool is_history);
     void on_message(zloop<task*> *loop, message_tdoa_locinfo& loc, bool is_history);
     void on_message(zloop<task*> *loop, message_pdoa_locinfo& loc, bool is_history);
+	void on_message(zloop<task*>* loop, message_ins_locinfo& loc, bool is_history);
 
     void init_card_from_db();
 

card_base.cpp

@@ -269,7 +269,34 @@ void card_location_base::on_message(zloop<task*>* loop, message_locinfo& loc,boo
  * */
 void card_location_base::on_message(zloop<task*>* loop, message_ins_locinfo& loc,bool is_history)
 {
+	/*m_ct = loc.m_card_ct;
+	m_time = loc.m_time_stamp;
+	auto site_ptr = sit_list::instance()->get(loc.m_site_id);
+	if(!site_ptr)
+	{
+		log_warn("接收到分站的数据，site=%d,card=%d,ct=%d,但是分站未定义",loc.m_site_id,m_id,loc.m_card_ct);
+		return;
+	}*/
+
+	//m_message_handle->on_message(loop, loc, is_history);
 	log_info("[solid_ball] start calc location");
+	
+	auto itm = m_msg_list.find(loc.m_card_ct);
+
+	if(itm == m_msg_list.end()){
+		std::vector<message_ins_locinfo> vtm;
+		vtm.push_back(loc);
+		m_msg_list.insert(std::make_pair(loc.m_card_ct, vtm));
+		itm = m_msg_list.find(loc.m_card_ct);
+	}else{
+		itm->second.push_back(loc);
+	}
+
+	if(itm->second.size() < 2){
+		return;
+	}
+
+	// ct切换了
 	m_ct = loc.m_card_ct;
 	m_time = loc.m_time_stamp;
 	auto site_ptr = sit_list::instance()->get(loc.m_site_id);
@@ -279,7 +306,97 @@ void card_location_base::on_message(zloop<task*>* loop, message_ins_locinfo& loc
 		return;
 	}
 
-	m_message_handle->on_message(loop, loc, is_history);
+	// 卡号
+	std::string sid = tool_byte::to_cardid(m_type, m_id);
+	std::string site_info = "";
+	std::string now = tool_time::to_str_ex(tool_time::now_to_ms());
+
+	// 存入数据库
+	for(auto itl : itm->second){
+		site_info += std::to_string(itl.m_site_id);
+		double d = itl.m_tof * 15.65 * 2.996 * 1E-4;
+		site_info += "," + std::to_string(d);
+		site_info += ",0.0";
+		double v = 0.0;
+		auto its = last_site_info.find(itl.m_site_id);
+		if(its == last_site_info.end()){
+			sinfo si;
+			si.sct = m_ct;
+			si.sdist = d;
+			last_site_info.insert(std::make_pair(itl.m_site_id, si));
+			v = 0.0;
+		}else{
+			double dt = 0.0;
+			if(m_ct > its->second.sct ){
+				dt = (m_ct - its->second.sct) / 1000.0;
+			}else{
+				dt = (m_ct + 65535 - its->second.sct) / 1000.0;
+			}
+			v = (d - its->second.sdist) / dt;
+
+			its->second.sct = m_ct; 	
+			its->second.sdist = d;
+		}
+		site_info += "," + std::to_string(v);
+		site_info += ";";
+	}
+	
+	// 数据入库
+	char sql[1024] = { 0 };
+	snprintf(sql, 
+			1024, 
+			"insert into his_raw_data_sb(card_id, x, y, z, speed, site_info, acc, acc_x, acc_y, acc_z, ang, ang_x, ang_y, ang_z, cur_time) values('%s', %.2f, %.2f, %.2f, %.2f, '%s', %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, '%s')",
+			sid.c_str(),
+			0.0,
+			0.0,
+			0.0,
+			0.0,
+			site_info.c_str(),
+			loc.m_ins_data.get_acc(),
+			loc.m_ins_data.get_acc(0),
+			loc.m_ins_data.get_acc(1),
+			loc.m_ins_data.get_acc(2),
+			loc.m_ins_data.get_gyro(),
+			loc.m_ins_data.get_gyro(0),
+			loc.m_ins_data.get_gyro(1),
+			loc.m_ins_data.get_gyro(2),
+			now.c_str()
+		);
+	db_tool::PushAsync(sql);
+    log_info("[sql] %s", sql);
+
+	// 更新数据到发送线程
+	sys::sb_data sd;
+	sd.cid = sid;
+	sd.speed = 0.0;
+	sd.site_info = site_info;
+	sd.acc = loc.m_ins_data.get_acc();
+	sd.ang = loc.m_ins_data.get_gyro();
+	std::copy(loc.m_ins_data.m_acc_data.begin(), loc.m_ins_data.m_acc_data.end(), sd.m_acc_data.begin());
+	std::copy(loc.m_ins_data.m_ang_data.begin(), loc.m_ins_data.m_ang_data.end(), sd.m_ang_data.begin());
+	sd.cur_time = now;
+
+	swsTimerThrd.upt_sb_data(std::move(sd));
+
+	logn_info(3, 
+			"[solid_ball] cid='%s', stamp=%d, sinfo='%s', acc = %.2f, acc_x = %.2f, acc_y = %.2f, acc_z = %.2f, ang = %.2f, ang_x = %.2f, ang_y = %.2f, ang_z =  %.2f, t = '%s')",
+			sid.c_str(),
+			m_ct,
+			site_info.c_str(),
+			loc.m_ins_data.get_acc(),
+			loc.m_ins_data.get_acc(0),
+			loc.m_ins_data.get_acc(1),
+			loc.m_ins_data.get_acc(2),
+			loc.m_ins_data.get_gyro(),
+			loc.m_ins_data.get_gyro(0),
+			loc.m_ins_data.get_gyro(1),
+			loc.m_ins_data.get_gyro(2),
+			now.c_str()
+	);
+
+	//清除数据
+	m_msg_list.erase(itm);
+	//m_message_handle->on_message(loop, loc, is_history);*/
 }
 
 /*

card_base.h

@@ -1,6 +1,7 @@
 #ifndef _CARD_BASE_HPP_
 #define _CARD_BASE_HPP_
 #include <vector>
+#include <map>
 #include <memory>
 #include <atomic>
 #include "point.h"
@@ -25,6 +26,7 @@ struct site;
 struct area_tool;
 struct message_tdoa_locinfo;
 struct message_pdoa_locinfo;
+struct message_ins_locinfo;
 
 namespace sys{
     struct _CARD_POS_;
@@ -98,6 +100,18 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
 	time_t m_help_last_time = 0;                  // 呼救持续时间
 	int    m_help_bit = 0;
 
+	// 基站对应的数据信息：距离+时间戳
+	struct sinfo{
+		int sct;
+		double sdist;
+
+		sinfo()
+			: sct(0)
+			, sdist(0.0)
+		{}
+	};
+	std::map<int, sinfo> last_site_info;
+
     //pdoa
     int m_last_ct           = -1;
     float m_last_dist           = 0.0;
@@ -122,6 +136,8 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
     bool m_enable_anti_collision = false;
     int m_call_level = 0;
 
+	std::map<int, std::vector<message_ins_locinfo>> m_msg_list;
+
     card_location_base()=default;
     card_location_base(const std::string&type,uint32_t id,uint16_t dis,int16_t t,int32_t,int32_t,uint32_t );
  
@@ -147,6 +163,7 @@ struct card_location_base:card,std::enable_shared_from_this<card_location_base>
     void on_message(zloop<task*>* loop, message_locinfo&loc, bool is_history);
     void on_message(zloop<task*>* loop, message_tdoa_locinfo& loc, bool is_history);
     void on_message(zloop<task*>* loop, message_pdoa_locinfo& loc, bool is_history);
+	void on_message(zloop<task*>* loop, message_ins_locinfo& loc, bool is_history);
 
 	void on_location(const std::vector<point>&vp, const std::vector<loc_message> &lm);
     void do_status(int st);

message.cpp

@@ -417,7 +417,7 @@ void message_ins_locinfo::zero_this()
     m_biz_type =
 	m_rssi = 0; 
     m_distance = 0.0;
-	m_ins_data.clear();
+	m_ins_data.reset();
 }
 
 
@@ -444,11 +444,12 @@ void message_ins_locinfo::load(zistream& is)
 	uint32_t tof = 0;
 	is >> bt >> tof;
 	m_tof = bt;
-	m_tof = (bt << 32) | tof;
+	m_tof = (m_tof << 32) | tof;
 
 	// 天线号，1字节
 	// 信号接收功率，1字节
-	is >> m_ant_id >> m_rssi;
+	is >> m_ant_id >> bt;
+	m_rssi = bt;
 	--m_ant_id;
 
 	// 加速度三轴，x,y,z
@@ -462,5 +463,7 @@ void message_ins_locinfo::load(zistream& is)
 		is >> id;
 		m_ins_data.m_ang_data[i] = id;
 	}
+
+	logn_info(3, "[solid_ball] cid=%d, ct=%d, tof=%d, ant_id=%d, rssi=%d, acc[0]=%d, acc[1]=%d, acc[2]=%d, ang[0]=%d, ang[1]=%d, ang[2]=%d", m_card_id, m_card_ct, tof, m_ant_id, m_rssi, m_ins_data.m_acc_data[0], m_ins_data.m_acc_data[1], m_ins_data.m_acc_data[2], m_ins_data.m_ang_data[0], m_ins_data.m_ang_data[1], m_ins_data.m_ang_data[2]);
 }
 

message.h

@@ -75,9 +75,9 @@ struct ins_data{
 			H = ~H;
 			L = (~L) + 0X01;
 
-			ret = (-(((H << 8) | L) / 1000.0);
+			ret = (-(((H << 8) | L) / 1000.0));
 		}else{
-			ret = ((((H << 8) | L) / 1000.0);
+			ret = ((((H << 8) | L) / 1000.0));
 		}
 
 		return ret;
@@ -256,7 +256,7 @@ struct message_ins_locinfo: public message_locinfo{
 
 	void zero_this();
 	void load(zistream& is);	
-}
+};
 
 struct message_light: task{
     uint32_t m_light_id;

module_service/module_solid_ball.cpp

@@ -0,0 +1,24 @@
+#include "solid_ball_module.h"
+#include "log.h"
+
+double solid_ball_module::get_distance(const point& p, const std::string& sid)
+{
+	//
+	return 0;
+}
+
+// 实心球业务处理逻辑，判断空中状态和出手时刻
+void solid_ball_module::do_business(std::shared_ptr<sb_card> ptr_card)
+{
+	// 检查卡是否存在，不存在则插入
+	auto it_card = m_card_list.find(ptr_card->m_sid);
+	if(it_card == m_card_list.end()){
+		m_card_list.insert(std::make_pair(ptr_card->m_sid, ptr_card));
+		it_card = m_card_list.find(ptr_card->m_sid);
+	}
+
+	//如果是落地状态，后面的业务不执行
+	if(ptr_card->m_status == sb_status::sb_land){
+		return;
+	}
+}

module_service/module_solid_ball.h

@@ -0,0 +1,26 @@
+#include <tpm_common.h>
+#include <boost/serialization/singleton.hpp>
+
+class solid_ball_module{
+	public:
+		std::shared_ptr<sb_card> get(const std::string& sid)
+		{
+			auto itc = m_card_list.find(sid);
+			if(itc == m_card_list.end())
+			{
+				return nullptr;
+			}
+			return itc->second;
+		}
+
+		int do_business(std::shared_ptr<sb_card> ptr_card);
+
+	public:
+		std::map<std::string, std::shared_ptr<sb_card>> m_card_list;
+	private:
+		double get_distance(const point& p, const std::string& sid);
+};
+
+//单件定义
+using singleton_solid_ball_module = boost::serialization::singleton<solid_ball_module>
+#define ssolid_ball_module singleton_solid_ball_module::get_mutable_instance()

module_service/tpm_common.h

@@ -0,0 +1,5 @@
+#ifndef tpm_common_hpp
+#define tpm_common_hpp
+
+
+#endif

net-service.cpp

@@ -204,46 +204,46 @@ void net_service::on_message(const std::shared_ptr<client> &clt,const char*data,
 				}
 				break;
 			case CHAR_LOCATEDATA_TOF_INS:
-				// 实心球项目中的硬件数据协议：带加速度和角速度6轴数据
-				int32_t site_id = parse_data_anchor_opt(clt, is);
-
-				if(site_id < 0){
-					break;
-				}
+				{
+					// 实心球项目中的硬件数据协议：带加速度和角速度6轴数据
+					int32_t site_id = parse_data_anchor_opt(clt, is);
 
-				const auto& site_ptr = sit_list::instance()->get(static_cast<int32_t>(site_id));
-				if(!site_ptr){
-					log_error("在全局分站列表中找不到分站：%d", site_id);
-					break;
-				}
+					if(site_id < 0){
+						break;
+					}
 
-				if(clt->type() != 2){
-					site_ptr->set_client(clt);
-				}
+					const auto& site_ptr = sit_list::instance()->get(static_cast<int32_t>(site_id));
+					if(!site_ptr){
+						log_error("在全局分站列表中找不到分站：%d", site_id);
+						break;
+					}
 
-				module_device_net::instance()->do_business(clt->name(), site_ptr->m_id, site_ptr->m_device_type_id);
-				site_ptr->set_algo(LDT_TOF);
+					if(clt->type() != 2){
+						site_ptr->set_client(clt);
+					}
 
-				struct timeval tv;
-				gettimeofday(&tv, NULL);
-				uint64_t cur_time = tv.tv_sec*1000 + tv.tv_usec/1000;
-				int index = 0;
-				while(!is.eof()){
-					uint64_t tstamp = cur_time - 950 + index*45;
-					++index;
+					module_device_net::instance()->do_business(clt->name(), site_ptr->m_id, site_ptr->m_device_type_id);
+					site_ptr->set_algo(LDT_TOF);
 
-					task* t = task::alloc<message_ins_locinfo>();
-					message_ins_locinfo& m = t->body<message_ins_locinfo>();
-					m.load(is);
-					m.m_site_id = site_id;
-					m.m_time_stamp = tstamp;
-					m.m_loc_type = LDT_TOF;
-					t->m_cmd_code = cmd;
-					t->m_hash_id = m.m_card_id;
-					logn_info(3, "");
-					m_loc_worker->request(t);
+					struct timeval tv;
+					gettimeofday(&tv, NULL);
+					uint64_t cur_time = tv.tv_sec*1000 + tv.tv_usec/1000;
+					int index = 0;
+					while(!is.eof()){
+						uint64_t tstamp = cur_time - 950 + index*45;
+						++index;
+
+						task* t = task::alloc<message_ins_locinfo>();
+						message_ins_locinfo& m = t->body<message_ins_locinfo>();
+						m.load(is);
+						m.m_site_id = site_id;
+						m.m_time_stamp = tstamp;
+						m.m_loc_type = LDT_TOF;
+						t->m_cmd_code = cmd;
+						t->m_hash_id = m.m_card_id;
+						m_loc_worker->request(t);
+					}
 				}
-
 				break;
             case CHAR_LOCATEDATA_PDOA:
                 {
@@ -532,7 +532,7 @@ int32_t net_service::parse_data_anchor_opt(const std::shared_ptr<client>& clt, z
    	//site_ptr->m_device_type_id = (power>>3)&0x07;
 	site_ptr->m_net_device_status = (((power>>3)&0x07) == 0 ? 0: 1);
 
-    if(!check_message_time(clt, site_ptr, &site_tm, site_ct, power)){
+    /*if(!check_message_time(clt, site_ptr, &site_tm, site_ct, power)){
 		// 分站时间异常，直接校时
 		//switch(site_ptr->m_device_type_id)
 		switch(site_ptr->m_net_device_status)
@@ -549,7 +549,7 @@ int32_t net_service::parse_data_anchor_opt(const std::shared_ptr<client>& clt, z
 		}
 	
 		return -1;
-	}
+	}*/
 
     if(clt->type()!=2){
         site_ptr->set_client(clt);

websocket/jsonBuilder.cpp

@@ -944,6 +944,87 @@ namespace sys
 
 		return sb.GetString();
 	}
+
+	/*
+	 * 构建solid_ball_data的json数据
+	 * */
+	std::string jsonBuilder::build_sb_data(const std::vector<sb_data>& vtd)
+	{
+		if(vtd.size() == 0){
+			return "";
+		}
+		rapidjson::StringBuffer sb;
+		rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+		rapidjson::Document doc;
+		rapidjson::Document::AllocatorType& allocator = doc.GetAllocator();
+
+		rapidjson::Value root(rapidjson::kObjectType);
+		__SetCmd("sb_raw_data", root, allocator);
+		__AddVersion(root, allocator);
+
+		rapidjson::Value data(rapidjson::kArrayType);
+		for(const auto& it : vtd){
+			rapidjson::Value elem(rapidjson::kArrayType);
+			rapidjson::Value value(rapidjson::kObjectType);
+
+			log_info("[error] cid = %s, length = %d", it.cid.c_str(), it.cid.length());
+
+			value.SetString(it.cid.c_str(), it.cid.length(), allocator);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.x);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.y);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.z);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.speed);
+			elem.PushBack(value, allocator);
+
+			value.SetString(it.site_info.c_str(), it.site_info.length(), allocator);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.acc);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.m_acc_data[0]);
+			elem.PushBack(value, allocator);
+	
+			value.SetDouble(it.m_acc_data[1]);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.m_acc_data[2]);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.ang);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.m_ang_data[0]);
+			elem.PushBack(value, allocator);
+	
+			value.SetDouble(it.m_ang_data[1]);
+			elem.PushBack(value, allocator);
+
+			value.SetDouble(it.m_ang_data[2]);
+			elem.PushBack(value, allocator);
+
+			value.SetString(it.cur_time.c_str(), it.cur_time.length(), allocator);
+			elem.PushBack(value, allocator);
+
+			data.PushBack(elem, allocator);
+
+		}	
+
+		root.AddMember("data", data,  allocator);
+
+		root.Accept(writer);
+
+		return sb.GetString();
+
+	}
 }
 
 

websocket/jsonBuilder.h

@@ -247,6 +247,7 @@ namespace sys
         std::string build_traffic_light(const std::vector<light_state> lights);
 		std::string build_device_state(const std::vector<device_state> vtd);
 		std::string build_tof_data(const std::vector<tof_data>& vtd);
+		std::string build_sb_data(const std::vector<sb_data>& vtd);
 		/**
 		* @brief
 		生成车辆进入特殊区域jason函数。

websocket/wsTimerThread.cpp

@@ -169,13 +169,29 @@ namespace sys
 			//std::vector<tof_data>().swap(tof_data_list);
 			tof_data_list.clear();
 			//tof_data_list.resize(0);
-		}catch(...){
+		}catch(std::exception& e){
 			//log_error("[error] err=%s", e.what());
-			int a = 0;
-			log_error("[error] err");
+			log_error("[error] err=%s", e.what());
 		}
 	}
 
+	void wsTimerThread::send_solid_ball_data()
+	{
+		if(sb_data_list.empty()){
+			return;
+		}
+		
+		try{
+			std::lock_guard<std::mutex> lg(m_mtx);
+			std::vector<sb_data> vts;
+			vts.swap(sb_data_list);
+			std::string val = __jsBuilder.build_sb_data(vts);
+			swsClientMgr.send(JSON_CMD_VALUE_PUSH, val);
+			sb_data_list.clear();
+		}catch(std::exception& e){
+			log_error("[error] err=%s", e.what());
+		}
+	}
     /*
      * 定时器线程发送定位数据
      *
@@ -199,6 +215,9 @@ namespace sys
 				pOwner->send_tof_data();
 				__LastSendTime = t;
 			}
+			
+			pOwner->send_solid_ball_data();
+
           	boost::this_thread::sleep( boost::posix_time::millisec( 1 ) );
 		}
 
@@ -264,4 +283,14 @@ namespace sys
 			log_error("[error] err=%s", err.c_str());
 		}
 	}
+
+	void wsTimerThread::upt_sb_data(const sb_data& val)
+	{
+		try{
+			std::lock_guard<std::mutex> lg(m_mtx);
+			sb_data_list.push_back(val);
+		}catch(std::exception& e){
+			log_error("[error] err=%s", e.what());
+		}
+	}
 }

websocket/wsTimerThread.h

@@ -52,6 +52,7 @@ namespace sys
         std::vector<light_state> light_state_list;		// 红绿灯状态列表
 		std::vector<device_state> device_state_list;	// 设备状态列表
 		std::vector<tof_data> tof_data_list;			// tof数据待发送列表
+		std::vector<sb_data> sb_data_list;				// solid ball待发送数据列表
 	private:
 		/**
 		* @brief
@@ -106,6 +107,7 @@ namespace sys
         void send_light_state();
 		void send_device_state();
 		void send_tof_data();
+		void send_solid_ball_data();
 
 	protected:
 		/**
@@ -212,6 +214,7 @@ namespace sys
         void upt_light_state(const light_state& light);
 		void upt_device_state(const device_state& ds);
 		void upt_tof_data(const tof_data& td);
+		void upt_sb_data(const sb_data& val);
 	};
 }
 

websocket/ws_common.h

@@ -237,7 +237,43 @@ namespace sys
 			return *this;
 		}*/
 	};
+	
+	struct sb_data{
+		double x;
+		double y;
+		double z;
+		std::string cid;
+		double speed;
+		std::string site_info;
+		double acc;
+		std::array<double ,3> m_acc_data;
+		double ang;
+		std::array<double ,3> m_ang_data;
+		std::string cur_time;
+
+		sb_data()
+			: x(0.0)
+			, y(0.0)
+			, z(0.0)
+			, cid("")
+			, speed(0.0)
+			, site_info("")
+			, acc(0.0)
+			, ang(0.0)
+			, cur_time("")
+		{
+			m_acc_data.fill(0);
+			m_ang_data.fill(0);
+		}
 
+		void reset()
+		{
+			cid = cur_time = site_info = "";
+			x = y = z = speed = acc = ang = 0.0;
+			m_acc_data.fill(0);
+			m_ang_data.fill(0);
+		}
+	};
 }
 
 #endif

worker.cpp

@@ -207,6 +207,7 @@ struct worker_thread: loop_thread ,visitor<std::shared_ptr<card_location_base>>
 				t.destroy();
                 break;
 			case CHAR_LOCATEDATA_TOF_INS:
+				log_info("card loc message: 0x%04X", t.m_cmd_code);
 				card_list::instance()->on_message(this, t.body<message_ins_locinfo>(), false);
 				t.destroy();
 				break;