841 lines
33 KiB
C
841 lines
33 KiB
C
#include <stdbool.h>
|
||
#include <stdio.h>
|
||
#include "debug.h"
|
||
#include <string.h>
|
||
#include <udelay/udelay.h>
|
||
#include "interface_ethernet.h"
|
||
#include "eth_cfg.h"
|
||
#include "lwip/timeouts.h"
|
||
#include "sdrv_eth.h"
|
||
#include "app/app_param_manage.h"
|
||
#include "app/app_differential_drive.h"
|
||
|
||
|
||
//参数管理
|
||
#define MAX_BUFFER_SIZE (sizeof(((RequestContext *)0)->param_request->arr))
|
||
#define MAX_PACKET_SIZE 1024 // 定义最大包大小为1K
|
||
|
||
Timer ethernet_timer_interface;
|
||
Timer ethernet_timer_interface1;
|
||
Timer ethernet_timer_interface2;
|
||
|
||
|
||
struct udp_pcb *udpcb_1;
|
||
struct udp_pcb *udpcb_2;
|
||
struct udp_pcb *udpcb_3;
|
||
struct udp_pcb *udpcb_4;
|
||
struct udp_pcb *udpcb_5;
|
||
|
||
bool eth_pool_rx = false;
|
||
|
||
|
||
UnEthernetFault ethernet_fault_Info = {
|
||
.bit_data.auto_count = 0,
|
||
.bit_data.manual_count = 0,
|
||
.bit_data.auto_state = 0,
|
||
.bit_data.manual_state = 0,
|
||
};
|
||
|
||
|
||
|
||
StrEthernetParameter ethernet_parameter = {
|
||
.local_ip = {192,168,17,20},
|
||
.mask = {255,255,255,0},
|
||
.computer_ip = {192,168,17,3},
|
||
.local_communication_port = 8011,
|
||
.Local_upper_port = 8000,
|
||
.Local_download_port = 7811,
|
||
.upper_ip = {192,168,17,3},
|
||
.download_ip = {192,168,17,183},
|
||
.target_upper_port = 8000,
|
||
.target_download_port = 7811,
|
||
.target_communication_port = 8011,
|
||
};
|
||
|
||
|
||
|
||
static bool g_eth1_rx_pkt;
|
||
|
||
|
||
|
||
static void eth_dma_rx_int_cb(struct net_driver_s *dev)
|
||
{
|
||
if (dev == &g_eth1_dev)
|
||
g_eth1_rx_pkt = true;
|
||
|
||
// printf("eth_dma_rx_int_cb\n");
|
||
}
|
||
|
||
|
||
|
||
|
||
static void lwip_Ethernet(void *signal_id)
|
||
{
|
||
// uint32_t time_boot = getCurrentTime();//记录当前时间
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
// 接收ETH帧，并传递给LWIP协议栈
|
||
// 如果使能接收中断，因为ISR里会disable接收中断，因此，这里对于
|
||
// 中断接收标志(g_eth1_rx_pkt)的处理过程中不会有
|
||
// 新的接收中断被响应，所以，这里不存在原子性问题。
|
||
if ((!eth_pool_rx && g_eth1_rx_pkt) || eth_pool_rx)
|
||
{
|
||
g_eth1_rx_pkt = false;
|
||
dwc_eth_rx(&g_eth1_dev, eth_pool_rx);
|
||
}
|
||
|
||
// LWIP协议栈接口，用于做LWIP超时检测
|
||
sys_check_timeouts();
|
||
|
||
timerStart(ðernet_timer_interface1, 10,1); //10ms调用一次
|
||
|
||
// printf("lwipInterface spend time:%d\n",getCurrentTime() - time_boot);//检查app用了多长时间
|
||
}
|
||
|
||
|
||
//err_t
|
||
//udp_bind(struct udp_pcb *pcb, const ip_addr_t *ipaddr, u16_t port)
|
||
|
||
int8_t UdpSendToData(uint8_t udpcbID, uint8_t *buf, uint16_t len, uint8_t *sip, uint16_t port)
|
||
{
|
||
struct pbuf *DesPuff = NULL;
|
||
ip4_addr_t Desaddr;
|
||
int8_t SendState = 0;
|
||
//----------------------------------------------------------------------------------------------------
|
||
IP4_ADDR(&Desaddr,sip[0],sip[1],sip[2],sip[3]);//地址值转换
|
||
|
||
DesPuff = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_POOL); /* 申请内存 */
|
||
|
||
pbuf_take(DesPuff, buf, len); /* 将g_lwip_demo_sendbuf中的数据打包进pbuf结构中 */
|
||
|
||
if(UDPCB_1 == udpcbID)
|
||
{
|
||
SendState = udp_sendto(udpcb_1, DesPuff, &Desaddr, port);
|
||
}
|
||
else if(UDPCB_2 == udpcbID)
|
||
{
|
||
SendState = udp_sendto(udpcb_2, DesPuff, &Desaddr, port);
|
||
}
|
||
else if(UDPCB_3 == udpcbID)
|
||
{
|
||
SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port);
|
||
}
|
||
else if(UDPCB_3 == udpcbID)
|
||
{
|
||
SendState = udp_sendto(udpcb_3, DesPuff, &Desaddr, port);
|
||
}
|
||
else if(UDPCB_4 == udpcbID)
|
||
{
|
||
SendState = udp_sendto(udpcb_4, DesPuff, &Desaddr, port);
|
||
}
|
||
else if(UDPCB_5 == udpcbID)
|
||
{
|
||
SendState = udp_sendto(udpcb_5, DesPuff, &Desaddr, port);
|
||
}
|
||
else
|
||
{
|
||
}
|
||
pbuf_free(DesPuff); /* 释放内存 */
|
||
|
||
return SendState;
|
||
}
|
||
|
||
// udpcbID UDP索引
|
||
//recv 回调函数
|
||
//port 绑定端口
|
||
void UDP_Echo_Init(uint8_t udpcbID, udp_recv_fn recv, uint16_t port)
|
||
{
|
||
//------------------------------------------
|
||
if(UDPCB_1 == udpcbID)
|
||
{
|
||
/* 新建一个控制块*/
|
||
udpcb_1 = udp_new();
|
||
ASSERT(udpcb_1);
|
||
/* 绑定端口号 */
|
||
udp_bind(udpcb_1, IP_ADDR_ANY, port);
|
||
/* 注册接收数据回调函数 */
|
||
udp_recv(udpcb_1, recv, (void *)udpcbID);
|
||
}
|
||
else if(UDPCB_2 == udpcbID)
|
||
{
|
||
/* 新建一个控制块*/
|
||
udpcb_2 = udp_new();
|
||
ASSERT(udpcb_2);
|
||
/* 绑定端口号 */
|
||
udp_bind(udpcb_2, IP_ADDR_ANY, port);
|
||
/* 注册接收数据回调函数 */
|
||
udp_recv(udpcb_2, recv, (void *)udpcbID);
|
||
}
|
||
else if(UDPCB_3 == udpcbID)
|
||
{
|
||
/* 新建一个控制块*/
|
||
udpcb_3 = udp_new();
|
||
ASSERT(udpcb_3);
|
||
/* 绑定端口号 */
|
||
udp_bind(udpcb_3, IP_ADDR_ANY, port);
|
||
/* 注册接收数据回调函数 */
|
||
udp_recv(udpcb_3, recv, (void *)udpcbID);
|
||
}
|
||
else if(UDPCB_4 == udpcbID)
|
||
{
|
||
/* 新建一个控制块*/
|
||
udpcb_4 = udp_new();
|
||
ASSERT(udpcb_4);
|
||
/* 绑定端口号 */
|
||
udp_bind(udpcb_4, IP_ADDR_ANY, port);
|
||
/* 注册接收数据回调函数 */
|
||
udp_recv(udpcb_4, recv, (void *)udpcbID);
|
||
}
|
||
else if(UDPCB_5 == udpcbID)
|
||
{
|
||
/* 新建一个控制块*/
|
||
udpcb_5 = udp_new();
|
||
ASSERT(udpcb_5);
|
||
/* 绑定端口号 */
|
||
udp_bind(udpcb_5, IP_ADDR_ANY, port);
|
||
/* 注册接收数据回调函数 */
|
||
udp_recv(udpcb_5, recv, (void *)udpcbID);
|
||
}
|
||
else
|
||
{
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
||
{
|
||
uint8_t *buf = (uint8_t *)(p->payload);
|
||
uint16_t i = 0;
|
||
uint16_t len = 0;
|
||
uint16_t udp_temp = 0;
|
||
//------------------------------------------------------------------------------
|
||
|
||
if( (0xFF == buf[0] ) && ( 0xBB == buf[1] ) )//自主计算机手动遥控器或者远程遥控器
|
||
{
|
||
ethernet_fault_Info.bit_data.manual_count ++;
|
||
|
||
if( (p->len) >= sizeof(UnManualComputerInput) )//如果读取长度超过，就取最小长度
|
||
{
|
||
len = sizeof(UnManualComputerInput);
|
||
}
|
||
else
|
||
{
|
||
len = p->len;
|
||
}
|
||
|
||
for(i = 0; i < len; i++)
|
||
{
|
||
un_manual_computer_input.arr[i] = buf[i];
|
||
}
|
||
publishMessage(&un_manual_computer_input, 1);
|
||
|
||
// p->len = len;
|
||
// printf("Manualrecive len:%d\n",len);
|
||
// udp_sendto(upcb, p, addr, port);
|
||
}
|
||
else if( (0xFF == buf[0] ) && (0xCC == buf[1] ) )//自主计算器 自动
|
||
{
|
||
ethernet_fault_Info.bit_data.auto_count ++;
|
||
|
||
if( (p->len) >= sizeof(UnAutoComputerInput) )//如果读取长度超过，就取最小长度
|
||
{
|
||
len = sizeof(UnAutoComputerInput);
|
||
}
|
||
else
|
||
{
|
||
len = p->len;
|
||
}
|
||
|
||
|
||
// unsigned int frame_header : 16; // 帧头 固定值0xFFCC
|
||
// unsigned int frame_type : 16; // 帧类型 固定值0x0001
|
||
// unsigned int frame_length : 8; // 帧长 固定值0x19
|
||
// unsigned int heartbeat : 8; // 心跳 按帧累加
|
||
// unsigned int set_speed : 16; // 设定速度 系数0.01，正为前进，负为后退 单位m/s
|
||
// unsigned int set_curvature : 16; // 设定曲率 系数0.0001，正为左转，负为右转
|
||
// unsigned int latitude : 32; // 纬度 系数10^-7，431234567表示43.1234567度
|
||
// unsigned int longitude : 32; // 经度 系数10^-7，431234567表示43.1234567度
|
||
// unsigned int altitude : 32; // 高度 单位mm
|
||
// unsigned int heading : 16; // 航向 车辆航向，35999表示359.99度
|
||
// unsigned int crc : 8; // CRC 按字节累加之和，溢出取低8位
|
||
for(i = 0; i < len; i++)
|
||
{
|
||
un_auto_computer_input.arr[i] = buf[i];
|
||
}
|
||
|
||
udp_temp = ((un_auto_computer_input.bit_data.set_speed << 8) | (un_auto_computer_input.bit_data.set_speed >> 8));//设定速度 20240928 修改高低位交换
|
||
un_auto_computer_input.bit_data.set_speed = udp_temp;
|
||
|
||
udp_temp = ((un_auto_computer_input.bit_data.set_curvature << 8) | (un_auto_computer_input.bit_data.set_curvature >> 8));//设定曲率
|
||
un_auto_computer_input.bit_data.set_curvature = udp_temp;
|
||
publishMessage(&un_auto_computer_input, 1);
|
||
|
||
// p->len = len;
|
||
// printf("Autorecive len:%d\n",len);
|
||
// udp_sendto(upcb, p, addr, port);
|
||
}
|
||
else
|
||
{
|
||
}
|
||
|
||
pbuf_free(p);
|
||
}
|
||
|
||
|
||
|
||
void udp_Callback_2(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
||
{
|
||
uint8_t *buf = (uint8_t *)(p->payload);
|
||
uint8_t ip_addr[4] = {0,0,0,0};
|
||
|
||
//------------------------------------------------------------------------------------------------
|
||
|
||
ip_addr[0] = addr->addr & 0xff; /* IADDR4 */
|
||
ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */
|
||
ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */
|
||
ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */
|
||
|
||
ethernet_parameter.target_upper_port = port;//保存接收到的端口和IP，下次发送对应的IP和端口
|
||
|
||
ethernet_parameter.upper_ip[0] = ip_addr[0];
|
||
ethernet_parameter.upper_ip[1] = ip_addr[1];
|
||
ethernet_parameter.upper_ip[2] = ip_addr[2];
|
||
ethernet_parameter.upper_ip[3] = ip_addr[3];
|
||
|
||
if( (0x80 == buf[0]) && (0x00 == buf[1]) )
|
||
{
|
||
memcpy((uint8_t *)&(un_request_frame.arr[0]),buf, sizeof(UnRequestFrame));
|
||
publishMessage(&un_request_frame, 1);
|
||
}
|
||
|
||
pbuf_free(p);
|
||
}
|
||
|
||
|
||
void udp_Callback_4(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
||
{
|
||
uint8_t *buf = (uint8_t *)(p->payload);
|
||
uint8_t ip_addr[4] = {0,0,0,0};
|
||
//------------------------------------------------------------------------------------------------
|
||
//20250822 增加保存刷写上位机IP
|
||
ip_addr[0] = addr->addr & 0xff; /* IADDR4 */
|
||
ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */
|
||
ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */
|
||
ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */
|
||
|
||
ethernet_parameter.download_ip[0] = ip_addr[0];
|
||
ethernet_parameter.download_ip[1] = ip_addr[1];
|
||
ethernet_parameter.download_ip[2] = ip_addr[2];
|
||
ethernet_parameter.download_ip[3] = ip_addr[3];
|
||
|
||
|
||
boot_eth_flag = true;
|
||
FrameHeader = ( (buf[0] << 8) | (buf[1]) );
|
||
printf("FrameHeader %d\n",FrameHeader);
|
||
|
||
pbuf_free(p);
|
||
}
|
||
|
||
|
||
void udp_Callback_5(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
||
{
|
||
uint8_t *buf = (uint8_t *)(p->payload);
|
||
//------------------------------------------------------------------------------------------------
|
||
|
||
if(FEEDDOG_HEADER == ( (buf[0] << 8) | (buf[1]) ))
|
||
{
|
||
WDTReFresh_flag = true;//更新喂狗标志
|
||
printf("Feed dog flag received %d\n",getCurrentTime());
|
||
}
|
||
|
||
|
||
pbuf_free(p);
|
||
}
|
||
|
||
|
||
//100ms 调用一次
|
||
void ethernetTimerProcess(void)
|
||
{
|
||
static uint8_t ethernet_timer = 0;
|
||
static uint8_t ethernet_temp[4] = {0,0,0,0};//中间判断值
|
||
uint8_t temp = 0;
|
||
|
||
ethernet_timer ++;
|
||
if(ethernet_timer >= 5)//500ms判断一次
|
||
{
|
||
ethernet_timer = 0;
|
||
|
||
//自主计算机故障判断------------------------------------------------------------
|
||
if(ethernet_fault_Info.bit_data.auto_count == ethernet_temp[0])//数据一样表示故障
|
||
{
|
||
ethernet_fault_Info.bit_data.auto_state = FAULT;
|
||
}
|
||
else
|
||
{
|
||
ethernet_fault_Info.bit_data.auto_state = NORMAL;
|
||
ethernet_temp[0] = ethernet_fault_Info.bit_data.auto_count;//数据更新
|
||
}
|
||
|
||
if(ethernet_fault_Info.bit_data.auto_state != ethernet_temp[1])
|
||
{
|
||
ethernet_temp[1] = ethernet_fault_Info.bit_data.auto_state;
|
||
temp ++;
|
||
}
|
||
|
||
|
||
//手动故障判断------------------------------------------------------------
|
||
if(ethernet_fault_Info.bit_data.manual_count == ethernet_temp[2])//数据一样表示故障
|
||
{
|
||
ethernet_fault_Info.bit_data.manual_state = FAULT;
|
||
}
|
||
else
|
||
{
|
||
ethernet_fault_Info.bit_data.manual_state = NORMAL;
|
||
ethernet_temp[2] = ethernet_fault_Info.bit_data.manual_count;//数据更新
|
||
}
|
||
|
||
if(ethernet_fault_Info.bit_data.manual_state != ethernet_temp[3])
|
||
{
|
||
ethernet_temp[3] = ethernet_fault_Info.bit_data.manual_state;
|
||
temp ++;
|
||
}
|
||
}
|
||
|
||
publishMessage(ðernet_fault_Info, 1);// 状态变化 发送信号
|
||
}
|
||
|
||
|
||
static void vehicleInfOoutput(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_vehicle_Info_output), sizeof(un_vehicle_Info_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||
}
|
||
|
||
static void motorStatusOutput(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_motor_status_output), sizeof(un_motor_status_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||
}
|
||
|
||
static void pidOutput(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_pid_output), sizeof(un_pid_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||
}
|
||
|
||
static void remoteControlOutput(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_remote_control_output), sizeof(un_remote_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||
}
|
||
|
||
static void manualControlOutput(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_manual_control_output),sizeof(un_manual_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||
}
|
||
|
||
static void autoControlOutput(void *signal_id)
|
||
{
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
UdpSendToData(UDPCB_2,(uint8_t *)(&un_auto_control_output), sizeof(un_auto_control_output), (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
//以太网 发送
|
||
void ethernetSendAll(void *signal_id)
|
||
{
|
||
static uint8_t computer_output_cnt = 0;
|
||
uint32_t crc_temp = 0;
|
||
uint8_t i = 0;
|
||
uint16_t Rg_Tmp = 0;
|
||
float Rg_FloatTmp = 0;
|
||
static uint8_t eth_cnt = 0;
|
||
static uint8_t app_output_cnt = 0;
|
||
uint8_t BOOT_Arr[2] = {0x01,0x02};//上电发送app帧给上位机用
|
||
//-------------------------------------------------------------------------
|
||
//自主计算机以太网50ms发送 20210811修改为100ms发送
|
||
//20210827修改为高位在前低位再后与自主计算机保持一致
|
||
computer_output_cnt ++;
|
||
if(computer_output_cnt >= 1)
|
||
{
|
||
computer_output_cnt = 0;
|
||
//按字节累加和，注意不是信号 20210826修改按照字节累加
|
||
|
||
// printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
|
||
// printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
|
||
|
||
un_computer_output.bit_data.frame_header = 0xCCFF;
|
||
un_computer_output.bit_data.frame_type = 0x1100;
|
||
un_computer_output.bit_data.frame_length = 0x0B;
|
||
un_computer_output.bit_data.accumulated = eth_cnt ++;
|
||
|
||
Rg_FloatTmp = (diff_data.left_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// 转/分钟——》km/h 一圈1.8米
|
||
Rg_FloatTmp = Rg_FloatTmp*100;//,最后需要扩大100倍 ,系数0.01
|
||
Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//最后转换为无符号型); //20231012修改轮速发送 20231226-3号车修改方向
|
||
un_computer_output.bit_data.speed = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//高位在前，地位灾后
|
||
|
||
Rg_FloatTmp = (diff_data.right_motor_speed*(float)getParam("whl_dia") * M_PI*60)/1000.0/(float)getParam("gRatio");// 转/分钟——》km/h 一圈1.8米
|
||
Rg_FloatTmp = Rg_FloatTmp*100;//,最后需要扩大100倍 ,系数0.01
|
||
Rg_Tmp = (uint16_t)((int16_t)(Rg_FloatTmp));//最后转换为无符号型); //20231012修改轮速发送 20231226-3号车修改方向
|
||
un_computer_output.bit_data.curvature = ((Rg_Tmp << 8) | (Rg_Tmp >> 8));//高位在前，地位灾后
|
||
|
||
crc_temp = 0;
|
||
for(i = 0; i<10 ;i++)//长度固定
|
||
{
|
||
crc_temp = crc_temp + (un_computer_output.arr[i]);
|
||
}
|
||
un_computer_output.bit_data.crc = (uint8_t)crc_temp;
|
||
|
||
UdpSendToData(UDPCB_1,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT);
|
||
UdpSendToData(UDPCB_4,(uint8_t *)(&un_computer_output.arr[0]), 11, (uint8_t *)ðernet_parameter.computer_ip[0], COMMUNICATION_PORT);
|
||
}
|
||
|
||
|
||
if(app_output_cnt < 10)//20250109 100ms 发送10帧app帧。解决以太网没有初始化完成就发送的丢帧问题
|
||
{
|
||
app_output_cnt ++;
|
||
UdpSendToData(UDPCB_4, BOOT_Arr, 2, (uint8_t *)ðernet_parameter.download_ip[0], BOOT_TX_PORT);
|
||
}
|
||
}
|
||
|
||
// 以太网定时器信号处理函数
|
||
static void ethernetInterfaceTimerProcess(void *signal_id)
|
||
{
|
||
uint32_t time_boot = getCurrentTime();//记录当前时间
|
||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||
|
||
ethernetSendAll(signal_id);
|
||
|
||
ethernetTimerProcess();//故障判断
|
||
|
||
timerStart(ðernet_timer_interface, 100,1); //100ms调用一次
|
||
|
||
// printf("ethernetInterface spend time:%d\n",getCurrentTime() - time_boot);//检查app用了多长时间
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
static unsigned char receive_buffer[MAX_BUFFER_SIZE];
|
||
//参数管理回调
|
||
void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
||
{
|
||
static unsigned int received_size = 0;
|
||
uint8_t *buf = (uint8_t *)(p->payload);
|
||
|
||
// 检查是否是新的帧头
|
||
if ((0x80 == buf[1]) && (0xFF == buf[0]))
|
||
{
|
||
// 重置接收过程
|
||
received_size = 0;
|
||
printf("检测到新的帧头,重置接收过程\n");
|
||
}
|
||
|
||
// 检查是否有足够的空间来存储新数据
|
||
if (received_size + p->len <= MAX_BUFFER_SIZE)
|
||
{
|
||
printf("p->len %d :\n" , p->len);
|
||
|
||
memcpy(receive_buffer + received_size, buf, p->len);
|
||
received_size += p->len;
|
||
|
||
// 检查是否接收到完整的数据
|
||
if (received_size >= sizeof(request_context.param_request->arr))
|
||
{
|
||
printf("received_size : %d :\n" , received_size);
|
||
|
||
for(u16_t i = 0; i< sizeof(request_context.param_request->arr); i++)
|
||
{
|
||
request_context.param_request->arr[i] = receive_buffer[i];
|
||
}
|
||
|
||
memcpy(request_context.param_request->arr, receive_buffer, sizeof(request_context.param_request->arr));
|
||
request_context.sender_ip = (unsigned int)addr->addr;
|
||
request_context.sender_port = port;
|
||
|
||
// 打印 request_context.param_request->arr 的内容
|
||
// printf("接收到的 param_request->arr 内容:\n");
|
||
// for (size_t i = 0; i < sizeof(request_context.param_request->arr); ++i)
|
||
// {
|
||
// printf("%02X ", request_context.param_request->arr[i]);
|
||
// if ((i + 1) % 1024 == 0 || i == sizeof(request_context.param_request->arr) - 1)
|
||
// {
|
||
// printf("\n");
|
||
// feedWatchdog();
|
||
// }
|
||
// }
|
||
|
||
publishMessage(&request_context, 1);
|
||
|
||
printf("received_size：%d \n" , received_size);
|
||
// 重置接收缓冲区
|
||
received_size = 0;
|
||
printf("接收到完整数据,处理完毕\n");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 缓冲区溢出,重置接收
|
||
received_size = 0;
|
||
printf("接收缓冲区溢出,数据丢弃\n");
|
||
}
|
||
|
||
unsigned int tmp_count = pbuf_free(p);
|
||
printf("pbuf_free已经释放！tmp_count :%d \n" , tmp_count);
|
||
p = NULL;
|
||
}
|
||
|
||
void OnParamSend(void *data)
|
||
{
|
||
uint8_t ip_addr[4] = {0,0,0,0};
|
||
RequestContext *signal = (RequestContext *)(data);
|
||
|
||
ip_addr[0] = signal->sender_ip & 0xff; /* IADDR4 */
|
||
ip_addr[1] = (signal->sender_ip >> 8) & 0xff; /* IADDR3 */
|
||
ip_addr[2] = (signal->sender_ip >> 16) & 0xff; /* IADDR2 */
|
||
ip_addr[3] = (signal->sender_ip >> 24) & 0xff; /* IADDR1 */
|
||
|
||
uint32_t total_size = sizeof(signal->param_request->arr);
|
||
uint32_t sent_size = 0;
|
||
|
||
|
||
while (sent_size < total_size)
|
||
{
|
||
uint32_t remaining = total_size - sent_size;
|
||
uint32_t packet_size = (remaining > MAX_PACKET_SIZE) ? MAX_PACKET_SIZE : remaining;
|
||
|
||
UdpSendToData(UDPCB_3,
|
||
(uint8_t *)(signal->param_request->arr) + sent_size,
|
||
packet_size,
|
||
ip_addr,
|
||
signal->sender_port);
|
||
|
||
sent_size += packet_size;
|
||
// 添加小延迟,避免包丢失
|
||
udelay(1000);
|
||
}
|
||
printf("数据发送完成！%d \n", sent_size);
|
||
}
|
||
|
||
void ethernetRequest(void *signal_id)
|
||
{
|
||
static uint8_t ethernetRequestcnt = 0;
|
||
uint8_t request_ip[4] = {192,168,17,88};
|
||
uint16_t request_port = 8080;
|
||
uint8_t request_buf[200] = {0};
|
||
static uint8_t RequestStaACC0 = 0;
|
||
static uint8_t RequestStaACC1 = 0;
|
||
static uint8_t RequestStaACC2 = 0;
|
||
static uint8_t RequestStaACC3 = 0;
|
||
|
||
uint16_t RgExchangeTemp = 0;
|
||
uint32_t Rg32ExchangeTemp = 0;
|
||
uint32_t TempAcc = 0;
|
||
uint8_t i = 0;
|
||
|
||
ethernetRequestcnt ++;
|
||
|
||
if(ethernetRequestcnt >= 10)
|
||
{
|
||
ethernetRequestcnt = 0;
|
||
|
||
//车辆状态
|
||
un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//帧头
|
||
un_vehicle_Info_output.bit_data.frame_type = 0x2000;//帧类型
|
||
un_vehicle_Info_output.bit_data.frame_length = 0x2900;//帧长
|
||
un_vehicle_Info_output.bit_data.accumulated = RequestStaACC0++;//累加值
|
||
|
||
TempAcc = 0;
|
||
for (i = 0; i < 40; i++)//累加前40个字节
|
||
{
|
||
TempAcc = TempAcc + (uint32_t)(un_vehicle_Info_output.arr[i]);
|
||
}
|
||
un_vehicle_Info_output.bit_data.crc = (uint8_t)TempAcc;//累加值
|
||
|
||
|
||
//电机状态
|
||
un_motor_status_output.bit_data.frame_header = 0xCCAA;//帧头
|
||
un_motor_status_output.bit_data.frame_type = 0x2100;//帧类型
|
||
un_motor_status_output.bit_data.frame_length = 0x2200;//帧长
|
||
un_motor_status_output.bit_data.accumulated = RequestStaACC1++;//累加值
|
||
RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ;
|
||
// un_motor_status_output.bit_data.left_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//左侧扭矩限制
|
||
// un_motor_status_output.bit_data.right_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//右侧扭矩限制
|
||
// un_motor_status_output.bit_data.left_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//左侧馈电功率
|
||
// un_motor_status_output.bit_data.right_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//右侧馈电功率
|
||
// un_motor_status_output.bit_data.left_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//左侧放电功率
|
||
// un_motor_status_output.bit_data.right_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//右侧放电功率
|
||
|
||
TempAcc = 0;
|
||
for (i = 0; i < 32; i++)//累加前31个字节
|
||
{
|
||
TempAcc = TempAcc + (uint32_t)(un_motor_status_output.arr[i]);
|
||
un_motor_status_output.bit_data.checksum = (uint8_t)TempAcc;//累加值
|
||
}
|
||
|
||
//PID
|
||
|
||
un_pid_output.bit_data.frame_header = 0xCCAA;//帧头
|
||
un_pid_output.bit_data.frame_type = 0x2200;//帧类型
|
||
un_pid_output.bit_data.frame_length = 0x3800;//帧长
|
||
un_pid_output.bit_data.accumulated = RequestStaACC2++;//累加值
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kp")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.rc_straight_p = Rg32ExchangeTemp;//遥控直行P参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ki")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.rc_straight_i = Rg32ExchangeTemp;//遥控直行I参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.rc_straight_d = Rg32ExchangeTemp;//遥控直行D参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_il")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.auto_straight_p = Rg32ExchangeTemp;//自主直行P参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ol")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.auto_straight_i = Rg32ExchangeTemp;//自主直行I参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.auto_straight_d = Rg32ExchangeTemp;//自主直行D参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kp")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.rc_turn_p = Rg32ExchangeTemp;//遥控转弯P参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ki")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.rc_turn_i = Rg32ExchangeTemp;//遥控转弯I参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kd")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.rc_turn_d = Rg32ExchangeTemp;//遥控转弯D参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_il")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.auto_turn_p = Rg32ExchangeTemp;//自主转弯P参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ol")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.auto_turn_i = Rg32ExchangeTemp;//自主转弯I参数
|
||
|
||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
|
||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||
un_pid_output.bit_data.auto_turn_d = Rg32ExchangeTemp;//自主转弯I参数
|
||
|
||
TempAcc = 0;
|
||
for (i = 0; i < 55; i++)//累加前40个字节
|
||
{
|
||
TempAcc = TempAcc + (uint32_t)(un_pid_output.arr[i]);
|
||
}
|
||
un_pid_output.bit_data.checksum = (uint8_t)TempAcc;//累加值
|
||
|
||
|
||
//遥控
|
||
un_remote_control_output.bit_data.frame_header = 0xCCAA;//帧头
|
||
un_remote_control_output.bit_data.frame_type = 0x2400;//帧类型
|
||
un_remote_control_output.bit_data.frame_length = 0x1400;//帧长
|
||
un_remote_control_output.bit_data.accumulated = RequestStaACC3++;//累加值
|
||
//RCH_3
|
||
|
||
TempAcc = 0;
|
||
for (i = 0; i < 17; i++)//累加前40个字节
|
||
{
|
||
TempAcc = TempAcc + (uint32_t)(un_remote_control_output.arr[i]);
|
||
}
|
||
un_remote_control_output.bit_data.crc = (uint8_t)TempAcc;//累加值
|
||
|
||
//手动控制
|
||
un_manual_control_output.bit_data.frame_header = 0xBBFF;
|
||
un_manual_control_output.bit_data.frame_type = 0x2500;
|
||
un_manual_control_output.bit_data.frame_length = 0x0F;
|
||
TempAcc = 0;
|
||
for (i = 0; i < 14; i++)//累加前40个字节
|
||
{
|
||
TempAcc = TempAcc + (uint32_t)(un_manual_control_output.arr[i]);
|
||
}
|
||
un_manual_control_output.bit_data.crc_2 = (uint8_t)TempAcc;//累加值
|
||
|
||
//自主控制
|
||
un_auto_control_output.bit_data.frame_header = 0xCCFF;
|
||
un_auto_control_output.bit_data.frame_type = 0x2600;
|
||
un_auto_control_output.bit_data.frame_length = 0x1B;
|
||
TempAcc = 0;
|
||
for (i = 0; i < 27; i++)//累加前40个字节
|
||
{
|
||
TempAcc = TempAcc + (uint32_t)(un_auto_control_output.arr[i]);
|
||
}
|
||
un_auto_control_output.bit_data.crc = (uint8_t)TempAcc;//累加值
|
||
|
||
memcpy(request_buf,(uint8_t *)(&un_vehicle_Info_output),sizeof(un_vehicle_Info_output));
|
||
memcpy(&request_buf[sizeof(un_vehicle_Info_output)],(uint8_t *)(&un_motor_status_output),sizeof(un_motor_status_output));
|
||
memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output)],(uint8_t *)(&un_pid_output),sizeof(un_pid_output));
|
||
memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output)],(uint8_t *)(&un_remote_control_output),sizeof(un_remote_control_output));
|
||
memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output) + sizeof(un_remote_control_output)],(uint8_t *)(&un_manual_control_output),sizeof(un_manual_control_output));
|
||
memcpy(&request_buf[sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output) + sizeof(un_remote_control_output) + sizeof(un_manual_control_output)],(uint8_t *)(&un_auto_control_output),sizeof(un_auto_control_output));
|
||
|
||
UdpSendToData(UDPCB_2,request_buf,sizeof(un_vehicle_Info_output) + sizeof(un_motor_status_output) + sizeof(un_pid_output) + sizeof(un_remote_control_output) + sizeof(un_manual_control_output) + sizeof(un_auto_control_output), request_ip, request_port);
|
||
}
|
||
timerStart(ðernet_timer_interface2, 1,1); //10ms调用一次
|
||
}
|
||
|
||
// APP模块的初始化
|
||
void ethernetInterfaceInit(void)
|
||
{
|
||
//-----------------------------------
|
||
if (!eth_pool_rx) {
|
||
// 使能DMA Rx中断。Tx未使用中断，因为ETH速率很高，
|
||
// Tx中断处理耗时反而可能拖慢发送速度。
|
||
dwc_eth_enable_dma_rx_int(&g_eth1_dev, eth_dma_rx_int_cb);
|
||
}
|
||
|
||
timerInit(ðernet_timer_interface1);
|
||
timerInit(ðernet_timer_interface);
|
||
// timerInit(ðernet_timer_interface2);
|
||
|
||
// 订阅定时器信号，用于定时采集
|
||
subscribe(ðernet_timer_interface, ethernetInterfaceTimerProcess);
|
||
subscribe(ðernet_timer_interface1, lwip_Ethernet);
|
||
// subscribe(ðernet_timer_interface2, ethernetRequest);
|
||
|
||
subscribe(&un_vehicle_Info_output, vehicleInfOoutput);
|
||
subscribe(&un_motor_status_output, motorStatusOutput);
|
||
subscribe(&un_pid_output, pidOutput);
|
||
subscribe(&un_remote_control_output, remoteControlOutput);
|
||
subscribe(&un_manual_control_output, manualControlOutput);
|
||
subscribe(&un_auto_control_output, autoControlOutput);
|
||
|
||
// 订阅消息，使用静态成员函数作为回调
|
||
subscribe(&request_send, OnParamSend);
|
||
|
||
|
||
timerStart(ðernet_timer_interface, 100,1); //100ms调用一次
|
||
timerStart(ðernet_timer_interface1, 10,1); //10ms调用一次
|
||
// timerStart(ðernet_timer_interface2, 1,1); //10ms调用一次
|
||
|
||
|
||
printf( "ethernetInterface: initial OK %d\n",getCurrentTime());
|
||
}
|
||
|
||
|