ESAR - Extraordinarily Simple AIS Receiver

ESAR.c

/*
 * source: https://www.rtl-sdr.com/esar-extraordinarily-simple-ais-receiver-written-in-c/
 * code originally written for MS Visual Studio by Richard Gosiorovsky,
 * but slightly adapted to compile using clang on Linux.
 *
 * compile:
 *  $ gcc -Wall -Werror -o ESAR ESAR.c
 *
 * run:
 *  $ rtl_tcp -f 162e6 -s 300000 -a 127.0.0.1 -p 2345 -g 48.0
 *  $ ./ESAR
 */

/*
 * ESAR - Extraordinarily Simple AIS Receiver
 *
 * Copyright (c) 2021-2022 by Richard Gosiorovsky
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

// Reference: Recommendation ITU-R M.1371-5 (02/2014)

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

// =================================== AIS - decoder ===================================

int bits2int(char *bitstream, int from, int n)
{
	unsigned int r = 0;
	for (int i = from; i < from+n; i++)
	{
		r <<= 1;
		if (bitstream[i >> 3] & (1 << (7 - i & 7)))
		{
			r |= 1;
		}
	}
	return r;
}

char *bits2chars(char *r, char *bitstream, int from, int n)
{
	for (int i = 0; i < n / 6; i++)
	{
		r[i] = bits2int(bitstream, from + i * 6, 6);
		if (r[i] < 32)
		{
			r[i] += 64;
		}
	}
	r[n / 6] = 0;
	return r;
}

int sgn_lon(int x)
{	// extract sign -W +E
	return (x & (1 << 27)) ? (x - (1 << 28)) : x;
}
int sgn_lat(int x)
{	// extract sign -S +N
	return (x & (1 << 26)) ? (x - (1 << 27)) : x;
}

void parse_AIS_message(char *p)
{
	// Geographical position
	int lat, lon;

	int mmid = bits2int(p, 0, 6);
	printf(" %2d ", mmid);
	int mmsi = bits2int(p, 8, 30);
	printf(" %9d ", mmsi);

	switch (mmid)
	{
		case 1:
		case 2:
		case 3:	// Shipborne mobile equipment
			lon = sgn_lon(bits2int(p, 61, 28));
			lat = sgn_lat(bits2int(p, 89, 27));
			printf(" %11.6lf %11.6lf ",
				(double) lon / 600000,
				(double) lat / 600000
			);
			printf(" %3.0lf km/h   %5.1lf\n",
				0.1852 * bits2int(p, 50, 10),
				(double) bits2int(p, 116, 12) / 10
			);
		break;

		case 4:	// Base station
			lon = sgn_lon(bits2int(p,  79, 28));
			lat = sgn_lat(bits2int(p, 107, 27));
			printf(" %11.6lf %11.6lf ",
				(double) lon / 600000,
				(double) lat / 600000
			);
			printf(" %d/%d/%d ",
				bits2int(p, 38, 14),
				bits2int(p, 52, 4),
				bits2int(p, 56, 5)
			);	// date
			printf(" %02d:%02d:%02d \n",
				bits2int(p, 61, 5),
				bits2int(p, 66, 6),
				bits2int(p, 72, 6)
			);	// time
		break;

		case 5:	// Static and voyage related vessel data
			char csgn[16], name[32], dest[32];

			printf(" %s << %s >> %s\n",
				bits2chars(csgn, p, 70, 42),
				bits2chars(name, p, 112, 120),
				bits2chars(dest, p, 302, 120)
			);
		break;

		default:
			printf(" Unknown message ID\n");
		break;
	}
}

unsigned short crc16(char *buff, int n)  // Frame Check Sequence, CRC-16-CCITT (0xFFFF)
{
	unsigned short crc = 0xFFFF;
	for (int i = 0; i < n; i++)
	{
		unsigned char data = buff[i];
		data ^= crc & 0xff;
		data ^= data << 4;
		crc = ((data << 8) | (crc >> 8)) ^ (data >> 4) ^ (data << 3);
	}
	return ~crc;
}

#define PL 32  // HDLC synchronisation pattern legnth

int AIS_decode(int n, int rate, int *sA, int *sF, int i)
{
	int u, j, k = 0;

	// find 100 consecutive samples with amplitude >= 4
	for(; i < n; i++)
	{
		if (sA[i] < 4 * 4)
		{
			k = 0;
		}
		else if (++k >= 100)
		{
			break;
		}
	}

	i -= k;
	if (i > n-500)
	{
		return i;  // End of buffer
	}

	int pattern[PL] = {  1,1,0,0,1,1,0,0,  1,1,0,0,1,1,0,0,  1,1,0,0,1,1,0,0,  1,1,1,1,1,1,1,0  };	// NRZI
					 //  0 1 0 1 0 1 0 1   0 1 0 1 0 1 0 1   0 1 0 1 0 1 0 1   0 1 1 1 1 1 1 0		// preamble and 0x7E

	for (j = 0; j < PL; j++)
	{
		if (pattern[j] == 0)
		{
			pattern[j] =  1;
		}
		else
		{
			pattern[j] = -1;
		}
	}

	int smax = 0, imax = 0;
	double T = ((double)rate) / 9600.0;  // GMSK - 9600 Bd

	for (k = 0; k < 20 * T; k++)
	{	// find maximal correlation with pattern on interval <0, 20 * T> (i.e. synchronisation)
		int s = 0;
		for (j = 0; j < PL; j++)
		{
			int s0 = pattern[j] * sF[i + k + (int) (j * T + 0.5)];
			if (s0 < 0.0)
			{
				break;
			}
			s+=s0;
		}
		if (j == PL && s > smax)
		{
			smax = s;
			imax = k;
		}
	}

	if (smax == 0)
	{
		for (k = 0; k < 20 * T; k++)
		{	// try opposite polarisation
			int s = 0;
			for (j = 0; j < PL; j++)
			{
				int s0 = pattern[j] * sF[i + k + (int) (j * T + 0.5)];
				if (s0 > 0.0)
				{
					break;
				}
				s+=s0;
			}
			if (j == PL && s < smax)
			{
				smax = s;
				imax = k;
			}
		}
	}

	if (smax == 0)
	{	// HDLC Synch not found
		return i + 220 * T;
	}

	i += imax;	// move to the beginning of AIS frame

	u = k = 0;
	unsigned char msg[256];  msg[0] = 0;
	unsigned char out, old_bit = 99, bit;
	char o1,  o2,  o3,  o4,  o5;
		 o1 = o2 = o3 = o4 = o5 = 0;

	for (j = 0; j < (n - i) / T; j++)
	{	// HDLC decoding
		if (sA[i + (int) (j * T + 0.5)] < 2 * 2)
		{
			break;	// weak signal
		}

		bit = (sF[ i + (int) (j * T + 0.5)] > 0) ? 0 : 1;

		out = (bit != old_bit) ? 0 : 1;	// NRZI decoding (change = 0, no change = 1)
		old_bit = bit;

		// bit-stuffing: if 6 consecutive one's then zero is inserted because of flag 0x7E distinguishing => skip zero
		if (o1 == 1 && o2 == 1 && o3 == 1 && o4 == 1 && o5 == 1)
		{
			o1 = o2 = o3 = o4 = o5 = 0;
			if (out == 0)
			{
				continue;
			}
		}
		o5 = o4;
		o4 = o3;
		o3 = o2;
		o2 = o1;
		o1 = out;

		if (out == 1)
		{	// bits to byte (LSF)
			msg[u] |= 1 << k;
		}

		if (++k == 8)
		{
			k = 0;
			u++;
			msg[u] = 0;
		}
	}

	// CRC check :

	int msgID = bits2int(&msg[4], 0, 6);
	int msglen = (msgID == 5) ? 53 : 21;	// Message 5 is 424 bits long

	int crc0 = *((unsigned short *) & msg[msglen + 4]);
	int crc  = crc16(&msg[4], msglen);

	if (crc == crc0)
	{
		parse_AIS_message(&msg[4]);
	}

	return i + j * T;
}

// ================================= Tuning, Filtering, Demodulation =====================================

#define NIQ 300000  // 1 buf per second

#define FL 31  // FIR coeffs multiplied by factor 2^20
int h3[FL] = { 349525, 288373, 143167, 0, -69570, -54470, 0, 36711, 30962, 0, -22642, -19513, 0, 14571, 12587, 0, -9335, -7997, 0, 5785, 4877, 0, -3395, -2804, 0, 1878, 1532, 0, -1044, -891, 0 };  // 1/3 band
int h8[FL] = { 131072, 127428, 116895, 100620, 80332, 58108, 36092, 16222, 0, -11660, -18487, -20817, -19463, -15544, -10278, -4797, 0, 3534, 5569, 6171, 5631, 4356, 2772, 1239, 0, -830, -1251, -1339, -1205, -951, -648 };  // 1/8 band (6.25 kHz)

inline int fir_sample(int *x, int n, int h[]);
int fir_sample(int *x, int n, int h[])
{
	int s = h[0] * x[n - 1];
	for (int i = 1; i < n; i++)
	{
		s += h[i] * (x[n - i - 1] + x[n + i - 1]);
	}
	return s >> 19;
}

void proces_buff(int n, unsigned char *buff)
{
	int i, rate = 300000;
	static int I1[NIQ], Q1[NIQ], I2[NIQ / 3], Q2[NIQ / 3];

	for (i = 0; i < n; i++)
	{
		I1[i] = buff[2*i]   - 128;
		Q1[i] = buff[2*i+1] - 128;
	}

	n /=3;
	rate /= 3;	// originally intended for 100 kHz sampling rate, but RTL doesn't support it

	for (i = 0; i < n-10; i++)
	{	// third-sampling with anti-aliasing
		I1[i] = fir_sample(&I1[3 * i], FL, h3);
		Q1[i] = fir_sample(&Q1[3 * i], FL, h3);
	}

	for (i = 0; i < n; i += 4)
	{	// split I/Q stream into AIS channels 1 & 2
		// shift stream by 25 kHz (PI/2) to channel 2
		I2[i + 0] =  I1[i + 0];
		Q2[i + 0] =  Q1[i + 0];
		I2[i + 1] =  Q1[i + 1];
		Q2[i + 1] = -I1[i + 1];
		I2[i + 2] = -I1[i + 2];
		Q2[i + 2] = -Q1[i + 2];
		I2[i + 3] = -Q1[i + 3];
		Q2[i + 3] =  I1[i + 3];
		// shift it back by 50 kHz (PI) to channel 1
		I1[i + 1] = -I2[i + 1];
		Q1[i + 1] = -Q2[i + 1];
		I1[i + 2] =  I2[i + 2];
		Q1[i + 2] =  Q2[i + 2];
		I1[i + 3] = -I2[i + 3];
		Q1[i + 3] = -Q2[i + 3];
	}

	#define DCM 2	// works fine also with DCM 1, 3

	n /= DCM;
	rate /= DCM;

	for (i = 0; i < n-15; i++)
	{	// half-sampling with low-pass 6.25 kHz
		I1[i] = fir_sample(&I1[DCM * i], FL, h8);
		Q1[i] = fir_sample(&Q1[DCM * i], FL, h8);
		I2[i] = fir_sample(&I2[DCM * i], FL, h8);
		Q2[i] = fir_sample(&Q2[DCM * i], FL, h8);
	}

	for (i = 0; i < n-1; i++)
	{
		// FM demodulation
		Q1[i] = Q1[i + 1] * I1[i + 0] - Q1[i + 0] * I1[i + 1];
		Q2[i] = Q2[i + 1] * I2[i + 0] - Q2[i + 0] * I2[i + 1];
		// AM demodulation
		I1[i] = I1[i + 1] * I1[i + 1] + Q1[i + 1] * Q1[i + 1];
		I2[i] = I2[i + 1] * I2[i + 1] + Q2[i + 1] * Q2[i + 1];
	}

	i = 0;
	while (i < n-500)
	{	// Channel 1
		i = AIS_decode(n, rate, I1, Q1, i);
	}
	i = 0;
	while (i < n-500)
	{	// Channel 2
		i = AIS_decode(n, rate, I2, Q2, i);
	}

	fflush(stdout);
}

// =========================================== IQ data from socket ==========================================

#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <unistd.h>

int tcp_recv(char *host, char *port)
{
	int sock = 0, client_fd;
	struct addrinfo *result = NULL, *ptr = NULL, hints;

	memset (&hints, 0, sizeof (hints));
	hints.ai_family = AF_INET;
	hints.ai_socktype = SOCK_STREAM;
	hints.ai_protocol = IPPROTO_TCP;

	if (getaddrinfo(host, port, &hints, &result) != 0)
	{
		return 2;
	}

	for (ptr = result; ptr != NULL; ptr = ptr->ai_next)
	{	// Attempt to connect to an address until one succeeds
		if ((sock = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol)) < 0)
		{
			printf("Socket creation error\n");
			return 3;
		}
		if ((client_fd = connect(sock, ptr->ai_addr, (int)ptr->ai_addrlen)) < 0)
		{
			close(client_fd);
			printf("Connection Failed\nDid you run\n$ rtl_tcp -f 162e6 -s 300000 -a 127.0.0.1 -p 2345 -g 48.0\n");
			continue;
		}
		break;
	}

	int n;
	static unsigned char buff[2 * NIQ];
	if ((n = read(sock, buff, 2*NIQ)) > 0)
	{	// initial packet
		printf("\n === (%d bytes) %s === \n\n", n, buff);
	}
	printf(" MID    MMSI      longitude   latitude     speed    course   \n");
	printf("-------------------------------------------------------------\n");
	while((n = read(sock, buff, 2 * NIQ, MSG_WAITALL)) > 0)
	{
		proces_buff(n / 2, buff);
	}

	close(client_fd);
	return n;
}

int main()
{	// before start:  run rtl_tcp.exe -f 162e6 -s 300000 -a 127.0.0.1 -p 2345 -g 48.0
	int r = tcp_recv("127.0.0.1", "2345");
	printf("\n status = %d \n", r);
	return 0;
}