Applied globalisation of dhcp_server struct

program/CMakeLists.txt

@@ -17,6 +17,7 @@ pico_sdk_init()
 add_executable(controller
   src/main.c
   src/controller.c
+  src/inputs.c
   src/wifi/wifi_operator.c
   src/wifi/udp_server.c
   src/wifi/dhcp_server.c
@@ -29,9 +30,7 @@ target_include_directories(controller PRIVATE
 
 target_link_libraries(controller
   pico_stdlib
-  hardware_i2c
-  hardware_pwm
-  hardware_uart
+  hardware_adc
   pico_cyw43_arch_lwip_poll
 )
 

program/src/controller.c

@@ -18,6 +18,8 @@ void controller_init(void)
 
     cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
 
+    inputs_init();
+
     if(wifi_operator_init())
         controller.is_running = false;
 
@@ -40,30 +42,31 @@ void controller_init(void)
 
 static inline void update_time(void)
 {
-    static float last_time = 0.0;
-    float start_time = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
-    controller.delta_time_ms = start_time - last_time;
-    last_time = start_time;
+    static float last_time_ms = 0.0;
+    float start_time_ms = (float)clock() * 1000.0f / (float)CLOCKS_PER_SEC;
+    controller.delta_time_ms = start_time_ms - last_time_ms;
+    last_time_ms = start_time_ms;
 
-    static float elapsed_time = 0.0f;
-    elapsed_time += controller.delta_time_ms;
+    static float elapsed_time_ms = 0.0f;
+    elapsed_time_ms += controller.delta_time_ms;
 
     static bool led_state = false;
-    if(elapsed_time >= 1000.0f)
+    if(elapsed_time_ms >= 1000.0f)
     {
-        elapsed_time = 0.0f;
-
         cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state);
 
+        elapsed_time_ms = 0.0f;
         led_state = !led_state;
     }
 }
 
 void controller_handle_inputs_outputs(void)
 {
+    update_time();
+
     cyw43_arch_poll();
 
-    update_time();
+    inputs_update();
 
     udp_server_send();
 
@@ -73,4 +76,8 @@ void controller_handle_inputs_outputs(void)
 void controller_deinit(void)
 {
     udp_server_deinit();
+
+    dhcp_server_deinit();
+
+    cyw43_arch_deinit();
 }

program/src/headers/controller.h

@@ -2,10 +2,12 @@
 #define ROBOT_H
 
 #include <stdbool.h>
+#include "headers/inputs.h"
 #include "wifi/headers/dhcp_server.h"
 #include "wifi/headers/udp_server.h"
 
 typedef struct controller_t {
+    inputs_t inputs;
     dhcp_server_t dhcp_server;
     udp_server_t udp_server;
 

program/src/headers/inputs.h

@@ -0,0 +1,41 @@
+#ifndef INPUTS_H
+#define INPUTS_H
+
+#include <pico/types.h>
+
+typedef enum inputs_pin_t {
+    JOYSTICK_X_AXIS_PIN = 26,
+    JOYSTICK_Y_AXIS_PIN = 27,
+
+    BUTTON_BLACK_PIN = 1,
+    BUTTON_BLUE_PIN = 5,
+    BUTTON_WHITE_PIN = 6,
+    BUTTON_GREEN_PIN = 2,
+
+    BUTTON_L_PIN = 9,
+    BUTTON_R_PIN = 13,
+}  inputs_pin_t;
+
+#define JOYSTICK_X_AXIS_ADC_INPUT 0
+#define JOYSTICK_Y_AXIS_ADC_INPUT 1
+
+typedef struct inputs_t {
+    int8_t joystick_x;
+    int8_t joystick_y;
+
+    struct {
+        bool button_black : 1;
+        bool button_blue  : 1;
+        bool button_white : 1;
+        bool button_green : 1;
+        bool button_l     : 1;
+        bool button_r     : 1;
+    } buttons;
+} __attribute__((__packed__)) inputs_t;
+
+// Init all inputs
+void inputs_init(void);
+// Update inputs
+void inputs_update(void);
+
+#endif // INPUTS_H

program/src/inputs.c

@@ -0,0 +1,51 @@
+#include "headers/inputs.h"
+
+#include <pico/stdlib.h>
+#include <hardware/gpio.h>
+#include <hardware/adc.h>
+#include "headers/controller.h"
+
+#define JOYSTICK_DEAD_ZONE 3
+
+void inputs_init(void)
+{
+    adc_init();
+    adc_gpio_init(JOYSTICK_X_AXIS_PIN);
+    adc_gpio_init(JOYSTICK_Y_AXIS_PIN);
+
+    const uint BUTTONS_PIN[] = {BUTTON_BLACK_PIN, BUTTON_BLUE_PIN, BUTTON_WHITE_PIN, BUTTON_GREEN_PIN, BUTTON_L_PIN, BUTTON_R_PIN};
+
+    for(size_t i = 0; i < sizeof(BUTTONS_PIN) / sizeof(uint); i++)
+    {
+        gpio_init(BUTTONS_PIN[i]);
+        gpio_set_dir(BUTTONS_PIN[i], GPIO_IN);
+        gpio_pull_up(BUTTONS_PIN[i]);
+    }
+}
+
+void inputs_update(void)
+{
+    // Update joystick x and y position
+    adc_select_input(JOYSTICK_X_AXIS_ADC_INPUT);
+    uint16_t joystick_raw = adc_read();
+    controller.inputs.joystick_x = (joystick_raw >> 4) - 128;
+
+    adc_select_input(JOYSTICK_Y_AXIS_ADC_INPUT);
+    joystick_raw = adc_read();
+    controller.inputs.joystick_y = (joystick_raw >> 4) - 128;
+
+    // Dead zone
+    if(abs(controller.inputs.joystick_x) < JOYSTICK_DEAD_ZONE)
+        controller.inputs.joystick_x = 0;
+
+    if(abs(controller.inputs.joystick_y) < JOYSTICK_DEAD_ZONE)
+        controller.inputs.joystick_y = 0;
+
+    // Buttons
+    controller.inputs.buttons.button_black = !gpio_get(BUTTON_BLACK_PIN);
+    controller.inputs.buttons.button_blue  = !gpio_get(BUTTON_BLUE_PIN);
+    controller.inputs.buttons.button_white = !gpio_get(BUTTON_WHITE_PIN);
+    controller.inputs.buttons.button_green = !gpio_get(BUTTON_GREEN_PIN);
+    controller.inputs.buttons.button_l     = !gpio_get(BUTTON_L_PIN);
+    controller.inputs.buttons.button_r     = !gpio_get(BUTTON_R_PIN);
+}

program/src/wifi/dhcp_server.c

@@ -60,7 +60,7 @@ typedef struct {
     uint8_t options[312]; // optional parameters, variable, starts with magic
 } dhcp_msg_t;
 
-static int dhcp_socket_sendto(struct udp_pcb **pcb, struct netif *nif, const void *buf, size_t len, uint32_t ip, uint16_t port)
+static void dhcp_socket_sendto(struct netif *nif, const void *buf, size_t len, uint32_t ip, uint16_t port)
 {
     if(len > 0xffff)
     {
@@ -70,7 +70,8 @@ static int dhcp_socket_sendto(struct udp_pcb **pcb, struct netif *nif, const voi
     struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_RAM);
     if(p == NULL)
     {
-        return -ENOMEM;
+        puts("Pbuf allocation error");
+        return;
     }
 
     memcpy(p->payload, buf, len);
@@ -82,21 +83,19 @@ static int dhcp_socket_sendto(struct udp_pcb **pcb, struct netif *nif, const voi
 
     if(nif != NULL)
     {
-        err = udp_sendto_if(*pcb, p, &dest, port, nif);
+        err = udp_sendto_if(controller.dhcp_server.pcb, p, &dest, port, nif);
     }
     else
     {
-        err = udp_sendto(*pcb, p, &dest, port);
+        err = udp_sendto(controller.dhcp_server.pcb, p, &dest, port);
     }
 
     pbuf_free(p);
 
     if(err != ERR_OK)
     {
-        return err;
+        printf("Error sending dhcp packets : %d\n", err);
     }
-
-    return len;
 }
 
 static uint8_t *opt_find(uint8_t *opt, uint8_t cmd)
@@ -144,10 +143,8 @@ static void opt_write_u32(uint8_t **opt, uint8_t cmd, uint32_t val)
     *opt = o;
 }
 
-static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct pbuf *p, const ip_addr_t *src_addr __unused, u16_t src_port __unused)
+static void dhcp_server_process(void *arg __unused, struct udp_pcb *upcb __unused, struct pbuf *p, const ip_addr_t *src_addr __unused, u16_t src_port __unused)
 {
-    dhcp_server_t *d = arg;
-
     // This is around 548 bytes
     dhcp_msg_t dhcp_msg;
 
@@ -162,7 +159,7 @@ static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct
     }
 
     dhcp_msg.op = DHCPOFFER;
-    memcpy(&dhcp_msg.yiaddr, &ip4_addr_get_u32(ip_2_ip4(&d->ip)), 4);
+    memcpy(&dhcp_msg.yiaddr, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip)), 4);
 
     uint8_t *opt = (uint8_t *)&dhcp_msg.options;
     opt += 4; // assume magic cookie: 99, 130, 83, 99
@@ -177,21 +174,21 @@ static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct
         case DHCPDISCOVER: {
             int yi = DHCPS_MAX_IP;
             for (int i = 0; i < DHCPS_MAX_IP; ++i) {
-                if (memcmp(d->lease[i].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
+                if (memcmp(controller.dhcp_server.lease[i].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
                     // MAC match, use this IP address
                     yi = i;
                     break;
                 }
                 if (yi == DHCPS_MAX_IP) {
                     // Look for a free IP address
-                    if (memcmp(d->lease[i].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
+                    if (memcmp(controller.dhcp_server.lease[i].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
                         // IP available
                         yi = i;
                     }
-                    uint32_t expiry = d->lease[i].expiry << 16 | 0xffff;
+                    uint32_t expiry = controller.dhcp_server.lease[i].expiry << 16 | 0xffff;
                     if ((int32_t)(expiry - cyw43_hal_ticks_ms()) < 0) {
                         // IP expired, reuse it
-                        memset(d->lease[i].mac, 0, MAC_LEN);
+                        memset(controller.dhcp_server.lease[i].mac, 0, MAC_LEN);
                         yi = i;
                     }
                 }
@@ -211,7 +208,7 @@ static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct
                 // Should be NACK
                 goto ignore_request;
             }
-            if (memcmp(o + 2, &ip4_addr_get_u32(ip_2_ip4(&d->ip)), 3) != 0) {
+            if (memcmp(o + 2, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip)), 3) != 0) {
                 // Should be NACK
                 goto ignore_request;
             }
@@ -220,17 +217,17 @@ static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct
                 // Should be NACK
                 goto ignore_request;
             }
-            if (memcmp(d->lease[yi].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
+            if (memcmp(controller.dhcp_server.lease[yi].mac, dhcp_msg.chaddr, MAC_LEN) == 0) {
                 // MAC match, ok to use this IP address
-            } else if (memcmp(d->lease[yi].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
+            } else if (memcmp(controller.dhcp_server.lease[yi].mac, "\x00\x00\x00\x00\x00\x00", MAC_LEN) == 0) {
                 // IP unused, ok to use this IP address
-                memcpy(d->lease[yi].mac, dhcp_msg.chaddr, MAC_LEN);
+                memcpy(controller.dhcp_server.lease[yi].mac, dhcp_msg.chaddr, MAC_LEN);
             } else {
                 // IP already in use
                 // Should be NACK
                 goto ignore_request;
             }
-            d->lease[yi].expiry = (cyw43_hal_ticks_ms() + DEFAULT_LEASE_TIME_S * 1000) >> 16;
+            controller.dhcp_server.lease[yi].expiry = (cyw43_hal_ticks_ms() + DEFAULT_LEASE_TIME_S * 1000) >> 16;
             dhcp_msg.yiaddr[3] = DHCPS_BASE_IP + yi;
             opt_write_u8(&opt, DHCP_OPT_MSG_TYPE, DHCPACK);
             printf("DHCPS: client connected: MAC=%02x:%02x:%02x:%02x:%02x:%02x IP=%u.%u.%u.%u\n",
@@ -243,14 +240,14 @@ static void dhcp_server_process(void *arg, struct udp_pcb *upcb __unused, struct
             goto ignore_request;
     }
 
-    opt_write_n(&opt, DHCP_OPT_SERVER_ID, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip)));
-    opt_write_n(&opt, DHCP_OPT_SUBNET_MASK, 4, &ip4_addr_get_u32(ip_2_ip4(&d->nm)));
-    opt_write_n(&opt, DHCP_OPT_ROUTER, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); // aka gateway; can have multiple addresses
-    opt_write_n(&opt, DHCP_OPT_DNS, 4, &ip4_addr_get_u32(ip_2_ip4(&d->ip))); // this server is the dns
+    opt_write_n(&opt, DHCP_OPT_SERVER_ID, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip)));
+    opt_write_n(&opt, DHCP_OPT_SUBNET_MASK, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.nm)));
+    opt_write_n(&opt, DHCP_OPT_ROUTER, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip))); // aka gateway; can have multiple addresses
+    opt_write_n(&opt, DHCP_OPT_DNS, 4, &ip4_addr_get_u32(ip_2_ip4(&controller.dhcp_server.ip))); // this server is the dns
     opt_write_u32(&opt, DHCP_OPT_IP_LEASE_TIME, DEFAULT_LEASE_TIME_S);
     *opt++ = DHCP_OPT_END;
     struct netif *nif = ip_current_input_netif();
-    dhcp_socket_sendto(&d->pcb, nif, &dhcp_msg, opt - (uint8_t *)&dhcp_msg, 0xffffffff, PORT_DHCP_CLIENT);
+    dhcp_socket_sendto(nif, &dhcp_msg, opt - (uint8_t *)&dhcp_msg, 0xffffffff, PORT_DHCP_CLIENT);
 
 ignore_request:
     pbuf_free(p);
@@ -264,18 +261,18 @@ int dhcp_server_init(void)
 
     controller.dhcp_server.pcb = udp_new();
     if (controller.dhcp_server.pcb == NULL) {
-        return -ENOMEM;
-    }
-
-    // Register callback
-    udp_recv(controller.dhcp_server.pcb, dhcp_server_process, (void *)&controller.dhcp_server);
-
-    if(udp_bind(controller.dhcp_server.pcb, IP_ANY_TYPE, PORT_DHCP_SERVER))
-    {
+        puts("Error creating DHCP server");
         return -1;
     }
 
-    puts("DHCP server started");
+    // Register callback
+    udp_recv(controller.dhcp_server.pcb, dhcp_server_process, NULL);
+
+    if(udp_bind(controller.dhcp_server.pcb, IP_ANY_TYPE, PORT_DHCP_SERVER))
+    {
+        puts("Error binding DHCP server");
+        return -1;
+    }
 
     return 0;
 }

program/src/wifi/udp_server.c

@@ -1,9 +1,11 @@
 #include "headers/udp_server.h"
 
 #include <stdio.h>
+#include <string.h>
 #include "headers/controller.h"
 
-#define MSG_LEN 128
+#define MSG_LEN (sizeof(uint16_t) + sizeof(inputs_t)) / sizeof(uint8_t)
+#define MSG_DELAY_MS 50.0f
 
 int udp_server_init(void)
 {
@@ -16,34 +18,48 @@ int udp_server_init(void)
 
     if(udp_bind(controller.udp_server.pcb, IP_ADDR_ANY, UDP_SERVER_PORT))
     {
-        printf("Error bind UDP server");
+        puts("Error binding UDP server");
         return -1;
     }
 
-    puts("UDP server started");
-
     return 0;
 }
 
 void udp_server_send(void)
 {
-    static float elapsed_time = 0.0f;
-    elapsed_time += controller.delta_time_ms;
+    static float elapsed_time_ms = 0.0f;
+    elapsed_time_ms += controller.delta_time_ms;
 
     static uint8_t counter = 0;
 
-    if(elapsed_time >= 1000.0f)
+    if(elapsed_time_ms >= MSG_DELAY_MS)
     {
-        elapsed_time = 0.0f;
+        static union data_t {
+            struct {
+                uint16_t packet_number;
+                inputs_t inputs;
+            } hard;
+
+            uint8_t raw[MSG_LEN];
+        } data = {0};
+
+        data.hard.packet_number++;
+        data.hard.inputs = controller.inputs;
 
         struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, MSG_LEN, PBUF_RAM);
-        uint8_t *payload = (uint8_t *)p->payload;
-        payload[0] = counter;
+        memcpy((uint8_t *)p->payload, data.raw, MSG_LEN);
+        //printf("%d:%d   %d\n", data.raw[2], data.raw[3], data.raw[4]);
 
-        udp_sendto(controller.udp_server.pcb, p, IP_ADDR_ANY, UDP_CLIENT_PORT);
+        err_t error_code = udp_sendto(controller.udp_server.pcb, p, IP_ADDR_ANY, UDP_CLIENT_PORT);
 
         pbuf_free(p);
 
+        if(error_code)
+        {
+            printf("UDP send error : %d\n", error_code);
+        }
+
+        elapsed_time_ms = 0.0f;
         counter++;
     }
 }

program/src/wifi/wifi_operator.c

@@ -14,10 +14,7 @@ int wifi_operator_init(void)
     }
 
     cyw43_arch_enable_ap_mode(WIFI_OPERATOR_SSID, WIFI_OPERATOR_PASSWORD, CYW43_AUTH_WPA2_AES_PSK);
-
     cyw43_wifi_ap_set_channel(&cyw43_state, WIFI_OPERATOR_CHANNEL);
 
-    puts("AP mode enabled");
-
     return 0;
 }