684 lines
20 KiB
Plaintext
684 lines
20 KiB
Plaintext
import socket from '@ohos.net.socket'
|
||
import Queue from '@ohos.util.Queue'
|
||
import { LogHelper } from '../LogHelper'
|
||
|
||
interface ApaListener {
|
||
onApaStart(): void
|
||
|
||
onApaStop(): void
|
||
}
|
||
|
||
interface AutomaticListener {
|
||
onPathTrackingStart(): void
|
||
|
||
onPathTrackingStop(): void
|
||
}
|
||
|
||
class ApaListener implements ApaListener {
|
||
private listener: AutomaticListener
|
||
|
||
constructor(listener: AutomaticListener) {
|
||
this.listener = listener
|
||
}
|
||
|
||
onApaStart() {
|
||
if (this.listener) {
|
||
this.listener.onPathTrackingStart()
|
||
}
|
||
}
|
||
|
||
onApaStop() {
|
||
if (this.listener) {
|
||
this.listener.onPathTrackingStop()
|
||
}
|
||
}
|
||
}
|
||
|
||
function sleep(time: number) {
|
||
return new Promise<void>((resolve, reject) => {
|
||
setTimeout(() => {
|
||
resolve()
|
||
}, time)
|
||
})
|
||
}
|
||
|
||
class TrackInfo {
|
||
public static CAR_IN_LAND = 1; //车在平地
|
||
public static CAR_IN_RAMP_UP = 2; //车在上坡
|
||
public static CAR_IN_RAMP_DOWN = 3; //车在下坡
|
||
// private static TYPE_1_LIB = 1; //库位
|
||
// private static TYPE_2_UPRAMP = 2; //上坡模式
|
||
// private static TYPE_3_DOWNRAMP = 3;
|
||
}
|
||
|
||
|
||
class TrackMode {
|
||
public static MODE_TEACH = "teach";
|
||
public static MODE_LIB = "lib";
|
||
public static MODE_PATH = "path";
|
||
}
|
||
|
||
|
||
class CtrlParam {
|
||
public static k = 0.1; // look forward gain
|
||
public static Lfc = 6.5; // [m] look-ahead distance
|
||
public static Kp = 1.0; // speed proportional gain
|
||
public static dt = 0.1; // [s] time tick
|
||
public static WB = 2.42; // [m] wheel base of vehicle
|
||
public static STEERING_RATIO = 16; //哪吒V型车转向比（哪吒底盘人员提供，与速度关系不大，与方向盘关系大，一般为抛物线关系，已获取到文档）
|
||
}
|
||
|
||
class Message {
|
||
deadline: number;
|
||
angle = 0;
|
||
|
||
constructor(deadline: number, angle: number) {
|
||
this.deadline = deadline
|
||
this.angle = angle
|
||
}
|
||
}
|
||
|
||
|
||
export class WireControl {
|
||
private static FXP_GAP_TIME: number = 180;
|
||
private static FXP_GAP_DEGREE: number = 40;
|
||
private TAG: string = "automatic";
|
||
private mAutomaticListener: AutomaticListener;
|
||
private mApaListener: ApaListener;
|
||
private socket: socket.UDPSocket
|
||
private apaIp: string;
|
||
private apaPort: number;
|
||
private APA2_HEAD: number[] = [0x08, 0x00, 0x00, 0x02, 0xba]; // 8 0 0 2 186
|
||
private APA3_HEAD: number[] = [0x08, 0x00, 0x00, 0x02, 0x7b]; // 8 0 0 2
|
||
private APA4_HEAD: number[] = [0x08, 0x00, 0x00, 0x02, 0x53];
|
||
private APA_WORKING: number = 0;
|
||
private APA_BRAKE_STATUS: number = 0;
|
||
private APA_InfoDisplayReq: number = 0; // Advanced Parking Information display request
|
||
private APA_EPSAngleReq: number = 1; // APA是否请求转向
|
||
private APA_EPSAngleReqValidity: number = 1; // APA请求转向有效位
|
||
private APA_EPSAngleValueReq: number = 0; //@@@ APA请求转向角度（-540~+540）
|
||
private APA_WorkSts_CH: number = 0; // APA状态
|
||
private APA_TurnLightReq: number = 0; // APA请求左/右转向闪灯
|
||
private APA3_GearPosReq: number = 0; //@@@ 目标档位信号 0X1:P 0X2:R 0X3:N 0X4:D
|
||
private APA3_GearPosReqValidity: number = 1; // 目标档位信号的有效性
|
||
private APA3_TorqReq: number = 0; // 请求VCU目标扭矩
|
||
private APA3_TorqReqValue: number = 512; //@@@ 请求VCU目标扭矩值
|
||
private APA3_TorqReqValidity: number = 1; // 请求VCU目标扭矩状态
|
||
private APA4_ApaMod: number = 0; // APA工作状态
|
||
private APA4_ApaBrkMod: number = 0; // APA制动控制模式
|
||
private APA4_ApaStopReq: number = 0; //@@@ APA请求Stop 0是松 1是拉
|
||
private APA4_ApaEpbReq: number = 0; // APA请求EPB协助
|
||
private APA4_ApaMaxSpd: number = 0; //@@@ APA最大允许车速
|
||
private APA4_ApaTarDecel: number = 0; // APA请求目标减速度
|
||
private APA4_APASwFd: number = 0; // APA设置开关反馈信息
|
||
private APA_Fr02_MsgCounter: number = 0; // 校验位
|
||
private APA3_Fr03_MsgCounter: number = 0; // 校验位
|
||
private APA4_Fr04_MsgCounter: number = 0; // 校验位
|
||
// APA2
|
||
private APA2_APA_HornReq: number = 0; // 喇叭 0x0——关 0x1——开
|
||
//0:unInit; 1:initing; 2:init fail; 3:init success
|
||
private initStatus: number;
|
||
private apaWorking: boolean;
|
||
private isInit: boolean;
|
||
private lastAngle: number = 0;
|
||
private angleHandle: number = 0;
|
||
private lastItem: string = "";
|
||
private lastDoor: number = -1;
|
||
protected cruiseModeSwitch: boolean = true;
|
||
protected rampUpModeSwitch: boolean = true;
|
||
protected rampDownModeSwitch: boolean = true;
|
||
|
||
constructor() {
|
||
this.mApaListener = new ApaListener(this.mAutomaticListener)
|
||
}
|
||
|
||
public setMAutomaticListener(mAutomaticListener: AutomaticListener) {
|
||
this.mAutomaticListener = mAutomaticListener;
|
||
}
|
||
|
||
public getMAutomaticListener() {
|
||
return this.mAutomaticListener;
|
||
}
|
||
|
||
public init(ip: string, port: number) {
|
||
this.apaIp = ip;
|
||
this.apaPort = port;
|
||
this.socket = socket.constructUDPSocketInstance()
|
||
this.socket.setExtraOptions({ reuseAddress: true });
|
||
return this.socket.bind({ address: "0.0.0.0", port: 31021 }).then(() => {
|
||
this.isInit = true;
|
||
})
|
||
}
|
||
|
||
public apaStart() {
|
||
if (!this.isInit) {
|
||
return
|
||
}
|
||
this.APA_EPSAngleValueReq = 0;
|
||
this.lastAngle = 0;
|
||
if (this.apaWorking) return;
|
||
this.apaWorking = true;
|
||
setInterval(() => {
|
||
if (this.apaWorking) {
|
||
this.sendAutomaticMsg()
|
||
}
|
||
}, 20)
|
||
sleep(3000).then(() => {
|
||
this.initStatus = 1;
|
||
this.APA_EPSAngleValueReq = 0;
|
||
this.APA_WorkSts_CH = 0x2;
|
||
return sleep(2000)
|
||
}).then(() => {
|
||
this.APA_WorkSts_CH = 0x3;
|
||
this.APA3_TorqReq = 1;
|
||
this.APA4_ApaBrkMod = 1;
|
||
this.APA4_ApaMod = 1;
|
||
this.APA4_APASwFd = 1;
|
||
return sleep(60)
|
||
}).then(() => {
|
||
this.APA_WorkSts_CH = 0x4;
|
||
this.APA4_ApaBrkMod = 2;
|
||
this.APA4_ApaStopReq = 1;
|
||
this.APA4_ApaEpbReq = 1;
|
||
this.APA4_ApaTarDecel = 0xc8;
|
||
return sleep(2000)
|
||
}).then(() => {
|
||
this.APA4_ApaMaxSpd = 0x80;
|
||
this.APA4_ApaTarDecel = 0;
|
||
this.initStatus = 3;
|
||
return sleep(2000)
|
||
});
|
||
this.mApaListener.onApaStart();
|
||
}
|
||
|
||
public apaStop() {
|
||
if (!this.isInit) return;
|
||
this.mApaListener.onApaStop();
|
||
this.apaOff();
|
||
sleep(3000).then(() => {
|
||
this.apaWorking = false
|
||
})
|
||
}
|
||
|
||
public apaOff() {
|
||
this.initStatus = 0;
|
||
this.brakeOn();
|
||
this.APA_WorkSts_CH = 0x0;
|
||
this.APA3_TorqReq = 0;
|
||
this.APA4_ApaBrkMod = 0;
|
||
this.APA4_ApaMod = 0;
|
||
this.APA4_ApaStopReq = 0;
|
||
this.APA4_ApaEpbReq = 0;
|
||
this.APA4_ApaMaxSpd = 0;
|
||
this.APA4_ApaTarDecel = 0;
|
||
this.APA4_APASwFd = 0;
|
||
this.APA_EPSAngleValueReq = 0;
|
||
}
|
||
|
||
public apaInit() {
|
||
if (this.initStatus != 0) return;
|
||
this.initStatus = 1;
|
||
this.APA_EPSAngleValueReq = 0;
|
||
this.APA_WorkSts_CH = 0x2;
|
||
sleep(2000).then(() => {
|
||
this.APA_WorkSts_CH = 0x3;
|
||
this.APA3_TorqReq = 1;
|
||
this.APA4_ApaBrkMod = 1;
|
||
this.APA4_ApaMod = 1;
|
||
this.APA4_APASwFd = 1;
|
||
return sleep(60)
|
||
}).then(() => {
|
||
this.APA_WorkSts_CH = 0x4;
|
||
this.APA4_ApaBrkMod = 2;
|
||
this.APA4_ApaStopReq = 1;
|
||
this.APA4_ApaEpbReq = 1;
|
||
this.APA4_ApaTarDecel = 0xc8;
|
||
return sleep(2000)
|
||
}).then(() => {
|
||
this.APA4_ApaStopReq = 0;
|
||
this.APA4_ApaMaxSpd = 0x80; //0xc0;
|
||
this.APA4_ApaTarDecel = 0;
|
||
this.initStatus = 3;
|
||
return sleep(2000)
|
||
}).then(() => {
|
||
this.moveUp();
|
||
})
|
||
|
||
}
|
||
|
||
// 设定控制模式
|
||
private setControlMode(carLocate: number, mode: string) {
|
||
switch (carLocate) {
|
||
case -1:
|
||
case TrackInfo.CAR_IN_LAND:
|
||
this.cruiseMode();
|
||
break;
|
||
case TrackInfo.CAR_IN_RAMP_UP:
|
||
this.rampUpMode();
|
||
break;
|
||
case TrackInfo.CAR_IN_RAMP_DOWN:
|
||
this.rampDownMode();
|
||
break;
|
||
}
|
||
|
||
if (mode === TrackMode.MODE_LIB) {
|
||
this.lowSpeedCruiseMode();
|
||
}
|
||
}
|
||
|
||
/**
|
||
* apa指令接口
|
||
* @param cmd
|
||
*/
|
||
|
||
public apaCmd(cmd: string, mode: string, carLocate: number, isPathTrackEnd: boolean, doorSwitch: number) {
|
||
if (doorSwitch == 1 && this.lastDoor != 1) {
|
||
this.brakeOn();
|
||
} else if (doorSwitch == 0 && this.lastDoor != 0) {
|
||
this.moveUp();
|
||
}
|
||
this.lastDoor = doorSwitch;
|
||
|
||
if (isPathTrackEnd) {
|
||
this.brakeOn();
|
||
LogHelper.I(LogHelper.RESULT_PURPERSUIT, "导航到最后点位，停车拉手刹！");
|
||
return;
|
||
}
|
||
|
||
if (cmd === "") {
|
||
return;
|
||
}
|
||
let params = cmd.split(";");
|
||
let fxp = parseFloat(params[0]);
|
||
let move = parseInt(params[1]);
|
||
|
||
if (fxp != -1000) {
|
||
this.fxpCtrl(fxp, mode);
|
||
}
|
||
|
||
this.setControlMode(carLocate, mode);
|
||
|
||
if (move != -1000) {
|
||
if (move == 1) {
|
||
this.moveUp();
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "前进");
|
||
} else if (move == -1) {
|
||
this.moveDown();
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "后退");
|
||
} else if (move == 0) {
|
||
this.moveStop();
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "停止");
|
||
} else if (move == 10) {
|
||
this.brakeOn();
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "刹车");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
private handleAngle(di: number) {
|
||
|
||
let angle = di * 180 / Math.PI;
|
||
if (angle > 45) {
|
||
angle = 45;
|
||
} else if (angle < -45) {
|
||
angle = -45;
|
||
}
|
||
let fxp: number = angle * CtrlParam.STEERING_RATIO;
|
||
|
||
return fxp;
|
||
}
|
||
|
||
public fxpCtrl(angle: number, mode: string) {
|
||
let angle2 = 0;
|
||
if (mode === TrackMode.MODE_PATH) {
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "输出方向盘PATH：" + this.handleAngle(angle));
|
||
this.APA4_ApaMaxSpd = 0xFF; //0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码 0x13F-5码 0x17F_6码 0x1BF_7码 0x200_8码
|
||
// 0x300——12码 0x3c0——15码 0x500——20码 0x640——25码 0x780——30码
|
||
angle2 = this.handleAngle(angle);
|
||
} else if (mode === TrackMode.MODE_TEACH) {
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "输出方向盘：" + angle);
|
||
this.APA4_ApaMaxSpd = 0x80; //0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码 0x13F-5码 0x17F_6码 0x1BF_7码 0x200_8码
|
||
angle2 = angle;
|
||
} else {
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "输出方向盘：" + angle);
|
||
this.APA4_ApaMaxSpd = 0x80; //0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码 0x13F-5码 0x17F_6码 0x1BF_7码 0x200_8码
|
||
angle2 = angle;
|
||
}
|
||
|
||
if (this.initStatus != 3) return;
|
||
if (angle2 > 540) {
|
||
angle2 = 540;
|
||
} else if (angle < -540) {
|
||
angle2 = -540;
|
||
}
|
||
if (mode === TrackMode.MODE_PATH) {
|
||
this.fxp4(angle2);
|
||
} else {
|
||
this.fxp4(angle2);
|
||
}
|
||
|
||
}
|
||
|
||
private fxp2(angle: number) {
|
||
// 要实现方向盘的逐步转向，哪吒控制协议不支持单步过大的操作，之前的转向
|
||
// 策略为：每次转30度，300毫秒完成一次转向
|
||
if (Math.abs(this.lastAngle - angle) < 10) {
|
||
return;
|
||
}
|
||
|
||
if (Math.abs(angle - this.APA_EPSAngleValueReq) < 50) {
|
||
this.APA_EPSAngleValueReq = angle;
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "-----方向盘变化小，直接执行：" + this.APA_EPSAngleValueReq);
|
||
return;
|
||
}
|
||
|
||
this.angleHandle = angle - this.lastAngle;
|
||
|
||
let queue = new Queue<Message>()
|
||
let now = new Date().getUTCMilliseconds();
|
||
let angleHandleAbs = Math.abs(this.angleHandle);
|
||
|
||
for (let i = 0; i < 30; i++) {
|
||
if (angleHandleAbs > 0) {
|
||
if (this.angleHandle > 0) {
|
||
queue.add(new Message(now + i * WireControl.FXP_GAP_TIME, Math.min(WireControl.FXP_GAP_DEGREE, angleHandleAbs)));
|
||
} else {
|
||
queue.add(new Message(now + i * WireControl.FXP_GAP_TIME, -Math.min(WireControl.FXP_GAP_DEGREE, angleHandleAbs)));
|
||
}
|
||
angleHandleAbs -= WireControl.FXP_GAP_DEGREE;
|
||
} else {
|
||
break;
|
||
}
|
||
}
|
||
|
||
let temp = "";
|
||
for (let ms of queue) {
|
||
temp = temp + "&" + ms.angle;
|
||
}
|
||
|
||
LogHelper.I(LogHelper.RESULT_TRACKING, "queue：" + temp)
|
||
|
||
setInterval(() => {
|
||
if (queue.length > 0) {
|
||
this.APA_EPSAngleValueReq += queue.pop().angle;
|
||
}
|
||
}, WireControl.FXP_GAP_TIME)
|
||
|
||
this.lastAngle = angle;
|
||
}
|
||
|
||
private fxp3(fxpAngle: number) {
|
||
let gap = Math.abs(fxpAngle - this.APA_EPSAngleValueReq);
|
||
if (gap <= 10) {
|
||
} else {
|
||
let step = gap * 45 / 540;
|
||
step = Math.min(45, step);
|
||
step = Math.max(2, step);
|
||
LogHelper.I(this.TAG, "AAA step:" + step);
|
||
if (fxpAngle > this.APA_EPSAngleValueReq) {
|
||
this.APA_EPSAngleValueReq += step;
|
||
} else if (fxpAngle < this.APA_EPSAngleValueReq) {
|
||
this.APA_EPSAngleValueReq -= step;
|
||
}
|
||
}
|
||
}
|
||
|
||
public fxp4(fxpAngle: number) {
|
||
LogHelper.I("xwq22", "angle:" + fxpAngle + " APA_EPSAngleValueReq:" + this.APA_EPSAngleValueReq);
|
||
let gap = Math.abs(fxpAngle - this.APA_EPSAngleValueReq);
|
||
if (gap <= 10) {
|
||
// do nothing
|
||
} else if (gap < 40) {
|
||
this.APA_EPSAngleValueReq = fxpAngle;
|
||
} else {
|
||
if (fxpAngle > this.APA_EPSAngleValueReq) {
|
||
this.APA_EPSAngleValueReq += 40;
|
||
} else if (fxpAngle < this.APA_EPSAngleValueReq) {
|
||
this.APA_EPSAngleValueReq -= 40;
|
||
}
|
||
}
|
||
}
|
||
|
||
private fxp(angle: number) {
|
||
LogHelper.I(this.TAG, "angle:" + angle + " APA_EPSAngleValueReq:" + this.APA_EPSAngleValueReq);
|
||
let gap = Math.abs(angle - this.APA_EPSAngleValueReq);
|
||
if (gap <= 10) {
|
||
this.APA_EPSAngleValueReq = angle;
|
||
} else {
|
||
let step = gap * 45 / 540;
|
||
step = Math.min(45, step);
|
||
step = Math.max(2, step);
|
||
if (angle > this.APA_EPSAngleValueReq) {
|
||
this.APA_EPSAngleValueReq += step;
|
||
} else if (angle < this.APA_EPSAngleValueReq) {
|
||
this.APA_EPSAngleValueReq -= step;
|
||
}
|
||
}
|
||
}
|
||
|
||
private lowSpeedCruiseMode() {
|
||
this.APA3_TorqReqValue = 536; // 正常536 坡道610
|
||
this.APA4_ApaMaxSpd = 0x40; //0xc0;0x80 //0x40为1码、0x80为2码、c0为3码 0xFF-4码 0x13F-5码 0x17F_6码 0x1BF_7码 0x200_8码
|
||
}
|
||
|
||
//正常巡航模式
|
||
private cruiseMode() {
|
||
if (this.cruiseModeSwitch) {
|
||
this.APA3_TorqReqValue = 536; // 正常536 坡道610
|
||
// APA4_ApaMaxSpd = 0xc0;//0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码 0x13F-5码 0x17F_6码 0x1BF_7码 0x200_8码
|
||
|
||
this.cruiseModeSwitch = false;
|
||
this.rampUpModeSwitch = true;
|
||
this.rampDownModeSwitch = true;
|
||
}
|
||
}
|
||
|
||
// 坡道上坡模式
|
||
private rampUpMode() {
|
||
if (this.rampUpModeSwitch) {
|
||
this.APA3_TorqReqValue = 610; // 正常536 坡道610
|
||
// APA4_ApaMaxSpd = 0xc0;//0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码 0x13F-5码 0x200_8码
|
||
|
||
this.rampUpModeSwitch = false;
|
||
this.cruiseModeSwitch = true;
|
||
this.rampDownModeSwitch = true;
|
||
}
|
||
|
||
}
|
||
|
||
// 坡道下坡模式
|
||
private rampDownMode() {
|
||
if (this.rampDownModeSwitch) {
|
||
this.APA3_TorqReqValue = 488; // 正常536 坡道610
|
||
this.APA4_ApaTarDecel = 0x76C; //0x12C:300 0xc8:200(当前刹车力度) 0x190:400 0x1F4:500 0x258:600 0x320:800 0x3E8:1000 0x5DC:1500 0x76C:1900
|
||
this.rampDownModeSwitch = false;
|
||
this.cruiseModeSwitch = true;
|
||
this.rampUpModeSwitch = true;
|
||
}
|
||
|
||
}
|
||
|
||
// 起步
|
||
public moveUp() {
|
||
if (this.initStatus != 3) return;
|
||
this.brakeOff();
|
||
this.APA3_GearPosReq = 0x4;
|
||
this.APA3_TorqReqValue = 536; // 正常536 坡道610
|
||
// this.carDir = "车辆状态：前进";
|
||
}
|
||
|
||
// 停车
|
||
public moveStop() {
|
||
if (this.initStatus != 3) return;
|
||
this.APA3_GearPosReq = 0x3;
|
||
this.APA3_TorqReqValue = 512;
|
||
// carDir = "车辆状态：停车";
|
||
}
|
||
|
||
// 后退
|
||
public moveDown() {
|
||
if (this.initStatus != 3) return;
|
||
this.brakeOff();
|
||
this.APA3_GearPosReq = 0x2;
|
||
this.APA3_TorqReqValue = 488;
|
||
// carDir = "车辆状态：后退";
|
||
}
|
||
|
||
// 拉手刹
|
||
public brakeOn() {
|
||
if (this.initStatus != 3) return;
|
||
this.moveStop();
|
||
this.APA4_ApaStopReq = 0x1;
|
||
this.APA4_ApaTarDecel = 0xc8;
|
||
}
|
||
|
||
// 松手刹
|
||
public brakeOff() {
|
||
if (this.initStatus != 3) return;
|
||
this.APA4_ApaStopReq = 0;
|
||
this.APA4_ApaTarDecel = 0;
|
||
}
|
||
|
||
// 左转向灯
|
||
public leftLight() {
|
||
this.APA_TurnLightReq = 0x1;
|
||
}
|
||
|
||
// 右转向灯
|
||
public rightLight() {
|
||
this.APA_TurnLightReq = 0x2;
|
||
}
|
||
|
||
// 双跳灯
|
||
public doubleLight() {
|
||
this.APA_TurnLightReq = 0x3;
|
||
}
|
||
|
||
// 关闭转向灯
|
||
public shutLight() {
|
||
this.APA_TurnLightReq = 0x0;
|
||
}
|
||
|
||
// 开喇叭
|
||
public onHorn() {
|
||
this.APA2_APA_HornReq = 0x1;
|
||
}
|
||
|
||
// 关喇叭
|
||
public offHorn() {
|
||
this.APA2_APA_HornReq = 0x0;
|
||
}
|
||
|
||
// 增加扭矩
|
||
public addTorq() {
|
||
this.APA3_TorqReqValue += 5;
|
||
}
|
||
|
||
// 减少扭矩
|
||
public redTorq() {
|
||
this.APA3_TorqReqValue -= 5;
|
||
}
|
||
|
||
// 设置档位
|
||
public setGear(gear: string) {
|
||
if ("N" === gear) { //空挡N
|
||
this.APA3_GearPosReq = 0x3;
|
||
} else if ("P" === gear) { //驻车档P
|
||
this.APA3_GearPosReq = 0x1;
|
||
} else if ("R" === gear) { //倒挡R
|
||
this.APA3_GearPosReq = 0x2;
|
||
} else if ("D" === gear) { //前进D
|
||
this.APA3_GearPosReq = 0x4;
|
||
}
|
||
}
|
||
|
||
private sendAutomaticMsg() {
|
||
this.sendAPA4Data();
|
||
this.sendAPA3Data();
|
||
this.sendAPA2Data();
|
||
}
|
||
|
||
private sendAPA2Data() {
|
||
let data = new Uint8Array(13);
|
||
data.set(this.APA2_HEAD);
|
||
let index = 5;
|
||
data[index++] = this.APA_InfoDisplayReq;
|
||
let angle = (-this.APA_EPSAngleValueReq + 780) * 10;
|
||
data[index++] = ((angle & 0xff00) >> 8);
|
||
data[index++] = (angle & 0xff);
|
||
// 20240527 添加喇叭信号
|
||
data[index++] = ((((this.APA_EPSAngleReq & 0x1) << 7) | ((this.APA_EPSAngleReqValidity & 0x1) << 6) | ((this.APA2_APA_HornReq & 0x1) << 4)) | 0x22);
|
||
data[index++] = (((this.APA_WorkSts_CH & 0x7) << 5) | (this.APA_TurnLightReq & 0x3));
|
||
data[index++] = 0;
|
||
this.APA_Fr02_MsgCounter += 1;
|
||
if (this.APA_Fr02_MsgCounter > 15) {
|
||
this.APA_Fr02_MsgCounter = 0;
|
||
}
|
||
data[index++] = this.APA_Fr02_MsgCounter & 0xf;
|
||
let checkSum = 0;
|
||
for (let i = 5; i < 12; i++) {
|
||
checkSum ^= data[i];
|
||
}
|
||
data[index++] = checkSum & 0xff;
|
||
this.sendAutomaticData(data);
|
||
}
|
||
|
||
private sendAPA3Data() {
|
||
let data = new Uint8Array(13);
|
||
data.set(this.APA3_HEAD);
|
||
let index = 5;
|
||
data[index++] = 0;
|
||
data[index++] = 0;
|
||
data[index++] = ((this.APA3_GearPosReq & 0x7) << 5) | (this.APA3_GearPosReqValidity << 4) | (this.APA3_TorqReq << 3) | ((this.APA3_TorqReqValue & 0x380) >> 7);
|
||
data[index++] = (this.APA3_TorqReqValue & 0x7f) << 1
|
||
data[index++] = this.APA3_TorqReqValidity << 7;
|
||
data[index++] = 0;
|
||
this.APA3_Fr03_MsgCounter += 1;
|
||
if (this.APA3_Fr03_MsgCounter > 15) {
|
||
this.APA3_Fr03_MsgCounter = 0;
|
||
}
|
||
|
||
data[index++] = this.APA3_Fr03_MsgCounter & 0xf;
|
||
let checkSum = 0;
|
||
for (let i = 5; i < 12; i++) {
|
||
checkSum ^= data[i];
|
||
}
|
||
|
||
data[index++] = checkSum & 0xff;
|
||
this.sendAutomaticData(data);
|
||
}
|
||
|
||
private sendAPA4Data() {
|
||
let data = new Uint8Array(13);
|
||
data.set(this.APA4_HEAD);
|
||
let index = 5;
|
||
data[index++] = 0x4 | (this.APA4_ApaEpbReq & 0x3) | (this.APA4_APASwFd << 3);
|
||
data[index++] = 0;
|
||
data[index++] = 0x00 | (this.APA4_ApaTarDecel & 0x700) >> 8; // 第二字节 去9 10 11 三位
|
||
data[index++] = this.APA4_ApaTarDecel & 0xff; // 第三字节 取低八位
|
||
data[index++] = (this.APA4_ApaMaxSpd & 0xff00) >> 8; // 第四字节 取 速度的高八位
|
||
data[index++] = this.APA4_ApaMaxSpd & 0xff; // 第五字节 取 速度的低八位
|
||
this.APA4_Fr04_MsgCounter += 1;
|
||
if (this.APA4_Fr04_MsgCounter > 15) {
|
||
this.APA4_Fr04_MsgCounter = 0;
|
||
}
|
||
data[index++] = (this.APA4_Fr04_MsgCounter & 0xf) | (this.APA4_ApaStopReq << 7) | (this.APA4_ApaMod << 6) | (this.APA4_ApaBrkMod << 4);
|
||
let checkSum = 0;
|
||
for (let i = 5; i < 12; i++) {
|
||
checkSum ^= data[i];
|
||
}
|
||
data[index++] = checkSum & 0xff;
|
||
this.sendAutomaticData(data);
|
||
}
|
||
|
||
private sendAutomaticData(data: Uint8Array) {
|
||
this.socket.send({
|
||
address: {
|
||
address: this.apaIp,
|
||
port: this.apaPort,
|
||
},
|
||
data: data.buffer
|
||
})
|
||
}
|
||
} |