|
|
@@ -1,5 +1,4 @@
|
|
|
#include "can_bus.h"
|
|
|
-#include "vehicle.h"
|
|
|
#include <thread>
|
|
|
#include "message_queue.h"
|
|
|
#include <math.h>
|
|
|
@@ -216,7 +215,104 @@ void CCanBusSensor::Notify(struct can_frame *date) //上传状态信息
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-/*-----------------小车控制------------------*/
|
|
|
+
|
|
|
+/*------------------小车控制起-------------------*/
|
|
|
+
|
|
|
+void CCanBusSensor::order(cannet_frame* frames){
|
|
|
+
|
|
|
+ cannet_frame& frame = frames[0];
|
|
|
+ Sendframe[0].can_id = 0x421;
|
|
|
+ Sendframe[0].can_dlc = frame.dlc;
|
|
|
+
|
|
|
+ memcpy(Sendframe[0].data,order_on,frame.dlc);
|
|
|
+ order_flag = true;
|
|
|
+
|
|
|
+ if(!order_flag){
|
|
|
+ cout<<"order_flag: false"<<endl;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void CCanBusSensor::toggleLamp(cannet_frame* frames){
|
|
|
+
|
|
|
+ cannet_frame& frame = frames[0];
|
|
|
+ Sendframe[0].can_id = 0x121;
|
|
|
+ Sendframe[0].can_dlc = frame.dlc;
|
|
|
+
|
|
|
+ switch (frame.data[0])
|
|
|
+ {
|
|
|
+ case 0x00:
|
|
|
+ memcpy(Sendframe[0].data,headlamp_off,frame.dlc); //前后车灯关
|
|
|
+ std::cout<<"前车灯关,后车灯关 "<<std::endl;
|
|
|
+ break;
|
|
|
+
|
|
|
+ case 0x10:
|
|
|
+ memcpy(Sendframe[0].data,headlamp_on,frame.dlc); //前车灯开
|
|
|
+ std::cout<<"前车灯开,后车灯关 "<<std::endl;
|
|
|
+ break;
|
|
|
+
|
|
|
+ case 0x08:
|
|
|
+ memcpy(Sendframe[0].data,taillamp_on,frame.dlc); //后车灯开
|
|
|
+ std::cout<<"前车灯关,后车灯开 "<<std::endl;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void CCanBusSensor::car_stop(cannet_frame* frames){
|
|
|
+
|
|
|
+ cannet_frame& frame = frames[0];
|
|
|
+ Sendframe[0].can_id = 0x111;
|
|
|
+ Sendframe[0].can_dlc = frame.dlc;
|
|
|
+
|
|
|
+ if(frame.data[2]==0x00){
|
|
|
+ memcpy(Sendframe[0].data,stop,frame.dlc); //急停
|
|
|
+
|
|
|
+ stop_flag = true;
|
|
|
+ if(stop_flag){
|
|
|
+ std::cout<<"stop_flag: true"<<std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void CCanBusSensor::move_run(cannet_frame* frames){
|
|
|
+
|
|
|
+ cannet_frame& frame = frames[0];
|
|
|
+ int32_t canid = frame.canid;
|
|
|
+
|
|
|
+ Sendframe[0].can_id = 0x111;
|
|
|
+ Sendframe[0].can_dlc = frame.dlc;
|
|
|
+
|
|
|
+ if(frame.data[4]>0x02 && frame.data[4]<=0x7F){
|
|
|
+ Sendframe[0].data = move_forward; //油门前进
|
|
|
+ run_flag = true;
|
|
|
+ }
|
|
|
+ else if(frame.data[5]>0x02 && frame.data[5]<=0x7F){
|
|
|
+ memcpy(Sendframe[0].data,stop,frame.dlc); //刹车后退
|
|
|
+ run_flag = true;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void CCanBusSensor::move_rot(cannet_frame* frames){
|
|
|
+
|
|
|
+ cannet_frame& frame = frames[0];
|
|
|
+ int32_t canid = frame.canid;
|
|
|
+
|
|
|
+ Sendframe[1].can_id = 0x111;
|
|
|
+ Sendframe[1].can_dlc = frame.dlc;
|
|
|
+
|
|
|
+ if(frame.data[0] != 0x01){
|
|
|
+ memcpy(Sendframe[0].data,move_rotation,frame.dlc); //转弯
|
|
|
+ rot_flag = true;
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+/*------------------小车控制止-------------------*/
|
|
|
+
|
|
|
|
|
|
void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count) //下发控制指令
|
|
|
{
|
|
|
@@ -232,26 +328,80 @@ void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count) //下发控
|
|
|
switch (canid)
|
|
|
{
|
|
|
case 0x181:
|
|
|
- Sendframe[0].can_id = 0x121;
|
|
|
+ // Sendframe[0].can_id = 0x421;
|
|
|
Sendframe[0].can_dlc = frame.dlc;
|
|
|
|
|
|
- // Hand_Throttle = make_int16(frame.data[1],frame.data[0]); //手油门
|
|
|
- // Foot_Throttle = make_int16(frame.data[3],frame.data[2]); //脚油门
|
|
|
- // Brake = make_int16(frame.data[5],frame.data[4]); //刹车
|
|
|
-
|
|
|
// memcpy(Sendframe[0].data, frame.data, frame.dlc);
|
|
|
+ // Hand_Throttle = make_int16(frame.data[1],frame.data[0]);
|
|
|
+ // Foot_Throttle = make_int16(frame.data[3],frame.data[2]);
|
|
|
+ // Brake = make_int16(frame.data[5],frame.data[4]);
|
|
|
+ // cout<<"receive 181: "<<Hand_Throttle<<" "<<Foot_Throttle<<" "<<Brake<<endl;
|
|
|
+
|
|
|
+ // 指令模式
|
|
|
+ // memcpy(Sendframe[0].data,order_on,frame.dlc);
|
|
|
+
|
|
|
+ CCanBusSensor::order(&frame);
|
|
|
+ CCanBusSensor::toggleLamp(&frame);
|
|
|
+ // CCanBusSensor::move_run(&frame); //move_run和car
|
|
|
+ // CCanBusSensor::car_stop(&frame);
|
|
|
+
|
|
|
+ // 车灯
|
|
|
+ // Sendframe[0].can_id = 0x121;
|
|
|
+ // memcpy(Sendframe[0].data,headlamp_on,frame.dlc);
|
|
|
+ // switch (frame.data[0])
|
|
|
+ // {
|
|
|
+ // case 0x00:
|
|
|
+ // memcpy(Sendframe[0].data,headlamp_off,frame.dlc); //前后车灯关
|
|
|
+ // std::cout<<"前车灯关,后车灯关 "<<std::endl;
|
|
|
+ // break;
|
|
|
+
|
|
|
+ // case 0x10:
|
|
|
+ // memcpy(Sendframe[0].data,headlamp_on,frame.dlc); //前车灯开
|
|
|
+ // std::cout<<"前车灯开,后车灯关 "<<std::endl;
|
|
|
+ // break;
|
|
|
+
|
|
|
+ // case 0x08:
|
|
|
+ // memcpy(Sendframe[0].data,taillamp_on,frame.dlc); //后车灯开
|
|
|
+ // std::cout<<"前车灯关,后车灯开 "<<std::endl;
|
|
|
+ // break;
|
|
|
+ // }
|
|
|
+
|
|
|
+
|
|
|
+ // 前进后退
|
|
|
+ // Sendframe[0].can_id = 0x111;
|
|
|
+ // if(frame.data[4]>0x02 && frame.data[4]<=0x7F){
|
|
|
+ // memcpy(Sendframe[0].data,move_forward,frame.dlc); //油门前进
|
|
|
+ // run_flag = true;
|
|
|
+ // }
|
|
|
+ // else if(frame.data[5]>0x02 && frame.data[5]<=0x7F){
|
|
|
+ // memcpy(Sendframe[0].data,stop,frame.dlc); //刹车后退
|
|
|
+ // run_flag = true;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // // 急停
|
|
|
+ // Sendframe[0].can_id = 0x111;
|
|
|
+ // if(frame.data[2]==0x00 && frame.data[3]==0xFF){
|
|
|
+ // memcpy(Sendframe[0].data,stop,frame.dlc); //急停
|
|
|
+ // stop_flag = true;
|
|
|
+ // if(stop_flag){
|
|
|
+ // cout<<"stop_flag: true"<<endl;
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+
|
|
|
|
|
|
- memcpy(Sendframe[0].data,order_on,sizeof(order_on));
|
|
|
- memcpy(Sendframe[0].data,headlamp_on,sizeof(headlamp_on));
|
|
|
|
|
|
- if(frame.data[0]==0x10)
|
|
|
- memcpy(Sendframe[0].data,headlamp_on,sizeof(headlamp_on));
|
|
|
- else
|
|
|
- memcpy(Sendframe[0].data,headlamp_off,sizeof(headlamp_off));
|
|
|
|
|
|
-
|
|
|
break;
|
|
|
|
|
|
+ // case 0x0CFDD634:
|
|
|
+
|
|
|
+ // Sendframe[1].can_dlc = frame.dlc;
|
|
|
+
|
|
|
+ // CCanBusSensor::order(&frame);
|
|
|
+ // CCanBusSensor::move_rot(&frame);
|
|
|
+
|
|
|
+ // break;
|
|
|
+
|
|
|
case 0x182:
|
|
|
Sendframe[1].can_id = 0x182;
|
|
|
Sendframe[1].can_dlc = frame.dlc;
|
Casper