531 lines
25 KiB
C++
531 lines
25 KiB
C++
#ifndef UDEVICE_SECDISTSTATE_H
|
|
#define UDEVICE_SECDISTSTATE_H
|
|
#include <string>
|
|
#include <json/json.h>
|
|
#include "../driver.h"
|
|
#include "../logc/loguru.hpp"
|
|
#include <cstring>
|
|
|
|
class uDevice_secdistStateMsg
|
|
{
|
|
private:
|
|
/* data */
|
|
public:
|
|
uDevice_secdistStateMsg(){};
|
|
~uDevice_secdistStateMsg(){};
|
|
std::string key_fb = "uDevice_secdistState_st";
|
|
std::string key_cmd = "uPower_secdistState_cmd";
|
|
|
|
struct DeviceSecdistState
|
|
{
|
|
// UNIT1
|
|
u_int8_t bowCover1;
|
|
u_int8_t bowCover2;
|
|
u_int8_t bowCover3;
|
|
u_int8_t bowCover4;
|
|
u_int8_t bowCover5;
|
|
u_int8_t bowCover6;
|
|
u_int8_t singleBeamSonar1;
|
|
u_int8_t singleBeamSonar2;
|
|
u_int8_t singleBeamSonar3;
|
|
u_int8_t singleBeamSonar4;
|
|
u_int8_t singleBeamSonar5;
|
|
u_int8_t singleBeamSonar6;
|
|
u_int8_t forwardSonar;
|
|
u_int8_t depth3;
|
|
u_int8_t soundVeloc;
|
|
u_int8_t bowRudderServo;
|
|
u_int8_t bowRudderDeploy;
|
|
u_int8_t ctd1;
|
|
u_int8_t waterInject;
|
|
|
|
// UNIT2
|
|
u_int8_t sideSonar;
|
|
u_int8_t bowWaterInet1;
|
|
u_int8_t bowWaterInet2;
|
|
u_int8_t bowWaterInet3;
|
|
u_int8_t bowWaterInet4;
|
|
u_int8_t bowWaterInet5;
|
|
u_int8_t bowWaterInet6;
|
|
u_int8_t bowWaterInet7;
|
|
u_int8_t bowWaterInet8;
|
|
u_int8_t launchControler;
|
|
u_int8_t electSensor;
|
|
u_int8_t lowFreqSonar;
|
|
|
|
// UNIT3
|
|
u_int8_t buoyancyAdjust1;
|
|
u_int8_t buoyancyAdjust2;
|
|
u_int8_t buoyancyAdjust3;
|
|
u_int8_t USBL;
|
|
u_int8_t depth1;
|
|
u_int8_t commDev;
|
|
u_int8_t mastLiftingServo;
|
|
u_int8_t DVL;
|
|
u_int8_t ADCP;
|
|
u_int8_t inOutLetWater1;
|
|
u_int8_t inOutLetWater2;
|
|
|
|
// UNIT4
|
|
u_int8_t singleBeamSonar7;
|
|
u_int8_t singleBeamSonar8;
|
|
u_int8_t buoyage;
|
|
u_int8_t bowThrow;
|
|
u_int8_t depth2;
|
|
u_int8_t thruster;
|
|
u_int8_t dragSonar;
|
|
u_int8_t haulingMachine;
|
|
u_int8_t buoyancyAdjust4;
|
|
u_int8_t buoyancyAdjust5;
|
|
u_int8_t sternRudderServo1;
|
|
u_int8_t sternRudderServo2;
|
|
u_int8_t sternRudderServo3;
|
|
u_int8_t sternRudderServo4;
|
|
u_int8_t ctd2;
|
|
u_int8_t commDevStern;
|
|
u_int8_t inOutLetWater3;
|
|
|
|
// UNIT5
|
|
u_int8_t secINS;
|
|
u_int8_t photoSensor;
|
|
u_int8_t sternThrow;
|
|
u_int8_t mainINS;
|
|
} deviceSecdistState;
|
|
|
|
|
|
|
|
bool parseSecdistStateFromJson(DeviceSecdistState &state, const std::string &strJson){
|
|
Json::Value root;
|
|
Json::CharReaderBuilder readerBuilder;
|
|
std::unique_ptr<Json::CharReader> reader(readerBuilder.newCharReader());
|
|
std::string errors;
|
|
|
|
if(!reader->parse(strJson.c_str(), strJson.c_str() + strJson.length(), &root, &errors)){
|
|
LOG_F(ERROR, "Failed to parse JSON: %s", errors.c_str());
|
|
return false;
|
|
}
|
|
|
|
auto checkField = [&root](const std::string& field) -> bool {
|
|
if(!root.isMember(field) || !root[field].isUInt()) {
|
|
// std::cerr << "Missing or invalid field: " << field << std::endl;
|
|
// LOG_F(ERROR, "Missing or invalid field: %s", field.c_str());
|
|
return false;
|
|
}
|
|
return true;
|
|
};
|
|
|
|
bool ok = true;
|
|
|
|
// UNIT1
|
|
if(!checkField("bowCover1_uint8")) ok=false; state.bowCover1 = root["bowCover1_uint8"].asUInt();
|
|
if(!checkField("bowCover2_uint8")) ok=false; state.bowCover2 = root["bowCover2_uint8"].asUInt();
|
|
if(!checkField("bowCover3_uint8")) ok=false; state.bowCover3 = root["bowCover3_uint8"].asUInt();
|
|
if(!checkField("bowCover4_uint8")) ok=false; state.bowCover4 = root["bowCover4_uint8"].asUInt();
|
|
if(!checkField("bowCover5_uint8")) ok=false; state.bowCover5 = root["bowCover5_uint8"].asUInt();
|
|
if(!checkField("bowCover6_uint8")) ok=false; state.bowCover6 = root["bowCover6_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar1_uint8")) ok=false; state.singleBeamSonar1 = root["singleBeamSonar1_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar2_uint8")) ok=false; state.singleBeamSonar2 = root["singleBeamSonar2_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar3_uint8")) ok=false; state.singleBeamSonar3 = root["singleBeamSonar3_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar4_uint8")) ok=false; state.singleBeamSonar4 = root["singleBeamSonar4_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar5_uint8")) ok=false; state.singleBeamSonar5 = root["singleBeamSonar5_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar6_uint8")) ok=false; state.singleBeamSonar6 = root["singleBeamSonar6_uint8"].asUInt();
|
|
if(!checkField("forwardSonar_uint8")) ok=false; state.forwardSonar = root["forwardSonar_uint8"].asUInt();
|
|
if(!checkField("depth3_uint8")) ok=false; state.depth3 = root["depth3_uint8"].asUInt();
|
|
if(!checkField("soundVeloc_uint8")) ok=false; state.soundVeloc = root["soundVeloc_uint8"].asUInt();
|
|
if(!checkField("bowRudderServo_uint8")) ok=false; state.bowRudderServo = root["bowRudderServo_uint8"].asUInt();
|
|
if(!checkField("bowRudderDeploy_uint8")) ok=false; state.bowRudderDeploy = root["bowRudderDeploy_uint8"].asUInt();
|
|
if(!checkField("ctd1_uint8")) ok=false; state.ctd1 = root["ctd1_uint8"].asUInt();
|
|
if(!checkField("waterInject_uint8")) ok=false; state.waterInject = root["waterInject_uint8"].asUInt();
|
|
|
|
// UNIT2
|
|
if(!checkField("sideSonar_uint8")) ok=false; state.sideSonar = root["sideSonar_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet1_uint8")) ok=false; state.bowWaterInet1 = root["bowWaterInet1_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet2_uint8")) ok=false; state.bowWaterInet2 = root["bowWaterInet2_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet3_uint8")) ok=false; state.bowWaterInet3 = root["bowWaterInet3_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet4_uint8")) ok=false; state.bowWaterInet4 = root["bowWaterInet4_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet5_uint8")) ok=false; state.bowWaterInet5 = root["bowWaterInet5_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet6_uint8")) ok=false; state.bowWaterInet6 = root["bowWaterInet6_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet7_uint8")) ok=false; state.bowWaterInet7 = root["bowWaterInet7_uint8"].asUInt();
|
|
if(!checkField("bowWaterInet8_uint8")) ok=false; state.bowWaterInet8 = root["bowWaterInet8_uint8"].asUInt();
|
|
if(!checkField("launchControler_uint8")) ok=false; state.launchControler = root["launchControler_uint8"].asUInt();
|
|
if(!checkField("electSensor_uint8")) ok=false; state.electSensor = root["electSensor_uint8"].asUInt();
|
|
if(!checkField("lowFreqSonar_uint8")) ok=false; state.lowFreqSonar = root["lowFreqSonar_uint8"].asUInt();
|
|
|
|
// UNIT3
|
|
if(!checkField("buoyancyAdjust1_uint8")) ok=false; state.buoyancyAdjust1 = root["buoyancyAdjust1_uint8"].asUInt();
|
|
if(!checkField("buoyancyAdjust2_uint8")) ok=false; state.buoyancyAdjust2 = root["buoyancyAdjust2_uint8"].asUInt();
|
|
if(!checkField("buoyancyAdjust3_uint8")) ok=false; state.buoyancyAdjust3 = root["buoyancyAdjust3_uint8"].asUInt();
|
|
if(!checkField("USBL_uint8")) ok=false; state.USBL = root["USBL_uint8"].asUInt();
|
|
if(!checkField("depth1_uint8")) ok=false; state.depth1 = root["depth1_uint8"].asUInt();
|
|
if(!checkField("commDev_uint8")) ok=false; state.commDev = root["commDev_uint8"].asUInt();
|
|
if(!checkField("mastLiftingServo_uint8")) ok=false; state.mastLiftingServo = root["mastLiftingServo_uint8"].asUInt();
|
|
if(!checkField("DVL_uint8")) ok=false; state.DVL = root["DVL_uint8"].asUInt();
|
|
if(!checkField("ADCP_uint8")) ok=false; state.ADCP = root["ADCP_uint8"].asUInt();
|
|
if(!checkField("inOutLetWater1_uint8")) ok=false; state.inOutLetWater1 = root["inOutLetWater1_uint8"].asUInt();
|
|
if(!checkField("inOutLetWater2_uint8")) ok=false; state.inOutLetWater2 = root["inOutLetWater2_uint8"].asUInt();
|
|
|
|
// UNIT4
|
|
if(!checkField("singleBeamSonar7_uint8")) ok=false; state.singleBeamSonar7 = root["singleBeamSonar7_uint8"].asUInt();
|
|
if(!checkField("singleBeamSonar8_uint8")) ok=false; state.singleBeamSonar8 = root["singleBeamSonar8_uint8"].asUInt();
|
|
if(!checkField("buoyage_uint8")) ok=false; state.buoyage = root["buoyage_uint8"].asUInt();
|
|
if(!checkField("bowThrow_uint8")) ok=false; state.bowThrow = root["bowThrow_uint8"].asUInt();
|
|
if(!checkField("depth2_uint8")) ok=false; state.depth2 = root["depth2_uint8"].asUInt();
|
|
if(!checkField("thruster_uint8")) ok=false; state.thruster = root["thruster_uint8"].asUInt();
|
|
if(!checkField("dragSonar_uint8")) ok=false; state.dragSonar = root["dragSonar_uint8"].asUInt();
|
|
if(!checkField("haulingMachine_uint8")) ok=false; state.haulingMachine = root["haulingMachine_uint8"].asUInt();
|
|
if(!checkField("buoyancyAdjust4_uint8")) ok=false; state.buoyancyAdjust4 = root["buoyancyAdjust4_uint8"].asUInt();
|
|
if(!checkField("buoyancyAdjust5_uint8")) ok=false; state.buoyancyAdjust5 = root["buoyancyAdjust5_uint8"].asUInt();
|
|
if(!checkField("sternRudderServo1_uint8")) ok=false; state.sternRudderServo1 = root["sternRudderServo1_uint8"].asUInt();
|
|
if(!checkField("sternRudderServo2_uint8")) ok=false; state.sternRudderServo2 = root["sternRudderServo2_uint8"].asUInt();
|
|
if(!checkField("sternRudderServo3_uint8")) ok=false; state.sternRudderServo3 = root["sternRudderServo3_uint8"].asUInt();
|
|
if(!checkField("sternRudderServo4_uint8")) ok=false; state.sternRudderServo4 = root["sternRudderServo4_uint8"].asUInt();
|
|
if(!checkField("ctd2_uint8")) ok=false; state.ctd2 = root["ctd2_uint8"].asUInt();
|
|
if(!checkField("commDevStern_uint8")) ok=false; state.commDevStern = root["commDevStern_uint8"].asUInt();
|
|
if(!checkField("inOutLetWater3_uint8")) ok=false; state.inOutLetWater3 = root["inOutLetWater3_uint8"].asUInt();
|
|
|
|
// UNIT5
|
|
if(!checkField("secINS_uint8")) ok=false; state.secINS = root["secINS_uint8"].asUInt();
|
|
if(!checkField("photoSensor_uint8")) ok=false; state.photoSensor = root["photoSensor_uint8"].asUInt();
|
|
if(!checkField("sternThrow_uint8")) ok=false; state.sternThrow = root["sternThrow_uint8"].asUInt();
|
|
if(!checkField("mainINS_uint8")) ok=false; state.mainINS = root["mainINS_uint8"].asUInt();
|
|
|
|
//不检查字段缺失
|
|
ok = true;
|
|
return ok;
|
|
};
|
|
std::string buildSecdistStateToJson(DeviceSecdistState &state){
|
|
Json::Value root;
|
|
|
|
// UNIT1
|
|
root["bowCover1_uint8"] = state.bowCover1;
|
|
root["bowCover2_uint8"] = state.bowCover2;
|
|
root["bowCover3_uint8"] = state.bowCover3;
|
|
root["bowCover4_uint8"] = state.bowCover4;
|
|
root["bowCover5_uint8"] = state.bowCover5;
|
|
root["bowCover6_uint8"] = state.bowCover6;
|
|
root["singleBeamSonar1_uint8"] = state.singleBeamSonar1;
|
|
root["singleBeamSonar2_uint8"] = state.singleBeamSonar2;
|
|
root["singleBeamSonar3_uint8"] = state.singleBeamSonar3;
|
|
root["singleBeamSonar4_uint8"] = state.singleBeamSonar4;
|
|
root["singleBeamSonar5_uint8"] = state.singleBeamSonar5;
|
|
root["singleBeamSonar6_uint8"] = state.singleBeamSonar6;
|
|
root["forwardSonar_uint8"] = state.forwardSonar;
|
|
root["depth3_uint8"] = state.depth3;
|
|
root["soundVeloc_uint8"] = state.soundVeloc;
|
|
root["bowRudderServo_uint8"] = state.bowRudderServo;
|
|
root["bowRudderDeploy_uint8"] = state.bowRudderDeploy;
|
|
root["ctd1_uint8"] = state.ctd1;
|
|
root["waterInject_uint8"] = state.waterInject;
|
|
|
|
// UNIT2
|
|
root["sideSonar_uint8"] = state.sideSonar;
|
|
root["bowWaterInet1_uint8"] = state.bowWaterInet1;
|
|
root["bowWaterInet2_uint8"] = state.bowWaterInet2;
|
|
root["bowWaterInet3_uint8"] = state.bowWaterInet3;
|
|
root["bowWaterInet4_uint8"] = state.bowWaterInet4;
|
|
root["bowWaterInet5_uint8"] = state.bowWaterInet5;
|
|
root["bowWaterInet6_uint8"] = state.bowWaterInet6;
|
|
root["bowWaterInet7_uint8"] = state.bowWaterInet7;
|
|
root["bowWaterInet8_uint8"] = state.bowWaterInet8;
|
|
root["launchControler_uint8"] = state.launchControler;
|
|
root["electSensor_uint8"] = state.electSensor;
|
|
root["lowFreqSonar_uint8"] = state.lowFreqSonar;
|
|
|
|
// UNIT3
|
|
root["buoyancyAdjust1_uint8"] = state.buoyancyAdjust1;
|
|
root["buoyancyAdjust2_uint8"] = state.buoyancyAdjust2;
|
|
root["buoyancyAdjust3_uint8"] = state.buoyancyAdjust3;
|
|
root["USBL_uint8"] = state.USBL;
|
|
root["depth1_uint8"] = state.depth1;
|
|
root["commDev_uint8"] = state.commDev;
|
|
root["mastLiftingServo_uint8"] = state.mastLiftingServo;
|
|
root["DVL_uint8"] = state.DVL;
|
|
root["ADCP_uint8"] = state.ADCP;
|
|
root["inOutLetWater1_uint8"] = state.inOutLetWater1;
|
|
root["inOutLetWater2_uint8"] = state.inOutLetWater2;
|
|
|
|
// UNIT4
|
|
root["singleBeamSonar7_uint8"] = state.singleBeamSonar7;
|
|
root["singleBeamSonar8_uint8"] = state.singleBeamSonar8;
|
|
root["buoyage_uint8"] = state.buoyage;
|
|
root["bowThrow_uint8"] = state.bowThrow;
|
|
root["depth2_uint8"] = state.depth2;
|
|
root["thruster_uint8"] = state.thruster;
|
|
root["dragSonar_uint8"] = state.dragSonar;
|
|
root["haulingMachine_uint8"] = state.haulingMachine;
|
|
root["buoyancyAdjust4_uint8"] = state.buoyancyAdjust4;
|
|
root["buoyancyAdjust5_uint8"] = state.buoyancyAdjust5;
|
|
root["sternRudderServo1_uint8"] = state.sternRudderServo1;
|
|
root["sternRudderServo2_uint8"] = state.sternRudderServo2;
|
|
root["sternRudderServo3_uint8"] = state.sternRudderServo3;
|
|
root["sternRudderServo4_uint8"] = state.sternRudderServo4;
|
|
root["ctd2_uint8"] = state.ctd2;
|
|
root["commDevStern_uint8"] = state.commDevStern;
|
|
root["inOutLetWater3_uint8"] = state.inOutLetWater3;
|
|
|
|
// UNIT5
|
|
root["secINS_uint8"] = state.secINS;
|
|
root["photoSensor_uint8"] = state.photoSensor;
|
|
root["sternThrow_uint8"] = state.sternThrow;
|
|
root["mainINS_uint8"] = state.mainINS;
|
|
|
|
Json::StreamWriterBuilder writerBuilder;
|
|
writerBuilder.settings_["indentation"] = ""; // 紧凑格式，无缩进
|
|
return Json::writeString(writerBuilder, root);
|
|
};
|
|
|
|
bool converFromDriverTable(DeviceSecdistState &state,const DriverTable &driver){
|
|
// UNIT1
|
|
state.bowCover1 = driver.bowCover1;
|
|
state.bowCover2 = driver.bowCover2;
|
|
state.bowCover3 = driver.bowCover3;
|
|
state.bowCover4 = driver.bowCover4;
|
|
state.bowCover5 = driver.bowCover5;
|
|
state.bowCover6 = driver.bowCover6;
|
|
state.singleBeamSonar1 = driver.singleBeamSonar1;
|
|
state.singleBeamSonar2 = driver.singleBeamSonar2;
|
|
state.singleBeamSonar3 = driver.singleBeamSonar3;
|
|
state.singleBeamSonar4 = driver.singleBeamSonar4;
|
|
state.singleBeamSonar5 = driver.singleBeamSonar5;
|
|
state.singleBeamSonar6 = driver.singleBeamSonar6;
|
|
state.forwardSonar = driver.forwardSonar;
|
|
state.depth3 = driver.depth3;
|
|
state.soundVeloc = driver.soundVeloc;
|
|
state.bowRudderServo = driver.bowRudderServo1;
|
|
state.bowRudderDeploy = driver.bowRudderDeploy;
|
|
state.ctd1 = driver.ctd1;
|
|
state.waterInject = driver.waterInject;
|
|
|
|
// UNIT2
|
|
state.sideSonar = driver.sideSonar;
|
|
state.bowWaterInet1 = driver.bowWaterInet1;
|
|
state.bowWaterInet2 = driver.bowWaterInet2;
|
|
state.bowWaterInet3 = driver.bowWaterInet3;
|
|
state.bowWaterInet4 = driver.bowWaterInet4;
|
|
state.bowWaterInet5 = driver.bowWaterInet5;
|
|
state.bowWaterInet6 = driver.bowWaterInet6;
|
|
state.bowWaterInet7 = driver.bowWaterInet7;
|
|
state.bowWaterInet8 = driver.bowWaterInet8;
|
|
state.launchControler = driver.launchControler;
|
|
state.electSensor = driver.electSensor;
|
|
state.lowFreqSonar = driver.lowFreqSonar;
|
|
|
|
// UNIT3
|
|
state.buoyancyAdjust1 = driver.buoyancyAdjust1;
|
|
state.buoyancyAdjust2 = driver.buoyancyAdjust2;
|
|
state.buoyancyAdjust3 = driver.buoyancyAdjust3;
|
|
state.USBL = driver.USBL;
|
|
state.depth1 = driver.depth1;
|
|
state.commDev = driver.commMast;
|
|
state.mastLiftingServo = driver.mastLiftingServo;
|
|
state.DVL = driver.DVL;
|
|
state.ADCP = driver.DVL; // Assuming ADCP is same as DVL
|
|
//出入水传感器1、2共用一个继电器
|
|
state.inOutLetWater1 = driver.inputWaterSensor1;
|
|
state.inOutLetWater2 = driver.inputWaterSensor1;
|
|
// state.inOutLetWater2 = driver.inputWaterSensor2;
|
|
|
|
// UNIT4
|
|
state.singleBeamSonar7 = driver.singleBeamSonar7;
|
|
state.singleBeamSonar8 = driver.singleBeamSonar8;
|
|
state.buoyage = driver.buoyage;
|
|
state.bowThrow = driver.bowThrow;
|
|
state.depth2 = driver.depth2;
|
|
state.thruster = driver.thruster;
|
|
state.haulingMachine = 2; // No corresponding field in DriverTable
|
|
state.buoyancyAdjust4 = driver.buoyancyAdjust4;
|
|
state.buoyancyAdjust5 = driver.buoyancyAdjust5;
|
|
state.sternRudderServo1 = driver.sternRudderServo1;
|
|
state.sternRudderServo2 = driver.sternRudderServo2;
|
|
state.sternRudderServo3 = driver.sternRudderServo3;
|
|
state.sternRudderServo4 = driver.sternRudderServo4;
|
|
state.ctd2 = driver.ctd2;
|
|
state.commDevStern = 2; // No corresponding field in DriverTable
|
|
state.inOutLetWater3 = driver.inputWaterSensor3;
|
|
|
|
// UNIT5
|
|
state.secINS = driver.ins;
|
|
state.photoSensor = driver.photoSensor;
|
|
state.sternThrow = driver.sternThrow;
|
|
state.mainINS = driver.ins;
|
|
|
|
// 未映射字段
|
|
// driver.overDeep1
|
|
// driver.overDeep2
|
|
// driver.ntp
|
|
state.dragSonar = 2; // Assuming dragSonar is same as lowFreqSonar
|
|
|
|
|
|
return true;
|
|
};
|
|
|
|
bool converToDriverTable(const DeviceSecdistState &state, DriverTable &driver){
|
|
// UNIT1
|
|
driver.bowCover1 = state.bowCover1;
|
|
driver.bowCover2 = state.bowCover2;
|
|
driver.bowCover3 = state.bowCover3;
|
|
driver.bowCover4 = state.bowCover4;
|
|
driver.bowCover5 = state.bowCover5;
|
|
driver.bowCover6 = state.bowCover6;
|
|
driver.singleBeamSonar1 = state.singleBeamSonar1;
|
|
driver.singleBeamSonar2 = state.singleBeamSonar2;
|
|
driver.singleBeamSonar3 = state.singleBeamSonar3;
|
|
driver.singleBeamSonar4 = state.singleBeamSonar4;
|
|
driver.singleBeamSonar5 = state.singleBeamSonar5;
|
|
driver.singleBeamSonar6 = state.singleBeamSonar6;
|
|
driver.forwardSonar = state.forwardSonar;
|
|
driver.depth3 = state.depth3;
|
|
driver.soundVeloc = state.soundVeloc;
|
|
driver.bowRudderServo1 = state.bowRudderServo;
|
|
driver.bowRudderServo2 = state.bowRudderServo;
|
|
driver.bowRudderDeploy = state.bowRudderDeploy;
|
|
driver.ctd1 = state.ctd1;
|
|
driver.waterInject = state.waterInject;
|
|
|
|
// UNIT2
|
|
driver.sideSonar = state.sideSonar;
|
|
driver.bowWaterInet1 = state.bowWaterInet1;
|
|
driver.bowWaterInet2 = state.bowWaterInet2;
|
|
driver.bowWaterInet3 = state.bowWaterInet3;
|
|
driver.bowWaterInet4 = state.bowWaterInet4;
|
|
driver.bowWaterInet5 = state.bowWaterInet5;
|
|
driver.bowWaterInet6 = state.bowWaterInet6;
|
|
driver.bowWaterInet7 = state.bowWaterInet7;
|
|
driver.bowWaterInet8 = state.bowWaterInet8;
|
|
driver.launchControler = state.launchControler;
|
|
driver.electSensor = state.electSensor;
|
|
driver.lowFreqSonar = state.lowFreqSonar;
|
|
|
|
// UNIT3
|
|
driver.buoyancyAdjust1 = state.buoyancyAdjust1;
|
|
driver.buoyancyAdjust2 = state.buoyancyAdjust2;
|
|
driver.buoyancyAdjust3 = state.buoyancyAdjust3;
|
|
driver.USBL = state.USBL;
|
|
driver.depth1 = state.depth1;
|
|
driver.commMast = state.commDev;
|
|
driver.ntp = 1;
|
|
driver.mastLiftingServo = state.mastLiftingServo;
|
|
driver.DVL = state.DVL;
|
|
driver.inputWaterSensor1 = state.inOutLetWater1;
|
|
driver.inputWaterSensor2 = state.inOutLetWater2;
|
|
driver.overDeep1 = (state.depth1 == 1) ? 1 : 0;
|
|
driver.overDeep2 = (state.depth2 == 1 || state.depth3 == 1) ? 1 : 0;
|
|
|
|
// UNIT4
|
|
driver.singleBeamSonar7 = state.singleBeamSonar7;
|
|
driver.singleBeamSonar8 = state.singleBeamSonar8;
|
|
driver.buoyage = state.buoyage;
|
|
driver.bowThrow = state.bowThrow;
|
|
driver.depth2 = state.depth2;
|
|
driver.thruster = state.thruster;
|
|
driver.buoyancyAdjust4 = state.buoyancyAdjust4;
|
|
driver.buoyancyAdjust5 = state.buoyancyAdjust5;
|
|
driver.sternRudderServo1 = state.sternRudderServo1;
|
|
driver.sternRudderServo2 = state.sternRudderServo2;
|
|
driver.sternRudderServo3 = state.sternRudderServo3;
|
|
driver.sternRudderServo4 = state.sternRudderServo4;
|
|
driver.ctd2 = state.ctd2;
|
|
driver.inputWaterSensor3 = state.inOutLetWater3;
|
|
|
|
// UNIT5
|
|
driver.ins = state.secINS | state.mainINS;
|
|
driver.photoSensor = state.photoSensor;
|
|
driver.sternThrow = state.sternThrow;
|
|
|
|
return true;
|
|
}
|
|
|
|
std::string buildMsg(DriverTable &driver){
|
|
DeviceSecdistState s;
|
|
converFromDriverTable(s,driver);
|
|
// std::cout << (int)s.depth1 << std::endl;
|
|
// std::cout << buildSecdistStateToJson(s) << std::endl;
|
|
return buildSecdistStateToJson(s);
|
|
};
|
|
|
|
bool parseMsg(const std::string &strJson,DriverTable &driver){
|
|
DeviceSecdistState s;
|
|
if(!parseSecdistStateFromJson(s,strJson)){
|
|
LOG_F(WARNING, "parseSecdistStateFromJson failed");
|
|
return false;
|
|
}
|
|
converToDriverTable(s,driver);
|
|
return true;
|
|
};
|
|
|
|
std::string buildMsg(DriverTable driver, DriverTable driver_state){
|
|
DeviceSecdistState s;
|
|
DriverTable driverCopy = driver;
|
|
for (size_t i = 0; i < sizeof(DriverTable); ++i) {
|
|
unsigned char* driverCopyPtr = reinterpret_cast<unsigned char*>(&driverCopy) + i;
|
|
unsigned char* statePtr = reinterpret_cast<unsigned char*>(&driver_state) + i;
|
|
if (*driverCopyPtr == *statePtr) {
|
|
*driverCopyPtr = 2;
|
|
}
|
|
}
|
|
converFromDriverTable(s, driverCopy);
|
|
// std::cout << (int)s.depth1 << std::endl;
|
|
// std::cout << buildSecdistStateToJson(s) << std::endl;
|
|
return buildSecdistStateToJson(s);
|
|
};
|
|
|
|
std::string buildMsg(DriverTable driver, DriverTable driver_state, unsigned int nCheck){
|
|
static size_t callCount = 0; // 静态变量，记录函数调用次数
|
|
DeviceSecdistState s;
|
|
DriverTable driverCopy = driver;
|
|
if (nCheck > 0 && callCount % nCheck == 0)
|
|
{
|
|
for (size_t i = 0; i < sizeof(DriverTable); ++i) {
|
|
unsigned char* driverCopyPtr = reinterpret_cast<unsigned char*>(&driverCopy) + i;
|
|
unsigned char* statePtr = reinterpret_cast<unsigned char*>(&driver_state) + i;
|
|
if (*driverCopyPtr == *statePtr) {
|
|
*driverCopyPtr = 2;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
memset(&driverCopy, 2, sizeof(DriverTable));
|
|
}
|
|
callCount ++;
|
|
converFromDriverTable(s, driverCopy);
|
|
// std::cout << (int)s.depth1 << std::endl;
|
|
// std::cout << buildSecdistStateToJson(s) << std::endl;
|
|
return buildSecdistStateToJson(s);
|
|
};
|
|
|
|
static DriverTable fault_state;
|
|
std::string buildMsg(DriverTable driver, DriverTable driver_state, DriverTable &driver_fault_state){
|
|
static size_t f_callCount = 0; // 静态变量，记录函数调用次数
|
|
unsigned int nCheck = 10;
|
|
DeviceSecdistState s;
|
|
DriverTable driverCopy = driver;
|
|
for (size_t i = 0; i < sizeof(DriverTable); ++i)
|
|
{
|
|
unsigned char* driverCopyPtr = reinterpret_cast<unsigned char*>(&driverCopy) + i;
|
|
unsigned char* statePtr = reinterpret_cast<unsigned char*>(&driver_state) + i;
|
|
unsigned char* faultStatePtr = reinterpret_cast<unsigned char*>(&driver_fault_state) + i;
|
|
unsigned char* faultState = reinterpret_cast<unsigned char*>(&fault_state) + i;
|
|
if (*driverCopyPtr == *statePtr)
|
|
{
|
|
*driverCopyPtr = 2;
|
|
*faultStatePtr = 0;
|
|
*faultState = 0;
|
|
}
|
|
else
|
|
{
|
|
*faultState = *faultState + 1;
|
|
if(*faultState >= 5)
|
|
{
|
|
//只有开启失败才计入故障
|
|
if(*driverCopyPtr == 1)
|
|
*faultStatePtr = 1;
|
|
*faultState = 5;
|
|
}
|
|
}
|
|
}
|
|
f_callCount ++;
|
|
converFromDriverTable(s, driverCopy);
|
|
return buildSecdistStateToJson(s);
|
|
};
|
|
};
|
|
#endif |