195 lines
7.3 KiB
C++
195 lines
7.3 KiB
C++
#pragma once
|
||
|
||
#include <linux/can.h>
|
||
|
||
#include <atomic>
|
||
#include <cstdint>
|
||
#include <mutex>
|
||
#include <nlohmann/json.hpp>
|
||
#include <stdexcept>
|
||
#include <unordered_map>
|
||
#include <vector>
|
||
|
||
// 远程控车结构体
|
||
typedef struct
|
||
{
|
||
// Byte 1
|
||
uint8_t throttlePercent; // 油门开度, 0~100%
|
||
|
||
// Byte 2-3
|
||
int16_t steeringAngle; // 目标转向角度, 单位：度或千分度
|
||
|
||
// Byte 4
|
||
uint8_t brakePercent; // 制动踏板开度, 0~100%
|
||
|
||
uint8_t targetGear = 0;
|
||
// Byte 5
|
||
uint8_t gear : 2; // 档位: 0=P,1=R,2=N,3=D
|
||
uint8_t steeringMode : 2; // 转向模式: 0=八字,1=斜行,2=前半八,3=后半八
|
||
uint8_t reserved1 : 1; // 保留
|
||
uint8_t highVoltage : 1; // 高压控制: 0=上电,1=下电
|
||
uint8_t resetFault : 1; // 智驾故障复位: 0=无效,1=有效
|
||
uint8_t overload : 1; // 重载信号: 0=轻载,1=重载
|
||
|
||
// Byte 6
|
||
uint8_t hazardLights : 1; // 双闪灯
|
||
uint8_t turnLeft : 1; // 左转向灯
|
||
uint8_t turnRight : 1; // 右转向灯
|
||
uint8_t horn : 1; // 喇叭
|
||
uint8_t eStop : 1; // 急停按钮
|
||
uint8_t drivingLights : 1; // 行车灯
|
||
uint8_t outlineLights : 1; // 示廓灯
|
||
uint8_t autoStandby : 1; // 智驾待机模式
|
||
|
||
// Byte 7
|
||
uint8_t frontLowBeam : 1; // 前近光灯
|
||
uint8_t frontHighBeam : 1; // 前远光灯
|
||
uint8_t rearLowBeam : 1; // 后近光灯
|
||
uint8_t rearHighBeam : 1; // 后远光灯
|
||
uint8_t fogLamp : 1; // 雾灯
|
||
uint8_t reserved2 : 3; // 保留
|
||
|
||
// Byte 8
|
||
uint8_t heartbeat : 4; // 心跳计数
|
||
uint8_t remoteMode : 1; // 远程接管模式
|
||
uint8_t vehicleLock : 1; // 锁车状态
|
||
uint8_t reserved3 : 2; // 保留
|
||
|
||
struct can_frame packCANFrame(uint32_t can_id, bool extended = true) const
|
||
{
|
||
struct can_frame frame;
|
||
frame.can_dlc = 8; // CAN 数据长度固定 8
|
||
if (extended)
|
||
frame.can_id = can_id | CAN_EFF_FLAG;
|
||
else
|
||
frame.can_id = can_id; // 标准帧
|
||
|
||
frame.data[0] = throttlePercent;
|
||
|
||
frame.data[1] = steeringAngle & 0xFF; // 低字节
|
||
frame.data[2] = (steeringAngle >> 8) & 0xFF; // 高字节
|
||
|
||
frame.data[3] = brakePercent;
|
||
|
||
frame.data[4] = (gear & 0x03) | ((steeringMode & 0x03) << 2) | ((reserved1 & 0x01) << 4) |
|
||
((highVoltage & 0x01) << 5) | ((resetFault & 0x01) << 6) | ((overload & 0x01) << 7);
|
||
|
||
frame.data[5] = (hazardLights & 0x01) | ((turnLeft & 0x01) << 1) | ((turnRight & 0x01) << 2) |
|
||
((horn & 0x01) << 3) | ((eStop & 0x01) << 4) | ((drivingLights & 0x01) << 5) |
|
||
((outlineLights & 0x01) << 6) | ((autoStandby & 0x01) << 7);
|
||
|
||
frame.data[6] = (frontLowBeam & 0x01) | ((frontHighBeam & 0x01) << 1) | ((rearLowBeam & 0x01) << 2) |
|
||
((rearHighBeam & 0x01) << 3) | ((fogLamp & 0x01) << 4) | ((reserved2 & 0x07) << 5);
|
||
|
||
frame.data[7] =
|
||
(heartbeat & 0x0F) | ((remoteMode & 0x01) << 4) | ((vehicleLock & 0x01) << 5) | ((reserved3 & 0x03) << 6);
|
||
|
||
return frame;
|
||
}
|
||
} Remote_Ctl_Def;
|
||
|
||
// 车辆信息上报结构体
|
||
typedef struct
|
||
{
|
||
int type = 0; // 车辆类型，0：徐工集卡 1:风润
|
||
int mode = 0; // 控制模式，0：手动驾驶，1：自动驾驶，2：遥控驾驶，3：远程驾驶
|
||
int driveMode = 0; // 行车模式，1：位置，2：后半八，3：前半八，4：八字，5： 斜行
|
||
int gear = 0; // 档位，0：N档，1：D档，2：R档，3：P档
|
||
int speed = 0; // 车辆速度，单位：km/h
|
||
int horn = 0; // 喇叭，0：关，1：开
|
||
int turnLight = 0; // 转向灯，0：关，1：左转，2：右转
|
||
int load = 0; // 重载信号，0：轻载，1：重载
|
||
int voltage = 0; // 高压上下电，0：下电，1：上电
|
||
int positionLight = 0; // 示廓灯，0：关，1：开
|
||
int warning = 0; // 双闪，0：关，1：开
|
||
int fogLight = 0; // 雾灯，0：关，1：开
|
||
int headlamp = 0; // 前大灯，0：关，1：近光灯，2：远光灯
|
||
int tailLight = 0; // 后大灯，0：关，1：近光灯，2：远光灯
|
||
int lock = 0; // 锁车，0：解锁，1：上锁
|
||
int mileage = 0; // 累计里程，单位：km
|
||
int power = 0; // 电量，范围：0~100，单位：%
|
||
long long timestamp = 0; // 上报时间戳，13位，精确到ms
|
||
int emergencyStop = 0; // 急停
|
||
int faultReset = 0; // 智驾故障复位
|
||
int standbyMode = 0; // 智驾待机模式
|
||
|
||
std::string to_json()
|
||
{
|
||
// 自动更新时间戳
|
||
timestamp =
|
||
std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch())
|
||
.count();
|
||
|
||
nlohmann::json j;
|
||
j["type"] = type;
|
||
j["mode"] = mode;
|
||
j["driveMode"] = driveMode;
|
||
j["gear"] = gear;
|
||
j["speed"] = speed;
|
||
j["horn"] = horn;
|
||
j["turnLight"] = turnLight;
|
||
j["load"] = load;
|
||
j["voltage"] = voltage;
|
||
j["positionLight"] = positionLight;
|
||
j["warning"] = warning;
|
||
j["fogLight"] = fogLight;
|
||
j["headlamp"] = headlamp;
|
||
j["tailLight"] = tailLight;
|
||
j["lock"] = lock;
|
||
j["mileage"] = mileage;
|
||
j["power"] = power;
|
||
j["timestamp"] = timestamp;
|
||
j["emergencyStop"] = emergencyStop;
|
||
j["faultReset"] = faultReset;
|
||
j["standbyMode"] = standbyMode;
|
||
|
||
return j.dump();
|
||
}
|
||
|
||
} VehicleInfoReport;
|
||
|
||
// MQTT 指令列表宏
|
||
#define COMMAND_LIST(X) \
|
||
X(Steering, "steering") /*转向*/ \
|
||
X(Throttle, "throttle") /*油门*/ \
|
||
X(Brake, "brake") /*刹车*/ \
|
||
X(Horn, "horn") /*喇叭*/ \
|
||
X(TurnLight, "turnLight") /*转向灯*/ \
|
||
X(EmergencyStop, "emergencyStop") /*急停*/ \
|
||
X(Mode, "mode") /*使能模式*/ \
|
||
X(Gear, "gear") /*挡位*/ \
|
||
X(DriveMode, "driveMode") /*行车模式*/ \
|
||
X(Load, "load") /*重载信号*/ \
|
||
X(Voltage, "voltage") /*高压上下电*/ \
|
||
X(PositionLight, "positionLight") /*示廓灯*/ \
|
||
X(Warning, "warning") /*双闪*/ \
|
||
X(FogLight, "fogLight") /*雾灯*/ \
|
||
X(Headlamp, "headlamp") /*前大灯*/ \
|
||
X(TailLight, "tailLight") /*后大灯*/ \
|
||
X(Lock, "lock") /*锁车*/ \
|
||
X(StopCtrl, "stopCtrl") /*停止远控*/ \
|
||
X(StartCtrl, "startCtrl") /*开始远控*/
|
||
|
||
// 枚举类型定义
|
||
|
||
enum class CommandType
|
||
{
|
||
#define GEN_ENUM(name, str) name,
|
||
COMMAND_LIST(GEN_ENUM)
|
||
#undef GEN_ENUM
|
||
Unknown
|
||
};
|
||
|
||
// 字符串到枚举的映射
|
||
static const std::unordered_map<std::string, CommandType> command_map = {
|
||
#define GEN_MAP(name, str) {str, CommandType::name},
|
||
COMMAND_LIST(GEN_MAP)
|
||
#undef GEN_MAP
|
||
};
|
||
|
||
extern Remote_Ctl_Def veh_rc_data;
|
||
extern std::mutex veh_rc_mutex;
|
||
|
||
extern VehicleInfoReport veh_report_data;
|
||
extern std::mutex veh_report_mutex;
|