diff --git a/Makefile b/Makefile
index bf295216..a2c69fcb 100644
--- a/Makefile
+++ b/Makefile
@@ -41,9 +41,10 @@ cubesatsim: libax5043.a
 cubesatsim: afsk/ax25.o
 cubesatsim: afsk/ax5043.o
 cubesatsim: TelemEncoding.o
+cubesatsim: sensor_extension.o
 cubesatsim: main.o
 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
+	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -o cubesatsim -Wall -Wextra -L./ afsk/ax25.o afsk/ax5043.o TelemEncoding.o sensor_extension.o codecAO40.o main.o -lwiringPi -lax5043 -lm
 
 telem: telem.o
 	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -o telem -Wall -Wextra -L./ telem.o -lwiringPi 
@@ -52,6 +53,9 @@ TelemEncoding.o: TelemEncoding.c
 TelemEncoding.o: TelemEncoding.h
 	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -Wall -Wextra -c TelemEncoding.c
 
+sensor_extension.o: sensor_extension.c
+	gcc -std=gnu99 $(DEBUG_BEHAVIOR) -Wall -Wextra -c sensor_extension.c
+
 codecAO40.o: codecAO40.c
 codecAO40.o: codecAO40.h
 	gcc -std=gnu99  $(DEBUG_BEHAVIOR) -Wall -Wextra -c codecAO40.c
diff --git a/config b/config
index 47e1e809..092c9d55 100755
--- a/config
+++ b/config
@@ -54,7 +54,7 @@ function transmit_command_fsk {
 	 	echo "Resuming command and control"
 		sudo systemctl start command
 
-	else
+	
 
 		STRING="AMSAT-11>APCSS:=3901.40N\07704.39WShi hi MODE=f"
 		sudo rm /home/pi/CubeSatSim/t.txt > /dev/null 2>&1
@@ -89,7 +89,7 @@ function transmit_command_bpsk {
 	 	echo "Resuming command and control"
 		sudo systemctl start command
 
-	else
+	
 
 		STRING="AMSAT-11>APCSS:=3901.40N\07704.39WShi hi MODE=b"
 		sudo rm /home/pi/CubeSatSim/t.txt > /dev/null 2>&1
@@ -210,7 +210,8 @@ function transmit_command_beacon {
 	exit
 }
 
-echo "CubeSatSim v2.1 configuration tool"
+echo
+echo "CubeSatSim v2.2 configuration tool"
 echo
 # echo $1
 # echo $2
@@ -222,8 +223,13 @@ sudo modprobe snd-aloop
 if [ -z "$2" ] ; then
 	noreboot=0
 else
-	noreboot=1
-	echo "Reboot disabled"
+	if [ "$2" = "n" ] ; then
+		echo "Reboot disabled"
+		noreboot=1
+	else
+		fail=$2
+	fi
+
 fi
 
 # echo "No reboot"
@@ -274,7 +280,67 @@ if [ "$1" = "" ]; then
 #		sim="no"
 		echo "Simulated Telemetry is OFF"
 	fi
-	echo
+#	echo
+
+	FILE=/home/pi/CubeSatSim/failure_mode.txt
+	if [ -f "$FILE" ]; then
+		if [[ $(grep "\-1" $FILE) ]]; then
+			echo "No simulated failure"
+		else
+			fail=$(<$FILE)
+			echo -n "Simulated "
+#			cat $FILE
+
+			case $fail in
+
+				1)
+					echo "+Y Solar Panel Unplugged (1)"
+					;;	
+				2)
+					echo "+X Solar Panel Failure (2)"
+					;;
+				3)
+					echo "-X Solar Panel Degredation (3)"
+					;;
+				4)
+					echo "-Y Solar Panel Short Circuit (4)"
+					;;
+				5)
+					echo "Failed I2C Bus 1 (5)"
+					;;
+				6)
+					echo "Failed I2C Bus 3 (6)"
+					;;
+				7)
+					echo "Failed Camera (7)"
+					;;
+				8)
+					echo "Failed Payload (8)"
+					;;
+				9)
+					echo "Failed BME Sensor (9)"
+					;;
+				10)
+					echo "Failed MPU Sensor (10)"
+					;;
+				"11")
+					echo "Failed FM Audio (11)"
+					;;
+				*)
+					echo "Unknown Failure"
+					;;
+			esac
+#			echo $fail
+		fi
+	else
+		echo "No simulated failure"
+	fi
+
+	if [ "${12}" = "y" ] || [ "${12}" = "yes" ] ; then
+		echo "Random Failure Mode is ON with time period" ${13} "seconds"
+	else
+		echo "Random Failure Mode is OFF"
+	fi
 
 	if [ "$9" = "yes" ] || [ "$9" = "y" ]; then
 		echo "Balloon mode is ON"
@@ -282,7 +348,7 @@ if [ "$1" = "" ]; then
 		echo "Balloon mode is OFF"
 	fi
 
-	echo
+#	echo
 	echo -n "Current command count is: "
 	cat /home/pi/CubeSatSim/command_count.txt
 	echo
@@ -293,7 +359,7 @@ if [ "$1" = "" ]; then
 
 	echo -n "Squelch level is: "
 	echo $6
-	echo
+#	echo
 
 	FILE=/home/pi/CubeSatSim/command_control
 	if [ -f "$FILE" ]; then
@@ -312,14 +378,14 @@ if [ "$1" = "" ]; then
     		echo "Radio command and control is OFF"
 	fi
 
-	echo
+#	echo
 
 	echo -n "RX PL code is: "
 	echo -n ${10}
 #	echo
 	echo -n " TX PL code is: "
 	echo ${11}
-	echo
+#	echo
 
 	FILE=/home/pi/CubeSatSim/battery_saver
 	if [ -f "$FILE" ]; then
@@ -328,7 +394,7 @@ if [ "$1" = "" ]; then
     		echo "Battery saver mode is OFF"
 	fi
 
-	echo
+#	echo
 	FILE=/home/pi/CubeSatSim/beacon_off
 	if [ -f "$FILE" ]; then
     		echo "Transmit beacon telemetry is OFF"
@@ -340,7 +406,7 @@ if [ "$1" = "" ]; then
 	echo -e "Current sim.cfg configuration file:"	
 #	echo
 	
-	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
 	echo
 	
 	echo "To change, include an OPTION"
@@ -486,7 +552,7 @@ elif [ "$1" = "-t" ]; then
 	
 	echo
 	
-#	$1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} 
+#	$1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${12} ${13} 
 
 	echo "Do you want Simulated Telemetry ON (y/n) "
 	read sim
@@ -498,13 +564,14 @@ elif [ "$1" = "-t" ]; then
 	else
 		sim="no"
 		echo "Simulated Telemetry is OFF"
+		echo "-1" > /home/pi/CubeSatSim/failure_mode.txt  # make sure to turn off any simulated failures
 	fi
 	
 #	echo
 	echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 #	echo
-	echo $1 $2 $3 $4 $sim $6 $7 $8 $9 ${10} ${11}
-	echo $1 $2 $3 $4 $sim $6 $7 $8 $9 ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+	echo $1 $2 $3 $4 $sim $6 $7 $8 $9 ${10} ${11} ${12} ${13}
+	echo $1 $2 $3 $4 $sim $6 $7 $8 $9 ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	echo
 ##	echo "Rebooting CubeSatSim with new configuration file"
 ##	echo
@@ -531,7 +598,7 @@ elif [ "$1" = "-c" ]; then
 	echo $1
 	echo
 	
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} 
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${12} ${13}
 
 	echo "Enter callsign in all capitals: "
 	read callsign
@@ -545,8 +612,8 @@ elif [ "$1" = "-c" ]; then
 	
 		echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 
-		echo $callsign $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
-		echo $callsign $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+		echo $callsign $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
+		echo $callsign $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	fi
 	
 	if [ "$norestart" = "1" ]; then
@@ -579,7 +646,7 @@ elif [ "$1" = "-r" ]; then
 	echo $2
 	echo
 	
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} 
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13} 
 
 	echo -e "Enter Reset Count (integer): "
 
@@ -599,8 +666,8 @@ elif [ "$1" = "-r" ]; then
 
 		echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 
-		echo $1 $resets $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
-		echo $1 $resets $3 $4 $5 $6 $7 $8 $9 ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+		echo $1 $resets $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
+		echo $1 $resets $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	fi	
 
 	if [ "$norestart" = "1" ]; then
@@ -634,7 +701,7 @@ elif [ "$1" = "-l" ]; then
 	echo $3
 	echo
 	
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} 
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
 
 	echo -e "Enter latitude  (decimal degrees, positive is north): "
 
@@ -676,8 +743,8 @@ elif [ "$1" = "-l" ]; then
 	fi 
 
 	echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
-	echo $1 $2 $lat $long $5 $6 $7 $8 $9 ${10} ${11}
-	echo $1 $2 $lat $long $5 $6 $7 $8 $9 ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+	echo $1 $2 $lat $long $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
+	echo $1 $2 $lat $long $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	
 	if [ "$norestart" = "1" ]; then
 		echo 
@@ -969,7 +1036,7 @@ elif [ "$1" = "-q" ]; then
 	echo $6
 	echo
 	
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
 
 	echo -e "Enter squelch  (integer 1 - 8): "
 
@@ -991,8 +1058,8 @@ elif [ "$1" = "-q" ]; then
 #	echo
 	echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 #	echo
-	echo $1 $2 $3 $4 $5 $sq $7 $8 $9 ${10} ${11}
-	echo $1 $2 $3 $4 $4 $sq $7 $8 $9 ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+	echo $1 $2 $3 $4 $5 $sq $7 $8 $9 ${10} ${11} ${12} ${13}
+	echo $1 $2 $3 $4 $4 $sq $7 $8 $9 ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	echo
 	echo "Restarting CubeSatSim with new configuration file"
 ##	echo
@@ -1014,7 +1081,7 @@ elif [ "$1" = "-P" ]; then
 
 	echo	
 	echo "Editing the PL (Private Line) CTCSS/CDCSS setting in"
-	echo  "the configuration file for CubeSatSim"	
+	echo "the configuration file for CubeSatSim"	
 	echo
 	
 	value=`cat /home/pi/CubeSatSim/sim.cfg`
@@ -1028,7 +1095,7 @@ elif [ "$1" = "-P" ]; then
 	echo ${11}
 	echo
 	
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
 
 	echo -e "Enter RX PL value integer 0: None, 01-38： CTCSS （analog, 39-121：CDCSS （digital）"
 
@@ -1071,8 +1138,8 @@ elif [ "$1" = "-P" ]; then
 #	echo
 	echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 #	echo
-	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 $rxpl $txpl
-	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 $rxpl $txpl > /home/pi/CubeSatSim/sim.cfg
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 $rxpl $txpl ${12} ${13}
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 $rxpl $txpl ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	echo
 ##	echo "Rebooting CubeSatSim with new configuration file"
 ##	echo
@@ -1102,7 +1169,7 @@ elif [ "$1" = "-F" ]; then
 	echo $8
 	echo	
 
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
 
 	echo "Enter tx frequency as 4XX.XXXX: "
 	read tx
@@ -1125,8 +1192,8 @@ elif [ "$1" = "-F" ]; then
 	
 		echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 
-		echo $1 $2 $3 $4 $5 $6 $tx $rx $9 ${10} ${11}
-		echo $1 $2 $3 $4 $5 $6 $tx $rx $9 ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+		echo $1 $2 $3 $4 $5 $6 $tx $rx $9 ${10} ${11} ${12} ${13}
+		echo $1 $2 $3 $4 $5 $6 $tx $rx $9 ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 #	fi
 	
 #	if [ "$norestart" = "1" ]; then
@@ -1208,7 +1275,7 @@ elif [ "$1" = "-H" ]; then
 	
 	echo
 	
-#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11}
+#	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13}
 
 	echo "Do you want Balloon mode ON (y/n) "
 	read hab
@@ -1225,8 +1292,8 @@ elif [ "$1" = "-H" ]; then
 #	echo
 	echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
 #	echo
-	echo $1 $2 $3 $4 $5 $6 $7 $8 $hab ${10} ${11}
-	echo $1 $2 $3 $4 $5 $6 $7 $8 $hab ${10} ${11} > /home/pi/CubeSatSim/sim.cfg
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $hab ${10} ${11} ${12} ${13}
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $hab ${10} ${11} ${12} ${13} > /home/pi/CubeSatSim/sim.cfg
 	echo
 ##	echo "Rebooting CubeSatSim with new configuration file"
 ##	echo
@@ -1430,6 +1497,281 @@ elif [ "$1" = "-j" ]; then
 		restart=1
 #	fi
 
+elif [ "$1" = "-M" ]; then
+
+	if [ $fail ]; then
+		MODE=$fail
+	else
+
+		FILE=/home/pi/CubeSatSim/failure_mode.txt
+		if [ -f "$FILE" ]; then
+			if [[ $(grep "\-1" $FILE) ]]; then
+				echo "Currently, no simulated failure (0)"
+			else
+				fail=$(<$FILE)
+				echo -n "Currently, simulated "
+				case $fail in
+
+				1)
+					echo "+Y Solar Panel Unplugged (1)"
+					;;			
+				2)
+					echo "+X Solar Panel Failure (2)"
+					;;
+				3)
+					echo "-X Solar Panel Degredation (3)"
+					;;
+				4)
+					echo "-Y Solar Panel Short Circuit (4)"
+					;;
+				5)
+					echo "Failed I2C Bus 1 (5)"
+					;;
+				6)
+					echo "Failed I2C Bus 3 (6)"
+					;;
+				7)
+					echo "Failed Camera (7)"
+					;;
+				8)
+					echo "Failed Payload (8)"
+					;;
+				9)
+					echo "Failed BME Sensor (9)"
+					;;
+				"10")
+					echo "Failed MPU Sensor (10)"
+					;;
+				"11")
+					echo "Failed FM Audio (11)"
+					;;
+				*)		
+					echo "Unknown Failure"
+					;;
+				esac
+			fi
+		else
+			echo "Currently, no simulated failure"
+		fi
+	
+		echo
+		echo "Set simulated failure mode (or Return to turn OFF)"
+		echo
+	
+		echo " 0  No Failure (turn OFF)"
+		echo " 1  +Y Solar Panel Unplugged"
+		echo " 2  +X Solar Panel Failure"
+		echo " 3  -X Solar Panel Degredation"
+		echo " 4  -Y Solar Panel Short Circuit"
+		echo " 5  Failed I2C Bus 1"
+		echo " 6  Failed I2C Bus 3"
+		echo " 7  Failed Camera"
+		echo " 8  Failed Payload"
+		echo " 9  Failed BME Sensor"
+		echo "10  Failed MPU Sensor"
+		echo "11  Failed FM Audio"
+		echo
+	
+		echo "Enter the failure number to set: 0 - 11" 
+		read MODE
+		echo
+	fi
+
+	if [ "$MODE" = "0" ]; then
+		echo "Setting No Simulated Failure"
+		MODE=-1
+#    elif [ "$MODE" = "12" ]; then
+	
+#		if [ "$norestart" = "1" ]; then
+#			echo
+#		else
+#			reboot=1
+#		fi
+
+	else	
+		case $MODE in
+			1)
+				echo "+Y Solar Panel Unplugged"
+				;;			
+			2)
+				echo "Setting Simulated +X Solar Panel Failure"
+				;;
+			3)
+				echo "Setting Simulated -X Solar Panel Degredation"
+				;;
+			4)
+				echo "Setting Simulated -Y Solar Panel Short Circuit"
+				;;
+			5)
+				echo "Setting Simulated Failed I2C Bus 1"
+				;;
+			6)
+				echo "Setting Simulated Failed I2C Bus 3"
+				;;
+			7)
+				echo "Setting Simulated Failed Camera"
+				;;
+			8)
+				echo "Setting Simulated Failed Payload"
+				;;
+			9)
+				echo "Setting Simulated Failed BME Sensor"
+				;;
+			"10")
+				echo "Setting Simulated Failed MPU Sensor"
+				;;
+			"11")
+				echo "Setting Failed FM Audio"
+				;;
+			*)
+				echo "Setting No Simulated Failure"
+				MODE=-1
+				;;
+		esac
+	fi
+
+#	echo $MODE
+	echo
+	echo $MODE > /home/pi/CubeSatSim/failure_mode.txt
+
+	echo "Changing simulated failure mode to $MODE" | wall
+
+
+
+elif [ "$1" = "-N" ]; then
+
+	FILE=/home/pi/CubeSatSim/failure_mode.txt
+	if [ -f "$FILE" ]; then
+		if [[ $(grep "\-1" $FILE) ]]; then
+			echo "No simulated failure"
+			fail=0
+		else	
+			echo "Simulated failure mode"
+			fail=$(<$FILE)
+			echo $fail
+		fi
+	else
+		echo "No simulated failure"
+		fail=0
+	fi
+
+	if [ $fail == 0 ]; then
+		echo "Changing to next mode"
+	
+		value=`cat /home/pi/CubeSatSim/.mode`
+		echo "$value" > /dev/null
+		set -- $value
+	
+		if [ "$1" = "a" ]; then
+			echo "Current mode is APRS"
+			echo "Next mode is FSK"
+			/home/pi/CubeSatSim/config -f
+		elif [ "$1" = "m" ]; then
+			echo "Current mode is CW"
+			echo "Next mode is FunCube"
+			/home/pi/CubeSatSim/config -j
+		elif [ "$1" = "f" ]; then
+			echo "Current mode is FSK"
+			echo "Next mode is BPSK"
+			/home/pi/CubeSatSim/config -b
+		elif [ "$1" = "b" ]; then
+			echo "Current mode is BPSK"	
+			echo "Next mode is SSTV"
+			/home/pi/CubeSatSim/config -s
+		elif [ "$1" = "s" ]; then
+			echo "Current mode is SSTV"
+			echo "Next mode is CW"
+			/home/pi/CubeSatSim/config -m
+		elif [ "$1" = "e" ]; then
+			echo "Current mode is Repeater"	
+			echo "Next mode is APRS"
+			/home/pi/CubeSatSim/config -a
+		elif [ "$1" = "j" ]; then
+			echo "Current mode is FUNcube"	
+			echo "Next mode is Repeater"
+			/home/pi/CubeSatSim/config -e
+		else
+			echo "Unknown mode"
+		fi
+	else
+		echo "Changing to next simulated failure mode"
+		fail=$((fail + 1))
+#		if [ $fail == 12 ]; then
+		if [ "$fail" -gt "11" ]; then
+			fail=1
+		fi
+		echo $fail
+		/home/pi/CubeSatSim/config -M $fail
+
+	fi
+
+elif [ "$1" = "-U" ]; then
+
+	echo	
+	echo "Changing the Random Simulated Failure setting in"
+	echo "the configuration file for CubeSatSim"	
+	echo
+	
+	value=`cat /home/pi/CubeSatSim/sim.cfg`
+	echo "$value" > /dev/null
+	set -- $value
+
+	if [ "${12}" = "yes" ] || [ "${12}" = "y" ]; then
+		echo "Random Simulated Failure is ON"
+	else
+		echo "Random Simulated Failure is OFF"
+	fi
+	
+	echo
+	
+#	$1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${12} ${13} 
+
+	echo "Do you want Random Simulated Failure ON (y/n) "
+	read sim
+	echo
+	
+	if [ "$sim" = "y" ] || [ "$sim" = "yes" ]  ; then
+		sim="yes"
+		echo "Random Simulated Failure is ON"
+		echo	
+		echo "A new random failure is selected every"
+		echo  ${13} "seconds."	
+		echo 
+		echo "Enter a new value or Return keeps current value."
+			
+		echo "Enter time in seconds (integer): "
+	
+		read time
+	
+		if [ -z $time ] ; then
+			time="${13}"
+			echo "Keeping value of " $time " seconds"
+		fi
+	
+		if ! [[ $time =~ ^[0-9]+$ ]] ; then
+			echo "Error: not an integer!"
+			time="${13}"
+			echo "Keeping value of" $time
+		fi
+
+	else
+		sim="no"
+		echo "Random Simulated Failure is OFF"
+		time="${13}"
+#		echo "-1" > /home/pi/CubeSatSim/failure_mode.txt  # make sure to turn off any simulated failures
+	fi
+	
+#	echo
+	echo -e "\nCubeSatSim configuration sim.cfg file updated to: \n"
+#	echo
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} $sim $time
+	echo $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} $sim $time > /home/pi/CubeSatSim/sim.cfg
+	echo
+
+	if [ "${12}" != "$sim" ] || [ "${13}" != "$time" ] ; then
+		reboot=1
+	fi
+	
 elif [ "$1" = "-h" ]; then
 
 	echo "config OPTION"
@@ -1468,7 +1810,9 @@ elif [ "$1" = "-h" ]; then
 	echo "   -o     Change telemetry beacon transmit state"
 	echo "   -L     Change microphone level for command and control"
 	echo "   -g     Reset configuration back to default settings"
-
+	echo "   -M     Set simulated failure mode"
+	echo "   -U     Change the random failure mode setting"
+	echo "   -N     Set next mode or failure"
 	echo
 	exit
 
diff --git a/install b/install
index 76b2c3da..6bf50f0f 100755
--- a/install
+++ b/install
@@ -1,6 +1,17 @@
 #!/bin/bash
 
-echo -e "\ninstallation script for CubeSatSim v2.1\n"
+echo -e "\ninstallation script for CubeSatSim v2.2\n"
+
+ if [[ $(grep '11.' /etc/debian_version) ]]; then
+    echo "Installing on Debian 11 (Bullseye)"
+  else
+    echo "The OS is not Debian 11 (Bullseye)!"
+	echo "Installation is not likely to work."
+	echo "It is recommended you start with"
+ 	echo "Bullseye as your OS."
+	echo
+	sleep 10
+  fi
 
 FILE=/home/pi/CubeSatSim/sim.cfg
 if [ -f "$FILE" ]; then
@@ -112,7 +123,7 @@ 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
+unzip -u fctelem.zip
 
 cd
 echo "Installing fcdctl to set FUNcubeDongle Pro gain"
diff --git a/main.c b/main.c
index f5115e60..107c1593 100644
--- a/main.c
+++ b/main.c
@@ -25,10 +25,11 @@
 
 int main(int argc, char * argv[]) {
 	
-  printf("\n\nCubeSatSim v2.1 starting...\n\n");
+  printf("\n\nCubeSatSim v2.2 starting...\n\n");
 
   wiringPiSetup();	
-		
+
+  strcpy(fail_yes, "no");		
   // Open configuration file with callsign and reset count	
   FILE * config_file = fopen("/home/pi/CubeSatSim/sim.cfg", "r");
   if (config_file == NULL) {
@@ -41,11 +42,11 @@ int main(int argc, char * argv[]) {
 
 //  char * cfg_buf[100];
 
-  fscanf(config_file, "%s %d %f %f %s %d %s %s %s %d %d", 
-	  call, & reset_count, & lat_file, & long_file, sim_yes, & squelch, tx, rx, hab_yes, & rx_pl, & tx_pl);
+  fscanf(config_file, "%s %d %f %f %s %d %s %s %s %d %d %s %d", 
+	  call, &reset_count, &lat_file, &long_file, sim_yes, &squelch, tx, rx, hab_yes, &rx_pl, &tx_pl, fail_yes, &fail_time);
   fclose(config_file);
-  fprintf(stderr,"Config file /home/pi/CubeSatSim/sim.cfg contains %s %d %f %f %s %d %s %s %s %d %d\n", 
-	  call, reset_count, lat_file, long_file, sim_yes, squelch, tx, rx, hab_yes, rx_pl, tx_pl);
+  fprintf(stderr,"Config file /home/pi/CubeSatSim/sim.cfg contains %s %d %f %f %s %d %s %s %s %d %d %s %d\n", 
+	  call, reset_count, lat_file, long_file, sim_yes, squelch, tx, rx, hab_yes, rx_pl, tx_pl, fail_yes, fail_time);
 
   fprintf(stderr, "Transmit on %s MHz Receive on %s MHz\n", tx, rx);
 
@@ -86,6 +87,10 @@ int main(int argc, char * argv[]) {
 	  hab_mode = TRUE;
 	  fprintf(stderr, "HAB mode is ON\n");
   }	
+  if (strcmp(fail_yes, "yes") == 0) {
+	  fail_rnd_mode = TRUE;
+	  fprintf(stderr, "Random fail mode is ON\n");
+  }		
 	
   FILE * command_file = fopen("/home/pi/CubeSatSim/command_control", "r");
   if (command_file == NULL) {	  	
@@ -272,47 +277,10 @@ int main(int argc, char * argv[]) {
 /**/	
 
   fflush(stderr);
-  
-  if (mode == AFSK)
-  {
-  // Check for SPI and AX-5043 Digital Transceiver Board	
-    FILE * file = popen("sudo raspi-config nonint get_spi", "r");
-//  printf("getc: %c \n", fgetc(file));
-    if (fgetc(file) == 48) {
-      printf("SPI is enabled!\n");
-
-      FILE * file2 = popen("ls /dev/spidev0.* 2>&1", "r");
-              printf("Result getc: %c \n", getc(file2));
-
-      if (fgetc(file2) != 'l') {
-        printf("SPI devices present!\n");
-      //	  }
-
-        setSpiChannel(SPI_CHANNEL);
-        setSpiSpeed(SPI_SPEED);
-        initializeSpi();
-        ax25_init( & hax25, (uint8_t * ) dest_addr, 11, (uint8_t * ) call, 11, AX25_PREAMBLE_LEN, AX25_POSTAMBLE_LEN);
-        if (init_rf()) {
-          printf("AX5043 successfully initialized!\n");
-          ax5043 = TRUE;
-          cw_id = OFF;
-//        mode = AFSK;
-        //		cycle = OFF;
-          printf("Mode AFSK with AX5043\n");
-          transmit = TRUE;
-//	sleep(10);  // just in case CW ID is sent      
-        } else
-          printf("AX5043 not present!\n");
-          pclose(file2);	    
-      }
-    }
-    pclose(file);
-  }	
-
+	
   txLed = 2;
   txLedOn = HIGH;
   txLedOff = LOW;
-  vB5 = TRUE;
   onLed = 27;
   onLedOn = HIGH;
   onLedOff = LOW;
@@ -326,34 +294,23 @@ int main(int argc, char * argv[]) {
   }
 
   config_file = fopen("sim.cfg", "w");
-  fprintf(config_file, "%s %d %8.4f %8.4f %s %d %s %s %s %d %d", call, reset_count, lat_file, long_file, sim_yes, squelch, tx, rx, hab_yes, rx_pl, tx_pl);
+  fprintf(config_file, "%s %d %8.4f %8.4f %s %d %s %s %s %d %d %s %d", 
+	  call, reset_count, lat_file, long_file, sim_yes, squelch, tx, rx, hab_yes, rx_pl, tx_pl, fail_yes, fail_time);
   //    fprintf(config_file, "%s %d", call, reset_count);
   fclose(config_file);
   config_file = fopen("sim.cfg", "r");
 
-  if (vB4) {
-    map[BAT] = BAT2;
-    map[BAT2] = BAT;
-    snprintf(busStr, 10, "%d %d", i2c_bus1, test_i2c_bus(0));
-  } else if (vB5) {
-    map[MINUS_X] = MINUS_Y;
-    map[PLUS_Z] = MINUS_X;	
-    map[MINUS_Y] = PLUS_Z;		  
+  map[MINUS_X] = MINUS_Y;
+  map[PLUS_Z] = MINUS_X;	
+  map[MINUS_Y] = PLUS_Z;		  
 
-    if (access("/dev/i2c-11", W_OK | R_OK) >= 0) { // Test if I2C Bus 11 is present			
+  if (access("/dev/i2c-11", W_OK | R_OK) >= 0) { // Test if I2C Bus 11 is present			
       printf("/dev/i2c-11 is present\n\n");
       snprintf(busStr, 10, "%d %d", test_i2c_bus(1), test_i2c_bus(11));
-    } else {
-      snprintf(busStr, 10, "%d %d", i2c_bus1, i2c_bus3);
-    }
   } else {
-    map[BAT2] = MINUS_Z;
-    map[BAT] = BAT2;
-    map[PLUS_Z] = BAT;
-    map[MINUS_Z] = PLUS_Z;
-    snprintf(busStr, 10, "%d %d", i2c_bus1, test_i2c_bus(0));
-    voltageThreshold = 8.0;
+      snprintf(busStr, 10, "%d %d", i2c_bus1, i2c_bus3);
   }
+ 
 	
   // check for camera	
 //  char cmdbuffer1[1000];
@@ -373,8 +330,6 @@ int main(int argc, char * argv[]) {
   //file5 = popen("sudo rm /home/pi/CubeSatSim/camera_out.jpg.wav > /dev/null 2>&1", "r");
   pclose(file5);
 	
-  if (!ax5043) // don't test for payload if AX5043 is present
-  {
     payload = OFF;
     fprintf(stderr,"Opening serial\n");
     if ((uart_fd = serialOpen("/dev/ttyAMA0", 115200)) >= 0) {  // was 9600
@@ -386,7 +341,8 @@ int main(int argc, char * argv[]) {
     } else {
       fprintf(stderr, "Unable to open UART: %s\n -> Did you configure /boot/config.txt and /boot/cmdline.txt?\n", strerror(errno));
     }
-  }
+
+	sensor_setup();
 
   if ((i2c_bus3 == OFF) || (sim_mode == TRUE)) {
 
@@ -396,29 +352,33 @@ int main(int argc, char * argv[]) {
 
     srand((unsigned int)time(0));
 
-    axis[0] = rnd_float(-0.2, 0.2);
-    if (axis[0] == 0)
-      axis[0] = rnd_float(-0.2, 0.2);
-    axis[1] = rnd_float(-0.2, 0.2);
-    axis[2] = (rnd_float(-0.2, 0.2) > 0) ? 1.0 : -1.0;
+    axis[X] = rnd_float(-0.2, 0.2);
+    if (axis[X] == 0)
+      axis[X] = rnd_float(-0.2, 0.2);
+    axis[Y] = rnd_float(-0.2, 0.2);
+	float axis_z;
+	axis_z  = sqrt(1 - axis[X] * axis[X] - axis[Y] * axis[Y]);  
+    axis[Z] = (rnd_float(-0.2, 0.2) > 0) ? axis_z : -1.0 * axis_z;
 
-    angle[0] = (float) atan(axis[1] / axis[2]);
-    angle[1] = (float) atan(axis[2] / axis[0]);
-    angle[2] = (float) atan(axis[1] / axis[0]);
+    angle[X] = (float) atan(axis[Y] / axis[Z]);
+    angle[Y] = (float) atan(axis[Z] / axis[X]);
+    angle[Z] = (float) atan(axis[Y] / axis[X]);
 
-    volts_max[0] = rnd_float(4.5, 5.5) * (float) sin(angle[1]);
-    volts_max[1] = rnd_float(4.5, 5.5) * (float) cos(angle[0]);
-    volts_max[2] = rnd_float(4.5, 5.5) * (float) cos(angle[1] - angle[0]);
+    volts_max[X] = rnd_float(9.0, 12.0) * (float) sin(angle[Y]);
+    volts_max[Y] = rnd_float(9.0, 12.0) * (float) cos(angle[X]);
+    volts_max[Z] = rnd_float(9.0, 12.0) * (float) cos(angle[Y] - angle[X]);
 
-    float amps_avg = rnd_float(150, 300);
+    float amps_avg = rnd_float(150, 750);
 
-    amps_max[0] = (amps_avg + rnd_float(-25.0, 25.0)) * (float) sin(angle[1]);
-    amps_max[1] = (amps_avg + rnd_float(-25.0, 25.0)) * (float) cos(angle[0]);
-    amps_max[2] = (amps_avg + rnd_float(-25.0, 25.0)) * (float) cos(angle[1] - angle[0]);
+    amps_max[X] = (amps_avg + rnd_float(-25.0, 25.0)) * (float) sin(angle[Y]);
+    amps_max[Y] = (amps_avg + rnd_float(-25.0, 25.0)) * (float) cos(angle[X]);
+    amps_max[Z] = (amps_avg + rnd_float(-25.0, 25.0)) * (float) cos(angle[Y] - angle[X]);
 
-    batt = rnd_float(3.8, 4.3);
+    batt = rnd_float(3.8, 4.1);
     speed = rnd_float(1.0, 2.5);
     eclipse = (rnd_float(-1, +4) > 0) ? 1.0 : 0.0;
+	atmosphere = (rnd_float(-1, +4) > 0) ? 0.0 : 1.0;
+	  
 //    eclipse = 1;	  
     period = rnd_float(150, 300);
     tempS = rnd_float(20, 55);
@@ -426,7 +386,7 @@ int main(int argc, char * argv[]) {
     temp_min = rnd_float(10, 20);
 
 //    #ifdef DEBUG_LOGGING
-    for (int i = 0; i < 3; i++)
+    for (int i = X; i <= Z; 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
@@ -548,8 +508,9 @@ int main(int argc, char * argv[]) {
       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 (firstTime == 0)	
+  	firstTime = 1;
 	  
 //  if (!sim_mode)  // always read sensors, even in sim mode
   {
@@ -588,11 +549,13 @@ int main(int argc, char * argv[]) {
     fflush(stdout);
     fflush(stderr);
 //    frames_sent++;
+//	if (!sim_mode)  {
+    	sensor_payload[0] = '\0';
+	    memset(sensor, 0, sizeof(sensor));
+//	}
 	  
-    sensor_payload[0] = 0;
     memset(voltage, 0, sizeof(voltage));
     memset(current, 0, sizeof(current));
-    memset(sensor, 0, sizeof(sensor));
     memset(other, 0, sizeof(other));	
 	  
     FILE * uptime_file = fopen("/proc/uptime", "r");
@@ -605,6 +568,35 @@ int main(int argc, char * argv[]) {
 //    #endif
     fclose(uptime_file);
 
+  if (fail_rnd_mode) {
+//	if (loop % 10 == 0) { 	
+	if ((loopTime - failTime) > fail_time * 1000)	{
+//  	  failureMode = (int) rnd_float(1, FAIL_COUNT);
+  	  failureMode = (int) rnd_float(1, 9);
+	  printf("Sim Mode Random Failure Change\n");
+	  FILE * failure_mode_file = fopen("/home/pi/CubeSatSim/failure_mode.txt", "w");
+	  fprintf(failure_mode_file, "%d", failureMode);	
+	  fclose(failure_mode_file);
+	  failTime = loopTime;	
+    }
+  }
+//  else
+//  {
+//	  failureMode = OFF;
+	  FILE * failure_mode_file = fopen("/home/pi/CubeSatSim/failure_mode.txt", "r");
+	  if (failure_mode_file != NULL) {	
+	    char failure_string[10];	
+	    if ( (fgets(failure_string, 10, failure_mode_file)) != NULL)  {
+	     failureMode = atoi(failure_string); 
+		 fclose(failure_mode_file);	
+		 printf("Failure mode: %d\n", failureMode);	
+		}
+	  } else {
+		failureMode = FAIL_NONE;
+		printf("No simulated failure.\n");
+	  }
+//  }	  
+
    {
       int count1;
       char * token;
@@ -635,38 +627,55 @@ int main(int argc, char * argv[]) {
           }
         }
         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 (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]];  // 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 2 is present\n");
+				fprintf(stderr, "Turning off sim_mode since battery sensor is present\n");
 			}
 		}
-	else  {
-		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]] + current[map[BAT2]];  // Sum BAT and BAT2 currents
 	   
    }
 
-//      if (payload == ON) {  // moved to here
-      if (!ax5043) {	      
-//      if ((payload == ON) && (mode != BPSK)) {  // moved to here
-//        STEMBoardFailure = 0;
-	payload = get_payload_serial(FALSE);      
-        printf("get_payload_status: %d \n", payload);  // not debug
+
+  
+   
+	payload = get_payload_serial(FALSE); // not debug
+    printf("get_payload_status: %d \n", payload);
+	if (payload == FALSE) {
+		payload = pi_sensors(buffer2);  
+		printf("pi_sensors status: %d \n", payload);   
+	}
+  
 	fflush(stdout); 
 //	printf("String: %s\n", buffer2);       
 	fflush(stdout);   
-	strcpy(sensor_payload, buffer2);      
-//	printf(" Response from STEM Payload board: %s\n", sensor_payload);
-
+	strcpy(sensor_payload, buffer2);  
+
+     printf(" Response from STEM Payload: %s\n", sensor_payload);
+
+	 char sensor_buffer[30];
+	 int sensor_count;  
+	 sensor_buffer[0] = 0;  
+	 sensor_count = sensor_loop(sensor_buffer);
+	 if (sensor_count > NEW_SENSOR_FIELDS_MAX)
+		 sensor_count = NEW_SENSOR_FIELDS_MAX;
+	 if (sensor_count > 0)  {
+		char space[] = " ";
+		strcat(sensor_payload, space);
+	 	strcat(sensor_payload, sensor_buffer);  
+	 	printf(" Payload after new sensor read: %s\n", sensor_payload);
+	 }
         telem_file = fopen("/home/pi/CubeSatSim/telem.txt", "a");
 //        printf("Writing payload string\n");
         time_t timeStamp;
@@ -676,17 +685,30 @@ int main(int argc, char * argv[]) {
         char timeStampNoNl[31], bat_string[31];    
         snprintf(timeStampNoNl, 30, "%.24s", ctime(&timeStamp)); 
 //        printf("TimeStamp: %s\n", timeStampNoNl);
-
+/*
 	if (c2cStatus == DISABLED)      
 		snprintf(bat_string, 30, "BAT %4.2f %5.1f", batteryVoltage, batteryCurrent);	
 	else
 		snprintf(bat_string, 30, "BAT %4.2f %5.1f C", batteryVoltage, batteryCurrent);	
-		
+	*/   
+		snprintf(bat_string, 30, "BAT %.2f %.1f", batteryVoltage, batteryCurrent);	
+	    if (c2cStatus != DISABLED)
+			strcat(bat_string," C");
+	    if (sim_mode || (failureMode != FAIL_NONE))
+			strcat(bat_string," S");
         fprintf(telem_file, "%s %s %s\n", timeStampNoNl, bat_string, sensor_payload);	 // write telemetry string to telem.txt file    
         fclose(telem_file);
-      
+
+		if (failureMode == FAIL_PAYLOAD) {
+			sensor_payload[0] = '\0';  // This will cause the payload to not be processed.
+			printf("Simulated Payload Failure.\n");
+		}
+
+////	    if (!sim_mode) {
+		{	
         if ((sensor_payload[0] == 'O') && (sensor_payload[1] == 'K')) // only process if valid payload response
         {
+//		  printf("Valid Payload!\n");  	
           int count1;
           char * token;
  
@@ -696,12 +718,15 @@ int main(int argc, char * argv[]) {
           for (count1 = 0; count1 < SENSOR_FIELDS; count1++) {
             if (token != NULL) {
               sensor[count1] = (float) atof(token);
+			  strcpy(sensor_string[count1], token);	
 //              #ifdef DEBUG_LOGGING
 //                printf("sensor: %f ", sensor[count1]);  // print sensor data
+//				printf("Sensor String %d is %s\n",count1, sensor_string[count1]);
 //              #endif
               token = strtok(NULL, space);
             }
           }
+	  
 
           printf("\n");
 //	  if (sensor[GPS1] != 0) {     		
@@ -720,10 +745,11 @@ int main(int argc, char * argv[]) {
 			newGpsTime = millis();  
 		}
 	  }
-        }
-	else
-		; //payload = OFF;  // turn off since STEM Payload is not responding
-      }
+    }
+  	} 
+//	else
+//		; //payload = OFF;  // turn off since STEM Payload is not responding
+      
       if ((millis() - newGpsTime) > 60000) {
 		longitude += rnd_float(-0.05, 0.05) / 100.0;  // was .05
      		latitude += rnd_float(-0.05, 0.05) / 100.0;	      
@@ -731,8 +757,103 @@ int main(int argc, char * argv[]) {
 //	        printf("GPS Location with Rnd: APRS %07.2f, %08.2f \n", toAprsFormat(latitude), toAprsFormat(longitude));    
 	      	newGpsTime = millis();  
       }
-	  
+
+    if (sim_mode && (failureMode != FAIL_PAYLOAD) && !payload) {
+		printf("Generating simulated payload telemetry\n");
+		if (atmosphere == 0) {
+			sensor[PRES] = 0;
+			strcpy(sensor_string[PRES], "0.0");
+			sensor[ALT] = 400;
+			strcpy(sensor_string[ALT], "30000");
+			sensor[HUMI] = 0;
+			strcpy(sensor_string[HUMI], "0.0");
+			sensor[TEMP] = 0;	
+			strcpy(sensor_string[TEMP], "0.0");
+		} else {
+			sensor[PRES] = 1015;
+			strcpy(sensor_string[PRES], "1015");
+			sensor[ALT] = 75;
+			strcpy(sensor_string[ALT], "75");
+			sensor[HUMI] = 48;
+			strcpy(sensor_string[HUMI], "48");
+			sensor[TEMP] = 27;
+			strcpy(sensor_string[TEMP], "27.0");
+		}
+	   char sensor_number[20];	  
+	   sensor[ACCEL_X] = axis[X];
+	   sprintf(sensor_number, "%.2f", axis[X]);	  
+	   strcpy(sensor_string[ACCEL_X], sensor_number);	  
+	   sensor[ACCEL_Y] = axis[Y];
+	   sprintf(sensor_number, "%.2f", axis[Y]);	  
+	   strcpy(sensor_string[ACCEL_Y], sensor_number);	  	  
+	   sensor[ACCEL_Z] = axis[Z];
+	   sprintf(sensor_number, "%.2f", axis[Z]);	  
+	   strcpy(sensor_string[ACCEL_Z], sensor_number);	  
+		  
+	   float spin;
+	   spin = rnd_float(-30.0, 30.0);	  
+	   sensor[GYRO_X] = axis[X] * spin;
+	   sprintf(sensor_number, "%.2f", sensor[GYRO_X]);	  
+	   strcpy(sensor_string[GYRO_X], sensor_number);	  	  
+	   sensor[GYRO_Y] = axis[Y] * spin;
+	   sprintf(sensor_number, "%.2f", sensor[GYRO_Y]);	  
+	   strcpy(sensor_string[GYRO_Y], sensor_number);		  
+	   sensor[GYRO_Z] = axis[Z] * spin;
+	   sprintf(sensor_number, "%.2f", sensor[GYRO_Z]);	  
+	   strcpy(sensor_string[GYRO_Z], sensor_number);	
+	
+	//   printf("sim sensor: %s\n", sensor_string[GYRO_Z]);	  
+	   printf("sim sensor spin: %f value: %f length: %d string: %s\n", spin, sensor[GYRO_Z], strlen(sensor_string[GYRO_Z]), sensor_string[GYRO_Z]); 
+   }	   
+
+	  if (failureMode == FAIL_BME) {
+			sensor[TEMP] = 0.0;
+		    strcpy(sensor_string[TEMP], "0.0");
+			sensor[PRES] = 0.0;		  
+		    strcpy(sensor_string[PRES], "0.0");
+		    sensor[HUMI] = 0.0;
+		    strcpy(sensor_string[HUMI], "0.0");
+		    sensor[ALT] = 0.0;	
+		    strcpy(sensor_string[ALT], "0.0");				
+		    printf("Simulated BME Failure!\n");
+	  }
+
+	  if (failureMode == FAIL_MPU) {
+			sensor[ACCEL_X] = 0.0;
+		    strcpy(sensor_string[ACCEL_X], "0.0");
+			sensor[ACCEL_Y] = 0.0;
+		    strcpy(sensor_string[ACCEL_Y], "0.0");		  
+			sensor[ACCEL_Z] = 0.0;
+		    strcpy(sensor_string[ACCEL_Z], "0.0");
+			sensor[GYRO_X] = 0.0;
+		    strcpy(sensor_string[GYRO_X], "0.0");
+			sensor[GYRO_Y] = 0.0;
+		    strcpy(sensor_string[GYRO_Y], "0.0");
+			sensor[GYRO_Z] = 0.0;
+		    strcpy(sensor_string[GYRO_Z], "0.0");		  
+		    printf("Simulated MPU Failure!\n");
+	  }
+
+		if ((failureMode == FAIL_BME) || (failureMode == FAIL_MPU) || sim_mode) // recreaate sensor_payload string	
+		{  
+		  sensor_payload[0] = '\0';
+		  strcpy(sensor_string[0], "OK");
+		  strcpy(sensor_string[1], "BME280");
+		  strcpy(sensor_string[6], "MPU6050");
+			
+          for (count1 = 0; count1 < SENSOR_FIELDS; count1++) {
+			  strcat(sensor_payload, sensor_string[count1]);
+			  strcat(sensor_payload, " ");
+            }
+		  printf("Updated Sensor String: %s\n", sensor_payload);	
+		}
+		else if (failureMode != FAIL_PAYLOAD) {
+		  printf("Restoring sensor_payload\n");	
+		  strcpy(sensor_payload, buffer2);  // restore sensor_payload after strtok operation
+		}
+	   
       if ((sensor_payload[0] == 'O') && (sensor_payload[1] == 'K')) {
+//		printf("Valid Payload!!\n");  
         for (int count1 = 0; count1 < SENSOR_FIELDS; count1++) {
           if (sensor[count1] < sensor_min[count1])
             sensor_min[count1] = sensor[count1];
@@ -769,15 +890,6 @@ int main(int argc, char * argv[]) {
       current[map[PLUS_Z]] = (Zi >= 0) ? Zi : 0;
       current[map[MINUS_Z]] = (Zi >= 0) ? 0 : ((-1.0f) * Zi);
 
-      voltage[map[PLUS_X]] = (Xv >= 1) ? Xv : rnd_float(0.9, 1.1);
-      voltage[map[MINUS_X]] = (Xv <= -1) ? ((-1.0f) * Xv) : rnd_float(0.9, 1.1);
-      voltage[map[PLUS_Y]] = (Yv >= 1) ? Yv : rnd_float(0.9, 1.1);
-      voltage[map[MINUS_Y]] = (Yv <= -1) ? ((-1.0f) * Yv) : rnd_float(0.9, 1.1);
-      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]]);
-
       tempS += (eclipse > 0) ? ((temp_max - tempS) / 50.0f) : ((temp_min - tempS) / 50.0f);
       tempS += +rnd_float(-1.0, 1.0);
       //  IHUcpuTemp = (int)((tempS + rnd_float(-1.0, 1.0)) * 10 + 0.5);
@@ -790,27 +902,43 @@ int main(int argc, char * argv[]) {
 //      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]] = rnd_float(285, 305) - charging;
+      current[map[BAT]] = rnd_float(320, 510) - 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]]);
 
-      batt -= (batt > 3.5) ? current[map[BAT]] / 30000 : current[map[BAT]] / 3000;
-      if (batt < 3.0) {
-        batt = 3.0;
+      batt -= (batt > 3.5) ? current[map[BAT]] / 300000 : current[map[BAT]] / 30000;
+      if (batt < 3.6) {
+        batt = 3.6;
         SafeMode = 1;
         printf("Safe Mode!\n");
       } else
         SafeMode= 0;
 
-      if (batt > 4.5)
-        batt = 4.5;
+      if (batt > 4.1)
+        batt = 4.1;
 
       voltage[map[BAT]] = batt + rnd_float(-0.01, 0.01);
+		
+      float Vm, Vp;
+	  Vm = batt + 0.5;	
+	  Vp = (Xv > 0) ? Xv : rnd_float(0.0, 0.1);
+	  voltage[map[PLUS_X]] = (Vp >= Vm) ? (Vm + rnd_float(-0.1, 0.1)) : Vp;
+      Vp = (Xv < 0) ? ((-1.0f) * Xv) : rnd_float(0.0, 0.1);
+	  voltage[map[MINUS_X]] = (Vp >= Vm) ? (Vm + rnd_float(-0.1, 0.1)) : Vp;	
+      Vp = (Yv > 0) ? Yv : rnd_float(0.0, 0.1);
+	  voltage[map[PLUS_Y]] = (Vp >= Vm) ? (Vm + rnd_float(-0.1, 0.1)) : Vp;	
+      Vp = (Yv < 0) ? ((-1.0f) * Yv) : rnd_float(0.0, 0.1);
+	  voltage[map[MINUS_Y]] = (Vp >= Vm) ? (Vm + rnd_float(-0.1, 0.1)) : Vp;		
+      Vp = (Zv > 0) ? Zv : rnd_float(0.0, 0.1);
+	  voltage[map[PLUS_Z]] = (Vp >= Vm) ? (Vm + rnd_float(-0.1, 0.1)) : Vp;	
+      Vp = (Zv < 0) ? ((-1.0f) * Zv) : rnd_float(0.0, 0.1);
+	  voltage[map[MINUS_Z]] = (Vp >= Vm) ? (Vm + rnd_float(-0.1, 0.1)) : Vp;	
+
+      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]]);
 
       // end of simulated telemetry
-    }
-    else {
-      }
+	  }
+	
       FILE * cpuTempSensor = fopen("/sys/class/thermal/thermal_zone0/temp", "r");
       if (cpuTempSensor) {
    //     double cpuTemp;
@@ -879,14 +1007,33 @@ int main(int argc, char * argv[]) {
     FILE * fp = fopen("/home/pi/CubeSatSim/telem_string.txt", "w");
     if (fp != NULL)  {	  
 //    	printf("Writing telem_string.txt\n");
-	if (batteryVoltage != 4.5)
+	if (batteryVoltage != 4.5) {
+/*		
 		if (c2cStatus == DISABLED)
     			fprintf(fp, "BAT %4.2fV %4.0fmA\n", batteryVoltage, batteryCurrent);
 		else
     			fprintf(fp, "BAT %4.2fV %4.0fmA C\n", batteryVoltage, batteryCurrent);	// show command and control is on		
-	else
-    		fprintf(fp, "\n");	// don't show voltage and current if it isn't a sensor value
+		*/
+		fprintf(fp, "BAT %.2fV %.0fmA", batteryVoltage, batteryCurrent);
+		if (c2cStatus != DISABLED)
+			fprintf(fp," C");
+	    if (sim_mode || (failureMode != FAIL_NONE))
+			fprintf(fp," S\n");
+		else
+			fprintf(fp,"\n");
+	}
+	else {
+		printf("Write simulted voltage and current to telem_string.txt\n");
+ //   		fprintf(fp, "\n");	// don't show voltage and current if it isn't a sensor value
+		fprintf(fp, "BAT %.2fV %.0fmA", voltage[map[BAT]], current[map[BAT]]);  // display simulated voltage and current
+		if (c2cStatus != DISABLED)
+			fprintf(fp," C");
+	    if (sim_mode || (failureMode != FAIL_NONE))
+			fprintf(fp," S\n");
+		else
+			fprintf(fp,"\n");
 		
+	}
     	fclose(fp);	 
     } else 
 	    printf("Error writing to telem_string.txt\n");
@@ -989,20 +1136,6 @@ int upper_digit(int number) {
   return digit;
 }
 
-static int init_rf() {
-  int ret;
-  fprintf(stderr, "Initializing AX5043\n");
-
-  ret = ax5043_init( & hax5043, XTAL_FREQ_HZ, VCO_INTERNAL);
-  if (ret != PQWS_SUCCESS) {
-    fprintf(stderr,
-      "ERROR: Failed to initialize AX5043 with error code %d\n", ret);
-    //       exit(EXIT_FAILURE);
-    return (0);
-  }
-  return (1);
-}
-
 void get_tlm(void) {
 
   FILE * txResult;
@@ -1065,17 +1198,16 @@ void get_tlm(void) {
 //    char footer_str[] = "-11>APCSS:010101/hi hi ' >> t.txt && touch /home/pi/CubeSatSim/ready";  // transmit is done by transmit.py
     char footer_str[] = " && echo 'AMSAT-11>APCSS:010101/hi hi ' >> t.txt && touch /home/pi/CubeSatSim/ready";  // transmit is done by transmit.py
     char footer_str2[] = " && touch /home/pi/CubeSatSim/ready"; 
+	char zero[] = "0.0";  
 	  
-    if (ax5043) {
-      strcpy(str, header_str);
-    } else {
+   
       strcpy(str, header_str3);
 //    }
       if (mode == AFSK) {
         strcat(str, call);
         strcat(str, header_str2);	    
       }	    
-    }
+    
 //      printf("Str: %s \n", str);
       if (mode != CW) {
          //	sprintf(header_str2b, "=%7.2f%c%c%c%08.2f%cShi hi ",4003.79,'N',0x5c,0x5c,07534.33,'W');  // add APRS lat and long
@@ -1088,9 +1220,6 @@ void get_tlm(void) {
         else
           sprintf(header_long, "%08.2f%c",toAprsFormat( longitude) * (-1.0), 'W'); // long
 	      
-        if (ax5043)
-          sprintf(header_str2b, "=%s%c%sShi hi ", header_lat, 0x5c, header_long); // add APRS lat and long	    
-        else
 //#ifdef HAB
 	if (hab_mode)	
 		sprintf(header_str2b, "=%s%c%sOhi hi ", header_lat, 0x2f, header_long); // add APRS lat and long with Balloon HAB icon
@@ -1105,8 +1234,11 @@ void get_tlm(void) {
         strcat(str, header_str4);
 	strcat(str, call);
 	if (c2cStatus != DISABLED) {
-		strcat(str, header_c2c);
+		strcat(str, "  C");
 	}
+	if (sim_mode || failureMode != FAIL_NONE) {
+		strcat(str, "  S");
+	}	  
 
 	sprintf(tlm_str, "%s' > cw0.txt", &str);   
 	printf("CW string to execute: %s\n", &tlm_str);     
@@ -1138,16 +1270,23 @@ void get_tlm(void) {
 //      fclose(file_append);
 //    }
   } else {  // APRS
-
+/*
       if (c2cStatus == 0)	   
         sprintf(tlm_str, "BAT %4.2f %5.1f ", batteryVoltage, batteryCurrent); 
       else
         sprintf(tlm_str, "BAT %4.2f %5.1f C ", batteryVoltage, batteryCurrent); 
-	      
-      strcat(str, tlm_str);
+*/
+		snprintf(tlm_str, 30, "BAT %.2f %.1f ", batteryVoltage, batteryCurrent);	
+		if (c2cStatus != DISABLED)
+			strcat(tlm_str,"C ");
+		if (sim_mode || (failureMode != FAIL_NONE))
+			strcat(tlm_str,"S ");
+//	    printf("tlm_str: %s\n", tlm_str);
+      	strcat(str, tlm_str);
   }  
-    strcpy(sensor_payload, buffer2);      	  
-    printf(" Response from STEM Payload board:: %s\n", sensor_payload);
+	  
+//    strcpy(sensor_payload, buffer2);      	  
+    printf(" sensor_payload: %s\n", sensor_payload);
 //    printf(" Str so far: %s\n", str);   
 	  
     if (mode != CW) 
@@ -1171,30 +1310,7 @@ void get_tlm(void) {
         sleep(5);	      
       }    
     } 
-    else if (ax5043) {
-      digitalWrite(txLed, txLedOn);
-      fprintf(stderr, "INFO: Transmitting X.25 packet using AX5043\n");
-      memcpy(data, str, strnlen(str, 256));
-      printf("data: %s \n", data);	    
-      int ret = ax25_tx_frame( & hax25, & hax5043, data, strnlen(str, 256));
-      if (ret) {
-        fprintf(stderr,
-          "ERROR: Failed to transmit AX.25 frame with error code %d\n",
-          ret);
-        exit(EXIT_FAILURE);
-      }
-      ax5043_wait_for_transmit();
-      digitalWrite(txLed, txLedOff);
-
-      if (ret) {
-        fprintf(stderr,
-          "ERROR: Failed to transmit entire AX.25 frame with error code %d\n",
-          ret);
-        exit(EXIT_FAILURE);
-      }
-      sleep(4);  // was 2
-	    
-    } else {  // APRS using transmit
+    else {  // APRS using transmit
 	    
       strcat(str, footer_str1);
 //      strcat(str, call);
@@ -1203,7 +1319,7 @@ void get_tlm(void) {
       else
       	strcat(str, footer_str2);
 	    
-      fprintf(stderr, "String to execute: %s\n", str);
+      fprintf(stderr, "APRS String to execute: %s\n", str);
 	    
       printf("\n\nTelemetry string is %s \n\n", str);	
 	    
@@ -1234,6 +1350,7 @@ void get_tlm_fox() {
   int i;
 
   long int sync = syncWord;
+  int cam = ON;
 
   smaller = (int) (S_RATE / (2 * freq_Hz));
 
@@ -1261,10 +1378,68 @@ void get_tlm_fox() {
   int posXi = 0, negXi = 0, posYi = 0, negYi = 0, posZi = 0, negZi = 0;
   int head_offset = 0;
 
+  STEMBoardFailure = 1;
+
   short int buffer_test[bufLen];
   int buffSize;
   buffSize = (int) sizeof(buffer_test);
 	
+  if (failureMode == FAIL_NONE) 
+	  printf("No Simulated Failure!\n");	
+//  if (failureMode == -1) {
+//	  failureMode = (int) rnd_float(1, FAIL_COUNT);
+//	  printf("Random Failure\n");
+//  }
+  if (failureMode == FAIL_UNPLUG) {
+	  voltage[map[PLUS_Y]] = rnd_float(0.8, 0.95);
+	  current[map[PLUS_Y]] = 0.0;
+	  printf("+Y Solar Unplugged Failure\n");
+  }	
+  if (failureMode == FAIL_SOLAR) {
+	  voltage[map[PLUS_X]] = 0.0;
+	  current[map[PLUS_X]] = 0.0;
+	  printf("+X Solar Simulated Failure\n");
+  }
+  if (failureMode == FAIL_DEGRADE) {
+	  voltage[map[MINUS_X]] = voltage[map[MINUS_X]] * 0.5;
+	  current[map[MINUS_X]] = current[map[MINUS_X]] * 0.5;
+	  printf("-X Solar Deg Simulated Failure\n");	  
+  }
+  if (failureMode == FAIL_SHORT) {
+	  voltage[map[MINUS_Y]] = 0.0;
+	  printf("-Y Solar SC Simulated Failure!\n");	  
+  }
+  if (failureMode == FAIL_I2C1)  {
+	  voltage[map[PLUS_X]] = 0.0;
+	  current[map[PLUS_X]] = 0.0;
+	  voltage[map[PLUS_Y]] = 0.0;
+	  current[map[PLUS_Y]] = 0.0;
+	  voltage[map[BAT]] = 0.0;
+	  current[map[BAT]] = 0.0;
+	  voltage[map[BAT2]] = 0.0;
+	  current[map[BAT2]] = 0.0;	  
+	  printf("I2C Bus 1 Simulated Failure!\n");	
+  } 
+  if (failureMode == FAIL_I2C3)  {
+	  voltage[map[MINUS_X]] = 0.0;
+	  current[map[MINUS_X]] = 0.0;
+	  voltage[map[MINUS_Y]] = 0.0;
+	  current[map[MINUS_Y]] = 0.0;
+	  voltage[map[MINUS_Z]] = 0.0;
+	  current[map[MINUS_Z]] = 0.0;	  
+	  voltage[map[PLUS_Z]] = 0.0;
+	  current[map[PLUS_Z]] = 0.0;	  
+	  printf("I2C Bus 3 Simulated Failure!\n");	
+  }		
+  if (failureMode == FAIL_PAYLOAD) {
+	  payload = OFF;
+	  printf("Payload Simulated Failure!\n");	  
+  }
+  if (failureMode == FAIL_CAMERA) {
+	  cam = OFF;
+	  printf("Camera Simulated Failure!\n");	  
+  }
+
   if (mode == FSK)
     id = 7;
   else
@@ -1273,7 +1448,8 @@ void get_tlm_fox() {
   //  for (int frames = 0; frames < FRAME_CNT; frames++) 
   for (int frames = 0; frames < frameCnt; frames++) {
   
-    if (firstTime != ON) {
+//    if (firstTime != ON) {
+	if (TRUE) {	
       // delay for sample period
 
 /**/
@@ -1291,7 +1467,7 @@ void get_tlm_fox() {
       
       sampleTime = (unsigned int) millis();
     } else
-      printf("first time - no sleep\n");
+      printf("first or second time - no sleep\n");
 
     printf("++++ Loop time: %5.3f sec +++++\n", (millis() - loopTime)/1000.0);
     fflush(stdout);
@@ -1411,7 +1587,7 @@ void get_tlm_fox() {
 
     encodeA(b, 9 + head_offset, battCurr);
 
-    encodeB(b, 10 + head_offset, (int)(sensor[TEMP] * 10 + 0.5)); // Temp	  
+	encodeB(b, 10 + head_offset, (int)(sensor[TEMP] * 10 + 0.5)); // Temp	  
 
     if (mode == FSK) {
       encodeA(b, 12 + head_offset, posXv);
@@ -1565,24 +1741,51 @@ void get_tlm_fox() {
     encodeA(b, 33 + head_offset, (int)(sensor[PRES] + 0.5)); // Pressure
     encodeB(b, 34 + head_offset, (int)(sensor[ALT] / 10.0 + 0.5)); // Altitude
 
-    encodeA(b, 36 + head_offset, Resets);
-    encodeB(b, 37 + head_offset, (int)(other[RSSI] + 0.5) + 2048);
+	
+		      encodeB(b_min, 49 + head_offset, (int)(sensor_min[XS1]));
+		      encodeA(b_min, 0 + head_offset, (int)(sensor_min[XS2]));
+		      encodeB(b_min, 1 + head_offset, (int)(sensor_min[XS3]));	      	      	      
+	    }      
+	    else
+	    {	        	    
+		      encodeB(b_min, 4 + head_offset, 2048); // 0
+		      encodeA(b_min, 6 + head_offset, 2048); // 0
+		      encodeB(b_min, 7 + head_offset, 2048); // 0	    
+	
+		      encodeB(b_min, 40 + head_offset, 2048);
+		      encodeA(b_min, 42 + head_offset, 2048);
+		      encodeB(b_min, 43 + head_offset, 2048);
+	
+		      encodeA(b_min, 48 + head_offset, 2048);
+	//	      encodeB(b_min, 49 + head_offset, 2048);
+	    }	
 
-    encodeA(b, 39 + head_offset, (int)(other[IHU_TEMP] * 10 + 0.5));
+    }    
+    encodeA(b, 30 + head_offset, BAT2Voltage);
 
-    encodeB(b, 40 + head_offset, (int)(sensor[GYRO_X] + 0.5) + 2048);
-    encodeA(b, 42 + head_offset, (int)(sensor[GYRO_Y] + 0.5) + 2048);
-    encodeB(b, 43 + head_offset, (int)(sensor[GYRO_Z] + 0.5) + 2048);
+    encodeB(b, 31 + head_offset, ((int)(other[SPIN] * 10)) + 2048);
 
-    encodeA(b, 45 + head_offset, (int)(sensor[HUMI] * 10 + 0.5)); // in place of sensor1
+	  		encodeA(b, 33 + head_offset, (int)(sensor[PRES] + 0.5)); // Pressure
+		    encodeB(b, 34 + head_offset, (int)(sensor[ALT] * 10.0 + 0.5)); // Altitude
+		    encodeA(b, 45 + head_offset, (int)(sensor[HUMI] * 10 + 0.5)); // in place of sensor1
+		    encodeA(b, 39 + head_offset, (int)(other[TEMP] * 10 + 0.5));
+ 
+	    encodeA(b, 36 + head_offset, Resets);
+	    encodeB(b, 37 + head_offset, (int)(other[RSSI] + 0.5) + 2048);
+	
+	
+		    encodeB(b, 40 + head_offset, (int)(sensor[GYRO_X] + 0.5) + 2048);
+		    encodeA(b, 42 + head_offset, (int)(sensor[GYRO_Y] + 0.5) + 2048);
+		    encodeB(b, 43 + head_offset, (int)(sensor[GYRO_Z] + 0.5) + 2048);
+		encodeA(b, 48 + head_offset, (int)(sensor[DTEMP] * 10 + 0.5) + 2048);
+		encodeB(b, 49 + head_offset, (int)(sensor[XS1]));
+    	encodeA(b, 0 + head_offset, (int)(sensor[XS2]));
+    	encodeB(b, 1 + head_offset, (int)(sensor[XS3]));
 
     encodeB(b, 46 + head_offset, BAT2Current);
-    encodeA(b, 48 + head_offset, (int)(sensor[DTEMP] * 10 + 0.5) + 2048);
-//    encodeB(b, 49 + head_offset, (int)(sensor[XS1] * 10 + 0.5) + 2048);
+    encodeA(b, 39 + head_offset, (int)(other[IHU_TEMP] * 10 + 0.5));
 	  
-    encodeB(b, 49 + head_offset, (int)(sensor[XS1]));
-    encodeA(b, 0 + head_offset, (int)(sensor[XS2]));
-    encodeB(b, 1 + head_offset, (int)(sensor[XS3]));
+//    encodeB(b, 49 + head_offset, (int)(sensor[XS1] * 10 + 0.5) + 2048);
 	  
     FILE * command_count_file = fopen("/home/pi/CubeSatSim/command_count.txt", "r");
     if (command_count_file != NULL) {	
@@ -1594,10 +1797,28 @@ void get_tlm_fox() {
 	    printf("Error opening command_count.txt!\n");
     fclose(command_count_file);
     
-//    printf("Command count: %d\n", groundCommandCount);	  
-    
-    int status = STEMBoardFailure + SafeMode * 2 + sim_mode * 4 + PayloadFailure1 * 8 +
-      (i2c_bus0 == OFF) * 16 + (i2c_bus1 == OFF) * 32 + (i2c_bus3 == OFF) * 64 + (camera == OFF) * 128 + groundCommandCount * 256;
+//    printf("Command count: %d\n", groundCommandCount);
+	int simulated; 
+	simulated = sim_mode;  
+	if (failureMode != FAIL_NONE)  {
+		simulated = TRUE;
+		printf("Showing Simulated in FoxTelem\n");
+	}
+	int i2c_1, i2c_3; 
+	i2c_1 = i2c_bus1;  
+	i2c_3 = i2c_bus3; 
+//	printf("Bus1: %d Bus2: %d  \n", i2c_1, i2c_3);
+	if (failureMode == FAIL_I2C1)  {
+		i2c_1 = OFF;
+//		printf("I2C Bus 1 Simulated Failure\n");
+	} else 	if (failureMode == FAIL_I2C3)  {
+		i2c_3 = OFF;
+//		printf("I2C Bus 3 Simulated Failure\n");
+	}		  
+//  int status = STEMBoardFailure + SafeMode * 2 + sim_mode * 4 + PayloadFailure1 * 8 +    
+//      (i2c_bus0 == OFF) * 16 + (i2c_bus1 == OFF) * 32 + (i2c_bus3 == OFF) * 64 + (camera == OFF) * 128 + groundCommandCount * 256;
+    int status = STEMBoardFailure + SafeMode * 2 + simulated * 4 + PayloadFailure1 * 8 +
+      (i2c_bus0 == OFF) * 16 + (i2c_1 == OFF) * 32 + (i2c_3 == OFF) * 64 + (cam == OFF) * 128 + groundCommandCount * 256;
 
     encodeA(b, 51 + head_offset, status);
     encodeB(b, 52 + head_offset, rxAntennaDeployed + txAntennaDeployed * 2 + c2cStatus * 4);
@@ -2726,3 +2947,50 @@ void socket_send(int length) {
   if (socket_open == 1)	
     firstTime = 0;
 }
+
+int pi_sensors(char *buffer) 
+{
+	char sensor_buffer[1000];
+	FILE *sensor_read;
+	
+	strcpy(buffer, "OK BME280 ");
+	
+	sensor_read = sopen("/home/pi/raspberry-pi-bme280/bme280");  // read BME if present 
+    fgets(sensor_buffer, 1000, sensor_read);
+    fprintf(stderr, "bme result: %s\n", sensor_buffer);
+    fclose(sensor_read);
+	if (sensor_buffer[strlen(sensor_buffer) - 1] == '\n')
+		sensor_buffer[strlen(sensor_buffer) - 1] = '\0'; // remove newline at end
+	strcat(buffer, sensor_buffer);
+
+	strcat(buffer, " MPU6050 ");
+	
+	sensor_read = sopen("/home/pi/MPU6050-C-CPP-Library-for-Raspberry-Pi/mpu6050");  // read MPU if present 
+    fgets(sensor_buffer, 1000, sensor_read);
+    fprintf(stderr, "mpu result: %s\n", sensor_buffer);
+    fclose(sensor_read);
+	if (sensor_buffer[strlen(sensor_buffer) - 1] == '\n')
+		sensor_buffer[strlen(sensor_buffer) - 1] = '\0'; // remove newline at end
+	strcat(buffer, sensor_buffer);
+	
+	fprintf(stderr, "pi_sensors string: %s\n", buffer);
+
+	strcat(buffer, " GPS 0.0 0.0 0.0 TMP 0.0"); // place holders for GPS and diode temp
+/*
+	strcat(buffer, " YPR ");
+	
+	sensor_read = sopen("/home/pi/MPU6050-C-CPP-Library-for-Raspberry-Pi/ypr");  // read MPU if present 
+    fgets(sensor_buffer, 1000, sensor_read);
+    fprintf(stderr, "mpu result: %s\n", sensor_buffer);
+    fclose(sensor_read);
+	if (sensor_buffer[strlen(sensor_buffer) - 1] == '\n')
+		sensor_buffer[strlen(sensor_buffer) - 1] = '\0'; // remove newline at end	
+	strcat(buffer, sensor_buffer);  // no need to remove newline at end
+	
+	fprintf(stderr, "pi_sensors string: %s\n", buffer);
+*/
+	if (strcmp(buffer, "OK BME280 0.0 0.0 0.0 0.0 MPU6050 0.0 0.0 0.0 0.0 0.0 0.0 GPS 0.0 0.0 0.0 TMP 0.0 YPR 0.0 0.0 0.0") == 0)
+		return (0);
+	else		
+		return (1);
+}
diff --git a/main.h b/main.h
index 1c2e5e36..0ba9c824 100644
--- a/main.h
+++ b/main.h
@@ -52,11 +52,15 @@
 #define XS1 20 // NEW user defined token will be position 19
 #define XS2 21  
 #define XS3 22
-#define SENSOR_FIELDS 26
+#define NEW_SENSOR_FIELDS_MAX 6
+#define SENSOR_FIELDS (26 + 6)
 #define FC_EPS 1
 #define FC_BOB 25
 #define FC_SW 50
 #define FC_PAYLOAD 55
+#define X 0
+#define Y 1
+#define Z 2
 
 #define RSSI 0
 #define IHU_TEMP 2
@@ -73,9 +77,6 @@ uint32_t tx_freq_hz = 434900000 + FREQUENCY_OFFSET;
 uint8_t data[1024];
 uint32_t tx_channel = 0;
 
-ax5043_conf_t hax5043;
-ax25_conf_t hax25;
-
 int twosToInt(int val, int len);
 float toAprsFormat(float input);
 float rnd_float(double min, double max);
@@ -101,6 +102,7 @@ int socket_open = 0;
 int sock = 0;
 int loop = -1, loop_count = 0;
 int firstTime = ON; // 0;
+int secondTime = ON;
 long start;
 int testCount = 0;
 long time_start;
@@ -124,6 +126,21 @@ FILE *image_file;
 #define REPEATER 7
 #define TXCOMMAND 12
 
+#define FAIL_COUNT 11
+#define FAIL_NONE -1
+#define FAIL_UNPLUG 1
+#define FAIL_SOLAR 2
+#define FAIL_DEGRADE 3
+#define FAIL_SHORT 4
+#define FAIL_I2C1 5
+#define FAIL_I2C3 6
+#define FAIL_CAMERA 7
+#define FAIL_PAYLOAD 8
+#define FAIL_BME 9
+#define FAIL_MPU 10
+#define FAIL_AUDIO 11
+int failureMode = FAIL_NONE;
+
 int transmitStatus = -1;
 
 float amplitude; // = ; // 20000; // 32767/(10%amp+5%amp+100%amp)
@@ -148,6 +165,8 @@ long int uptime;
 char call[5];
 char sim_yes[10];
 char hab_yes[10];
+char fail_yes[10];
+int fail_time = 60;
 int squelch = 3; // default squelch
 char rx[12], tx[12];
 int tx_pl = 0;
@@ -158,16 +177,16 @@ float sleepTime;
 unsigned int sampleTime = 0;
 int frames_sent = 0;
 int cw_id = ON;
-int vB4 = FALSE, vB5 = FALSE, vB3 = FALSE, ax5043 = FALSE, transmit = FALSE, onLed, onLedOn, onLedOff, txLed, txLedOn, txLedOff, payload = OFF;
+int transmit = FALSE, onLed, onLedOn, onLedOff, txLed, txLedOn, txLedOff, payload = OFF;
 // float voltageThreshold = 3.6, batteryVoltage = 4.5, batteryCurrent = 0, currentThreshold = 100;
-float voltageThreshold = 3.5, batteryVoltage = 4.5, batteryCurrent = 0, currentThreshold = 100;
+float voltageThreshold = 3.55, batteryVoltage = 4.5, batteryCurrent = 0, currentThreshold = 100;
 float latitude = 39.027702f, longitude = -77.078064f;
 float lat_file, long_file;
 double cpuTemp;
 int frameTime;
 long int newGpsTime;
 
-float axis[3], angle[3], volts_max[3], amps_max[3], batt, speed, period, tempS, temp_max, temp_min, eclipse;
+float axis[3], angle[3], volts_max[3], amps_max[3], batt, speed, period, tempS, temp_max, temp_min, eclipse, atmosphere;
 int i2c_bus0 = OFF, i2c_bus1 = OFF, i2c_bus3 = OFF, camera = OFF, sim_mode = FALSE, SafeMode = FALSE;
 int rxAntennaDeployed = 0, txAntennaDeployed = 0, c2cStatus = 0;
 int sim_config = FALSE; // sim mode not set by configuration
@@ -175,6 +194,7 @@ double eclipse_time;
 
 float voltage[9], current[9], sensor[SENSOR_FIELDS], other[3];
 char sensor_payload[500];
+char sensor_string[SENSOR_FIELDS][32];
 
 int test_i2c_bus(int bus);
 
@@ -211,8 +231,10 @@ int pi_zero_2_offset = 0;
 
 
 int hab_mode = FALSE; 
+int fail_rnd_mode = FALSE;
 int battery_saver_mode = FALSE;
 long int loopTime;
+long int failTime = 0;
 
 int error_count = 0;
 int groundCommandCount = 0;
@@ -223,6 +245,9 @@ int groundCommandCount = 0;
     int m_ileaver_index;               /* Byte counter for interleaver */
     unsigned char m_conv_sr;           /* Convolutional encoder shift register state */
 
+void sensor_setup();  // defined in sensor_extension.c
+int sensor_loop(char *sensor_buffer);  // defined in sensor_extension.c
+int pi_sensors(char *buffer); // used to read BME and MPU sensor if connected to Pi
 
 // from funcubeLib/common/fecConstants.h
 
diff --git a/sensor_extension.c b/sensor_extension.c
new file mode 100644
index 00000000..5c958a5a
--- /dev/null
+++ b/sensor_extension.c
@@ -0,0 +1,34 @@
+// Use this template for adding additional sensors
+// see example ...
+
+// put your library includes here
+#include <stdio.h>
+#include <string.h>
+
+// put your globals here
+
+
+// put your setup code here
+void sensor_setup() {
+  
+//  printf("Starting new sensor!\n");
+
+}
+
+// put your loop code here
+// Very Important: only use print, not println!!
+int sensor_loop(char *sensor_buffer) {
+
+   int sensors = 0;  // set to the number of sensor readings adding.
+   sensor_buffer[0] = 0; // make sure buffer is empty
+  
+//   printf("Reading new sensors!\n");
+  
+//   sensors = 3;
+//   strcpy(sensor_buffer, "NEW 0.0 0.0 0.0");
+
+//   printf("New sensor string: %s\n", sensor_buffer);
+
+  return(sensors);
+  
+}
diff --git a/telem.c b/telem.c
index 1fc534a5..ce4f14c3 100644
--- a/telem.c
+++ b/telem.c
@@ -15,7 +15,7 @@ int main(int argc, char *argv[]) {
     }
   }
 
-  printf("CubeSatSim v2.1 INA219 Voltage and Current Telemetry\n");  
+  printf("CubeSatSim v2.2 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 10cee57d..6ac1b713 100644
--- a/transmit.py
+++ b/transmit.py
@@ -12,6 +12,30 @@ from PIL import Image, ImageDraw, ImageFont, ImageColor
 import serial	
 import random
 
+def sim_failure_check():
+	try:
+		global card
+		global cam_fail
+		cam_fail = False
+		file = open("/home/pi/CubeSatSim/failure_mode.txt")
+		fail_mode = int(file.read(2))
+#		print("Fail_mode: ")
+#		print(fail_mode)
+		if (fail_mode == 11):
+			card = "Device"  # Change audio so no FM audio plays
+			print("Failure mode no FM audio")
+		elif (fail_mode == 7):
+			cam_fail = True
+			print("Failure mode camera fail")	
+		elif (fail_mode == -1):
+			print("No failure mode")	
+		else:
+			print("Other failure mode")
+			card = "Headphones"
+	except:
+		print("No failure mode")
+		card = "Headphones"
+
 def battery_saver_check():
 	try:
 		global txc
@@ -95,6 +119,8 @@ def increment_mode():
 		print("can't write to .mode file")
 		
 def camera_photo():
+	global cam_fail
+	sim_failure_check()
 	system("sudo rm /home/pi/CubeSatSim/camera_out.jpg")
 	stored_image = False
 	try:
@@ -102,6 +128,10 @@ def camera_photo():
 		f = open("/home/pi/CubeSatSim/camera_out.jpg")
 		f.close()
 		print("Photo taken")
+		if (cam_fail == True):
+			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
 	except:
 		system("cp /home/pi/CubeSatSim/sstv//sstv_image_2_320_x_256.jpeg /home/pi/CubeSatSim/camera_out.jpg")
 		print("Using stored image")
@@ -126,11 +156,11 @@ def camera_photo():
 	#					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')
+		draw.text((112, 12), telem_string, font=font2, fill='black')  # was 122
+		draw.text((110, 10), telem_string, font=font2, fill='white')  # was 120
 		img.save(file)
 
-print("CubeSatSim v2.1 transmit.py starting...")
+print("CubeSatSim v2.2 transmit.py starting...")
 
 pd = 21
 ptt = 20
@@ -446,6 +476,7 @@ if __name__ == "__main__":
 #						
 #						battery_saver_check()
 						if (txc):
+							sim_failure_check()
 #							output(pd, 1)
 							sleep(0.1) # add delay before transmit
 							output (ptt, 0)
@@ -472,6 +503,7 @@ if __name__ == "__main__":
 						print("Ready for next packet!")
 						
 					sleep(0.5)
+	
 				except:
 #					command_control_check()
 					sleep(1)
@@ -498,6 +530,7 @@ if __name__ == "__main__":
 							output(txLed, txLedOn)					
 	
 							if (txc):
+								sim_failure_check()
 #								output (pd, 1)
 								sleep(0.3)
 								output (ptt, 0)	
@@ -559,6 +592,7 @@ if __name__ == "__main__":
 #						battery_saver_check()
 
 						if (txc):
+							sim_failure_check()
 #							output(pd, 1)
 							output (ptt, 0)
 							system("aplay -D plughw:CARD=" + card + ",DEV=0 /home/pi/CubeSatSim/sstv_image_2_320_x_256.jpg.wav")
@@ -577,35 +611,7 @@ if __name__ == "__main__":
 					print("image 2 did not load - copy from CubeSatSim/sstv directory")
 				while 1:
 #					command_control_check()			
-					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)
-					
-#					command_control_check()			
-					
+					camera_photo()					
 					system("/home/pi/PiSSTVpp/pisstvpp -r 48000 -p s2 /home/pi/CubeSatSim/camera_out.jpg") 
 					system("sudo rm /home/pi/CubeSatSim/camera_out.jpg > /dev/null 2>&1") 
 
@@ -619,6 +625,7 @@ if __name__ == "__main__":
 #						battery_saver_check()
 
 						if (txc):
+							sim_failure_check()
 #							output(pd, 1)
 							output (ptt, 0)
 							system("aplay -D plughw:CARD=" + card + ",DEV=0 /home/pi/CubeSatSim/camera_out.jpg.wav")	
@@ -656,6 +663,7 @@ if __name__ == "__main__":
 #						battery_saver_check()
 
 						if (txc):
+							sim_failure_check()
 #							output(pd, 1)
 							output (ptt, 0)
 							system("aplay -D plughw:CARD=" + card + ",DEV=0 /home/pi/CubeSatSim/sstv_image_1_320_x_256.jpg.wav")
@@ -692,6 +700,7 @@ if __name__ == "__main__":
 #							battery_saver_check()
 
 							if (txc):
+								sim_failure_check()
 #								output(pd, 1)
 								output (ptt, 0)
 								system("aplay -D plughw:CARD=" + card + ",DEV=0 /home/pi/CubeSatSim/sstv_image_1_320_x_256.jpg.wav")
@@ -723,6 +732,7 @@ if __name__ == "__main__":
 #							battery_saver_check()
 						
 							if (txc):
+								sim_failure_check()
 #								output(pd, 1)
 								output (ptt, 0)		
 								system("aplay -D plughw:CARD=" + card + ",DEV=0 /home/pi/CubeSatSim/sstv.wav")
diff --git a/update b/update
index c2e2ffbe..8d316d44 100755
--- a/update
+++ b/update
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-echo -e "\nupdate script for CubeSatSim v2.1\n"
+echo -e "\nupdate script for CubeSatSim v2.2\n"
 
 FLAG=0
 checkout=0
@@ -263,7 +263,7 @@ if [ ! -d "/home/pi/fctelem" ]; then
 	mkdir /home/pi/fctelem/public_html
 	cd fctelem
 	wget https://github.com/alanbjohnston/go/releases/download/v0.2/fctelem.zip
-	unzip fctelem.zip
+	unzip -u fctelem.zip
 	FLAG=1
 elif [ ! -f "/home/pi/fctelem/v0.2" ]; then
 	echo "Updating fctelem binary to version v0.2 for FUNcube mode"
@@ -273,15 +273,61 @@ elif [ ! -f "/home/pi/fctelem/v0.2" ]; then
 	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
+	unzip -u fctelem.zip
 	FLAG=1
 fi
 
+if [ ! -f "/home/pi/MPU6050-C-CPP-Library-for-Raspberry-Pi/mpu6050" ]; then
+	echo "Installing MPU6050-C-CPP-Library-for-Raspberry-Pi"
+	sudo apt-get install -y libi2c-dev
+	cd
+	git clone https://github.com/alanbjohnston/MPU6050-C-CPP-Library-for-Raspberry-Pi.git
+	cd MPU6050-C-CPP-Library-for-Raspberry-Pi
+	sudo make install
+	make payload
+fi
+
+cd /home/pi/MPU6050-C-CPP-Library-for-Raspberry-Pi
+git checkout master
+git pull --no-rebase > .updated_p
+
+if [[ $(grep 'changed' /home/pi/MPU6050-C-CPP-Library-for-Raspberry-Pi/.updated_p) ]]; then
+
+	echo "updating MPU6050-C-CPP-Library-for-Raspberry-Pi"
+	sudo make install
+	make payload
+else
+	echo "nothing to do for MPU6050-C-CPP-Library-for-Raspberry-Pi."
+fi 
+
+if [ ! -f "/home/pi/raspberry-pi-bme280/bme280" ]; then
+	echo "Installing raspberry-pi-bme280"
+	sudo apt-get install -y libi2c-dev
+	cd
+	git clone https://github.com/alanbjohnston/raspberry-pi-bme280.git
+	cd raspberry-pi-bme280
+	git checkout payload
+	make 
+fi
+
+cd /home/pi/raspberry-pi-bme280
+git checkout payload
+git pull --no-rebase > .updated_p
+
+if [[ $(grep 'changed' /home/pi/raspberry-pi-bme280/.updated_p) ]]; then
+
+	echo "updating raspberry-pi-bme280"
+	make
+
+else
+	echo "nothing to do for raspberry-pi-bme280."
+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 -y
+	sudo apt-get install -y python3-smbus libusb-1.0 
 	cd
 	git clone https://github.com/csete/fcdctl.git
 	cd fcdctl
@@ -290,24 +336,22 @@ fi
 
 cd /home/pi/pi-power-button
 
-  git checkout master
-
   git pull --no-rebase > .updated_p
 
-  grep 'changed' /home/pi/pi-power-button/.updated_p
-  if [[ $(grep 'changed' /home/pi/pi-power-button/.updated_p) ]]; then
+  git checkout master
+
+#  grep 'changed' /home/pi/pi-power-button/.updated_p
+#  if [[ $(grep 'changed' /home/pi/pi-power-button/.updated_p) ]]; then
   
     echo "updating pi-power-button."
-
-#    git checkout master
     
     script/install
 
     FLAG=1
 
-  else
-    echo "nothing to do for pi-power-button."
-  fi 
+ # else
+ #   echo "nothing to do for pi-power-button."
+ # fi 
 
 cd /home/pi/ssdv