diff --git a/Makefile b/Makefile
index dd699fbd..bf295216 100644
--- a/Makefile
+++ b/Makefile
@@ -42,7 +42,8 @@ cubesatsim: afsk/ax25.o
 cubesatsim: afsk/ax5043.o
 cubesatsim: TelemEncoding.o
 cubesatsim: main.o
-	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -o cubesatsim -Wall -Wextra -L./ afsk/ax25.o afsk/ax5043.o TelemEncoding.o main.o -lwiringPi -lax5043 -lm
+cubesatsim: codecAO40.o
+	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -o cubesatsim -Wall -Wextra -L./ afsk/ax25.o afsk/ax5043.o TelemEncoding.o codecAO40.o main.o -lwiringPi -lax5043 -lm
 
 telem: telem.o
 	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -o telem -Wall -Wextra -L./ telem.o -lwiringPi 
@@ -51,6 +52,10 @@ TelemEncoding.o: TelemEncoding.c
 TelemEncoding.o: TelemEncoding.h
 	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -Wall -Wextra -c TelemEncoding.c
 
+codecAO40.o: codecAO40.c
+codecAO40.o: codecAO40.h
+	gcc -std=gnu99  $(DEBUG_BEHAVIOR) -Wall -Wextra -c codecAO40.c
+
 ax5043/generated/configcommon.o: ax5043/generated/configcommon.c
 ax5043/generated/configcommon.o: ax5043/generated/configrx.h
 ax5043/generated/configcommon.o: ax5043/generated/configtx.h
diff --git a/asound.conf b/asound.conf
new file mode 100644
index 00000000..783b59f7
--- /dev/null
+++ b/asound.conf
@@ -0,0 +1,46 @@
+pcm.!default {
+    type asym
+    playback.pcm "softvol"
+    capture.pcm "shared_mic"
+}
+
+ctl.!default {
+    type hw
+    card 2
+}
+
+# Playback with software volume and mixing
+pcm.softvol {
+    type softvol
+    slave.pcm "shared_speaker"
+    control {
+        name "Master"
+        card 2
+    }
+}
+
+pcm.shared_speaker {
+    type dmix
+    ipc_key 1024
+    slave {
+        pcm "hw:2,0"
+        rate 48000
+        period_time 0
+        period_size 1024
+        buffer_size 4096
+    }
+}
+
+# Recording with input mixing
+pcm.shared_mic {
+    type dsnoop
+    ipc_key 2048
+    slave {
+        pcm "hw:2,0"
+#        channels 1
+        rate 48000
+        period_time 0
+        period_size 1024
+        buffer_size 4096
+    }
+}
diff --git a/codecAO40.c b/codecAO40.c
new file mode 100644
index 00000000..f1646e46
--- /dev/null
+++ b/codecAO40.c
@@ -0,0 +1,529 @@
+#include <string.h>
+#include "codecAO40.h"
+
+/* ---------------------- */
+/* AO40 Encoder - Decoder */
+/* ---------------------- */
+
+/* Scramble and RS encode 256 byte blocks of data into 5200 bits
+ *  or Descramble and RS decode 5200 bits into the 256 bytes of data
+ *
+ * --------------------------------------------------------------------------
+ * This decoder has evolved extensively through the work of Phil Karn.  It draws
+ * on his own ideas and optimisations, and on the work of others.  The lineage
+ * is as below, and parts of the authors' notices are included here.  (JRM)
+
+ * AO40 encoder / decoder
+ * Copyright 2002 Phil Karn, KA9Q
+ * May be used under the terms of the GNU General Public License (GPL)
+ *
+ * Reed-Solomon coding and decoding
+ * Phil Karn (karn@ka9q.ampr.org) September 1996
+ *
+ * This file is derived from the program "new_rs_erasures.c" by Robert
+ * Morelos-Zaragoza (robert@spectra.eng.hawaii.edu) and Hari Thirumoorthy
+ * (harit@spectra.eng.hawaii.edu), Aug 1995
+ * --------------------------------------------------------------------------
+ *
+ * From the RM-Z & HT program:
+ * The encoding and decoding methods are based on the
+ * book "Error Control Coding: Fundamentals and Applications",
+ * by Lin and Costello, Prentice Hall, 1983, ISBN 0-13-283796-X
+ * Portions of this program are from a Reed-Solomon encoder/decoder
+ * in C, written by Simon Rockliff (simon@augean.ua.oz.au) on 21/9/89.
+ * --------------------------------------------------------------------------
+ *
+ * From the 1989/1991 SR program (also based on Lin and Costello):
+ * This program may be freely modified and/or given to whoever wants it.
+ * A condition of such distribution is that the author's contribution be
+ * acknowledged by his name being left in the comments heading the program,
+ *                               Simon Rockliff, 26th June 1991
+ *
+ */
+
+/* Defines for RS Decoder(s) */
+#ifndef min
+#define min(a,b)        ((a) < (b) ? (a) : (b))
+#endif
+
+/*
+CCodecAO40::CCodecAO40(void)
+{
+}
+
+
+CCodecAO40::~CCodecAO40(void)
+{
+}
+
+*/
+//int CCodecAO40::mod255(int x) {
+int mod255(int x) {
+    while (x >= 255) {
+        x -= 255;
+        x = (x >> 8) + (x & 255);
+    }
+    return x;
+}
+
+//int CCodecAO40::decode_rs_8(char *data, int *eras_pos, int no_eras){
+int decode_rs_8(char *data, int *eras_pos, int no_eras){
+
+    int deg_lambda, el, deg_omega;
+    int i, j, r,k;
+    unsigned char u,q,tmp,num1,num2,den,discr_r;
+    unsigned char lambda[NROOTS+1], s[NROOTS];   /* Err+Eras Locator poly and syndrome poly */
+    unsigned char b[NROOTS+1], t[NROOTS+1], omega[NROOTS+1];
+    unsigned char root[NROOTS], reg[NROOTS+1], loc[NROOTS];
+    int syn_error, count;
+
+    /* form the syndromes; i.e., evaluate data(x) at roots of g(x) */
+    for(i=0;i<NROOTS;i++)
+        s[i] = data[0];
+
+    for(j=1;j<NN;j++){
+        for(i=0;i<NROOTS;i++){
+            if(s[i] == 0){
+                s[i] = data[j];
+            } else {
+                s[i] = data[j] ^ ALPHA_TO[mod255(INDEX_OF[s[i]] + (FCR+i)*PRIM)];
+            }
+        }
+    }
+
+    /* Convert syndromes to index form, checking for nonzero condition */
+    syn_error = 0;
+    for(i=0;i<NROOTS;i++){
+        syn_error |= s[i];
+        s[i] = INDEX_OF[s[i]];
+    }
+
+    if (!syn_error) {
+        /* if syndrome is zero, data[] is a codeword and there are no
+        * errors to correct. So return data[] unmodified
+        */
+        count = 0;
+        goto finish;
+    }
+    memset(&lambda[1],0,NROOTS*sizeof(lambda[0]));
+    lambda[0] = 1;
+
+    if (no_eras > 0) {
+        /* Init lambda to be the erasure locator polynomial */
+        lambda[1] = ALPHA_TO[mod255(PRIM*(NN-1-eras_pos[0]))];
+        for (i = 1; i < no_eras; i++) {
+            u = mod255(PRIM*(NN-1-eras_pos[i]));
+            for (j = i+1; j > 0; j--) {
+                tmp = INDEX_OF[lambda[j - 1]];
+                if(tmp != A0)
+                    lambda[j] ^= ALPHA_TO[mod255(u + tmp)];
+            }
+        }
+    }
+    for(i=0;i<NROOTS+1;i++)
+        b[i] = INDEX_OF[lambda[i]];
+
+    /*
+    * Begin Berlekamp-Massey algorithm to determine error+erasure
+    * locator polynomial
+    */
+    r = no_eras;
+    el = no_eras;
+    while (++r <= NROOTS) {       /* r is the step number */
+        /* Compute discrepancy at the r-th step in poly-form */
+        discr_r = 0;
+        for (i = 0; i < r; i++){
+            if ((lambda[i] != 0) && (s[r-i-1] != A0)) {
+                discr_r ^= ALPHA_TO[mod255(INDEX_OF[lambda[i]] + s[r-i-1])];
+            }
+        }
+        discr_r = INDEX_OF[discr_r];        /* Index form */
+        if (discr_r == A0) {
+            /* 2 lines below: B(x) <-- x*B(x) */
+            memmove(&b[1],b,NROOTS*sizeof(b[0]));
+            b[0] = A0;
+        } else {
+            /* 7 lines below: T(x) <-- lambda(x) - discr_r*x*b(x) */
+            t[0] = lambda[0];
+            for (i = 0 ; i < NROOTS; i++) {
+                if(b[i] != A0)
+                    t[i+1] = lambda[i+1] ^ ALPHA_TO[mod255(discr_r + b[i])];
+                else
+                    t[i+1] = lambda[i+1];
+            }
+            if (2 * el <= r + no_eras - 1) {
+                el = r + no_eras - el;
+                /*
+                * 2 lines below: B(x) <-- inv(discr_r) *
+                * lambda(x)
+                */
+                for (i = 0; i <= NROOTS; i++)
+                    b[i] = (lambda[i] == 0) ? A0 : mod255(INDEX_OF[lambda[i]] - discr_r + NN);
+            } else {
+                /* 2 lines below: B(x) <-- x*B(x) */
+                memmove(&b[1],b,NROOTS*sizeof(b[0]));
+                b[0] = A0;
+            }
+            memcpy(lambda,t,(NROOTS+1)*sizeof(t[0]));
+        }
+    }
+
+    /* Convert lambda to index form and compute deg(lambda(x)) */
+    deg_lambda = 0;
+    for(i=0;i<NROOTS+1;i++){
+        lambda[i] = INDEX_OF[lambda[i]];
+        if(lambda[i] != A0)
+            deg_lambda = i;
+    }
+    /* Find roots of the error+erasure locator polynomial by Chien search */
+    memcpy(&reg[1],&lambda[1],NROOTS*sizeof(reg[0]));
+    count = 0;            /* Number of roots of lambda(x) */
+    for (i = 1,k=IPRIM-1; i <= NN; i++,k = mod255(k+IPRIM)) {
+        q = 1; /* lambda[0] is always 0 */
+        for (j = deg_lambda; j > 0; j--){
+            if (reg[j] != A0) {
+                reg[j] = mod255(reg[j] + j);
+                q ^= ALPHA_TO[reg[j]];
+            }
+        }
+        if (q != 0)
+            continue; /* Not a root */
+        /* store root (index-form) and error location number */
+        root[count] = i;
+        loc[count] = k;
+        /* If we've already found max possible roots,
+        * abort the search to save time
+        */
+        if(++count == deg_lambda)
+            break;
+    }
+    if (deg_lambda != count) {
+        /*
+        * deg(lambda) unequal to number of roots => uncorrectable
+        * error detected
+        */
+        count = -1;
+        goto finish;
+    }
+    /*
+    * Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo
+    * x**NROOTS). in index form. Also find deg(omega).
+    */
+    deg_omega = 0;
+    for (i = 0; i < NROOTS;i++){
+        tmp = 0;
+        j = (deg_lambda < i) ? deg_lambda : i;
+        for(;j >= 0; j--){
+            if ((s[i - j] != A0) && (lambda[j] != A0))
+                tmp ^= ALPHA_TO[mod255(s[i - j] + lambda[j])];
+        }
+        if(tmp != 0)
+            deg_omega = i;
+        omega[i] = INDEX_OF[tmp];
+    }
+    omega[NROOTS] = A0;
+
+    /*
+    * Compute error values in poly-form. num1 = omega(inv(X(l))), num2 =
+    * inv(X(l))**(FCR-1) and den = lambda_pr(inv(X(l))) all in poly-form
+    */
+    for (j = count-1; j >=0; j--) {
+        num1 = 0;
+        for (i = deg_omega; i >= 0; i--) {
+            if (omega[i] != A0)
+                num1  ^= ALPHA_TO[mod255(omega[i] + i * root[j])];
+        }
+        num2 = ALPHA_TO[mod255(root[j] * (FCR - 1) + NN)];
+        den = 0;
+
+        /* lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] */
+        for (i = min(deg_lambda,NROOTS-1) & ~1; i >= 0; i -=2) {
+            if(lambda[i+1] != A0)
+                den ^= ALPHA_TO[mod255(lambda[i+1] + i * root[j])];
+        }
+        if (den == 0) {
+            count = -1;
+            goto finish;
+        }
+        /* Apply error to data */
+        if (num1 != 0) {
+            data[loc[j]] ^= ALPHA_TO[mod255(INDEX_OF[num1] + INDEX_OF[num2] + NN - INDEX_OF[den])];
+        }
+    }
+finish:
+    if(eras_pos != NULL){
+        for(i=0;i<count;i++)
+            eras_pos[i] = loc[i];
+    }
+    return count;
+}
+
+/* ---------- */
+/* Re-encoder */
+/* ---------- */
+
+/* Reference encoder for proposed coded AO-40 telemetry format - v1.0  7 Jan 2002
+* Copyright 2002, Phil Karn, KA9Q
+* This software may be used under the terms of the GNU Public License (GPL)
+*/
+
+/* Adapted from  the above enc_ref.c  as used by the spacecraft (JRM) */
+
+
+/* Write one binary channel symbol into the interleaver frame and update the pointers */
+//void CCodecAO40::interleave_symbol(int c)
+void interleave_symbol(int c)
+{
+    int row,col;
+    col=m_ileaver_index/COLUMNS;
+    row=m_ileaver_index%COLUMNS;
+    if(c)
+    {
+        m_encoded[row*ROWS+col] = 1;
+    }
+    m_ileaver_index++;
+}
+
+/* Convolutionally encode and interleave one byte */
+//void CCodecAO40::encode_and_interleave(unsigned char c,int cnt){
+void encode_and_interleave(unsigned char c,int cnt){
+    while(cnt-- != 0)
+    {
+        m_conv_sr = (m_conv_sr << 1) | (c >> 7);
+        c <<= 1;
+        interleave_symbol( Partab[m_conv_sr & CPOLYA]);
+        interleave_symbol(!Partab[m_conv_sr & CPOLYB]); /* Second encoder symbol is inverted */
+    }
+}
+
+/* Scramble a byte, convolutionally encode and interleave into frame */
+//void CCodecAO40::scramble_and_encode(unsigned char c){
+void scramble_and_encode(unsigned char c){
+    c ^= Scrambler[m_encoded_bytes]; /* Scramble byte */
+    encode_and_interleave(c,8); /* RS encode and place into reencode buffer */
+}
+
+/* Encodes the 256 byte source block RSdecdata[] into 5200 byte block of symbols
+*  results stored in m_encoded.
+*  On success encoded buffer is returned, an zeroed buffer on failure
+*/
+//const unsigned char *CCodecAO40::encode(unsigned char *source_bytes, int byte_count)
+const unsigned char *encode(unsigned char *source_bytes, int byte_count)
+{
+    if(BLOCKSIZE != byte_count || NULL == source_bytes )
+    {
+		memset(m_encoded, 0, BLOCKSIZE);
+        return m_encoded;
+    }
+
+    init_encoder();
+
+    for(int i=0;i<BLOCKSIZE;i++)
+    {
+        encode_byte(source_bytes[i]) ;
+    }
+
+    for(int i=0;i<64;i++)
+    {
+        encode_parity();
+    }    
+
+    return m_encoded;
+}
+
+/* The original three C user's entry points now follow.  They are:
+
+init_encoder()                   Called once before using system.
+encode_byte(unsigned char c)     Called with each user byte (i.e. 256 calls)
+encode_parity()                  Called 64 times to finish off
+
+*/
+
+/* This function initializes the encoder. */
+//void CCodecAO40::init_encoder(void){
+void init_encoder(void){
+    int i,j,sr;
+
+    m_encoded_bytes  = 0;
+    m_conv_sr = 0;
+    m_ileaver_index  = COLUMNS;   /* Sync vector is in first column; data starts here */
+
+    for(j=0;j<RSBLOCKS;j++)       /* Flush parity array */
+    {
+        for(i=0;i<NROOTS;i++)
+        {
+            m_RS_block[j][i] = 0;
+        }
+    }
+
+    /* Clear re-encoded array */
+    for(i=0;i<SYMPBLOCK;i++)
+    {
+        m_encoded[i] = 0;
+    }
+
+    /* Generate sync vector, interleave into re-encode array, 1st column */
+    sr = 0x7f;
+    for(i=0;i<65;i++)
+    {
+        if(sr & 64)
+        {
+            m_encoded[ROWS*i] = 1;      /* Every 80th symbol is a sync bit */
+        }
+        sr = (sr << 1) | Partab[sr & SYNC_POLY];
+    }
+}
+
+/* This function is called with each user data byte to be encoded into the
+* current frame. It should be called in sequence 256 times per frame, followed
+* by 64 calls to encode_parity().
+*/
+//void CCodecAO40::encode_byte(unsigned char c){
+void encode_byte(unsigned char c){
+    unsigned char *rp;                /* RS block pointer */
+    int i;
+    unsigned char feedback;
+
+    /* Update the appropriate Reed-Solomon codeword */
+    rp = m_RS_block[m_encoded_bytes & 1];
+
+    /* Compute feedback term */
+    feedback = INDEX_OF[c ^ rp[0]];
+
+    /* If feedback is non-zero, multiply by each generator polynomial coefficient and
+    * add to corresponding shift register elements
+    */
+    if(feedback != A0){
+        int j;
+
+        /* This loop exploits the palindromic nature of the generator polynomial
+        * to halve the number of discrete multiplications
+        */
+        for(j=0;j<15;j++){
+            unsigned char t;
+
+            t = ALPHA_TO[mod255(feedback + RS_poly[j])];
+            rp[j+1] ^= t; rp[31-j] ^= t;
+        }
+        rp[16] ^= ALPHA_TO[mod255(feedback + RS_poly[15])];
+    }
+
+    /* Shift 32 byte RS register one position down */
+    for(i=0;i<31;i++)
+        rp[i] = rp[i+1];
+
+    /* Handle highest order coefficient, which is unity */
+    if(feedback != A0){
+        rp[31] = ALPHA_TO[feedback];
+    } else {
+        rp[31] = 0;
+    }
+    scramble_and_encode(c);
+    m_encoded_bytes++;
+}
+
+/* This function should be called 64 times after the 256 data bytes
+* have been passed to update_encoder. Each call scrambles, encodes and
+* interleaves one byte of Reed-Solomon parity.
+*/
+//void CCodecAO40::encode_parity(void) {
+void encode_parity(void) {
+    unsigned char c;
+
+    c =  m_RS_block[m_encoded_bytes & 1][(m_encoded_bytes-256)>>1];
+    scramble_and_encode(c);
+    if(++m_encoded_bytes == 320){
+        /* Tail off the convolutional encoder (flush) */
+        encode_and_interleave(0,6);
+    }
+}
+
+
+
+//void CCodecAO40::descramble_to_rsblocks(unsigned char viterbi_decoded[NBITS_OUT],    char rsblocks[RSBLOCKS][NN])
+void descramble_to_rsblocks(unsigned char viterbi_decoded[NBITS_OUT],    char rsblocks[RSBLOCKS][NN])
+{
+    /* interleave into Reed Solomon codeblocks */
+    memset(rsblocks,0,RSBLOCKS*NN);  /* Zero rsblocks array */
+    int di = 0;
+    int si = 0;
+    for(int col=RSPAD;col<NN;col++)
+    {
+        for(int row=0;row<RSBLOCKS;row++)
+        {
+            rsblocks[row][col] = viterbi_decoded[di++] ^ Scrambler[si++];  /* Remove scrambling */
+        }
+    }
+}
+
+/* -------- */
+/* Decoder  */
+/* -------- */
+
+    /* ------------------- */
+
+    /* There are two RS decoders, processing 128 bytes each.
+    *
+    * If both RS decoders are SUCCESSFUL
+    * On exit:
+    *   rs_failures = 0
+    *   rserrs[x]   = number of errors corrected;  range 0 to 16   (x= 0 or 1)
+    *   Data output is in array RSdecdata[256].
+    *
+    * If an RS decoder FAILS
+    * On exit:
+    *   rs_failures = 1 or 2   (i.e. != 0)
+    *   rserrs[x] contains -1
+    *   Data output should not be used.
+    */
+//int CCodecAO40::decode(unsigned char vitdecdata[NBITS_OUT], unsigned char *decoded_data)
+int decode(unsigned char vitdecdata[NBITS_OUT], unsigned char *decoded_data)
+{    
+    int row, col, row_errors, total_errors;    
+    char rsblocks[RSBLOCKS][NN];
+
+    descramble_to_rsblocks(vitdecdata, rsblocks);        
+
+    /* Run RS-decoder(s) */
+    row_errors = total_errors = 0;
+    for(row=0; row<RSBLOCKS && row_errors!= -1; row++)
+    {
+        // decode row, returns -1 on failure or number of corrected errors
+        row_errors = decode_rs_8(rsblocks[row],NULL,0);
+        total_errors += row_errors;
+    }
+
+    if(row_errors != -1)
+    {
+        /* if frame decoded OK, deinterleave data from RS codeword(s) */
+        int j = 0;
+        for(col=RSPAD;col<KK;col++)
+        {
+            for(row=0;row<RSBLOCKS;row++)
+            {
+                decoded_data[j++] = rsblocks[row][col];
+            }
+        }
+    }
+    else
+    {
+        total_errors = -1;
+    }
+
+    return total_errors;
+}
+
+/* Compairs raw input symbols to current buffer of encoded symbols and counts the errors */            
+//int CCodecAO40::count_errors(unsigned char *raw_symbols)       
+int count_errors(unsigned char *raw_symbols)       
+{  
+    int error_count = 0;
+    for(int i=0;i<SYMPBLOCK;i++)
+    {
+        if ( m_encoded[i] != (raw_symbols[i]>>7) ) 
+        {
+            error_count++ ;
+        }
+    }
+    return error_count;
+}
diff --git a/codecAO40.h b/codecAO40.h
new file mode 100644
index 00000000..3ca2c11c
--- /dev/null
+++ b/codecAO40.h
@@ -0,0 +1,110 @@
+/*    AO40 encoder / decoder
+ *    Copyright 2002 Phil Karn, KA9Q
+ *    May be used under the terms of the GNU General Public License (GPL) 
+ *    See CodecAO40.cpp for lineage
+ *
+ *    This file is part of FUNcubeLib.
+ *
+ *    FUNcubeLib is free software: you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation, either version 3 of the License, or
+ *    (at your option) any later version.
+ *
+ *    FUNcubeLib is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with FUNcubeLib If not, see <http://www.gnu.org/licenses/>.
+ *
+*/
+
+#pragma once
+
+//#include "fecConstants.h"
+
+
+/*
+	Amsat P3 FEC Encoder/decoder system. Look-up tables
+	Created by Phil Karn KA9Q and James Miller G3RUH
+	Last modified 2003 Jun 20  
+*/
+
+/* Defines for Viterbi Decoder for r=1/2 k=7  (to CCSDS convention) */
+#define K 7                      /* Constraint length */
+#define N 2                      /* Number of symbols per data bit */
+#define CPOLYA 0x4f              /* First  convolutional encoder polynomial */
+#define CPOLYB 0x6d              /* Second convolutional encoder polynomial */
+
+#define SYNC_POLY   0x48         /* Sync vector PN polynomial */
+
+#define NN        255
+#define KK        223
+#define NROOTS     32            /* NN-KK */
+#define A0        (NN)
+#define FCR       112
+#define PRIM       11
+#define IPRIM     116
+#define BLOCKSIZE 256            /* Data bytes per frame */
+#define RSBLOCKS    2            /* Number of RS decoder blocks */
+#define RSPAD      95            /* Unused bytes in block  (KK-BLOCKSIZE/RSBLOCKS) */
+
+/* Defines for Interleaver */
+#define ROWS       80            /* Block interleaver rows */
+#define COLUMNS    65            /* Block interleaver columns */
+#define SYMPBLOCK (ROWS*COLUMNS) /* Encoded symbols per block */
+
+/* Number of symbols in an FEC block that are */
+/* passed to the Viterbi decoder  (320*8 + 6) */
+#define NBITS ((BLOCKSIZE+NROOTS*RSBLOCKS)*8+K-1)
+/* Number of bits obtained from Viterbi decoder */
+#define NBITS_OUT (BLOCKSIZE+NROOTS*RSBLOCKS)
+
+
+extern unsigned char m_RS_block[RSBLOCKS][NROOTS]; /* RS parity blocks */
+extern     unsigned char m_encoded[SYMPBLOCK] ;       /* encoded symbols */
+extern     int m_encoded_bytes;               /* Byte counter for encode_data() */
+extern     int m_ileaver_index;               /* Byte counter for interleaver */
+extern     unsigned char m_conv_sr;           /* Convolutional encoder shift register state */
+
+extern const unsigned char RS_poly[];
+extern const unsigned char ALPHA_TO[];
+extern const unsigned char INDEX_OF[];
+extern const unsigned char Partab[];
+extern const unsigned char Scrambler[];
+
+//class CCodecAO40
+//{
+//public:
+//	CCodecAO40(void);
+//    virtual ~CCodecAO40(void);
+
+    int decode(unsigned char viterbi_decoded[NBITS_OUT], unsigned char *decoded_data);
+
+    /* Encodes the 256 byte source block into 5200 byte block of symbols into m_encoded buffer */
+     const unsigned char *encode(
+        unsigned char *source_bytes,  /* input to encode */    
+        int byte_count);              /* input length in bytes */
+    
+    /* Compares raw input symbols to current buffer of encoded symbols and counts the errors */            
+    int count_errors( unsigned char *raw_symbols);        
+
+//private:
+    int mod255(int x);
+    int decode_rs_8(char *data, int *eras_pos, int no_eras);
+    void scramble_and_encode(unsigned char c);
+    void encode_and_interleave(unsigned char c,int cnt);
+
+    void descramble_to_rsblocks(
+        unsigned char viterbi_decoded[NBITS_OUT],
+        char rsblocks[RSBLOCKS][NN]);
+
+    void init_encoder(void);
+    void encode_byte(unsigned char c);
+    void encode_parity(void);
+    
+    void interleave_symbol(int c);
+
+
+//};
diff --git a/command b/command
index 1349f45a..f4c96b1d 100755
--- a/command
+++ b/command
@@ -1,6 +1,8 @@
 #!/bin/bash
 
-echo -e "\nCommand and Control script for CubeSatSim v2.0\n"
+echo -e "\nCommand and Control script for CubeSatSim v2.1\n"
+
+sudo modprobe snd-aloop
 
 	FILE=/home/pi/CubeSatSim/command_control
 	if [ -f "$FILE" ]; then
@@ -35,9 +37,17 @@ else
 
 fi
 
-echo "Waiting 20 seconds for USB"
+uptime=`cat /proc/uptime | awk '{printf "%0.f", $1}'`
+
+echo -n "Uptime since boot is "
+echo $uptime
+
+if [[ "$uptime" -lt  "60" ]]; then
 
-sleep 20
+	echo "Waiting 20 seconds for USB"
+
+	sleep 20
+fi
 
 FILE=/home/pi/CubeSatSim/command_control_direwolf
 if [[ $(arecord -l | grep "USB Audio Device") ]] && [ -f "$FILE" ]; then
diff --git a/config b/config
index 104a17e3..8cf523a3 100755
--- a/config
+++ b/config
@@ -210,7 +210,7 @@ function transmit_command_beacon {
 	exit
 }
 
-echo "CubeSatSim v2.0 configuration tool"
+echo "CubeSatSim v2.1 configuration tool"
 echo
 # echo $1
 # echo $2
@@ -249,6 +249,8 @@ if [ "$1" = "" ]; then
 		echo "Mode is SSTV"
 	elif [ "$1" = "e" ]; then
 		echo "Mode is Repeater"	
+	elif [ "$1" = "j" ]; then
+		echo "Mode is FunCube"	
 	elif [ "$1" = "n" ]; then
 		echo -n "Mode is Transmit Commands with "
 		FILE=/home/pi/CubeSatSim/transmit_dtmf
@@ -366,7 +368,7 @@ elif [ "$1" = "-a" ]; then
 
 		echo "changing CubeSatSim to AFSK mode"
 		sudo echo "a" > /home/pi/CubeSatSim/.mode	
-		if [ "$1" == "f" ] || [ "$1" == "b" ]  || [ "$1" == "e" ] ; then
+		if [ "$1" == "f" ] || [ "$1" == "b" ]  || [ "$1" == "e" ] || [ "$1" == "j" ] ; then
 			FILE=/home/pi/CubeSatSim/battery_saver
 			if [ -f "$FILE" ]; then
 				restart=1
@@ -392,7 +394,7 @@ elif [ "$1" = "-m" ]; then
 
 		echo "changing CubeSatSim to CW mode"
 		sudo echo "m" > /home/pi/CubeSatSim/.mode
-		if [ "$1" == "f" ] || [ "$1" == "b" ]  || [ "$1" == "e" ] ; then
+		if [ "$1" == "f" ] || [ "$1" == "b" ]  || [ "$1" == "e" ] || [ "$1" == "j" ] ; then
 			FILE=/home/pi/CubeSatSim/battery_saver
 			if [ -f "$FILE" ]; then
 				restart=1
@@ -452,7 +454,7 @@ elif [ "$1" = "-s" ]; then
 
 		echo "changing CubeSatSim to SSTV mode"
 		sudo echo "s" > /home/pi/CubeSatSim/.mode
-		if [ "$1" == "f" ] || [ "$1" == "b" ]  || [ "$1" == "e" ] ; then
+		if [ "$1" == "f" ] || [ "$1" == "b" ]  || [ "$1" == "e" ] || [ "$1" == "j" ] ; then
 
 			FILE=/home/pi/CubeSatSim/battery_saver
 			if [ -f "$FILE" ]; then
@@ -743,6 +745,8 @@ elif [ "$1" = "-T" ]; then
 #			reboot=1
 			echo "restarting command and control"
 			sudo systemctl restart command
+			echo "restarting transmit"
+			sudo systemctl restart transmit
 ##			sudo reboot now	
 		fi
 
@@ -759,6 +763,8 @@ elif [ "$1" = "-T" ]; then
 			echo "restarting command and control"
 #			reboot=1
 			sudo systemctl restart command
+			echo "restarting transmit"
+			sudo systemctl restart transmit
 ##			sudo reboot now	
 		fi
 
@@ -1403,6 +1409,22 @@ elif [ "$1" = "-g" ]; then
 		echo "Not resetting"
 
 	fi
+elif [ "$1" = "-j" ]; then
+
+	value=`cat /home/pi/CubeSatSim/.mode`
+	echo "$value" > /dev/null
+	set -- $value
+
+#	if [ "$1" == "n" ]; then
+
+#		transmit_command_bpsk
+
+#	else
+
+		echo "changing CubeSatSim to FunCube mode"
+		sudo echo "j" > /home/pi/CubeSatSim/.mode
+		restart=1
+#	fi
 
 elif [ "$1" = "-h" ]; then
 
@@ -1416,7 +1438,8 @@ elif [ "$1" = "-h" ]; then
 	echo "   -f     Change to FSK/DUV mode"
 	echo "   -b     Change to BPSK mode"
 	echo "   -s     Change to SSTV mode"
-	echo "   -n     Change to Transmit Commands mode"
+	echo "   -j     Change to FunCube mode"
+  	echo "   -n     Change to Transmit Commands mode"
 	echo "   -e     Change to Repeater mode"
 	echo "   -i     Restart CubeSatsim software"
 	echo "   -c     Change the CALLSIGN in the configuration file sim.cfg"
diff --git a/direwolf-cc.conf b/direwolf-cc.conf
index 8419355f..26f8805b 100644
--- a/direwolf-cc.conf
+++ b/direwolf-cc.conf
@@ -1,2 +1,2 @@
-ADEVICE hw:CARD=Device,DEV=0 default 
+ADEVICE shared_mic hw:CARD=Loopback,DEV=1 
 DTMF
diff --git a/dtmf_aprs_cc.py b/dtmf_aprs_cc.py
index befe58c7..12d69af3 100644
--- a/dtmf_aprs_cc.py
+++ b/dtmf_aprs_cc.py
@@ -71,6 +71,25 @@ if __name__ == "__main__":
 			system("echo '\nCW Mode!!\n'")
 			mode = 'm'
 			change_mode = True
+
+		if ((line.find("MODE=e")) > 0):
+			system("echo '\nRepeater Mode!!\n'")
+			mode = 'e'
+			change_mode = True
+			counter = (counter + 1) % 2
+		if ((line.find("DTMF>APDW15:t6#")) > 0):
+			system("echo '\nRepeater Mode!!\n'")
+			mode = 'e'
+			change_mode = True
+		if ((line.find("MODE=j")) > 0):
+			system("echo '\nFunCube Mode!!\n'")
+			mode = 'j'
+			change_mode = True
+			counter = (counter + 1) % 2
+		if ((line.find("DTMF>APDW15:t7#")) > 0):
+			system("echo '\nFunCube Mode!!\n'")
+			mode = 'j'
+			change_mode = True				
 		if ((line.find("MODE=n")) > 0):
 			system("echo '\nTransmit Commands Mode!!\n'")
 			mode = 'n'
@@ -80,7 +99,6 @@ if __name__ == "__main__":
 			system("echo '\nTransmit Commands Mode!!\n'")
 			mode = 'n'
 			change_mode = True
-# Currently, C2C does not support Repeater mode e			
 		if ((line.find("MODE=o")) > 0):
 			system("echo '\nBeacon Mode toggle!!\n'")
 			mode = 'o'
@@ -91,7 +109,8 @@ if __name__ == "__main__":
 			mode = 'o'
 			change_mode = True
 			
-		if (debug_mode == False)  and (change_mode == True) and (counter == 1): # skip every other APRS command since Direwolf prints them twice
+#		if (debug_mode == False)  and (change_mode == True) and (counter == 1): # skip every other APRS command since Direwolf prints them twice
+		if (debug_mode == False)  and (change_mode == True): # skip every other APRS command since Direwolf prints them twice
 			GPIO.setmode(GPIO.BCM)
 			GPIO.setwarnings(False)
 			GPIO.setup(powerPin, GPIO.OUT)
@@ -160,6 +179,63 @@ if __name__ == "__main__":
 				sleep(0.1)
 				GPIO.output(powerPin, 1)
 				sleep(1)
+
+			elif (mode == 'e'):
+				GPIO.output(powerPin, 0) # blink six times
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)	
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1);
+				GPIO.output(powerPin, 1)	
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(1)
+
+			elif (mode == 'j'):
+				GPIO.output(powerPin, 0) # blink seven times
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)	
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1);
+				GPIO.output(powerPin, 1)	
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(0.1)
+				GPIO.output(powerPin, 0)
+				sleep(0.1)
+				GPIO.output(powerPin, 1)
+				sleep(1)			
+			
 			elif (mode == 'a'):
 				mode = 'a'
 				GPIO.output(powerPin, 0) # blink one time
diff --git a/fecConstants.h b/fecConstants.h
new file mode 100644
index 00000000..850c0aa6
--- /dev/null
+++ b/fecConstants.h
@@ -0,0 +1,128 @@
+#pragma once
+
+/*
+	Amsat P3 FEC Encoder/decoder system. Look-up tables
+	Created by Phil Karn KA9Q and James Miller G3RUH
+	Last modified 2003 Jun 20  
+*/
+
+/* Defines for Viterbi Decoder for r=1/2 k=7  (to CCSDS convention) */
+#define K 7                      /* Constraint length */
+#define N 2                      /* Number of symbols per data bit */
+#define CPOLYA 0x4f              /* First  convolutional encoder polynomial */
+#define CPOLYB 0x6d              /* Second convolutional encoder polynomial */
+
+#define SYNC_POLY   0x48         /* Sync vector PN polynomial */
+
+#define NN        255
+#define KK        223
+#define NROOTS     32            /* NN-KK */
+#define A0        (NN)
+#define FCR       112
+#define PRIM       11
+#define IPRIM     116
+#define BLOCKSIZE 256            /* Data bytes per frame */
+#define RSBLOCKS    2            /* Number of RS decoder blocks */
+#define RSPAD      95            /* Unused bytes in block  (KK-BLOCKSIZE/RSBLOCKS) */
+
+/* Defines for Interleaver */
+#define ROWS       80            /* Block interleaver rows */
+#define COLUMNS    65            /* Block interleaver columns */
+#define SYMPBLOCK (ROWS*COLUMNS) /* Encoded symbols per block */
+
+/* Number of symbols in an FEC block that are */
+/* passed to the Viterbi decoder  (320*8 + 6) */
+#define NBITS ((BLOCKSIZE+NROOTS*RSBLOCKS)*8+K-1)
+/* Number of bits obtained from Viterbi decoder */
+#define NBITS_OUT (BLOCKSIZE+NROOTS*RSBLOCKS)
+
+const unsigned char RS_poly[] = {
+    249, 59, 66,  4, 43,126,251, 97, 30,  3,213, 50, 66,170,  5, 24
+};
+
+/* Tables for RS decoder */
+/* Galois field log/antilog tables */
+const unsigned char ALPHA_TO[] =
+{
+    0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x87, 0x89, 0x95, 0xad, 0xdd, 0x3d, 0x7a, 0xf4,
+    0x6f, 0xde, 0x3b, 0x76, 0xec, 0x5f, 0xbe, 0xfb, 0x71, 0xe2, 0x43, 0x86, 0x8b, 0x91, 0xa5, 0xcd,
+    0x1d, 0x3a, 0x74, 0xe8, 0x57, 0xae, 0xdb, 0x31, 0x62, 0xc4, 0x0f, 0x1e, 0x3c, 0x78, 0xf0, 0x67,
+    0xce, 0x1b, 0x36, 0x6c, 0xd8, 0x37, 0x6e, 0xdc, 0x3f, 0x7e, 0xfc, 0x7f, 0xfe, 0x7b, 0xf6, 0x6b,
+    0xd6, 0x2b, 0x56, 0xac, 0xdf, 0x39, 0x72, 0xe4, 0x4f, 0x9e, 0xbb, 0xf1, 0x65, 0xca, 0x13, 0x26,
+    0x4c, 0x98, 0xb7, 0xe9, 0x55, 0xaa, 0xd3, 0x21, 0x42, 0x84, 0x8f, 0x99, 0xb5, 0xed, 0x5d, 0xba,
+    0xf3, 0x61, 0xc2, 0x03, 0x06, 0x0c, 0x18, 0x30, 0x60, 0xc0, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0,
+    0x47, 0x8e, 0x9b, 0xb1, 0xe5, 0x4d, 0x9a, 0xb3, 0xe1, 0x45, 0x8a, 0x93, 0xa1, 0xc5, 0x0d, 0x1a,
+    0x34, 0x68, 0xd0, 0x27, 0x4e, 0x9c, 0xbf, 0xf9, 0x75, 0xea, 0x53, 0xa6, 0xcb, 0x11, 0x22, 0x44,
+    0x88, 0x97, 0xa9, 0xd5, 0x2d, 0x5a, 0xb4, 0xef, 0x59, 0xb2, 0xe3, 0x41, 0x82, 0x83, 0x81, 0x85,
+    0x8d, 0x9d, 0xbd, 0xfd, 0x7d, 0xfa, 0x73, 0xe6, 0x4b, 0x96, 0xab, 0xd1, 0x25, 0x4a, 0x94, 0xaf,
+    0xd9, 0x35, 0x6a, 0xd4, 0x2f, 0x5e, 0xbc, 0xff, 0x79, 0xf2, 0x63, 0xc6, 0x0b, 0x16, 0x2c, 0x58,
+    0xb0, 0xe7, 0x49, 0x92, 0xa3, 0xc1, 0x05, 0x0a, 0x14, 0x28, 0x50, 0xa0, 0xc7, 0x09, 0x12, 0x24,
+    0x48, 0x90, 0xa7, 0xc9, 0x15, 0x2a, 0x54, 0xa8, 0xd7, 0x29, 0x52, 0xa4, 0xcf, 0x19, 0x32, 0x64,
+    0xc8, 0x17, 0x2e, 0x5c, 0xb8, 0xf7, 0x69, 0xd2, 0x23, 0x46, 0x8c, 0x9f, 0xb9, 0xf5, 0x6d, 0xda,
+    0x33, 0x66, 0xcc, 0x1f, 0x3e, 0x7c, 0xf8, 0x77, 0xee, 0x5b, 0xb6, 0xeb, 0x51, 0xa2, 0xc3, 0x00,
+};
+
+const unsigned char INDEX_OF[]=
+{
+    0xff, 0x00, 0x01, 0x63, 0x02, 0xc6, 0x64, 0x6a, 0x03, 0xcd, 0xc7, 0xbc, 0x65, 0x7e, 0x6b, 0x2a,
+    0x04, 0x8d, 0xce, 0x4e, 0xc8, 0xd4, 0xbd, 0xe1, 0x66, 0xdd, 0x7f, 0x31, 0x6c, 0x20, 0x2b, 0xf3,
+    0x05, 0x57, 0x8e, 0xe8, 0xcf, 0xac, 0x4f, 0x83, 0xc9, 0xd9, 0xd5, 0x41, 0xbe, 0x94, 0xe2, 0xb4,
+    0x67, 0x27, 0xde, 0xf0, 0x80, 0xb1, 0x32, 0x35, 0x6d, 0x45, 0x21, 0x12, 0x2c, 0x0d, 0xf4, 0x38,
+    0x06, 0x9b, 0x58, 0x1a, 0x8f, 0x79, 0xe9, 0x70, 0xd0, 0xc2, 0xad, 0xa8, 0x50, 0x75, 0x84, 0x48,
+    0xca, 0xfc, 0xda, 0x8a, 0xd6, 0x54, 0x42, 0x24, 0xbf, 0x98, 0x95, 0xf9, 0xe3, 0x5e, 0xb5, 0x15,
+    0x68, 0x61, 0x28, 0xba, 0xdf, 0x4c, 0xf1, 0x2f, 0x81, 0xe6, 0xb2, 0x3f, 0x33, 0xee, 0x36, 0x10,
+    0x6e, 0x18, 0x46, 0xa6, 0x22, 0x88, 0x13, 0xf7, 0x2d, 0xb8, 0x0e, 0x3d, 0xf5, 0xa4, 0x39, 0x3b,
+    0x07, 0x9e, 0x9c, 0x9d, 0x59, 0x9f, 0x1b, 0x08, 0x90, 0x09, 0x7a, 0x1c, 0xea, 0xa0, 0x71, 0x5a,
+    0xd1, 0x1d, 0xc3, 0x7b, 0xae, 0x0a, 0xa9, 0x91, 0x51, 0x5b, 0x76, 0x72, 0x85, 0xa1, 0x49, 0xeb,
+    0xcb, 0x7c, 0xfd, 0xc4, 0xdb, 0x1e, 0x8b, 0xd2, 0xd7, 0x92, 0x55, 0xaa, 0x43, 0x0b, 0x25, 0xaf,
+    0xc0, 0x73, 0x99, 0x77, 0x96, 0x5c, 0xfa, 0x52, 0xe4, 0xec, 0x5f, 0x4a, 0xb6, 0xa2, 0x16, 0x86,
+    0x69, 0xc5, 0x62, 0xfe, 0x29, 0x7d, 0xbb, 0xcc, 0xe0, 0xd3, 0x4d, 0x8c, 0xf2, 0x1f, 0x30, 0xdc,
+    0x82, 0xab, 0xe7, 0x56, 0xb3, 0x93, 0x40, 0xd8, 0x34, 0xb0, 0xef, 0x26, 0x37, 0x0c, 0x11, 0x44,
+    0x6f, 0x78, 0x19, 0x9a, 0x47, 0x74, 0xa7, 0xc1, 0x23, 0x53, 0x89, 0xfb, 0x14, 0x5d, 0xf8, 0x97,
+    0x2e, 0x4b, 0xb9, 0x60, 0x0f, 0xed, 0x3e, 0xe5, 0xf6, 0x87, 0xa5, 0x17, 0x3a, 0xa3, 0x3c, 0xb7,
+};
+
+/* 8-bit parity table */
+const unsigned char Partab[] = {
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+};
+
+/* Scramble byte table */
+const unsigned char Scrambler[]=
+{
+    0xff, 0x48, 0x0e, 0xc0, 0x9a, 0x0d, 0x70, 0xbc, 0x8e, 0x2c, 0x93, 0xad, 0xa7, 0xb7, 0x46, 0xce,
+    0x5a, 0x97, 0x7d, 0xcc, 0x32, 0xa2, 0xbf, 0x3e, 0x0a, 0x10, 0xf1, 0x88, 0x94, 0xcd, 0xea, 0xb1,
+    0xfe, 0x90, 0x1d, 0x81, 0x34, 0x1a, 0xe1, 0x79, 0x1c, 0x59, 0x27, 0x5b, 0x4f, 0x6e, 0x8d, 0x9c,
+    0xb5, 0x2e, 0xfb, 0x98, 0x65, 0x45, 0x7e, 0x7c, 0x14, 0x21, 0xe3, 0x11, 0x29, 0x9b, 0xd5, 0x63,
+    0xfd, 0x20, 0x3b, 0x02, 0x68, 0x35, 0xc2, 0xf2, 0x38, 0xb2, 0x4e, 0xb6, 0x9e, 0xdd, 0x1b, 0x39,
+    0x6a, 0x5d, 0xf7, 0x30, 0xca, 0x8a, 0xfc, 0xf8, 0x28, 0x43, 0xc6, 0x22, 0x53, 0x37, 0xaa, 0xc7,
+    0xfa, 0x40, 0x76, 0x04, 0xd0, 0x6b, 0x85, 0xe4, 0x71, 0x64, 0x9d, 0x6d, 0x3d, 0xba, 0x36, 0x72,
+    0xd4, 0xbb, 0xee, 0x61, 0x95, 0x15, 0xf9, 0xf0, 0x50, 0x87, 0x8c, 0x44, 0xa6, 0x6f, 0x55, 0x8f,
+    0xf4, 0x80, 0xec, 0x09, 0xa0, 0xd7, 0x0b, 0xc8, 0xe2, 0xc9, 0x3a, 0xda, 0x7b, 0x74, 0x6c, 0xe5,
+    0xa9, 0x77, 0xdc, 0xc3, 0x2a, 0x2b, 0xf3, 0xe0, 0xa1, 0x0f, 0x18, 0x89, 0x4c, 0xde, 0xab, 0x1f,
+    0xe9, 0x01, 0xd8, 0x13, 0x41, 0xae, 0x17, 0x91, 0xc5, 0x92, 0x75, 0xb4, 0xf6, 0xe8, 0xd9, 0xcb,
+    0x52, 0xef, 0xb9, 0x86, 0x54, 0x57, 0xe7, 0xc1, 0x42, 0x1e, 0x31, 0x12, 0x99, 0xbd, 0x56, 0x3f,
+    0xd2, 0x03, 0xb0, 0x26, 0x83, 0x5c, 0x2f, 0x23, 0x8b, 0x24, 0xeb, 0x69, 0xed, 0xd1, 0xb3, 0x96,
+    0xa5, 0xdf, 0x73, 0x0c, 0xa8, 0xaf, 0xcf, 0x82, 0x84, 0x3c, 0x62, 0x25, 0x33, 0x7a, 0xac, 0x7f,
+    0xa4, 0x07, 0x60, 0x4d, 0x06, 0xb8, 0x5e, 0x47, 0x16, 0x49, 0xd6, 0xd3, 0xdb, 0xa3, 0x67, 0x2d,
+    0x4b, 0xbe, 0xe6, 0x19, 0x51, 0x5f, 0x9f, 0x05, 0x08, 0x78, 0xc4, 0x4a, 0x66, 0xf5, 0x58, 0xff,
+    0x48, 0x0e, 0xc0, 0x9a, 0x0d, 0x70, 0xbc, 0x8e, 0x2c, 0x93, 0xad, 0xa7, 0xb7, 0x46, 0xce, 0x5a,
+    0x97, 0x7d, 0xcc, 0x32, 0xa2, 0xbf, 0x3e, 0x0a, 0x10, 0xf1, 0x88, 0x94, 0xcd, 0xea, 0xb1, 0xfe,
+    0x90, 0x1d, 0x81, 0x34, 0x1a, 0xe1, 0x79, 0x1c, 0x59, 0x27, 0x5b, 0x4f, 0x6e, 0x8d, 0x9c, 0xb5,
+    0x2e, 0xfb, 0x98, 0x65, 0x45, 0x7e, 0x7c, 0x14, 0x21, 0xe3, 0x11, 0x29, 0x9b, 0xd5, 0x63, 0xfd,
+};
diff --git a/groundstation/bands.json b/groundstation/bands.json
index ea638154..dde2db0a 100644
--- a/groundstation/bands.json
+++ b/groundstation/bands.json
@@ -173,7 +173,8 @@
     "upper_bound": 440000000,
     "frequencies": {
       "packet": 434900000,
-      "pocsag": 439987500
+      "pocsag": 439987500,
+      "sstv": { "frequency": 434900000, "underlying": "nfm" }
     }
   },
   {
diff --git a/groundstation/cubicsdr.sh b/groundstation/cubicsdr.sh
index 3f4b6b62..55ba8412 100755
--- a/groundstation/cubicsdr.sh
+++ b/groundstation/cubicsdr.sh
@@ -29,6 +29,8 @@ sudo killall -9 rtl_tcp &>/dev/null
 
 sudo killall -9 CubicSDR &>/dev/null
 
+sudo killall -9 sdrpp &>/dev/null
+
 #sudo kill `ps -aux | grep cubicsdr-packet | grep -v grep | awk '{ print $2 }'` &>/dev/null && killall inotifywait &>/dev/null
 
 #sudo kill `ps -aux | grep packet | grep -v grep | awk '{ print $2 }'` &>/dev/null && killall inotifywait &>/dev/null
diff --git a/groundstation/direwolf/direwolf.conf b/groundstation/direwolf/direwolf.conf
index ce8b6823..7bf93998 100644
--- a/groundstation/direwolf/direwolf.conf
+++ b/groundstation/direwolf/direwolf.conf
@@ -1 +1 @@
-ADEVICE plughw:CARD=Loopback,DEV=1 plughw:CARD=b1,DEV=0
+ADEVICE plughw:CARD=Loopback,DEV=1
diff --git a/groundstation/fc_block_decode.py b/groundstation/fc_block_decode.py
new file mode 100644
index 00000000..b9fd7f6d
--- /dev/null
+++ b/groundstation/fc_block_decode.py
@@ -0,0 +1,211 @@
+import sys
+from os import system
+from time import sleep
+import logging
+import random
+from PIL import Image, ImageDraw, ImageFont, ImageColor
+import subprocess
+import io
+import datetime
+
+logging.basicConfig(format='%(message)s')
+# logging.warning('CC-Warning!')
+
+
+def fstr(template):
+	return eval(f"f'{template}'")	
+
+def system_and_print(string):
+	print(string)
+	system(string)
+	
+FC_EPS = 1
+FC_BOB = 25
+FC_SW = 50
+FC_PAYLOAD = 55
+extended = 1
+
+sequence, image_id, image_count  = 0, 0, 0
+Vx, Vy, Vz, Vb = 0, 0, 0, 0
+Ix, Iy, Iz, Ic, Ib = 0, 0, 0, 0, 0
+frame_count, frame_type = 0, " "
+frequency_string, frequency, errors, first_byte = " ", 0, 0, 0
+
+#html_dir  = "/home/pi/CubeSatSim/groundstation/public_html/"
+html_dir = "/home/pi/fctelem/public_html/"
+image_dir = "/home/pi/fctelem/"
+image = "image_file"
+ssdv = "/home/pi/ssdv/ssdv -d -J "
+
+date_time_string = str(datetime.datetime.now())
+date_time = date_time_string[:21]
+
+system_and_print("sudo rm " + image_dir + image)
+#system_and_print("sudo rm " + html_dir + "*")
+system_and_print("sudo rm " + html_dir + "images/*")
+
+try:
+	with open('/home/pi/fctelem/fctelem.cfg', 'r') as config:
+		config_string = config.read()
+		print(config_string)
+except:
+	print("Error loading fctelem.cfg")
+
+#system_and_print("cp /home/pi/CubeSatSim/sstv/sstv_image_1_320_x_256.jpg " + html_dir + "image_file.jpeg")
+
+head_string = '<HEAD><meta http-equiv="refresh" content="5"><title>FunCube CubeSatSim Telemetry</title></HEAD>\n<HTML>\n<H2>FunCube CubeSatSim Telemetry</H2>' + \
+		'fcdctl ' + config_string + '<p><pre>  <img height="256" width="320" src="' + image + \
+		'.jpeg"><br>                 <A HREF="images" target="_blank">All images</a><br>'
+foot_string = '</HTML>'
+telem_string_format = "           Image: {image_id:3d} count: {image_count:2d}<p>" + \
+		" Vx(mV): {Vx:5d}   Vy(mV): {Vy:5d}   Vz(mV): {Vz:5d}<p>" + \
+		" Ix(mA): {Ix:5d}   Iy(mA): {Iy:5d}   Iz(mA): {Iz:5d}<p>" + \
+  		"     Vbat(mV): {Vb:5d}   Ibat(mA): {Ib:5d}<br><br></pre>" + \
+    		" Freq: {frequency:10.1f} errors: {errors} Seq: {sequence:d} {frame_type} frames: {frame_count:d}"
+label_string = "date time            ,  frm,   freq off, err,   h,   seq,     frame, img,  c,    Vx,    Vy,    Vz,    Ix,    Iy,    Iz,    Vb,    Ib "
+label_string_html = label_string.replace(" ","&nbsp;")
+csv_format = "{date_time:21s}, {frame_count:4d}, {frequency:10.1f}, {errors:3d}, {first_byte: 2x}, {sequence:5d}, {frame_type:9s}, {image_id:3d}, {image_count:2d}, " + \
+		"{Vx:5d}, {Vy:5d}, {Vz:5d}, {Ix:5d}, {Iy:5d}, {Iz:5d}, {Vb:5d}, {Ib:5d}"
+
+
+telem_string = fstr(telem_string_format)
+with open(html_dir + "index.html", "w+") as html_file:
+	html_file.write(head_string)
+	html_file.write(telem_string)
+	html_file.write(foot_string)
+
+with open(html_dir + "images/telem.csv.txt", "w+") as csv_file:
+#	csv_file.write(csv_format)
+	csv_file.write(label_string)
+	csv_file.write("\n")
+
+image_id = 256 		# set illegal image ID for null # random.randint(0, 255)
+image_count = 1;
+
+# sys.exit()
+
+if __name__ == "__main__":
+	debug_mode = False
+	counter = 1
+	if (len(sys.argv)) > 1:
+#        	print("There are arguments!")
+		if ('d' == sys.argv[1]):
+			debug_mode = True
+	print(debug_mode)
+	for line in sys.stdin:
+		if (debug_mode):
+			print(line, end =" ")
+			logging.warning(line)
+			
+		if ((line.find("data: ")) > 0):
+			print("\ndata block found!\n")
+			frame_count += 1
+			data_block_string = line.split()
+#			print(data_block_string)
+			frequency_string = data_block_string[2]
+			print(frequency_string)
+			frequency = float(frequency_string[:len(frequency_string) - 2])
+			print(frequency)
+			errors = int(data_block_string[5])
+			data_block = [int(number_string,16) for number_string in data_block_string[7:]]
+			first_byte = data_block[0]
+			if (first_byte == 0xE0) or (first_byte == 0xE1):
+				if (data_block[0] == 0xE0):
+					frame_type = "RT1+IMG1"
+				if (data_block[0] == 0xE1):
+					frame_type = "RT2+IMG2"
+				print(frame_type)	
+				print(data_block[extended + 51], data_block[extended + 50], data_block[extended + 49])
+				sequence = data_block[extended + 51] + data_block[extended + 50] * 2**8 + data_block[extended + 49] * 2**16	
+				print("Sequence number: {:d}".format(sequence))
+				Vx = (data_block[extended + FC_EPS + 0] * 2**6) + (data_block[extended + FC_EPS + 1] >> 2)
+				Vy = (0x03 & data_block[extended + FC_EPS + 1]) * 2**12 + data_block[extended + FC_EPS + 2] * 2**4 + (data_block[extended + FC_EPS + 3] >> 4)
+				Vz = (0x0f & data_block[extended + FC_EPS + 3]) * 2**10 + data_block[extended + FC_EPS + 4] * 2**2 + (data_block[extended + FC_EPS + 5] >> 6)
+				Vb = (0x3f & data_block[extended + FC_EPS + 5]) * 2**8 + data_block[extended + FC_EPS + 6]
+				Ix = data_block[extended + FC_EPS + 7] * 2**2 + (data_block[extended + FC_EPS + 8] >> 6)
+				Iy = (0x3f & data_block[extended + FC_EPS + 8]) * 2**4 + (data_block[extended + FC_EPS + 9] >> 4)
+				Iz = (0x0f & data_block[extended + FC_EPS + 9]) * 2**6 + (data_block[extended + FC_EPS + 10] >> 2)
+				Ic = (0x03 & data_block[extended + FC_EPS + 10]) * 2**8 + data_block[extended + FC_EPS + 11] 
+				Ib = data_block[extended + FC_EPS + 12] * 2**2 + ((0xc0 & data_block[extended + FC_EPS + 13]) >> 6)
+				if (Ib == 0):
+					Ib = 0 - Ic					
+				print("Vx: {:d} mV Vy: {:d} mV Vz: {:d} mV".format(Vx, Vy, Vz))
+				print('Payload {:x} {:x} \n'.format(data_block[FC_PAYLOAD + extended], data_block[FC_PAYLOAD + extended + 1]))
+				if (data_block[FC_PAYLOAD + extended] == 0x55) and (data_block[FC_PAYLOAD + extended + 1] == 0x68):
+					try:
+						print("Writing this image payload block to file " + image + "_payload")
+						immutable_payload = bytes(bytearray(data_block[(FC_PAYLOAD + extended):]))   # payload)
+#						print(immutable_payload)
+						with open(image_dir + image + "_payload", "wb") as binary_file:
+    							binary_file.write(immutable_payload)
+					except:
+						print("File error")
+#					try:
+					print("Processing payload with ssdv, saving image to " + image + "_paylad.jpeg and log to ssdv_output")
+					system_and_print(ssdv + image_dir + image + "_payload " + 
+					       image_dir + image + "_payload.jpeg 2>&1 | sudo tee /home/pi/fctelem/ssdv_output")
+					print("Parsing ssdv_output")
+					with open("/home/pi/fctelem/ssdv_output", "r") as file:
+						for line in file:
+#							print("line:")
+#							print(line)
+							if ((line.find("mage ID:")) > 0):
+#								print("\nImage ID found!\n")
+								image_id_string = line.split()
+#								print(image_id_string)
+								new_image_id = int(image_id_string[2], 16)
+#								print(new_image_id)
+								if (new_image_id != image_id):
+									print("End of image")
+									if (image_id != 256):
+										print("Saving complete image")
+										system_and_print("cp " + html_dir + "image_file.jpeg " + html_dir + "images/" + image + str(image_id) + "." + str(image_count) + ".jpeg")
+										system_and_print("sudo rm " + image_dir + image)
+									else:
+										system_and_print("sudo rm " + image_dir + image)
+									print("New Image ID: ")
+									print(new_image_id)
+									image_id = new_image_id
+									image_count = 1								
+								else:
+									image_count += 1
+									print("new image_count:")
+									print(image_count)
+								print("Appending block to file " + image)	
+								with open(image_dir + image, "ab") as binary_file:
+    									binary_file.write(immutable_payload)
+								print("Running ssdv with all blocks in file " + image + " saving as " + html_dir + "image_file.jpeg")
+								system_and_print(ssdv + image_dir + image + " " + html_dir + "image_file.jpeg")
+								telem_string = fstr(telem_string_format)
+								print("Writing index.html file")
+								with open(html_dir + "index.html", "w") as html_file:
+									html_file.write(head_string)
+									html_file.write(telem_string)
+									html_file.write("<br><pre>")
+									with open(html_dir + "/images/telem.csv.txt", "r") as csv_file:
+										for line in csv_file:
+											html_file.write(line)
+											html_file.write("<br>")
+									date_time_string = str(datetime.datetime.now())
+									date_time = date_time_string[:21]		
+									tlm_string = fstr(csv_format)		
+									html_file.write(tlm_string)
+									html_file.write("</pre><br>")
+									html_file.write(foot_string)
+				else:
+					print("Payload not an image!")
+		#			image_id = 256 # set illegal image_id to force new image
+			else:
+				print("Unknown Sat Id or Frame")
+				sequence, image_id, image_count  = 0, 0, 0
+				Vx, Vy, Vz, Vb = 0, 0, 0, 0
+				Ix, Iy, Iz, Ic, Ib = 0, 0, 0, 0, 0
+				frame_count, frame_type = 0, " "
+				frequency_string, errors = " ", 0
+				
+			tlm_string = fstr(csv_format)	
+			with open(html_dir + "/images/telem.csv.txt", "a") as csv_file:
+				csv_file.write(tlm_string)
+				csv_file.write("\n")
+
+						
diff --git a/groundstation/fctelem.desktop b/groundstation/fctelem.desktop
new file mode 100644
index 00000000..c9ea58f6
--- /dev/null
+++ b/groundstation/fctelem.desktop
@@ -0,0 +1,10 @@
+[Desktop Entry]
+Type=Application
+Exec=/home/pi/CubeSatSim/groundstation/fctelem.sh
+Name=FunCube Telem
+Comment=FunCube Telemetry Web App 
+Icon=/home/pi/Icons/fc-icon.png
+Path=/home/pi
+Terminal=true
+Categories=HamRadio
+Keywords=Ham Radio;SDR
diff --git a/groundstation/fctelem.sh b/groundstation/fctelem.sh
new file mode 100755
index 00000000..fed1aeca
--- /dev/null
+++ b/groundstation/fctelem.sh
@@ -0,0 +1,153 @@
+#!/bin/bash
+# script to run FunCube Pi Telemetry App
+
+echo "Script to run FunCube CubeSatSim Telemetry"
+
+echo 
+
+echo "The Chromium browser will load in a few seconds with fctelem."
+
+echo "You can also use another web browser if you are on the same network as your Pi."
+
+
+echo 
+
+ip=$(hostname -I|cut -f1 -d ' ')
+
+echo "IP Address to use in web browser is: $ip:8000"
+
+echo
+
+ssid=$(iwgetid -r)
+
+echo "Note: you need to be on the Wifi network: $ssid"
+
+echo
+
+sudo killall -9 fctelem &>/dev/null
+
+sudo killall -9 python3 &>/dev/null
+
+#sudo killall -9 java &>/dev/null
+
+#sudo killall -9 rtl_fm &>/dev/null
+
+#pkill -o chromium &>/dev/null
+
+#sudo killall -9 rtl_tcp &>/dev/null
+
+#sudo killall -9 CubicSDR &>/dev/null
+
+#sudo killall -9 qsstv &>/dev/null
+
+#sudo killall -9 aplay &>/dev/null
+
+#sudo killall -9 direwolf &>/dev/null
+
+#sudo killall -9 zenity &>/dev/null
+
+#sudo systemctl stop rtl_tcp
+
+#sudo systemctl stop openwebrx
+
+
+if [[ $(/home/pi/fcdctl/fcdctl -l | grep "No FCD found") ]]; then
+    echo "No FunCube Dongle Found!"
+    echo "Plug in FCD and try running again"
+    sleep 30
+    exit
+else
+    echo "FCD Found!  Setting Gain"
+
+    FILE=/home/pi/fctelem/fctelem.cfg
+	if [ -f "$FILE" ]; then
+    
+#        config_string=$(</home/pi/fctelem/fctelem.cfg)
+        config_string=$(<$FILE)
+
+    else
+        echo "No fctelem.cfg file"
+        echo "Creating it"
+        config_string="-g 1 -m 1 -i 0"
+        echo $config_string >> FILE
+    fi    
+    
+    /home/pi/fcdctl/fcdctl $config_string
+    
+ #   /home/pi/fcdctl/fcdctl -g 1 -m 1 -i 0
+    echo
+fi
+
+
+frequency=$(zenity --list 2>/dev/null --width=410 --height=180 --title="FunCube Telem Decoding" --text="Choose the frequency for FunCube decoding:" --column="kHz" --column="Use" 434900 "CubeSatSim" Other "Choose another frequency")
+
+echo $frequency
+
+if [ -z "$frequency" ]; then 
+
+	echo "No choice made.  Exiting."
+	sleep 3
+	exit
+
+fi
+
+if [ "$frequency" = "434900" ]; then
+
+	frequency=434900000
+
+elif [ "$frequency" = "Other" ]; then
+
+	echo
+	
+	echo "Enter the frequency in kiloHertz"
+	
+	echo
+	
+	read -r frequency
+	
+	frequency=$frequency"000"
+
+fi
+
+echo "Frequency is" $frequency
+echo
+echo "If your CubeSatSim is transmitting in FunCube mode (mode 7) you should get some frames after 30 seconds"
+echo
+
+sleep 3
+
+echo "connectport=64516 connectaddress='localhost' autotuneoffset=100000 outdir='/home/pi/go/app/fctelem/data' frequency=$frequency" > /home/pi/fctelem/fcdecode.conf
+
+# FILE=/home/pi/CubeSatSim/groundstation/public_html
+FILE=/home/pi/CubeSatSim/fctelem/public_html
+if [ ! -d "$FILE" ]; then
+    echo "Making public_html directory"
+    mkdir /home/pi/fctelem/public_html
+    mkdir /home/pi/fctelem/public_html/images
+fi
+
+cd /home/pi/fctelem/public_html
+
+cp /home/pi/CubeSatSim/groundstation/index.html .
+
+cp /home/pi/CubeSatSim/sstv/sstv_image_1_320_x_256.jpg ./image_file.jpeg
+
+python3 -m http.server &
+
+setsid chromium-browser --check-for-update-interval=1 --simulate-critical-update  --noerrdialogs --disable-infobars http://127.0.0.1:8000 &>/dev/null & 
+
+cd /home/pi/fctelem
+
+sudo ./fctelem | python3 /home/pi/CubeSatSim/groundstation/fc_block_decode.py
+
+# sleep 10
+
+#read val
+
+#sudo killall -9 fctelem &>/dev/null
+
+#sudo killall -9 python3 &>/dev/null
+
+sleep 10
+
+#$SHELL
diff --git a/groundstation/fox.sh b/groundstation/fox.sh
index 3c1daf1a..0ca2c1e5 100755
--- a/groundstation/fox.sh
+++ b/groundstation/fox.sh
@@ -19,6 +19,8 @@ sudo killall -9 java &>/dev/null
 
 sudo killall -9 CubicSDR &>/dev/null
 
+sudo killall -9 sdrpp &>/dev/null
+
 sudo killall -9 direwolf &>/dev/null
 
 sudo killall -9 aplay &>/dev/null
diff --git a/groundstation/index.html b/groundstation/index.html
new file mode 100644
index 00000000..8baea30d
--- /dev/null
+++ b/groundstation/index.html
@@ -0,0 +1,9 @@
+<HEAD><meta http-equiv="refresh" content="5"></HEAD>
+<HTML>
+FunCube CubeSatSim Telemetry
+<p>
+<img src="image_file.jpeg">
+<!--
+<img src="image_file.jpeg" id="reloader" onLoad="setTimeout( () => { document.getElementById('reloader').src='image_file.jpeg' + '?' + new Date().getMilliseconds() } ,5000)" /> 
+-->
+</p>
diff --git a/groundstation/loc.sh b/groundstation/loc.sh
index 5a1b6881..bdbedc77 100755
--- a/groundstation/loc.sh
+++ b/groundstation/loc.sh
@@ -5,6 +5,8 @@ sudo killall -9 java &>/dev/null
 
 sudo killall -9 gpredict &>/dev/null
 
+source /home/pi/venv/bin/activate
+
 python3 /home/pi/CubeSatSim/groundstation/loc-foxtelem.py
 
 #/usr/bin/gpredict
diff --git a/groundstation/packet.sh b/groundstation/packet.sh
index 87620bdc..133ff021 100755
--- a/groundstation/packet.sh
+++ b/groundstation/packet.sh
@@ -24,6 +24,8 @@ sudo killall -9 java &>/dev/null
 
 sudo killall -9 CubicSDR &>/dev/null
 
+sudo killall -9 sdrpp &>/dev/null
+
 sudo killall -9 zenity &>/dev/null
 
 echo
@@ -175,7 +177,8 @@ else
 
   echo -e "Auto decoding APRS packets on $frequency Hz"
 
-  direwolf -r 48000 -c /home/pi/CubeSatSim/groundstation/direwolf/direwolf.conf -t 0 &
+ # direwolf -r 48000 -c /home/pi/CubeSatSim/groundstation/direwolf/direwolf.conf -t 0 &
+  direwolf -r 48000 -c /home/pi/CubeSatSim/groundstation/direwolf/direwolf.conf  &
 
 fi
 
@@ -186,6 +189,6 @@ echo "$value" > /dev/null
 set -- $value
 
 #rtl_fm -M fm -f 144.39M -s 48k | aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1
-rtl_fm -M fm -f $frequency -s 48k | tee >(aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1) | aplay -D hw:0,0 -r 48000 -t raw -f S16_LE -c 1
+rtl_fm -M fm -f $frequency -s 48k | tee >(aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1) | aplay -r 48000 -t raw -f S16_LE -c 1
 
 sleep 5
diff --git a/groundstation/rtl-tcp.sh b/groundstation/rtl-tcp.sh
index c8af8906..84b35f01 100755
--- a/groundstation/rtl-tcp.sh
+++ b/groundstation/rtl-tcp.sh
@@ -37,6 +37,8 @@ sudo killall -9 rtl_fm &>/dev/null
 
 sudo killall -9 CubicSDR &>/dev/null
 
+sudo killall -9 sdrpp &>/dev/null
+
 sudo killall -9 rtl_tcp &>/dev/null
 
 sudo killall -9 qsstv &>/dev/null
@@ -48,7 +50,7 @@ sudo killall -9 aplay &>/dev/null
 sudo killall -9 zenity &>/dev/null
 
 
-sudo /bin/sh -c '/usr/local/bin/rtl_tcp -a $(hostname -I|cut -f1 -d " ")'
+sudo /bin/sh -c 'rtl_tcp -a $(hostname -I|cut -f1 -d " ")'
 
 sleep 5
 
diff --git a/groundstation/sdr.sh b/groundstation/sdr.sh
index 0783d9ca..016f4f20 100755
--- a/groundstation/sdr.sh
+++ b/groundstation/sdr.sh
@@ -34,6 +34,8 @@ sudo killall -9 rtl_tcp &>/dev/null
 
 sudo killall -9 CubicSDR &>/dev/null
 
+sudo killall -9 sdrpp &>/dev/null
+
 sudo killall -9 qsstv &>/dev/null
 
 sudo killall -9 aplay &>/dev/null
diff --git a/groundstation/sstv_decode_prompt.sh b/groundstation/sstv_decode_prompt.sh
index 76eb611f..ec5ab709 100755
--- a/groundstation/sstv_decode_prompt.sh
+++ b/groundstation/sstv_decode_prompt.sh
@@ -26,6 +26,8 @@ sudo killall -9 java &>/dev/null
 
 sudo killall -9 CubicSDR &>/dev/null
 
+sudo killall -9 sdrpp &>/dev/null
+
 sudo killall -9 zenity &>/dev/null
 
 #echo "s" >> .mode
@@ -128,7 +130,7 @@ set -- $value
 
 #rtl_fm -M fm -f 434.9M -s 48k | aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1 
 #rtl_fm -M fm -f 434.9M -s 48k | tee >(aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1) | aplay -D hw:0,0 -r 48000 -t raw -f S16_LE -c 1
-rtl_fm -M fm -f $frequency -s 48k | tee >(aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1) | aplay -D hw:0,0 -r 48000 -t raw -f S16_LE -c 1
+rtl_fm -M fm -f $frequency -s 48k | tee >(aplay -D hw:${2:0:1},0,0 -r 48000 -t raw -f S16_LE -c 1) | aplay -r 48000 -t raw -f S16_LE -c 1
 
 sleep 5
 
diff --git a/install b/install
index 046ab3c8..535fad29 100755
--- a/install
+++ b/install
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-echo -e "\ninstallation script for CubeSatSim v2.0\n"
+echo -e "\ninstallation script for CubeSatSim v2.1\n"
 
 FILE=/home/pi/CubeSatSim/sim.cfg
 if [ -f "$FILE" ]; then
@@ -56,7 +56,7 @@ sudo dpkg -i debian-template/wiringpi-2.61-1.deb
 cd
 
 #changed to python3-smbus
-sudo apt install -y python3-pip python3-smbus libjpeg-dev zlib1g-dev libfreetype6-dev libopenjp2-7 libtiff5 python3-pil python3-serial
+sudo apt install -y python3-pip python3-smbus libjpeg-dev zlib1g-dev libfreetype6-dev libopenjp2-7 libtiff5 python3-pil python3-serial libusb-1.0
 
 sudo pip3 install --upgrade setuptools
 
@@ -95,13 +95,35 @@ git clone https://github.com/alanbjohnston/pi-power-button.git
 
 cd pi-power-button
 
-git checkout master
+git checkout 7-modes
 
 ./script/install
 
+cd
 
+echo "Installing SSDV for FunCube mode"
+git clone https://github.com/alanbjohnston/ssdv.git  # install ssdv for FunCube images
+cd ssdv
+make
 cd
 
+echo "Installing fctelem binary v0.2 for FunCube mode"
+mkdir /home/pi/fctelem
+mkdir /home/pi/fctelem/public_html
+cd fctelem
+wget https://github.com/alanbjohnston/go/releases/download/v0.2/fctelem.zip
+unzip fctelem.zip
+
+cd
+echo "Installing fcdctl to set FunCubeDongle Pro gain"
+# sudo rm /var/lib/dpkg/info/python3-pip.list
+# sudo apt install python3-pip --reinstall
+# sudo apt-get install -y python3-smbus libusb-1.0
+cd
+git clone https://github.com/csete/fcdctl.git
+cd fcdctl
+make fcdpp
+
 git clone https://github.com/alanbjohnston/PiSSTVpp.git
 
 cd PiSSTVpp
@@ -125,6 +147,10 @@ cd rpitx
 
 cd
 
+git clone https://github.com/alanbjohnston/ssdv.git  # install ssdv for FunCube images
+cd ssdv
+make
+
 cd
 
 sudo cp ~/CubeSatSim/systemd/cubesatsim.service /etc/systemd/system/cubesatsim.service
@@ -139,6 +165,8 @@ sudo cp ~/CubeSatSim/systemd/command.service /etc/systemd/system/command.service
 
 sudo systemctl enable command
 
+sudo cp /home/pi/CubeSatSim/asound.conf /etc/asound.conf
+
 sudo cp /boot/config.txt /boot/config.txt.0
 
 sudo cp /boot/cmdline.txt /boot/cmdline.txt.0
diff --git a/main.c b/main.c
index 36dfce4c..7ef1b269 100644
--- a/main.c
+++ b/main.c
@@ -18,14 +18,14 @@
  *  You should have received a copy of the GNU General Public License
  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
- 
+
 #include "main.h"
 
 //#define HAB  // uncomment to change APRS icon from Satellite to Balloon and only BAT telemetry
 
 int main(int argc, char * argv[]) {
 	
-  printf("\n\nCubeSatSim v2.0 starting...\n\n");
+  printf("\n\nCubeSatSim v2.1 starting...\n\n");
 
   wiringPiSetup();	
 		
@@ -190,6 +190,9 @@ int main(int argc, char * argv[]) {
     } else if ( * argv[1] == 'm') {
       mode = CW;
       printf("Mode is CW\n");
+    } else if ( * argv[1] == 'j') {
+      mode = FC;
+      printf("Mode is FunCube\n");
     } else {
       printf("Mode is BPSK\n");
     }
@@ -228,6 +231,9 @@ int main(int argc, char * argv[]) {
      } else if ( mode_string == 'm') {
         mode = CW;
         printf("Mode is CW\n");
+     } else if ( mode_string == 'j') {
+        mode = FC;
+        printf("Mode is FunCube\n");
      } else if ( mode_string == 'e') {
         mode = REPEATER;
         printf("Mode is Repeater\n");
@@ -408,16 +414,17 @@ int main(int argc, char * argv[]) {
     batt = rnd_float(3.8, 4.3);
     speed = rnd_float(1.0, 2.5);
     eclipse = (rnd_float(-1, +4) > 0) ? 1.0 : 0.0;
+//    eclipse = 1;	  
     period = rnd_float(150, 300);
     tempS = rnd_float(20, 55);
     temp_max = rnd_float(50, 70);
     temp_min = rnd_float(10, 20);
 
-    #ifdef DEBUG_LOGGING
+//    #ifdef DEBUG_LOGGING
     for (int i = 0; i < 3; i++)
       printf("axis: %f angle: %f v: %f i: %f \n", axis[i], angle[i], volts_max[i], amps_max[i]);
     printf("batt: %f speed: %f eclipse_time: %f eclipse: %f period: %f temp: %f max: %f min: %f\n", batt, speed, eclipse_time, eclipse, period, tempS, temp_max, temp_min);
-    #endif
+//    #endif
 
     time_start = (long int) millis();
 
@@ -455,6 +462,7 @@ int main(int argc, char * argv[]) {
 	      
       printf("\n FSK Mode, %d bits per frame, %d bits per second, %d ms per frame, %d ms sample period\n",
         bufLen / (samples * frameCnt), bitRate, frameTime, samplePeriod);
+	   
     } else if (mode == BPSK) {
       bitRate = 1200;
       rsFrames = 3;
@@ -487,6 +495,43 @@ int main(int argc, char * argv[]) {
 //	printf(" %d", sin_map[j]);	 		
       }	 		
       printf("\n");
+	   
+    } else if (mode == FC) {  // for now copy BPSK settings
+      bitRate = 1200;
+//      rsFrames = 3;
+//      payloads = 6;
+//      rsFrameLen = 159;
+      headerLen = 768; // 8;
+      dataLen = 5200; // 78;
+      syncBits = 32;  // 31;
+      syncWord = 0x1acffc1d; // 0b1000111110011010010000101011101;
+//      parityLen = 32;
+      amplitude = 32767;
+      samples = S_RATE / bitRate;
+ //     bufLen = (frameCnt * (syncBits + 10 * (headerLen + rsFrames * (rsFrameLen + parityLen))) * samples);
+      bufLen = (headerLen + syncBits + dataLen)/8;
+
+ //        samplePeriod = ((float)((syncBits + 10 * (headerLen + rsFrames * (rsFrameLen + parityLen))))/(float)bitRate) * 1000 - 1800;
+      samplePeriod = 5000;  
+      //    samplePeriod = 3000;
+      //    sleepTime = 3.0;
+      //samplePeriod = 2200; // reduce dut to python and sensor querying delays
+//      sleepTime = 2.2f;
+	   
+//      frameTime = ((float)((float)bufLen / (samples * frameCnt * bitRate))) * 1000; // frame time in ms
+      frameTime = 5000;
+	   
+      printf("\n FC Mode, bufLen: %d,  %d bits per frame, %d bits per second, %d ms per frame %d ms sample period\n",
+      bufLen, bufLen / samples, bitRate, frameTime, samplePeriod);
+	   
+      sin_samples = S_RATE/freq_Hz;	 		
+      for (int j = 0; j < sin_samples; j++) {	 		
+        sin_map[j] = (short int)(amplitude * sin((float)(2 * M_PI * j / sin_samples)));	 
+
+      FILE * delete_image = popen("sudo rm /home/pi/CubeSatSim/image_file.bin", "r");  // delete any previous camera images
+      pclose(delete_image); 	      
+      }	 		
+      printf("\n");
    }
 	
   memset(voltage, 0, sizeof(voltage));
@@ -496,6 +541,9 @@ int main(int argc, char * argv[]) {
 	
   if (((mode == FSK) || (mode == BPSK))) // && !sim_mode)
       get_tlm_fox();	// fill transmit buffer with reset count 0 packets that will be ignored
+  else if (((mode == FC))) // && !sim_mode)
+      get_tlm_fc();	// fill transmit buffer with reset count 0 packets that will be ignored
+	
   firstTime = 1;
 	  
 //  if (!sim_mode)  // always read sensors, even in sim mode
@@ -581,16 +629,24 @@ int main(int argc, char * argv[]) {
             token = strtok(NULL, space);
           }
         }
-        if (voltage[map[BAT]] == 0.0)
-		batteryVoltage = 4.5;
+        if (voltage[map[BAT]] == 0.0)  // No BAT Board
+		if (voltage[map[BAT2]] == 0.0) // No BAT2 Board
+			batteryVoltage = 4.5;
+		else {
+			batteryVoltage = voltage[map[BAT2]];  // only BAT2 Board present
+			if (sim_mode && !sim_config) {	// if Voltage sensor on Battery board is present, exit simulated telemetry mode
+				sim_mode = FALSE; 
+				fprintf(stderr, "Turning off sim_mode since battery sensor 2 is present\n");
+			}
+		}
 	else  {
-		batteryVoltage = voltage[map[BAT]];
+		batteryVoltage = voltage[map[BAT]];  // BAT Board present
 		if (sim_mode && !sim_config) {	// if Voltage sensor on Battery board is present, exit simulated telemetry mode
 			sim_mode = FALSE; 
 			fprintf(stderr, "Turning off sim_mode since battery sensor is present\n");
 		}
 	}
-        batteryCurrent = current[map[BAT]];
+        batteryCurrent = current[map[BAT]] + current[map[BAT2]];  // Sum BAT and BAT2 currents
 	   
    }
 
@@ -699,7 +755,7 @@ int main(int argc, char * argv[]) {
       double Yv = eclipse * volts_max[1] * (float) sin((2.0 * 3.14 * time / (46.0 * speed)) + (3.14 / 2.0)) + rnd_float(-0.2, 0.2);
       double Zv = 2.0 * eclipse * volts_max[2] * (float) sin((2.0 * 3.14 * time / (46.0 * speed)) + 3.14 + angle[2]) + rnd_float(-0.2, 0.2);
 
-      // printf("Yi: %f Zi: %f %f %f Zv: %f \n", Yi, Zi, amps_max[2], angle[2], Zv);
+       printf("Yi: %f Zi: %f %f %f Zv: %f \n", Yi, Zi, amps_max[2], angle[2], Zv);
 
       current[map[PLUS_X]] = (Xi >= 0) ? Xi : 0;
       current[map[MINUS_X]] = (Xi >= 0) ? 0 : ((-1.0f) * Xi);
@@ -715,7 +771,7 @@ int main(int argc, char * argv[]) {
       voltage[map[PLUS_Z]] = (Zv >= 1) ? Zv : rnd_float(0.9, 1.1);
       voltage[map[MINUS_Z]] = (Zv <= -1) ? ((-1.0f) * Zv) : rnd_float(0.9, 1.1);
 
-      // printf("temp: %f Time: %f Eclipse: %d : %f %f | %f %f | %f %f\n",tempS, time, eclipse, voltage[map[PLUS_X]], voltage[map[MINUS_X]], voltage[map[PLUS_Y]], voltage[map[MINUS_Y]], current[map[PLUS_Z]], current[map[MINUS_Z]]);
+       printf("temp: %f Time: %f Eclipse: %d : %f %f | %f %f | %f %f\n",tempS, time, eclipse, voltage[map[PLUS_X]], voltage[map[MINUS_X]], voltage[map[PLUS_Y]], voltage[map[MINUS_Y]], current[map[PLUS_Z]], current[map[MINUS_Z]]);
 
       tempS += (eclipse > 0) ? ((temp_max - tempS) / 50.0f) : ((temp_min - tempS) / 50.0f);
       tempS += +rnd_float(-1.0, 1.0);
@@ -725,12 +781,13 @@ int main(int argc, char * argv[]) {
       voltage[map[BAT2]] = 0.0; // rnd_float(5.0, 5.005);
       current[map[BAT2]] = 0.0; // rnd_float(158, 171);
 
-      //  float charging = current[map[PLUS_X]] + current[map[MINUS_X]] + current[map[PLUS_Y]] + current[map[MINUS_Y]] + current[map[PLUS_Z]] + current[map[MINUS_Z]];
-      float charging = eclipse * (fabs(amps_max[0] * 0.707) + fabs(amps_max[1] * 0.707) + rnd_float(-4.0, 4.0));
+      float charging = current[map[PLUS_X]] + current[map[MINUS_X]] + current[map[PLUS_Y]] + current[map[MINUS_Y]] + current[map[PLUS_Z]] + current[map[MINUS_Z]];
+//      float charging = eclipse * (fabs(amps_max[0] * 0.707) + fabs(amps_max[1] * 0.707) + rnd_float(-4.0, 4.0));
 
-      current[map[BAT]] = ((current[map[BAT2]] * voltage[map[BAT2]]) / batt) - charging;
+//      current[map[BAT]] = ((current[map[BAT2]] * voltage[map[BAT2]]) / batt) - charging;
+      current[map[BAT]] = rnd_float(285, 305) - charging;
 
-      //  printf("charging: %f bat curr: %f bus curr: %f bat volt: %f bus volt: %f \n",charging, current[map[BAT]], current[map[BAT2]], batt, voltage[map[BAT2]]);
+        printf("charging: %f bat curr: %f bus curr: %f bat volt: %f bus volt: %f \n",charging, current[map[BAT]], current[map[BAT2]], batt, voltage[map[BAT2]]);
 
       batt -= (batt > 3.5) ? current[map[BAT]] / 30000 : current[map[BAT]] / 3000;
       if (batt < 3.0) {
@@ -766,9 +823,9 @@ int main(int argc, char * argv[]) {
       fclose(cpuTempSensor);
     }   
 	  
-    #ifdef DEBUG_LOGGING
-//    fprintf(stderr, "INFO: Battery voltage: %5.2f V  Threshold %5.2f V Current: %6.1f mA Threshold: %6.1f mA\n", batteryVoltage, voltageThreshold, batteryCurrent, currentThreshold);
-    #endif
+//    #ifdef DEBUG_LOGGING
+    fprintf(stderr, "INFO: Battery voltage: %5.2f V  Threshold %5.2f V Current: %6.1f mA Threshold: %6.1f mA\n", batteryVoltage, voltageThreshold, batteryCurrent, currentThreshold);
+//    #endif
 	  
     if ((batteryCurrent > currentThreshold) && (batteryVoltage < (voltageThreshold + 0.15)) && !sim_mode && !hab_mode)
     {
@@ -869,6 +926,8 @@ int main(int argc, char * argv[]) {
 	    
     } else if ((mode == FSK) || (mode == BPSK)) {// FSK or BPSK
       get_tlm_fox();
+    } else if ((mode == FC)) {
+      get_tlm_fc();
     } else {  				// SSTV	    
 //      fprintf(stderr, "Sleeping\n");
       sleep(30);	    
@@ -879,7 +938,7 @@ int main(int argc, char * argv[]) {
     #endif
   }
 
-  if (mode == BPSK) {
+  if ((mode == BPSK) || (mode == FC)) {
 //    digitalWrite(txLed, txLedOn);
     #ifdef DEBUG_LOGGING
 //    printf("Tx LED On 1\n");
@@ -1663,7 +1722,7 @@ void get_tlm_fox() {
         val = sync;
         data = val & 1 << (bit - 1);
         //   	printf ("%d i: %d new frame %d sync bit %d = %d \n",
-        //  		 ctr/SAMPLES, i, frames, bit, (data > 0) );
+        //  		 ctr/, i, frames, bit, (data > 0) );
         if (mode == FSK) {
           phase = ((data != 0) * 2) - 1;
           //		printf("Sending a %d\n", phase);
@@ -1691,7 +1750,7 @@ void get_tlm_fox() {
         val = data10[symbol];
         data = val & 1 << (bit - 1);
         //		printf ("%d i: %d new frame %d data10[%d] = %x bit %d = %d \n",
-        //	    		 ctr/SAMPLES, i, frames, symbol, val, bit, (data > 0) );
+        //	    		 ctr/, i, frames, symbol, val, bit, (data > 0) );
         if (mode == FSK) {
           phase = ((data != 0) * 2) - 1;
           //			printf("Sending a %d\n", phase);
@@ -1713,7 +1772,7 @@ void get_tlm_fox() {
   //	printf("\ctr/samples = %d ctr/(samples*10) = %d\n\n", ctr/samples, ctr/(samples*10));
   #endif
 
-  int error = 0;
+  //int error = 0;
   // int count;
   //  for (count = 0; count < dataLen; count++) {
   //      printf("%02X", b[count]);
@@ -1722,6 +1781,8 @@ void get_tlm_fox() {
 
   // socket write
 
+  socket_send(ctr);
+/*	
   if (!socket_open && transmit) {
     printf("Opening socket!\n");
  //   struct sockaddr_in address;
@@ -1810,6 +1871,7 @@ void get_tlm_fox() {
       sock_ret = send(sock, &buffer[sock_ret], (unsigned int)(ctr * 2 + 2 - sock_ret), 0);
 //      printf("socket send 2 %d ms bytes: %d \n\n", millis() - start, sock_ret);
     }
+*/	  
 	  
     loop_count++;	  
     if ((firstTime == 1) || (((loop_count % 180) == 0) && (mode == FSK)) || (((loop_count % 80) == 0) && (mode == BPSK))) // do first time and was every 180 samples
@@ -1830,41 +1892,42 @@ void get_tlm_fox() {
 	    
       for (int times = 0; times < max; times++) 	    
       {
-	      start = millis();  // send frame until buffer fills
-	      sock_ret = send(sock, buffer, (unsigned int)(ctr * 2 + 2), 0);
+///	      start = millis();  // send frame until buffer fills
+	      socket_send(ctr);
+///	      sock_ret = send(sock, buffer, (unsigned int)(ctr * 2 + 2), 0);
 //	      printf("socket send %d in %d ms bytes: %d \n\n",times + 2, (unsigned int)millis() - start, sock_ret);
 	      
-	      if ((millis() - start) > 500) {
-		      printf("Buffer over filled!\n");
-		      break;
-	      }
+///	      if ((millis() - start) > 500) {
+///		      printf("Buffer over filled!\n");
+///		      break;
+///	      }
 
-	      if (sock_ret < (ctr * 2 + 2)) {
+///	      if (sock_ret < (ctr * 2 + 2)) {
 	  //    printf("Not resending\n");
-		sleep(0.5);
-		sock_ret = send(sock, &buffer[sock_ret], (unsigned int)(ctr * 2 + 2 - sock_ret), 0);
-		printf("socket resend %d in %d ms bytes: %d \n\n",times, millis() - start, sock_ret);
-	      }
+///		sleep(0.5);
+///		sock_ret = send(sock, &buffer[sock_ret], (unsigned int)(ctr * 2 + 2 - sock_ret), 0);
+///		printf("socket resend %d in %d ms bytes: %d \n\n",times, millis() - start, sock_ret);
+///	      }
       }
       sampleTime = (unsigned int) millis(); // resetting time for sleeping
-      fflush(stdout);
+ //     fflush(stdout);
 //      if (firstTime == 1)
 //	      max -= 1;
     }
 
-    if (sock_ret == -1) {
-      printf("Error: %s \n", strerror(errno));
-      socket_open = 0;
+///    if (sock_ret == -1) {
+///      printf("Error: %s \n", strerror(errno));
+///      socket_open = 0;
       //transmitStatus = -1;
-    }
-  }
+///    }
+///  }
   if (!transmit) {
     fprintf(stderr, "\nNo CubeSatSim Band Pass Filter detected.  No transmissions after the CW ID.\n");
     fprintf(stderr, " See http://cubesatsim.org/wiki for info about building a CubeSatSim\n\n");
   }
 
-  if (socket_open == 1)	
-    firstTime = 0;
+///  if (socket_open == 1)	
+///    firstTime = 0;
 //  else if (frames_sent > 0) //5)
 //    firstTime = 0;
 
@@ -1928,13 +1991,16 @@ void write_wave(int i, short int *buffer)
 		}
 		else
 		{
-			if ((ctr - flip_ctr) < smaller)
-//  		 		buffer[ctr++] = (short int)(amplitude * 0.4 * phase * sin((float)(2*M_PI*i*freq_Hz/S_RATE)));	 	  		 	buffer[ctr++] = (short int)(amplitude * 0.4 * phase * sin((float)(2*M_PI*i*freq_Hz/S_RATE)));	
+			if ((ctr - flip_ctr) < smaller) {
+//  		 		buffer[ctr++] = (short int)(amplitude * 0.4 * phase * sin((float)(2*M_PI*i*freq_Hz/S_RATE)));	
   		 		buffer[ctr++] = (short int)(phase * sin_map[ctr % sin_samples] / 2);
+//				if (ctr < 1000) printf("*");
+			}
 		else
-//  		 		buffer[ctr++] = (short int)(amplitude * 0.4 * phase * sin((float)(2*M_PI*i*freq_Hz/S_RATE)));	 	 		 	buffer[ctr++] = (short int)(amplitude * phase * sin((float)(2*M_PI*i*freq_Hz/S_RATE)));	
-  		 		buffer[ctr++] = (short int)(phase * sin_map[ctr % sin_samples]); 		 } 			
-//		printf("%d %d \n", i, buffer[ctr - 1]);
+//  		 		buffer[ctr++] = (short int)(amplitude * 0.4 * phase * sin((float)(2*M_PI*i*freq_Hz/S_RATE)));
+  		 		buffer[ctr++] = (short int)(phase * sin_map[ctr % sin_samples]); 
+		} 			
+//		if (ctr < 1000) printf("%d %d %d \n", ctr, i, buffer[ctr - 1]);
 
 }
 
@@ -2204,7 +2270,8 @@ if (setting == ON) {
 		FILE *command = popen("touch /home/pi/CubeSatSim/battery_saver", "r");
 		pclose(command);
 		fprintf(stderr,"Turning Safe Mode ON\n"); 
-		fprintf(stderr,"Turning Battery saver mode ON\n");  
+		fprintf(stderr,"Turning Battery saver mode ON\n"); 
+		battery_saver_mode = ON;
 		if ((mode == AFSK) || (mode == SSTV) || (mode == CW)) {
 			command = popen("echo 'reboot due to turning ON Safe Mode!' | wall", "r");
 			pclose(command);
@@ -2221,6 +2288,7 @@ if (setting == ON) {
 		FILE *command = popen("rm /home/pi/CubeSatSim/battery_saver", "r");
 		pclose(command);
 		fprintf(stderr,"Turning Battery saver mode OFF\n"); 
+		battery_saver_mode = OFF;
 		if ((mode == AFSK) || (mode == SSTV) || (mode == CW)) {
 			command = popen("echo 'reboot due to turning OFF Safe Mode!' | wall", "r");
 			pclose(command);
@@ -2238,3 +2306,400 @@ if (setting == ON) {
   }
   return;
 }
+
+void get_tlm_fc() {  // FunCube Mode telemetry generation
+
+//# define FC_EM
+#define JY_1	
+	
+	/* create data, stream, and waveform buffers */
+
+  	unsigned char source_bytes[256];
+	int byte_count = 256;
+
+	/* write telemetry into data buffer */
+
+//	printf("\nBLOCKSIZE = %d\n", BLOCKSIZE);
+//	printf("\nSYMPBLOCK = %d\n", SYMPBLOCK);
+
+	memset(source_bytes, 0x00, sizeof(source_bytes));
+#ifdef FC_EM	
+	source_bytes[0] = 0b00000001 ;	// Sat Id is FunCube-EM
+#endif
+#ifdef JY_1	
+//	source_bytes[0] = 0b11000001 ;    // Sat Id is extended, Frame 2 (RT2 + WO2)
+	source_bytes[0] = 0xE0 | 0x20 | 0x00; // 1;    // Sat Id is extended, Frame 34 (RT2 + IMG2)
+
+	source_bytes[0] = source_bytes[0] | ( 0x01 & (uint8_t)(sequence % 2));  // alternate last bit for RT1, RT2.
+
+	//	source_bytes[1] = 0x08 ; // extended Nayify - works per code
+	source_bytes[1] = 0x10 ; // extended JY-1 - works, no documentation
+	int extended = 1;
+
+//	if (sequence > 10) {
+		if (image_file == NULL)  {
+			image_file = fopen("/home/pi/CubeSatSim/image_file.bin", "r");
+			image_id++;
+			printf("Opening file image_file.bin for image_id: %d\n", image_id);
+		}
+//	}
+	int pos = FC_PAYLOAD + extended;	  
+	int value;
+	if (image_file != NULL) {
+		printf("Writing image data to payload\n");
+		while ((pos < 256) && ((value = getc(image_file)) != EOF)) {
+			source_bytes[pos++] = value;
+//			printf("%2x ", value);
+		}
+		if (value == EOF) {
+			image_file = NULL;
+			printf("End of file reached! Delete image_file.bin");
+			FILE * delete_image = popen("sudo rm /home/pi/CubeSatSim/image_file.bin", "r");
+  			pclose(delete_image); 
+		}
+	}
+	
+#endif	
+
+//	printf("Volts: %f %f %f %f \n", voltage[map[BAT]], voltage[map[PLUS_X]] , voltage[map[PLUS_Y]], voltage[map[PLUS_Z]]); 
+//	printf("AmpsPlus: %f %f %f %f \n", current[map[BAT]], current[map[PLUS_X]] , current[map[PLUS_Y]], current[map[PLUS_Z]]); 
+//	printf("AmpsMinus: %f %f %f %f \n", current[map[BAT2]], current[map[MINUS_X]] , current[map[MINUS_Y]], current[map[MINUS_Z]]); 
+
+	float xmax = (voltage[map[PLUS_X]] > voltage[map[MINUS_X]]) ? voltage[map[PLUS_X]] : voltage[map[MINUS_X]];
+	float ymax = (voltage[map[PLUS_Y]] > voltage[map[MINUS_Y]]) ? voltage[map[PLUS_Y]] : voltage[map[MINUS_Y]];
+	float zmax = (voltage[map[PLUS_Z]] > voltage[map[MINUS_Z]]) ? voltage[map[PLUS_Z]] : voltage[map[MINUS_Z]];
+
+//	printf("Vmax: %f %f %f \n", xmax, ymax, zmax);
+	
+	uint16_t x = (uint16_t)(xmax * 1000) & 0x3fff;  // 14 bits
+	uint16_t y = (uint16_t)(ymax * 1000) & 0x3fff;
+	uint16_t z = (uint16_t)(zmax * 1000) & 0x3fff;
+	uint16_t b = (uint16_t)(voltage[map[BAT]] * 1000) & 0x3fff;
+
+	uint16_t ix = (uint16_t)((current[map[PLUS_X]] + current[map[MINUS_X]])) & 0x3ff;  // 10 bits
+	uint16_t iy = (uint16_t)((current[map[PLUS_Y]] + current[map[MINUS_Y]])) & 0x3ff;
+	uint16_t iz = (uint16_t)((current[map[PLUS_Z]] + current[map[MINUS_Z]])) & 0x3ff;
+
+	uint16_t ic = 0; 
+	uint16_t ib = 0;
+	
+	if (current[map[BAT]] < 0 ) 
+		ic = (uint16_t)(current[map[BAT]] * (-1)) & 0x3ff;  // charging current
+	else 
+		ib = (uint16_t)(current[map[BAT]]) & 0x3ff;  // supplying current
+
+//	printf("X %x Y %x Z %x B %x\n", x, y, z, b);
+//	printf("iX %x iY %x iZ %x iB %x iC\n", ix, iy, iz, ib, ic);
+	
+#ifdef JY_1	
+	source_bytes[extended + FC_EPS + 0] = 0xff & (x >> 6);  // Vx
+	source_bytes[extended + FC_EPS + 1] = 0xfc & (x << 2);
+        source_bytes[extended + FC_EPS + 1] = source_bytes[extended + FC_EPS + 1] | (0x03 & (y >> 12));
+	source_bytes[extended + FC_EPS + 2] = 0xff & (y >> 2);  // Vy
+	source_bytes[extended + FC_EPS + 3] = 0xf0 & (y << 4);
+
+	source_bytes[extended + FC_EPS + 3] = source_bytes[extended + FC_EPS + 3] | (0x0f & (z >> 10));
+	source_bytes[extended + FC_EPS + 4] = 0xff & (z >> 2);  // Vz
+	source_bytes[extended + FC_EPS + 5] = 0xc0 & (z << 6);
+	
+	source_bytes[extended + FC_EPS + 5] = source_bytes[extended + FC_EPS + 5] | (0x3f & (b >> 8));  
+	source_bytes[extended + FC_EPS + 6] = 0xff & (b >> 0);  // Vb
+
+	source_bytes[extended + FC_EPS + 7] = 0xff & (ix >> 2);   // ix
+	source_bytes[extended + FC_EPS + 8] = 0xc0 & (iy << 6);   // iy
+
+	source_bytes[extended + FC_EPS + 8] = source_bytes[extended + FC_EPS + 8] | (0x3f & (iy >> 4));  
+	source_bytes[extended + FC_EPS + 9] = 0xf0 & (iy << 4);	
+
+	source_bytes[extended + FC_EPS + 9] = source_bytes[extended + FC_EPS + 9] | (0x0f & (iz >> 6));  
+	source_bytes[extended + FC_EPS + 10] = 0x3f & (iz << 2);  // iz
+
+	source_bytes[extended + FC_EPS + 10] = source_bytes[extended + FC_EPS + 10] | (0x03 & (ic >> 8));  
+	source_bytes[extended + FC_EPS + 11] = 0xff & (ic << 0);  // ic battery charging curent	
+
+	source_bytes[extended + FC_EPS + 12] = 0xff & (ib >> 2);  // ib battery discharging current
+	source_bytes[extended + FC_EPS + 13] = 0xc0 & (ib << 6);  
+
+	source_bytes[extended + FC_EPS + 13] = source_bytes[extended + FC_EPS + 13] | 0x3f & (((unsigned long int)reset_count) >> 2);
+	source_bytes[extended + FC_EPS + 14] = 0xff & (((unsigned long int)reset_count) << 6); // reset count
+
+	uint8_t temp = (int)(other[IHU_TEMP] + 0.5);
+
+	source_bytes[extended + FC_EPS + 17] = source_bytes[extended + FC_EPS + 17] | 0x3f & (temp >> 2);  // cpu temp
+	source_bytes[extended + FC_EPS + 18] = 0xff & (temp << 6);
+
+	source_bytes[extended + 48] = 0x0c;  // Antenna 1 and 2 deployed
+	
+	source_bytes[extended + 49] = 0xff & ((unsigned long int)sequence >> 16);  // sequence number
+	source_bytes[extended + 50] = 0xff & ((unsigned long int)sequence >> 8);
+	source_bytes[extended + 51] = 0xff & (unsigned long int)sequence++;
+
+	uint16_t groundCommandCount = 0;
+	FILE * command_count_file = fopen("/home/pi/CubeSatSim/command_count.txt", "r");
+	if (command_count_file != NULL) {	
+		char count_string[10];	
+		if ( (fgets(count_string, 10, command_count_file)) != NULL)
+			groundCommandCount = (uint16_t) atoi(count_string);     
+	} else 
+		printf("Error opening command_count.txt!\n");
+	fclose(command_count_file);
+
+//	source_bytes[extended + 52] = 0xfc & (groundCommandCount << 2);  // command doesn't work
+
+	source_bytes[extended + 53] = 0x0f;  // SW valid 
+	source_bytes[extended + 54] = 0xe0;  // SW valid
+
+	if ((ix + iy + iz) < 4)
+		source_bytes[extended + 54] = source_bytes[extended + 54] | 0x10;  // eclipse
+	if (SafeMode == 1)
+		source_bytes[extended + 54] = source_bytes[extended + 54] | 0x08;  // safe mode
+#endif
+	
+#ifdef FC_EM
+	source_bytes[FC_EPS + 0] = 0xff & (((unsigned int)((voltage[map[PLUS_X]] + voltage[map[MINUS_X]]) * 1000) >> 8));  // mV
+	source_bytes[FC_EPS + 1] = 0xff & ((unsigned int)((voltage[map[PLUS_X]] + voltage[map[MINUS_X]]) * 1000));
+	source_bytes[FC_EPS + 2] = 0xff & (((unsigned int)((voltage[map[PLUS_Y]] + voltage[map[MINUS_Y]]) * 1000) >> 8));  // mV
+	source_bytes[FC_EPS + 3] = 0xff & ((unsigned int)((voltage[map[PLUS_Y]] + voltage[map[MINUS_Y]]) * 1000));
+	source_bytes[FC_EPS + 4] = 0xff & (((unsigned int)((voltage[map[PLUS_Z]] + voltage[map[MINUS_Z]]) * 1000) >> 8));  // mV
+	source_bytes[FC_EPS + 5] = 0xff & ((unsigned int)((voltage[map[PLUS_Z]] + voltage[map[MINUS_Z]]) * 1000));
+	unsigned int total_solar_current = (unsigned int) (current[map[PLUS_X]] + current[map[MINUS_X]] 
+				+ current[map[PLUS_Y]] + current[map[MINUS_Y]] 
+				+ current[map[PLUS_Z]] + current[map[MINUS_Z]]);
+	source_bytes[FC_EPS + 6] = 0xff & total_solar_current >> 8;
+	source_bytes[FC_EPS + 7] = 0xff & total_solar_current;		
+	source_bytes[FC_EPS + 8] = 0xff & (((unsigned int)(voltage[map[BAT]] * 1000) >> 8));  // mV
+	source_bytes[FC_EPS + 9] = 0xff & ((unsigned int)(voltage[map[BAT]] * 1000));
+	source_bytes[FC_EPS + 10] = 0xff & (((unsigned int)(current[map[BAT]] * 1) >> 8));  // mA
+	source_bytes[FC_EPS + 11] = 0xff & ((unsigned int)(current[map[BAT]] * 1));
+	source_bytes[FC_EPS + 12] = 0xff & (((unsigned long int)reset_count >> 8));
+	source_bytes[FC_EPS + 13] = 0xff & ((unsigned long int)reset_count);
+	
+	source_bytes[FC_SW + 0] = 0xff & ((unsigned long int)sequence >> 16); // Sequence number
+	source_bytes[FC_SW + 1] = 0xff & ((unsigned long int)sequence >> 8);
+	source_bytes[FC_SW + 2] = 0xff & (unsigned long int)sequence++;
+		
+#endif
+
+/**/
+	printf("\nsource_bytes\n");
+	for (int i=0; i<256; i++)
+		printf("%x ", source_bytes[i]);
+	printf("\n\n");
+/**/
+
+	/* convert data buffer into stream buffer */
+
+	const unsigned char* encoded_bytes = encode(source_bytes, byte_count);
+/*
+	printf("\nencoded_bytes\n");
+	for (int i=0; i<5200; i++)
+		printf("%d", encoded_bytes[i]);
+	printf("\n\n");
+*/	
+	/* convert to waveform buffer */
+
+    int data;
+    int val;
+    int i;	
+    ctr = 0;
+    int symbol = 0;
+    smaller = (int) (S_RATE / (2 * freq_Hz));
+//    printf("\n\nsmaller = %d \n\n",smaller);	
+	
+    for (i = 1; i <= headerLen * samples; i++) {
+      write_wave(ctr, buffer);	
+      if ((i % samples) == 0) {
+	phase *= -1;
+        if ((ctr - smaller) > 0) {
+		int j;
+        	for (j = 1; j <= smaller; j++) {
+                	buffer[ctr - j] = buffer[ctr - j] * 0.5;
+//			if (ctr < 1000) printf("# %d %d\n", ctr - j, buffer[ctr - j]);
+		}
+        }
+        flip_ctr = ctr;
+      }
+    }
+	
+    for (i = 1; i <= syncBits * samples; i++) {
+      write_wave(ctr, buffer);
+      //		printf("%d ",ctr);
+      if ((i % samples) == 0) {
+        int bit = syncBits - i / samples + 1;
+        val = syncWord;
+        data = val & 1 << (bit - 1);
+//           	printf ("--- %d i: %d sync bit %d = %d \n",
+//          		 ctr, i, bit, (data > 0) );        
+        if (data == 0) {
+            phase *= -1;
+            if ((ctr - smaller) > 0) {
+	      int j;
+              for (j = 1; j <= smaller; j++)
+                buffer[ctr - j] = buffer[ctr - j] * 0.5;
+            }
+            flip_ctr = ctr;
+        }
+      }
+    }
+	
+    for (i = 1; i <= (dataLen * samples); i++) // 5200   
+    {
+      write_wave(ctr, buffer);
+      if ((i % samples) == 0) {
+        symbol = i / samples - 1;
+//	if (i < 100) printf("symbol = %d\n",symbol);      
+	data = encoded_bytes[symbol];      
+	if (data == 0) {
+	   phase *= -1;
+	   if ((ctr - smaller) > 0) {
+	     int j;   
+	     for (j = 1; j <= smaller; j++) {
+		buffer[ctr - j] = buffer[ctr - j] * 0.5;
+//		if (ctr < 1000) printf("# %d %d\n", ctr - j, buffer[ctr - j]);
+	     }
+           }
+	   flip_ctr = ctr;
+	}
+      }
+    }
+// printf("symbol = %d\n",symbol);
+// printf("\nctr = %d\n\n", ctr);
+
+//  socket_send((((headerLen + syncBits + dataLen) * samples) * 2) + 2);
+  socket_send(ctr);
+	
+  if (!transmit) {
+    fprintf(stderr, "\nNo CubeSatSim Band Pass Filter detected.  No transmissions after the CW ID.\n");
+    fprintf(stderr, " See http://cubesatsim.org/wiki for info about building a CubeSatSim\n\n");
+  }
+
+  int startSleep = millis();	    
+  if ((millis() - sampleTime) < ((unsigned int)frameTime)) // - 750 + pi_zero_2_offset))  
+        sleep(1.0); 
+  while ((millis() - sampleTime) < ((unsigned int)frameTime)) // - 750 + pi_zero_2_offset))  
+        sleep(0.1); 
+  printf("Start sleep %d Sleep period: %d  while period: %d\n", startSleep, millis() - startSleep, millis() - sampleTime);
+  sampleTime = (unsigned int) millis(); // resetting time for sleeping
+  fflush(stdout);
+}
+
+void socket_send(int length) {
+
+  printf("Socket_send!\n");	
+  int error = 0;
+
+  if (!socket_open && transmit) { // open socket if not open
+    printf("Opening socket!\n");
+ //   struct sockaddr_in address;
+ //   int valread;
+    struct sockaddr_in serv_addr;
+    //    char *hello = "Hello from client"; 
+    //    char buffer[1024] = {0}; 
+    if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+      printf("\n Socket creation error \n");
+      error = 1;
+    }
+
+    memset( & serv_addr, '0', sizeof(serv_addr));
+
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_port = htons(PORT);
+
+    // Convert IPv4 and IPv6 addresses from text to binary form 
+    if (inet_pton(AF_INET, "127.0.0.1", & serv_addr.sin_addr) <= 0) {
+      printf("\nInvalid address/ Address not supported \n");
+      error = 1;
+    }
+
+    if (connect(sock, (struct sockaddr * ) & serv_addr, sizeof(serv_addr)) < 0) {
+      printf("\nConnection Failed \n");
+      printf("Error: %s\n", strerror(errno));
+      error = 1;
+
+    // try again
+      error = 0;
+      if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+        printf("\n Socket creation error \n");
+        error = 1;
+      }
+
+      memset( & serv_addr, '0', sizeof(serv_addr));
+
+      serv_addr.sin_family = AF_INET;
+      serv_addr.sin_port = htons(PORT);
+
+      // Convert IPv4 and IPv6 addresses from text to binary form 
+      if (inet_pton(AF_INET, "127.0.0.1", & serv_addr.sin_addr) <= 0) {
+        printf("\nInvalid address/ Address not supported \n");
+        error = 1;
+      }
+
+      if (connect(sock, (struct sockaddr * ) & serv_addr, sizeof(serv_addr)) < 0) {
+        printf("\nConnection Failed \n");
+        printf("Error: %s\n", strerror(errno));
+        error = 1;
+      }	    
+    }
+    if (error == 1) {
+      printf("Socket error count: %d\n", error_count);  	    
+//    ; //transmitStatus = -1;
+      if (error_count++ > 5) { 
+	  printf("Restarting transmit\n");    
+  	  FILE * transmit_restartf = popen("sudo systemctl restart transmit", "r");
+  	  pclose(transmit_restartf);	      
+          sleep(10);  // was 5 // sleep if socket connection refused
+      }	    
+    }
+    else {
+      socket_open = 1;
+      error_count = 0;	    
+    }
+  }
+
+/* write waveform buffer over socket */
+
+//  int length = (((headerLen + syncBits + dataLen) * samples) * 2) + 2;	// ctr * 2 + 2 like bpsk due to 2 bytes per sample.
+  length = length * 2 + 2;  // convert from samples to bytes
+//  printf("length in bytes: %d\n", length);	
+
+  if (!error && transmit) {
+ //   printf("Sending %d buffer bytes over socket after %d ms!\n", ctr, (long unsigned int)millis() - start);
+    start = millis();
+    int sock_ret = send(sock, buffer, length, 0);
+    printf("socket send 1 %d ms bytes: %d \n\n", (unsigned int)millis() - start, sock_ret);
+    fflush(stdout);	  
+    
+    if (sock_ret < length) {
+  //    printf("Not resending\n");
+      sleep(0.5);
+      sock_ret = send(sock, &buffer[sock_ret], length - sock_ret, 0);
+//      printf("socket send 2 %d ms bytes: %d \n\n", millis() - start, sock_ret);
+    }
+	  
+//    loop_count++;	  
+
+    if (sock_ret == -1) {
+      printf("Error: %s \n", strerror(errno));
+      socket_open = 0;
+    }
+  }
+
+/*	
+  if (!transmit) {
+    fprintf(stderr, "\nNo CubeSatSim Band Pass Filter detected.  No transmissions after the CW ID.\n");
+    fprintf(stderr, " See http://cubesatsim.org/wiki for info about building a CubeSatSim\n\n");
+  }
+
+  int startSleep = millis();	    
+  if ((millis() - sampleTime) < ((unsigned int)frameTime)) // - 750 + pi_zero_2_offset))  
+        sleep(1.0); 
+  while ((millis() - sampleTime) < ((unsigned int)frameTime)) // - 750 + pi_zero_2_offset))  
+        sleep(0.1); 
+  printf("Start sleep %d Sleep period: %d  while period: %d\n", startSleep, millis() - startSleep, millis() - sampleTime);
+  sampleTime = (unsigned int) millis(); // resetting time for sleeping
+  fflush(stdout);
+	*/
+	
+  if (socket_open == 1)	
+    firstTime = 0;
+}
diff --git a/main.h b/main.h
index 6708d1af..8c886988 100644
--- a/main.h
+++ b/main.h
@@ -13,6 +13,7 @@
 #include <time.h>
 #include <math.h>
 //#include "TelemEncoding.h"
+#include "codecAO40.h"
 #include <sys/socket.h>
 #include <stdlib.h>
 #include <netinet/in.h>
@@ -52,6 +53,10 @@
 #define XS2 21  
 #define XS3 22
 #define SENSOR_FIELDS 26
+#define FC_EPS 1
+#define FC_BOB 25
+#define FC_SW 50
+#define FC_PAYLOAD 55
 
 #define RSSI 0
 #define IHU_TEMP 2
@@ -76,6 +81,7 @@ float toAprsFormat(float input);
 float rnd_float(double min, double max);
 void get_tlm();
 void get_tlm_fox();
+void get_tlm_fc();
 int encodeA(short int * b, int index, int val);
 int encodeB(short int * b, int index, int val);
 void config_x25();
@@ -86,7 +92,10 @@ void update_rs(unsigned char parity[32], unsigned char c);
 void write_little_endian(unsigned int word, int num_bytes, FILE *wav_file);
 static int init_rf();
 extern int Encode_8b10b[][256];
+extern const unsigned char ALPHA_TO[];
+// const unsigned char *CCodecAO40::encode(unsigned char *source_bytes, int byte_count);
 void program_radio();
+void socket_send(int length);
 
 int socket_open = 0;
 int sock = 0;
@@ -100,6 +109,9 @@ FILE * file1;
 short int buffer[2336400]; // max size for 10 frames count of BPSK
 FILE *sopen(const char *program);
 FILE *telem_file;	
+long int sequence = 0;
+int image_id = 0;
+FILE *image_file;
 
 #define S_RATE	(48000) // (44100)
 
@@ -108,6 +120,7 @@ FILE *telem_file;
 #define BPSK 3
 #define SSTV 4
 #define CW 5
+#define FC 7
 #define REPEATER 11
 #define TXCOMMAND 12
 
@@ -203,3 +216,103 @@ long int loopTime;
 
 int error_count = 0;
 int groundCommandCount = 0;
+
+    unsigned char m_RS_block[RSBLOCKS][NROOTS]; /* RS parity blocks */
+    unsigned char m_encoded[SYMPBLOCK] ;       /* encoded symbols */
+    int m_encoded_bytes;               /* Byte counter for encode_data() */
+    int m_ileaver_index;               /* Byte counter for interleaver */
+    unsigned char m_conv_sr;           /* Convolutional encoder shift register state */
+
+
+// from funcubeLib/common/fecConstants.h
+
+const unsigned char RS_poly[] = {
+    249, 59, 66,  4, 43,126,251, 97, 30,  3,213, 50, 66,170,  5, 24
+};
+
+/* Tables for RS decoder */
+/* Galois field log/antilog tables */
+const unsigned char ALPHA_TO[] =
+{
+    0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x87, 0x89, 0x95, 0xad, 0xdd, 0x3d, 0x7a, 0xf4,
+    0x6f, 0xde, 0x3b, 0x76, 0xec, 0x5f, 0xbe, 0xfb, 0x71, 0xe2, 0x43, 0x86, 0x8b, 0x91, 0xa5, 0xcd,
+    0x1d, 0x3a, 0x74, 0xe8, 0x57, 0xae, 0xdb, 0x31, 0x62, 0xc4, 0x0f, 0x1e, 0x3c, 0x78, 0xf0, 0x67,
+    0xce, 0x1b, 0x36, 0x6c, 0xd8, 0x37, 0x6e, 0xdc, 0x3f, 0x7e, 0xfc, 0x7f, 0xfe, 0x7b, 0xf6, 0x6b,
+    0xd6, 0x2b, 0x56, 0xac, 0xdf, 0x39, 0x72, 0xe4, 0x4f, 0x9e, 0xbb, 0xf1, 0x65, 0xca, 0x13, 0x26,
+    0x4c, 0x98, 0xb7, 0xe9, 0x55, 0xaa, 0xd3, 0x21, 0x42, 0x84, 0x8f, 0x99, 0xb5, 0xed, 0x5d, 0xba,
+    0xf3, 0x61, 0xc2, 0x03, 0x06, 0x0c, 0x18, 0x30, 0x60, 0xc0, 0x07, 0x0e, 0x1c, 0x38, 0x70, 0xe0,
+    0x47, 0x8e, 0x9b, 0xb1, 0xe5, 0x4d, 0x9a, 0xb3, 0xe1, 0x45, 0x8a, 0x93, 0xa1, 0xc5, 0x0d, 0x1a,
+    0x34, 0x68, 0xd0, 0x27, 0x4e, 0x9c, 0xbf, 0xf9, 0x75, 0xea, 0x53, 0xa6, 0xcb, 0x11, 0x22, 0x44,
+    0x88, 0x97, 0xa9, 0xd5, 0x2d, 0x5a, 0xb4, 0xef, 0x59, 0xb2, 0xe3, 0x41, 0x82, 0x83, 0x81, 0x85,
+    0x8d, 0x9d, 0xbd, 0xfd, 0x7d, 0xfa, 0x73, 0xe6, 0x4b, 0x96, 0xab, 0xd1, 0x25, 0x4a, 0x94, 0xaf,
+    0xd9, 0x35, 0x6a, 0xd4, 0x2f, 0x5e, 0xbc, 0xff, 0x79, 0xf2, 0x63, 0xc6, 0x0b, 0x16, 0x2c, 0x58,
+    0xb0, 0xe7, 0x49, 0x92, 0xa3, 0xc1, 0x05, 0x0a, 0x14, 0x28, 0x50, 0xa0, 0xc7, 0x09, 0x12, 0x24,
+    0x48, 0x90, 0xa7, 0xc9, 0x15, 0x2a, 0x54, 0xa8, 0xd7, 0x29, 0x52, 0xa4, 0xcf, 0x19, 0x32, 0x64,
+    0xc8, 0x17, 0x2e, 0x5c, 0xb8, 0xf7, 0x69, 0xd2, 0x23, 0x46, 0x8c, 0x9f, 0xb9, 0xf5, 0x6d, 0xda,
+    0x33, 0x66, 0xcc, 0x1f, 0x3e, 0x7c, 0xf8, 0x77, 0xee, 0x5b, 0xb6, 0xeb, 0x51, 0xa2, 0xc3, 0x00,
+};
+
+const unsigned char INDEX_OF[]=
+{
+    0xff, 0x00, 0x01, 0x63, 0x02, 0xc6, 0x64, 0x6a, 0x03, 0xcd, 0xc7, 0xbc, 0x65, 0x7e, 0x6b, 0x2a,
+    0x04, 0x8d, 0xce, 0x4e, 0xc8, 0xd4, 0xbd, 0xe1, 0x66, 0xdd, 0x7f, 0x31, 0x6c, 0x20, 0x2b, 0xf3,
+    0x05, 0x57, 0x8e, 0xe8, 0xcf, 0xac, 0x4f, 0x83, 0xc9, 0xd9, 0xd5, 0x41, 0xbe, 0x94, 0xe2, 0xb4,
+    0x67, 0x27, 0xde, 0xf0, 0x80, 0xb1, 0x32, 0x35, 0x6d, 0x45, 0x21, 0x12, 0x2c, 0x0d, 0xf4, 0x38,
+    0x06, 0x9b, 0x58, 0x1a, 0x8f, 0x79, 0xe9, 0x70, 0xd0, 0xc2, 0xad, 0xa8, 0x50, 0x75, 0x84, 0x48,
+    0xca, 0xfc, 0xda, 0x8a, 0xd6, 0x54, 0x42, 0x24, 0xbf, 0x98, 0x95, 0xf9, 0xe3, 0x5e, 0xb5, 0x15,
+    0x68, 0x61, 0x28, 0xba, 0xdf, 0x4c, 0xf1, 0x2f, 0x81, 0xe6, 0xb2, 0x3f, 0x33, 0xee, 0x36, 0x10,
+    0x6e, 0x18, 0x46, 0xa6, 0x22, 0x88, 0x13, 0xf7, 0x2d, 0xb8, 0x0e, 0x3d, 0xf5, 0xa4, 0x39, 0x3b,
+    0x07, 0x9e, 0x9c, 0x9d, 0x59, 0x9f, 0x1b, 0x08, 0x90, 0x09, 0x7a, 0x1c, 0xea, 0xa0, 0x71, 0x5a,
+    0xd1, 0x1d, 0xc3, 0x7b, 0xae, 0x0a, 0xa9, 0x91, 0x51, 0x5b, 0x76, 0x72, 0x85, 0xa1, 0x49, 0xeb,
+    0xcb, 0x7c, 0xfd, 0xc4, 0xdb, 0x1e, 0x8b, 0xd2, 0xd7, 0x92, 0x55, 0xaa, 0x43, 0x0b, 0x25, 0xaf,
+    0xc0, 0x73, 0x99, 0x77, 0x96, 0x5c, 0xfa, 0x52, 0xe4, 0xec, 0x5f, 0x4a, 0xb6, 0xa2, 0x16, 0x86,
+    0x69, 0xc5, 0x62, 0xfe, 0x29, 0x7d, 0xbb, 0xcc, 0xe0, 0xd3, 0x4d, 0x8c, 0xf2, 0x1f, 0x30, 0xdc,
+    0x82, 0xab, 0xe7, 0x56, 0xb3, 0x93, 0x40, 0xd8, 0x34, 0xb0, 0xef, 0x26, 0x37, 0x0c, 0x11, 0x44,
+    0x6f, 0x78, 0x19, 0x9a, 0x47, 0x74, 0xa7, 0xc1, 0x23, 0x53, 0x89, 0xfb, 0x14, 0x5d, 0xf8, 0x97,
+    0x2e, 0x4b, 0xb9, 0x60, 0x0f, 0xed, 0x3e, 0xe5, 0xf6, 0x87, 0xa5, 0x17, 0x3a, 0xa3, 0x3c, 0xb7,
+};
+
+/* 8-bit parity table */
+const unsigned char Partab[] = {
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
+    0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
+};
+
+/* Scramble byte table */
+const unsigned char Scrambler[]=
+{
+    0xff, 0x48, 0x0e, 0xc0, 0x9a, 0x0d, 0x70, 0xbc, 0x8e, 0x2c, 0x93, 0xad, 0xa7, 0xb7, 0x46, 0xce,
+    0x5a, 0x97, 0x7d, 0xcc, 0x32, 0xa2, 0xbf, 0x3e, 0x0a, 0x10, 0xf1, 0x88, 0x94, 0xcd, 0xea, 0xb1,
+    0xfe, 0x90, 0x1d, 0x81, 0x34, 0x1a, 0xe1, 0x79, 0x1c, 0x59, 0x27, 0x5b, 0x4f, 0x6e, 0x8d, 0x9c,
+    0xb5, 0x2e, 0xfb, 0x98, 0x65, 0x45, 0x7e, 0x7c, 0x14, 0x21, 0xe3, 0x11, 0x29, 0x9b, 0xd5, 0x63,
+    0xfd, 0x20, 0x3b, 0x02, 0x68, 0x35, 0xc2, 0xf2, 0x38, 0xb2, 0x4e, 0xb6, 0x9e, 0xdd, 0x1b, 0x39,
+    0x6a, 0x5d, 0xf7, 0x30, 0xca, 0x8a, 0xfc, 0xf8, 0x28, 0x43, 0xc6, 0x22, 0x53, 0x37, 0xaa, 0xc7,
+    0xfa, 0x40, 0x76, 0x04, 0xd0, 0x6b, 0x85, 0xe4, 0x71, 0x64, 0x9d, 0x6d, 0x3d, 0xba, 0x36, 0x72,
+    0xd4, 0xbb, 0xee, 0x61, 0x95, 0x15, 0xf9, 0xf0, 0x50, 0x87, 0x8c, 0x44, 0xa6, 0x6f, 0x55, 0x8f,
+    0xf4, 0x80, 0xec, 0x09, 0xa0, 0xd7, 0x0b, 0xc8, 0xe2, 0xc9, 0x3a, 0xda, 0x7b, 0x74, 0x6c, 0xe5,
+    0xa9, 0x77, 0xdc, 0xc3, 0x2a, 0x2b, 0xf3, 0xe0, 0xa1, 0x0f, 0x18, 0x89, 0x4c, 0xde, 0xab, 0x1f,
+    0xe9, 0x01, 0xd8, 0x13, 0x41, 0xae, 0x17, 0x91, 0xc5, 0x92, 0x75, 0xb4, 0xf6, 0xe8, 0xd9, 0xcb,
+    0x52, 0xef, 0xb9, 0x86, 0x54, 0x57, 0xe7, 0xc1, 0x42, 0x1e, 0x31, 0x12, 0x99, 0xbd, 0x56, 0x3f,
+    0xd2, 0x03, 0xb0, 0x26, 0x83, 0x5c, 0x2f, 0x23, 0x8b, 0x24, 0xeb, 0x69, 0xed, 0xd1, 0xb3, 0x96,
+    0xa5, 0xdf, 0x73, 0x0c, 0xa8, 0xaf, 0xcf, 0x82, 0x84, 0x3c, 0x62, 0x25, 0x33, 0x7a, 0xac, 0x7f,
+    0xa4, 0x07, 0x60, 0x4d, 0x06, 0xb8, 0x5e, 0x47, 0x16, 0x49, 0xd6, 0xd3, 0xdb, 0xa3, 0x67, 0x2d,
+    0x4b, 0xbe, 0xe6, 0x19, 0x51, 0x5f, 0x9f, 0x05, 0x08, 0x78, 0xc4, 0x4a, 0x66, 0xf5, 0x58, 0xff,
+    0x48, 0x0e, 0xc0, 0x9a, 0x0d, 0x70, 0xbc, 0x8e, 0x2c, 0x93, 0xad, 0xa7, 0xb7, 0x46, 0xce, 0x5a,
+    0x97, 0x7d, 0xcc, 0x32, 0xa2, 0xbf, 0x3e, 0x0a, 0x10, 0xf1, 0x88, 0x94, 0xcd, 0xea, 0xb1, 0xfe,
+    0x90, 0x1d, 0x81, 0x34, 0x1a, 0xe1, 0x79, 0x1c, 0x59, 0x27, 0x5b, 0x4f, 0x6e, 0x8d, 0x9c, 0xb5,
+    0x2e, 0xfb, 0x98, 0x65, 0x45, 0x7e, 0x7c, 0x14, 0x21, 0xe3, 0x11, 0x29, 0x9b, 0xd5, 0x63, 0xfd,
+};
diff --git a/sstv/sstv_image_2_320_x_256.jpeg b/sstv/sstv_image_2_320_x_256.jpeg
new file mode 100644
index 00000000..588a1aa7
Binary files /dev/null and b/sstv/sstv_image_2_320_x_256.jpeg differ
diff --git a/telem.c b/telem.c
index 7afceef2..1fc534a5 100644
--- a/telem.c
+++ b/telem.c
@@ -15,7 +15,7 @@ int main(int argc, char *argv[]) {
     }
   }
 
-  printf("CubeSatSim v2.0 INA219 Voltage and Current Telemetry\n");  
+  printf("CubeSatSim v2.1 INA219 Voltage and Current Telemetry\n");  
   map[MINUS_X] = MINUS_Y;
   map[PLUS_Z] = MINUS_X;	
   map[MINUS_Y] = PLUS_Z;			
diff --git a/transmit.py b/transmit.py
index 3c6404cd..e1e52c62 100644
--- a/transmit.py
+++ b/transmit.py
@@ -3,13 +3,14 @@
 import RPi.GPIO as GPIO
 from RPi.GPIO import output
 #import subprocess
-#import time
+import time
 from time import sleep
 #import os
 import sys
 from os import system
 from PIL import Image, ImageDraw, ImageFont, ImageColor
 import serial	
+import random
 
 def battery_saver_check():
 	try:
@@ -22,6 +23,14 @@ def battery_saver_check():
 	except:
 		print("battery saver not activated")
 #		txc = True
+
+def blink(times):
+	powerPin = 16
+	for i in range(times):
+		GPIO.output(powerPin, 0) # blink two times
+		sleep(0.1)
+		GPIO.output(powerPin, 1)
+		sleep(0.1)
 		
 def increment_mode():
 	print("increment mode")
@@ -38,76 +47,26 @@ def increment_mode():
 	print(mode)
 	if (mode == 'a'):
 		mode = 'f'
-		GPIO.output(powerPin, 0) # blink two times
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
+		blink(2)
 		sleep(2.5)
 
 	elif (mode == 'f'):
 		mode = 'b'
-		GPIO.output(powerPin, 0) # blink three times
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)	
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
+		blink(3)
 		sleep(2.5)
 	
 	elif (mode == 'b'):
 		mode = 's'
-		GPIO.output(powerPin, 0) # blink four times
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)	
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)	
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
+		blink(4)
 		sleep(2.5)
 
 	elif (mode == 's'):
 		mode = 'm'
-		GPIO.output(powerPin, 0) # blink five times
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)	
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1);
-		GPIO.output(powerPin, 1)	
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
-		sleep(0.1)
-		GPIO.output(powerPin, 0)
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
+		blink(5)
 		sleep(2.5)
 	else:
 		mode = 'a'
-		GPIO.output(powerPin, 0) # blink one time
-		sleep(0.1)
-		GPIO.output(powerPin, 1)
+		blink(1)
 		sleep(2.5)
 
 	try:	
@@ -135,8 +94,43 @@ def increment_mode():
 	except:
 		print("can't write to .mode file")
 		
-
-print("CubeSatSim v2.0 transmit.py starting...")
+def camera_photo():
+	system("sudo rm /home/pi/CubeSatSim/camera_out.jpg")
+	stored_image = False
+	try:
+		system("raspistill -o /home/pi/CubeSatSim/camera_out.jpg -w 320 -h 256") #  > /dev/null 2>&1")
+		f = open("/home/pi/CubeSatSim/camera_out.jpg")
+		f.close()
+		print("Photo taken")
+	except:
+		system("cp /home/pi/CubeSatSim/sstv//sstv_image_2_320_x_256.jpeg /home/pi/CubeSatSim/camera_out.jpg")
+		print("Using stored image")
+		stored_image = True
+	if (stored_image == False):	
+		file='/home/pi/CubeSatSim/camera_out.jpg'
+		font1 = ImageFont.truetype('DejaVuSerif.ttf', 20)
+		font2 = ImageFont.truetype('DejaVuSerif-Bold.ttf', 16)
+	
+		try:
+			filep = open("/home/pi/CubeSatSim/telem_string.txt")
+			telem_string = filep.readline()
+		except:
+			telem_string = ""
+			if (debug_mode == 1):
+				print("Can't read telem_string.txt")		
+		print(telem_string)
+		
+		img = Image.open(file)
+		draw = ImageDraw.Draw(img) 
+	#					draw.text((10, 10), callsign, font=font2, fill='white')
+	#					draw.text((120, 10), telem_string, font=font2, fill='white')					
+		draw.text((12, 12), callsign, font=font1, fill='black')
+		draw.text((10, 10), callsign, font=font1, fill='white')
+		draw.text((122, 12), telem_string, font=font2, fill='black')
+		draw.text((120, 10), telem_string, font=font2, fill='white')
+		img.save(file)
+
+print("CubeSatSim v2.1 transmit.py starting...")
 
 pd = 21
 ptt = 20
@@ -276,7 +270,8 @@ if __name__ == "__main__":
 	rx_value = '0'
 	sq = '0'
 	tx = '434.9000'	
-	rx = '435.0000'	
+	rx = '435.0000'
+	txr = '144.9000'
 	
 	try:
 		file = open("/home/pi/CubeSatSim/sim.cfg")
@@ -289,11 +284,16 @@ if __name__ == "__main__":
 				sq = 0 # turn off squelch for Pacsat
 			print(sq)
 		if len(config) > 6:
-                        txf = float(config[6])
+			txf = float(config[6])
 #                        print(txf)
 #                        print( "{:.4f}".format(txf))
-                        tx = "{:.4f}".format(txf)
-                        print(tx)
+                        
+			if (mode == 'e'):
+				txr = txf - 290.0 # Cross Band Repeater mode transmit frequency in 2m band
+				tx = "{:.4f}".format(txr)
+			else:
+				tx = "{:.4f}".format(txf)
+			print("Transmit frequency: ",tx)
 		if len(config) > 7:
                         rxf = float(config[7])
 #                        print(rxf)
@@ -316,11 +316,11 @@ if __name__ == "__main__":
 		print
 #		print(callsign)
 		print(sq)
-		if sq == '8':
-			print("squelch set to 8, no command input!")
-			no_command = True
-		else:
-			no_command = False		
+#		if sq == '8':
+#			print("squelch set to 8, no command input!")
+#			no_command = True
+#		else:
+		no_command = False		
 		print(no_command)
 	except:
 		callsign = "AMSAT"
@@ -355,24 +355,25 @@ if __name__ == "__main__":
 	card = "Headphones"  # default using pcm audio output of Pi Zero
 #	card = "Device" # using USB sound card for audio output	
 
-	print("Programming FM module!\n");	
-	output(pd, 1)
-	output (ptt, 1)
-	try:
-		ser = serial.Serial("/dev/ttyAMA0", 9600)
-		print(ser.portstr)
-#		uhf_string = "AT+DMOSETGROUP=0," + rx +"," + tx + ",0,3,0,0\r\n"
-		uhf_string = "AT+DMOSETGROUP=0," + rx + "," + tx + "," + rxpl_value + "," + sq + "," + txpl_value + ",0\r\n"
-		print(uhf_string)
-		for i in range(6):
-#			ser.write(b"AT+DMOSETGROUP=0,435.0000,434.9000,0,3,0,0\r\n")
-			ser.write(uhf_string.encode())
-			sleep(0.1)
-		ser.close()
-		ser = serial.Serial("/dev/ttyAMA0", 115200) # reset back to 115200 for cubesatsim code for payload sensor data
-	except:
-		print("Error in serial write")
-	output(pd, 0)
+	if (mode != 'e'):
+		print("Programming FM module!\n");	
+		output(pd, 1)
+		output (ptt, 1)
+		try:
+			ser = serial.Serial("/dev/ttyAMA0", 9600)
+			print(ser.portstr)
+	#		uhf_string = "AT+DMOSETGROUP=0," + rx +"," + tx + ",0,3,0,0\r\n"
+			uhf_string = "AT+DMOSETGROUP=0," + rx + "," + tx + "," + rxpl_value + "," + sq + "," + txpl_value + ",0\r\n"
+			print(uhf_string)
+			for i in range(6):
+	#			ser.write(b"AT+DMOSETGROUP=0,435.0000,434.9000,0,3,0,0\r\n")
+				ser.write(uhf_string.encode())
+				sleep(0.1)
+			ser.close()
+			ser = serial.Serial("/dev/ttyAMA0", 115200) # reset back to 115200 for cubesatsim code for payload sensor data
+		except:
+			print("Error in serial write")
+		output(pd, 0)
 
 #	if (mode != 'x') and (skip == False):
 #		sleep(10)  # delay so cubesatsim code catches up
@@ -383,7 +384,8 @@ if __name__ == "__main__":
 	
 #	if (mode != ) and (command_tx == True):	
 #	if (command_tx == True):	
-	if ((mode == 'a') or (mode == 'b') or (mode == 'f') or (mode == 's')) and (command_tx == True) and (skip == False):	
+##	if ((mode == 'a') or (mode == 'b') or (mode == 'f') or (mode == 's') or (mode == 'j')) and (command_tx == True) and (skip == False):	
+	if (((mode == 'a') or (mode == 'b') or (mode == 'f') or (mode == 's') or (mode == 'j')) and (command_tx == True) and (skip == False)) or ((mode == 'e') and (command_tx == True)):	
 #		battery_saver_mode
 		GPIO.setmode(GPIO.BCM)  # added to make Tx LED work on Pi Zero 2 and Pi 4		
 		GPIO.setup(txLed, GPIO.OUT)	
@@ -397,14 +399,18 @@ if __name__ == "__main__":
 #			output (ptt, 1)
 #			output(pd, 0)
 #		else:	
-		if (True):
+		if (no_command):
 			if (debug_mode == 1):
-#				system("echo 'hi hi de " + callsign + "' > id.txt && gen_packets -M 20 /home/pi/CubeSatSim/id.txt -o /home/pi/CubeSatSim/morse.wav -r 48000 > /dev/null 2>&1 && cat /home/pi/CubeSatSim/morse.wav | csdr convert_i16_f | csdr gain_ff 7000 | csdr convert_f_samplerf 20833 | sudo /home/pi/rpitx/rpitx -i- -m RF -f 434.9e3")
 				system("echo 'hi hi de " + callsign + "' > id.txt && gen_packets -M 20 /home/pi/CubeSatSim/id.txt -o /home/pi/CubeSatSim/morse.wav -r 48000 > /dev/null 2>&1 && cat /home/pi/CubeSatSim/morse.wav | csdr convert_i16_f | csdr gain_ff 7000 | csdr convert_f_samplerf 20833 | sudo /home/pi/rpitx/rpitx -i- -m RF -f " + tx + "e3")
 			else:
-#				system("echo 'hi hi de " + callsign + "' > id.txt && gen_packets -M 20 /home/pi/CubeSatSim/id.txt -o /home/pi/CubeSatSim/morse.wav -r 48000 > /dev/null 2>&1 && cat /home/pi/CubeSatSim/morse.wav | csdr convert_i16_f | csdr gain_ff 7000 | csdr convert_f_samplerf 20833 | sudo /home/pi/rpitx/rpitx -i- -m RF -f 434.9e3 > /dev/null 2>&1")
 				system("echo 'hi hi de " + callsign + "' > id.txt && gen_packets -M 20 /home/pi/CubeSatSim/id.txt -o /home/pi/CubeSatSim/morse.wav -r 48000 > /dev/null 2>&1 && cat /home/pi/CubeSatSim/morse.wav | csdr convert_i16_f | csdr gain_ff 7000 | csdr convert_f_samplerf 20833 | sudo /home/pi/rpitx/rpitx -i- -m RF -f " + tx + "e3 > /dev/null 2>&1")
-		
+		else:
+			if (debug_mode == 1):
+				system("echo 'hi hi de " + callsign + "  C" + "' > id.txt && gen_packets -M 20 /home/pi/CubeSatSim/id.txt -o /home/pi/CubeSatSim/morse.wav -r 48000 > /dev/null 2>&1 && cat /home/pi/CubeSatSim/morse.wav | csdr convert_i16_f | csdr gain_ff 7000 | csdr convert_f_samplerf 20833 | sudo /home/pi/rpitx/rpitx -i- -m RF -f " + tx + "e3")
+			else:
+				system("echo 'hi hi de " + callsign + "  C" + "' > id.txt && gen_packets -M 20 /home/pi/CubeSatSim/id.txt -o /home/pi/CubeSatSim/morse.wav -r 48000 > /dev/null 2>&1 && cat /home/pi/CubeSatSim/morse.wav | csdr convert_i16_f | csdr gain_ff 7000 | csdr convert_f_samplerf 20833 | sudo /home/pi/rpitx/rpitx -i- -m RF -f " + tx + "e3 > /dev/null 2>&1")
+
+			
 		output(txLed, txLedOff)
 
 		sleep(1)
@@ -580,32 +586,32 @@ if __name__ == "__main__":
 					print("image 2 did not load - copy from CubeSatSim/sstv directory")
 				while 1:
 #					command_control_check()			
-					
-					system("raspistill -o /home/pi/CubeSatSim/camera_out.jpg -w 320 -h 256") #  > /dev/null 2>&1")
-					print("Photo taken")
-
-					file='/home/pi/CubeSatSim/camera_out.jpg'
-					font1 = ImageFont.truetype('DejaVuSerif.ttf', 20)
-					font2 = ImageFont.truetype('DejaVuSerif-Bold.ttf', 16)
-
-					try:
-						filep = open("/home/pi/CubeSatSim/telem_string.txt")
-						telem_string = filep.readline()
-					except:
-						telem_string = ""
-						if (debug_mode == 1):
-							print("Can't read telem_string.txt")		
-					print(telem_string)
-					
-					img = Image.open(file)
-					draw = ImageDraw.Draw(img) 
+					camera_photo()
+##					system("raspistill -o /home/pi/CubeSatSim/camera_out.jpg -w 320 -h 256") #  > /dev/null 2>&1")
+##					print("Photo taken")
+##
+##					file='/home/pi/CubeSatSim/camera_out.jpg'
+##					font1 = ImageFont.truetype('DejaVuSerif.ttf', 20)
+##					font2 = ImageFont.truetype('DejaVuSerif-Bold.ttf', 16)
+##
+##					try:
+##						filep = open("/home/pi/CubeSatSim/telem_string.txt")
+##						telem_string = filep.readline()
+##					except:
+##						telem_string = ""
+##						if (debug_mode == 1):
+##							print("Can't read telem_string.txt")		
+##					print(telem_string)
+##					
+##					img = Image.open(file)
+##					draw = ImageDraw.Draw(img) 
 #					draw.text((10, 10), callsign, font=font2, fill='white')
 #					draw.text((120, 10), telem_string, font=font2, fill='white')					
-					draw.text((12, 12), callsign, font=font1, fill='black')
-					draw.text((10, 10), callsign, font=font1, fill='white')
-					draw.text((122, 12), telem_string, font=font2, fill='black')
-					draw.text((120, 10), telem_string, font=font2, fill='white')
-					img.save(file)
+##					draw.text((12, 12), callsign, font=font1, fill='black')
+##					draw.text((10, 10), callsign, font=font1, fill='white')
+##					draw.text((122, 12), telem_string, font=font2, fill='black')
+##					draw.text((120, 10), telem_string, font=font2, fill='white')
+##					img.save(file)
 					
 #					command_control_check()			
 					
@@ -739,9 +745,12 @@ if __name__ == "__main__":
 #							output(pd, 0)
 						sleep(10)
 						
-		elif (mode == 'b'):
+		elif (mode == 'b') or (mode == 'j'):
 #			command_control_check()	
-			print("BPSK")
+			if (mode == 'b'):
+				print("BPSK")
+			else:
+				print("FunCube")
 			print("turn on FM rx")
 			output(pd, 1)
 			output(ptt, 1)
@@ -753,7 +762,10 @@ if __name__ == "__main__":
 #				system("sudo nc -l 8080 | csdr convert_i16_f | csdr fir_interpolate_cc 2 | csdr dsb_fc | csdr bandpass_fir_fft_cc 0.002 0.06 0.01 | csdr fastagc_ff | sudo /home/pi/rpitx/sendiq -i /dev/stdin -s 96000 -f 434.9e6 -t float &")
 				system("sudo nc -l 8080 | csdr convert_i16_f | csdr fir_interpolate_cc 2 | csdr dsb_fc | csdr bandpass_fir_fft_cc 0.002 0.06 0.01 | csdr fastagc_ff | sudo /home/pi/rpitx/sendiq -i /dev/stdin -s 96000 -f " + tx + "e6 -t float &")
 			print("Turning LED on/off and listening for carrier")
+			image_id = random.randint(0, 255)
+			print("Initial image_id: " + str(image_id) + "\n")
 			while 1:
+#				print ("LED on")
 				output(txLed, txLedOff)
 				sleep(0.4)
 #				if (command_tx == False):
@@ -768,45 +780,83 @@ if __name__ == "__main__":
 					output(txLed, txLedOn)
 #					print(txLed)
 #					print(txLedOn)
-				sleep(4.2)
+
+				if (mode == 'b'):
+					sleep(4.2)	
+				else:  # FunCube mode image
+					for i in range(4):
+#						print("Checking image_file.bin")
+						try:
+							file = open("/home/pi/CubeSatSim/image_file.bin")
+							file.close()
+	#						image_present = True
+							sleep(1.0)
+						except:
+	#						image_present = False
+						
+	#					if (image_present == False):
+							start = time.perf_counter()
+							camera_photo()
+							system("/home/pi/ssdv/ssdv -e -n -i " + str(image_id) + " -q 3 -J /home/pi/CubeSatSim/camera_out.jpg /home/pi/CubeSatSim/image_file.bin")
+							print("image_id: " + str(image_id) + "\n")
+							image_id = ( image_id + 1 ) % 256
+							print("new image_id: " + str(image_id) + "\n")
+							elapsed_time = time.perf_counter() - start
+							print("Elapsed time: ")
+							print(elapsed_time)
+							if (elapsed_time < 9):
+								sleep(9 - time.perf_counter() + start)
+	#					else:	
+					sleep(0.6)
 		elif (mode == 'e'):  # code based on https://zr6aic.blogspot.com/2016/11/creating-2m-fm-repeater-with-raspberry.html
-			print("Repeater")
-			print("Stopping command and control")
-			system("sudo systemctl stop command")
+			print("Cross Band Repeater Mode")
+#			print("Stopping command and control")
+#			system("sudo systemctl stop command")
 			print("turn on FM rx")
 			output(pd, 1)
 			output(ptt, 1)
 			GPIO.setmode(GPIO.BCM)  # added to make Tx LED work on Pi 4
 			GPIO.setup(txLed, GPIO.OUT)
-			GPIO.setup(powerPin, GPIO.OUT)
+#			GPIO.setup(powerPin, GPIO.OUT)
 			GPIO.setup(squelch, GPIO.IN, pull_up_down=GPIO.PUD_UP)  ## pull up in case pin is not connected	
-			GPIO.output(powerPin, 0)
+#			GPIO.output(powerPin, 1)  # was 0
+#			txf = float(tx) - 288.9
+#			print("Transmit frequency: ",txf)
+			if (command_tx != True):
+				print("Beacon mode off so no repeater transmission")
+
+			print("Ready to detect carrier")
 			while True:
-				sleep(0.5)
-				if (GPIO.input(squelch) == False):
+				if (GPIO.input(squelch) == False) and (command_tx == True):
 					print("Carrier detected, starting repeater")
 					GPIO.setmode(GPIO.BCM)  # added to make Tx LED work on Pi Zero 2 and Pi 4		
 					GPIO.setup(txLed, GPIO.OUT)						
 					output(txLed, txLedOn)
 #					system("arecord -D plughw:CARD=Device,DEV=0  | csdr convert_i16_f | csdr gain_ff 14000 | csdr convert_f_samplerf 20833 | sudo rpitx -i- -m RF -f " + tx + "e3 &")
 ##					system("arecord -D plughw:CARD=Device,DEV=0 -f S16_LE -r 48000 -c 1 | csdr convert_s16_f | csdr gain_ff 14000 | csdr convert_f_samplerf 20833 | sudo rpitx -i- -m RF -f " + tx + "e3 &")
-					system("sudo nc -l 8011 | csdr convert_i16_f | csdr gain_ff 16000 | csdr convert_f_samplerf 20833 | sudo rpitx -i- -m RF -f " + tx + "e3 &")
+					system("sudo nc -l 8011 | csdr convert_i16_f | csdr gain_ff 16000 | csdr convert_f_samplerf 20833 | sudo rpitx -i- -m RF -f " + tx + "e3 > /dev/null 2>&1 &")
 					sleep(1)
 					system("sudo arecord -D plughw:CARD=Device,DEV=0 -r48000 -fS16_LE -c1 | nc localhost 8011 &")
 					GPIO.output(powerPin, 1)
 					sleep(0.5)
-					GPIO.output(powerPin, 0)
+#					system("sudo arecord -D plughw:1 -r48000 -fS16_LE -c1 | nc localhost 8011 &")
+					system("sudo arecord -D shared_mic -r48000 -fS16_LE -c1 | nc localhost 8011 &")
+#					GPIO.output(powerPin, 1)
+#					sleep(0.5)
+#					GPIO.output(powerPin, 0)
 					while (GPIO.input(squelch) == False):
 						sleep(1)
 					print("No carrier detected, stopping repeater")
 					output(txLed, txLedOff)
-					system("sudo killall -9 arecord")
-					system("sudo killall -9 nc")
-					system("sudo killall -9 rpitx")
+					system("sudo killall -9 arecord > /dev/null 2>&1")
+					system("sudo killall -9 nc > /dev/null 2>&1")
+					system("sudo killall -9 rpitx > /dev/null 2>&1")
 					print("Resetting audio")
 					system("sudo /etc/init.d/alsa-utils stop")
 					system("sudo /etc/init.d/alsa-utils start")
 					print("Finished resetting audio")
+					print("Ready to detect carrier")
+	
 		else:
 			print("FSK") 
 			print("turn on FM rx")
diff --git a/update b/update
index fcef1749..e4d95acb 100755
--- a/update
+++ b/update
@@ -1,9 +1,21 @@
 #!/bin/bash
 
-echo -e "\nupdate script for CubeSatSim v2.0\n"
+echo -e "\nupdate script for CubeSatSim v2.1\n"
+
+FLAG=0
+checkout=0
+
+if [ -z "$1" ] ; then
+	checkout=0
+else
+	checkout=1
+	branch="$1"
+	echo -n "changing to branch "
+	echo $branch
+	FLAG=1
+fi
 
 if [ "$1" = "n" ] ; then
-# if [ -z "$2" ] ; then
 	noreboot=1
 else
 	noreboot=0
@@ -23,7 +35,7 @@ if [ "$1" = "u" ]; then
 
 fi
 
-sudo apt-get install -y python3-smbus
+# sudo apt-get install -y python3-smbus libusb-1.0
 
 sudo sed -i 's/update.sh/update /g' /etc/motd
 
@@ -39,6 +51,11 @@ cd /home/pi/CubeSatSim
 
 git pull --no-rebase > .updated
 
+if [ $checkout -eq 1 ]; then
+	git checkout $branch	
+	git pull --no-rebase
+fi
+
 make debug
 
 FILE=/home/pi/CubeSatSim/command_tx
@@ -57,8 +74,6 @@ else
     echo "0\n" > /home/pi/CubeSatSim/command_count.txt
 fi
 
-FLAG=0
-
 if [[ $(diff systemd/cubesatsim.service /etc/systemd/system/cubesatsim.service) ]]; then
   echo "changed cubesatsim.service."
   sudo cp /home/pi/CubeSatSim/systemd/cubesatsim.service /etc/systemd/system/cubesatsim.service
@@ -104,6 +119,21 @@ else
   FLAG=1
 fi
 
+FILE=/etc/asound.conf
+if [ -f "$FILE" ]; then
+  if [[ $(diff /home/pi/CubeSatSim/asound.conf /etc/asound.conf) ]]; then
+    echo "changed /etc/asound.conf."
+    sudo cp /home/pi/CubeSatSim/asound.conf /etc/asound.conf
+    FLAG=1
+  else
+    echo "no change to /etc/asound.conf."
+  fi
+else
+  echo "creating /etc/asound.conf."
+  sudo cp /home/pi/CubeSatSim/asound.conf /etc/asound.conf
+  FLAG=1
+fi
+
 FILE=/home/pi/CubeSatSim/sstv_image_1_320_x_256.jpg
 if [ ! -f "$FILE" ]; then
     echo "Copying SSTV image 1."
@@ -115,7 +145,7 @@ if [ ! -f "$FILE" ]; then
     echo "Copying SSTV image 2."
     cp /home/pi/CubeSatSim/sstv/sstv_image_2_320_x_256.jpg /home/pi/CubeSatSim/sstv_image_2_320_x_256.jpg
 fi
-      
+
 grep 'update' /home/pi/CubeSatSim/.updated
 if [[ $(grep 'update' /home/pi/CubeSatSim/.updated) ]]; then
   echo "update script updated, running again"
@@ -213,10 +243,52 @@ if [ ! -d "/home/pi/WiringPi" ]; then
 
 fi
 
-cd /home/pi/pi-power-button
+if [ ! -d "/home/pi/ssdv" ]; then
+
+	echo "Installing SSDV for FunCube mode"
+	cd
+	git clone https://github.com/alanbjohnston/ssdv.git  # install ssdv for FunCube images
+	cd ssdv
+	make
+	cd
+	FLAG=1
+fi
+
+if [ ! -d "/home/pi/fctelem" ]; then
+	echo "Installing fctelem binary v0.2 for FunCube mode"
+	cd
+	mkdir /home/pi/fctelem
+	mkdir /home/pi/fctelem/public_html
+	cd fctelem
+	wget https://github.com/alanbjohnston/go/releases/download/v0.2/fctelem.zip
+	unzip fctelem.zip
+	FLAG=1
+elif [ ! -f "/home/pi/fctelem/v0.2" ]; then
+	echo "Updating fctelem binary to version v0.2 for FunCube mode"
+	cd
+	cd /home/pi/fctelem
+	sudo mv fctelem fctelem.bk
+	sudo mv fcdecode.conf fcdecode.conf.bk
+	sudo mv fctelem.zip fctelem.zip.1
+	wget https://github.com/alanbjohnston/go/releases/download/v0.2/fctelem.zip
+	unzip fctelem.zip
+	FLAG=1
+fi
+
+if [ ! -f "/home/pi/fcdctl/fcdctl" ]; then
+	echo "Installing fcdctl to set FunCubeDongle Pro gain"
+	sudo rm /var/lib/dpkg/info/python3-pip.list
+	sudo apt install python3-pip --reinstall
+	sudo apt-get install -y python3-smbus libusb-1.0
+	cd
+	git clone https://github.com/csete/fcdctl.git
+	cd fcdctl
+	make fcdpp
+fi
 
-git checkout master
+cd /home/pi/pi-power-button
 
+# git checkout master
 
   git pull --no-rebase > .updated_p
 
@@ -224,12 +296,13 @@ git checkout master
   if [[ $(grep 'changed' /home/pi/pi-power-button/.updated_p) ]]; then
   
     echo "updating pi-power-button."
+
+    git checkout 7-modes
     
     script/install
 
     FLAG=1
 
-
   else
     echo "nothing to do for pi-power-button."
   fi 
@@ -348,7 +421,7 @@ if [ "$noreboot" = "0" ] ; then
 	fi
 else
 	if [ $FLAG -eq 1 ]; then
-		echo "reboot needed for changes to take effect"
+		echo "reboot needed for changes to take effect" | wall
 	fi
 fi