diff --git a/cubesatsim/cubesatsim.h b/cubesatsim/cubesatsim.h
index dd7eb8eb..8801e904 100644
--- a/cubesatsim/cubesatsim.h
+++ b/cubesatsim/cubesatsim.h
@@ -33,7 +33,7 @@
 #include "hardware/pwm.h"  // pwm 
 #include "hardware/sync.h" // wait for interrupt 
 #include "RPi_Pico_TimerInterrupt.h"
-#include <WiFi.h>
+//#include <WiFi.h>
 #include "hardware/gpio.h"
 #include "hardware/adc.h"
 #include <ctype.h>
@@ -137,7 +137,7 @@ char serial_string[128];
 //#define WAV_DATA_LENGTH (50000 * 8)
 
 uint32_t tx_freq_hz = 434900000 + FREQUENCY_OFFSET;
-uint8_t data[1024];
+//uint8_t data[1024];
 uint32_t tx_channel = 0;
 
 //ax5043_conf_t hax5043;
@@ -208,6 +208,8 @@ void read_config_file();
 void write_config_file(); 
 void read_mode(); 
 void write_mode(); 
+void start_clockgen();
+
 
 #ifndef STASSID
 #define STASSID "Pico"
@@ -231,7 +233,8 @@ long time_start;
 //char cmdbuffer[1000];
 //FILE * file1;//
 #define BUFFER_SIZE  5841 //970 // (970 * 2) //  * 2)
-short int buffer[BUFFER_SIZE]; // 50000]; //BUFFER_SIZE]; // ctr is an int // 100000]; // 50000]; // 25000]; // 10240]; // was 2336400]; // max size for 10 frames count of BPSK
+//short int buffer[BUFFER_SIZE]; // 50000]; //BUFFER_SIZE]; // ctr is an int // 100000]; // 50000]; // 25000]; // 10240]; // was 2336400]; // max size for 10 frames count of BPSK
+byte buffer[BUFFER_SIZE]; // 50000]; //BUFFER_SIZE]; // ctr is an int // 100000]; // 50000]; // 25000]; // 10240]; // was 2336400]; // max size for 10 frames count of BPSK
 //short int buffer[(WAV_DATA_LENGTH/8)];
 //FILE *sopen(const char *program);
 char tlm_str[1000];	
@@ -260,7 +263,7 @@ int ctr = 0;
 int rd = 0;
 int nrd;
 void write_to_buffer(int i, int symbol, int val);
-void write_wave(int i, short int * buffer);
+void write_wave(int i, byte * buffer);
 void sleep(float time);
 int uart_fd;
 
@@ -336,7 +339,7 @@ int pwm_amplitude = 50; //50 //100;
 int pwm_value;
 int tx_bit = 1;
 */
-float delay_time;
+volatile unsigned long delay_time;
 bool  polarity = true;
 pwm_config config;
 int bpsk_pin_slice_A;
@@ -368,11 +371,13 @@ RPI_PICO_Timer ITimer0(0);
 RPI_PICO_Timer ITimer1(1);
 
 bool timer0_on = false;
+volatile int timer_counter;
 
 char callsign[20];
 int morse_timing = 60;  // ms for a dit
 int morse_freq = 1800; // Hz
-int morse_table[44][6] = {  // 0-9, A-Z only by (ASCII - 48)
+//int morse_table[44][6] = {  // 0-9, A-Z only by (ASCII - 48)
+byte morse_table[44][6] = {  // 0-9, A-Z only by (ASCII - 48)
   { 3, 3, 3, 3, 3, 0 },	// 0		
   { 1, 3, 3, 3, 3, 0 },	// 1		
   { 1, 1, 3, 3, 3, 0 },	// 2		
diff --git a/cubesatsim/cubesatsim.ino b/cubesatsim/cubesatsim.ino
index b3ad0d25..a482dfdd 100644
--- a/cubesatsim/cubesatsim.ino
+++ b/cubesatsim/cubesatsim.ino
@@ -36,15 +36,16 @@
 #include "hardware/sync.h" // wait for interrupt 
 #include "RPi_Pico_ISR_Timer.h"
 #include "RPi_Pico_TimerInterrupt.h"
-#include <WiFi.h>
+//#include <WiFi.h>
 #include "hardware/gpio.h"
 #include "hardware/adc.h"
 #include "SSTV-Arduino-Scottie1-Library.h"
 #include "LittleFS.h"
+#include <Adafruit_SI5351_Library.h>
 
 // jpg files to be stored in flash storage on Pico (FS 512kB setting)
 #include "sstv1.h"
-#include "sstv2.h"
+//#include "sstv2.h"
 
 Adafruit_INA219 ina219_1_0x40;
 Adafruit_INA219 ina219_1_0x41(0x41);
@@ -55,8 +56,12 @@ Adafruit_INA219 ina219_2_0x41(0x41);
 Adafruit_INA219 ina219_2_0x44(0x44);
 Adafruit_INA219 ina219_2_0x45(0x45);
 
-WiFiServer server(port);
-WiFiClient client;
+Adafruit_SI5351 clockgen = Adafruit_SI5351();
+
+unsigned long micros3;
+
+//WiFiServer server(port);
+//WiFiClient client;
 
 //#define PICO_W    // define if Pico W board.  Otherwise, compilation fail for Pico or runtime fail if compile as Pico W
 
@@ -83,10 +88,12 @@ void setup() {
 	
 // otherwise, run CubeSatSim Pico code
   
-  Serial.println("CubeSatSim Pico v0.22 starting...\n");
+  Serial.println("CubeSatSim Pico v0.25 starting...\n");
 	
   config_gpio();	
 	
+  start_clockgen();		
+	
   EEPROM.begin(512);	
 	
 #ifndef ARDUINO_ARCH_RP2040
@@ -139,15 +146,10 @@ void setup() {
   start_button_isr(); 
 	
   setup_sstv();
-  if (start_camera())  {
-    camera_detected = true;
-    Serial.println("Camera detected!");
-  } else {
-    camera_detected = false;
-    Serial.println("No camera detected!");
-  }	
-  start_isr();
-  start_pwm();
+
+  camera_detected = start_camera();	
+
+//  start_pwm();
 	
 /**/
   Serial.println("Transmitting callsign");	
@@ -159,6 +161,8 @@ void setup() {
 	
   config_telem();	
 	
+  start_isr();	
+	
 // setup radio depending on mode 
   config_radio();	
 		
@@ -198,25 +202,26 @@ void loop() {
     if (mode == AFSK) {  
       send_aprs_packet();
     } else if (mode == CW) {
+      Serial.printf("DE %s \n", callsign);	    
       send_cw();
     }
   }
   else if (mode == SSTV)
   {
+      first_time_sstv = false;	  
       char image_file[128];
       if (first_time_sstv) {  
 //      if (false) {    // turn this off for now
         strcpy(image_file, sstv1_filename);
 	first_time_sstv = false;
       } else {
-	if (camera_detected) {      
-          Serial.println("Getting image file");
- 	  get_camera_image();      
+	if (camera_detected = get_camera_image()) {      
+          Serial.println("Getting image file");   
 //          Serial.println("Got image file");	      
 	  char camera_file[] = "/cam.jpg";      
 	  strcpy(image_file, camera_file);      
 	} else	      
-	  strcpy(image_file, sstv2_filename);     // 2nd stored image
+	  strcpy(image_file, sstv1_filename);     // 2nd stored image
       }    
       if (debug_mode)  {	  
         Serial.print("\nSending SSTV image ");
@@ -225,14 +230,21 @@ void loop() {
 //      send_sstv("/cam.raw");
 	  
 //      send_sstv(image_file);
+//      LittleFS.remove("/cam.bin");	  
+      show_dir();	  
+      char output_file2[] = "/cam2.bin"; 	  
+      jpeg_decode(image_file, output_file2, true); // debug_mode);
+      show_dir();	  
+      char telem_display[] = " BATT:    STATUS:   TEMP:  ";	  
+      char output_file[] = "/cam.bin"; 
+      digitalWrite(PTT_PIN, HIGH);  // shouldn't need this but
+      rotate_image(output_file2, output_file, telem_display);	  
+      show_dir();
 	  
-      char output_file[] = "/cam.bin"; 	  
-      jpeg_decode(image_file, output_file, debug_mode);
-
       if (debug_mode)	  	  
         Serial.println("Start transmit!!!");
       digitalWrite(PTT_PIN, LOW);  // start transmit
-      if (!wifi) 
+//      if (!wifi) 
         digitalWrite(LED_BUILTIN, HIGH);	
       digitalWrite(MAIN_LED_BLUE, HIGH);	    
 
@@ -241,7 +253,7 @@ void loop() {
       if (debug_mode)	  
         Serial.println("Stop transmit!");
       digitalWrite(PTT_PIN, HIGH);  // stop transmit
-      if (!wifi) 
+//      if (!wifi) 
         digitalWrite(LED_BUILTIN, HIGH);	
       digitalWrite(MAIN_LED_BLUE, LOW);	    
 	  
@@ -253,21 +265,21 @@ void loop() {
 	
 //  while ((millis() - sampleTime) < ((unsigned int)samplePeriod)) // - 250))  // was 250 100
   while ((millis() - sampleTime) < ((unsigned int)frameTime)) // - 250))  // was 250 100
-    sleep(0.1); // 25); // 0.5);  // 25);
+    sleep(0.01); // was 0.1 sec
   sampleTime = (unsigned int) millis();	  
 	
 //  delay(2000);
 //  test_radio();
 
   if ((mode == FSK) || (mode == BPSK)) {
-	  if (!wifi) 
+//	  if (!wifi) 
 	    digitalWrite(LED_BUILTIN, LOW);	
 	  digitalWrite(MAIN_LED_BLUE, LOW);
 
 	//  delay(3000);	
 	  sleep(0.2); // 2.845); // 3.0);
 
-	  if (!wifi) 
+//	  if (!wifi) 
 	   	  digitalWrite(LED_BUILTIN, HIGH);
 	  digitalWrite(MAIN_LED_BLUE, HIGH);
   }
@@ -308,8 +320,8 @@ bool TimerHandler1(struct repeating_timer *t) {
   if (BOOTSEL)	  // boot selector button is pressed on Pico
       process_bootsel();
 
-  if (wifi) 
-    check_for_browser();
+//  if (wifi) 
+//    check_for_browser();
 	
   return(true);	
 }
@@ -473,11 +485,13 @@ void transmit_on() {
     digitalWrite(MAIN_LED_BLUE, HIGH);	
     digitalWrite(PTT_PIN, LOW);  
   } 
-  else if (mode == BPSK) {
+  else if ((mode == BPSK) || (mode == FSK)) {
     if (debug_mode)	  
-      Serial.println("Transmit on!");
-    pwm_set_gpio_level(BPSK_PWM_A_PIN, (config.top + 1) * 0.5);
-    pwm_set_gpio_level(BPSK_PWM_B_PIN, (config.top + 1) * 0.5);	
+      Serial.println("Transmit on!!!");
+//  pwm_set_gpio_level(BPSK_PWM_A_PIN, (config.top + 1) * 0.5);
+//  pwm_set_gpio_level(BPSK_PWM_B_PIN, (config.top + 1) * 0.5);	
+    Serial.println("Enable clock outputs");	  	  
+    clockgen.enableOutputs(true);
   }
   else if (mode == CW) {
  //   Serial.println("Transmit on!");
@@ -492,10 +506,13 @@ void transmit_off() {
   if (debug_mode)	
    Serial.println("Transmit off!");
   digitalWrite(MAIN_LED_BLUE, LOW);
-// ITimer0.stopTimer();	  // stop BPSK ISR timer
-  if (mode == BPSK) {
-    pwm_set_gpio_level(BPSK_PWM_A_PIN, 0);	
-    pwm_set_gpio_level(BPSK_PWM_B_PIN, 0);
+  if ((mode == BPSK) || (mode == FSK)) {
+    digitalWrite(BPSK_CONTROL_A, LOW); 	  
+    digitalWrite(BPSK_CONTROL_B, LOW); 	  	  
+//    pwm_set_gpio_level(BPSK_PWM_A_PIN, 0);	
+//    pwm_set_gpio_level(BPSK_PWM_B_PIN, 0);
+    Serial.println("Disable clock outputs");	  
+    clockgen.enableOutputs(false);	  
   }
   else if (mode == SSTV) 
     sstv_end();
@@ -520,7 +537,8 @@ void config_telem() {
   if (mode == FSK) {
     Serial.println("Configuring for FSK\n");
     bitRate = 200;
-    delay_time = (1.0 / 200.0);		  
+    delay_time = 1E6 / bitRate;		  
+    Serial.println(delay_time);	  
     rsFrames = 1;
     payloads = 1;
     rsFrameLen = 64;
@@ -535,13 +553,14 @@ void config_telem() {
     samples = sample_rate / bitRate;
 //    Serial.println(samples);	  
     bufLen = (frameCnt * (syncBits + 10 * (headerLen + rsFrames * (rsFrameLen + parityLen))) * samples);
-//    Serial.println(bufLen);	  
+    bufLen = 970; // 2000;	  
+    Serial.println(bufLen);	  
     samplePeriod =  (int) (((float)((syncBits + 10 * (headerLen + rsFrames * (rsFrameLen + parityLen)))) / (float) bitRate) * 1000 - 500);
     sleepTime = 0.1;
 //    Serial.println(samplePeriod);
 	  
     frameTime = ((float)((float)bufLen / (samples * frameCnt * bitRate))) * 1000; // frame time in ms
-//    Serial.println(frameTime);
+    Serial.println(frameTime);
 //    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);
     memset(buffer, 0xa5, sizeof(buffer)); 
@@ -549,7 +568,8 @@ void config_telem() {
     Serial.println("Configuring for BPSK\n");
     bitRate = 1200;
 //    delay_time = (1.0 / 1200.0);	
-    delay_time = (1.0 / 200.0);	
+    delay_time = 1E6 / bitRate;	
+    Serial.println(delay_time);	  
     rsFrames = 3;
     payloads = 6;
     rsFrameLen = 159;
@@ -848,9 +868,11 @@ void get_tlm_fox() {
   int posXv = 0, negXv = 0, posYv = 0, negYv = 0, posZv = 0, negZv = 0;
   int posXi = 0, negXi = 0, posYi = 0, negYi = 0, posZi = 0, negZi = 0;
   int head_offset = 0;
-  short int buffer_test[bufLen];
-  int buffSize;
-  buffSize = (int) sizeof(buffer_test);
+  //short int buffer_test[bufLen];
+  //int buffSize;
+  //buffSize = (int) sizeof(buffer_test);
+	
+  ctr = 0;  	
 	
   parse_payload();	
 	
@@ -1138,7 +1160,7 @@ void get_tlm_fox() {
     encodeB(b, 49 + head_offset, (int)(sensor[XS2] * 10 + 0.5) + 2048);
 //	  	      Serial.println("D");	  
     int status = STEMBoardFailure + SafeMode * 2 + sim_mode * 4 + PayloadFailure1 * 8 +
-      (i2c_bus0 == false) * 16 + (i2c_bus1 == false) * 32 + (i2c_bus3 == false) * 64 + (camera == OFF) * 128 + groundCommandCount * 256;
+      (i2c_bus0 == false) * 16 + (i2c_bus1 == false) * 32 + (i2c_bus3 == false) * 64 + (camera_detected == OFF) * 128 + groundCommandCount * 256;
     encodeA(b, 51 + head_offset, status);
     encodeB(b, 52 + head_offset, rxAntennaDeployed + txAntennaDeployed * 2);
 //	  	      Serial.println("E");	  
@@ -1270,17 +1292,13 @@ void get_tlm_fox() {
         //  		 ctr/SAMPLES, i, frames, bit, (data > 0) );
         if (mode == FSK) {
           phase = ((data != 0) * 2) - 1;
-          //		printf("Sending a %d\n", phase);
         } else {
           if (data == 0) {
             phase *= -1;
-//            if ((ctr - smaller) > 0) {
-//              for (int j = 1; j <= smaller; j++)
-//                buffer[ctr - j] = buffer[ctr - j] * 0.4;
- //           }
-//            flip_ctr = ctr;
-          }
         }
+ //	printf("Sending a %d\n", phase);
+		
+       }
       }
     }
 ///    #ifdef DEBUG_LOGGING
@@ -1302,17 +1320,17 @@ void get_tlm_fox() {
 //	Serial.print(" ");      
         if (mode == FSK) {
           phase = ((data != 0) * 2) - 1;
-          //			printf("Sending a %d\n", phase);
+//          Serial.printf("Sending a %d\n", phase);
         } else {
           if (data == 0) {
             phase *= -1;
 //            if ((ctr - smaller) > 0) {
 //              for (int j = 1; j <= smaller; j++)
 //                buffer[ctr - j] = buffer[ctr - j] * 0.4;
-//            }
+          }
 //            flip_ctr = ctr;
           }
-        }
+///        }
 //	Serial.println("AA");      
       }
 //	Serial.println("BB");     
@@ -1330,38 +1348,34 @@ void get_tlm_fox() {
   }
 }
 
-void write_wave(int i, short int *buffer)
+//void write_wave(int i, short int *buffer)
+void write_wave(int i, byte *buffer)
 {
-	if (mode == FSK)
-	{
-//		if ((ctr - flip_ctr) < smaller)  // No wave shaping
-//			buffer[ctr++] = (short int)(0.1 * phase * (ctr - flip_ctr) / smaller);
-//		else
-			buffer[ctr++] = (short int)(0.25 * amplitude * phase);
-//		        Serial.print(buffer[ctr - 1]);
-//		        Serial.print(" ");
-	}
-	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)));	
-//			buffer[ctr++] = (short int)(phase * sin_map[ctr % sin_samples] / 2);
-//	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]); 		 } 			
-			buffer[ctr++] = (short int)(phase); 
-	} 			
-	//		printf("%d %d \n", i, buffer[ctr - 1]);
-//	if (ctr > BUFFER_SIZE) {
-//		ctr = ctr - BUFFER_SIZE;
-	if (ctr > bufLen) {
-		ctr = ctr - bufLen;
-		if (debug_mode) {
-//		  Serial.print("\r");
-//		  Serial.print(" ");
-		  Serial.println(millis());
-		}
-	}
+    buffer[ctr++] = (byte)(phase == 1); 
+//    Serial.printf("buffer is %d \n", buffer[ctr - 1]);
+//	Serial.print(ctr);
+//	Serial.print(" ");
+	
+    if (ctr > bufLen) {
+	ctr = ctr - bufLen;
+//	if (debug_mode) {
+//	  Serial.print("ctr reset ");
+    if (bufLen != 0) {
+      float avg_time = (float)(micros() - micros3)/(float)bufLen;
+//      if ((avg_time > (delay_time * 1.15)) || debug_mode) {	    
+        Serial.print("r Microseconds: ");
+        Serial.println(avg_time);
+//      }
+    }
+//  }	  
+    micros3 = micros();	    
+	    
+//	  Serial.print(" ");
+//	  Serial.println(millis());
+//	}
+    }
+//	Serial.printf(" b: %d ", buffer[ctr - 1]);
+
 }
 
 int encodeA(short int  *b, int index, int val) {
@@ -2082,6 +2096,7 @@ void write_little_endian(unsigned int word, int num_bytes, FILE *wav_file)
 
 void config_radio()
 {
+  Serial.println("Configuring radio");	
 /*	
   if (!wifi) 
      pinMode(LED_BUILTIN, OUTPUT);
@@ -2095,25 +2110,24 @@ void config_radio()
   pinMode(TEMPERATURE_PIN, INPUT);
   pinMode(AUDIO_IN_PIN, INPUT);
 */	
-  if ((mode == AFSK) || (mode == FSK) || (mode == SSTV) || (mode == CW)) {
-	  
-    digitalWrite(PD_PIN, HIGH);  // Enable SR_FRS 
+  if ((mode == AFSK) || (mode == SSTV) || (mode == CW)) {
 	  
+    digitalWrite(PD_PIN, HIGH);  // Enable SR_FRS
 //    pinMode(AUDIO_OUT_PIN, OUTPUT);	  
-
     program_radio();
-  	
-//  } else if (mode == FSK)	  {  // moved to below
-//    transmit_on();
+
   } else if (mode == BPSK)  {
-//    start_pwm();
-//    start_isr();	  
+	  
+    clockgen.setClockBPSK();
+    Serial.println("Config clock for BPSK");	  	  
     transmit_on();	
-  }
-	
-  if ((mode == FSK)) //  || (mode == SSTV))
-//    start_isr();   
+  }	
+  else if (mode == FSK)  {//  || (mode == SSTV))
+
+    clockgen.setClockFSK(); 
+    Serial.println("Config clock for FSK");	 
     transmit_on();
+  }
 }
 
 void test_radio()
@@ -2634,6 +2648,7 @@ void read_payload()
 //  delay(100);
 }
 
+/*
 void payload_OK_only()
 {
   payload_str[0] = '\0';  // clear the payload string
@@ -2821,7 +2836,8 @@ void payload_OK_only()
 
   delay(100);
 }
-	
+
+*/	
 /*
 void eeprom_word_write(int addr, int val)
 {
@@ -3002,8 +3018,10 @@ void start_pwm() {
     pwm_config_set_output_polarity( &config, true, false);	
     pwm_init(bpsk_pin_slice_A, &config, true);
     pwm_init(bpsk_pin_slice_B, &config, true);
-    pwm_set_gpio_level(BPSK_PWM_A_PIN, (config.top + 1) * 0.5);
-    pwm_set_gpio_level(BPSK_PWM_B_PIN, (config.top + 1) * 0.5);	
+//    pwm_set_gpio_level(BPSK_PWM_A_PIN, (config.top + 1) * 0.5);
+//    pwm_set_gpio_level(BPSK_PWM_B_PIN, (config.top + 1) * 0.5);	
+    pwm_set_gpio_level(BPSK_PWM_A_PIN, 0);  // start with off
+    pwm_set_gpio_level(BPSK_PWM_B_PIN, 0);	
 }
 /*
 void pwm_interrupt_handler() {
@@ -3148,7 +3166,7 @@ void process_pushbutton() {
 	
 //  return;  /// just skip for now
 	
-  if (!wifi) 	   	
+//  if (!wifi) 	   	
    digitalWrite(LED_BUILTIN, HIGH);  // make sure built in LED is on before starting to blink
 	
   sleep(1.0);
@@ -3177,10 +3195,9 @@ void process_pushbutton() {
 	  
   pb_value = digitalRead(MAIN_PB_PIN);
   if ((pb_value == RELEASED) && (release == FALSE)) {
-    Serial.println("PB: FSK not supported");
+    Serial.println("PB: Switch to FSK");
     release = TRUE;
-//    new_mode = FSK;
-//    setup();
+    new_mode = FSK;
   }
 	
   if (release == FALSE) {
@@ -3246,7 +3263,7 @@ void process_bootsel() {
 	
   int release = FALSE;
 	
-  if (!wifi) 
+//  if (!wifi) 
     digitalWrite(LED_BUILTIN, HIGH);  // make sure built in LED is on before blinking	
 	
   sleep(1.0);
@@ -3275,10 +3292,9 @@ void process_bootsel() {
 	  
 //  pb_value = digitalRead(MAIN_PB_PIN);
   if ((!BOOTSEL) && (release == FALSE)) {
-    Serial.println("BOOTSEL: FSK not supported");
+    Serial.println("BOOTSEL: Switch to FSK");
     release = TRUE;
-//    new_mode = FSK;
-//    setup();
+    new_mode = FSK;
   }
 	
   if (release == FALSE) {
@@ -3335,17 +3351,17 @@ void process_bootsel() {
   }
   if (new_mode != mode)
     transmit_off();
-  sleep(2.0);	
+//  sleep(2.0);	
 }
 
 void blinkTimes(int blinks) {
   for (int i = 0; i < blinks; i++) {
     digitalWrite(MAIN_LED_GREEN, LOW);
-    if (!wifi) 
+//    if (!wifi) 
       digitalWrite(LED_BUILTIN, LOW);
     sleep(0.1);
     digitalWrite(MAIN_LED_GREEN, HIGH);
-    if (!wifi) 
+//    if (!wifi) 
        digitalWrite(LED_BUILTIN, HIGH);
     sleep(0.1);
   }
@@ -3369,7 +3385,7 @@ void config_gpio() {
   pinMode(AUDIO_OUT_PIN, OUTPUT);	
 
   // set LEDs and blink once	
-  if (!wifi) 
+//  if (!wifi) 
     pinMode(LED_BUILTIN, OUTPUT);  // Set LED pin to output
   pinMode(MAIN_LED_GREEN, OUTPUT);  // Set Main Green LED pin to output
   blink_pin(MAIN_LED_GREEN, 150);
@@ -3423,57 +3439,55 @@ void config_gpio() {
 bool TimerHandler0(struct repeating_timer *t) {
 
 //  digitalWrite(STEM_LED_GREEN, !green_led_counter++);
-
-  if (mode == BPSK) {	  // only do this if BPSK mode.  Should turn off timer interrupt when not BPSK in future
-//  Serial.print("l1 ");
-//  Serial.print(wav_position);
-//  Serial.print(" ");
-  while ((micros() - micro_timer2) < 832)	{ } 
-  busy_wait_at_least_cycles(51);	// 300 ns  
+  timer_counter = (timer_counter++) % 5;
+  if ((mode == BPSK) || ((mode == FSK) && (timer_counter == 0)))  {	  // only do this if BPSK mode or every 6 times in FSK mode
+//    Serial.print("l1 ");
+//    Serial.print(wav_position);
+//    Serial.print(" ");
+    while ((micros() - micro_timer2) < delay_time)	{ } 
+//    if (mode == BPSK)	  
+//      busy_wait_at_least_cycles(51);	// 300 ns  
 //  if ((micros() - micro_timer2) > 834)	  
 //    Serial.println(micros() - micro_timer2);	  
-  micro_timer2 = micros();	  	
-  if (buffer[wav_position++] > 0) {	  
-    digitalWrite(BPSK_CONTROL_A, HIGH);
-//    delayMicroseconds(2);    	  
-    digitalWrite(BPSK_CONTROL_B, LOW);  	  
-  } else {
-    digitalWrite(BPSK_CONTROL_B, HIGH);  
-//    delayMicroseconds(2);    	  
-    digitalWrite(BPSK_CONTROL_A, LOW);	    
-  }
-/*	
-    tx_bit = (buffer[wav_position] > 0) ? HIGH: LOW;
-		
-   digitalWrite(AUDIO_OUT_PIN, tx_bit);		
-
-    tx_bit = (buffer[wav_position++] > 0) ? true: false;
-*/    
-/*	
-    if (tx_bit)
-      Serial.print("-");
-    else
-      Serial.print("_");
-*/
-/*	
-    pwm_config_set_output_polarity( &config, tx_bit, tx_bit);	
-    pwm_init(bpsk_pin_slice, &config, true);
-    pwm_set_gpio_level(BPSK_PWM_PIN, (config.top + 1) * 0.5);	 
-*/	
-  if (wav_position > bufLen) { // 300) {
-	wav_position = wav_position - bufLen;
+    micro_timer2 = micros();	  	
+    if (buffer[wav_position++] > 0) {	  
+//      digitalWrite(BPSK_CONTROL_B, LOW);  
+      digitalWrite(BPSK_CONTROL_B, LOW);
+//      delayMicroseconds(10);    	  
+      digitalWrite(BPSK_CONTROL_A, HIGH);  
+//      Serial.print("-");	    
+//      clockgen.enableOutputOnly(1);	  
+    } else {
+//      digitalWrite(BPSK_CONTROL_A, LOW);  
+      digitalWrite(BPSK_CONTROL_A, LOW);  
+//      delayMicroseconds(10);    	  
+      digitalWrite(BPSK_CONTROL_B, HIGH);	
+//      Serial.print("_");	 
+//      clockgen.enableOutputOnly(0);	  
+    }	
+    if (wav_position > bufLen) { // 300) {
+	wav_position = wav_position % bufLen;
 //	Serial.print("\nR");
 //	Serial.print(" ");
 //	Serial.println(millis());
-/*
-  if ((micros() - micro_timer)/bufLen > 835)  {	  
+/**/
+//  if ((micros() - micro_timer)/bufLen > (delay_time + 10))  {	  
+/*	    
     if (bufLen != 0) {		  
       Serial.print("R Microseconds: ");
       Serial.println((float)(micros() - micro_timer)/(float)bufLen);
     }
-  }	  
+//  }	
+*/	    
+    if (bufLen != 0) {
+      float avg_time = (float)(micros() - micro_timer)/(float)bufLen;
+      if ((avg_time > (delay_time * 1.15)) || debug_mode) {	    
+        Serial.print("R Microseconds: ");
+        Serial.println(avg_time);
+      }
+    }	    
     micro_timer = micros();
-*/	  
+/**/	  
   } else {  
 //      Serial.print("R' Microseconds: ");
 //      Serial.println(micros() - micro_timer2);
@@ -3491,20 +3505,21 @@ bool TimerHandler0(struct repeating_timer *t) {
   return true;	
 }
 
+
 void start_isr() {
 	
 //	return;
   if (!timer0_on) {	
 //  if (true) {	                         // always start ISR handler
-	Serial.println("Starting ISR for BPSK");
-	
+	Serial.println("Starting ISR for BPSK/FSK");
+	  
+	timer_counter = 0;
 	pinMode(BPSK_CONTROL_A, OUTPUT);
-	pinMode(BPSK_CONTROL_B, OUTPUT);	
-	
-//  if (ITimer0.attachInterruptInterval(833, TimerHandler0))	
-//  if (ITimer0.attachInterruptInterval(804, TimerHandler0))	
-    if (ITimer0.attachInterruptInterval(800, TimerHandler0))	// was was 828 (841) and 828
-//  if (ITimer0.attachInterruptInterval(1667, TimerHandler0))
+	pinMode(BPSK_CONTROL_B, OUTPUT);
+        digitalWrite(BPSK_CONTROL_A, LOW); 	 // start with off 
+        digitalWrite(BPSK_CONTROL_B, LOW); 	  
+
+   if (ITimer0.attachInterruptInterval(833 - 32, TimerHandler0))	
     {
       if (debug_mode) 	    
         Serial.print(F("Starting ITimer0 OK, micros() = ")); Serial.println(micros());
@@ -3512,11 +3527,12 @@ void start_isr() {
     }
     else
     Serial.println(F("Can't set ITimer0. Select another Timer, freq. or timer"));
+	    
 	  
   } else {
 //     ITimer0.restartTimer();
 //     Serial.println("Restarting ITimer0 for BPSK");	  
-	Serial.println("Don't restart ITimer0 for BPSK");	
+	Serial.println("Don't start ITimer0 for BPSK and FSK");	
   }
 }
   
@@ -3534,7 +3550,8 @@ void start_button_isr() {
     Serial.println(F("Can't set ITimer1. Select another Timer, freq. or timer"));
 
 }
-  
+
+/*
 void client_print_string(char *string)
 {
   int count = 0;
@@ -3628,13 +3645,13 @@ void check_for_browser() {
        }
      }
   }
-/*
+
   Serial.println(" ");
   print_string(var);
   print_string(val);
   Serial.println(" "); 
   Serial.println(strlen(val));
-*/
+
   if (!strcmp(var, "call") && (strlen(val) > 0)) {
      Serial.print("Changing callsign to ");
      print_string(val);
@@ -3677,9 +3694,10 @@ void configure_wifi() {
     server.begin();
   }
 }
-
+*/
+	
 void transmit_cw(int freq, float duration) {  // freq in Hz, duration in milliseconds
-  if (!wifi) 
+ // if (!wifi) 
     digitalWrite(LED_BUILTIN, HIGH);	// Transmit LED on
   digitalWrite(MAIN_LED_BLUE, HIGH);	
 	
@@ -3694,7 +3712,7 @@ void transmit_cw(int freq, float duration) {  // freq in Hz, duration in millise
     sleep(min(time_left, period_us) / 1.0E6);  
   }
   digitalWrite(AUDIO_OUT_PIN, LOW);	
-  if (!wifi) 
+//  if (!wifi) 
     digitalWrite(LED_BUILTIN, LOW);	// Transmit LED off
   digitalWrite(MAIN_LED_BLUE, LOW);	
 }
@@ -3713,7 +3731,7 @@ void transmit_callsign(char *callsign) {
   }
   transmit_off();	
   transmit_string(id);	
-  transmit_on();	
+//  transmit_on();	
 }
 
 void transmit_string(char *string) {
@@ -3830,7 +3848,7 @@ void load_files() {
     }
     f.close();
   }
-
+/*
   f = LittleFS.open("sstv_image_2_320_x_240.jpg", "r");
   if (f) {	
     Serial.println("Image sstv_image_2_320_x_240.jpg already in FS");
@@ -3844,7 +3862,7 @@ void load_files() {
     }
     f.close();
   }
-	
+*/	
   show_dir();
 }
 
@@ -3897,7 +3915,8 @@ void serial_input() {
 		   
      case 'f':
      case 'F':
-      Serial.println("FSK/DUV mode not supported");	     
+      Serial.println("Change to FSK mode");
+       new_mode = FSK;	    
        break;	
 		   
      case 'b':
@@ -3975,9 +3994,12 @@ void serial_input() {
 		   
        break;
     }
+    if ((mode == SSTV) && prompt)  // stop SSTV if need to prompt for input
+      sstv_end();	    
+	    
     if (new_mode != mode)
       transmit_off();
-    sleep(2.0);		    
+//    sleep(2.0);		    
    }
  }
  }
@@ -4008,7 +4030,10 @@ void prompt_for_input() {
        Serial.println("?  Query sensors");	
        Serial.println("v  Read INA219 voltage and current");	
        Serial.println("o  Read diode temperature");	
-       Serial.println("d  Change debug mode\n");		  
+       Serial.println("d  Change debug mode\n");
+		  
+       Serial.printf("Config file /sim.cfg contains %s %d %f %f %s\n\n", callsign, reset_count, lat_file, long_file, sim_yes);
+		  
        break;	
 		  
     case PROMPT_CALLSIGN:
@@ -4219,6 +4244,9 @@ void set_lat_lon() {
 
 void program_radio() {
 	
+  digitalWrite(PD_PIN, HIGH);  // enable SR_FRS
+  digitalWrite(PTT_PIN, HIGH);  // stop transmit	
+	
   DumbTXSWS mySerial(SWTX_PIN); // TX pin
   mySerial.begin(9600);
     
@@ -4228,9 +4256,10 @@ void program_radio() {
 //    mySerial.println("AT+DMOSETGROUP=0,434.9000,434.9000,1,2,1,1\r");    
 //     mySerial.println("AT+DMOSETGROUP=0,434.9000,434.9000,0,8,0,0\r");  
 //     mySerial.println("AT+DMOSETGROUP=0,432.2510,432.2510,0,8,0,0\r");  
-     mySerial.println("AT+DMOSETGROUP=0,432.2500,432.2500,0,8,0,0\r");  
+//     mySerial.println("AT+DMOSETGROUP=0,432.2500,432.2500,0,8,0,0\r");  
+     mySerial.println("AT+DMOSETGROUP=0,434.9100,434.9100,0,8,0,0\r");  
 //   sleep(0.5);	  
-   mySerial.println("AT+DMOSETMIC=6,0\r");  // was 8
+   mySerial.println("AT+DMOSETMIC=8,0\r");  // was 8
 	
   }
 }
@@ -4270,3 +4299,19 @@ void write_mode() {
   mode_file.close();
 //  Serial.println("Write complete");	
 }
+
+void start_clockgen() {
+
+  if (clockgen.begin() != ERROR_NONE)
+  {
+    /* There was a problem detecting the IC ... check your connections */
+    Serial.print("No Si5351 detected ... Check your wiring or I2C ADDR!");
+    return;	  
+  }
+
+  Serial.println("Starting clockgen frequency 434.9 MHz");
+	
+//  clockgen.setClockFSK();  // default to FSK
+  clockgen.enableOutputs(false);	
+	
+}
diff --git a/cubesatsim/esp32-cam-send-jpeg-serial/esp32-cam-send-jpeg-serial.ino b/cubesatsim/esp32-cam-send-jpeg-serial/esp32-cam-send-jpeg-serial.ino
index 71449d30..fcb62e9d 100644
--- a/cubesatsim/esp32-cam-send-jpeg-serial/esp32-cam-send-jpeg-serial.ino
+++ b/cubesatsim/esp32-cam-send-jpeg-serial/esp32-cam-send-jpeg-serial.ino
@@ -23,14 +23,91 @@
 #include <Arduino.h>
 #include "esp_camera.h"
 #include <FastCRC.h>
-#include "FS.h"
+//#include "FS.h"
 #include "SPIFFS.h"
+#include <TJpg_Decoder.h>
 
 // some info: https://visualgdb.com/tutorials/esp32/camera/
 
 //#define DEBUG
 #define FORMAT_SPIFFS_IF_FAILED true
 
+//FONTS
+const uint8_t b_fonts[43][11] = {
+        {0x00, 0x18, 0x24, 0x62, 0x62, 0x62, 0x7E, 0x62, 0x62, 0x62, 0x00}, //00: A
+        {0x00, 0x7C, 0x32, 0x32, 0x32, 0x3C, 0x32, 0x32, 0x32, 0x7C, 0x00}, //01: B
+        {0x00, 0x3C, 0x62, 0x62, 0x60, 0x60, 0x60, 0x62, 0x62, 0x3C, 0x00}, //02: C
+        {0x00, 0x7C, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x7C, 0x00}, //03: D
+        {0x00, 0x7E, 0x60, 0x60, 0x60, 0x7C, 0x60, 0x60, 0x60, 0x7E, 0x00}, //04: E
+        {0x00, 0x7E, 0x60, 0x60, 0x60, 0x7C, 0x60, 0x60, 0x60, 0x60, 0x00}, //05: F
+        {0x00, 0x3C, 0x62, 0x62, 0x60, 0x60, 0x66, 0x62, 0x62, 0x3C, 0x00}, //06: G
+        {0x00, 0x62, 0x62, 0x62, 0x62, 0x7E, 0x62, 0x62, 0x62, 0x62, 0x00}, //07: H
+        {0x00, 0x3C, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x3C, 0x00}, //08: I
+        {0x00, 0x1E, 0x0C, 0x0C, 0x0C, 0x0C, 0x4C, 0x4C, 0x4C, 0x38, 0x00}, //09: J
+        {0x00, 0x62, 0x64, 0x68, 0x70, 0x68, 0x64, 0x62, 0x62, 0x62, 0x00}, //10: K
+        {0x00, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x7E, 0x00}, //11: L
+        {0x00, 0x42, 0x62, 0x76, 0x6A, 0x62, 0x62, 0x62, 0x62, 0x62, 0x00}, //12: M
+        {0x00, 0x42, 0x62, 0x72, 0x6A, 0x66, 0x62, 0x62, 0x62, 0x62, 0x00}, //13: N
+        {0x00, 0x3C, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x3C, 0x00}, //14: O
+        {0x00, 0x7C, 0x62, 0x62, 0x62, 0x7C, 0x60, 0x60, 0x60, 0x60, 0x00}, //15: P
+        {0x00, 0x3C, 0x62, 0x62, 0x62, 0x62, 0x62, 0x6A, 0x6A, 0x3C, 0x08}, //16: Q
+        {0x00, 0x7C, 0x62, 0x62, 0x62, 0x7C, 0x68, 0x64, 0x62, 0x62, 0x00}, //17: R
+        {0x00, 0x3C, 0x62, 0x60, 0x60, 0x3C, 0x06, 0x06, 0x46, 0x3C, 0x00}, //18: S
+        {0x00, 0x7E, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00}, //19: T
+        {0x00, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x3C, 0x00}, //20: U
+        {0x00, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x22, 0x14, 0x08, 0x00}, //21: V
+        {0x00, 0x62, 0x62, 0x62, 0x62, 0x62, 0x6A, 0x76, 0x62, 0x42, 0x00}, //22: W
+        {0x00, 0x42, 0x62, 0x74, 0x38, 0x1C, 0x2E, 0x46, 0x42, 0x42, 0x00}, //23: X
+        {0x00, 0x42, 0x62, 0x74, 0x38, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00}, //24: Y
+        {0x00, 0x7E, 0x06, 0x0E, 0x0C, 0x18, 0x30, 0x70, 0x60, 0x7E, 0x00}, //25: Z
+        {0x00, 0x3C, 0x62, 0x62, 0x66, 0x6A, 0x72, 0x62, 0x62, 0x3C, 0x00}, //26: 0
+        {0x00, 0x38, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00}, //27: 1
+        {0x00, 0x3C, 0x46, 0x06, 0x06, 0x1C, 0x20, 0x60, 0x60, 0x7E, 0x00}, //28: 2
+        {0x00, 0x3C, 0x46, 0x06, 0x06, 0x1C, 0x06, 0x06, 0x46, 0x3C, 0x00}, //29: 3
+        {0x00, 0x0C, 0x1C, 0x2C, 0x4C, 0x4C, 0x7E, 0x0C, 0x0C, 0x0C, 0x00}, //30: 4
+        {0x00, 0x7E, 0x60, 0x60, 0x60, 0x7C, 0x06, 0x06, 0x46, 0x3C, 0x00}, //31: 5
+        {0x00, 0x3C, 0x62, 0x60, 0x60, 0x7C, 0x62, 0x62, 0x62, 0x3C, 0x00}, //32: 6
+        {0x00, 0x7E, 0x06, 0x0C, 0x18, 0x30, 0x30, 0x30, 0x30, 0x30, 0x00}, //33: 7
+        {0x00, 0x3C, 0x62, 0x62, 0x62, 0x3C, 0x62, 0x62, 0x62, 0x3C, 0x00}, //34: 8
+        {0x00, 0x3C, 0x46, 0x46, 0x46, 0x3E, 0x06, 0x06, 0x46, 0x3C, 0x00}, //35: 9
+        {0x00, 0x00, 0x02, 0x06, 0x0E, 0x1C, 0x38, 0x70, 0x60, 0x40, 0x00}, //36: /
+        {0x00, 0x00, 0x00, 0x00, 0x00, 0x7E, 0x7E, 0x00, 0x00, 0x00, 0x00}, //37: -
+        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x06, 0x00}, //38: .
+        {0x00, 0x3C, 0x46, 0x06, 0x06, 0x0C, 0x10, 0x00, 0x30, 0x30, 0x00}, //39: ?
+        {0x00, 0x18, 0x18, 0x18, 0x18, 0x10, 0x10, 0x00, 0x18, 0x18, 0x00}, //40: !
+        {0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00}, //41: :
+        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}  //42: space
+};
+
+// Nibble font table
+const uint8_t l_fonts[23][5] = {
+  { 0xE2, 0xA6, 0xA2, 0xA2, 0xE2 }, // 0: 01
+  { 0xEE, 0x22, 0xE6, 0x82, 0xEE }, // 1: 23
+  { 0xAE, 0xA8, 0xEE, 0x22, 0x2E }, // 2: 45
+  { 0x8E, 0x82, 0xE2, 0xA2, 0xE2 }, // 3: 67
+  { 0xEE, 0xAA, 0xEE, 0xA2, 0xE2 }, // 4: 89
+  { 0x00, 0x22, 0x00, 0x22, 0x04 }, // 5: :;
+  { 0x20, 0x4E, 0x80, 0x4E, 0x20 }, // 6: <=
+  { 0x8E, 0x42, 0x26, 0x40, 0x84 }, // 7: >?
+  { 0x64, 0x9A, 0xBE, 0x8A, 0x7A }, // 8: @A
+  { 0xC6, 0xA8, 0xC8, 0xA8, 0xC6 }, // 9: BC
+  { 0xCE, 0xA8, 0xAC, 0xA8, 0xCE }, // 10: DE
+  { 0xE6, 0x88, 0xCE, 0x8A, 0x86 }, // 11: FG
+  { 0xA4, 0xA4, 0xE4, 0xA4, 0xA4 }, // 12: HI
+  { 0x69, 0x2A, 0x2C, 0x2A, 0x49 }, // 13: JK
+  { 0x8A, 0x8E, 0x8E, 0x8A, 0xEA }, // 14: LM
+  { 0x04, 0x9A, 0xDA, 0xBA, 0x94 }, // 15: NO
+  { 0xC4, 0xAA, 0xCA, 0x8E, 0x86 }, // 16: PQ
+  { 0xC6, 0xA8, 0xC4, 0xA2, 0xAC }, // 17: RS
+  { 0xE0, 0x4A, 0x4A, 0x4A, 0x44 }, // 18: TU
+  { 0x09, 0xA9, 0xA9, 0x6F, 0x26 }, // 19: vW (sort of..)
+  { 0x0A, 0xAA, 0x46, 0xA2, 0x04 }, // 20: XY
+  { 0xE6, 0x24, 0x44, 0x84, 0xE6 }, // 21: Z[
+  { 0x00, 0x00, 0x00, 0x00, 0x00 }  // 22: SPACE
+};
+
+char input_buffer[240][240][3];
+
 void app_main();
 void send_image_serial(char *filename);
 void save_camera_image(char* filename);
@@ -49,6 +126,11 @@ char file_buf[50000];
 int file_len;
 bool file_flag = false;
 camera_fb_t *pic;
+File inFile;
+File outFile;
+int blocks = 0;
+//int counter = 0;
+bool write_complete = false;
 
 // example code from https://github.com/espressif/arduino-esp32/blob/master/libraries/LittleFS/examples/LITTLEFS_test/LITTLEFS_test.ino
 //
@@ -96,9 +178,11 @@ void setup() {
 
   delay(5000);  
 
-  Serial.begin(112500);
+  Serial.begin(115200);
 
   initialize_camera();
+        
+  config_camera();      
 
   Serial.println("ESP32-CAM Camera initialized");
 
@@ -234,9 +318,22 @@ static esp_err_t init_camera()
     //        ESP_LOGE(TAG, "Camera Init Failed");
     return err;
   }
+   
+        
   return ESP_OK;
 }
 
+void config_camera() {
+ 
+  sensor_t * s = esp_camera_sensor_get();
+        
+//  s->set_brightness(s, 2);     // -2 to 2
+//  s->set_contrast(s, 0);       // -2 to 2
+//  s->set_saturation(s, 1);     // -2 to 2  
+  s->set_hmirror(s, 1);        // 0 = disable , 1 = enable
+//  s->set_vflip(s, 1);         
+}
+
 void initialize_camera() {
   if (ESP_OK != init_camera()) {
     Serial.println("Failed to initialize camera!");
@@ -461,3 +558,724 @@ void send_image_serial(char *filename)
     }
   Serial.println("File sent!");
 }
+
+void print_hex(byte octet) {
+      char hexValue[5];
+      sprintf(hexValue, "%02X", octet);
+      Serial.print(hexValue); 
+}
+
+char img_block[320][8][3];   // 320 pixels per row, 8 rows, 3 values (RGB) per.
+  
+bool get_block(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t* bitmap)
+{
+#ifdef DEBUG  
+  Serial.println("\nBlock callback");
+  Serial.println(x);
+  Serial.println(y);
+  Serial.println(w);
+  Serial.println(h);
+#endif
+  
+//  return 1;
+
+  uint16_t pixel_value;
+  uint16_t *pixel;
+  bool last_block = ((x == (320 - w)) & (y == (240 - h)));
+//  char buffer[16 * 8 * 3];
+  int counter = 0;
+  //int x_block = (x / w) % w;
+  //int y_block = (y / h) % h;
+  
+/*  
+  if (((y % h) == 0) && ((x % w) == 0)) {
+    Serial.print("\nStart of row! x = ");
+    Serial.print(x);
+    Serial.print(" y = ");
+    Serial.println(y);
+  }
+*/
+  pixel = bitmap;
+  uint32_t total_pixels = w * h;
+
+//  while (total_pixels--) {
+    while (counter < total_pixels) {
+    pixel_value = *pixel;
+    
+    int y_rel = counter / w;
+    int x_rel = counter % w; //  - y_rel * w;
+/*  
+    Serial.print("Relative x + x = ");
+    Serial.print(x_rel + x);
+    Serial.print(" y + y = ");
+    Serial.print(y_rel);
+    Serial.print(" counter = ");
+    Serial.println(counter);
+*/
+  /*
+    if ((x == 0) && (y == 0)) {
+      Serial.print(" ");
+      Serial.print(pixel_value, HEX);
+      Serial.print(" ");
+    }
+*/    
+//    buffer[counter++] = pixel_value >> 8;
+//    buffer[counter++] = pixel_value;
+
+    byte red = (pixel_value & 0b1111100000000000) >> 8;
+    byte green = (pixel_value & 0b0000011111100000) >> 3;
+    byte blue = (pixel_value & 0b0000000000011111) << 3;
+    
+//    buffer[counter++] = red;
+//    buffer[counter++] = green;
+//    buffer[counter++] = blue;
+    
+    img_block[x_rel + x][y_rel][0] = red;
+    img_block[x_rel + x][y_rel][1] = green;
+    img_block[x_rel + x][y_rel][2] = blue;  
+  
+#ifdef DEBUG   
+    Serial.print("\npixel_value: ");  
+    Serial.print(pixel_value);  
+    Serial.print("\nRGB: ");  
+    print_hex(red);
+    print_hex(green);
+    print_hex(blue);
+      
+    Serial.println("\n img_block: ");
+    for (int k = 0; k < 3; k ++)  
+      print_hex(*(&img_block[x_rel + x][y_rel][0] + k));      
+#endif    
+ /*
+    if (counter >= 155000) {
+      Serial.println("Resetting counter****************************************\n");
+      counter = 0;
+    }
+*/
+    counter++;
+    pixel++;
+  }
+  
+//  Serial.println("\nWriting block to file");
+//  Serial.print("Sizeof buffer: ");
+//  Serial.println(sizeof(buffer));
+  if (x == 304) {
+    if (outFile) {
+      Serial.println("********************************************* Writing block!");
+      outFile.write((const uint8_t*)&img_block[0][0][0], sizeof(img_block));  
+    } else
+      Serial.println("Problem writing block");
+    counter = 0;
+  }
+  if (last_block) {
+    Serial.println("Complete!\n\n");
+  }
+/*
+    for (int i = 0; i < counter; i++) {
+//      Serial.print(buffer[i], HEX);
+      char hexValue[5];
+      sprintf(hexValue, "%02X", buffer[i]);
+      Serial.print(hexValue);
+    }
+ */ 
+    
+//    Serial.print("\n\n Size: ");
+//    Serial.println(counter);
+    
+//    write_complete = true;
+//  }
+
+//  delay(1000);
+
+  blocks++;
+
+  return 1;
+}
+ 
+int JpegDec_i;
+int JpegDec_j;
+int JpegDec_height = 240;
+int JpegDec_width = 320;
+byte  JpegDec_sortBuf[15360]; //320(px)*16(lines)*3(bytes) // Header buffer
+int JpegDec_pxSkip;
+uint8_t *JpegDec_pImg;
+int JpegDec_x, JpegDec_y, JpegDec_bx, JpegDec_by;
+int JpegDec_comps = 3;
+  
+bool merged_get_block(int16_t x, int16_t y, uint16_t w, uint16_t h, uint16_t* bitmap)
+{
+#ifdef DEBUG
+  Serial.println("\nBlock callback");
+  Serial.println(x);
+  Serial.println(y);
+  Serial.println(w);
+  Serial.println(h);
+#endif
+  int JpegDec_MCUx = x;
+  int JpegDec_MCUy = y;
+  int JpegDec_MCUHeight = h;
+  int JpegDec_MCUWidth = w;
+
+//  JpegDec_pImg = bitmap;
+  
+//    JpegDec_pImg = JpegDec_pImage;
+    for(JpegDec_by=0; JpegDec_by<JpegDec_MCUHeight; JpegDec_by++){
+      for(JpegDec_bx=0; JpegDec_bx<JpegDec_MCUWidth; JpegDec_bx++){
+//        JpegDec_x = JpegDec_MCUx * JpegDec_MCUWidth + JpegDec_bx;
+//        JpegDec_y = JpegDec_MCUy * JpegDec_MCUHeight + JpegDec_by;
+        JpegDec_x = JpegDec_MCUx + JpegDec_bx;
+        JpegDec_y = JpegDec_MCUy + JpegDec_by;
+        if(JpegDec_x<JpegDec_width && JpegDec_y<JpegDec_height){
+//          if(JpegDec_comps == 1){ // Grayscale
+//            //sprintf(str,"%u", pImg[0]);
+//            outFile.write(JpegDec_pImg, 1);
+//          }else
+        { // RGB
+            // When saving to the SD, write 16 lines on one time
+            // First we write on the array 16 lines and then we save to SD
+            JpegDec_pxSkip = ((JpegDec_y - (16 * JpegDec_j)) * 320) + JpegDec_x;
+          
+            int pixel_value = *bitmap;
+            
+            byte red = (pixel_value & 0b1111100000000000) >> 8;
+            byte green = (pixel_value & 0b0000011111100000) >> 3;
+            byte blue = (pixel_value & 0b0000000000011111) << 3;            
+            
+            JpegDec_sortBuf[(3 * JpegDec_pxSkip) + 0] = red;  // JpegDec_pImg[0];
+            JpegDec_sortBuf[(3 * JpegDec_pxSkip) + 1] = green; // JpegDec_pImg[1];
+            JpegDec_sortBuf[(3 * JpegDec_pxSkip) + 2] = blue; // JpegDec_pImg[2];
+#ifdef DEBUG          
+            Serial.print("sortBuf index = ");
+            Serial.println((3 * JpegDec_pxSkip));
+#endif
+            JpegDec_i++;
+            if(JpegDec_i == 5120){ //320(px)x16(lines)
+#ifdef DEBUG
+              Serial.println("Writing lines!");
+#endif
+              //              for(k = 0; k < 15360; k++){
+//                imgFile.write(sortBuf[k]);
+//              }
+              outFile.write(JpegDec_sortBuf, sizeof(JpegDec_sortBuf));
+              JpegDec_i = 0;
+              JpegDec_j++; //15(sections)
+            }
+          }
+        }
+//        JpegDec_pImg += JpegDec_comps ;
+        bitmap++;
+      }
+    }
+#ifdef DEBUG  
+  Serial.println("Block processed!");
+#endif
+  return 1;
+}
+
+void jpeg_decode(char* filename, char* fileout, bool debug){
+  uint8_t *pImg;
+//  uint16_t *pImg;
+  int x,y, bx,by;
+  byte sortBuf[15360]; //320(px)*16(lines)*3(bytes) // Header buffer
+  int i,j,k;
+  int pxSkip;
+  
+  // Open the file for writing
+//  File imgFile = SD.open(fileout, FILE_WRITE);
+  outFile = SPIFFS.open(fileout, "w+");
+  
+  if (outFile) {
+    if (debug)
+      Serial.println("Output opened");
+  }
+  else
+    Serial.println("Failed to open output");
+
+ /*  
+  for(i = 0; i < 15360; i++){ // Cleaning Header Buffer array
+    sortBuf[i] = 0xFF;
+  }
+
+  for(i = 0; i < 12; i++){
+    byte fontNumber;
+    char ch;
+    ch = charId[i];
+    for(y = 0; y < 11; y++){
+      for(x = 0; x < 8; x++){
+        pxSkip = 16 + (320 * (y + 3)) + (3 * 8 * i) + (3 * x); //Width: x3
+
+        uint8_t mask;
+        mask = pow(2, 7 - x);
+
+        if(ch >= 65 && ch <= 90){ // A to Z
+                fontNumber = ch - 65;
+        }
+        else if(ch >= 48 && ch <= 57){ //0 to 9
+                fontNumber = ch - 22;
+        }
+        else if(ch == '/'){fontNumber = 36;}
+        else if(ch == '-'){fontNumber = 37;}
+        else if(ch == '.'){fontNumber = 38;}
+        else if(ch == '?'){fontNumber = 39;}
+        else if(ch == '!'){fontNumber = 40;}
+        else if(ch == ':'){fontNumber = 41;}
+        else if(ch == ' '){fontNumber = 42;}
+        else              {fontNumber = 42;}
+
+        if((b_fonts[fontNumber][y] & mask) != 0){
+          for(j = 0; j < 9; j++){
+                  sortBuf[(3 * pxSkip) + j] = 0x00;
+          }
+        }
+      }
+    }
+  }
+
+//  for(k = 0; k < 15360; k++){  // Adding header to the binary file
+//    imgFile.write(sortBuf[k]);
+//  }
+  outFile.write(sortBuf, sizeof(sortBuf));
+*/
+        
+//  writeFooter(&outFile);  //Writing first 10560 bytes (11*320*3)  // write footer after rotate
+  
+  // Decoding start
+  
+  if (debug)
+    Serial.println("Starting jpeg decode");
+  
+  JpegDec_i = 0;
+  JpegDec_j = 0;
+  
+  uint16_t w = 0, h = 0;
+  // TJpgDec.getFsJpgSize(&w, &h, "/cam.jpg", LittleFS); // Note name preceded with "/"
+  TJpgDec.getFsJpgSize(&w, &h, filename, SPIFFS); // Note name preceded with "/"
+  
+  if (debug) {
+    Serial.print("Width = "); 
+    Serial.print(w); 
+    Serial.print(", height = "); 
+    Serial.println(h);
+  }
+  
+  if ((w == 0) && (h == 0)) {
+    Serial.println("Failed to open jpeg input");
+    return;
+  }
+//  counter = 0;
+//  write_complete = false;
+  
+  TJpgDec.setJpgScale(1);
+  TJpgDec.setSwapBytes(false);    // was true
+//  TJpgDec.setCallback(get_block);  
+  TJpgDec.setCallback(merged_get_block);  
+  //TJpgDec.drawFsJpg(0, 0, "/cam.jpg", LittleFS);
+  TJpgDec.drawFsJpg(0, 0, filename, SPIFFS);
+  
+  if (debug)
+    Serial.println("Draw complete");
+  
+//  while (!write_complete) { Serial.println("Waiting..."); delay(500);}
+
+/*  
+  JpegDec.decodeFile(filename);
+  // Image Information
+  Serial.print("Width     :");
+  Serial.println(JpegDec.width);
+  Serial.print("Height    :");
+  Serial.println(JpegDec.height);
+  Serial.print("Components:");
+  Serial.println(JpegDec.comps);
+  Serial.print("MCU / row :");
+  Serial.println(JpegDec.MCUSPerRow);
+  Serial.print("MCU / col :");
+  Serial.println(JpegDec.MCUSPerCol);
+  Serial.print("Scan type :");
+  Serial.println(JpegDec.scanType);
+  Serial.print("MCU width :");
+  Serial.println(JpegDec.MCUWidth);
+  Serial.print("MCU height:");
+  Serial.println(JpegDec.MCUHeight);
+  Serial.println("");
+*/
+  if (debug)
+    Serial.println("Writing bin to FS");
+
+//  imgFile.write(JpegDec.pImage, sizeof(JpegDec.pImage));
+  
+/*  
+  i = 0;
+  j = 0;
+  while(JpegDec.read()){
+    pImg = JpegDec.pImage ;
+    for(by=0; by<JpegDec.MCUHeight; by++){
+      for(bx=0; bx<JpegDec.MCUWidth; bx++){
+        x = JpegDec.MCUx * JpegDec.MCUWidth + bx;
+        y = JpegDec.MCUy * JpegDec.MCUHeight + by;
+        if(x<JpegDec.width && y<JpegDec.height){
+          if(JpegDec.comps == 1){ // Grayscale
+            //sprintf(str,"%u", pImg[0]);
+            imgFile.write(pImg, sizeof(pImg));
+          }else{ // RGB
+            // When saving to the SD, write 16 lines on one time
+            // First we write on the array 16 lines and then we save to SD
+            pxSkip = ((y - (16 * j)) * 320) + x;
+            sortBuf[(3 * pxSkip) + 0] = pImg[0];
+            sortBuf[(3 * pxSkip) + 1] = pImg[1];
+            sortBuf[(3 * pxSkip) + 2] = pImg[2];
+            i++;
+            if(i == 5120){ //320(px)x16(lines)
+//              for(k = 0; k < 15360; k++){
+                imgFile.write(sortBuf, sizeof(sortBuf));
+//              }
+              i = 0;
+              j++; //15(sections)
+            }
+          }
+        }
+        pImg += JpegDec.comps ;
+      }
+    }
+  }
+*/
+  if (debug)
+    Serial.println("Bin has been written to FS");
+  outFile.close();
+}
+
+/*  
+//void shot_pic(){
+void get_pic() {
+  int32_t start_time = millis();  
+  
+  
+  time = millis() - start_time;
+  Serial.println("get_pic done!");
+  Serial.print(start_time); 
+  Serial.println(" ms elapsed");
+  
+  return; 
+}
+  
+  // Try to locate the camera
+  if (cam.begin()) {
+    Serial.println("Camera Found:");
+  } else {
+    Serial.println("No camera found?");
+    return;
+  }
+  for (int i = 0; i <= 10; i++){
+    cam.setImageSize(VC0706_320x240);
+  }
+  Serial.println("Snap in 3 secs...");
+  delay(3000);
+  if (! cam.takePicture())
+    Serial.println("Failed to snap!");
+  else
+    Serial.println("Picture taken!");
+  // Create an image with the name IMAGExx.JPG`
+  strcpy(pic_filename, "IMAGE00.JPG");
+  for (int i = 0; i < 100; i++) {
+    pic_filename[5] = '0' + i/10;
+    pic_filename[6] = '0' + i%10;
+    // create if does not exist, do not open existing, write, sync after write
+    if (! SD.exists(pic_filename)) {
+      break;
+    }
+  }
+  // Open the file for writing
+  File imgFile = SD.open(pic_filename, FILE_WRITE);
+  // Get the size of the image (frame) taken
+  uint16_t jpglen = cam.frameLength();
+  Serial.print("Storing ");
+  Serial.print(jpglen, DEC);
+  Serial.print(" byte image.");
+  int32_t time = millis();
+  pinMode(8, OUTPUT);
+  // Read all the data up to # bytes!
+  byte wCount = 0; // For counting # of writes
+  while (jpglen > 0) {
+    // read 32 bytes at a time;
+    uint8_t *buffer;
+    uint8_t bytesToRead = min(32, jpglen); // change 32 to 64 for a speedup but may not work with all setups!
+    buffer = cam.readPicture(bytesToRead);
+    imgFile.write(buffer, bytesToRead);
+    if(++wCount >= 64) { // Every 2K, give a little feedback so it doesn't appear locked up
+      Serial.print('.');
+      wCount = 0;
+    }
+    //Serial.print("Read ");  Serial.print(bytesToRead, DEC); Serial.println(" bytes");
+    jpglen -= bytesToRead;
+  }
+  imgFile.close();
+  time = millis() - time;
+  Serial.println("done!");
+  Serial.print(time); Serial.println(" ms elapsed");
+  
+*/  
+//}
+
+/**     Write on a file with 11 lines the values of the GPS
+ * @param dst Given an opened File stream then write data to dst.
+ * @param latitude Floating point latitude value in degrees/min as received from the GPS (DDMM.MMMM)
+ * @param lat N/S
+ * @param longitude Floating point longitude value in degrees/min as received from the GPS (DDMM.MMMM)
+ * @param lon E/W
+ * @param altitude Altitude in meters above MSL
+ */
+
+/*
+//void writeFooter(File* dst, nmea_float_t latitude, char lat, nmea_float_t longitude, char lon, nmea_float_t altitude){    //Write 16 lines with values
+void writeFooter(File* dst){
+  int x,y;
+  byte sortBuf[10560]; //320(px)*11(lines)*3(bytes) // Header buffer
+  int i,j,k;
+  int pxSkip;
+  char res[51] = "LAT: 1234.1234N     LONG: 1234.1234W     ALT:10000";
+  for(i = 0; i < 10560; i++){ // Cleaning Header Buffer array
+    sortBuf[i] = 0xFF;
+  }
+  for(i = 0; i < sizeof(res); i++){
+    byte fontNumber;
+    char ch;
+    ch = res[i];
+    for(y = 0; y < 5; y++){
+      for(x = 0; x < 4; x++){
+        //pxSkip = HORIZONTALOFFSET + VERSTICALOFFSET + (BITSPERWORD * i);
+        //pxSkip = 16 + (320 * (y + 3)) + (4 * 2 * i) + (2 * x); Width: x2
+        pxSkip = 16 + (320 * (y + 3)) + (4 * i) + x;
+        // If ch is pair mask is: 11110000, if no 00001111
+        uint8_t sl = (ch % 2)? 3 : 7 ;
+        uint8_t mask = pow(2, sl - x);
+        if(ch >= 48 && ch <=91){
+          fontNumber = (ch-48)/2;
+        }
+        else {
+          fontNumber = 22;
+        }
+        if((l_fonts[fontNumber][y] & mask) != 0){
+          for(j = 0; j < 3; j++){
+                  sortBuf[(3 * pxSkip) + j] = 0x00;
+          }
+        }
+      }
+    }
+  }
+//  for(k = 0; k < 10560; k++){  // Adding header to the binary file
+//    dst->write(sortBuf[k]);
+    dst->write(sortBuf, sizeof(sortBuf));
+//  }
+}
+*/
+
+void raw_decode(char* filename, char* fileout){  // used to decode .raw files in RGB565 format
+
+// Open the input file for reading
+  inFile = SPIFFS.open(filename, "r");
+  
+  if (inFile)
+    Serial.println("Input opened");
+  else {
+    Serial.println("Failed to open input");
+    return;
+  }
+// Open the output file for writing
+  outFile = SPIFFS.open(fileout, "w+");
+  
+  if (outFile)
+    Serial.println("Output opened");
+  else {
+    Serial.println("Failed to open output");
+    return;
+  }
+  char buff[2];
+  char buffer[3];
+  
+  int i = 0;
+//  int redx = 128;
+//  int greenx = 128;
+//  int bluex = 128;
+  
+//  while (i++ < (320 * 240 * 3)) {
+  while (i++ < (320 * 240 * 1.49)) {
+    inFile.readBytes(buff, 2);
+    
+#ifdef DEBUG    
+    print_hex(buff[0]);
+    print_hex(buff[1]);
+#endif
+    
+    int pixel_value = (buff[0] << 8) + buff[1];  // endian for raw
+ 
+    byte red = (pixel_value & 0b1111100000000000) >> 8;
+    byte green = (pixel_value & 0b0000011111100000) >> 3;
+    byte blue = (pixel_value & 0b0000000000011111) << 3;
+
+#ifdef TEST_PATTERN    
+    int size = 5; // 46;
+    int y = (int)( i / 320 );
+    int x = (int)( i - y * 320 );
+    int box = (int)(x/size) + (int)(y/size);  
+ 
+    if (y < 10) { // 20) {
+      red = 0;
+      green = 255;
+      blue = 0;
+    }   
+    else if ( box == ( (int)(box / 2) * 2)) {
+//      Serial.println(x);
+//      Serial.println(y);
+//      Serial.println(box);
+//      Serial.println(" ");
+      red = 255; //(100 + x) % 256;
+      green = 0; // ;
+      blue = 0;    
+    } else  {
+//      Serial.println(x);
+//      Serial.println(y);
+//      Serial.println(box);
+//      Serial.println(" ");
+      red = 0;
+      green = 0;
+      blue = 255; //(100 + y) % 256;
+    }  
+#endif    
+    
+      buffer[0] = red;
+      buffer[1] = green;
+      buffer[2] = blue;    
+/*     
+    if (y < 20) { // 20) {
+      buffer[0] = 0;
+      buffer[1] = 255;
+      buffer[2] = 0;
+    } else {
+    
+      buffer[0] = redx;
+      buffer[1] = greenx;
+      buffer[2] = bluex;
+    }
+ */
+    
+    int bytes = outFile.write((const uint8_t*)buffer, 3);
+//    Serial.println(bytes);
+    if (bytes < 3) 
+      Serial.println("Error writing output file");
+    
+  #ifdef DEBUG    
+    print_hex(red);
+    print_hex(green);
+    print_hex(blue);
+    
+//    delay(100);
+  #endif    
+  }
+  inFile.close();
+  outFile.close();
+}
+
+/*  
+//void writeFooter(File* dst, nmea_float_t latitude, char lat, nmea_float_t longitude, char lon, nmea_float_t altitude){    //Write 16 lines with values
+void writeFooter(File* dst, char *telemetry){
+  int x,y;
+  byte sortBuf[10560]; //320(px)*11(lines)*3(bytes) // Header buffer
+  int i,j,k;
+  int pxSkip;
+
+  char res[51]; //  = "LAT: 1234.1234N     LONG: 1234.1234W     ALT:10000";
+  
+  if (strlen(telemetry) > 50)
+    telemetry[50] = '\0';
+  strcpy(res, telemetry);
+
+  for(i = 0; i < 10560; i++){ // Cleaning Header Buffer array
+    sortBuf[i] = 0xFF;
+  }
+
+  for(i = 0; i < sizeof(res); i++){
+    byte fontNumber;
+    char ch;
+    ch = res[i];
+    for(y = 0; y < 5; y++){
+      for(x = 0; x < 4; x++){
+        //pxSkip = HORIZONTALOFFSET + VERSTICALOFFSET + (BITSPERWORD * i);
+        //pxSkip = 16 + (320 * (y + 3)) + (4 * 2 * i) + (2 * x); Width: x2
+        pxSkip = 16 + (320 * (y + 3)) + (4 * i) + x;
+
+        // If ch is pair mask is: 11110000, if no 00001111
+        uint8_t sl = (ch % 2)? 3 : 7 ;
+        uint8_t mask = pow(2, sl - x);
+
+        if(ch >= 48 && ch <=91){
+          fontNumber = (ch-48)/2;
+        }
+        else {
+          fontNumber = 22;
+        }
+
+        if((l_fonts[fontNumber][y] & mask) != 0){
+          for(j = 0; j < 3; j++){
+                  sortBuf[(3 * pxSkip) + j] = 0x00;
+          }
+        }
+      }
+    }
+  }
+
+  for(k = 0; k < 10560; k++){  // Adding header to the binary file
+    dst->write(sortBuf[k]);
+  }
+}  
+
+*/
+  
+void rotate_image(char *file_input, char *file_output, char *telemetry) {
+  
+  File input_file = SPIFFS.open(file_input, "r");           
+  
+  char pixel[3];
+  int side = (320 - 240)/2;
+  for (int y = 0; y < 240; y++) {
+    for (int x = 0; x < 320; x++) {
+      input_file.readBytes(pixel, sizeof(pixel));
+      if (( x >= side) && (x < (320 - side))) {
+        input_buffer[y][x - side][0] = pixel[0];
+        input_buffer[y][x - side][1] = pixel[1];        
+        input_buffer[y][x - side][2] = pixel[2];      
+      }
+    }
+  }
+  input_file.close();
+  
+  SPIFFS.remove(file_input);
+  
+  Serial.println("Input file read and deleted");
+  Serial.println(side);
+  
+  input_file = SPIFFS.open(file_input, "w+"); 
+  
+  //writeFooter(&input_file, telemetry); 
+  
+  char side_pixel[] = { 0xff, 0xff, 0xff };
+  for (int y = 0; y < 240; y++) {
+    Serial.println(" ");
+    for (int x = 0; x < 320; x++) {
+      if (( x >= side) && (x < (320 - side))) {
+        Serial.print("+");
+//        Serial.print(x - side);
+//        Serial.print(" ");
+        pixel[0] = input_buffer[x - side][y][0];
+        pixel[1] = input_buffer[x - side][y][1];
+        pixel[2] = input_buffer[x - side][y][2];       
+        if (input_file.write((const uint8_t*)pixel, sizeof(pixel)) < 3)
+          Serial.println("Error writing to file");
+      } else {
+        Serial.print("-");
+        if (input_file.write((const uint8_t*)side_pixel, sizeof(side_pixel)) < 3)
+          Serial.println("Error writing to file");         
+      } 
+    }
+  }
+  
+  input_file.close();
+}
diff --git a/cubesatsim/pico-get-jpeg-serial.cpp b/cubesatsim/pico-get-jpeg-serial.cpp
index 833b4719..19774cac 100644
--- a/cubesatsim/pico-get-jpeg-serial.cpp
+++ b/cubesatsim/pico-get-jpeg-serial.cpp
@@ -70,7 +70,19 @@ bool start_camera() {
 #endif
    LittleFS.begin();
  
-   return(get_camera_image());
+   bool camera_present = false;	
+   int tries = 0;
+   while ((tries++ < 5) && !camera_present) {
+    if (get_camera_image())  {
+      camera_present = true;
+      Serial.println("Camera detected!");
+    }	    
+  } 
+  if (!camera_present) {
+    Serial.println("No camera detected!");
+  }
+ 
+   return(camera_present);
 }
 
 void show_dir2() {
diff --git a/cubesatsim/pico-get-jpeg-serial/examples/pico-get-jpeg-serial.ino b/cubesatsim/pico-get-jpeg-serial/examples/pico-get-jpeg-serial.ino
deleted file mode 100644
index 925cdec4..00000000
--- a/cubesatsim/pico-get-jpeg-serial/examples/pico-get-jpeg-serial.ino
+++ /dev/null
@@ -1,228 +0,0 @@
-#include <LittleFS.h>
-#include <FastCRC.h>
-
-bool finished = false;
-
-char buffer[100001];
-int index1 = 0;
-char start_flag[] = "3d99de816e5ad7742b61a37c39141783";
-char end_flag[] = "f681a5c52351befe0e3524eb1a40f14b7803317a";
-int flag_count = 0;
-int start_flag_detected = false;
-int start_flag_complete = false;
-int end_flag_detected = false;
-int jpeg_start = 0;
-FastCRC8 CRC8;
-
-//#define DEBUG
-
-#define PICOW true
-int led_pin = LED_BUILTIN;
-
-void setup() {
- 
-  // put your setup code here, to run once:
-  
-if (PICOW)
-  led_pin = STEM_LED2_PIN;
-  
-   Serial.begin(115200);
-
-   delay(5000);
-
-   pinMode(led_pin, OUTPUT);
-
-    digitalWrite(led_pin, LOW);
-    delay(500);  
-    digitalWrite(led_pin, HIGH);  
-    delay(500);  
-    digitalWrite(led_pin, LOW);  
-    delay(500);  
-    digitalWrite(led_pin, HIGH); 
-    delay(500); 
-    if (PICOW)
-       digitalWrite(led_pin, LOW); 
-    
-    delay(2000);
-
-   Serial2.setRX(9);
-   delay(100);
-   Serial2.setTX(8);
-   delay(100);
-   Serial2.begin(115200);
-
-   Serial.println("Starting");
-
-   LittleFS.begin();
-}
-
-void show_dir() {
-  int count = 0;
-  Dir dir = LittleFS.openDir("/");
-// or Dir dir = LittleFS.openDir("/data");
-  Serial.println(">");
-  while (dir.next()) {
-    count++;
-    Serial.print(count);
-    Serial.print(" ");
-    Serial.print(dir.fileName());
-  //  if(dir.fileSize()) {
-        File f = dir.openFile("r");
-        Serial.print(" ");
-        Serial.println(f.size());
- //   } else
- //       Serial.println(" ");
-  }
-  Serial.println(">");
-}
-
-void write_jpg() {
-/*
-  Serial.println("---------------");
-  Serial.println(buffer[jpeg_start], HEX);
-  Serial.println(buffer[jpeg_start + 1], HEX);
-  Serial.println(buffer[jpeg_start + 2], HEX);
-  Serial.println(buffer[jpeg_start + 3], HEX);
-*/      
-  if ((buffer[jpeg_start] == 0xff) && (buffer[jpeg_start + 1] == 0xd8)
-       && (buffer[index1 - 2] == 0xff) && (buffer[index1 - 1] == 0xd9)) {
- 
-    Serial.println("Received a JPEG! Writing to file.");
-    File i = LittleFS.open("/cam.jpg", "w+");
-    if (i) {
-      i.write(&buffer[jpeg_start], (size_t) (index1 - jpeg_start));       
-      finished = true;
-     } else
-        Serial.println("Error opening cam.jpg");
-        
-//      Serial.println("---------------");
-      i.close();
-//    }
-  } else
-    Serial.println("Not a JPEG");
-
-    show_dir();
-/*
-    delay(2000);
-    
-    char read_values[2];
-    File i = LittleFS.open("/cam.jpg", "r");
-    while (i.available()) {
-      i.readBytes(read_values, 1);
-     char hexValue[5];
-     sprintf(hexValue, "%02X", read_values[0]);
-     Serial.print(hexValue);    
-    }
-    i.close();
-    Serial.println("\n\n finished read");  
-
- */      
-}
-
-void loop() {
-
-  char input_file[] = "/cam.jpg"; 
-  char output_file[] = "/cam.bin"; 
-  
-  get_image_file();
-
-  Serial.println("Got image from ESP-32-CAM!");
-
-  delay(1000);
-
-}
-
-
-void get_image_file()  {
-  finished = false;
-  while (!finished) {
-  // put your main code here, to run repeatedly:
-   if (Serial2.available()) {      // If anything comes in Serial2
-      byte octet = Serial2.read(); 
-
-     if (start_flag_complete) {
-//       Serial.println("Start flag complete detected");
-       buffer[index1++] = octet;
-       if (octet == end_flag[flag_count]) {  // looking for end flag
-//         if (end_flag_detected) {
-            flag_count++;
-//            Serial.println("Found part of end flag!");
-            if (flag_count >= strlen(end_flag)) {  // complete image           
-///              buffer[index1++] = octet;
-              Serial.println("\nFound end flag");
-//              Serial.println(octet, HEX);
-              while(!Serial2.available()) { }     // Wait for another byte
-//              octet = Serial2.read(); 
-//              buffer[index1++] = octet;
-//              Serial.println(octet, HEX);
-//              while(!Serial2.available()) { }     // Wait for another byte
-              int received_crc = Serial2.read(); 
-//              buffer[index1++] = octet;
-                            
-              Serial.print("\nFile length: ");
-              Serial.println(index1 - (int)strlen(end_flag));
-//              index1 -= 1; // 40;
-//              Serial.println(buffer[index1 - 1], HEX); 
-//              int received_crc = buffer[index1];
-//              index1 -= 1;
-
-              uint8_t * data = (uint8_t *) &buffer[0];
-#ifdef DEBUG
-      Serial.println("\nCRC cacluation data:");
-      Serial.println(buffer[0], HEX);
-      Serial.println(buffer[index1 - 1], HEX);
-      Serial.println(index1);
-      Serial.println(received_crc, HEX);     
- #endif             
-              int calculated_crc = CRC8.smbus(data, index1);
- //             Serial.println(calculated_crc, HEX);
-              if (received_crc == calculated_crc)
-                Serial.println("CRC check succeeded!");
-              else  
-               Serial.println("CRC check failed!"); 
-
-              index1 -= 40;                         
-              write_jpg();
-              index1 = 0;           
-              start_flag_complete = false;
-              start_flag_detected = false; // get ready for next image 
-              end_flag_detected = false;
-              flag_count = 0; 
-//              delay(6000);
-            }
-         } else {
-           flag_count = 0;
-         }
- ///        buffer[index1++] = octet;
-           
-#ifdef DEBUG    
-           char hexValue[5];
-           if (octet != 0x66) {
-             sprintf(hexValue, "%02X", octet);
-             Serial.print(hexValue);
-           } else {
-//             Serial.println("\n********************************************* Got a 66!");
-             Serial.print("66");
-           } 
-//             Serial.write(octet);
-#endif             
-           if (index1 > 100000)
-             index1 = 0; 
-//         }
-    } else if (octet == start_flag[flag_count]) {  // looking for start flag
-          start_flag_detected = true;
-          flag_count++;
-//          Serial.println("Found part of start flag! ");
-          if (flag_count >= strlen(start_flag)) {
-            flag_count = 0;
-            start_flag_complete = true;
-            Serial.println("Found start flag!\n");        
-          }
-      } else {  // not the flag, keep looking
-          start_flag_detected = false;
-          flag_count = 0;
-     //   Serial.println("Resetting. Not start flag.");        
-       } 
-    }
-  }
-}
diff --git a/cubesatsim/pico-get-jpeg-serial/pico-get-jpeg-serial.cpp b/cubesatsim/pico-get-jpeg-serial/pico-get-jpeg-serial.cpp
deleted file mode 100644
index 12d83125..00000000
--- a/cubesatsim/pico-get-jpeg-serial/pico-get-jpeg-serial.cpp
+++ /dev/null
@@ -1,212 +0,0 @@
-#include "pico-get-jpeg-serial.h"
-
-/*
-void setup() {
- 
-  // put your setup code here, to run once:
-  
-if (PICOW)
-  led_pin = STEM_LED2_PIN;
-  
-   Serial.begin(115200);
-
-   delay(5000);
-
-   pinMode(led_pin, OUTPUT);
-
-    digitalWrite(led_pin, LOW);
-    delay(500);  
-    digitalWrite(led_pin, HIGH);  
-    delay(500);  
-    digitalWrite(led_pin, LOW);  
-    delay(500);  
-    digitalWrite(led_pin, HIGH); 
-    delay(500); 
-    if (PICOW)
-       digitalWrite(led_pin, LOW); 
-    
-    delay(2000);
-
-   Serial2.setRX(9);
-   delay(100);
-   Serial2.setTX(8);
-   delay(100);
-   Serial2.begin(115200);
-
-   Serial.println("Starting");
-
-   LittleFS.begin();
-}
-
-void show_dir() {
-  int count = 0;
-  Dir dir = LittleFS.openDir("/");
-// or Dir dir = LittleFS.openDir("/data");
-  Serial.println(">");
-  while (dir.next()) {
-    count++;
-    Serial.print(count);
-    Serial.print(" ");
-    Serial.print(dir.fileName());
-  //  if(dir.fileSize()) {
-        File f = dir.openFile("r");
-        Serial.print(" ");
-        Serial.println(f.size());
- //   } else
- //       Serial.println(" ");
-  }
-  Serial.println(">");
-}
-*/
-
-void write_jpg() {
-/*
-  Serial.println("---------------");
-  Serial.println(buffer[jpeg_start], HEX);
-  Serial.println(buffer[jpeg_start + 1], HEX);
-  Serial.println(buffer[jpeg_start + 2], HEX);
-  Serial.println(buffer[jpeg_start + 3], HEX);
-*/      
-  if ((buffer[jpeg_start] == 0xff) && (buffer[jpeg_start + 1] == 0xd8)
-       && (buffer[index1 - 2] == 0xff) && (buffer[index1 - 1] == 0xd9)) {
- 
-    Serial.println("Received a JPEG! Writing to file.");
-    File i = LittleFS.open("/cam.jpg", "w+");
-    if (i) {
-      i.write(&buffer[jpeg_start], (size_t) (index1 - jpeg_start));       
-      finished = true;
-     } else
-        Serial.println("Error opening cam.jpg");
-        
-//      Serial.println("---------------");
-      i.close();
-//    }
-  } else
-    Serial.println("Not a JPEG");
-
-    show_dir();
-/*
-    delay(2000);
-    
-    char read_values[2];
-    File i = LittleFS.open("/cam.jpg", "r");
-    while (i.available()) {
-      i.readBytes(read_values, 1);
-     char hexValue[5];
-     sprintf(hexValue, "%02X", read_values[0]);
-     Serial.print(hexValue);    
-    }
-    i.close();
-    Serial.println("\n\n finished read");  
-
- */      
-}
-
-/*
-void loop() {
-
-  char input_file[] = "/cam.jpg"; 
-  char output_file[] = "/cam.bin"; 
-  
-  get_image_file();
-
-  Serial.println("Got image from ESP-32-CAM!");
-
-  delay(1000);
-
-}
-*/
-
-void get_image_file()  {
-  finished = false;
-  while (!finished) {
-  // put your main code here, to run repeatedly:
-   if (Serial2.available()) {      // If anything comes in Serial2
-      byte octet = Serial2.read(); 
-
-     if (start_flag_complete) {
-//       Serial.println("Start flag complete detected");
-       buffer[index1++] = octet;
-       if (octet == end_flag[flag_count]) {  // looking for end flag
-//         if (end_flag_detected) {
-            flag_count++;
-//            Serial.println("Found part of end flag!");
-            if (flag_count >= strlen(end_flag)) {  // complete image           
-///              buffer[index1++] = octet;
-              Serial.println("\nFound end flag");
-//              Serial.println(octet, HEX);
-              while(!Serial2.available()) { }     // Wait for another byte
-//              octet = Serial2.read(); 
-//              buffer[index1++] = octet;
-//              Serial.println(octet, HEX);
-//              while(!Serial2.available()) { }     // Wait for another byte
-              int received_crc = Serial2.read(); 
-//              buffer[index1++] = octet;
-                            
-              Serial.print("\nFile length: ");
-              Serial.println(index1 - (int)strlen(end_flag));
-//              index1 -= 1; // 40;
-//              Serial.println(buffer[index1 - 1], HEX); 
-//              int received_crc = buffer[index1];
-//              index1 -= 1;
-
-              uint8_t * data = (uint8_t *) &buffer[0];
-#ifdef DEBUG
-      Serial.println("\nCRC cacluation data:");
-      Serial.println(buffer[0], HEX);
-      Serial.println(buffer[index1 - 1], HEX);
-      Serial.println(index1);
-      Serial.println(received_crc, HEX);     
- #endif             
-              int calculated_crc = CRC8.smbus(data, index1);
- //             Serial.println(calculated_crc, HEX);
-              if (received_crc == calculated_crc)
-                Serial.println("CRC check succeeded!");
-              else  
-               Serial.println("CRC check failed!"); 
-
-              index1 -= 40;                         
-              write_jpg();
-              index1 = 0;           
-              start_flag_complete = false;
-              start_flag_detected = false; // get ready for next image 
-              end_flag_detected = false;
-              flag_count = 0; 
-//              delay(6000);
-            }
-         } else {
-           flag_count = 0;
-         }
- ///        buffer[index1++] = octet;
-           
-#ifdef DEBUG    
-           char hexValue[5];
-           if (octet != 0x66) {
-             sprintf(hexValue, "%02X", octet);
-             Serial.print(hexValue);
-           } else {
-//             Serial.println("\n********************************************* Got a 66!");
-             Serial.print("66");
-           } 
-//             Serial.write(octet);
-#endif             
-           if (index1 > 100000)
-             index1 = 0; 
-//         }
-    } else if (octet == start_flag[flag_count]) {  // looking for start flag
-          start_flag_detected = true;
-          flag_count++;
-//          Serial.println("Found part of start flag! ");
-          if (flag_count >= strlen(start_flag)) {
-            flag_count = 0;
-            start_flag_complete = true;
-            Serial.println("Found start flag!\n");        
-          }
-      } else {  // not the flag, keep looking
-          start_flag_detected = false;
-          flag_count = 0;
-     //   Serial.println("Resetting. Not start flag.");        
-       } 
-    }
-  }
-}
diff --git a/cubesatsim/pico-get-jpeg-serial/pico-get-jpeg-serial.h b/cubesatsim/pico-get-jpeg-serial/pico-get-jpeg-serial.h
deleted file mode 100644
index c8363370..00000000
--- a/cubesatsim/pico-get-jpeg-serial/pico-get-jpeg-serial.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#include <LittleFS.h>
-#include <FastCRC.h>
-
-bool finished = false;
-
-//char buffer[100001];
-int index1 = 0;
-char start_flag[] = "3d99de816e5ad7742b61a37c39141783";
-char end_flag[] = "f681a5c52351befe0e3524eb1a40f14b7803317a";
-int flag_count = 0;
-int start_flag_detected = false;
-int start_flag_complete = false;
-int end_flag_detected = false;
-int jpeg_start = 0;
-FastCRC8 CRC8;
-
-//#define DEBUG
-
-//#define PICOW true
-int led_pin = LED_BUILTIN;
-
-void get_image_file();