---

nees-adxl.c

Go to the documentation of this file.

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <signal.h>
#include <time.h>
#include <pthread.h>
#include <getopt.h>

#include <sys/termios.h>
#include <fcntl.h>

#include "nsds_util.h"
#include "flog.h"
#include "adxl_util.h"

// ---------------------------------------------------------------------
// Globals

const int DAQ_PORT = 55050;

bool streaming_active;

const int TCP_Q_LEN = 1;

struct
{
    pthread_mutex_t   mutex;               
    bool              active[NUM_ADXL_CHANNELS];
    int               num_active;
} chan_struct;

typedef struct
{
    char   *serial_port;
    int    socket_fd;
    int    delay_usec;
} thread_params_t;

                
// ---------------------------------------------------------------------
int data_active_count(void)
{
    char         me[] = "data_active_count";
    int          rc;
    int          count;
    
    // This should be unthinkable, check anyway
    rc = pthread_mutex_lock(&chan_struct.mutex);
    if(rc != 0)
    {
        flog_usr(FLOG_ERROR, FL_ERR_MUTEX_FAILURE, me,
                 "Error locking mutex!");
        return(-1);
    }

    // Copy data
    count = chan_struct.num_active;
    
    rc = pthread_mutex_unlock(&chan_struct.mutex);
    if(rc != 0)
        flog_usr(FLOG_ERROR, FL_ERR_MUTEX_FAILURE, me,
                 "Error unlocking mutex!");
    
    return(count);
}
    
// ---------------------------------------------------------------------
bool data_channel_enabled(const int channel_id)
{
    char   me[] = "data_channel_enabled";
    int    rc;
    bool   is_act = false;
    
    // Range check
    if((channel_id < 0) || (channel_id >= NUM_ADXL_CHANNELS))
    {
        flog_usr(FLOG_ERROR, FL_ERR_BAD_PARAM, me,
                 "Error, invalid channel ID %d", channel_id);
        return(false);
    }

    // This should be unthinkable, check anyway
    rc = pthread_mutex_lock(&chan_struct.mutex);
    if(rc != 0)
    {
        flog_usr(FLOG_ERROR, FL_ERR_MUTEX_FAILURE, me,
                 "Error locking mutex!");
        return(false);
    }

    is_act = chan_struct.active[channel_id];
    
    rc = pthread_mutex_unlock(&chan_struct.mutex);
    if(rc != 0)
        flog_usr(FLOG_ERROR, FL_ERR_MUTEX_FAILURE, me,
                 "Error unlocking mutex!");
    
    return(is_act);
}

// ---------------------------------------------------------------------
int data_channel_flag(const int channel_id, const bool subscribe)
{
    char      me[] = "data_channel_flag";
    int       rc;
    bool      old_setting;
    
    // Range check
    if((channel_id < 0) || (channel_id >= NUM_ADXL_CHANNELS))
    {
        flog_usr(FLOG_ERROR, FL_ERR_BAD_PARAM, me,
                 "Error, invalid channel ID %d", channel_id);
        return(-1);
    }

    // This should be unthinkable, check anyway
    rc = pthread_mutex_lock(&chan_struct.mutex);
    if(rc != 0)
    {
        flog_usr(FLOG_ERROR, FL_ERR_MUTEX_FAILURE, me,
                 "Error locking mutex!");
        return(rc);
    }

    // Save old
    old_setting = chan_struct.active[channel_id];
    
    // Flag as requested - idempotent
    chan_struct.active[channel_id] = subscribe;
    
    if(subscribe != old_setting)
    {
        if(subscribe)
            chan_struct.num_active++;
        else
            chan_struct.num_active--;
        
        if(chan_struct.num_active < 0)
        {
            flog_usr(FLOG_ERROR, FL_ERR_NOCANDO, me,
                     "Underflow in active channel count; code error!");
            chan_struct.num_active = 0;
        }
        
        if(chan_struct.num_active > NUM_ADXL_CHANNELS)
        {
            flog_usr(FLOG_ERROR, FL_ERR_NOCANDO, me,
                     "Overflow in active channel count; code error!");
            chan_struct.num_active = (NUM_ADXL_CHANNELS - 1);
        }
    }
    
    rc = pthread_mutex_unlock(&chan_struct.mutex);
    if(rc != 0)
    {
        flog_usr(FLOG_ERROR, FL_ERR_MUTEX_FAILURE, me,
                 "Error unlocking mutex!");
        return(rc);
    }

    // DDT, once mutex released
    if((subscribe) && (old_setting != subscribe))
    {
        flog_usr(FLOG_L2BUG, 0, me,
                 "Channel %d marked as active", channel_id);
    }
    else if(old_setting != subscribe)
    {
        flog_usr(FLOG_L2BUG, 0, me,
                 "Channel %d marked as inactive", channel_id);
    }
    
    return(0);
}
    

// ---------------------------------------------------------------------
void *daq_thread_main(void *arg)
{
    char          me[] = "daq_thread_main";
    thread_params_t *params = (thread_params_t *) arg;
    int           data_socket;
    uint32_t      idx = 0;
    uint32_t      data_idx = 0;
    uint32_t      byte_count = 0;
    char          data_buf[1024] = "";
    char          datum_buf[ADXL_DATUM_LEN] = "";
    int           rc;
    time_t        t_start, t_delta;
    int           COMM_FD = -1;
    adxl_reading_t data;

    
    // Check argument
    if(params == NULL)
    {
        flog_usr(FLOG_ERROR, FL_ERR_NULL_PTR, me, "Null pointer to thread!");
        return(NULL);
    }

    // Open the serial port to the ADXL
    COMM_FD = adxl_open(params->serial_port);
    if(COMM_FD <= 0)
    {
            flog_usr(FLOG_ERROR, FL_ERR_UNKNOWN, me,
                     "Unable to open ADXL serial port");
            return(NULL);
    }
    
    // Save args into local variable
    data_socket = params->socket_fd;

    flog_usr(FLOG_NOTICE, 0, me, "Data thread running");
    
    t_start = time(NULL);

    // Run until port closed, error, control-c or done streaming
    while((streaming_active) && (!control_break))
    {
        // read a value from the daq, blocking read
        rc = adxl_read(COMM_FD, &data);
        if(rc != 0)
        {
            flog_usr(FLOG_ERROR, FL_ERR_UNKNOWN, me,
                     "Error %d reading from ADXL", rc);
            goto thread_bailout;
        }
        
        // If no channels open, re-loop
        if(data_active_count() <= 0)
            continue;
        
        // Start buffer off with timestamp
        strcpy(data_buf, gen_timestamp());
        
        // Loop over channels, appending subscribed ones
        for(idx = 0; idx < NUM_ADXL_CHANNELS; idx++)
        {
            if(data_channel_enabled(idx))
            {
                // Format it up, NSDS-style, i.e. channel# / value
                sprintf(datum_buf, "\t%d\t%6.5f",
                        idx, data.G[idx]);
                
                // Append to send buffer
                strcat(data_buf, datum_buf);
            }
        }
        
        // Send it off
        rc = tcp_nl_write(data_socket, data_buf);
        if(rc != strlen(data_buf))
        {
            flog_usr(FLOG_ERROR, FL_ERR_TRANSCEIVE, me,
                     "Error sending data, short write");
            goto thread_bailout;
        }
        
        // Track ttl xmitted bytes
        byte_count += rc;
        data_idx++;
        
        // DDT, dump to screen every now and then
//      flog_usr(FLOG_DEBUG, 0, me, data_buf);

        // If user asked for slower sample rate, sleep between readings
        if(params->delay_usec > 0)
            usleep(params->delay_usec);
    }
    
thread_bailout:
    
    // Figure elapsed time, to nearest second
    t_delta = time(NULL) - t_start;
    if(t_delta == 0)
        t_delta = 1;
    
    flog_usr(FLOG_NOTICE, 0, me,
             "Data thread exiting, %d points sent", data_idx + 1);
    flog_usr(FLOG_CNOTICE, 0, me, 
             "appx %3.1f kb/sec and %5.1f samples/sec",
             (float) (byte_count >> 10) / t_delta,
             (data_idx + 1.0) / (float) t_delta);
    
    // Notify main thread
    streaming_active = false;
    
    adxl_close(COMM_FD);
    
    return(NULL);
}


// ---------------------------------------------------------------------
int daq_do_work(const int control_socket, const int data_socket,
    const thread_params_t *thread_params)
{
    char              me[] = "daq_do_work";
    int               rc, bytes_read, idx;
    time_t            req_timeout = 0;
    char              cmd_buf[MAX_CMD_LEN] = "";
    char              daq_stopped[] = "stopped";
    char              daq_running[] = "streaming";
    char              *daq_status = NULL;
    char              wtf[] = "Unknown command";
    char              strm_ok[] = "Streaming data on data channel from port";
    char              strm_stop[] = "Stopping data on data channel from port";
    pthread_t         data_thread;
    char              *cptr = NULL;
    char              *s_ptr = NULL;
    char              reply_buf[128] = "";
    int               chan_id;
    char              tok_buf[MAX_CMD_LEN] = "";
    char              channel_list[NUM_ADXL_CHANNELS * 5] = "";
    
    
    flog_usr(FLOG_PROGRESS, 0, me, "Waiting for request (indefinite wait)...");
    
    // Clear buffers
    memset(cmd_buf, 0x00, MAX_CMD_LEN);
    memset(channel_list, 0x00, NUM_ADXL_CHANNELS * 4);
    
    // Build list of channels, static. Used to respond to 'list-channels'
    for(idx = 0; idx < NUM_ADXL_CHANNELS; idx++)
    {
        if(idx < (NUM_ADXL_CHANNELS - 1))
            sprintf(reply_buf, "%d,", idx);
        else
            sprintf(reply_buf, "%d", idx);

        strcat(channel_list, reply_buf);
    }

    // DDT
    assert(strlen(channel_list) < ((NUM_ADXL_CHANNELS * 4) - 1));

    // Read request from NSDS
    bytes_read = tcp_nl_read(control_socket, cmd_buf, req_timeout);
    if(bytes_read <= 0)
    {
        flog_usr(FLOG_NOTICE, FL_ERR_TRANSCEIVE, me,
                 "Error on request read");
        return(1);
    }
    
    // Hardwired command recognition for now!
    if(strstr(cmd_buf, "list-channels") != NULL)
    {
        flog_usr(FLOG_PROGRESS, 0, me,
                 "Got request for channel list");

        // Send pre-built list
        rc = tcp_nl_write(control_socket, channel_list);
        if(rc != strlen(channel_list))
        {
            flog_usr(FLOG_ERROR, FL_ERR_TRANSCEIVE, me,
                     "Error writing status response on control channel");
            return(2);
        }
    }
    else if(strstr(cmd_buf, "daq-status") != NULL)
    {
        if(streaming_active == true)
            daq_status = daq_running;
        else
            daq_status = daq_stopped;

        flog_usr(FLOG_L2BUG, 0, me,
                 "Got status request '%s' OK, responding with '%s'",
                 cmd_buf, daq_status);

        rc = tcp_nl_write(control_socket, daq_status);
        if(rc != strlen(daq_status))
        {
            flog_usr(FLOG_ERROR, FL_ERR_TRANSCEIVE, me,
                     "Error writing status response on control channel");
            return(2);
        }
    }
    else if(strstr(cmd_buf, "open-port") != NULL)
    {
        // Save a copy to munge
        strcpy(tok_buf, cmd_buf);

        // Start the parser, ignore first result ('open-port')
        cptr = strtok_r(tok_buf, CMD_BUF_DELIM_CHARS, &s_ptr);
        cptr = strtok_r(NULL, CMD_BUF_DELIM_CHARS, &s_ptr);
        // Cptr should now point to numeric argument
        if(cptr == NULL)
        {
            flog_usr(FLOG_ERROR, FL_ERR_BAD_PARAM, me,
                     "Invalid data port open, channel ID not found");

            // Send out NAK message, blind send
            tcp_nl_write(control_socket, BAD_PORT);
            return(-1);
        }

        // Single arg in channel ID
        chan_id = atoi(cptr);

        flog_usr(FLOG_L2BUG, 0, me,
                 "Got data subscription request for channel %d", chan_id);

        // If this is the first request, we have to start the data thread
        if(streaming_active == false)
        {
            flog_usr(FLOG_CL2BUG, 0, me, "Starting data thread");

            // Set boolean semaphore for data thread
            streaming_active = true;

            // Clear out all old subscriptions
            for(idx = 0; idx < NUM_ADXL_CHANNELS; idx++)
                data_channel_flag(idx, false);

            // Mark this one as active
            rc = data_channel_flag(chan_id, true);
            if(rc != 0)
            {
                flog_usr(FLOG_ERROR, FL_ERR_BAD_PARAM, me,
                         "Error %d activating channel %d", rc, chan_id);
                return(rc);
            }

            // Fire up a thread
            rc = pthread_create(&data_thread, NULL, daq_thread_main,
                                (void *) thread_params);
            if(rc != 0)
            {
                flog_usr(FLOG_ERROR, FL_ERR_SYSTEM, me,
                         "Error on thread creation!");
                return(3);
            }

            // Let 'er rip
            pthread_detach(data_thread);
            pthread_setconcurrency(2);

            flog_usr(FLOG_CL2BUG, 0, me, "Data thread started OK");
        }
        else // Streaming already up, just add new channel
        {
            flog_usr(FLOG_CL2BUG, 0, me, "adding to active list");

            // Mark as active
            rc = data_channel_flag(chan_id, true);
            if(rc != 0)
            {
                flog_usr(FLOG_ERROR, FL_ERR_BAD_PARAM, me,
                         "Error %d activating channel %d", rc, chan_id);
                return(rc);
            }
        }

        // Append channel ID
        sprintf(reply_buf, "%s %d", strm_ok, chan_id);

        // Ack to driver, on _control_ channel
        rc = tcp_nl_write(control_socket, reply_buf);
        if(rc != strlen(reply_buf))
        {
            flog_usr(FLOG_ERROR, FL_ERR_TRANSCEIVE, me,
                     "Error writing response on control channel");
            return(2);
        }
    }
    else if(strstr(cmd_buf, "close-port") != NULL)
    {
        // Save a copy to munge
        strcpy(tok_buf, cmd_buf);

        // Start the parser, ignore first result ('close-port')
        cptr = strtok_r(tok_buf, CMD_BUF_DELIM_CHARS, &s_ptr);
        cptr = strtok_r(NULL, CMD_BUF_DELIM_CHARS, &s_ptr);
        // Cptr should now point to numeric argument
        if(cptr == NULL)
        {
            flog_usr(FLOG_ERROR, FL_ERR_BAD_PARAM, me,
                     "Invalid data port close, channel ID not found");
            tcp_nl_write(control_socket, BAD_PORT);
            return(-1);
        }

        // Single arg in channel ID
        chan_id = atoi(cptr);

        flog_usr(FLOG_NOTICE, 0, me,
                 "Got port close request on channel %d", chan_id);

        // Mark as inactive
        rc = data_channel_flag(chan_id, false);

        // No more channels? Close thread
        if(data_active_count() <= 0)
            streaming_active = false;

        // Append channel ID
        sprintf(reply_buf, "%s %d", strm_stop, chan_id);

        // Ack to driver, on _control_ channel
        rc = tcp_nl_write(control_socket, reply_buf);
        if(rc != strlen(reply_buf))
        {
            flog_usr(FLOG_ERROR, FL_ERR_TRANSCEIVE, me,
                     "Error writing response on control channel");
            return(2);
        }
    }
    else // all other commands
    {
        flog_usr(FLOG_ERROR, FL_ERR_NOCANDO, me,
                 "Got unknown command '%s'", cmd_buf);
        rc = tcp_nl_write(control_socket, wtf);
        if(rc != strlen(wtf))
        {
            flog_usr(FLOG_ERROR, FL_ERR_TRANSCEIVE, me,
                     "Error writing response on control channel");
            return(2);
        }
    }

    // Make super-sure response is sent
    fsync(control_socket);

    flog_usr(FLOG_L2BUG, 0, me, "done");

    return(0);
}

// ---------------------------------------------------------------------
void daq_main_loop(const int daq_port,
                   const char *serial_portname, const int sample_delay)
{
    char               me[] = "daq_main_loop";
    int                ctrl_master, data_master;
    int                control_socket, data_socket;
    struct sockaddr_in fsin;
    int                addr_len = sizeof(fsin);
    bool               do_init = false;
    int                cmd_count = 0;
    thread_params_t    thread_params;

    // Set up master control socket
    ctrl_master = tcp_socket_make(daq_port, TCP_Q_LEN);
    if(ctrl_master <= 0)
        return;
        
    // Set up master data socket
    data_master = tcp_socket_make(daq_port + 1, TCP_Q_LEN);
    if(data_master <= 0)
        return;

    // ------------------------
    // Main loop, runs until control-c
    while(control_break == false)
    {
        // If restarting, close all first
        if(do_init == true)
        {
            flog_usr(FLOG_PROGRESS, 0, me,
                     "Re-init DAQ, closing connection");

            tcp_close(control_socket);
            tcp_close(data_socket);

            // Tell data thread to exit
            streaming_active = false;
        }
        else // Set flag for subsequent loops
            do_init = true;

        
        // Off we go
        flog_usr(FLOG_L1BUG, 0, me, "Waiting for driver control connection...");

        control_socket = accept(ctrl_master,
                                (struct sockaddr *) &fsin, &addr_len);
        // Check who connected
        flog_usr(FLOG_CL1BUG, 0, me,
                 "Driver '%s' connected to control channel",
                 tcp_peername(control_socket));

        flog_usr(FLOG_L1BUG, 0, me, "Waiting for driver data connection...");

        data_socket = accept(data_master,
                             (struct sockaddr *) &fsin, &addr_len);

        // Check who connected
        flog_usr(FLOG_CL1BUG, 0, me,
                 "Driver '%s' connected to data channel",
                 tcp_peername(data_socket));

        // Populate struct used by data thread
        thread_params.serial_port = (char *) serial_portname;
        thread_params.socket_fd = data_socket;
        thread_params.delay_usec = sample_delay;
    
        /*
          Call the routine to read and respond
          If an error, we will re-init the TCP connections.
        */
        while(daq_do_work(control_socket, data_socket, &thread_params) == 0)
        {
            flog_usr(FLOG_L2BUG, 0, me,
                     "Command #%d completed OK", cmd_count++);
        }
    }

    return;
}
        
// ---------------------------------------------------------------------
int main(int argc, char *argv[])
{
    char          me[] = "main";
    uint16_t      tcp_port = 0;
    int           help_flag = 0;
    int           idx, rc;
    char          def_serial[] = "/dev/ttyS1";
    int           sample_rate = 0;
    char          *serial_portname = def_serial;
    uint32_t      sample_delay = 0;
    struct option long_options[] = 
        {
            {"port", required_argument, 0, 'p'},
            {"adxl_port", required_argument, 0, 'a'},
            {"rate", required_argument, 0, 'r'},
            {"help", no_argument, &help_flag, 'h'},
            {0, 0, 0, 0}
        };
    
    // Set defaults before (possibly) overriden by command line
    tcp_port = DAQ_PORT;
    sample_rate = 0;
    

    // Set logging options
    flog_set_report(FLOG_L3BUG, FLOG_QUIET, FLOG_QUIET);
    flog_set_style(0x2019, 0x0038, 0x6008);    

    // Parse command line with getopt_long
    while(1)
    {
        rc = getopt_long(argc, argv, "p:r:a:h",
                         long_options, &idx);
        if(rc == -1)
            break;
        
        switch(rc)
        {
        case 0:
            /* If this option set a flag, do nothing else now. */
            break;
            
        case 'p':
            tcp_port = atoi(optarg);
            flog_usr(FLOG_NOTICE, 0, me,
                     "Setting TCP server port to %d", tcp_port);
            break;
            
        case 'a':
            serial_portname = optarg;
            flog_usr(FLOG_NOTICE, 0, me, "Serial port set to '%s'",
                     serial_portname);
            break;
            
        case 'r':
                sample_rate = atoi(optarg);

                flog_usr(FLOG_NOTICE, 0, me,
                         "Setting sample rate to %d Hz", sample_rate);
                break;

            case '?':
                /* getopt_long already printed an error message. */
                break;

            case 'h':
                help_flag = 1;
                break;

            default:
                flog_usr(FLOG_ERROR, FL_ERR_SYSTEM, me,
                         "Unreachable case in getopt_long parse");
                exit(1);
        }
    }

    if(help_flag != 0)
    {
        print_args(long_options);
        return(1);
    }

    // Range-check sample rate
    if(sample_rate > 0)
    {
        if(sample_rate > ADXL_MAX_RATE_HZ)
        {
            flog_usr(FLOG_NOTICE, FL_ERR_BAD_PARAM, me,
                     "Sample rate %d Hz too high - setting to device max %d",
                     sample_rate, ADXL_MAX_RATE_HZ);
            // We lied - set to zero and data thread will not delay
            sample_rate = 0;
        }

        // Convert sample rate into microseconds for usleep()
        sample_delay = 1000000 / sample_rate;
    }
    else
        sample_delay = 0;
        
    
    flog_usr(FLOG_NOTICE, 0, me,
             "Compiled for %d data channels max", NUM_ADXL_CHANNELS);
    
    // Signal handler
    flog_usr(FLOG_NOTICE, 0, me, "Installing signal handler");
    control_break = false;
    signal(SIGINT, adxl_sighandler);
    signal(SIGPIPE, adxl_sighandler);

    // Call main routine
    daq_main_loop(tcp_port, serial_portname, sample_delay);

    // Bail
    flog_usr(FLOG_NOTICE, 0, me, "Done");
    return(0);
}

---