Datalink - UWARG/PICpilot GitHub Wiki

The datalink is one of the most important peripheral components in an unmanned system. It provides information on the status of the aircraft (telemetry) and provides the crucial functionality of an uplink, in order to be able to communicate with the system and make changes to its overall functionality.

Make note that the details of this system can change frequently due to the requirements of each user. The telemetry data may change from the ones present in this document. Likewise, uplink commands may change based on newly implemented features and requirements. Although, this document should be updated whenever there is a change, this may not always occur. Therefore, be careful when referencing data in this section.

Note that all telemetry data must be visible within the scope of the AttitudeManager.c file.

Every command that is sent to the UAV must be predefined with an ID, as well as an associated function. Some commands only change variable values, whereas some call functions with the associated data as a parameter.

Prior to usage, the datalink must be initialized. This is done so in _main.c _using initDataLink(). This simply initializes the UART2 interface (see UART section) for appropriate usage with the datalink.

After initialization, the data link can be used. The interface used to queue data to the datalink is present in the AttitudeManager.c file.

Data is exported to the data link at a certain frequency (according to a clock). This is done by calling writeDatalink(frequency), where frequency is the time between packets. This subroutine creates a structure (defined in net.h) which contains memory locations for every variable. This data is then pushed to be processed in net_outbound.c.

if (time - lastTime > frequency) {

    lastTime = time;

    struct telem_block* statusData = createTelemetryBlock();

    statusData->lat = gps_Latitude;

    statusData->lon = gps_Longitude;

    ...

return pushOutboundTelemetryQueue(statusData);

}

When all the data is assembled in the struct, pushOutboundTelemetryQueue(statusData) is called. This pushes the data onto a queue to be processed later:

int pushOutboundTelemetryQueue(struct telem_block *telem) {

    if (getOutboundQueueLength() >= OUTBOUND_QUEUE_SIZE) {

        return -1;

    }

    outBuffer[outbuff_end] = telem;

    outbuff_end++;

    outbuff_end = outbuff_end % OUTBOUND_QUEUE_SIZE;

    return getOutboundQueueLength();

}

Note that this is a circular buffer. When the buffer reaches the OUTBOUND_QUEUE_SIZE, the outbuff_end variable starts from 0 and overwrites the old data.

Every once in a while, the data accumulated must be processed. As a result, every iteration of the program runs a subroutine to maintain and cleanup the circular buffer. For the outgoing buffer, this method is outboundBufferMaintenance():

if ( stagingBuffer.sendIndex >= PACKET_LENGTH ) {

    destroyTelemetryBlock(stagingBuffer.telemetry.asStruct);

    if ( getOutboundQueueLength() ) {

        stageTelemetryBlock(popOutboundTelemetryQueue());

    }

} else if ( stagingBuffer.telemetry.asStruct == 0 && getOutboundQueueLength() ) {

    stageTelemetryBlock(popOutboundTelemetryQueue());

}

Note that the structure of stagingBuffer is as follows:

struct telem_buffer {

    unsigned int sendIndex;             // index into telemetry to send

    unsigned char header[API_HEADER_LENGTH];    // The header for the telem

    union {

        struct telem_block *asStruct;   // The telemetry block being sent

        unsigned char *asArray;         // The telemetry intepreted as an array

    } telemetry;

    unsigned char checksum;             // The checksum so far

};

Note that the _stagingBuffer _converts the data into a data link friendly format. The data link hardware requires that each data packet must be sent with a header, the data, and a checksum (For more specification see the XBEE section). These are 3 components of the telem_buffer structure. The 4th component is the sendIndex variable. This value is used to keep track (index) what data has already been sent or still needs to be sent.

After sufficient error checking (making sure sendIndex is less than the allowed packet size), stageTelemetryBlock(popOutboundTelemetryQueue()) is called. This method takes (pops) the next struct of data and stages it to be sent. stageTelemetryBlock() is responsible for converting the telemetry data into a telem_buffer structure.

void stageTelemetryBlock(struct telem_block *telem) {

    stagingBuffer.telemetry.asStruct = telem;

    generateApiHeader(stagingBuffer.header, 0);

    stagingBuffer.checksum = 0;

    // Send index should be reset last for reasons

    stagingBuffer.sendIndex = 0;

    sendNextByte();

}

The first line of the subroutine adds the data into the packet. The second line (generateApiHeader(stagingBuffer.header,0)) creates an appropriate header in the stagingBuffer.header memory address with a data frame of 0. (See the XBEE section for the datasheet). The API header includes information involving which device the packet should be sent to, the length of the packet, as well as acknowledgement options, and packet types (data packet, configuration packet, status packet). After the checksum and sendIndex are explicitly reset, the sending process begins with sendNextByte():

void sendNextByte(void) {

    unsigned char sendByte; // The byte to send

    if ( stagingBuffer.sendIndex < API_HEADER_LENGTH ) {

        //while (U2STAbits.TRMT == 0);

        sendByte = stagingBuffer.header[stagingBuffer.sendIndex] & 0xFF;

        // Compute checksum

        if (stagingBuffer.sendIndex >= 3) {

            stagingBuffer.checksum += sendByte & 0xFF;

        }

    } else if ( stagingBuffer.sendIndex < PACKET_LENGTH - 1 ) {

        sendByte = stagingBuffer.telemetry.asArray[stagingBuffer.sendIndex - API_HEADER_LENGTH] & 0xFF;

        stagingBuffer.checksum += sendByte & 0xFF;

    } else if ( stagingBuffer.sendIndex == PACKET_LENGTH - 1) {

        sendByte = 0xFF - (stagingBuffer.checksum & 0xFF);

    } else {

        IFS1bits.U2TXIF = 0;

        return;

    }

    stagingBuffer.sendIndex++;

    IFS1bits.U2TXIF = 0;

    U2TXREG = sendByte;

}

All the "if" statements above, compile the header, the data and the checksum together. Note that the checksum is the bitwise inverse of the actual sum: sendByte = 0xFF - (stagingBuffer.checksum & 0xFF). The most important part of this process is the last line, where each byte is sent to the UART transmit buffer. Since the UART transmit process is interrupt-based, each interrupt keeps calling sendNextByte(), until there is no more data left:

void __attribute__((__interrupt__, no_auto_psv)) _U2TXInterrupt(void) {

    // Short circuit if nothing in the staging area yet

    if ( stagingBuffer.telemetry.asStruct == 0 ) {

        IFS1bits.U2TXIF = 0;

        return;

    }

    sendNextByte();

}

The process can be described through this flowchart:

Once every iteration, a command is read from the uplink queue. This is done by calling readDatalink(). The command popCommand() is called. If any new commands have been received, popCommand() will return a command struct (defined in net.h):

struct command {

    unsigned char cmd;

    unsigned char data\_length;

    unsigned char data[101];

};

It is fairly straight forward. The structure contains a _cmd.cmd _which indicates the command ID. This ID corresponds to a certain function that needs to be completed. Following the pop command are a series of case statements (one for each command ID). For instance, if the command ID is 30, the following command is run (in net_inbound.c):

struct command* cmd = popCommand();

//TODO: Add rudimentary input validation

if ( cmd ) {

    if (lastCommandSentCode == cmd->cmd){

        lastCommandSentCode++;

    }

    else{

        lastCommandSentCode = cmd->cmd * 100;

    }

    switch (cmd->cmd) {

        ...

        case SET_THROTTLE:

            sp_ThrottleRate = (int)(*(int*)(&cmd->data) / 100.0  * (890 - 454) + 454);

            break;

        ...
    }

}

The throttle ends up being set to the value indicated in the cmd.data location. In addition, the last command read is stored and sent to the ground station as verification that the command was received.

The _popCommand() _function waits and reads the next available command from a circular buffer (note the INBOUND_QUEUE_SIZE variable). If the command doesn't exist, it exits the function.

In order for the command structure to exist, the U2RXInterrupt must have been triggered. This occurs when new data is sent.

void __attribute__((__interrupt__, no_auto_psv)) _U2RXInterrupt(void) {

    unsigned char data = U2RXREG;

    if ( rawPacketStatus[packetPos] != BUSY ) {    // no buffer available to write

        packetPos = ( packetPos + 1  ) % RAW_PACKET_BUFFER_SIZE;

        IFS1bits.U2RXIF = 0;

        return;

    }

    switch ( payloadPos ) {

        case 0:

            if ( data != START_DELIMITER ) {

                IFS1bits.U2RXIF = 0;

                return;

            }

            break;

        case 1:

            if ( data != 0 ) {

                payloadPos = 0;

                IFS1bits.U2RXIF = 0;

                return;                 // packet length < 100 bytes, so msb == 0

            }

            break;

        case 2:

            payloadLength[packetPos] = data;

            break;

        default:        // Normally, don't do anything special

            break;

    }

    rawPackets[packetPos][payloadPos++] = data;

    if ( payloadPos && payloadPos == payloadLength[packetPos] + 3 + 1) {   // at end of packet

        rawPacketStatus[packetPos] = READY;

        payloadPos = 0;

        packetPos = ( packetPos + 1  ) % RAW_PACKET_BUFFER_SIZE;

        if ( rawPacketStatus[packetPos] == EMPTY ) {

            rawPacketStatus[packetPos] = BUSY;

        }

    }

    IFS1bits.U2RXIF = 0;

}

The first thing that occurs when new data arrives is a check to see if there is enough memory to store the data. A if statement is used to check if the buffer (rawPacketStatus) is busy or not. Note that the interrupt will only record the data, if the current packetPos marked busy.

If the buffer is full, the next buffer location is checked. Otherwise, the packet is parsed byte by byte.

Firstly, the start delimiter is looked for using a case statement. Until the start delimiter is found, nothing happens. Secondly, for case 1 and 2, the length of the packet is check and recorded. Once the length of the packet is known, the data is read into a 2d array called rawPackets. This array contains each byte of every packet in the circular buffer. Once all the data is copied into the array, the packet is marked as READY, and the next one is marked BUSY if it is EMPTY, and the processing of the data begins on the next maintenance cycle when inboundBufferMaintenance() is called from main.c:

void inboundBufferMaintenance(void) {

    int i;

    for ( i = 0; i < RAW_PACKET_BUFFER_SIZE; i++ ) {

        if ( rawPacketStatus[i] == READY && checkPacket(rawPackets[i]) ) {

            struct command\* cmd = createCommand( rawPackets[i] );

            if ( cmd ) {            // create command was successful ?

                pushCommand( cmd ); // queue it up

                rawPacketStatus[i] = EMPTY;         // buffer is now good for writing another packet

            }

        }

    }

    if ( rawPacketStatus[0] == EMPTY ) {

        rawPacketStatus[0] = BUSY;

    }

}

This subroutine iterates through each buffer location and checks for any READY packets. If one is found, and it has been verified through a checksum, the command structure is created from the data using the createCommand(rawPackets[i]) method. Once this is done, the previous spot is marked EMPTY. The first buffer is always primed if it is empty.