msg.c

// Author: Charles Randolph
// Subauthors: Ester Vicario Bravo, Grzegorz Krukiewicz-Gacek

#include "msg.h"
#include "../crc/crc16.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>


/*
 *******************************************************************************
 *                             SYMBOLIC CONSTANTS                              *
 *******************************************************************************
*/


#define ERR_MSG(msg)		{ fprintf(stderr, \
	"%s:%d Err: %s!\n", __FILE__, __LINE__, msg); }


// How many bytes the send automaton will attempt to send at a time
#define SEND_QUEUE_BURST_SIZE		4


/*
 *******************************************************************************
 *                          Internal Type Definitions                          *
 *******************************************************************************
*/


// Describes the states of the message receiving automaton
typedef enum automaton_recv_state {
	STATE_RECV_MARKERS = 0,
	STATE_RECV_TYPE,
	STATE_RECV_PAYLOAD,
	STATE_RECV_CRC
} automaton_recv_state;


// Describes the states of the message sending automaton
typedef enum automaton_send_state {
	STATE_SEND_READY,
	STATE_SEND_BUSY
} automaton_send_state;


/*
 *******************************************************************************
 *                              Global Variables                               *
 *******************************************************************************
*/


// Maps msg_type to size of payload (NOT including markers/type/checksum)
ssize_t table_msg_payload_size[] = {
	33,										// MSG_TYPE_TELEMETRY
	16,										// MSG_TYPE_INPUT_CTRL
	1,										// MSG_TYPE_CALIBRATE
	1,										// MSG_TYPE_SET_STATE		
	12,										// MSG_TYPE_TELEMETRY_MOTORS
	13,										// MSG_TYPE_TELEMETRY_SENSORS_A
	10,										// MSG_TYPE_TELEMETRY_SENSORS_B
	10,										// MSG_TYPE_TELEMETRY_SENSORS_C
};


/*
 *******************************************************************************
 *                   Internal Read/Write for File-Descriptor                   *
 *******************************************************************************
*/


/*
 * Writes `size` bytes from buffer `b` to `fd`, which bridges to an RS232 port.
 * This function blocks until all bytes have been written.
 * Returns zero on success, and nonzero on error.
*/
static int write_rs232_fd (int fd, unsigned char *b, ssize_t size) {
	ssize_t written = 0, remaining = size;
	do {
		written = write(fd, b + written, remaining);
		if (written == -1) {
			ERR_MSG("Could not write to the RS232 File-Descriptor");
			return -1;
		}
		remaining -= written;
	} while (remaining > 0);

	return 0;
}


/*
 *******************************************************************************
 *                    Internal Packing Function Definitions                    *
 *******************************************************************************
*/
static void pack_msg_telemetry_motors (msg_data_telemetry_motors *data, unsigned char *b) {

	// Write timestamp to bytes [0-3]
	b[0] = data->time >> 24;
	b[1] = (data->time >> 16) & 0xFF;
	b[2] = (data->time >> 8)  & 0xFF;
	b[3] = data->time         & 0xFF;

	// Write motor RPMS to bytes [4-11]
	b[4]	= data->ae1 	>> 8;	b[5]	= data->ae1 	& 0xFF;
	b[6]	= data->ae2 	>> 8;	b[7]	= data->ae2 	& 0xFF;
	b[8]	= data->ae3		>> 8; 	b[9] 	= data->ae3		& 0xFF;
	b[10]	= data->ae4 	>> 8;	b[11]	= data->ae4 	& 0xFF;
}

static void pack_msg_telemetry_sensors_b (msg_data_telemetry_sensors_b *data, unsigned char *b) {

	// Write timestamp to bytes [0-3]
	b[0] = data->time >> 24;
	b[1] = (data->time >> 16) & 0xFF;
	b[2] = (data->time >> 8)  & 0xFF;
	b[3] = data->time         & 0xFF;

	// Write attitude to bytes [13-18] (two bytes per value)
	b[4]	= data->psi 	>> 8;	b[5]	= data->psi 	& 0xFF;
	b[6]	= data->theta 	>> 8;	b[7]	= data->theta 	& 0xFF;
	b[8]	= data->phi		>> 8; 	b[9] 	= data->phi		& 0xFF;
}

static void pack_msg_telemetry_sensors_a (msg_data_telemetry_sensors_a *data, unsigned char *b) {

	// Write timestamp to bytes [0-3]
	b[0] = data->time >> 24;
	b[1] = (data->time >> 16) & 0xFF;
	b[2] = (data->time >> 8)  & 0xFF;
	b[3] = data->time         & 0xFF;

	// Write battery voltage to byte [12]
	b[4]	= data->charge;

	// Write temp to byte [25-28]
	b[5] = data->temp >> 24; 		b[6] = (data->temp >> 16) & 0xFF;
	b[7] = (data->temp >> 8) & 0xFF; b[8] = data->temp & 0xFF;

	// Write pressure to byte [29-32]
	b[9] = data->pres >> 24;		b[10] = (data->pres >> 16) & 0xFF;
	b[11] = (data->pres >> 8) & 0xFF; b[12] = data->pres & 0xFF;
}

static void pack_msg_telemetry (msg_data_telemetry *data, unsigned char *b) {

	// Write timestamp to bytes [0-3]
	b[0] = data->time >> 24;
	b[1] = (data->time >> 16) & 0xFF;
	b[2] = (data->time >> 8)  & 0xFF;
	b[3] = data->time         & 0xFF;

	// Write motor RPMS to bytes [4-11]
	b[4]	= data->ae1 	>> 8;	b[5]	= data->ae1 	& 0xFF;
	b[6]	= data->ae2 	>> 8;	b[7]	= data->ae2 	& 0xFF;
	b[8]	= data->ae3		>> 8; 	b[9] 	= data->ae3		& 0xFF;
	b[10]	= data->ae4 	>> 8;	b[11]	= data->ae4 	& 0xFF;

	// Write battery voltage to byte [12]
	b[12]	= data->charge;

	// Write attitude to bytes [13-18] (two bytes per value)
	b[13]	= data->psi 	>> 8;	b[14]	= data->psi 	& 0xFF;
	b[15]	= data->theta 	>> 8;	b[16]	= data->theta 	& 0xFF;
	b[17]	= data->phi		>> 8; 	b[18] 	= data->phi		& 0xFF;

	// Write rates to bytes [19-24] (two bytes per value)
	b[19]	= data->sp 		>> 8;	b[20]	= data->sp 		& 0xFF;
	b[21]	= data->sq 		>> 8;	b[22]	= data->sq 		& 0xFF;
	b[23]	= data->sr		>> 8; 	b[24] 	= data->sr		& 0xFF;

	// Write temp to byte [25-28]
	b[25] = data->temp >> 24; 		b[26] = (data->temp >> 16) & 0xFF;
	b[27] = (data->temp >> 8) & 0xFF; b[28] = data->temp & 0xFF;

	// Write pressure to byte [29-32]
	b[29] = data->pres >> 24;		b[30] = (data->pres >> 16) & 0xFF;
	b[31] = (data->pres >> 8) & 0xFF; b[32] = data->pres & 0xFF;
}


static void pack_msg_input_ctrl (msg_data_input_ctrl *data, unsigned char *b) {
	b[0] = data->desired_phi >> 8;	 	b[1] = data->desired_phi & 0xFF; 
	b[2] = data->desired_theta >> 8; 	b[3] = data->desired_theta & 0xFF;
	b[4] = data->desired_sr >> 8; 		b[5] = data->desired_sr & 0xFF;
	b[6] = data->desired_lift >> 8; 	b[7] = data->desired_lift & 0xFF;
	b[8] = data->P >> 8; 				b[9] = data->P & 0xFF;
	b[10] = data->P1 >> 8; 				b[11] = data->P1 & 0xFF;
	b[12] = data->P2 >> 8; 				b[13] = data->P2 & 0xFF;
	b[14] = data->H >> 8; 				b[15] = data->H & 0xFF;
}


static void pack_msg_calibrate (msg_data_calibrate *data, unsigned char *b) {
	b[0] = data->p;
}


static void pack_msg_set_state (msg_data_set_state *data, unsigned char *b) {
	b[0] = data->state;
}


/*
 *******************************************************************************
 *                   Internal Unpacking Function Definitions                   *
 *******************************************************************************
*/


static void unpack_msg_telemetry (msg_data_telemetry *data, unsigned char *b) {

	// Read timestamp from bytes [0-4]
	data->time = b[0];  data->time <<= 8;
	data->time |= b[1]; data->time <<= 8;
	data->time |= b[2]; data->time <<= 8;
	data->time |= b[3];

	// Read motor RPMS from bytes [4-11]
	data->ae1 = b[4]; data->ae1 <<= 8; data->ae1 |= b[5];
	data->ae2 = b[6]; data->ae2 <<= 8; data->ae2 |= b[7];
	data->ae3 = b[8]; data->ae3 <<= 8; data->ae3 |= b[9];
	data->ae4 = b[10]; data->ae4 <<= 8; data->ae4 |= b[11];

	// Read battery voltage from byte [12]
	data->charge = b[12];

	// Read attitude from bytes [13-18]
	data->psi = b[13]; 	data->psi <<= 8; 	data->psi |= b[14];
	data->theta = b[15];	data->theta <<= 8;	data->theta |= b[16];
	data->phi = b[17]; 	data->phi <<= 8;	data->phi |= b[18];

	// Read rates from bytes [19-24]
	data->sp = b[19]; data->sp <<= 8; data->sp |= b[20];
	data->sq = b[21]; data->sq <<= 8; data->sq |= b[22];
	data->sr = b[23]; data->sr <<= 8; data->sr |= b[24];

	// Read temp from bytes [25-28]
	data->temp = b[25]; data->temp <<= 8;
	data->temp |= b[26]; data->temp <<= 8;
	data->temp |= b[27]; data->temp <<= 8;
	data->temp |= b[28];

	// Read pressure from bytes [29-32]
	data->pres 	= b[29]; data->pres <<= 8; 
	data->pres |= b[30]; data->pres <<= 8;
	data->pres |= b[31]; data->pres <<= 8;
	data->pres |= b[32];
}


static void unpack_msg_input_ctrl (msg_data_input_ctrl *data, unsigned char *b) {
	data->desired_phi = b[0];	data->desired_phi <<= 8; 	data->desired_phi |= b[1];
	data->desired_theta= b[2];	data->desired_theta <<= 8; 	data->desired_theta |= b[3];
	data->desired_sr = b[4];	data->desired_sr <<= 8; 	data->desired_sr |= b[5];
	data->desired_lift = b[6];	data->desired_lift <<= 8; 	data->desired_lift |= b[7];
	data->P = b[8];			data->P <<= 8; 				data->P |= b[9];
	data->P1 = b[10];		data->P1 <<= 8; 				data->P1 |= b[11];
	data->P2 = b[12];		data->P2 <<= 8; 				data->P2 |= b[13];
	data->H = b[14];		data->H <<= 8; 				data->H |= b[15];

}


static void unpack_msg_calibrate (msg_data_calibrate *data, unsigned char *b) {
	data->p = b[0];
}


static void unpack_msg_set_state (msg_data_set_state *data, unsigned char *b) {
	data->state = b[0];
}


/*
 *******************************************************************************
 *                Inline Packing/Unpacking Switching Functions                 *
 *******************************************************************************
*/


// Packs a message. Must ensure msg_type is valid before calling!
static inline void pack_msg (msg_type t, void *msg_data_p, unsigned char *buffer) {
	switch (t) {
		case MSG_TYPE_TELEMETRY_SENSORS_B:
			pack_msg_telemetry_sensors_b((msg_data_telemetry_sensors_b *)msg_data_p, buffer);
			break;
		case MSG_TYPE_TELEMETRY_SENSORS_A:
			pack_msg_telemetry_sensors_a((msg_data_telemetry_sensors_a *)msg_data_p, buffer);
			break;
		case MSG_TYPE_TELEMETRY_MOTORS:
			pack_msg_telemetry_motors((msg_data_telemetry_motors *)msg_data_p, buffer);
			break;
		case MSG_TYPE_TELEMETRY:
			pack_msg_telemetry((msg_data_telemetry *)msg_data_p, buffer);
			break;
		case MSG_TYPE_INPUT_CTRL:
			pack_msg_input_ctrl((msg_data_input_ctrl *)msg_data_p, buffer);
			break;
		case MSG_TYPE_CALIBRATE:
			pack_msg_calibrate((msg_data_calibrate *)msg_data_p, buffer);
			break;
		case MSG_TYPE_SET_STATE:
			pack_msg_set_state((msg_data_set_state *)msg_data_p, buffer);
			break;
		default:
			ERR_MSG("Cannot pack invalid msg_type!");
	}
}

// Unpacks a message. Must ensure msg_tyoe is valid before calling!
static inline void unpack_msg (msg_type t, void *msg_data_p, unsigned char *buffer) {
	switch (t) {
		case MSG_TYPE_TELEMETRY:
			unpack_msg_telemetry((msg_data_telemetry *)msg_data_p, buffer);
			break;
		case MSG_TYPE_INPUT_CTRL:
			unpack_msg_input_ctrl((msg_data_input_ctrl *)msg_data_p, buffer);
			break;

		case MSG_TYPE_CALIBRATE:
			unpack_msg_calibrate((msg_data_calibrate *)msg_data_p, buffer);
			break;

		case MSG_TYPE_SET_STATE:
			unpack_msg_set_state((msg_data_set_state *)msg_data_p, buffer);
			break;

		default:
			ERR_MSG("Cannot unpack invalid msg_type!");
	}	
}


/*
 *******************************************************************************
 *                  Internal Supporting Function Definitions                   *
 *******************************************************************************
*/


/* 
 * Returns nonzero if the 16-bit CRC appended to the given buffer checks out
 * Otherwise returns zero.
*/
int validate_buffer_crc (size_t size, unsigned char *b) {

	// Assumes the CRC composes the last two bytes of the given buffer
	size_t actual_size = size - 2;

	// Compute the CRC
	uint16_t crc = crc16(actual_size, b);

	// Return nonzero if the crc is valid
	return ((unsigned char)(b[actual_size]) == (unsigned char)(crc >> 8) &&
	    (unsigned char)(b[actual_size + 1]) == (unsigned char)(crc & 0xFF));
} 

/*
 *******************************************************************************
 *                        External Function Definitions                        *
 *******************************************************************************
*/


/* Returns nonzero if the message type is defined. Otherwise nonzero */
int msg_type_valid (msg_type t) {
	int is_valid = 0;
	switch (t) {
		case MSG_TYPE_TELEMETRY:
		case MSG_TYPE_INPUT_CTRL:
		case MSG_TYPE_CALIBRATE:
		case MSG_TYPE_SET_STATE:
		case MSG_TYPE_TELEMETRY_MOTORS:
		case MSG_TYPE_TELEMETRY_SENSORS_A:
		case MSG_TYPE_TELEMETRY_SENSORS_B:
		case MSG_TYPE_TELEMETRY_SENSORS_C:
			is_valid = 1;
	};
	return is_valid;
}

/*
 * Packs and writes a message of the given type to the provided RS232
 * file-descriptor. Requires pointer to structure containing the message
 * data. Returns zero on success, otherwise returns nonzero.
*/
int send_msg_fd (int fd, msg_type t, void *msg_data_p) {
	unsigned char buffer[MSG_MAX_SIZE] = {0};
	ssize_t payload_size, offset = 0;

	// Check the message type
	if (msg_type_valid(t) == 0) {
		ERR_MSG("Invalid message type");
		return -1;
	} else {
		payload_size = table_msg_payload_size[t];
	}

	// Set message markers
	buffer[offset++] = MSG_MARKER_BYTE; buffer[offset++] = MSG_MARKER_BYTE;

	// Set the message type
	buffer[offset++] = t;

	// Insert message body
	pack_msg(t, msg_data_p, buffer + offset);
	offset += payload_size;

	// Insert checksum
	uint16_t crc = crc16(offset, buffer);
	buffer[offset++] = (crc >> 8) & 0xFF;
	buffer[offset++] = (crc & 0xFF);

	return write_rs232_fd(fd, buffer, offset);
}


/*
 * Returns a result type indicating the status of receives messages
 * over the given RS232 file-descriptor.
 * - RECV_NOTHING means there is no message yet processed
 * - RECV_MESSAGE means a message arrived, and the parameters are set
 * - RECV_ERROR means an error occurred
 * If a message is received, the type is placed in `tp`, and the
 * structure in `msg_data_p`.
*/
recv_result recv_msg_fd (int fd, msg_type *tp, void *msg_data_p)  {
	static unsigned char buffer[MSG_MAX_SIZE];
	static msg_type t;
	static automaton_recv_state state = STATE_RECV_MARKERS;
	static int markers = 0;
	static off_t offset = 0;
	static ssize_t payload_size = 0;
	int msg_is_valid;
	unsigned char c;

	// Check the queue for bytes to process
	if (read(fd, &c, 1) != 1) {
		return RECV_NOTHING;
	}

	// Otherwise, process the symbol in the automaton
	switch (state) {

		// Processes message markers [0xFF,0xFF]
		case STATE_RECV_MARKERS: {
			if (c == MSG_MARKER_BYTE) {
				markers++;
			} else {
				markers = 0;
			}
			if (markers == 2) {
				markers = 0;
				buffer[0] = buffer[1] = MSG_MARKER_BYTE;
				offset = 2;
				state = STATE_RECV_TYPE;
			}
		}
		return RECV_NOTHING;

		// Processes the message type 
		case STATE_RECV_TYPE: {
			if (msg_type_valid((msg_type)c)) {
				t = buffer[offset++] = (msg_type)c;
				payload_size = table_msg_payload_size[t];
				state = STATE_RECV_PAYLOAD;
			} else {
				ERR_MSG("Received an invalid message state");
				break;
			}
		}
		return RECV_NOTHING;

		// Processes the message payload
		case STATE_RECV_PAYLOAD: {
			if (payload_size > 1) {
				buffer[offset++] = c;
				payload_size--;
			} else {
				buffer[offset++] = c;
				payload_size = 0;
				state = STATE_RECV_CRC;
			}
		}
		return RECV_NOTHING;

		// Processes the CRC (reuses markers variable)
		case STATE_RECV_CRC: {
			if (markers == 1) {
				buffer[offset++] = c;
				markers = 0;
				msg_is_valid = validate_buffer_crc(offset, buffer);
				offset = 0;
				state = STATE_RECV_MARKERS;
				if (msg_is_valid == 1) {
					*tp = t;
					unpack_msg(t, msg_data_p, buffer + 3);
					return RECV_MESSAGE;
				}
				ERR_MSG("Message CRC is invalid");
				break;
			} else {
				buffer[offset++] = c;
				markers++;
			}
		}
		return RECV_NOTHING;

		default:
			ERR_MSG("Unreachable code reached (god help us all)");
			break;
	}

	// Resets the machine. Should be executed on encountering anomalies
	state = STATE_RECV_MARKERS;
	markers = offset = payload_size = 0;
	return RECV_ERROR;
}


/*
 * Packs and sends a message of type `t` using the provided function `f`.
 * `f` should be able to safely insert bytes into the uart transmission queue.
 * Returns zero on success, otherwise nonzero
*/
int send_msg_queue (void (*f)(unsigned char *, ssize_t), msg_type t, void *msg_data_p) {
	unsigned char buffer[MSG_MAX_SIZE];
	off_t offset = 0;

	// Verify the message type and function
	if (msg_type_valid(t) == 0 || f == NULL) {
		return -1;
	}

	// Affix marker bytes, then the type
	buffer[offset++] = MSG_MARKER_BYTE;
	buffer[offset++] = MSG_MARKER_BYTE;
	buffer[offset++] = t;

	// Pack the message payload
	pack_msg(t, msg_data_p, buffer + offset);

	// Update the payload size
	offset += table_msg_payload_size[t];

	// Compute and append the CRC
	uint16_t crc = crc16(offset, buffer);
	buffer[offset++] = (crc >> 8) & 0xFF;
	buffer[offset++] = (crc & 0xFF);

	// Dispatch the message
	f(buffer, offset);

	return 0;
}


/*
 * Returns a result type indicating the status of the message receive queue
 * - RECV_NOTHING means there is no message yet processed
 * - RECV_MESSAGE means a message arrived, and the parameters are set
 * - RECV_ERROR means an error occurred
 * If a message is received, the type is placed in `tp`, and the
 * structure in `msg_data_p`.
*/
recv_result recv_msg_queue (queue *rqp, msg_type *tp, void *msg_data_p)  {
	static unsigned char buffer[MSG_MAX_SIZE];
	static msg_type t;
	static automaton_recv_state state = STATE_RECV_MARKERS;
	static int markers = 0;
	static off_t offset = 0;
	static ssize_t payload_size = 0;
	int exists_pending_data = 0, msg_is_valid;
	unsigned char c;

	// Check the queue for bytes to process (disable interrupts)
	// __disable_irq();
	if (rqp->count > 0) {
		exists_pending_data = 1;
		c = dequeue(rqp);
	}
	// __enable_irq();

	// If there is nothing to process, return immediately
	if (exists_pending_data == 0) {
		return RECV_NOTHING;
	}

	// Otherwise, process the symbol in the automaton
	switch (state) {

		// Processes message markers [0xFF,0xFF]
		case STATE_RECV_MARKERS: {
			if (c == MSG_MARKER_BYTE) {
				markers++;
			} else {
				markers = 0;
			}
			if (markers == 2) {
				markers = 0;
				buffer[0] = buffer[1] = MSG_MARKER_BYTE;
				offset = 2;
				state = STATE_RECV_TYPE;
			}
		}
		return RECV_NOTHING;

		// Processes the message type 
		case STATE_RECV_TYPE: {
			if (msg_type_valid((msg_type)c)) {
				t = buffer[offset++] = (msg_type)c;
				payload_size = table_msg_payload_size[t];
				state = STATE_RECV_PAYLOAD;
			} else {
				ERR_MSG("Received an invalid message state");
				break;
			}
		}
		return RECV_NOTHING;

		// Processes the message payload
		case STATE_RECV_PAYLOAD: {
			if (payload_size > 1) {
				buffer[offset++] = c;
				payload_size--;
			} else {
				buffer[offset++] = c;
				payload_size = 0;
				state = STATE_RECV_CRC;
			}
		}
		return RECV_NOTHING;

		// Processes the CRC (reuses markers variable)
		case STATE_RECV_CRC: {
			if (markers == 1) {
				buffer[offset++] = c;
				markers = 0;
				msg_is_valid = validate_buffer_crc(offset, buffer);
				offset = 0;
				state = STATE_RECV_MARKERS;
				if (msg_is_valid == 1) {
					*tp = t;
					unpack_msg(t, msg_data_p, buffer + 3);
					return RECV_MESSAGE;
				}
				break;
			} else {
				buffer[offset++] = c;
				markers++;
			}
		}
		return RECV_NOTHING;

		default:
			ERR_MSG("Unreachable code reached (god help us all)");
			break;
	}

	// Resets the machine. Should be executed on encountering anomalies
	state = STATE_RECV_MARKERS;
	markers = offset = payload_size = 0;
	return RECV_ERROR;
}