Sunday, March 24, 2013

ChibiOS, lwIP, UDP, and You

Introduction
It certainly has been a while since I posted anything.  I increased my course load in hopes of actually finishing college some day, and have had some family issues which ate up a great deal of my time. Hopefully, this post will be interesting enough make up for that fact.

I have recently joined a high power rocketry club, and am involved in the development of avionics software for the rocket.  Since I don't work in aerospace (telecom guy here) and know very little about embedded software development, I have been scrambling up the learning curve.

The club I have joined is in the process of converting their flight control systems from communicating over USB to ethernet.  The goal for this is reduced latency and better throughput.  We're using a bunch of Olimex STM32-E407 boards to relay sensor data back to the flight computer and to control various actuators.  The boards run ChibiOS/RT.  IP stuff is handled by lwIP.  I'm going to spend a little bit of time going over what has to be done in order to get everything up and running.  The code I've been working on does other stuff in addition to networking, so I'm just going to be posting network related snippets, instead of just doing a code dump.  If I miss something, I sincerely apologize.

main.c
The high level logic of the application lives in main.cThe following includes are added:


#include <lwip/ip_addr.h>
#include "data_udp.h"
#include "lwipopts.h"
#include "lwipthread.h"


The <lwip/ipaddr.h> and "lwipthread.h" headers are part of lwIP and have not been modified.  The data_udp.h file contains function prototypes and defines for UDP stuff that comes later.  The lwipopts.h header contains the configuration for lwIP.  Due to the simplicity of ChibiOS, all configuration is done at compile time, so you will probably want to edit that file.

The other relevant stuff in main.c is in the main() function.  Here, we declare the lwip configuration and  configure the ethernet interface on the board:
          struct lwipthread_opts   ip_opts;
    static       uint8_t      macAddress[6]    =     {0xC2, 0xAF, 0x51, 0x03, 0xCF, 0x46};
    struct ip_addr ip, gateway, netmask;
   IP4_ADDR(&ip,      10, 0, 0, 2);
   IP4_ADDR(&gateway, 10, 0, 0, 254);
   IP4_ADDR(&netmask, 255, 255, 255, 0);
   ip_opts.address    = ip.addr;
   ip_opts.netmask    = netmask.addr;
   ip_opts.gateway    = gateway.addr;
   ip_opts.macaddress = macAddress;


Now we fire off a thread for the UDP listener.  We are using a nice 32 bit ARM chip, so we can have multiple threads doing multiple things on our board.  Since we  have a metric shitton of GPIO pins and will probably want to do stuff to more than one of them at a time, I think this is the way to go.

chThdCreateStatic(wa_data_udp_receive_thread, sizeof(wa_data_udp_receive_thread), NORMALPRIO, data_udp_receive_thread, NULL);


data_udp.c
In this one, we again have some includes:
#include "lwip/opt.h"
#include "lwip/arch.h"
#include "lwip/api.h"
#include "lwip/ip_addr.h"

#include "data_udp.h"

All of these except data_udp.h are standard lwIP stuff.  The data_udp.h header has some defines that we need to get stuff working that I'll cover shortly.

First we set up a working area for the receive thread that gets started in main().
WORKING_AREA(wa_data_udp_receive_thread, DATA_UDP_SEND_THREAD_STACK_SIZE);


This function spins in the thread we created.  It spins, and lets the function data_udp_rx_serve() handle what packets come in.
msg_t data_udp_receive_thread(void *p) {
  void * arg __attribute__ ((unused)) = p;

  struct netconn *conn;

  chRegSetThreadName("data_udp_receive_thread");

  chThdSleepSeconds(2);


  IP4_ADDR(&ip_addr_fc, 10,0,0,2);
  /* Create a new UDP connection handle */
  conn = netconn_new(NETCONN_UDP);
  LWIP_ERROR("data_udp_receive_thread: invalid conn", (conn != NULL), return RDY_RESET;);

  netconn_bind(conn, &ip_addr_fc, DATA_UDP_RX_THREAD_PORT);

  while(1) {
    data_udp_rx_serve(conn);
  }
  return RDY_OK;
}


The data_udp_rx_serve function looks like this:
 static void data_udp_rx_serve(struct netconn *conn) {
  BaseSequentialStream *chp =  (BaseSequentialStream *)&SDU1;
  struct netbuf   *inbuf;
  struct pbuf *buf;
  char cmdbuf[64];
  uint16_t        buflen = 0;
  uint16_t        i      = 0;
  err_t           err;
  /*fill buffer with nulls*/
  for (i = 0; i < 64; i ++) {
    cmdbuf[i] = 0;
  }
  /* Read the data from the port, blocking if nothing yet there.
   We assume the request (the part we care about) is in one netbuf */
  chprintf(chp, ".w.\r\n");
  err = netconn_recv(conn, &inbuf);
  chprintf(chp, ".+.\r\n");
  if (err == ERR_OK) {
    /*netbuf_data(inbuf, (void **)&buf, &buflen);*/
     /*int bytesCopied = netbuf_copy(inbuf, (void **)&buf, &buflen); */
    int bytesCopied = netbuf_copy(inbuf, cmdbuf, 64);
   chprintf(chp, "\r\ndata_udp_rx: %s", cmdbuf);
    chprintf(chp, "\r\n");
    chprintf(chp, "copied %d bytes\n", bytesCopied);
    if (strncmp("GETPWMWIDTH", (const char *) cmdbuf, 11) == 0) {
        char respBuf[64];
        unsigned int pulseWidth = getPulseWidth();
        sprintf(respBuf, "PULSE WIDTH %d", pulseWidth);
        sendResponsePacket(respBuf);
    } else {
        sendResponsePacket("CMDUNDEF");
    };

  }
  /*fill buffer with nulls*/
  for (i = 0; i < 64; i ++) {
    cmdbuf[i] = 0;
  }
  netconn_close(conn);

 
  /* Delete the buffer (netconn_recv gives us ownership,
   so we have to make sure to deallocate the buffer) */
  netbuf_delete(inbuf);
}

Essentially, we block until we read a UDP packet.  After that we copy it out into a buffer and do a standard string comparison against the buffer's contents.  We respond to the contents by sending another packet with sendResponsePacket().


 void sendResponsePacket( char  payload[]) {
   struct     netconn    *conn;
   char                   msg[DATA_UDP_MSG_SIZE] ;
   struct     netbuf     *buf;
   char*                  data;
    struct ip_addr addr;
    addr.addr = UDP_TARGET;
   conn       = netconn_new( NETCONN_UDP );
   netconn_bind(conn, NULL, 35001 ); //local port

   netconn_connect(conn, &addr , DATA_UDP_REPLY_PORT );
   buf     =  netbuf_new();
   data    =  netbuf_alloc(buf, sizeof(msg));
   sprintf(msg, "%s", payload);
   memcpy (data, msg, sizeof (msg));
   netconn_send(conn, buf);
   netbuf_delete(buf); // De-allocate packet buffer
    netconn_disconnect(conn);
}

 
This essentially works as expected.  We create a new connection, and set our source port as 35001.  We then fire a packet off to the target, and call it a day.

data_udp.h
There are few interesting things in this file.  The port numbers are simply added as #define s, as are the stack sizes.  The only thing really worth mentioning is that the IP addresses must be defined in hex, and stored in network bit order.  The former can be accomplished by writing the hex representation of each octet in order, without the periods.  The latter is handled by the htonl() function:
 #define UDP_TARGET                                (htonl(0xA000001))        //10.0.0.1


Conclusion 
I'm a complete noob when it comes to embedded things, and a bit rusty with regard to C.  None the less, I seem to be able to get this board doing stuff.  Hopefully there will be epic rocket fun in my future.

1 comment:

  1. Hi,

    I took your send function as an example to send an UDP packet myself using ChiBIOS and LWIP. However the UDP packet is not coming out on the interface. It keeps silent.

    When I change NETCONN_UDP to NETCONN_TCP I see some ARP and TCP traffic. But with UDP nothing happens.

    I have enabled UDP inside my LWIP configuration. And no errors are returned on the send function. Would you maybe have an idea what could cause this?

    Cheers,
    Gerard

    ReplyDelete