72 lines
2.0 KiB
C
72 lines
2.0 KiB
C
#include "include/udp_client.h"
|
|
|
|
#include <stdio.h>
|
|
#include "include/udp_payload.h"
|
|
|
|
udp_client_t udp_client;
|
|
|
|
static inline void __not_in_flash_func(handle_receive)(struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
|
{
|
|
if(p->len >= 2)
|
|
{
|
|
uint8_t *payload = (uint8_t *)p->payload;
|
|
uint16_t len = p->len;
|
|
|
|
udp_client.message_callback(payload, len, addr, port);
|
|
}
|
|
|
|
pbuf_free(p);
|
|
}
|
|
|
|
static void __not_in_flash_func(udp_receive_callback)(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
|
|
{
|
|
udp_client_t *udp_client_received_data = (udp_client_t *)arg;
|
|
handle_receive(p, addr, port);
|
|
}
|
|
|
|
// Default callback func
|
|
static void __not_in_flash_func(default_message_callback)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, uint16_t port)
|
|
{
|
|
//printf("Received: len=%d from %s:%d\n", len, ipaddr_ntoa(addr), port);
|
|
|
|
//for(uint i = 0; i < len; i++) printf("payload[%d]=%d | ", i, payload[i]);
|
|
//puts("\n");
|
|
|
|
printf(">Robot angle : %d\n", (int16_t)((payload[UDP_PAYLOAD_ANGLE_H_BYTE] << 8) | payload[UDP_PAYLOAD_ANGLE_L_BYTE]));
|
|
printf(">Robot x speed : %d\n", (int8_t)payload[UDP_PAYLOAD_X_AXIS_SPEED_BYTE]);
|
|
printf(">Robot y speed : %d\n", (int8_t)payload[UDP_PAYLOAD_Y_AXIS_SPEED_BYTE]);
|
|
}
|
|
|
|
void udp_client_init(void)
|
|
{
|
|
//udp_client.message_callback = udp_client_message_handler;
|
|
udp_client.message_callback = default_message_callback;
|
|
|
|
udp_client.pcb = udp_new();
|
|
if(udp_client.pcb == NULL)
|
|
{
|
|
puts("Error creating UDP client");
|
|
return;
|
|
}
|
|
|
|
udp_recv(udp_client.pcb, udp_receive_callback, &udp_client);
|
|
|
|
err_t err = udp_bind(udp_client.pcb, IP_ADDR_ANY, UDP_CLIENT_PORT);
|
|
if(err != ERR_OK)
|
|
{
|
|
printf("Erreur bind UDP client: %d\n", err);
|
|
return;
|
|
}
|
|
|
|
printf("UDP client started on port %d\n", UDP_CLIENT_PORT);
|
|
}
|
|
|
|
void udp_client_exit(void)
|
|
{
|
|
if(udp_client.pcb)
|
|
{
|
|
udp_remove(udp_client.pcb);
|
|
udp_client.pcb = NULL;
|
|
}
|
|
}
|