From bcfe9b742c3808ee42afac3b84e32489b75e5358 Mon Sep 17 00:00:00 2001 From: baobrien Date: Sun, 17 Sep 2017 18:09:41 +0000 Subject: [PATCH] Pushed forward on TDMA RX git-svn-id: https://svn.code.sf.net/p/freetel/code@3366 01035d8c-6547-0410-b346-abe4f91aad63 --- codec2-dev/src/freedv_vhf_framing.c | 33 +++++- codec2-dev/src/freedv_vhf_framing.h | 49 +++++---- codec2-dev/src/fsk.c | 10 ++ codec2-dev/src/tdma.c | 161 +++++++++++++++++++++++++--- codec2-dev/src/tdma.h | 32 +++--- 5 files changed, 231 insertions(+), 54 deletions(-) diff --git a/codec2-dev/src/freedv_vhf_framing.c b/codec2-dev/src/freedv_vhf_framing.c index da3901ca..8eae3fe4 100644 --- a/codec2-dev/src/freedv_vhf_framing.c +++ b/codec2-dev/src/freedv_vhf_framing.c @@ -398,6 +398,30 @@ int fvhff_synchronized(struct freedv_vhf_deframer * def){ return (def->state) == ST_SYNC; } +/* Search for a complete UW in a buffer of bits */ +size_t fvhff_search_uw(const uint8_t bits[],size_t nbits, + const uint8_t uw[], size_t uw_len, + size_t * delta_out){ + + size_t ibits,iuw; + size_t delta_min = uw_len; + size_t delta; + size_t offset_min = 0; + /* Walk through buffer bits */ + for(ibits = 0; ibits < nbits-uw_len; ibits++){ + delta = 0; + for(iuw = 0; iuw < uw_len; iuw++){ + if(bits[ibits+iuw] != uw[iuw]) delta++; + } + if( delta < delta_min ){ + delta_min = delta; + offset_min = ibits; + } + } + if(delta_out != NULL) *delta_out = delta_min; + return offset_min; +} + /* See if the UW is where it should be, to within a tolerance, in a bit buffer */ static int fvhff_match_uw(struct freedv_vhf_deframer * def,uint8_t bits[],int tol,int *rdiff, enum frame_payload_type *pt){ int frame_type = def->ftype; @@ -460,7 +484,8 @@ static int fvhff_match_uw(struct freedv_vhf_deframer * def,uint8_t bits[],int to return r; } -static void fvhff_extract_frame_voice(struct freedv_vhf_deframer * def,uint8_t bits[],uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[]){ +static void fvhff_extract_frame_voice(struct freedv_vhf_deframer * def,uint8_t bits[], + uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[]){ int frame_type = def->ftype; int bitptr = def->bitptr; int frame_size = def->frame_size; @@ -700,7 +725,8 @@ static void fvhff_extract_frame_data(struct freedv_vhf_deframer * def,uint8_t bi } } -static void fvhff_extract_frame(struct freedv_vhf_deframer * def,uint8_t bits[],uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[],enum frame_payload_type pt){ +static void fvhff_extract_frame(struct freedv_vhf_deframer * def,uint8_t bits[],uint8_t codec2_out[], + uint8_t proto_out[],uint8_t vc_out[],enum frame_payload_type pt){ switch (pt) { case FRAME_PAYLOAD_TYPE_VOICE: fvhff_extract_frame_voice(def, bits, codec2_out, proto_out, vc_out); @@ -714,7 +740,8 @@ static void fvhff_extract_frame(struct freedv_vhf_deframer * def,uint8_t bits[], /* * Try to find the UW and extract codec/proto/vc bits in def->frame_size bits */ -int fvhff_deframe_bits(struct freedv_vhf_deframer * def,uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[],uint8_t bits_in[]){ +int fvhff_deframe_bits(struct freedv_vhf_deframer * def,uint8_t codec2_out[],uint8_t proto_out[], + uint8_t vc_out[],uint8_t bits_in[]){ uint8_t * strbits = def->bits; uint8_t * invbits = def->invbits; uint8_t * bits; diff --git a/codec2-dev/src/freedv_vhf_framing.h b/codec2-dev/src/freedv_vhf_framing.h index 85f6e7b9..d73ee1b2 100644 --- a/codec2-dev/src/freedv_vhf_framing.h +++ b/codec2-dev/src/freedv_vhf_framing.h @@ -33,16 +33,16 @@ along with this program; if not, see . */ -#ifndef _FREEDV_VHF_FRAMING_H -#define _FREEDV_VHF_FRAMING_H - -#include "freedv_data_channel.h" - -/* Standard frame type */ +#ifndef _FREEDV_VHF_FRAMING_H +#define _FREEDV_VHF_FRAMING_H + +#include "freedv_data_channel.h" + +/* Standard frame type */ #define FREEDV_VHF_FRAME_A 1 /* 2400A/B Frame */ #define FREEDV_HF_FRAME_B 2 /* 800XA Frame */ -#define FREEDV_VHF_FRAME_AT 3 /* 4800T Frame */ - +#define FREEDV_VHF_FRAME_AT 3 /* 4800T Frame */ + struct freedv_vhf_deframer { int ftype; /* Type of frame to be looking for */ int state; /* State of deframer */ @@ -51,20 +51,20 @@ struct freedv_vhf_deframer { int bitptr; /* Pointer into circular bit buffer */ int miss_cnt; /* How many UWs have been missed */ - int last_uw; /* How many bits since the last UW? */ + int last_uw; /* How many bits since the last UW? */ int frame_size; /* How big is a frame? */ - int uw_size; /* How big is the UW */ + int uw_size; /* How big is the UW */ int on_inv_bits; /* Are we using the inverted bits? */ - int sym_size; /* How many bits in a modem symbol */ + int sym_size; /* How many bits in a modem symbol */ float ber_est; /* Bit error rate estimate */ int total_uw_bits; /* Total RX-ed bits of UW */ int total_uw_err; /* Total errors in UW bits */ - - struct freedv_data_channel *fdc; -}; - -/* Init and allocate memory for a freedv-vhf framer/deframer */ + + struct freedv_data_channel *fdc; +}; + +/* Init and allocate memory for a freedv-vhf framer/deframer */ struct freedv_vhf_deframer * fvhff_create_deframer(uint8_t frame_type,int enable_bit_flip); /* Get size of various frame parameters */ @@ -79,15 +79,20 @@ int fvhff_get_varicode_size(struct freedv_vhf_deframer * def); /* Free the memory used by a freedv-vhf framer/deframer */ void fvhff_destroy_deframer(struct freedv_vhf_deframer * def); - -/* Place codec and other bits into a frame */ -void fvhff_frame_bits(int frame_type,uint8_t bits_out[],uint8_t codec2_in[],uint8_t proto_in[],uint8_t vc_in[]); + +/* Place codec and other bits into a frame */ +void fvhff_frame_bits(int frame_type,uint8_t bits_out[],uint8_t codec2_in[],uint8_t proto_in[],uint8_t vc_in[]); void fvhff_frame_data_bits(struct freedv_vhf_deframer * def, int frame_type,uint8_t bits_out[]); - -/* Find and extract frames from a stream of bits */ -int fvhff_deframe_bits(struct freedv_vhf_deframer * def,uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[],uint8_t bits_in[]); + +/* Find and extract frames from a stream of bits */ +int fvhff_deframe_bits(struct freedv_vhf_deframer * def,uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[],uint8_t bits_in[]); /* Is the de-framer synchronized? */ int fvhff_synchronized(struct freedv_vhf_deframer * def); +/* Search for a complete UW in a buffer of bits */ +size_t fvhff_search_uw(const uint8_t bits[],size_t nbits, + const uint8_t uw[], size_t uw_len, + size_t * delta_out); + #endif //_FREEDV_VHF_FRAMING_H diff --git a/codec2-dev/src/fsk.c b/codec2-dev/src/fsk.c index aef4d984..86d4fb7a 100644 --- a/codec2-dev/src/fsk.c +++ b/codec2-dev/src/fsk.c @@ -668,6 +668,10 @@ void fsk2_demod(struct FSK *fsk, uint8_t rx_bits[], float rx_sd[], COMP fsk_in[] #ifdef MODEMPROBE_ENABLE char mp_name_tmp[20]; /* Temporary string for modem probe trace names */ #endif + + //for(size_t jj = 0; jjf_est[m])/(float)(Fs))); phi_c[m] = cmult(dphi[m],phi_c[m]); + //fprintf(stderr,"F%d = %f",m,fsk->f_est[m]); /* Figure out how much to nudge each sample downmixer for every sample */ dphi[m] = comp_exp_j(2*M_PI*((fsk->f_est[m])/(float)(Fs))); @@ -823,6 +828,7 @@ void fsk2_demod(struct FSK *fsk, uint8_t rx_bits[], float rx_sd[], COMP fsk_in[] /* Spin the oscillator for the magic line shift */ phi_ft = cmult(phi_ft,dphift); } + //fprintf(stderr,"t_c: %f+%f i\n",t_c.real,t_c.imag); /* Get the magic angle */ norm_rx_timing = atan2f(t_c.imag,t_c.real)/(2*M_PI); rx_timing = norm_rx_timing*(float)P; @@ -857,6 +863,8 @@ void fsk2_demod(struct FSK *fsk, uint8_t rx_bits[], float rx_sd[], COMP fsk_in[] int low_sample = (int)floorf(rx_timing); float fract = rx_timing - (float)low_sample; int high_sample = (int)ceilf(rx_timing); + + //fprintf(stderr,"rx_timing: %f %f\n",rx_timing,fract); /* Vars for finding the max-of-4 for each bit */ float tmax[M]; @@ -871,6 +879,8 @@ void fsk2_demod(struct FSK *fsk, uint8_t rx_bits[], float rx_sd[], COMP fsk_in[] for(i=0; i #include #include +#include + + +static const uint8_t TDMA_UW_V[] = {0,1,1,0,0,1,1,1, + 1,0,1,0,1,1,0,1}; struct TDMA_MODEM * tdma_create(struct TDMA_MODE_SETTINGS mode){ struct TDMA_MODEM * tdma; @@ -44,7 +49,10 @@ struct TDMA_MODEM * tdma_create(struct TDMA_MODE_SETTINGS mode){ u32 M = mode.fsk_m; u32 P = Fs/Rs; u32 Ts = Fs/Rs; + COMP * samp_buffer = NULL; + size_t i; + assert( (Fs%Rs)==0 ); assert( M==2 || M==4); @@ -64,15 +72,19 @@ struct TDMA_MODEM * tdma_create(struct TDMA_MODE_SETTINGS mode){ tdma->settings = mode; tdma->state = no_sync; tdma->sample_sync_offset = 0; + tdma->slot_cur = 0; /* Allocate buffer for incoming samples */ /* TODO: We may only need a single slot's worth of samps -- look into this */ - COMP * samp_buffer = (COMP *) malloc(sizeof(COMP)*slot_size*Ts*n_slots); + samp_buffer = (COMP *) malloc(sizeof(COMP)*slot_size*Ts*n_slots); if(samp_buffer == NULL) goto cleanup_bad_alloc; tdma->sample_buffer = samp_buffer; + for(i=0; isample_buffer[i].real = 0; + tdma->sample_buffer[i].imag = 0; + } - size_t i; struct TDMA_SLOT * slot; struct TDMA_SLOT * last_slot; struct FSK * slot_fsk; @@ -85,8 +97,8 @@ struct TDMA_MODEM * tdma_create(struct TDMA_MODE_SETTINGS mode){ slot->next_slot = last_slot; slot->slot_local_frame_offset = 0; slot->state = rx_no_sync; - //slot_fsk = fsk_create_hbr(Fs,Rs,P,M,Rs,Rs); - slot_fsk = NULL; + slot_fsk = fsk_create_hbr(Fs,Rs,P,M,Rs,Rs); + if(slot_fsk == NULL) goto cleanup_bad_alloc; fsk_enable_burst_mode(slot_fsk, slot_size); @@ -94,8 +106,6 @@ struct TDMA_MODEM * tdma_create(struct TDMA_MODE_SETTINGS mode){ slot->fsk = slot_fsk; last_slot = slot; } - goto cleanup_bad_alloc; - /* TODO: Allocate slot modems. Get pilot detection working first */ return tdma; @@ -130,21 +140,107 @@ void tdma_print_stuff(struct TDMA_MODEM * tdma){ printf("sync_offset: %ld\n",tdma->sample_sync_offset); } -void tdma_destroy(struct TDMA_MODEM * tdma){ +void tdma_destroy(tdma_t * tdma){ /* TODO: Free slot modems (need to create them first) */ fsk_destroy(tdma->fsk_pilot); free(tdma->sample_buffer); free(tdma); } -u32 tdma_get_N(struct TDMA_MODEM * tdma){ +u32 tdma_get_N(tdma_t * tdma){ u32 slot_size = tdma->settings.slot_size; u32 Fs = tdma->settings.samp_rate; u32 Rs = tdma->settings.sym_rate; return slot_size * (Fs/Rs); } -void tdma_rx_no_sync(struct TDMA_MODEM * tdma, COMP * samps, u64 timestamp){ +static slot_t * tdma_get_slot(tdma_t * tdma, u32 slot_idx){ + /* Don't try and index beyond the end */ + if(slot_idx >= tdma->settings.n_slots) return NULL; + + size_t i; + slot_t * cur = tdma->slots; + for(i = 0; i < slot_idx; i++){ + /* Don't break */ + if(cur == NULL) return NULL; + cur = cur->next_slot; + } + return cur; +} + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" + +void tdma_demod_end_slot(tdma_t * tdma,u32 slot_idx){ + + struct TDMA_MODE_SETTINGS mode = tdma->settings; + u32 Rs = mode.sym_rate; + u32 Fs = mode.samp_rate; + u32 slot_size = mode.slot_size; + u32 frame_size = mode.frame_size; + u32 n_slots = mode.n_slots; + u32 M = mode.fsk_m; + u32 Ts = Fs/Rs; + u32 bits_per_sym = M==2?1:2; + u32 slot_samps = slot_size*Ts; + size_t nbits = slot_size*bits_per_sym; + u32 frame_bits = frame_size*bits_per_sym; + + u8 bit_buf[nbits]; + /* Samples that belong to this frame */ + COMP frame_samps[slot_size*Ts]; + COMP * sample_buffer = tdma->sample_buffer; + slot_t * slot = tdma_get_slot(tdma,slot_idx); + //slot_t * slot = tdma->slots; + struct FSK * fsk = slot->fsk; + + int nin = fsk_nin(fsk); + + /* Pull out the frame and demod */ + size_t move_samps = slot_samps*sizeof(COMP); + uintptr_t move_from = ((uintptr_t)sample_buffer) + (tdma->sample_sync_offset)*sizeof(COMP); + uintptr_t move_to = (uintptr_t)frame_samps; /* Don't really need this, but it's cleaner than doing it all in memmove */ + memcpy((void*)move_to,(void*)move_from,move_samps); + /* Demodulate the frame */ + fsk_demod(fsk,bit_buf,frame_samps); + + size_t delta,off; + off = fvhff_search_uw(bit_buf,nbits,TDMA_UW_V,16,&delta); + u32 f_start = off- (frame_bits-16)/2; + /* Calculate offset (in samps) from start of frame */ + /* Note: FSK outputs one bit from the last batch */ + u32 frame_offset = (f_start-bits_per_sym)*Ts; + + fprintf(stderr,"slot: %d offset: %d delta: %d f1:%.3f\n",slot_idx,off,delta,fsk->f_est[0]); + for(int i=0; ioff && i<=off+16) || i==f_start || i==(f_start+frame_bits)){ + if(bit_buf[i]) fprintf(stderr,"1̲"); + else fprintf(stderr,"0̲"); + } else fprintf(stderr,"%d",bit_buf[i]); + } + fprintf(stderr,"\n"); +} + +/* We got a new slot's worth of samples. Run the slot modem and try to get slot sync */ +/* This will probably also work for the slot_sync state */ +void tdma_rx_pilot_sync(tdma_t * tdma){ + struct TDMA_MODE_SETTINGS mode = tdma->settings; + u32 Rs = mode.sym_rate; + u32 Fs = mode.samp_rate; + u32 slot_size = mode.slot_size; + //u32 frame_size = mode.frame_size; + u32 n_slots = mode.n_slots; + u32 M = mode.fsk_m; + u32 Ts = Fs/Rs; + u32 bits_per_sym = M==2?1:2; + + tdma_demod_end_slot(tdma,tdma->slot_cur); + tdma->slot_cur++; + if(tdma->slot_cur >= n_slots) + tdma->slot_cur = 0; +} + +void tdma_rx_no_sync(tdma_t * tdma, COMP * samps, u64 timestamp){ struct TDMA_MODE_SETTINGS mode = tdma->settings; u32 Rs = mode.sym_rate; u32 Fs = mode.samp_rate; @@ -158,7 +254,8 @@ void tdma_rx_no_sync(struct TDMA_MODEM * tdma, COMP * samps, u64 timestamp){ //Number of bits per pilot modem chunk (half a slot) u32 n_pilot_bits = (slot_size/2)*bits_per_sym; //We look at a full slot for the UW - u8 pilot_bits[n_pilot_bits*2]; + u8 pilot_bits[n_pilot_bits]; + /* Pseudocode: @@ -177,20 +274,52 @@ void tdma_rx_no_sync(struct TDMA_MODEM * tdma, COMP * samps, u64 timestamp){ */ } -void tdma_rx(struct TDMA_MODEM * tdma, COMP * samps,u64 timestamp){ +void tdma_rx(tdma_t * tdma, COMP * samps,u64 timestamp){ + + COMP * sample_buffer = tdma->sample_buffer; + struct TDMA_MODE_SETTINGS mode = tdma->settings; + u32 Rs = mode.sym_rate; + u32 Fs = mode.samp_rate; + u32 slot_size = mode.slot_size; + //u32 frame_size = mode.frame_size; + u32 n_slots = mode.n_slots; + u32 M = mode.fsk_m; + u32 Ts = Fs/Rs; + u32 bits_per_sym = M==2?1:2; + u32 slot_samps = slot_size*Ts; + + /* Copy samples into the local buffer for some reason */ + /* Move the current samps in the buffer back by a slot or so */ + size_t move_samps = slot_samps*sizeof(COMP); + uintptr_t move_from = ((uintptr_t)sample_buffer) + (n_slots-1)*slot_samps*sizeof(COMP); + uintptr_t move_to = (uintptr_t)sample_buffer; /* Don't really need this, but it's cleaner than doing it all in memmove */ + memmove((void*)move_to,(void*)move_from,move_samps); + + move_samps = slot_samps*sizeof(COMP); + move_from = (uintptr_t)samps; + move_to = ((uintptr_t)sample_buffer) + (n_slots-1)*slot_samps*sizeof(COMP); + memcpy((void*)move_to,(void*)move_from,move_samps); + + /* Set the timestamp. Not sure if this makes sense */ + tdma->timestamp = timestamp - (slot_samps*(n_slots-1)); /* Staate machine for TDMA modem */ switch(tdma->state){ - no_sync: + case no_sync: tdma_rx_no_sync(tdma,samps,timestamp); break; - pilot_sync: + case pilot_sync: + tdma_rx_pilot_sync(tdma); break; - slot_sync; + case slot_sync: break; - master_sync: + case master_sync: break; default: break; } -} \ No newline at end of file + tdma->state = pilot_sync; +} + + +#pragma GCC diagnostic pop diff --git a/codec2-dev/src/tdma.h b/codec2-dev/src/tdma.h index c00b222c..de6d78b7 100644 --- a/codec2-dev/src/tdma.h +++ b/codec2-dev/src/tdma.h @@ -46,18 +46,18 @@ typedef float f32; /* The state for an individual slot */ enum slot_state { - rx_no_sync, /* Not synched */ - rx_sync, /* Sunk */ - tx_client, /* TX but timed from a different master */ - tx_master /* TX in master mode */ + rx_no_sync = 0, /* Not synched */ + rx_sync = 1, /* Sunk */ + tx_client = 2, /* TX but timed from a different master */ + tx_master = 3 /* TX in master mode */ }; /* The state of the entire TDMA modem */ enum tdma_state { - no_sync, /* No sync */ - pilot_sync, /* Pilot modem has gotten sync, but slots haven't*/ - slot_sync, /* One or more slots are sunk */ - master_sync, /* This modem is the TDMA master */ + no_sync = 0, /* No sync */ + pilot_sync = 1, /* Pilot modem has gotten sync, but slots haven't*/ + slot_sync = 2, /* One or more slots are sunk */ + master_sync = 3, /* This modem is the TDMA master */ }; /* TDMA frame type */ @@ -83,6 +83,8 @@ struct TDMA_SLOT { }; +typedef struct TDMA_SLOT slot_t; + /* Structure for tracking basic TDMA modem config */ struct TDMA_MODE_SETTINGS { uint32_t sym_rate; /* Modem symbol rate */ @@ -107,24 +109,28 @@ struct TDMA_MODEM { struct TDMA_MODE_SETTINGS settings; /* Basic TDMA config parameters */ COMP * sample_buffer; /* Buffer of incoming samples */ size_t sample_sync_offset; /* Offset into the sample buffer where slot 0 starts */ + uint64_t timestamp; /* Timestamp of oldest sample in samp buffer */ + uint32_t slot_cur; /* Current slot coming in */ }; +typedef struct TDMA_MODEM tdma_t; + /* Allocate and setup a new TDMA modem */ -struct TDMA_MODEM * tdma_create(struct TDMA_MODE_SETTINGS mode); +tdma_t * tdma_create(struct TDMA_MODE_SETTINGS mode); /* Tear down and free a TDMA modem */ -void tdma_destroy(struct TDMA_MODEM * tdma); +void tdma_destroy(tdma_t * tdma); /* Get the number of samples expected by RX for the next cycle */ -u32 tdma_get_N(struct TDMA_MODEM * tdma); +u32 tdma_get_N(tdma_t * tdma); /** Put 1 slot's worth of samples into the TDMA modem TODO: I'm still not entirely sure of what I want the semantics of this to look like */ -void tdma_rx(struct TDMA_MODEM * tdma, COMP * samps,u64 timestamp); +void tdma_rx(tdma_t * tdma, COMP * samps,u64 timestamp); /* Hideous debug function */ -void tdma_print_stuff(struct TDMA_MODEM * tdma); +void tdma_print_stuff(tdma_t * tdma); #endif -- 2.25.1