From d12c923594ef80f896b4b07db6b51266919498b3 Mon Sep 17 00:00:00 2001 From: Mikael Nousiainen Date: Wed, 11 Aug 2021 23:24:30 +0300 Subject: [PATCH] Add support for Horus 4FSK modulation. Increase APRS preamble size to improve reception. Fix slowness in I2C initialization. Fix incorrect GPS settings for airborne usage. Additional minor code clean-up and refactoring. --- README.md | 14 +- src/codecs/aprs/aprs.c | 4 +- src/codecs/bell/bell.h | 2 +- src/codecs/horus/horus_l2.c | 1183 ++++++++++++++++++++++++++ src/codecs/horus/horus_l2.h | 23 + src/codecs/horus/horus_packet_v1.c | 75 ++ src/codecs/horus/horus_packet_v1.h | 32 + src/codecs/jtencode/jtencode.cpp | 1 - src/codecs/jtencode/lib/JTEncode.cpp | 18 +- src/codecs/mfsk/mfsk.c | 188 ++++ src/codecs/mfsk/mfsk.h | 26 + src/config.c | 28 +- src/config.h | 48 +- src/drivers/si4032/si4032.c | 7 +- src/drivers/si4032/si4032.h | 1 + src/drivers/ubxg6010/ubxg6010.c | 140 +-- src/gps.h | 9 + src/hal/datatimer.c | 67 ++ src/hal/datatimer.h | 12 + src/hal/i2c.c | 8 +- src/hal/pwm.c | 5 + src/hal/pwm.h | 1 + src/log.c | 20 + src/log.h | 3 + src/main.c | 4 + src/radio.c | 95 ++- src/radio.h | 1 + src/radio_internal.h | 2 + src/radio_payload_aprs.c | 4 +- src/radio_payload_horus_v1.c | 39 + src/radio_payload_horus_v1.h | 8 + src/radio_si4032.c | 60 +- src/radio_si4032.h | 1 + src/telemetry.h | 2 +- src/template.c | 20 +- 35 files changed, 2016 insertions(+), 135 deletions(-) create mode 100644 src/codecs/horus/horus_l2.c create mode 100644 src/codecs/horus/horus_l2.h create mode 100644 src/codecs/horus/horus_packet_v1.c create mode 100644 src/codecs/horus/horus_packet_v1.h create mode 100644 src/codecs/mfsk/mfsk.c create mode 100644 src/codecs/mfsk/mfsk.h create mode 100644 src/hal/datatimer.c create mode 100644 src/hal/datatimer.h create mode 100644 src/log.c create mode 100644 src/radio_payload_horus_v1.c create mode 100644 src/radio_payload_horus_v1.h diff --git a/README.md b/README.md index 481b34f..e15e69f 100644 --- a/README.md +++ b/README.md @@ -4,15 +4,19 @@ This is a custom, amateur radio-oriented firmware for Vaisala RS41 radiosondes. Some of the code is based on an earlier RS41 firmware project called [RS41HUP](https://github.com/df8oe/RS41HUP), -but most of it has been rewritten from scratch. +but most of it has been rewritten from scratch. The Horus 4FSK code has been adapted from +the [darksidelemm fork of RS41HUP](https://github.com/darksidelemm/RS41HUP). The motivation to develop this firmware is to provide cleaner, customizable and more modular codebase for developing RS41 radiosonde-based experiments. The main features this firmware aims to implement are: -* Enhanced support for the internal Si4032 radio transmitter via PWM-based tone generation (and ultimately DMA-based symbol timing, if possible) +* Support for transmitting multiple modes consecutively with custom, rotating comment messages +* Support for standard 1200-baud APRS +* Support for [Horus 4FSK mode](https://github.com/projecthorus/horusdemodlib/wiki) that has improved performance compared to APRS or RTTY * Support for additional digital modes on HF/VHF amateur radio bands using an external Si5351 clock generator connected to the external I²C bus * Support for custom sensors via the external I²C bus +* Enhanced support for the internal Si4032 radio transmitter via PWM-based tone generation (and ultimately DMA-based symbol timing, if possible) * Extensibility to allow easy addition of new digital modes ## Features @@ -20,6 +24,11 @@ The main features this firmware aims to implement are: * APRS on 70cm amateur radio band using the internal Si4032 radio transmitter * Bell 202 frequencies are generated via hardware PWM, but the symbol timing is created in a loop with delay * There is also code available to use DMA transfers for symbol timing to achieve greater accuracy, but I have not been able to get the timings working correctly +* Horus 4FSK on 70cm amateur radio band using the internal Si4032 radio transmitter + * The Horus 4FSK mode has significantly [improved performance compared to APRS or RTTY](https://github.com/projecthorus/horusdemodlib/wiki) + * Use [horus-gui](https://github.com/projecthorus/horus-gui) software to receive the 4FSK mode and to submit packets to [Habhub](http://habhub.org/) high-altitude balloon tracking platform + * See [horus-gui installation and usage instructions](https://github.com/projecthorus/horusdemodlib/wiki/1.1-Horus-GUI-Reception-Guide-(Windows-Linux-OSX)) + * Based on [horusdemodlib](https://github.com/projecthorus/horusdemodlib) library that is responsible for demodulating the signal * Digital mode beacons on HF/VHF frequencies using a Si5351 clock generator connected to the external I²C bus of the RS41 radiosonde * The JTEncode library provides JT65/JT9/JT4/FT8/WSPR/FSQ beacon transmissions. I've decoded FT8 and WSPR successfully. * GPS-based scheduling is available for modes that require specific timing for transmissions @@ -225,4 +234,5 @@ rtl_fm -f 432500000 -M fm -s 250k -r 48000 -g 22 - | ./aprs - # Authors * Original codebase: DF8OE and other authors of the [RS41HUP](https://github.com/df8oe/RS41HUP) project +* Horus 4FSK code adapted from [darksidelemm fork of RS41HUP](https://github.com/darksidelemm/RS41HUP) project * Mikael Nousiainen OH3BHX diff --git a/src/codecs/aprs/aprs.c b/src/codecs/aprs/aprs.c index 6de78de..c3e5b43 100644 --- a/src/codecs/aprs/aprs.c +++ b/src/codecs/aprs/aprs.c @@ -64,10 +64,10 @@ size_t aprs_generate_position(uint8_t *payload, size_t length, telemetry_data *d symbol, heading_degrees, ground_speed_knots, - altitude_feet, + (int) altitude_feet, aprs_packet_counter, data->gps.satellites_visible, - data->internal_temperature_celsius_100 / 100, + (int) data->internal_temperature_celsius_100 / 100, data->battery_voltage_millivolts, (int16_t) ((float) data->gps.climb_cm_per_second / 100.0f), comment diff --git a/src/codecs/bell/bell.h b/src/codecs/bell/bell.h index 0abbb4a..6eabd72 100644 --- a/src/codecs/bell/bell.h +++ b/src/codecs/bell/bell.h @@ -6,7 +6,7 @@ #include "codecs/fsk/fsk.h" -#define BELL_FLAG_FIELD_COUNT_1200 25 +#define BELL_FLAG_FIELD_COUNT_1200 45 #define BELL_FLAG_FIELD_COUNT_300 45 void bell_encoder_new(fsk_encoder *encoder, uint32_t symbol_rate, uint16_t flag_field_count, fsk_tone *tones); diff --git a/src/codecs/horus/horus_l2.c b/src/codecs/horus/horus_l2.c new file mode 100644 index 0000000..ba5e4f2 --- /dev/null +++ b/src/codecs/horus/horus_l2.c @@ -0,0 +1,1183 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: horus_l2.c + AUTHOR......: David Rowe + DATE CREATED: Dec 2015 + + Horus telemetry layer 2 processing. Takes an array of 8 bit payload + data, generates parity bits for a (23,12) Golay code, interleaves + data and parity bits, pre-pends a Unique Word for modem sync. + Caller is responsible for providing storage for output packet. + + [ ] code based interleaver + [ ] test correction of 1,2 & 3 error patterms + + 1/ Unit test on a PC: + + $ gcc horus_l2.c -o horus_l2 -Wall -DHORUS_L2_UNITTEST + $ ./horus_l2 + + test 0: 22 bytes of payload data BER: 0.00 errors: 0 + test 0: 22 bytes of payload data BER: 0.01 errors: 0 + test 0: 22 bytes of payload data BER: 0.05 errors: 0 + test 0: 22 bytes of payload data BER: 0.10 errors: 7 + + This indicates it's correcting all channel errors for 22 bytes of + payload data, at bit error rate (BER) of 0, 0.01, 0.05. It falls + over at a BER of 0.10 which is expected. + + 2/ To build with just the tx function, ie for linking with the payload + firmware: + + $ gcc horus_l2.c -c -Wall + + By default the RX side is #ifdef-ed out, leaving the minimal amount + of code for tx. + + 3/ Generate some tx_bits as input for testing with fsk_horus: + + $ gcc horus_l2.c -o horus_l2 -Wall -DGEN_TX_BITS -DSCRAMBLER + $ ./horus_l2 + $ more ../octave/horus_tx_bits_binary.txt + + 4/ Unit testing interleaver: + + $ gcc horus_l2.c -o horus_l2 -Wall -DINTERLEAVER -DTEST_INTERLEAVER -DSCRAMBLER + + 5/ Compile for use as decoder called by fsk_horus.m and fsk_horus_stream.m: + + $ gcc horus_l2.c -o horus_l2 -Wall -DDEC_RX_BITS -DHORUS_L2_RX + +\*---------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include "horus_l2.h" + +#ifdef HORUS_L2_UNITTEST +#define HORUS_L2_RX +#endif + +#define RUN_TIME_TABLES + +#define INTERLEAVER +#define SCRAMBLER + +static char uw[] = {'$','$'}; + +/* Function Prototypes ------------------------------------------------*/ + +int32_t get_syndrome(int32_t pattern); +void golay23_init(void); +int golay23_decode(int received_codeword); +unsigned short gen_crc16(unsigned char* data_p, unsigned char length); +void interleave(unsigned char *inout, int nbytes, int dir); +void scramble(unsigned char *inout, int nbytes); + +/* Functions ----------------------------------------------------------*/ + +/* + We are using a Golay (23,12) code which has a codeword 23 bits + long. The tx packet format is: + + | Unique Word | payload data bits | parity bits | + + This function works out how much storage the caller of + horus_l2_encode_tx_packet() will need to store the tx packet + */ + +int horus_l2_get_num_tx_data_bytes(int num_payload_data_bytes) { + int num_payload_data_bits, num_golay_codewords; + int num_tx_data_bits, num_tx_data_bytes; + + num_payload_data_bits = num_payload_data_bytes*8; + num_golay_codewords = num_payload_data_bits/12; + if (num_payload_data_bits % 12) /* round up to 12 bits, may mean some unused bits */ + num_golay_codewords++; + + num_tx_data_bits = sizeof(uw)*8 + num_payload_data_bits + num_golay_codewords*11; + num_tx_data_bytes = num_tx_data_bits/8; + if (num_tx_data_bits % 8) /* round up to nearest byte, may mean some unused bits */ + num_tx_data_bytes++; + + #ifdef DEBUG0 + fprintf(stderr, "\nnum_payload_data_bytes: %d\n", num_payload_data_bytes); + fprintf(stderr, "num_golay_codewords...: %d\n", num_golay_codewords); + fprintf(stderr, "num_tx_data_bits......: %d\n", num_tx_data_bits); + fprintf(stderr, "num_tx_data_bytes.....: %d\n\n", num_tx_data_bytes); + #endif + + return num_tx_data_bytes; +} + + +/* + Takes an array of payload data bytes, prepends a unique word and appends + parity bits. + + The encoder will run on the payload on a small 8-bit uC. As we are + memory constrained so we do a lot of burrowing for bits out of + packed arrays, and don't use a LUT for Golay encoding. Hopefully it + will run fast enough. This was quite difficult to get going, + suspect there is a better way to write this. Oh well, have to start + somewhere. + */ + +int horus_l2_encode_tx_packet(unsigned char *output_tx_data, + unsigned char *input_payload_data, + int num_payload_data_bytes) +{ + int num_tx_data_bytes, num_payload_data_bits; + unsigned char *pout = output_tx_data; + int ninbit, ningolay, nparitybits; + int32_t ingolay, paritybyte, inbit, golayparity; + int ninbyte, shift, golayparitybit, i; + + num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(num_payload_data_bytes); + memcpy(pout, uw, sizeof(uw)); pout += sizeof(uw); + memcpy(pout, input_payload_data, num_payload_data_bytes); pout += num_payload_data_bytes; + + /* Read input bits one at a time. Fill input Golay codeword. Find output Golay codeword. + Write this to parity bits. Write parity bytes when we have 8 parity bits. Bits are + written MSB first. */ + + num_payload_data_bits = num_payload_data_bytes*8; + ninbit = 0; + ingolay = 0; + ningolay = 0; + paritybyte = 0; + nparitybits = 0; + + while (ninbit < num_payload_data_bits) { + + /* extract input data bit */ + + ninbyte = ninbit/8; + shift = 7 - (ninbit % 8); + inbit = (input_payload_data[ninbyte] >> shift) & 0x1; + #ifdef DEBUG1 + fprintf(stderr, "inbit %d ninbyte: %d inbyte: 0x%02x inbit: %d\n", + ninbit, ninbyte, input_payload_data[ninbyte], inbit); + #endif + ninbit++; + + /* build up input golay codeword */ + + ingolay = ingolay | inbit; + ningolay++; + + /* when we get 12 bits do a Golay encode */ + + if (ningolay % 12) { + ingolay <<= 1; + } + else { + #ifdef DEBUG0 + fprintf(stderr, " ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay); + #endif + golayparity = get_syndrome(ingolay<<11); + ingolay = 0; + + #ifdef DEBUG0 + fprintf(stderr, " golayparity: 0x%04x\n", golayparity); + #endif + + /* write parity bits to output data */ + + for (i=0; i<11; i++) { + golayparitybit = (golayparity >> (10-i)) & 0x1; + paritybyte = paritybyte | golayparitybit; + #ifdef DEBUG0 + fprintf(stderr, " i: %d golayparitybit: %d paritybyte: 0x%02x\n", + i, golayparitybit, paritybyte); + #endif + nparitybits++; + if (nparitybits % 8) { + paritybyte <<= 1; + } + else { + /* OK we have a full byte ready */ + *pout = paritybyte; + #ifdef DEBUG0 + fprintf(stderr," Write paritybyte: 0x%02x\n", paritybyte); + #endif + pout++; + paritybyte = 0; + } + } + } + } /* while(.... */ + + + /* Complete final Golay encode, we may have partially finished ingolay, paritybyte */ + + #ifdef DEBUG0 + fprintf(stderr, "finishing up .....\n"); + #endif + + if (ningolay % 12) { + ingolay >>= 1; + golayparity = get_syndrome(ingolay<<12); + #ifdef DEBUG0 + fprintf(stderr, " ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay); + fprintf(stderr, " golayparity: 0x%04x\n", golayparity); + #endif + + /* write parity bits to output data */ + + for (i=0; i<11; i++) { + golayparitybit = (golayparity >> (10 - i)) & 0x1; + paritybyte = paritybyte | golayparitybit; + #ifdef DEBUG1 + fprintf(stderr, " i: %d golayparitybit: %d paritybyte: 0x%02x\n", + i, golayparitybit, paritybyte); + #endif + nparitybits++; + if (nparitybits % 8) { + paritybyte <<= 1; + } + else { + /* OK we have a full byte ready */ + *pout++ = (unsigned char)paritybyte; + #ifdef DEBUG0 + fprintf(stderr," Write paritybyte: 0x%02x\n", paritybyte); + #endif + paritybyte = 0; + } + } + } + + /* and final, partially complete, parity byte */ + + if (nparitybits % 8) { + paritybyte <<= 7 - (nparitybits % 8); // use MS bits first + *pout++ = (unsigned char)paritybyte; + #ifdef DEBUG0 + fprintf(stderr," Write last paritybyte: 0x%02x nparitybits: %d \n", paritybyte, nparitybits); + #endif + } + + #ifdef DEBUG0 + fprintf(stderr, "\npout - output_tx_data: %ld num_tx_data_bytes: %d\n", + pout - output_tx_data, num_tx_data_bytes); + #endif + assert(pout == (output_tx_data + num_tx_data_bytes)); + + /* optional interleaver - we dont interleave UW */ + + #ifdef INTERLEAVER + interleave(&output_tx_data[sizeof(uw)], num_tx_data_bytes-2, 0); + #endif + + /* optional scrambler to prevent long strings of the same symbol + which upsets the modem - we dont scramble UW */ + + #ifdef SCRAMBLER + scramble(&output_tx_data[sizeof(uw)], num_tx_data_bytes-2); + #endif + + return num_tx_data_bytes; +} + + +#ifdef HORUS_L2_RX +void horus_l2_decode_rx_packet(unsigned char *output_payload_data, + unsigned char *input_rx_data, + int num_payload_data_bytes) +{ + int num_payload_data_bits; + unsigned char *pout = output_payload_data; + unsigned char *pin = input_rx_data; + int ninbit, ingolay, ningolay, paritybyte, nparitybits; + int ninbyte, shift, inbit, golayparitybit, i, outbit, outbyte, noutbits, outdata; + int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(num_payload_data_bytes); + + /* optional scrambler and interleaver - we dont interleave UW */ + + #ifdef SCRAMBLER + scramble(&input_rx_data[sizeof(uw)], num_tx_data_bytes-2); + #endif + + #ifdef INTERLEAVER + interleave(&input_rx_data[sizeof(uw)], num_tx_data_bytes-2, 1); + #endif + + pin = input_rx_data + sizeof(uw) + num_payload_data_bytes; + + /* Read input data bits one at a time. When we have 12 read 11 parity bits. Golay decode. + Write decoded (output data) bits every time we have 8 of them. */ + + num_payload_data_bits = num_payload_data_bytes*8; + ninbit = 0; + ingolay = 0; + ningolay = 0; + nparitybits = 0; + paritybyte = *pin++; + #ifdef DEBUG0 + fprintf(stderr," Read paritybyte: 0x%02x\n", paritybyte); + #endif + pout = output_payload_data; + noutbits = 0; + outbyte = 0; + + while (ninbit < num_payload_data_bits) { + + /* extract input data bit */ + + ninbyte = ninbit/8 + sizeof(uw); + shift = 7 - (ninbit % 8); + inbit = (input_rx_data[ninbyte] >> shift) & 0x1; + #ifdef DEBUG1 + fprintf(stderr, "inbit %d ninbyte: %d inbyte: 0x%02x inbit: %d\n", + ninbit, ninbyte, input_rx_data[ninbyte], inbit); + #endif + ninbit++; + + /* build up golay codeword */ + + ingolay = ingolay | inbit; + ningolay++; + ingolay <<= 1; + + /* when we get 12 data bits start reading parity bits */ + + if ((ningolay % 12) == 0) { + #ifdef DEBUG0 + fprintf(stderr, " ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay>>1); + #endif + for (i=0; i<11; i++) { + shift = 7 - (nparitybits % 8); + golayparitybit = (paritybyte >> shift) & 0x1; + ingolay |= golayparitybit; + if (i != 10) + ingolay <<=1; + nparitybits++; + if ((nparitybits % 8) == 0) { + /* OK grab a new byte */ + paritybyte = *pin++; + #ifdef DEBUG0 + fprintf(stderr," Read paritybyte: 0x%02x\n", paritybyte); + #endif + } + } + + #ifdef DEBUG0 + fprintf(stderr, " golay code word: 0x%04x\n", ingolay); + fprintf(stderr, " golay decode...: 0x%04x\n", golay23_decode(ingolay)); + #endif + + /* write decoded/error corrected bits to output payload data */ + + outdata = golay23_decode(ingolay) >> 11; + #ifdef DEBUG0 + fprintf(stderr, " outdata...: 0x%04x\n", outdata); + #endif + + for(i=0; i<12; i++) { + shift = 11 - i; + outbit = (outdata >> shift) & 0x1; + outbyte |= outbit; + noutbits++; + if (noutbits % 8) { + outbyte <<= 1; + } + else { + #ifdef DEBUG0 + fprintf(stderr, " output payload byte: 0x%02x\n", outbyte); + #endif + *pout++ = outbyte; + outbyte = 0; + } + } + + ingolay = 0; + } + } /* while(.... */ + + + #ifdef DEBUG0 + fprintf(stderr, "finishing up .....\n"); + #endif + + /* Complete final Golay decode */ + + int golayparity = 0; + if (ningolay % 12) { + for (i=0; i<11; i++) { + shift = 7 - (nparitybits % 8); + golayparitybit = (paritybyte >> shift) & 0x1; + golayparity |= golayparitybit; + if (i != 10) + golayparity <<=1; + nparitybits++; + if ((nparitybits % 8) == 0) { + /* OK grab a new byte */ + paritybyte = *pin++; + #ifdef DEBUG0 + fprintf(stderr," Read paritybyte: 0x%02x\n", paritybyte); + #endif + } + } + + ingolay >>= 1; + int codeword = (ingolay<<12) + golayparity; + #ifdef DEBUG0 + fprintf(stderr, " ningolay: %d ingolay: 0x%04x\n", ningolay, ingolay); + fprintf(stderr, " golay code word: 0x%04x\n", codeword); + fprintf(stderr, " golay decode...: 0x%04x\n", golay23_decode(codeword)); + #endif + + outdata = golay23_decode(codeword) >> 11; + #ifdef DEBUG0 + fprintf(stderr, " outdata...: 0x%04x\n", outdata); + fprintf(stderr, " num_payload_data_bits: %d noutbits: %d\n", num_payload_data_bits, noutbits); + #endif + + /* write final byte */ + + int ntogo = num_payload_data_bits - noutbits; + for(i=0; i> shift) & 0x1; + outbyte |= outbit; + noutbits++; + if (noutbits % 8) { + outbyte <<= 1; + } + else { + #ifdef DEBUG0 + fprintf(stderr, " output payload byte: 0x%02x\n", outbyte); + #endif + *pout++ = outbyte; + outbyte = 0; + } + } + } + + #ifdef DEBUG0 + fprintf(stderr, "\npin - output_payload_data: %ld num_payload_data_bytes: %d\n", + pout - output_payload_data, num_payload_data_bytes); + #endif + + assert(pout == (output_payload_data + num_payload_data_bytes)); + +} +#endif + +#ifdef INTERLEAVER + +uint16_t primes[] = { + 2, 3, 5, 7, 11, 13, 17, 19, 23, 29, + 31, 37, 41, 43, 47, 53, 59, 61, 67, 71, + 73, 79, 83, 89, 97, 101, 103, 107, 109, 113, + 127, 131, 137, 139, 149, 151, 157, 163, 167, 173, + 179, 181, 191, 193, 197, 199, 211, 223, 227, 229, + 233, 239, 241, 251, 257, 263, 269, 271, 277, 281, + 283, 293, 307, 311, 313, 317, 331, 337, 347 +}; + +void interleave(unsigned char *inout, int nbytes, int dir) +{ + uint16_t nbits = (uint16_t)nbytes*8; + uint32_t i, j, n, ibit, ibyte, ishift, jbyte, jshift; + uint32_t b; + unsigned char out[nbytes]; + + + memset(out, 0, nbytes); + + /* b chosen to be co-prime with nbits, I'm cheating by just finding the + nearest prime to nbits. It also uses storage, is run on every call, + and has an upper limit. Oh Well, still seems to interleave OK. */ + i = 1; + uint16_t imax = sizeof(primes)/sizeof(uint16_t); + while ((primes[i] < nbits) && (i < imax)) + i++; + b = primes[i-1]; + + for(n=0; n> ishift) & 0x1; + + jbyte = j/8; + jshift = j%8; + + /* write jbit to ibit position */ + + out[jbyte] |= ibit << jshift; // replace with i-th bit + //out[ibyte] |= ibit << ishift; // replace with i-th bit + } + + memcpy(inout, out, nbytes); + + #ifdef DEBUG0 + printf("\nInterleaver Out:\n"); + for (i=0; i> 1) ^ (scrambler & 0x1); + + /* modify i-th bit by xor-ing with scrambler output sequence */ + + ibyte = i/8; + ishift = i%8; + ibit = (inout[ibyte] >> ishift) & 0x1; + ibits = ibit ^ scrambler_out; // xor ibit with scrambler output + + mask = 1 << ishift; + inout[ibyte] &= ~mask; // clear i-th bit + inout[ibyte] |= ibits << ishift; // set to scrambled value + + /* update scrambler */ + + scrambler >>= 1; + scrambler |= scrambler_out << 14; + + #ifdef DEBUG0 + printf("i: %02d ibyte: %d ishift: %d ibit: %d ibits: %d scrambler_out: %d\n", + i, ibyte, ishift, ibit, ibits, scrambler_out); + #endif + + } + + #ifdef DEBUG0 + printf("\nScrambler Out:\n"); + for (i=0; i>b) & 0x1; + } + + return nerr; +} + +/* unit test designed to run on a PC */ + +/* Horus binary packet */ + +struct horus_packet_v1 +{ + uint8_t PayloadID; + uint16_t Counter; + uint8_t Hours; + uint8_t Minutes; + uint8_t Seconds; + float Latitude; + float Longitude; + uint16_t Altitude; + uint8_t Speed; // Speed in Knots (1-255 knots) + uint8_t Sats; + int8_t Temp; // Twos Complement Temp value. + uint8_t BattVoltage; // 0 = 0.5v, 255 = 2.0V, linear steps in-between. + uint16_t Checksum; // CRC16-CCITT Checksum. +} __attribute__ ((packed)); + +int main(void) { + int nbytes = sizeof(struct horus_packet_v1); + printf("test 0: BER: 0.00 ...........: %d\n", test_sending_bytes(nbytes, 0.00, 0)); + printf("test 1: BER: 0.01 ...........: %d\n", test_sending_bytes(nbytes, 0.01, 0)); + printf("test 2: BER: 0.05 ...........: %d\n", test_sending_bytes(nbytes, 0.05, 0)); + + /* we expect this always to fail, as chance of > 3 errors/codeword is high */ + + printf("test 3: BER: 0.10 ...........: %d\n", test_sending_bytes(nbytes, 0.10, 0)); + + /* -DINTERLEAVER will make this puppy pass */ + + printf("test 4: 8 bit burst error....: %d\n", test_sending_bytes(nbytes, 0.00, 1)); + + /* Insert 2 errors in every codeword, the maximum correction + capability of a Golay (23,12) code. note this one will fail + with -DINTERLEAVER, as we can't guarantee <= 3 errors per + codeword after interleaving */ + + printf("test 5: 1 error every 12 bits: %d\n", test_sending_bytes(nbytes, 0.00, 2)); + return 0; +} +#endif + + + +#ifdef GEN_TX_BITS +/* generate a file of tx_bits to modulate using fsk_horus.m for modem simulations */ + +int main(void) { + int nbytes = sizeof(struct horus_packet_v1); + struct horus_packet_v1 input_payload; + int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(nbytes); + unsigned char tx[num_tx_data_bytes]; + int i; + + /* all zeros is nastiest sequence for demod before scrambling */ + + memset(&input_payload, 0, nbytes); + input_payload.Checksum = gen_crc16((unsigned char*)&input_payload, nbytes-2); + + horus_l2_encode_tx_packet(tx, (unsigned char*)&input_payload, nbytes); + + FILE *f = fopen("../octave/horus_tx_bits_binary.txt","wt"); + assert(f != NULL); + int b, tx_bit; + for(i=0; i> (7-b)) & 0x1; /* msb first */ + fprintf(f,"%d ", tx_bit); + } + } + fclose(f); + + return 0; +} +#endif + + +#ifdef DEC_RX_BITS + +/* Decode a binary file rx_bytes, e.g. from fsk_horus.m */ + +int main(void) { + int nbytes = 22; + unsigned char output_payload[nbytes]; + int num_tx_data_bytes = horus_l2_get_num_tx_data_bytes(nbytes); + + /* real world data horus payload generated when running tx above */ + unsigned char rx[45] = { + 0x24,0x24,0x01,0x0b,0x00,0x00,0x05,0x3b,0xf2,0xa7,0x0b,0xc2,0x1b, + 0xaa,0x0a,0x43,0x7e,0x00,0x05,0x00,0x25,0xc0,0xce,0xbb,0x36,0x69, + 0x50,0x00,0x41,0xb0,0xa6,0x5e,0x91,0xa2,0xa3,0xf8,0x1d,0x00,0x00, + 0x0c,0x76,0xc6,0x05,0xb0,0xb8}; + int i, ret; + + assert(num_tx_data_bytes == 45); + + #define READ_FILE /* overwrite tx[] above, that's OK */ + #ifdef READ_FILE + FILE *f = fopen("../octave/horus_rx_bits_binary.bin","rb"); + assert(f != NULL); + ret = fread(rx, sizeof(char), num_tx_data_bytes, f); + assert(ret == num_tx_data_bytes); + fclose(f); + #endif + + golay23_init(); + horus_l2_decode_rx_packet(output_payload, rx, nbytes); + + #ifdef HEX_DUMP + fprintf(stderr, "\nOutput Payload:\n"); + for(i=0; i +#include +#include +#define X22 0x00400000 /* vector representation of X^{22} */ +#define X11 0x00000800 /* vector representation of X^{11} */ +#define MASK12 0xfffff800 /* auxiliary vector for testing */ +#define GENPOL 0x00000c75 /* generator polinomial, g(x) */ + +/* Global variables: + * + * pattern = error pattern, or information, or received vector + * encoding_table[] = encoding table + * decoding_table[] = decoding table + * data = information bits, i(x) + * codeword = code bits = x^{11}i(x) + (x^{11}i(x) mod g(x)) + * numerr = number of errors = Hamming weight of error polynomial e(x) + * position[] = error positions in the vector representation of e(x) + * recd = representation of corrupted received polynomial r(x) = c(x) + e(x) + * decerror = number of decoding errors + * a[] = auxiliary array to generate correctable error patterns + */ + +#ifdef HORUS_L2_RX +static int inited = 0; + +#ifdef RUN_TIME_TABLES +static int encoding_table[4096], decoding_table[2048]; +#else +#include "golayenctable.h" +#include "golaydectable.h" +#endif + +#ifdef RUN_TIME_TABLES +static int arr2int(int a[], int r) +/* + * Convert a binary vector of Hamming weight r, and nonzero positions in + * array a[1]...a[r], to a long integer \sum_{i=1}^r 2^{a[i]-1}. + */ +{ + int i; + long mul, result = 0, temp; + + for (i=1; i<=r; i++) { + mul = 1; + temp = a[i]-1; + while (temp--) + mul = mul << 1; + result += mul; + } + return(result); +} +#endif +#endif + +#ifdef HORUS_L2_RX +void nextcomb(int n, int r, int a[]) +/* + * Calculate next r-combination of an n-set. + */ +{ + int i, j; + + a[r]++; + if (a[r] <= n) + return; + j = r - 1; + while (a[j] == n - r + j) + j--; + for (i = r; i >= j; i--) + a[i] = a[j] + i - j + 1; + return; +} +#endif + +int32_t get_syndrome(int32_t pattern) +/* + * Compute the syndrome corresponding to the given pattern, i.e., the + * remainder after dividing the pattern (when considering it as the vector + * representation of a polynomial) by the generator polynomial, GENPOL. + * In the program this pattern has several meanings: (1) pattern = infomation + * bits, when constructing the encoding table; (2) pattern = error pattern, + * when constructing the decoding table; and (3) pattern = received vector, to + * obtain its syndrome in decoding. + */ +{ + int32_t aux = X22; + + if (pattern >= X11) + while (pattern & MASK12) { + while (!(aux & pattern)) + aux = aux >> 1; + pattern ^= (aux/X11) * GENPOL; + } + return(pattern); +} + +#ifdef HORUS_L2_RX + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: golay23_init() + AUTHOR......: David Rowe + DATE CREATED: 3 March 2013 + + Call this once when you start your program to init the Golay tables. + +\*---------------------------------------------------------------------------*/ + +void golay23_init(void) { +#ifdef RUN_TIME_TABLES + int i; + long temp; + int a[4]; + int pattern; + + /* + * --------------------------------------------------------------------- + * Generate ENCODING TABLE + * + * An entry to the table is an information vector, a 32-bit integer, + * whose 12 least significant positions are the information bits. The + * resulting value is a codeword in the (23,12,7) Golay code: A 32-bit + * integer whose 23 least significant bits are coded bits: Of these, the + * 12 most significant bits are information bits and the 11 least + * significant bits are redundant bits (systematic encoding). + * --------------------------------------------------------------------- + */ + for (pattern = 0; pattern < 4096; pattern++) { + temp = pattern << 11; /* multiply information by X^{11} */ + encoding_table[pattern] = temp + get_syndrome(temp);/* add redundancy */ + } + + /* + * --------------------------------------------------------------------- + * Generate DECODING TABLE + * + * An entry to the decoding table is a syndrome and the resulting value + * is the most likely error pattern. First an error pattern is generated. + * Then its syndrome is calculated and used as a pointer to the table + * where the error pattern value is stored. + * --------------------------------------------------------------------- + * + * (1) Error patterns of WEIGHT 1 (SINGLE ERRORS) + */ + decoding_table[0] = 0; + decoding_table[1] = 1; + temp = 1; + for (i=2; i<= 23; i++) { + temp *= 2; + decoding_table[get_syndrome(temp)] = temp; + } + /* + * (2) Error patterns of WEIGHT 2 (DOUBLE ERRORS) + */ + a[1] = 1; a[2] = 2; + temp = arr2int(a,2); + decoding_table[get_syndrome(temp)] = temp; + for (i=1; i<253; i++) { + nextcomb(23,2,a); + temp = arr2int(a,2); + decoding_table[get_syndrome(temp)] = temp; + } + /* + * (3) Error patterns of WEIGHT 3 (TRIPLE ERRORS) + */ + a[1] = 1; a[2] = 2; a[3] = 3; + temp = arr2int(a,3); + decoding_table[get_syndrome(temp)] = temp; + for (i=1; i<1771; i++) { + nextcomb(23,3,a); + temp = arr2int(a,3); + decoding_table[get_syndrome(temp)] = temp; + } +#endif + inited = 1; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: golay23_encode() + AUTHOR......: David Rowe + DATE CREATED: 3 March 2013 + + Given 12 bits of data retiurns a 23 bit codeword for transmission + over the channel. + +\*---------------------------------------------------------------------------*/ + +int golay23_encode(int data) { + assert(inited); + assert(data <= 0xfff); + + //printf("data: 0x%x\n", data); + return encoding_table[data]; +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: golay23_decode() + AUTHOR......: David Rowe + DATE CREATED: 3 March 2013 + + Given a 23 bit received codeword, returns the 12 bit corrected data. + +\*---------------------------------------------------------------------------*/ + +int golay23_decode(int received_codeword) { + assert(inited); + assert((received_codeword < (1<<23)) && (received_codeword >= 0)); + + //printf("syndrome: 0x%x\n", get_syndrome(received_codeword)); + return received_codeword ^= decoding_table[get_syndrome(received_codeword)]; +} + +int golay23_count_errors(int recd_codeword, int corrected_codeword) +{ + int errors = 0; + int diff, i; + + diff = recd_codeword ^ corrected_codeword; + for(i=0; i<23; i++) { + if (diff & 0x1) + errors++; + diff >>= 1; + } + + return errors; +} + +#endif + +// from http://stackoverflow.com/questions/10564491/function-to-calculate-a-crc16-checksum + +unsigned short gen_crc16(unsigned char* data_p, unsigned char length){ + unsigned char x; + unsigned short crc = 0xFFFF; + + while (length--){ + x = crc >> 8 ^ *data_p++; + x ^= x>>4; + crc = (crc << 8) ^ ((unsigned short)(x << 12)) ^ ((unsigned short)(x <<5)) ^ ((unsigned short)x); + } + return crc; +} + diff --git a/src/codecs/horus/horus_l2.h b/src/codecs/horus/horus_l2.h new file mode 100644 index 0000000..7ded29a --- /dev/null +++ b/src/codecs/horus/horus_l2.h @@ -0,0 +1,23 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: horus_l2.h + AUTHOR......: David Rowe + DATE CREATED: Dec 2015 + +\*---------------------------------------------------------------------------*/ + +#ifndef __HORUS_L2__ +#define __HORUS_L2__ + +int horus_l2_get_num_tx_data_bytes(int num_payload_data_bytes); + +/* returns number of output bytes in output_tx_data */ +int horus_l2_encode_tx_packet(unsigned char *output_tx_data, + unsigned char *input_payload_data, + int num_payload_data_bytes); + +void horus_l2_decode_rx_packet(unsigned char *output_payload_data, + unsigned char *input_rx_data, + int num_payload_data_bytes); + +#endif diff --git a/src/codecs/horus/horus_packet_v1.c b/src/codecs/horus/horus_packet_v1.c new file mode 100644 index 0000000..79336a1 --- /dev/null +++ b/src/codecs/horus/horus_packet_v1.c @@ -0,0 +1,75 @@ +#include +#include "horus_packet_v1.h" + +volatile uint16_t horus_packet_counter = 0; + +static uint16_t calculate_crc16_checksum(char *string, int len) +{ + uint16_t crc = 0xffff; + char i; + int ptr = 0; + while (ptr < len) { + ptr++; + crc = crc ^ (*(string++) << 8); + for (i = 0; i < 8; i++) { + if (crc & 0x8000) + crc = (uint16_t)((crc << 1) ^ 0x1021); + else + crc <<= 1; + } + } + return crc; +} + +size_t horus_packet_v1_create(uint8_t *payload, size_t length, telemetry_data *data, uint8_t payload_id) +{ + if (length < sizeof(horus_packet_v1)) { + return 0; + } + + gps_data *gps_data = &data->gps; + + float float_lat = (float) gps_data->latitude_degrees_1000000 / 10000000.0f; + float float_lon = (float) gps_data->longitude_degrees_1000000 / 10000000.0f; + + uint8_t volts_scaled = (uint8_t)(255 * (float) data->battery_voltage_millivolts / 5000.0f); + + horus_packet_counter++; + + // Assemble a binary packet + horus_packet_v1 horus_packet; + + horus_packet.PayloadID = payload_id % 256; + horus_packet.Counter = horus_packet_counter; + horus_packet.Hours = gps_data->hours; + horus_packet.Minutes = gps_data->minutes; + horus_packet.Seconds = gps_data->seconds; + horus_packet.Latitude = float_lat; + horus_packet.Longitude = float_lon; + horus_packet.Altitude = (uint16_t)(gps_data->altitude_mm / 1000); + horus_packet.Speed = (uint8_t)((float) gps_data->ground_speed_cm_per_second * 0.036); + + // Temporary pDOP info, to determine suitable pDOP limits. + // float pDop = (float)gps_data.pDOP/10.0; + // if (pDop>255.0){ + // pDop = 255.0; + // } + // mfsk_buffer.Speed = (uint8_t)pDop; + horus_packet.BattVoltage = volts_scaled; + horus_packet.Sats = gps_data->satellites_visible; + horus_packet.Temp = (int8_t) ((float) data->internal_temperature_celsius_100 / 100.0f); + + // Add onto the sats_raw value to indicate if the GPS is in regular tracking (+100) + // or power optimized tracker (+200) modes. + if (gps_data->power_safe_mode_state == POWER_SAFE_MODE_STATE_TRACKING) { + horus_packet.Sats += 100; + } else if (gps_data->power_safe_mode_state == POWER_SAFE_MODE_STATE_POWER_OPTIMIZED_TRACKING) { + horus_packet.Sats += 200; + } + + horus_packet.Checksum = (uint16_t) calculate_crc16_checksum((char *) &horus_packet, sizeof(horus_packet) - 2); + + memcpy(payload, &horus_packet, sizeof(horus_packet)); + + return sizeof(horus_packet); +} diff --git a/src/codecs/horus/horus_packet_v1.h b/src/codecs/horus/horus_packet_v1.h new file mode 100644 index 0000000..a5f2ad0 --- /dev/null +++ b/src/codecs/horus/horus_packet_v1.h @@ -0,0 +1,32 @@ +#ifndef __HORUS_PACKET_V1_H +#define __HORUS_PACKET_V1_H + +#include +#include +#include "telemetry.h" + +// Horus Binary v1 Packet Format +// See: https://github.com/projecthorus/horusdemodlib/wiki/4-Packet-Format-Details#horus-binary-v1-22-bytes +// Note that we need to pack this to 1-byte alignment, hence the #pragma flags below +// Refer: https://gcc.gnu.org/onlinedocs/gcc-4.4.4/gcc/Structure_002dPacking-Pragmas.html +#pragma pack(push, 1) +typedef struct _horus_packet_v1 { + uint8_t PayloadID; + uint16_t Counter; + uint8_t Hours; + uint8_t Minutes; + uint8_t Seconds; + float Latitude; // Latitude in degrees TODO: ? + float Longitude; // Longitude in degrees TODO: ? + uint16_t Altitude; // Altitude in meters + uint8_t Speed; // Speed in Knots (1-255 knots) -- TODO: It seems this is actually km/h? + uint8_t Sats; + int8_t Temp; // Temperature in Celsius, as a signed value (-128 to +128, though sensor limited to -64 to +64 deg C) + uint8_t BattVoltage; // 0 = 0v, 255 = 5.0V, linear steps in-between. + uint16_t Checksum; // CRC16-CCITT Checksum. +} horus_packet_v1; // __attribute__ ((packed)); // Doesn't work? +#pragma pack(pop) + +size_t horus_packet_v1_create(uint8_t *payload, size_t length, telemetry_data *data, uint8_t payload_id); + +#endif diff --git a/src/codecs/jtencode/jtencode.cpp b/src/codecs/jtencode/jtencode.cpp index d8536f5..3821ef3 100644 --- a/src/codecs/jtencode/jtencode.cpp +++ b/src/codecs/jtencode/jtencode.cpp @@ -6,7 +6,6 @@ #include "lib/JTEncode.h" #include "jtencode.h" -#include "log.h" // Some of the code is based on JTEncode examples: // diff --git a/src/codecs/jtencode/lib/JTEncode.cpp b/src/codecs/jtencode/lib/JTEncode.cpp index 5aceb21..73eda6a 100644 --- a/src/codecs/jtencode/lib/JTEncode.cpp +++ b/src/codecs/jtencode/lib/JTEncode.cpp @@ -245,7 +245,7 @@ void JTEncode::fsq_encode(const char * from_call, const char * message, uint8_t { char * tx_message; uint16_t symbol_pos = 0; - uint8_t i, fch, vcode1, vcode2, tone; + uint8_t i, fch, vcode1, vcode2; uint8_t cur_tone = 0; // Clear out the transmit buffer @@ -332,7 +332,7 @@ void JTEncode::fsq_dir_encode(const char * from_call, const char * to_call, cons { char * tx_message; uint16_t symbol_pos = 0; - uint8_t i, fch, vcode1, vcode2, tone, from_call_crc; + uint8_t i, fch, vcode1, vcode2, from_call_crc; uint8_t cur_tone = 0; // Generate a CRC on from_call @@ -421,8 +421,6 @@ void JTEncode::fsq_dir_encode(const char * from_call, const char * to_call, cons uint8_t ft8_buffer_s[FT8_BIT_COUNT]; void JTEncode::ft8_encode(const char * msg, uint8_t * symbols) { - uint8_t i; - char message[19]; memset(message, 0, 19); strcpy(message, msg); @@ -845,7 +843,7 @@ void JTEncode::ft8_bit_packing(char* message, uint8_t* codeword) memset(qa, 0, 10); memset(qb, 0, 10); - uint8_t i, j, x, i0; + uint8_t i, j, x, i0 = 0; uint32_t ireg = 0; // See if this is a telemetry message @@ -1011,7 +1009,7 @@ void JTEncode::jt65_interleave(uint8_t * s) uint8_t jt9_buffer_d[JT9_BIT_COUNT]; void JTEncode::jt9_interleave(uint8_t * s) { - uint8_t i, j; + uint8_t i; // Do the interleave for(i = 0; i < JT9_BIT_COUNT; i++) @@ -1020,8 +1018,9 @@ void JTEncode::jt9_interleave(uint8_t * s) #if defined(__arm__) jt9_buffer_d[jt9i[i]] = s[i]; #else - j = pgm_read_byte(&jt9i[i]); - jt9_buffer_d[j] = s[i]; + uint8_t j; + j = pgm_read_byte(&jt9i[i]); + jt9_buffer_d[j] = s[i]; #endif } @@ -1372,9 +1371,10 @@ uint8_t JTEncode::crc8(const char * text) { uint8_t crc = '\0'; uint8_t ch; + uint8_t len = (uint8_t) strlen(text); int i; - for(i = 0; i < strlen(text); i++) + for(i = 0; i < len; i++) { ch = text[i]; //#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega16U4__) diff --git a/src/codecs/mfsk/mfsk.c b/src/codecs/mfsk/mfsk.c new file mode 100644 index 0000000..862a7b9 --- /dev/null +++ b/src/codecs/mfsk/mfsk.c @@ -0,0 +1,188 @@ +#include +#include + +#include "mfsk.h" + +typedef struct _mfsk_encoder { + mfsk_type type; + uint32_t symbol_rate; + uint32_t tone_spacing_hz_100; + uint8_t max_nibble_index; + + uint16_t data_length; + uint8_t *data; + + uint16_t current_byte_index; + uint8_t current_nibble_index; + uint8_t current_byte; + + int8_t idle_tone_index; + int8_t current_tone_index; + + fsk_tone tones[16]; +} mfsk_encoder; + +#define MFSK_2_IDLE_TONE 0 +#define MFSK_4_IDLE_TONE 3 +#define MFSK_16_IDLE_TONE 3 + +void mfsk_encoder_new(fsk_encoder *encoder, mfsk_type type, uint32_t symbol_rate, uint32_t tone_spacing_hz_100) +{ + encoder->priv = malloc(sizeof(mfsk_encoder)); + memset(encoder->priv, 0, sizeof(mfsk_encoder)); + + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + mfsk->type = type; + mfsk->symbol_rate = symbol_rate; + mfsk->tone_spacing_hz_100 = tone_spacing_hz_100; + + switch (mfsk->type) { + case MFSK_2: + mfsk->idle_tone_index = MFSK_2_IDLE_TONE; + mfsk->max_nibble_index = 8; + break; + case MFSK_4: + mfsk->idle_tone_index = MFSK_4_IDLE_TONE; + mfsk->max_nibble_index = 4; + break; + case MFSK_16: + mfsk->idle_tone_index = MFSK_16_IDLE_TONE; + mfsk->max_nibble_index = 2; + break; + default: + break; + } + + for (int i = 0; i < type; i++) { + mfsk->tones[i].index = (int8_t) i; + mfsk->tones[i].frequency_hz_100 = i * tone_spacing_hz_100; + } +} + +void mfsk_encoder_destroy(fsk_encoder *encoder) +{ + if (encoder->priv != NULL) { + free(encoder->priv); + encoder->priv = NULL; + } +} + +fsk_tone *mfsk_get_idle_tone(fsk_encoder *encoder) +{ + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + return &mfsk->tones[mfsk->idle_tone_index]; +} + +void mfsk_encoder_set_data(fsk_encoder *encoder, uint16_t data_length, uint8_t *data) +{ + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + + mfsk->data = data; + mfsk->data_length = data_length; + + mfsk->current_tone_index = mfsk->idle_tone_index; + mfsk->current_byte_index = 0; + mfsk->current_nibble_index = 0; + mfsk->current_byte = data[0]; +} + +void mfsk_encoder_get_tones(fsk_encoder *encoder, int8_t *tone_count, fsk_tone **tones) +{ + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + + *tone_count = mfsk->type; + *tones = mfsk->tones; +} + +uint32_t mfsk_encoder_get_tone_spacing(fsk_encoder *encoder) +{ + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + return mfsk->tone_spacing_hz_100; +} + +uint32_t mfsk_encoder_get_symbol_rate(fsk_encoder *encoder) +{ + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + return mfsk->symbol_rate; +} + +uint32_t mfsk_encoder_get_symbol_delay(fsk_encoder *encoder) +{ + return 0; +} + +int8_t mfsk_encoder_next_tone(fsk_encoder *encoder) +{ + mfsk_encoder *mfsk = (mfsk_encoder *) encoder->priv; + + if (mfsk->current_byte_index >= mfsk->data_length) { + return -1; + } + + if (mfsk->current_nibble_index == mfsk->max_nibble_index) { + // Reached the end of the byte + mfsk->current_nibble_index = 0; + mfsk->current_byte_index++; + if (mfsk->current_byte_index >= mfsk->data_length) { + return -1; + } + mfsk->current_byte = mfsk->data[mfsk->current_byte_index]; + } + + int8_t symbol; + + switch (mfsk->type) { + case MFSK_2: + // Get the current symbol (the MSB). + if (((uint8_t) mfsk->current_byte & 0x80) >> 7) { + symbol = 2; + } else { + symbol = 0; + } + + // Shift it left to the nibble we are up to. + mfsk->current_byte = mfsk->current_byte << 1; + break; + case MFSK_4: + // Get the current symbol (the 2 bits we need). + symbol = (int8_t) ((mfsk->current_byte & 0xC0) >> 6); + + // Shift it left to the nibble we are up to. + mfsk->current_byte = mfsk->current_byte << 2; + break; + case MFSK_16: + // Get the current symbol (the 4 bits we need). + symbol = (int8_t) ((mfsk->current_byte & 0xF0) >> 4); + + // Shift it left to the nibble we are up to. + mfsk->current_byte = mfsk->current_byte << 4; + break; + default: + symbol = 0; + break; + } + + mfsk->current_nibble_index++; + + return symbol; +} + +fsk_encoder_api mfsk_fsk_encoder_api = { + .get_tones = mfsk_encoder_get_tones, + .get_tone_spacing = mfsk_encoder_get_tone_spacing, + .get_symbol_rate = mfsk_encoder_get_symbol_rate, + .get_symbol_delay = mfsk_encoder_get_symbol_delay, + .set_data = mfsk_encoder_set_data, + .next_tone = mfsk_encoder_next_tone, +}; + +#ifdef TEST +int mfsk_test_bits(char *buffer) +{ + // Add some test bits into the MFSK buffer, compatible with the first 200 test bits produced by fsk_get_test_bits from codec2-dev. + // Decode using (with GQRX outputting data to UDP): nc -l -u localhost 7355 | ./fsk_demod 4 48000 100 - - | ./fsk_put_test_bits - + sprintf(buffer, + "\x1b\x1b\x1b\x1b\xd8\x2c\xa2\x56\x3f\x4c\x08\xa2\xba\x6f\x84\x07\x6d\x82\xca\x25\x63\xf4\xc0\x8a\x2b\xa6\xf8\x40\x76"); + return 29; +} +#endif diff --git a/src/codecs/mfsk/mfsk.h b/src/codecs/mfsk/mfsk.h new file mode 100644 index 0000000..eada01d --- /dev/null +++ b/src/codecs/mfsk/mfsk.h @@ -0,0 +1,26 @@ +#ifndef __MFSK_H +#define __MFSK_H + +#include + +#include "codecs/fsk/fsk.h" + +typedef enum _mfsk_type { + MFSK_2 = 2, + MFSK_4 = 4, + MFSK_16 = 16, +} mfsk_type; + +void mfsk_encoder_new(fsk_encoder *encoder, mfsk_type type, uint32_t symbol_rate, uint32_t tone_spacing_hz_100); +void mfsk_encoder_destroy(fsk_encoder *encoder); +fsk_tone *mfsk_get_idle_tone(fsk_encoder *encoder); +void mfsk_encoder_set_data(fsk_encoder *encoder, uint16_t data_length, uint8_t *data); +void mfsk_encoder_get_tones(fsk_encoder *encoder, int8_t *tone_count, fsk_tone **tones); +uint32_t mfsk_encoder_get_tone_spacing(fsk_encoder *encoder); +uint32_t mfsk_encoder_get_symbol_rate(fsk_encoder *encoder); +uint32_t mfsk_encoder_get_symbol_delay(fsk_encoder *encoder); +int8_t mfsk_encoder_next_tone(fsk_encoder *encoder); + +extern fsk_encoder_api mfsk_fsk_encoder_api; + +#endif diff --git a/src/config.c b/src/config.c index 4590e41..d873bea 100644 --- a/src/config.c +++ b/src/config.c @@ -1,3 +1,8 @@ +/** + * The tracker firmware will transmit each of the message templates + * defined here one by one, starting again from the beginning once the last message is transmitted. + */ + #include #include "config.h" @@ -45,18 +50,33 @@ volatile bool system_initialized = false; * $he - Heading in degrees (up to 3 chars) */ +/** + * APRS mode comment messages. + * Maximum length: depends on the packet contents, but keeping this under 100 characters is usually safe. + * Note that many hardware APRS receivers show a limited number of APRS comment characters, such as 43 or 67 chars. + */ char *aprs_comment_templates[] = { - " B$bu $teC $hu% $prmb $hh:$mm:$ss @ $tow ms - RS41ng radiosonde firmware test", - " $loc12 - RS41ng radiosonde firmware test", +// " B$bu $teC $hu% $prmb $hh:$mm:$ss @ $tow ms - " APRS_COMMENT, +// " B$bu $teC $hu% $prmb - " APRS_COMMENT, +// " B$bu $loc12 $hh:$mm:$ss - " APRS_COMMENT, + " $loc12 - " APRS_COMMENT, NULL }; +/** + * FSQ mode comment message templates. + * Maximum length: 130 characters. + */ char *fsq_comment_templates[] = { - " $lat $lon, $alt m, $cl m/s, $gs km/h, $he deg - RS41ng radiosonde firmware test", - " $loc12, $teC $hu% $prmb $hh:$mm:$ss @ $tow ms - RS41ng radiosonde firmware test", + " $lat $lon, $alt m, $cl m/s, $gs km/h, $he deg - " FSQ_COMMENT, + " $loc12, $teC $hu% $prmb $hh:$mm:$ss @ $tow ms - " FSQ_COMMENT, NULL }; +/** + * FTx/JTxx mode message templates. + * Maximum length: 13 characters allowed by the protocols. + */ char *ftjt_message_templates[] = { "$cs $loc4", "$loc12", diff --git a/src/config.h b/src/config.h index 462d3bd..7f49a9b 100644 --- a/src/config.h +++ b/src/config.h @@ -8,35 +8,50 @@ #include -#define CALLSIGN "OH3BHX" - -#define RADIO_PAYLOAD_MAX_LENGTH 256 -#define RADIO_SYMBOL_DATA_MAX_LENGTH 512 -#define RADIO_PAYLOAD_MESSAGE_MAX_LENGTH 128 +// Set the tracker amateur radio call sign here +#define CALLSIGN "MYCALL" #define LEDS_ENABLE true - #define SENSOR_BMP280_ENABLE false - -#define RADIO_SI5351_ENABLE true +#define RADIO_SI5351_ENABLE false #define RADIO_POST_TRANSMIT_DELAY_MS 5000 -#define RADIO_TIME_SYNC_THRESHOLD_MS 1500 +#define RADIO_TIME_SYNC_THRESHOLD_MS 2000 /** * Si4032 transmit power: 0..7 * 0 = -1dBm, 1 = 2dBm, 2 = 5dBm, 3 = 8dBm, 4 = 11dBm, 5 = 14dBm, 6 = 17dBm, 7 = 20dBm */ #define RADIO_SI4032_TX_POWER 7 + +// Which modes to transmit using the built-in Si4032 transmitter chip +#define RADIO_SI4032_TX_CW false +#define RADIO_SI4032_TX_RTTY false +#define RADIO_SI4032_TX_APRS true +#define RADIO_SI4032_TX_HORUS_V1 true + +// Transmit frequencies for the Si4032 transmitter modes #define RADIO_SI4032_TX_FREQUENCY_CW 432060000 #define RADIO_SI4032_TX_FREQUENCY_RTTY 432060000 #define RADIO_SI4032_TX_FREQUENCY_APRS_1200 432500000 +// Use a frequency offset to place FSK tones slightly above the defined frequency for SSB reception +#define RADIO_SI4032_TX_FREQUENCY_HORUS_V1 432501000 /** * Si5351 transmit power: 0..3 * Si5351 drive strength: 0 = 2mA, 1 = 4mA, 2 = 6mA, 3 = 8mA */ #define RADIO_SI5351_TX_POWER 3 + +// Which modes to transmit using an externally connected Si5351 chip in the I²C bus +#define RADIO_SI5351_TX_JT9 false +#define RADIO_SI5351_TX_JT65 false +#define RADIO_SI5351_TX_JT4 false +#define RADIO_SI5351_TX_WSPR false +#define RADIO_SI5351_TX_FSQ false +#define RADIO_SI5351_TX_FT8 false + +// Transmit frequencies for the Si5351 transmitter modes #define RADIO_SI5351_TX_FREQUENCY_JT9 14085000UL // Was: 14078700UL #define RADIO_SI5351_TX_FREQUENCY_JT65 14085000UL // Was: 14078300UL #define RADIO_SI5351_TX_FREQUENCY_JT4 14085000UL // Was: 14078500UL @@ -46,12 +61,15 @@ #define LOCATOR_PAIR_COUNT_FULL 6 // max. 6 (12 characters WWL) +// WSPR settings #define WSPR_CALLSIGN CALLSIGN #define WSPR_LOCATOR_FIXED_ENABLED false #define WSPR_LOCATOR_FIXED "AA00" #define WSPR_DBM 10 +// FSQ settings #define FSQ_CALLSIGN_FROM CALLSIGN +#define FSQ_COMMENT "RS41ng radiosonde firmware test" /** * APRS SSID: @@ -79,11 +97,17 @@ // See APRS symbol table documentation in: http://www.aprs.org/symbols/symbolsX.txt #define APRS_SYMBOL_TABLE '/' // '/' denotes primary and '\\' denotes alternate APRS symbol table #define APRS_SYMBOL 'O' -#define APRS_COMMENT " RS41ng radiosonde firmware test" +#define APRS_COMMENT "RS41ng radiosonde firmware test" #define APRS_RELAYS "WIDE1-1,WIDE2-1" #define APRS_DESTINATION "APZ41N" #define APRS_DESTINATION_SSID '0' +// Use Horus payload ID 0 for tests (4FSKTEST) +#define HORUS_V1_PAYLOAD_ID 0 +#define HORUS_V1_BAUD_RATE 100 +#define HORUS_V1_FREQUENCY_OFFSET 0 +#define HORUS_V1_PREAMBLE_LENGTH 16 + // TODO: RTTY and CW settings (once modes are implemented) #define RTTY_LOCATOR_PAIR_COUNT 4 // max. 6 (12 characters WWL) @@ -91,6 +115,10 @@ #define CW_LOCATOR_PAIR_COUNT 4 // max. 6 (12 characters WWL) +#define RADIO_PAYLOAD_MAX_LENGTH 256 +#define RADIO_SYMBOL_DATA_MAX_LENGTH 512 +#define RADIO_PAYLOAD_MESSAGE_MAX_LENGTH 128 + extern bool leds_enabled; extern bool bmp280_enabled; extern bool si5351_enabled; diff --git a/src/drivers/si4032/si4032.c b/src/drivers/si4032/si4032.c index ecfe275..b0ea6e9 100644 --- a/src/drivers/si4032/si4032.c +++ b/src/drivers/si4032/si4032.c @@ -75,10 +75,15 @@ void si4032_set_tx_power(uint8_t power) */ void si4032_set_frequency_offset(uint16_t offset) { - si4032_write(0x73, 0); + si4032_write(0x73, offset); si4032_write(0x74, 0); } +inline void si4032_set_frequency_offset_small(uint8_t offset) +{ + si4032_write(0x73, offset); +} + void si4032_set_frequency_deviation(uint8_t deviation) { // The frequency deviation can be calculated: Fd = 625 Hz x fd[8:0]. diff --git a/src/drivers/si4032/si4032.h b/src/drivers/si4032/si4032.h index 97399c6..07b0efb 100644 --- a/src/drivers/si4032/si4032.h +++ b/src/drivers/si4032/si4032.h @@ -18,6 +18,7 @@ void si4032_use_direct_mode(bool use); void si4032_set_tx_frequency(float frequency_mhz); void si4032_set_tx_power(uint8_t power); void si4032_set_frequency_offset(uint16_t offset); +void si4032_set_frequency_offset_small(uint8_t offset); void si4032_set_frequency_deviation(uint8_t deviation); void si4032_set_modulation_type(si4032_modulation_type type); int32_t si4032_read_temperature_celsius_100(); diff --git a/src/drivers/ubxg6010/ubxg6010.c b/src/drivers/ubxg6010/ubxg6010.c index 1f0a2e1..dedc6de 100644 --- a/src/drivers/ubxg6010/ubxg6010.c +++ b/src/drivers/ubxg6010/ubxg6010.c @@ -118,6 +118,16 @@ typedef struct { uint32_t headAcc; //Heading accuracy estimate (both motion and vehicle) [1e-5 deg] } uBloxNAVVELNEDPayload; +typedef struct { + uint32_t iTOW; //GPS Millisecond Time of Week [- ms] + uint8_t gpsFix; //GPS Fix State + uint8_t flags; // LSB = gpsFixOK - the only valid way of determining if a fix is actually OK. + uint8_t fixStat; + uint8_t flags2; // Power Save Mode State + uint32_t ttff; + uint32_t msss; +} uBloxNAVSTATUSPayload; + typedef struct { uint8_t portID; //Port Identifier Number (see Serial [- -] uint8_t reserved1; //Reserved [- -] @@ -205,6 +215,7 @@ typedef union { uBloxNAVTIMEGPSPayload navtimegps; uBloxNAVTIMEUTCPayload navtimeutc; uBloxNAVVELNEDPayload navvelned; + uBloxNAVSTATUSPayload navstatus; uBloxACKACKayload ackack; uBloxCFGRSTPayload cfgrst; uBloxCFGRXMPayload cfgrxm; @@ -216,7 +227,7 @@ typedef struct __attribute__((packed)) { ubloxPacketData data; } uBloxPacket; -gps_data current_gps_data; +gps_data ubxg6010_current_gps_data; volatile bool gps_initialized = false; volatile bool ack_received = false; @@ -305,8 +316,8 @@ bool ubxg6010_send_packet_and_wait_for_ack(uBloxPacket *packet) bool ubxg6010_get_current_gps_data(gps_data *data) { system_disable_irq(); - memcpy(data, ¤t_gps_data, sizeof(gps_data)); - current_gps_data.updated = false; + memcpy(data, &ubxg6010_current_gps_data, sizeof(gps_data)); + ubxg6010_current_gps_data.updated = false; system_enable_irq(); return data->updated; @@ -425,16 +436,18 @@ uBloxPacket msgcfgnav5 = { .messageId=0x24, .payloadSize=sizeof(uBloxCFGNAV5Payload) }, + // NOTE: Dynamic model needs to be set to one of the Airborne modes to support high altitudes! + // Notes from darksidelemm RS41HUP fork: Tweaked the PDOP limits a bit, to make it a bit more likely to report a position. .data.cfgnav5={ .mask=0b0000001111111111, // Configure all settings - .dynModel=4, // Dynamic model: Airborne with <2g Acceleration -- TODO: 7 - .fixMode=3, // Fix mode: 3D only -- TODO: 2 + .dynModel=6, // Dynamic model: Airborne with <1g Acceleration + .fixMode=2, // Fix mode: 3D only .fixedAlt=0, // Fixed altitude (mean sea level) for 2D fix mode. .fixedAltVar=10000, // Fixed altitude variance for 2D mode. .minElev=5, // Minimum Elevation for a GNSS satellite to be used in NAV (degrees) .drLimit=0, // Maximum time to perform dead reckoning (linear extrapolation) in case of GPS signal loss (seconds) - .pDop=25, // Position DOP Mask to use - .tDop=25, // Time DOP Mask to use + .pDop=100, // Position DOP Mask to use (was 25) + .tDop=100, // Time DOP Mask to use (was 25) .pAcc=100, // Position Accuracy Mask (m) .tAcc=300, // Time Accuracy Mask (m) .staticHoldThresh=0, // Static hold threshold (cm/s) @@ -449,7 +462,7 @@ bool ubxg6010_init() { bool success; - memset(¤t_gps_data, 0, sizeof(gps_data)); + memset(&ubxg6010_current_gps_data, 0, sizeof(gps_data)); gps_initialized = false; @@ -483,6 +496,9 @@ bool ubxg6010_init() return false; } + // Rate of 1 for message: 0x01 0x02 Geodetic Position Solution + msgcfgmsg.data.cfgmsg.msgID = 0x02; + msgcfgmsg.data.cfgmsg.rate = 1; log_info("GPS: Requesting update messages from GPS chip\n"); success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg); if (!success) { @@ -496,7 +512,7 @@ bool ubxg6010_init() } // Rate of 1 for message: 0x01 0x06 Navigation Solution Information - msgcfgmsg.data.cfgmsg.msgID = 0x6; + msgcfgmsg.data.cfgmsg.msgID = 0x06; msgcfgmsg.data.cfgmsg.rate = 1; log_info("GPS: Requesting update messages from GPS chip\n"); success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg); @@ -531,6 +547,15 @@ bool ubxg6010_init() return false; } + // Configure rate of 2 for message: 0x01 0x03 Receiver Navigation Status + msgcfgmsg.data.cfgmsg.msgID = 0x03; + msgcfgmsg.data.cfgmsg.rate = 1; + log_info("GPS: Requesting update messages from GPS chip\n"); + success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg); + if (!success) { + return false; + } + // TODO: Is this message supported: Configure rate of 1 for message: 0x01 0x07 Position Velocity Time Solution /*msgcfgmsg.data.cfgmsg.msgID = 0x07; success = ubxg6010_send_packet_and_wait_for_ack(&msgcfgmsg); @@ -587,7 +612,7 @@ static void ubxg6010_handle_packet(uBloxPacket *pkt) uBloxChecksum *checksum = (uBloxChecksum *) (((uint8_t *) &pkt->data) + pkt->header.payloadSize); if (cksum.ck_a != checksum->ck_a || cksum.ck_b != checksum->ck_b) { - current_gps_data.bad_packets += 1; + ubxg6010_current_gps_data.bad_packets += 1; return; } @@ -595,62 +620,69 @@ static void ubxg6010_handle_packet(uBloxPacket *pkt) if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x07) { // TODO: It seems NAV PVT message is not supported by UBXG6010, confirm this - current_gps_data.ok_packets += 1; - current_gps_data.time_of_week_millis = pkt->data.navpvt.iTOW; - current_gps_data.year = pkt->data.navpvt.year; - current_gps_data.month = pkt->data.navpvt.month; - current_gps_data.day = pkt->data.navpvt.day; - current_gps_data.hours = pkt->data.navpvt.hour; - current_gps_data.minutes = pkt->data.navpvt.min; - current_gps_data.seconds = pkt->data.navpvt.sec; + ubxg6010_current_gps_data.ok_packets += 1; + ubxg6010_current_gps_data.time_of_week_millis = pkt->data.navpvt.iTOW; + ubxg6010_current_gps_data.year = pkt->data.navpvt.year; + ubxg6010_current_gps_data.month = pkt->data.navpvt.month; + ubxg6010_current_gps_data.day = pkt->data.navpvt.day; + ubxg6010_current_gps_data.hours = pkt->data.navpvt.hour; + ubxg6010_current_gps_data.minutes = pkt->data.navpvt.min; + ubxg6010_current_gps_data.seconds = pkt->data.navpvt.sec; - current_gps_data.fix = pkt->data.navpvt.fixType; - current_gps_data.latitude_degrees_1000000 = pkt->data.navpvt.lat; - current_gps_data.longitude_degrees_1000000 = pkt->data.navpvt.lon; - current_gps_data.altitude_mm = pkt->data.navpvt.hMSL; - current_gps_data.satellites_visible = pkt->data.navpvt.numSV; - current_gps_data.ground_speed_cm_per_second = pkt->data.navpvt.gSpeed; - current_gps_data.heading_degrees_100000 = pkt->data.navpvt.headMot; - current_gps_data.climb_cm_per_second = -pkt->data.navpvt.velD; + ubxg6010_current_gps_data.fix = pkt->data.navpvt.fixType; + ubxg6010_current_gps_data.latitude_degrees_1000000 = pkt->data.navpvt.lat; + ubxg6010_current_gps_data.longitude_degrees_1000000 = pkt->data.navpvt.lon; + ubxg6010_current_gps_data.altitude_mm = pkt->data.navpvt.hMSL; + ubxg6010_current_gps_data.satellites_visible = pkt->data.navpvt.numSV; + ubxg6010_current_gps_data.ground_speed_cm_per_second = pkt->data.navpvt.gSpeed; + ubxg6010_current_gps_data.heading_degrees_100000 = pkt->data.navpvt.headMot; + ubxg6010_current_gps_data.climb_cm_per_second = -pkt->data.navpvt.velD; - current_gps_data.updated = true; + ubxg6010_current_gps_data.updated = true; } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x12) { - current_gps_data.ok_packets += 1; - current_gps_data.time_of_week_millis = pkt->data.navvelned.iTOW; - current_gps_data.ground_speed_cm_per_second = pkt->data.navvelned.gSpeed; - current_gps_data.heading_degrees_100000 = pkt->data.navvelned.headMot; - current_gps_data.climb_cm_per_second = -pkt->data.navvelned.velD; + ubxg6010_current_gps_data.ok_packets += 1; + ubxg6010_current_gps_data.time_of_week_millis = pkt->data.navvelned.iTOW; + ubxg6010_current_gps_data.ground_speed_cm_per_second = pkt->data.navvelned.gSpeed; + ubxg6010_current_gps_data.heading_degrees_100000 = pkt->data.navvelned.headMot; + ubxg6010_current_gps_data.climb_cm_per_second = -pkt->data.navvelned.velD; - current_gps_data.updated = true; + ubxg6010_current_gps_data.updated = true; } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x02) { - current_gps_data.ok_packets += 1; - current_gps_data.time_of_week_millis = pkt->data.navposllh.iTOW; - current_gps_data.latitude_degrees_1000000 = pkt->data.navposllh.lat; - current_gps_data.longitude_degrees_1000000 = pkt->data.navposllh.lon; - current_gps_data.altitude_mm = pkt->data.navposllh.hMSL; + ubxg6010_current_gps_data.ok_packets += 1; + ubxg6010_current_gps_data.time_of_week_millis = pkt->data.navposllh.iTOW; + ubxg6010_current_gps_data.latitude_degrees_1000000 = pkt->data.navposllh.lat; + ubxg6010_current_gps_data.longitude_degrees_1000000 = pkt->data.navposllh.lon; + ubxg6010_current_gps_data.altitude_mm = pkt->data.navposllh.hMSL; - current_gps_data.updated = true; + ubxg6010_current_gps_data.updated = true; + } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x03) { + ubxg6010_current_gps_data.ok_packets += 1; + ubxg6010_current_gps_data.fix_ok = pkt->data.navstatus.flags & 0x01; + ubxg6010_current_gps_data.power_safe_mode_state = pkt->data.navstatus.flags2 & 0x03; + + ubxg6010_current_gps_data.updated = true; } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x06) { - current_gps_data.time_of_week_millis = pkt->data.navsol.iTOW; - current_gps_data.week = pkt->data.navsol.week; - current_gps_data.fix = pkt->data.navsol.gpsFix; - current_gps_data.satellites_visible = pkt->data.navsol.numSV; + ubxg6010_current_gps_data.time_of_week_millis = pkt->data.navsol.iTOW; + ubxg6010_current_gps_data.week = pkt->data.navsol.week; + ubxg6010_current_gps_data.fix = pkt->data.navsol.gpsFix; + ubxg6010_current_gps_data.satellites_visible = pkt->data.navsol.numSV; + ubxg6010_current_gps_data.position_dilution_of_precision = pkt->data.navsol.pDOP; - current_gps_data.updated = true; + ubxg6010_current_gps_data.updated = true; } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x20) { - current_gps_data.time_of_week_millis = pkt->data.navtimegps.iTOW; - current_gps_data.week = pkt->data.navtimegps.week; + ubxg6010_current_gps_data.time_of_week_millis = pkt->data.navtimegps.iTOW; + ubxg6010_current_gps_data.week = pkt->data.navtimegps.week; - current_gps_data.updated = true; + ubxg6010_current_gps_data.updated = true; } else if (pkt->header.messageClass == 0x01 && pkt->header.messageId == 0x21) { - current_gps_data.year = pkt->data.navtimeutc.year; - current_gps_data.month = pkt->data.navtimeutc.month; - current_gps_data.day = pkt->data.navtimeutc.day; - current_gps_data.hours = pkt->data.navtimeutc.hour; - current_gps_data.minutes = pkt->data.navtimeutc.min; - current_gps_data.seconds = pkt->data.navtimeutc.sec; + ubxg6010_current_gps_data.year = pkt->data.navtimeutc.year; + ubxg6010_current_gps_data.month = pkt->data.navtimeutc.month; + ubxg6010_current_gps_data.day = pkt->data.navtimeutc.day; + ubxg6010_current_gps_data.hours = pkt->data.navtimeutc.hour; + ubxg6010_current_gps_data.minutes = pkt->data.navtimeutc.min; + ubxg6010_current_gps_data.seconds = pkt->data.navtimeutc.sec; - current_gps_data.updated = true; + ubxg6010_current_gps_data.updated = true; } else if (pkt->header.messageClass == 0x05 && pkt->header.messageId == 0x01) { ack_received = true; } else if (pkt->header.messageClass == 0x05 && pkt->header.messageId == 0x00) { diff --git a/src/gps.h b/src/gps.h index db3ce81..da95760 100644 --- a/src/gps.h +++ b/src/gps.h @@ -4,6 +4,11 @@ #include #include +#define POWER_SAFE_MODE_STATE_ACQUISITION 0 +#define POWER_SAFE_MODE_STATE_TRACKING 1 +#define POWER_SAFE_MODE_STATE_POWER_OPTIMIZED_TRACKING 2 +#define POWER_SAFE_MODE_STATE_INACTIVE 3 + typedef struct _gps_data { bool updated; @@ -24,8 +29,12 @@ typedef struct _gps_data { int32_t climb_cm_per_second; uint8_t satellites_visible; uint8_t fix; + bool fix_ok; uint16_t ok_packets; uint16_t bad_packets; + + uint8_t power_safe_mode_state; + uint16_t position_dilution_of_precision; // pDOP } gps_data; #endif diff --git a/src/hal/datatimer.c b/src/hal/datatimer.c new file mode 100644 index 0000000..cdf5e95 --- /dev/null +++ b/src/hal/datatimer.c @@ -0,0 +1,67 @@ +#include +#include +#include + +#include "datatimer.h" + +void (*system_handle_data_timer_tick)() = NULL; + +void data_timer_init(uint32_t baud_rate) +{ + // Timer frequency = TIM_CLK/(TIM_PSC+1)/(TIM_ARR + 1) + // TIM_CLK = + // TIM_PSC = Prescaler + // TIM_ARR = Period + + TIM_DeInit(TIM2); + + TIM_TimeBaseInitTypeDef tim_init; + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB1Periph_TIM2, DISABLE); + + tim_init.TIM_Prescaler = 24 - 1; // tick every 1/1000000 s + tim_init.TIM_CounterMode = TIM_CounterMode_Up; + tim_init.TIM_Period = (uint16_t) ((1000000 / baud_rate) - 1); + tim_init.TIM_ClockDivision = TIM_CKD_DIV1; + tim_init.TIM_RepetitionCounter = 0; + + TIM_TimeBaseInit(TIM2, &tim_init); + + // No interrupts necessary for data timer, as it is only used for triggering DMA transfers + TIM_ClearITPendingBit(TIM2, TIM_IT_Update); + TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); + + NVIC_InitTypeDef nvic_init; + nvic_init.NVIC_IRQChannel = TIM2_IRQn; + nvic_init.NVIC_IRQChannelPreemptionPriority = 0; + nvic_init.NVIC_IRQChannelSubPriority = 1; + nvic_init.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic_init); + + TIM_Cmd(TIM2, ENABLE); +} + +void data_timer_uninit() +{ + TIM_Cmd(TIM2, DISABLE); + + NVIC_InitTypeDef nvic_init; + nvic_init.NVIC_IRQChannel = TIM2_IRQn; + nvic_init.NVIC_IRQChannelPreemptionPriority = 0; + nvic_init.NVIC_IRQChannelSubPriority = 1; + nvic_init.NVIC_IRQChannelCmd = DISABLE; + NVIC_Init(&nvic_init); + + TIM_ITConfig(TIM2, TIM_IT_Update, DISABLE); + TIM_ClearITPendingBit(TIM2, TIM_IT_Update); +} + +void TIM2_IRQHandler(void) +{ + if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) { + TIM_ClearITPendingBit(TIM2, TIM_IT_Update); + + system_handle_data_timer_tick(); + + } +} diff --git a/src/hal/datatimer.h b/src/hal/datatimer.h new file mode 100644 index 0000000..1f804e1 --- /dev/null +++ b/src/hal/datatimer.h @@ -0,0 +1,12 @@ +#ifndef __DATATIMER_H +#define __DATATIMER_H + +#include +#include + +void data_timer_init(uint32_t baud_rate); +void data_timer_uninit(); + +extern void (*system_handle_data_timer_tick)(); + +#endif diff --git a/src/hal/i2c.c b/src/hal/i2c.c index 298e8fe..1c70a5c 100644 --- a/src/hal/i2c.c +++ b/src/hal/i2c.c @@ -41,6 +41,8 @@ void i2c_init() // NOTE: I2C chip reset is necessary here! I2C_DeInit(I2C_PORT); + delay_ms(2); + I2C_InitTypeDef i2c_init; I2C_StructInit(&i2c_init); @@ -52,7 +54,11 @@ void i2c_init() I2C_Init(I2C_PORT, &i2c_init); - while (I2C_GetFlagStatus(I2C_PORT, I2C_FLAG_BUSY)); + // This loop may get stuck if there is nothing connected in the I²C port pins + int count = 1000; + while (I2C_GetFlagStatus(I2C_PORT, I2C_FLAG_BUSY) && count--) { + delay_ms(1); + } I2C_Cmd(I2C_PORT, ENABLE); } diff --git a/src/hal/pwm.c b/src/hal/pwm.c index f628e7a..493b404 100644 --- a/src/hal/pwm.c +++ b/src/hal/pwm.c @@ -57,6 +57,11 @@ void pwm_data_timer_dma_request_enable(bool enabled) TIM_DMACmd(TIM2, TIM_DMA_Update, enabled ? ENABLE : DISABLE); } +void pwm_data_timer_uninit() +{ + TIM_Cmd(TIM2, DISABLE); +} + void pwm_timer_init(uint32_t frequency_hz_100) { TIM_DeInit(TIM15); diff --git a/src/hal/pwm.h b/src/hal/pwm.h index d8a7be3..e212191 100644 --- a/src/hal/pwm.h +++ b/src/hal/pwm.h @@ -8,6 +8,7 @@ void pwm_data_timer_init(); void pwm_data_timer_dma_request_enable(bool enabled); +void pwm_data_timer_uninit(); void pwm_timer_init(uint32_t frequency_hz_100); void pwm_timer_pwm_enable(bool enabled); diff --git a/src/log.c b/src/log.c new file mode 100644 index 0000000..fa78536 --- /dev/null +++ b/src/log.c @@ -0,0 +1,20 @@ +#include "log.h" + +void log_bytes(int length, char *data) +{ + for (int i = 0; i < length; i++) { + char c = data[i]; + if (c >= 0x20 && c <= 0x7e) { + log_info("%c", c); + } else { + log_info(" [%02X] ", c); + } + } +} + +void log_bytes_hex(int length, char *data) +{ + for (int i = 0; i < length; i++) { + log_info("%02X ", data[i]); + } +} diff --git a/src/log.h b/src/log.h index 8bb5839..ff4bc20 100644 --- a/src/log.h +++ b/src/log.h @@ -23,4 +23,7 @@ #endif +void log_bytes(int length, char *data); +void log_bytes_hex(int length, char *data); + #endif diff --git a/src/main.c b/src/main.c index 4f7b385..16a0b32 100644 --- a/src/main.c +++ b/src/main.c @@ -3,6 +3,7 @@ #include "hal/spi.h" #include "hal/usart_gps.h" #include "hal/delay.h" +#include "hal/datatimer.h" #include "drivers/ubxg6010/ubxg6010.h" #include "drivers/si4032/si4032.h" #include "bmp280_handler.h" @@ -48,7 +49,10 @@ void handle_timer_tick() int main(void) { bool success; + + // Set up interrupt handlers system_handle_timer_tick = handle_timer_tick; + system_handle_data_timer_tick = radio_handle_data_timer_tick; usart_gps_handle_incoming_byte = ubxg6010_handle_incoming_byte; log_info("System init\n"); diff --git a/src/radio.c b/src/radio.c index ad0a611..9a6d4c8 100644 --- a/src/radio.c +++ b/src/radio.c @@ -7,22 +7,24 @@ #include "hal/delay.h" #include "hal/usart_gps.h" #include "codecs/bell/bell.h" +#include "codecs/mfsk/mfsk.h" #include "codecs/jtencode/jtencode.h" #include "drivers/ubxg6010/ubxg6010.h" #include "radio_internal.h" #include "radio_si4032.h" #include "radio_si5351.h" #include "radio_payload_aprs.h" +#include "radio_payload_horus_v1.h" #include "radio_payload_wspr.h" #include "radio_payload_jtencode.h" #include "radio_payload_fsq.h" radio_transmit_entry radio_transmit_schedule[] = { { - .enabled = true, + .enabled = RADIO_SI4032_TX_APRS, .radio_type = RADIO_TYPE_SI4032, .data_mode = RADIO_DATA_MODE_APRS_1200, - .time_sync_seconds = 0, + .time_sync_seconds = 1, .time_sync_seconds_offset = 0, .frequency = RADIO_SI4032_TX_FREQUENCY_APRS_1200, .tx_power = RADIO_SI4032_TX_POWER, @@ -31,7 +33,31 @@ radio_transmit_entry radio_transmit_schedule[] = { .fsk_encoder_api = &bell_fsk_encoder_api, }, { - .enabled = false, + .enabled = RADIO_SI4032_TX_HORUS_V1, + .radio_type = RADIO_TYPE_SI4032, + .data_mode = RADIO_DATA_MODE_HORUS_V1, + .time_sync_seconds = 1, + .time_sync_seconds_offset = 0, + .frequency = RADIO_SI4032_TX_FREQUENCY_HORUS_V1, + .tx_power = RADIO_SI4032_TX_POWER, + .symbol_rate = HORUS_V1_BAUD_RATE, + .payload_encoder = &radio_horus_v1_payload_encoder, + .fsk_encoder_api = &mfsk_fsk_encoder_api, + }, + { + .enabled = RADIO_SI5351_TX_WSPR, + .radio_type = RADIO_TYPE_SI5351, + .time_sync_seconds = 120, + .time_sync_seconds_offset = 1, + .data_mode = RADIO_DATA_MODE_WSPR, + .frequency = RADIO_SI5351_TX_FREQUENCY_WSPR, + .tx_power = RADIO_SI5351_TX_POWER, + .payload_encoder = &radio_wspr_payload_encoder, + .fsk_encoder_api = &jtencode_fsk_encoder_api, + .jtencode_mode_type = JTENCODE_MODE_WSPR, + }, + { + .enabled = RADIO_SI5351_TX_FT8, .radio_type = RADIO_TYPE_SI5351, .data_mode = RADIO_DATA_MODE_FT8, .time_sync_seconds = 15, @@ -43,7 +69,7 @@ radio_transmit_entry radio_transmit_schedule[] = { .jtencode_mode_type = JTENCODE_MODE_FT8, }, { - .enabled = false, + .enabled = RADIO_SI5351_TX_JT9, .radio_type = RADIO_TYPE_SI5351, .data_mode = RADIO_DATA_MODE_JT9, .time_sync_seconds = 60, @@ -55,7 +81,7 @@ radio_transmit_entry radio_transmit_schedule[] = { .jtencode_mode_type = JTENCODE_MODE_JT9, }, { - .enabled = false, + .enabled = RADIO_SI5351_TX_JT4, .radio_type = RADIO_TYPE_SI5351, .data_mode = RADIO_DATA_MODE_JT4, .time_sync_seconds = 60, @@ -67,7 +93,7 @@ radio_transmit_entry radio_transmit_schedule[] = { .jtencode_mode_type = JTENCODE_MODE_JT4, }, { - .enabled = false, + .enabled = RADIO_SI5351_TX_JT65, .radio_type = RADIO_TYPE_SI5351, .data_mode = RADIO_DATA_MODE_JT65, .time_sync_seconds = 60, @@ -79,7 +105,7 @@ radio_transmit_entry radio_transmit_schedule[] = { .jtencode_mode_type = JTENCODE_MODE_JT65, }, { - .enabled = true, + .enabled = RADIO_SI5351_TX_FSQ, .radio_type = RADIO_TYPE_SI5351, .data_mode = RADIO_DATA_MODE_FSQ_6, .time_sync_seconds = 0, @@ -90,18 +116,6 @@ radio_transmit_entry radio_transmit_schedule[] = { .fsk_encoder_api = &jtencode_fsk_encoder_api, .jtencode_mode_type = JTENCODE_MODE_FSQ_6, }, - { - .enabled = false, - .radio_type = RADIO_TYPE_SI5351, - .time_sync_seconds = 120, - .time_sync_seconds_offset = 1, - .data_mode = RADIO_DATA_MODE_WSPR, - .frequency = RADIO_SI5351_TX_FREQUENCY_WSPR, - .tx_power = RADIO_SI5351_TX_POWER, - .payload_encoder = &radio_wspr_payload_encoder, - .fsk_encoder_api = &jtencode_fsk_encoder_api, - .jtencode_mode_type = JTENCODE_MODE_WSPR, - }, { .end = true, } @@ -141,6 +155,7 @@ radio_module_state radio_shared_state = { .radio_symbol_count_loop = 0, .radio_dma_transfer_active = false, .radio_manual_transmit_active = false, + .radio_interrupt_transmit_active = false, .radio_current_fsk_tones = NULL, .radio_current_fsk_tone_count = 0, @@ -190,14 +205,9 @@ static bool radio_start_transmit(radio_transmit_entry *entry) #ifdef SEMIHOSTING_ENABLE log_info("Payload: "); - for (uint16_t i = 0; i < radio_current_payload_length; i++) { - char c = radio_current_payload[i]; - if (c >= 0x20 && c <= 0x7e) { - log_info("%c", c); - } else { - log_info(" [%02X] ", c); - } - } + log_bytes_hex(radio_current_payload_length, (char *) radio_current_payload); + log_info("\n "); + log_bytes(radio_current_payload_length, (char *) radio_current_payload); log_info("\n"); #endif @@ -214,6 +224,13 @@ static bool radio_start_transmit(radio_transmit_entry *entry) &radio_shared_state.radio_current_fsk_tones); entry->fsk_encoder_api->set_data(&entry->fsk_encoder, radio_current_payload_length, radio_current_payload); break; + case RADIO_DATA_MODE_HORUS_V1: + mfsk_encoder_new(&entry->fsk_encoder, MFSK_4, entry->symbol_rate, 100); + radio_shared_state.radio_current_symbol_rate = entry->fsk_encoder_api->get_symbol_rate(&entry->fsk_encoder); + entry->fsk_encoder_api->get_tones(&entry->fsk_encoder, &radio_shared_state.radio_current_fsk_tone_count, + &radio_shared_state.radio_current_fsk_tones); + entry->fsk_encoder_api->set_data(&entry->fsk_encoder, radio_current_payload_length, radio_current_payload); + break; case RADIO_DATA_MODE_WSPR: case RADIO_DATA_MODE_FT8: case RADIO_DATA_MODE_JT65: @@ -231,7 +248,8 @@ static bool radio_start_transmit(radio_transmit_entry *entry) strlcpy(locator, current_telemetry_data.locator, 4 + 1); } - success = jtencode_encoder_new(&entry->fsk_encoder, sizeof(radio_current_symbol_data), radio_current_symbol_data, + success = jtencode_encoder_new(&entry->fsk_encoder, sizeof(radio_current_symbol_data), + radio_current_symbol_data, entry->jtencode_mode_type, WSPR_CALLSIGN, locator, WSPR_DBM, FSQ_CALLSIGN_FROM); if (!success) { return false; @@ -311,6 +329,7 @@ static bool radio_stop_transmit(radio_transmit_entry *entry) radio_reset_transmit_state(); radio_shared_state.radio_manual_transmit_active = false; + radio_shared_state.radio_interrupt_transmit_active = false; radio_shared_state.radio_dma_transfer_active = false; switch (entry->data_mode) { @@ -321,6 +340,9 @@ static bool radio_stop_transmit(radio_transmit_entry *entry) case RADIO_DATA_MODE_APRS_1200: bell_encoder_destroy(&entry->fsk_encoder); break; + case RADIO_DATA_MODE_HORUS_V1: + mfsk_encoder_destroy(&entry->fsk_encoder); + break; case RADIO_DATA_MODE_WSPR: case RADIO_DATA_MODE_FT8: case RADIO_DATA_MODE_JT65: @@ -386,7 +408,8 @@ static void radio_next_transmit_entry() void radio_handle_timer_tick() { - if (!radio_module_initialized || radio_shared_state.radio_dma_transfer_active || radio_shared_state.radio_manual_transmit_active) { + if (!radio_module_initialized || radio_shared_state.radio_dma_transfer_active + || radio_shared_state.radio_manual_transmit_active || radio_shared_state.radio_interrupt_transmit_active) { return; } @@ -407,6 +430,11 @@ void radio_handle_timer_tick() } } +void radio_handle_data_timer_tick() +{ + radio_handle_data_timer_si4032(); +} + bool radio_handle_time_sync() { // TODO: How to poll GPS time? @@ -447,7 +475,7 @@ bool radio_handle_time_sync() bool is_scheduled_time = time_sync_period_millis < RADIO_TIME_SYNC_THRESHOLD_MS; - //log_info("time with offset: %lu, sync period: %lu, scheduled: %d\n", time_with_offset_millis, time_sync_period_millis, is_scheduled_time); + log_debug("Time with offset: %lu, sync period: %lu, scheduled: %d\n", time_with_offset_millis, time_sync_period_millis, is_scheduled_time); if (!is_scheduled_time) { log_info("Time: %lu, GPS fix: %d - Waiting for time sync at every %d seconds with offset of %d\n", time_millis, @@ -527,7 +555,7 @@ void radio_handle_main_loop() if (radio_shared_state.radio_transmission_active && radio_shared_state.radio_transmit_next_symbol_flag) { radio_shared_state.radio_transmit_next_symbol_flag = false; - if (!radio_shared_state.radio_manual_transmit_active) { + if (!radio_shared_state.radio_manual_transmit_active && !radio_shared_state.radio_interrupt_transmit_active) { bool success = radio_transmit_symbol(radio_current_transmit_entry); if (success) { if (first_symbol) { @@ -571,6 +599,9 @@ void radio_init() case RADIO_DATA_MODE_APRS_1200: entry->messages = aprs_comment_templates; break; + case RADIO_DATA_MODE_HORUS_V1: + // No messages + break; case RADIO_DATA_MODE_FT8: case RADIO_DATA_MODE_JT65: case RADIO_DATA_MODE_JT9: @@ -589,7 +620,7 @@ void radio_init() default: break; } - if (entry-> messages != NULL) { + if (entry->messages != NULL) { for (entry->message_count = 0; entry->messages[entry->message_count] != NULL; entry->message_count++); } } diff --git a/src/radio.h b/src/radio.h index f56c857..f2e5ab3 100644 --- a/src/radio.h +++ b/src/radio.h @@ -3,6 +3,7 @@ void radio_init(); void radio_handle_timer_tick(); +void radio_handle_data_timer_tick(); void radio_handle_main_loop(); #endif diff --git a/src/radio_internal.h b/src/radio_internal.h index 999c58f..57d717a 100644 --- a/src/radio_internal.h +++ b/src/radio_internal.h @@ -16,6 +16,7 @@ typedef enum _radio_data_mode { RADIO_DATA_MODE_CW = 1, RADIO_DATA_MODE_RTTY, RADIO_DATA_MODE_APRS_1200, + RADIO_DATA_MODE_HORUS_V1, RADIO_DATA_MODE_WSPR, RADIO_DATA_MODE_FT8, RADIO_DATA_MODE_JT65, @@ -64,6 +65,7 @@ typedef struct _radio_module_state { volatile bool radio_dma_transfer_active; volatile bool radio_manual_transmit_active; + volatile bool radio_interrupt_transmit_active; fsk_tone *radio_current_fsk_tones; int8_t radio_current_fsk_tone_count; diff --git a/src/radio_payload_aprs.c b/src/radio_payload_aprs.c index af2e91e..be91c89 100644 --- a/src/radio_payload_aprs.c +++ b/src/radio_payload_aprs.c @@ -7,10 +7,10 @@ #include "log.h" #include "radio_payload_aprs.h" -uint8_t aprs_packet[RADIO_PAYLOAD_MAX_LENGTH]; - uint16_t radio_aprs_encode(uint8_t *payload, uint16_t length, telemetry_data *telemetry_data, char *message) { + uint8_t aprs_packet[RADIO_PAYLOAD_MAX_LENGTH]; + aprs_generate_position(aprs_packet, sizeof(aprs_packet), telemetry_data, APRS_SYMBOL_TABLE, APRS_SYMBOL, false, message); diff --git a/src/radio_payload_horus_v1.c b/src/radio_payload_horus_v1.c new file mode 100644 index 0000000..a2261c0 --- /dev/null +++ b/src/radio_payload_horus_v1.c @@ -0,0 +1,39 @@ +#include + +#include "codecs/horus/horus_packet_v1.h" +#include "codecs/horus/horus_l2.h" +#include "config.h" +#include "telemetry.h" +#include "radio_payload_horus_v1.h" + +#include "log.h" + +uint16_t radio_horus_v1_encode(uint8_t *payload, uint16_t length, telemetry_data *telemetry_data, char *message) +{ + horus_packet_v1 horus_packet; + + size_t packet_length = horus_packet_v1_create((uint8_t *) &horus_packet, sizeof(horus_packet), + telemetry_data, HORUS_V1_PAYLOAD_ID); + +#ifdef SEMIHOSTING_ENABLE + log_info("Horus V1 packet: "); + log_bytes_hex((int) packet_length, (char *) &horus_packet); + log_info("\n"); +#endif + + // Preamble to help the decoder lock-on after a quiet period. + for (int i = 0; i < HORUS_V1_PREAMBLE_LENGTH; i++) { + payload[i] = 0x1b; + } + + // Encode the packet, and write into the mfsk buffer. + int encoded_length = horus_l2_encode_tx_packet( + (unsigned char *) payload + HORUS_V1_PREAMBLE_LENGTH, + (unsigned char *) &horus_packet, (int) packet_length); + + return encoded_length + HORUS_V1_PREAMBLE_LENGTH; +} + +payload_encoder radio_horus_v1_payload_encoder = { + .encode = radio_horus_v1_encode, +}; diff --git a/src/radio_payload_horus_v1.h b/src/radio_payload_horus_v1.h new file mode 100644 index 0000000..0ac589e --- /dev/null +++ b/src/radio_payload_horus_v1.h @@ -0,0 +1,8 @@ +#ifndef __RADIO_PAYLOAD_HORUS_V1_H +#define __RADIO_PAYLOAD_HORUS_V1_H + +#include "payload.h" + +extern payload_encoder radio_horus_v1_payload_encoder; + +#endif diff --git a/src/radio_si4032.c b/src/radio_si4032.c index bf2de49..870b042 100644 --- a/src/radio_si4032.c +++ b/src/radio_si4032.c @@ -5,10 +5,12 @@ #include "hal/spi.h" #include "hal/pwm.h" #include "hal/delay.h" +#include "hal/datatimer.h" #include "drivers/si4032/si4032.h" #include "log.h" #include "radio_si4032.h" +#include "codecs/mfsk/mfsk.h" /** * I have attempted to implement Bell 202 frequency generation using hardware DMA and PWM, but have failed to generate @@ -43,7 +45,7 @@ bool radio_start_transmit_si4032(radio_transmit_entry *entry, radio_module_state case RADIO_DATA_MODE_CW: frequency_offset = 1; modulation_type = SI4032_MODULATION_TYPE_OOK; - use_direct_mode = true; + use_direct_mode = false; break; case RADIO_DATA_MODE_RTTY: frequency_offset = 0; @@ -55,9 +57,19 @@ bool radio_start_transmit_si4032(radio_transmit_entry *entry, radio_module_state modulation_type = SI4032_MODULATION_TYPE_FSK; use_direct_mode = true; if (si4032_use_dma) { + pwm_data_timer_init(); radio_si4032_fill_pwm_buffer(0, PWM_TIMER_DMA_BUFFER_SIZE, pwm_timer_dma_buffer); } break; + case RADIO_DATA_MODE_HORUS_V1: { + fsk_tone *idle_tone = mfsk_get_idle_tone(&entry->fsk_encoder); + frequency_offset = (uint16_t) idle_tone->index + HORUS_V1_FREQUENCY_OFFSET; + modulation_type = SI4032_MODULATION_TYPE_OOK; + use_direct_mode = false; + + data_timer_init(entry->fsk_encoder_api->get_symbol_rate(&entry->fsk_encoder)); + break; + } default: return false; } @@ -87,6 +99,10 @@ bool radio_start_transmit_si4032(radio_transmit_entry *entry, radio_module_state shared_state->radio_manual_transmit_active = true; } break; + case RADIO_DATA_MODE_HORUS_V1: + system_disable_tick(); + shared_state->radio_interrupt_transmit_active = true; + break; default: break; } @@ -147,7 +163,7 @@ static void radio_handle_main_loop_manual_si4032(radio_transmit_entry *entry, ra pwm_timer_set_frequency(precalculated_pwm_periods[tone_index]); shared_state->radio_symbol_count_loop++; delay_us(symbol_delay_bell_202_1200bps_us); - }; + } radio_si4032_state_change = false; shared_state->radio_transmission_finished = true; @@ -162,7 +178,7 @@ static void radio_handle_main_loop_manual_si4032(radio_transmit_entry *entry, ra void radio_handle_main_loop_si4032(radio_transmit_entry *entry, radio_module_state *shared_state) { - if (entry->radio_type != RADIO_TYPE_SI4032) { + if (entry->radio_type != RADIO_TYPE_SI4032 || shared_state->radio_interrupt_transmit_active) { return; } @@ -178,15 +194,44 @@ void radio_handle_main_loop_si4032(radio_transmit_entry *entry, radio_module_sta } } +inline void radio_handle_data_timer_si4032() +{ + if (radio_current_transmit_entry->radio_type != RADIO_TYPE_SI4032 || !radio_shared_state.radio_interrupt_transmit_active) { + return; + } + + switch (radio_current_transmit_entry->data_mode) { + case RADIO_DATA_MODE_HORUS_V1: { + fsk_encoder_api *fsk_encoder_api = radio_current_transmit_entry->fsk_encoder_api; + fsk_encoder *fsk_enc = &radio_current_transmit_entry->fsk_encoder; + int8_t tone_index; + + tone_index = fsk_encoder_api->next_tone(fsk_enc); + if (tone_index < 0) { + log_info("Horus V1 TX finished\n"); + radio_shared_state.radio_interrupt_transmit_active = false; + radio_shared_state.radio_transmission_finished = true; + system_enable_tick(); + break; + } + + si4032_set_frequency_offset_small(tone_index + HORUS_V1_FREQUENCY_OFFSET); + radio_shared_state.radio_symbol_count_interrupt++; + break; + } + default: + break; + } +} + bool radio_stop_transmit_si4032(radio_transmit_entry *entry, radio_module_state *shared_state) { bool use_direct_mode = false; switch (entry->data_mode) { case RADIO_DATA_MODE_CW: - use_direct_mode = true; - break; case RADIO_DATA_MODE_RTTY: + case RADIO_DATA_MODE_HORUS_V1: use_direct_mode = false; break; case RADIO_DATA_MODE_APRS_1200: @@ -208,9 +253,14 @@ bool radio_stop_transmit_si4032(radio_transmit_entry *entry, radio_module_state switch (entry->data_mode) { case RADIO_DATA_MODE_APRS_1200: if (si4032_use_dma) { + pwm_data_timer_uninit(); system_enable_tick(); } break; + case RADIO_DATA_MODE_HORUS_V1: + data_timer_uninit(); + system_enable_tick(); + break; default: break; } diff --git a/src/radio_si4032.h b/src/radio_si4032.h index 56f1902..aea48a3 100644 --- a/src/radio_si4032.h +++ b/src/radio_si4032.h @@ -6,6 +6,7 @@ bool radio_start_transmit_si4032(radio_transmit_entry *entry, radio_module_state *shared_state); bool radio_transmit_symbol_si4032(radio_transmit_entry *entry, radio_module_state *shared_state); void radio_handle_main_loop_si4032(radio_transmit_entry *entry, radio_module_state *shared_state); +void radio_handle_data_timer_si4032(); bool radio_stop_transmit_si4032(radio_transmit_entry *entry, radio_module_state *shared_state); void radio_init_si4032(); diff --git a/src/telemetry.h b/src/telemetry.h index 141a2db..be9d5b2 100644 --- a/src/telemetry.h +++ b/src/telemetry.h @@ -21,6 +21,6 @@ typedef struct _telemetry_data { char locator[LOCATOR_PAIR_COUNT_FULL * 2 + 1]; } telemetry_data; -void telemetry_collect(); +void telemetry_collect(telemetry_data *data); #endif diff --git a/src/template.c b/src/template.c index ec65e88..9cd89f2 100644 --- a/src/template.c +++ b/src/template.c @@ -41,23 +41,23 @@ size_t template_replace(char *dest, size_t dest_len, char *src, telemetry_data * strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$bu", replacement); - snprintf(replacement, sizeof(replacement), "%d", data->temperature_celsius_100 / 100); + snprintf(replacement, sizeof(replacement), "%d", (int) data->temperature_celsius_100 / 100); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$te", replacement); - snprintf(replacement, sizeof(replacement), "%d", data->internal_temperature_celsius_100 / 100); + snprintf(replacement, sizeof(replacement), "%d", (int) data->internal_temperature_celsius_100 / 100); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$ti", replacement); - snprintf(replacement, sizeof(replacement), "%d", data->humidity_percentage_100 / 100); + snprintf(replacement, sizeof(replacement), "%d", (int) data->humidity_percentage_100 / 100); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$hu", replacement); - snprintf(replacement, sizeof(replacement), "%d", data->pressure_mbar_100 / 100); + snprintf(replacement, sizeof(replacement), "%d", (int) data->pressure_mbar_100 / 100); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$pr", replacement); - snprintf(replacement, sizeof(replacement), "%u", data->gps.time_of_week_millis); + snprintf(replacement, sizeof(replacement), "%u", (unsigned int) data->gps.time_of_week_millis); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$tow", replacement); @@ -77,15 +77,15 @@ size_t template_replace(char *dest, size_t dest_len, char *src, telemetry_data * strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$sv", replacement); - snprintf(replacement, sizeof(replacement), "%05d", data->gps.latitude_degrees_1000000 / 10000); + snprintf(replacement, sizeof(replacement), "%05d", (int) data->gps.latitude_degrees_1000000 / 10000); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$lat", replacement); - snprintf(replacement, sizeof(replacement), "%05d", data->gps.longitude_degrees_1000000 / 10000); + snprintf(replacement, sizeof(replacement), "%05d", (int) data->gps.longitude_degrees_1000000 / 10000); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$lon", replacement); - snprintf(replacement, sizeof(replacement), "%d", data->gps.altitude_mm / 1000); + snprintf(replacement, sizeof(replacement), "%d", (int) data->gps.altitude_mm / 1000); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$alt", replacement); @@ -93,11 +93,11 @@ size_t template_replace(char *dest, size_t dest_len, char *src, telemetry_data * strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$gs", replacement); - snprintf(replacement, sizeof(replacement), "%d", data->gps.climb_cm_per_second / 100); + snprintf(replacement, sizeof(replacement), "%d", (int) data->gps.climb_cm_per_second / 100); strlcpy(temp, dest, dest_len); str_replace(dest, dest_len, temp, "$cl", replacement); - snprintf(replacement, sizeof(replacement), "%03d", data->gps.heading_degrees_100000 / 100000); + snprintf(replacement, sizeof(replacement), "%03d", (int) data->gps.heading_degrees_100000 / 100000); strlcpy(temp, dest, dest_len); size_t len = str_replace(dest, dest_len, temp, "$he", replacement);