159 lines
4.5 KiB
C++
159 lines
4.5 KiB
C++
#include "SimMsg.h"
|
|
#include "ComS.h"
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
|
|
using namespace std;
|
|
|
|
void ComSWriteLog(const std::string &msg)
|
|
{
|
|
std::cout << msg<<std::endl;
|
|
}
|
|
|
|
string topic_name_cmd = "Command";
|
|
string topic_name_tlm = "Telemetry";
|
|
|
|
SimMsg* ComS_part = nullptr;
|
|
string servername = "Com_Service";
|
|
int cmd_result = 0;
|
|
|
|
//初始化
|
|
void ComS_init(uint8_t domainid, std::string appname)
|
|
{
|
|
std::vector<std::string> parameters;
|
|
string expression = "dest = '"+ servername + "'";
|
|
if (nullptr == ComS_part)
|
|
{
|
|
ComS_part=new SimMsg(domainid, 3000, appname, ComSWriteLog);
|
|
ComS_part->create_pub(topic_name_cmd);
|
|
ComS_part->create_pub(topic_name_tlm);
|
|
|
|
ComS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
|
|
ComS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
|
|
}
|
|
}
|
|
|
|
// 遥控接收回调函数
|
|
void command_callback(std::string src, std::string dest, std::string type,
|
|
std::string reserve1, std::string reserve2,
|
|
std::vector<uint8_t>& data) {
|
|
if (data.size() < 5)
|
|
{
|
|
std::cout << "指令长度错误！" << std::endl;
|
|
return;
|
|
}
|
|
|
|
switch(data[0])
|
|
{
|
|
case 0x80:
|
|
ComS_command_Pub(&data[4], "Mag_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x81:
|
|
ComS_command_Pub(&data[4], "Com_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x82:
|
|
ComS_command_Pub(&data[4], "Eng_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x83:
|
|
ComS_command_Pub(&data[4], "Thm_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x84:
|
|
ComS_command_Pub(&data[4], "Gnc_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x85:
|
|
ComS_command_Pub(&data[4], "Pld_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x90:
|
|
ComS_command_Pub(&data[4], "Obc_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x91:
|
|
ComS_command_Pub(&data[4], "Gnss_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x92:
|
|
ComS_command_Pub(&data[4], "Com_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x93:
|
|
ComS_command_Pub(&data[4], "Can_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x94:
|
|
ComS_command_Pub(&data[4], "Switch_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x95:
|
|
ComS_command_Pub(&data[4], "Ad_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x96:
|
|
ComS_command_Pub(&data[4], "Starsensor_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x97:
|
|
ComS_command_Pub(&data[4], "Mems_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x98:
|
|
ComS_command_Pub(&data[4], "Flywheel_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x99:
|
|
ComS_command_Pub(&data[4], "Gyro_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x9A:
|
|
ComS_command_Pub(&data[4], "opticaltracker_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
case 0x9B:
|
|
ComS_command_Pub(&data[4], "visionsensor_Hardware_Service", data.size() - 4);
|
|
break;
|
|
|
|
default:
|
|
std::cout << "指令去向错误!" << std::endl;
|
|
return;
|
|
|
|
}
|
|
}
|
|
|
|
// 遥测接收回调函数
|
|
void telemetry_callback(std::string src, std::string dest, std::string type,
|
|
std::string reserve1, std::string reserve2,
|
|
std::vector<uint8_t>& data) {
|
|
// 打印接收到的数据
|
|
std::cout << "Telemetry received from " << src << " to " << dest << std::endl;
|
|
std::cout << "Type: " << type << std::endl;
|
|
std::cout << "Data (" << data.size() << " bytes): ";
|
|
for (size_t i = 0; i < data.size(); ++i) {
|
|
std::cout << std::hex << std::uppercase << (int)data[i];
|
|
if (i < data.size() - 1) {
|
|
std::cout << ", ";
|
|
}
|
|
}
|
|
std::cout << std::dec << std::endl;
|
|
}
|
|
|
|
|
|
// 通信服务遥控发布
|
|
void ComS_command_Pub(uint8_t* data, string dest, uint16_t len)
|
|
{
|
|
ComS_part->publish(topic_name_cmd, "Com_Service", dest, "command", data, len);
|
|
}
|
|
|
|
// 通信服务遥测发布
|
|
void ComS_telemetry_Pub(uint8_t* data, string dest, uint16_t len)
|
|
{
|
|
ComS_part->publish(topic_name_tlm, "Com_Service", dest, "telemetry", data, len);
|
|
}
|