diff --git a/tracker/software/chconf.h b/tracker/software/chconf.h
index ad6b53a..1a2e626 100644
--- a/tracker/software/chconf.h
+++ b/tracker/software/chconf.h
@@ -58,7 +58,7 @@
  *          The value one is not valid, timeouts are rounded up to
  *          this value.
  */
-#define CH_CFG_ST_TIMEDELTA                 2
+#define CH_CFG_ST_TIMEDELTA                 0
 
 /** @} */
 
diff --git a/tracker/software/config.c b/tracker/software/config.c
index a52fe62..3cb5c13 100644
--- a/tracker/software/config.c
+++ b/tracker/software/config.c
@@ -1,14 +1,11 @@
-#include "config.h"
-#include "aprs.h"
-#include "debug.h"
-
-module_conf_t config[9];
-uint8_t ssdv_buffer[256*1024] __attribute__((aligned(32)));
-
 /* 
  * Position module configuration description
  * =========================================
  *
+ * This module activates the position transmission of the tracker. If one of the position modules activated, it will activate the tracking manager which
+ * samples GPS position and atmospheric data. If you want to save position data to the flash memory, you have to enable at lease one position module. You can
+ * transmit positions either using 2FSK (RTTY) with the UKHAS standard, CW (Morse) or APRS (AFSK or 2GFSK).
+ *
  * power				int(0-127)		Defines the radio power level. It ranges from 0 (low) to 127 (high). This value is sent into the Si4464 register. The
  * (required)							real output power is operation voltage dependent. The operation voltage can be set by RUN_3V in config.h. If USB is
  *										activated the operation voltage is 3V regardsless to which value RUN_3V is set. Running the PCB at 3V following table
@@ -52,6 +49,9 @@ uint8_t ssdv_buffer[256*1024] __attribute__((aligned(32)));
  *										  this option requires trigger.timeout to be set
  *										- TRIG_CONTINUOUSLY	Continue continuously (e.g. send new image once old image sent completely)
  *
+ * trigger.timeout		int				Amount of seconds of module cycle (in seconds). This option is only neccessary if trigger.type == TRIG_TIMEOUT.
+ * (default 0s)
+ *
  * ============================== The following options are needed if protocol == PROT_APRS_AFSK or protocol == PROT_APRS_2GFSK ===============================
  *
  * aprs_conf.callsign	string			Your amateur radio callsign (this requires an amateur radio license). This callsign will be used in the APRS protocol.
@@ -93,7 +93,7 @@ uint8_t ssdv_buffer[256*1024] __attribute__((aligned(32)));
  *
  * ============================================= The following options are needed if protocol == PROT_APRS_2GFSK ==============================================
  *
- * gfsk_conf.speed		int				2GFSK speed. Following values have been tested successfully: 9600, 19200, 38400, 76800, 96000.
+ * gfsk_conf.speed		int				2GFSK speed. Following values have been tested successfully: 9600, 19200.
  * (required)
  *
  * ============================================= The following options are needed if protocol == PROT_UKHAS_2FSK ==============================================
@@ -167,160 +167,336 @@ uint8_t ssdv_buffer[256*1024] __attribute__((aligned(32)));
  *										- <LOC>		Maidenhead locator
  */
 
+
+/*
+ * Image module configuration description
+ * ======================================
+ *
+ * This module activates the transmission of pictures from the camera. You can use either SSDV transsmitted with 2FSK (RTTY) or APRS/SSDV. If you choose
+ * APRS/SSDV, generic APRS packets will be transmitted which can be received by any APRS receiver and decoded with a computer. In order to decode 2FSK/SSDV
+ * please use DL-FlDigi (https://ukhas.org.uk/projects:dl-fldigi). 2FSK/SSDV is very stable even with low receiption signals but it needs a SSB receiver. If
+ * you want to use APRS/SSDV you are going to need a packet receiver (like TH-D72, TH-D74 or FT2D or generic TNC). The packets can be decoded by a python
+ * script in the /decoder folder of this repository.
+ *
+ * power				int(0-127)		Defines the radio power level. It ranges from 0 (low) to 127 (high). This value is sent into the Si4464 register. The
+ * (required)							real output power is operation voltage dependent. The operation voltage can be set by RUN_3V in config.h. If USB is
+ *										activated the operation voltage is 3V regardsless to which value RUN_3V is set. Running the PCB at 3V following table
+ *										applies:
+ *										20 dBm => 127
+ *										15 dBm => 40
+ *										10 dBm => 20
+ *										 5 dBm => 12
+ *										 0 dBm => 8
+ *
+ * protocol				prot_t			Possible Options:
+ * (required)							- PROT_APRS_AFSK	FM AFSK 1200baud transmission using the APRS/SSDV protocol
+ *										  this option requires aprs_conf to be set
+ * 										- PROT_APRS_2GFSK	2GFSK transmission using the APRS/SSDV protocol
+ *										  this option requires aprs_conf and gfsk_conf to be set
+ * 										- PROT_SSDV_2FSK	2FSK transmission using the SSDV protocol
+ *										  this option requires fsk_conf to be set
+ *
+ * frequency.type		freq_type_t		Defines the frequency type. This option will be FREQ_STATIC if not set.
+ * (default FREQ_STATIC)				Possible Options:
+ *										- FREQ_STATIC		Static frequency taken from frequency.hz
+ *										- FREQ_APRS_REGION	Using the APRS region frequency. The tracker will change its frequency specificly to the region
+ *															where it is located. e.g. 144.8MHz in Europe or 144.39MHz in the US. If the tracker doesnt know its
+ *															position it takes its frequency from frequency.hz as default. Note that the tracker knows its
+ *															position from its position log too. So it might use the last frequency which has been used before
+ *															resetting it.
+ *
+ * frequency.hz			int				Frequency that this Module will transmit on (in Hz). The tracker can transmit in the 2m band. This value will be used
+ * (required)							as default when frequency.type == FREQ_APRS_REGION and it doesnt know its position
+ *
+ * init_delay			int				Initial delay (in ms) before the module starts. This might be useful if you dont want to transmit so many APRS packets
+ * (default 0ms)						at the same time on the APRS network. This option is optional. It will be 0ms if not set.
+ *
+ * trigger.type			trigger_type_t	Event at which this module is triggered to transmit. This option will be TRIG_ONCE if not set.
+ * (default TRIG_ONCE)					Possible options:
+ *										- TRIG_ONCE			Trigger once and never again (e.g. transmit specific position packet only at startup)
+ *										- TRIG_NEW_POINT	Triggered when new track point available
+ *										- TRIG_TIMEOUT		Triggered by timeout (e.g. trasmit position every 120sec)
+ *										  this option requires trigger.timeout to be set
+ *										- TRIG_CONTINUOUSLY	Continue continuously (e.g. send new image once old image sent completely)
+ *
+ * trigger.timeout		int				Amount of seconds of module cycle (in seconds). This option is only neccessary if trigger.type == TRIG_TIMEOUT.
+ * (default 0s)
+ *
+ * ssdv_conf.callsign	string			The SSDV callsign (or stream identifier). This value helps the SSDV algorithm to assign packets from different images
+ * (required)							to the right data set. This is helpful if multiple modules transmit different images at the same time.
+ *
+ * ssdv_conf.ram_buffer	data			Array of bytes which is used by the module for buffering the image
+ * (required)
+ *
+ * ssdv_conf.ram_size					Size of buffer => sizeof(ssdv_conf.ram_buffer)
+ * (required)
+ *
+ * ssdv_conf.res		resolution_t	Resolution of the image
+ * (default RES_QVGA)					Possible options:
+ *										- RES_QVGA	QVGA Resolution (320x240px)
+ *										- RES_VGA	VGA Resolution (640x480px)
+ *										- RES_XGA	XGA Resolution (1204x768px)
+ *										- RES_UXGA	UXGA Resolution (1600x1200px)
+ *										- RES_MAX	The module samples the highest resolution which fits into ssdv_conf.ram_buffer.
+ *
+ * ssdv_conf.redundantTx	bool		Enables redudant packet transmission if set to true. This option will enable the packets to be transmitted twice.
+ *
+ * ssdv_conf.quality	int(0-7)		Quality (quantization) of the JPEG algorithm. It can be set from 0 (low quality) to 7 (high quality). (Recommended: 4)
+ *
+ * ============================== The following options are needed if protocol == PROT_APRS_AFSK or protocol == PROT_APRS_2GFSK ===============================
+ *
+ * aprs_conf.callsign	string			Your amateur radio callsign (this requires an amateur radio license). This callsign will be used in the APRS protocol.
+ * (required)							You can transmit on the 70cm band without a license but the transmitter would need a 70cm LPF therefore.
+ *
+ * aprs_conf.ssid		int(0-15)		APRS SSID (no SSID = 0)
+ * (default 0)
+ *
+ * aprs_conf.path		string			APRS digipeating path (default: no digipeating)
+ * (optional)
+ *
+ * aprs_conf.preamble	int				AFSK or 2GFSK preamble length (in ms). This value is required while its default is 0ms (and this would simply not work ;-) )
+ * (required)
+ *
+ * ============================================= The following options are needed if protocol == PROT_APRS_2GFSK ==============================================
+ *
+ * gfsk_conf.speed		int				2GFSK speed. Following values have been tested successfully: 9600, 19200.
+ * (required)
+ *
+ * ============================================== The following options are needed if protocol == PROT_SSDV_2FSK ==============================================
+ *
+ * fsk_conf.bits		int(7-8)		Bits
+ * (required)
+ *
+ * fsk_conf.stopbits	int(1-2)		Stopbits (Important: DL-FlDigi can decode SSDV only using 2 stopbits!)
+ * (required)
+ *
+ * fsk_conf.predelay	int				Predelay (in ms). The receiver needs to settle on the frequency for a while. Therefore is switched on some seconds
+ * (default 0ms)						before. By default its 0ms but that wouldnt work.
+ *
+ * fsk_conf.baud		int				Baudrate. Following values have been tested successfully: 50, 300, 600. (Recommended: 300 or 600)
+ * (required)
+ *
+ * fsk_conf.shift		int				Frequency shift of 2FSK
+ * (required)
+ */
+
+
+/* 
+ * Log module configuration description
+ * ====================================
+ *
+ * This module activates log (track point) transmission over APRS (AFSK or 2GFSK). This module can be used to receive log points while the tracker has been out
+ * of range from the APRS network.
+ * Note: Track points are saved on the flash memory without activation of the Log module. They are just not sent out.
+ *
+ * power				int(0-127)		Defines the radio power level. It ranges from 0 (low) to 127 (high). This value is sent into the Si4464 register. The
+ * (required)							real output power is operation voltage dependent. The operation voltage can be set by RUN_3V in config.h. If USB is
+ *										activated the operation voltage is 3V regardsless to which value RUN_3V is set. Running the PCB at 3V following table
+ *										applies:
+ *										20 dBm => 127
+ *										15 dBm => 40
+ *										10 dBm => 20
+ *										 5 dBm => 12
+ *										 0 dBm => 8
+ *
+ * protocol				prot_t			Possible Options:
+ * (required)							- PROT_APRS_AFSK	FM AFSK 1200baud transmission using the APRS protocol
+ *										  this option requires aprs_conf to be set
+ * 										- PROT_APRS_2GFSK	2GFSK transmission using the APRS protocol
+ *										  this option requires aprs_conf and gfsk_conf to be set
+ *
+ * frequency.type		freq_type_t		Defines the frequency type. This option will be FREQ_STATIC if not set.
+ * (default FREQ_STATIC)				Possible Options:
+ *										- FREQ_STATIC		Static frequency taken from frequency.hz
+ *										- FREQ_APRS_REGION	Using the APRS region frequency. The tracker will change its frequency specificly to the region
+ *															where it is located. e.g. 144.8MHz in Europe or 144.39MHz in the US. If the tracker doesnt know its
+ *															position it takes its frequency from frequency.hz as default. Note that the tracker knows its
+ *															position from its position log too. So it might use the last frequency which has been used before
+ *															resetting it.
+ *
+ * frequency.hz			int				Frequency that this Module will transmit on (in Hz). The tracker can transmit in the 2m band. This value will be used
+ * (required)							as default when frequency.type == FREQ_APRS_REGION and it doesnt know its position
+ *
+ * init_delay			int				Initial delay (in ms) before the module starts. This might be useful if you dont want to transmit so many APRS packets
+ * (default 0ms)						at the same time on the APRS network. This option is optional. It will be 0ms if not set.
+ *
+ * trigger.type			trigger_type_t	Event at which this module is triggered to transmit. This option will be TRIG_ONCE if not set.
+ * (default TRIG_ONCE)					Possible options:
+ *										- TRIG_ONCE			Trigger once and never again (e.g. transmit specific position packet only at startup)
+ *										- TRIG_NEW_POINT	Triggered when new track point available
+ *										- TRIG_TIMEOUT		Triggered by timeout (e.g. trasmit position every 120sec)
+ *										  this option requires trigger.timeout to be set
+ *										- TRIG_CONTINUOUSLY	Continue continuously (e.g. send new image once old image sent completely)
+ *
+ * trigger.timeout		int				Amount of seconds of module cycle (in seconds). This option is only neccessary if trigger.type == TRIG_TIMEOUT.
+ * (default 0s)
+ *
+ * aprs_conf.callsign	string			Your amateur radio callsign (this requires an amateur radio license). This callsign will be used in the APRS protocol.
+ * (required)							You can transmit on the 70cm band without a license but the transmitter would need a 70cm LPF therefore.
+ *
+ * aprs_conf.ssid		int(0-15)		APRS SSID (no SSID = 0)
+ * (default 0)
+ *
+ * aprs_conf.path		string			APRS digipeating path (default: no digipeating)
+ * (optional)
+ *
+ * aprs_conf.preamble	int				AFSK or 2GFSK preamble length (in ms). This value is required while its default is 0ms (and this would simply not work ;-) )
+ * (required)
+ *
+ * ============================================= The following options are needed if protocol == PROT_APRS_2GFSK ==============================================
+ *
+ * gfsk_conf.speed		int				2GFSK speed. Following values have been tested successfully: 9600, 19200.
+ * (required)
+ */
+
+
 // Put your configuration settings here
+
+// Global variables
+#include "config.h"
+#include "aprs.h"
+#include "debug.h"
+
+module_conf_t config[7];
+
+uint8_t ssdv_buffer[128*1024] __attribute__((aligned(32)));	// Image buffer
+
+systime_t track_cycle_time = S2ST(60);						// Tracking cycle (all peripheral data [airpressure, GPS, temperature, ...] is collected each 60 seconds
+systime_t log_cycle_time = S2ST(600);						// Log cycle time in seconds (600 seconds)
+bool keep_cam_switched_on =	false;							// Keep camera switched on and initialized after it has been switched on once, no configuration change is possible
+uint16_t gps_on_vbat = 3000;								// Battery voltage threshold at which GPS is switched on
+uint16_t gps_off_vbat = 2500;								// Battery voltage threshold at which GPS is switched off
+
 void start_user_modules(void)
 {
+	/*
+	 * Use one of these example blocks by uncommenting the start_*_thread() line.
+	 */
+
+	/* -------------------------------------------------- POSITION TRANSMISSION -------------------------------------------------- */
+
 	// Module POSITION, APRS 2m AFSK
-	config[0].power = 127;									// Power 20 dBm
-	config[0].protocol = PROT_APRS_AFSK;					// Protocol APRS, modulation AFSK
+	config[0].power = 127;									// Transmission Power
+	config[0].protocol = PROT_APRS_AFSK;					// Protocol APRS (AFSK)
 	config[0].frequency.type = FREQ_APRS_REGION;			// Dynamic frequency allocation
 	config[0].frequency.hz = 144800000;						// Default frequency 144.800 MHz
-	config[0].init_delay = 0;								// Module startup delay in msec
-	config[0].trigger.type = TRIG_NEW_POINT;				// Trigger when new track point released
-	chsnprintf(config[0].aprs_conf.callsign, 7, "DL7AD");	// APRS Callsign
+	config[0].trigger.type = TRIG_NEW_POINT;				// Transmit when tracking manager samples new tracking point
+	chsnprintf(config[0].aprs_conf.callsign, 16, "DL7AD");	// APRS Callsign
 	config[0].aprs_conf.ssid = 12;							// APRS SSID
 	config[0].aprs_conf.symbol = SYM_BALLOON;				// APRS Symbol
 	chsnprintf(config[0].aprs_conf.path, 16, "WIDE1-1");	// APRS Path
-	config[0].aprs_conf.preamble = 300;						// APRS Preamble
-	config[0].aprs_conf.tel[0] = TEL_VBAT;					// APRS Telemetry parameter 1
-	config[0].aprs_conf.tel[1] = TEL_PBAT;					// APRS Telemetry parameter 2
-	config[0].aprs_conf.tel[2] = TEL_RBAT;					// APRS Telemetry parameter 3
-	config[0].aprs_conf.tel[3] = TEL_TEMP;					// APRS Telemetry parameter 4
-	config[0].aprs_conf.tel[4] = TEL_HUM;					// APRS Telemetry parameter 5
+	config[0].aprs_conf.preamble = 300;						// APRS Preamble (300ms)
+	config[0].aprs_conf.tel[0] = TEL_VBAT;					// APRS Telemetry parameter 1: Battery voltage
+	config[0].aprs_conf.tel[1] = TEL_PBAT;					// APRS Telemetry parameter 2: Battery charge/discharge power
+	config[0].aprs_conf.tel[2] = TEL_RBAT;					// APRS Telemetry parameter 3: Battery impedance
+	config[0].aprs_conf.tel[3] = TEL_TEMP;					// APRS Telemetry parameter 4: Temperature
+	config[0].aprs_conf.tel[4] = TEL_PRESS;					// APRS Telemetry parameter 5: Airpressuse
 	config[0].aprs_conf.tel_enc = TRUE;						// Transmit Telemetry encoding information activated
 	config[0].aprs_conf.tel_enc_cycle = 3600;				// Transmit Telemetry encoding information every 3600sec
 	chsnprintf(config[0].aprs_conf.tel_comment, 30, "http://ssdv.habhub.org/DL7AD");// Telemetry comment
 	//start_position_thread(&config[0]);
 
-	// Module POSITION, APRS 2m 2GFSK
-	/*config[1].power = 127;								// Power 10 dBm
-	config[1].protocol = PROT_APRS_2GFSK;					// Protocol APRS, modulation 2GFSK
-	config[1].gfsk_conf.speed = 9600;						// 2GFSK Speed
-	config[1].frequency.type = FREQ_STATIC;					// Static frequency allocation
-	config[1].frequency.hz = 144860000;						// Default frequency 144.860 MHz
-	config[1].init_delay = 0;								// Module startup delay in msec
-	config[1].trigger.type = TRIG_NEW_POINT;				// Trigger when new track point released
-	chsnprintf(config[1].aprs_conf.callsign, 7, "DL4MDW");	// APRS Callsign
-	config[1].aprs_conf.ssid = 11;							// APRS SSID
-	config[1].aprs_conf.symbol = SYM_BALLOON;				// APRS Symbol
-	chsnprintf(config[1].aprs_conf.path, 16, "WIDE1-1");	// APRS Path
-	config[1].aprs_conf.preamble = 40;						// APRS Preamble
-	config[0].aprs_conf.tel[0] = TEL_VBAT;					// APRS Telemetry parameter 1
-	config[0].aprs_conf.tel[1] = TEL_VSOL;					// APRS Telemetry parameter 2
-	config[0].aprs_conf.tel[2] = TEL_PRESS;					// APRS Telemetry parameter 3
-	config[0].aprs_conf.tel[3] = TEL_TEMP;					// APRS Telemetry parameter 4
-	config[0].aprs_conf.tel[4] = TEL_PBAT;					// APRS Telemetry parameter 5
-	config[1].aprs_conf.tel_enc = TRUE;				// Transmit Telemetry encoding information enabled
-	config[1].aprs_conf.tel_enc_cycle = 3600;			// Transmit Telemetry encoding information every 3600sec
-	chsnprintf(config[1].aprs_conf.tel_comment, 18, "http://tkrahn.net");// Telemetry comment
-	start_position_thread(&config[1]);*/
-
 	// Module POSITION, UKHAS 2m 2FSK
-	/*config[2].power = 127;								// Power 10 dBm
-	config[2].protocol = PROT_UKHAS_2FSK;					// Protocol UKHAS, modulation 2FSK
+	config[1].power = 127;									// Transmission Power
+	config[1].protocol = PROT_UKHAS_2FSK;					// Protocol UKHAS (2FSK)
+	config[1].frequency.type = FREQ_STATIC;					// Static frequency allocation
+	config[1].frequency.hz = 144160000;						// Transmission frequency 144.160 MHz
+	config[1].trigger.type = TRIG_CONTINUOUSLY;				// Transmit continuously
+	config[1].fsk_conf.bits = 8;							// 8 bit
+	config[1].fsk_conf.stopbits = 2;						// 2 stopbits
+	config[1].fsk_conf.predelay = 1000;						// Preamble (1000ms)
+	config[1].fsk_conf.baud = 50;							// Baudrate
+	config[1].fsk_conf.shift = 425;							// Frequency shift in Hz
+	chsnprintf(config[1].ukhas_conf.callsign, 16, "DL7AD");	// UKHAS Callsign
+	chsnprintf(config[1].ukhas_conf.format, 150, "<CALL>,<ID>,<TIME>,<LAT>,<LON>,<ALT>,<SATS>,<TTFF>,<VBAT>,<PBAT>,<PRESS>,<TEMP>,<HUM>"); // UKHAS Format
+	//start_position_thread(&config[1]);
+
+	// Module POSITION, Morse 2m OOK
+	config[2].power = 127;									// Transmission Power
+	config[2].protocol = PROT_MORSE;						// Protocol Morse (OOK)
 	config[2].frequency.type = FREQ_STATIC;					// Static frequency allocation
-	config[2].frequency.hz = 144860000;						// Transmission frequency 144.860 MHz
-	config[2].init_delay = 2000;							// Module startup delay in msec
-	config[2].trigger.type = TRIG_NEW_POINT;				// Trigger when new track point released
-	config[2].fsk_conf.bits = 8;							// 8bit
-	config[2].fsk_conf.stopbits = 2;						// 2 Stopbits
-	config[2].fsk_conf.predelay = 1000;						// Predelay in ms (continuos carrier before actual transmission)
-	config[2].fsk_conf.baud = 600;							// Baudrate
-	config[2].fsk_conf.shift = 1000;						// Frequency shift in Hz
-	chsnprintf(config[2].ukhas_conf.callsign, 6, "DK0TU");	// UKHAS Callsign
-	chsnprintf(config[2].ukhas_conf.format, 94, "<CALL>,<ID>,<TIME>,<LAT>,<LON>,<ALT>,<SATS>,<TTFF>,<VBAT>,<PBAT>,<PRESS>,<TEMP>,<HUM>"); // UKHAS Format
-	start_position_thread(&config[2]);*/
+	config[2].frequency.hz = 144160000;						// Transmission frequency 144.160 MHz
+	config[2].trigger.type = TRIG_TIMEOUT;					// Periodic cycling (every 60 seconds)
+	config[2].trigger.timeout = 60;							// Timeout 60 sec
+	config[2].ook_conf.speed = 20;							// Speed 20 wpm
+	chsnprintf(config[2].morse_conf.callsign, 16, "DL7AD");	// Morse Callsign
+	chsnprintf(config[2].morse_conf.format, 50, "BALLOON <CALL> <LOC> <ALT>M"); // Morse Format
+	//start_position_thread(&config[2]);
+
+
+
+	/* ---------------------------------------------------- IMAGE TRANSMISSION --------------------------------------------------- */
 
 	// Module IMAGE, APRS 2m AFSK low-duty cycle
-	config[3].power = 127;									// Power 20 dBm
-	config[3].protocol = PROT_APRS_AFSK;					// Protocol APRS SSDV, modulation AFSK
+	config[3].power = 127;									// Transmission Power
+	config[3].protocol = PROT_APRS_AFSK;					// Protocol APRS/SSDV (AFSK)
 	config[3].frequency.type = FREQ_APRS_REGION;			// Dynamic frequency allocation
 	config[3].frequency.hz = 144800000;						// Transmission frequency 144.800 MHz
-	config[3].init_delay = 0;								// Module startup delay in msec
 	config[3].packet_spacing = 20000;						// Packet spacing in ms
-	//config[3].sleep_conf.type = SLEEP_WHEN_ISOL_BELOW_THRES;
-	//config[3].sleep_conf.isol_thres = 3;
-	config[3].trigger.type = TRIG_CONTINUOUSLY;				// Continuous Trigger
-	config[3].trigger.timeout = 10;							// Timeout 10 sec
-	chsnprintf(config[3].aprs_conf.callsign, 7, "DL7AD");	// APRS Callsign
+	config[3].trigger.type = TRIG_CONTINUOUSLY;				// Transmit continuously
+	chsnprintf(config[3].aprs_conf.callsign, 16, "DL7AD");	// APRS Callsign
 	config[3].aprs_conf.ssid = 12;							// APRS SSID
-	config[3].aprs_conf.preamble = 300;						// APRS Preamble
-	chsnprintf(config[3].ssdv_conf.callsign, 7, "DL7AD");	// SSDV Callsign
+	config[3].aprs_conf.preamble = 300;						// APRS Preamble (300ms)
+	chsnprintf(config[3].ssdv_conf.callsign, 7, "DL7AD1");	// SSDV Callsign
 	config[3].ssdv_conf.ram_buffer = ssdv_buffer;			// Camera buffer
 	config[3].ssdv_conf.ram_size = sizeof(ssdv_buffer);		// Buffer size
 	config[3].ssdv_conf.res = RES_QVGA;						// Resolution QVGA
-	config[3].ssdv_conf.redundantTx = true;					// Transmit packets twice
-	config[3].ssdv_conf.quality = 7;						// High quality
+	config[3].ssdv_conf.redundantTx = true;					// Redundant transmission (transmit packets twice)
+	config[3].ssdv_conf.quality = 4;						// Image quality
 	//start_image_thread(&config[3]);
 
-	// Module POSITION, Morse 2m OOK
-	/*config[4].power = 127;								// Power 10 dBm
-	config[4].protocol = PROT_MORSE;						// Protocol Morse, modulation OOK
-	config[4].frequency.type = FREQ_STATIC;					// Static frequency allocation
-	config[4].frequency.hz = 144857400;						// Transmission frequency 144.8574 MHz
-	config[4].init_delay = 20000;							// Module startup delay in msec
-	config[4].trigger.type = TRIG_TIMEOUT;					// Trigger transmission on timeout (Periodic cycling)
-	config[4].trigger.timeout = 1200;						// Timeout 1200 sec
-	config[4].ook_conf.speed = 20;							// Speed 20wpm
-	chsnprintf(config[4].morse_conf.callsign, 6, "DK0TU");	// Morse Callsign
-	chsnprintf(config[4].morse_conf.format, 43, "BALLOON <CALL> <LOC> <ALT>M WWW.TKRAHN.NET"); // Morse Format
-	start_position_thread(&config[4]);*/
-
 	// Module IMAGE, APRS 2m 2GFSK
-	config[5].power = 127;									// Power 20 dBm
-	config[5].protocol = PROT_APRS_2GFSK;					// Protocol APRS SSDV, modulation 2GFSK
-	config[5].gfsk_conf.speed = 9600;						// 2GFSK Speed
-	config[5].frequency.type = FREQ_STATIC;					// Static frequency allocation
-	config[5].frequency.hz = 144860000;						// Transmission frequency 144.860 MHz
-	//config[5].init_delay = 60000;							// Module startup delay in msec
-	//config[5].sleep_conf.type = SLEEP_WHEN_VBAT_BELOW_THRES;
-	//config[5].sleep_conf.vbat_thres = 4000;
-	config[5].trigger.type = TRIG_CONTINUOUSLY;					// Trigger transmission on timeout (Periodic cycling)
-	config[5].trigger.timeout = 30;							// Timeout 10 sec
-	chsnprintf(config[5].aprs_conf.callsign, 6, "DL7AD");	// APRS Callsign
-	config[5].aprs_conf.ssid = 12;							// APRS SSID
-	config[5].aprs_conf.preamble = 100;						// APRS Preamble
-	chsnprintf(config[5].ssdv_conf.callsign, 7, "DL7AD2");	// SSDV Callsign
-	config[5].ssdv_conf.ram_buffer = ssdv_buffer;			// Camera buffer
-	config[5].ssdv_conf.ram_size = sizeof(ssdv_buffer);		// Buffer size
-	config[5].ssdv_conf.res = RES_VGA;						// Resolution VGA
-	config[5].ssdv_conf.quality = 7;						// High quality
-	start_image_thread(&config[5]);
+	config[4].power = 127;									// Transmission Power
+	config[4].protocol = PROT_APRS_2GFSK;					// Protocol APRS/SSDV (2GFSK)
+	config[4].gfsk_conf.speed = 9600;						// 2GFSK Speed
+	config[4].frequency.type = FREQ_STATIC;					// Static frequency allocation
+	config[4].frequency.hz = 144860000;						// Transmission frequency 144.860 MHz
+	config[4].trigger.type = TRIG_CONTINUOUSLY;				// Transmit continuously
+	chsnprintf(config[4].aprs_conf.callsign, 16, "DL7AD");	// APRS Callsign
+	config[4].aprs_conf.ssid = 12;							// APRS SSID
+	config[4].aprs_conf.preamble = 100;						// APRS Preamble (100ms)
+	chsnprintf(config[4].ssdv_conf.callsign, 7, "DL7AD2");	// SSDV Callsign
+	config[4].ssdv_conf.ram_buffer = ssdv_buffer;			// Camera buffer
+	config[4].ssdv_conf.ram_size = sizeof(ssdv_buffer);		// Buffer size
+	config[4].ssdv_conf.res = RES_VGA;						// Resolution VGA
+	config[4].ssdv_conf.quality = 4;						// Image quality
+	start_image_thread(&config[4]);
 
 	// Module IMAGE, SSDV 2m 2FSK
-	/*config[6].power = 127;								// Power 20 dBm
-	config[6].protocol = PROT_SSDV_2FSK;					// Protocol SSDV, modulation 2FSK
-	config[6].frequency.type = FREQ_STATIC;					// Static frequency allocation
-	config[6].frequency.hz = 144860000;						// Transmission frequency 144.860 MHz
-	config[6].init_delay = 30000;							// Module startup delay in msec
-	config[6].packet_spacing = 6500;						// Packet spacing in ms
-	config[6].trigger.type = TRIG_TIMEOUT;					// Trigger transmission on timeout (Periodic cycling)
-	config[6].trigger.timeout = 1200;						// Transmit every 900 sec
-	config[6].fsk_conf.bits = 8;							// 8bit
-	config[6].fsk_conf.stopbits = 2;						// 2 Stopbits
-	config[6].fsk_conf.predelay = 100;						// Predelay in ms (continuos carrier before actual transmission)
-	config[6].fsk_conf.baud = 600;							// Baudrate
-	config[6].fsk_conf.shift = 1000;						// Frequency shift in Hz
-	chsnprintf(config[6].ssdv_conf.callsign, 6, "DK0TU");	// SSDV Callsign
-	config[6].ssdv_conf.ram_buffer = ssdv_buffer;			// Camera buffer
-	config[6].ssdv_conf.ram_size = sizeof(ssdv_buffer);		// Buffer size
-	config[6].ssdv_conf.res = RES_VGA;						// Resolution VGA
-	config[6].ssdv_conf.quality = 7;						// High quality
-	start_image_thread(&config[6]);*/
+	config[5].power = 127;									// Transmission Power
+	config[5].protocol = PROT_SSDV_2FSK;					// Protocol SSDV (2FSK)
+	config[5].frequency.type = FREQ_STATIC;					// Static frequency allocation
+	config[5].frequency.hz = 144160000;						// Transmission frequency 144.160 MHz
+	config[5].trigger.type = TRIG_CONTINUOUSLY;				// Transmit continuously
+	config[5].fsk_conf.bits = 8;							// 8bit
+	config[5].fsk_conf.stopbits = 2;						// 2 Stopbits
+	config[5].fsk_conf.predelay = 1000;						// Preamble (1000ms)
+	config[5].fsk_conf.baud = 600;							// Baudrate (600baud)
+	config[5].fsk_conf.shift = 1000;						// Frequency shift (1000Hz)
+	chsnprintf(config[5].ssdv_conf.callsign, 7, "DL7AD3");	// SSDV Callsign
+	config[5].ssdv_conf.ram_buffer = ssdv_buffer;			// Camera buffer
+	config[5].ssdv_conf.ram_size = sizeof(ssdv_buffer);		// Buffer size
+	config[5].ssdv_conf.res = RES_QVGA;						// Resolution QVGA
+	config[5].ssdv_conf.quality = 4;						// Image quality
+	//start_image_thread(&config[5]);
+
+
+
+	/* ----------------------------------------------------- LOG TRANSMISSION ---------------------------------------------------- */
 
 	// Module LOG, APRS 2m AFSK
-	/*config[8].power = 127;									// Power 20 dBm
-	config[8].protocol = PROT_APRS_AFSK;					// Protocol APRS, modulation AFSK
-	config[8].frequency.type = FREQ_APRS_REGION;			// Dynamic frequency allocation
-	config[8].frequency.hz = 144800000;						// Default frequency 144.800 MHz
-	config[8].init_delay = 10000;							// Module startup delay in msec
-	config[8].trigger.type = TRIG_TIMEOUT;					// Trigger transmission on timeout (Periodic cycling)
-	config[8].trigger.timeout = 180;						// Timeout 600 sec
-	chsnprintf(config[8].aprs_conf.callsign, 7, "DL4MDW");	// APRS Callsign
-	config[8].aprs_conf.ssid = 12;							// APRS SSID
-	chsnprintf(config[8].aprs_conf.path, 16, "WIDE1-1");	// APRS Path
-	config[8].aprs_conf.preamble = 300;						// APRS Preamble
-	start_logging_thread(&config[8]);*/
+	config[6].power = 127;									// Transmission Power
+	config[6].protocol = PROT_APRS_AFSK;					// Protocol APRS (AFSK)
+	config[6].frequency.type = FREQ_APRS_REGION;			// Dynamic frequency allocation
+	config[6].frequency.hz = 144800000;						// Default frequency 144.800 MHz
+	config[6].init_delay = 60000;							// Module startup delay (60 seconds)
+	config[6].trigger.type = TRIG_TIMEOUT;					// Periodic cycling (every 180 seconds)
+	config[6].trigger.timeout = 180;						// Timeout 180 sec
+	chsnprintf(config[6].aprs_conf.callsign, 16, "DL7AD");	// APRS Callsign
+	config[6].aprs_conf.ssid = 12;							// APRS SSID
+	chsnprintf(config[6].aprs_conf.path, 16, "WIDE1-1");	// APRS Path
+	config[6].aprs_conf.preamble = 300;						// APRS Preamble (300ms)
+	//start_logging_thread(&config[6]);
 }
 
 
diff --git a/tracker/software/config.h b/tracker/software/config.h
index 600a390..a11012b 100644
--- a/tracker/software/config.h
+++ b/tracker/software/config.h
@@ -8,16 +8,10 @@
 #include "radio.h"
 #include "sleep.h"
 
-#define TRACK_CYCLE_TIME		60			/* Tracking cycle (all peripheral data [airpressure, GPS, temperature, ...] is collected each x seconds */
-
-#define LOG_CYCLE_TIME			60			/* Log cycle time in seconds */
 #define LOG_FLASH_ADDR1			0x080C0000	/* Log flash memory address 1 */
 #define LOG_FLASH_ADDR2			0x080E0000	/* Log flash memory address 2 */
 #define LOG_SECTOR_SIZE			0x20000		/* Log flash memory size */
 
-#define GPS_ON_VBAT				3000		/* Battery voltage threshold at which GPS is switched on */
-#define GPS_OFF_VBAT			2500		/* Battery voltage threshold at which GPS is switched off */
-
 #define TRACE_TIME				TRUE		/* Enables time tracing on debugging port */
 #define TRACE_FILE				FALSE		/* Enables file and line tracing on debugging port */
 
@@ -25,10 +19,15 @@
 											 * With 1.8V only 15dBm can be done. Some serial-USB adapters also need a 3V IO level in
 											 * order to work. However 3V takes a lot of power in idle. You can save energy using 1.8V. */
 
-#define KEEP_CAM_SWITCHED_ON	TRUE		/* Keep camera switched on and initialized after it has been switched on once, no configuration change is possible */
-
 void start_user_modules(void);
-extern module_conf_t config[9];
+
+extern module_conf_t config[7];
+
+extern systime_t track_cycle_time;
+extern systime_t log_cycle_time;
+extern bool keep_cam_switched_on;
+extern uint16_t gps_on_vbat;
+extern uint16_t gps_off_vbat;
 
 #endif
 
diff --git a/tracker/software/drivers/ov5640.c b/tracker/software/drivers/ov5640.c
index 2de896b..4707b25 100644
--- a/tracker/software/drivers/ov5640.c
+++ b/tracker/software/drivers/ov5640.c
@@ -96,7 +96,7 @@ static const struct regval_list OV5640YUV_Sensor_Dvp_Init[] =
 	{ 0x3034, 0x1a }, 
 	{ 0x3035, 0x11 }, //15fps
 	{ 0x3036, 0x46 }, 
-	{ 0x3037, 0x12 }, 
+	{ 0x3037, 0x14 }, 
 	{ 0x3038, 0x00 }, 
 	{ 0x3039, 0x00 }, 
 
@@ -774,7 +774,7 @@ static bool analyze_image(uint8_t *image, uint32_t image_len)
 	uint32_t bi = 0;
 	uint8_t c = SSDV_OK;
 
-	ssdv_enc_init(&ssdv, SSDV_TYPE_NORMAL, "", 0, 0);
+	ssdv_enc_init(&ssdv, SSDV_TYPE_NORMAL, "", 0, 7);
 	ssdv_enc_set_buffer(&ssdv, pkt);
 
 	while(true) // FIXME: I get caught in these loops occasionally and never return
@@ -785,16 +785,22 @@ static bool analyze_image(uint8_t *image, uint32_t image_len)
 			uint8_t r = bi < image_len-128 ? 128 : image_len - bi;
 			bi += r;
 			if(r <= 0)
+			{
+				TRACE_ERROR("CAM  > Error in image");
 				return false;
+			}
 			ssdv_enc_feed(&ssdv, b, r);
 		}
 
 		if(c == SSDV_EOI) // End of image
 			return true;
 		if(c != SSDV_OK) // Error in JPEG image
+		{
+			TRACE_ERROR("CAM  > Error in image");
 			return false;
+		}
 
-		chThdSleepMilliseconds(10);
+		chThdSleepMilliseconds(5);
 	}
 }
 
@@ -814,7 +820,9 @@ bool OV5640_Snapshot2RAM(void)
 	do {
 
 		TRACE_INFO("CAM  > Capture image");
+		lockRadio(); // Lock radio because SPI and pseudo DCMI use the same DMA
 		status = OV5640_Capture();
+		unlockRadio(); // Unlock radio
 		TRACE_INFO("CAM  > Capture finished");
 
 		ov5640_conf->size_sampled = ov5640_conf->ram_size - 1;
@@ -1072,6 +1080,7 @@ CH_IRQ_HANDLER(Vector5C) {
 
 bool OV5640_Capture(void)
 {
+
 	/*
 	 * Note:
 	 *  If there are no Chibios devices enabled that use DMA then...
@@ -1210,7 +1219,7 @@ bool OV5640_Capture(void)
 	capture_finished = false;
 	vsync = false;
 
-	// Setup EXTI: EXTI1 PC for PC1 (VSYNC) and EXIT2 PC for PC2 (HREF)
+	// Setup EXTI: EXTI1 PC for PC1 (VSYNC)
 	SYSCFG->EXTICR[0] |= SYSCFG_EXTICR1_EXTI1_PC;
 	EXTI->IMR = EXTI_IMR_MR1; // Activate interrupt for chan1 (=>PC1) and chan2 (=>PC2)
 	EXTI->RTSR = EXTI_RTSR_TR1; // Listen on rising edge
@@ -1235,6 +1244,7 @@ bool OV5640_Capture(void)
 	}
 
     TRACE_INFO("CAM  > Capture success");
+
 	return true;
 }
 
diff --git a/tracker/software/drivers/pac1720.c b/tracker/software/drivers/pac1720.c
index e3eea11..62e9aeb 100644
--- a/tracker/software/drivers/pac1720.c
+++ b/tracker/software/drivers/pac1720.c
@@ -152,7 +152,7 @@ void pac1720_init(void)
 	I2C_write8(PAC1720_ADDRESS, PAC1720_CH2_VSENSE_SAMP_CONFIG, 0x5F);
 	I2C_write8(PAC1720_ADDRESS, PAC1720_V_SOURCE_SAMP_CONFIG,   0xFF);
 
-	TRACE_INFO("PAC  > Init PAC1720 continuous measurement");
-	chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(256), "PAC1720", NORMALPRIO, pac1720_thd, NULL);
+	//TRACE_INFO("PAC  > Init PAC1720 continuous measurement");
+	//chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(512), "PAC1720", NORMALPRIO, pac1720_thd, NULL);
 }
 
diff --git a/tracker/software/drivers/si4464.c b/tracker/software/drivers/si4464.c
index ab52fc1..7f9cabe 100644
--- a/tracker/software/drivers/si4464.c
+++ b/tracker/software/drivers/si4464.c
@@ -14,7 +14,7 @@
 static const SPIConfig ls_spicfg = {
 	.ssport	= PAL_PORT(LINE_RADIO_CS),
 	.sspad	= PAL_PAD(LINE_RADIO_CS),
-	.cr1	= SPI_CR1_MSTR | SPI_CR1_BR_0
+	.cr1	= SPI_CR1_MSTR
 };
 #define getSPIDriver() &ls_spicfg
 
@@ -31,17 +31,14 @@ void Si4464_Init(void) {
 	Si4464_shutdown();
 	chThdSleepMilliseconds(10);
 
-	// Initialize SPI
+	// Configure SPI pins
 	palSetLineMode(LINE_SPI_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST);		// SCK
 	palSetLineMode(LINE_SPI_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST);	// MISO
 	palSetLineMode(LINE_SPI_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST);	// MOSI
 	palSetLineMode(LINE_RADIO_CS, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);	// RADIO CS
+	palSetLineMode(LINE_RADIO_SDN, PAL_MODE_OUTPUT_PUSHPULL);							// RADIO SDN
 	palSetLine(LINE_RADIO_CS);
 
-	// Configure pins
-	palSetLineMode(LINE_RADIO_SDN, PAL_MODE_OUTPUT_PUSHPULL);	// RADIO SDN
-	palSetLineMode(LINE_RADIO_GPIO, PAL_MODE_OUTPUT_PUSHPULL);	// RADIO GPIO1
-
 	// Power up transmitter
 	palClearLine(LINE_RADIO_SDN);	// Radio SDN low (power up transmitter)
 	chThdSleepMilliseconds(10);		// Wait for transmitter to power up
@@ -55,20 +52,6 @@ void Si4464_Init(void) {
 	Si4464_write(init_command, 7);
 	chThdSleepMilliseconds(25);
 
-	// Set transmitter GPIOs
-	uint8_t gpio_pin_cfg_command[] = {
-		0x13,	// Command type = GPIO settings
-		0x00,	// GPIO0        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
-		0x44,	// GPIO1        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
-		0x00,	// GPIO2        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
-		0x00,	// GPIO3        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
-		0x00,	// NIRQ
-		0x00,	// SDO
-		0x00	// GEN_CONFIG
-	};
-	Si4464_write(gpio_pin_cfg_command, 8);
-	chThdSleepMilliseconds(25);
-
 	// Temperature readout
 	TRACE_INFO("SI   > Transmitter temperature %d degC", Si4464_getTemperature());
 	initialized = true;
@@ -81,29 +64,25 @@ void Si4464_write(uint8_t* txData, uint32_t len) {
 	// SPI transfer
 	spiAcquireBus(&SPID1);
 	spiStart(&SPID1, getSPIDriver());
+
 	spiSelect(&SPID1);
 	spiExchange(&SPID1, len, txData, rxData);
 	spiUnselect(&SPID1);
-	spiStop(&SPID1);
-	spiReleaseBus(&SPID1);
 
 	// Reqest ACK by Si4464
-	uint32_t counter = 0; // FIXME: Sometimes CTS is not returned by Si4464 correctly
 	rxData[1] = 0x00;
-	while(rxData[1] != 0xFF && ++counter < 2000) {
+	while(rxData[1] != 0xFF) {
 
 		// Request ACK by Si4464
 		uint8_t rx_ready[] = {0x44};
 
 		// SPI transfer
-		spiAcquireBus(&SPID1);
-		spiStart(&SPID1, getSPIDriver());
 		spiSelect(&SPID1);
 		spiExchange(&SPID1, 3, rx_ready, rxData);
 		spiUnselect(&SPID1);
-		spiStop(&SPID1);
-		spiReleaseBus(&SPID1);
 	}
+	spiStop(&SPID1);
+	spiReleaseBus(&SPID1);
 }
 
 /**
@@ -115,30 +94,26 @@ void Si4464_read(uint8_t* txData, uint32_t txlen, uint8_t* rxData, uint32_t rxle
 	// SPI transfer
 	spiAcquireBus(&SPID1);
 	spiStart(&SPID1, getSPIDriver());
+
 	spiSelect(&SPID1);
 	spiExchange(&SPID1, txlen, txData, null_spi);
 	spiUnselect(&SPID1);
-	spiStop(&SPID1);
-	spiReleaseBus(&SPID1);
 
 	// Reqest ACK by Si4464
-	uint32_t counter = 0; // FIXME: Sometimes CTS is not returned by Si4464 correctly
 	rxData[1] = 0x00;
-	while(rxData[1] != 0xFF && ++counter < 2000) {
+	while(rxData[1] != 0xFF) {
 
 		// Request ACK by Si4464
 		uint16_t rx_ready[rxlen];
 		rx_ready[0] = 0x44;
 
 		// SPI transfer
-		spiAcquireBus(&SPID1);
-		spiStart(&SPID1, getSPIDriver());
 		spiSelect(&SPID1);
 		spiExchange(&SPID1, rxlen, rx_ready, rxData);
 		spiUnselect(&SPID1);
-		spiStop(&SPID1);
-		spiReleaseBus(&SPID1);
 	}
+	spiStop(&SPID1);
+	spiReleaseBus(&SPID1);
 }
 
 void setFrequency(uint32_t freq, uint16_t shift) {
@@ -198,6 +173,21 @@ void setShift(uint16_t shift) {
 }
 
 void setModemAFSK(void) {
+	// Set transmitter GPIOs
+	palSetLineMode(LINE_RADIO_GPIO, PAL_MODE_OUTPUT_PUSHPULL); // GPIO1
+	uint8_t gpio_pin_cfg_command[] = {
+		0x13,	// Command type = GPIO settings
+		0x00,	// GPIO0        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x44,	// GPIO1        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO2        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO3        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// NIRQ
+		0x00,	// SDO
+		0x00	// GEN_CONFIG
+	};
+	Si4464_write(gpio_pin_cfg_command, 8);
+	chThdSleepMilliseconds(25);
+
 	// Disable preamble
 	uint8_t disable_preamble[] = {0x11, 0x10, 0x01, 0x00, 0x00};
 	Si4464_write(disable_preamble, 5);
@@ -233,14 +223,44 @@ void setModemAFSK(void) {
 }
 
 void setModemOOK(void) {
+	// Set transmitter GPIOs
+	palSetLineMode(LINE_RADIO_GPIO, PAL_MODE_OUTPUT_PUSHPULL); // GPIO1
+	uint8_t gpio_pin_cfg_command[] = {
+		0x13,	// Command type = GPIO settings
+		0x00,	// GPIO0        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x44,	// GPIO1        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO2        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO3        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// NIRQ
+		0x00,	// SDO
+		0x00	// GEN_CONFIG
+	};
+	Si4464_write(gpio_pin_cfg_command, 8);
+	chThdSleepMilliseconds(25);
+
 	// Use OOK from async GPIO1
 	uint8_t use_ook[] = {0x11, 0x20, 0x01, 0x00, 0xA9};
 	Si4464_write(use_ook, 5);
 }
 
 void setModem2FSK(void) {
+	// Set transmitter GPIOs
+	palSetLineMode(LINE_RADIO_GPIO, PAL_MODE_OUTPUT_PUSHPULL); // GPIO1
+	uint8_t gpio_pin_cfg_command[] = {
+		0x13,	// Command type = GPIO settings
+		0x00,	// GPIO0        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x44,	// GPIO1        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO2        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO3        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// NIRQ
+		0x00,	// SDO
+		0x00	// GEN_CONFIG
+	};
+	Si4464_write(gpio_pin_cfg_command, 8);
+	chThdSleepMilliseconds(25);
+
 	// Initialize high tone
-	RADIO_MOD_GPIO(HIGH);
+	RADIO_WRITE_GPIO(HIGH);
 
 	// use 2FSK from async GPIO1
 	uint8_t use_2fsk[] = {0x11, 0x20, 0x01, 0x00, 0xAA};
@@ -248,6 +268,25 @@ void setModem2FSK(void) {
 }
 
 void setModem2GFSK(gfsk_conf_t* conf) {
+	// Set transmitter GPIOs
+	palSetLineMode(LINE_RADIO_GPIO, PAL_MODE_INPUT); // GPIO1
+	uint8_t gpio_pin_cfg_command[] = {
+		0x13,	// Command type = GPIO settings
+		0x00,	// GPIO0        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x23,	// GPIO1        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO2        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// GPIO3        0 - PULL_CTL[1bit] - GPIO_MODE[6bit]
+		0x00,	// NIRQ
+		0x00,	// SDO
+		0x00	// GEN_CONFIG
+	};
+	Si4464_write(gpio_pin_cfg_command, 8);
+	chThdSleepMilliseconds(25);
+
+	// Set FIFO empty interrupt threshold (32 byte)
+	uint8_t set_fifo_irq[] = {0x11, 0x12, 0x01, 0x0B, 0x20};
+	Si4464_write(set_fifo_irq, 5);
+
 	// Disable preamble
 	uint8_t disable_preamble[] = {0x11, 0x10, 0x01, 0x00, 0x00};
 	Si4464_write(disable_preamble, 5);
@@ -301,7 +340,7 @@ void stopTx(void) {
 void Si4464_shutdown(void) {
 	palSetLine(LINE_RADIO_SDN);	// Power down chip
 	palSetLine(LINE_IO_LED1);	// Set indication LED
-	RADIO_MOD_GPIO(false);		// Set GPIO1 low
+	RADIO_WRITE_GPIO(false);		// Set GPIO1 low
 	initialized = false;
 }
 
diff --git a/tracker/software/drivers/si4464.h b/tracker/software/drivers/si4464.h
index 73c73b9..7531784 100644
--- a/tracker/software/drivers/si4464.h
+++ b/tracker/software/drivers/si4464.h
@@ -6,7 +6,8 @@
 #include "defines.h"
 #include "types.h"
 
-#define RADIO_MOD_GPIO(state)	palWriteLine(LINE_RADIO_GPIO, state)
+#define RADIO_WRITE_GPIO(state)	palWriteLine(LINE_RADIO_GPIO, state)
+#define RADIO_READ_GPIO()		palReadLine(LINE_RADIO_GPIO)
 #define inRadioBand(freq) 		(RADIO_MIN_FREQ <= (freq) && (freq) <= RADIO_MAX_FREQ)
 
 #define SI4464_STATE_NOCHANGE	0
diff --git a/tracker/software/drivers/wrapper/pi2c.c b/tracker/software/drivers/wrapper/pi2c.c
index c3e72b0..b7ddab8 100644
--- a/tracker/software/drivers/wrapper/pi2c.c
+++ b/tracker/software/drivers/wrapper/pi2c.c
@@ -14,46 +14,45 @@ const I2CConfig _i2cfg = {
 	FAST_DUTY_CYCLE_2,
 };
 
-static bool I2C_transmit(I2CDriver *driver, uint8_t addr, uint8_t *txbuf, uint32_t txbytes, uint8_t *rxbuf, uint32_t rxbytes, systime_t timeout) {
-	i2cAcquireBus(driver);
-	msg_t i2c_status = i2cMasterTransmitTimeout(driver, addr, txbuf, txbytes, rxbuf, rxbytes, timeout);
+static bool I2C_transmit(uint8_t addr, uint8_t *txbuf, uint32_t txbytes, uint8_t *rxbuf, uint32_t rxbytes, systime_t timeout) {
+	i2cAcquireBus(I2C_DRIVER);
+	i2cStart(I2C_DRIVER, &_i2cfg);
+	msg_t i2c_status = i2cMasterTransmitTimeout(I2C_DRIVER, addr, txbuf, txbytes, rxbuf, rxbytes, timeout);
+	i2cStop(I2C_DRIVER);
+	i2cReleaseBus(I2C_DRIVER);
+
 	if(i2c_status == MSG_TIMEOUT) { // Restart I2C at timeout
-		TRACE_ERROR("I2C  > TIMEOUT > RESTART (ADDR 0x%02x)", addr);
-		i2cStop(driver);
-		i2cStart(driver, &_i2cfg);
+		TRACE_ERROR("I2C  > TIMEOUT (ADDR 0x%02x)", addr);
 	} else if(i2c_status == MSG_RESET) {
 		TRACE_ERROR("I2C  > RESET (ADDR 0x%02x)", addr);
 	}
-	i2cReleaseBus(driver);
+
 	return i2c_status == MSG_OK;
 }
 
 void pi2cInit(void)
 {
-	TRACE_INFO("I2C  > Initialize I2C");
-
+	TRACE_INFO("I2C  > Initialize I2C Pins");
 	palSetLineMode(LINE_I2C_SDA, PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN); // SDA
 	palSetLineMode(LINE_I2C_SCL, PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN); // SCL
-
-	i2cStart(&I2CD1, &_i2cfg);
 }
 
 bool I2C_write8(uint8_t address, uint8_t reg, uint8_t value)
 {
 	uint8_t txbuf[] = {reg, value};
-	return I2C_transmit(I2C_DRIVER, address, txbuf, 2, NULL, 0, MS2ST(100));
+	return I2C_transmit(address, txbuf, 2, NULL, 0, MS2ST(100));
 }
 
 bool I2C_writeN(uint8_t address, uint8_t *txbuf, uint32_t length)
 {
-	return I2C_transmit(I2C_DRIVER, address, txbuf, length, NULL, 0, MS2ST(100));
+	return I2C_transmit(address, txbuf, length, NULL, 0, MS2ST(100));
 }
 
 bool I2C_read8(uint8_t address, uint8_t reg, uint8_t *val)
 {
 	uint8_t txbuf[] = {reg};
 	uint8_t rxbuf[1];
-	bool ret = I2C_transmit(I2C_DRIVER, address, txbuf, 1, rxbuf, 1, MS2ST(100));
+	bool ret = I2C_transmit(address, txbuf, 1, rxbuf, 1, MS2ST(100));
 	*val = rxbuf[0];
 	return ret;
 }
@@ -62,7 +61,7 @@ bool I2C_read16(uint8_t address, uint8_t reg, uint16_t *val)
 {
 	uint8_t txbuf[] = {reg};
 	uint8_t rxbuf[2];
-	bool ret = I2C_transmit(I2C_DRIVER, address, txbuf, 1, rxbuf, 2, MS2ST(100));
+	bool ret = I2C_transmit(address, txbuf, 1, rxbuf, 2, MS2ST(100));
 	*val =  (rxbuf[0] << 8) | rxbuf[1];
 	return ret;
 }
@@ -77,7 +76,7 @@ bool I2C_read8_16bitreg(uint8_t address, uint16_t reg, uint8_t *val) // 16bit re
 {
 	uint8_t txbuf[] = {reg >> 8, reg & 0xFF};
 	uint8_t rxbuf[1];
-	bool ret = I2C_transmit(I2C_DRIVER, address, txbuf, 2, rxbuf, 1, MS2ST(100));
+	bool ret = I2C_transmit(address, txbuf, 2, rxbuf, 1, MS2ST(100));
 	*val = rxbuf[0];
 	return ret;
 }
@@ -85,6 +84,6 @@ bool I2C_read8_16bitreg(uint8_t address, uint16_t reg, uint8_t *val) // 16bit re
 bool I2C_write8_16bitreg(uint8_t address, uint16_t reg, uint8_t value) // 16bit register (for OV5640)
 {
 	uint8_t txbuf[] = {reg >> 8, reg & 0xFF, value};
-	return I2C_transmit(I2C_DRIVER, address, txbuf, 3, NULL, 0, MS2ST(100));
+	return I2C_transmit(address, txbuf, 3, NULL, 0, MS2ST(100));
 }
 
diff --git a/tracker/software/mcuconf.h b/tracker/software/mcuconf.h
index 8303d1e..d725fac 100644
--- a/tracker/software/mcuconf.h
+++ b/tracker/software/mcuconf.h
@@ -48,13 +48,13 @@
 #define STM32_PLLN_VALUE                    192
 #define STM32_PLLP_VALUE                    4
 #define STM32_PLLQ_VALUE                    4
-#define STM32_HPRE                          STM32_HPRE_DIV8
+#define STM32_HPRE                          STM32_HPRE_DIV1
 #define STM32_PPRE1                         STM32_PPRE1_DIV1
 #define STM32_PPRE2                         STM32_PPRE2_DIV1
 #define STM32_RTCSEL                        STM32_RTCSEL_LSI
 #define STM32_RTCPRE_VALUE                  8
-#define STM32_MCO1SEL                       STM32_MCO1SEL_HSE
-#define STM32_MCO1PRE                       STM32_MCO1PRE_DIV2
+#define STM32_MCO1SEL                       STM32_MCO1SEL_PLL
+#define STM32_MCO1PRE                       STM32_MCO1PRE_DIV4
 #define STM32_MCO2SEL                       STM32_MCO2SEL_SYSCLK
 #define STM32_MCO2PRE                       STM32_MCO2PRE_DIV5
 #define STM32_I2SSRC                        STM32_I2SSRC_CKIN
@@ -124,9 +124,9 @@
 #define STM32_I2C_I2C1_IRQ_PRIORITY         5
 #define STM32_I2C_I2C2_IRQ_PRIORITY         5
 #define STM32_I2C_I2C3_IRQ_PRIORITY         5
-#define STM32_I2C_I2C1_DMA_PRIORITY         3
-#define STM32_I2C_I2C2_DMA_PRIORITY         3
-#define STM32_I2C_I2C3_DMA_PRIORITY         3
+#define STM32_I2C_I2C1_DMA_PRIORITY         1
+#define STM32_I2C_I2C2_DMA_PRIORITY         1
+#define STM32_I2C_I2C3_DMA_PRIORITY         1
 #define STM32_I2C_DMA_ERROR_HOOK(i2cp)      osalSysHalt("DMA failure")
 
 /*
@@ -201,17 +201,17 @@
 #define STM32_SPI_SPI1_TX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 3)
 #define STM32_SPI_SPI2_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 3)
 #define STM32_SPI_SPI2_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 4)
-#define STM32_SPI_SPI3_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 0)
+#define STM32_SPI_SPI3_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 2)
 #define STM32_SPI_SPI3_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 7)
 #define STM32_SPI_SPI4_RX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 4)
 #define STM32_SPI_SPI4_TX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 1)
 #define STM32_SPI_SPI5_RX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 5)
 #define STM32_SPI_SPI5_TX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 6)
-#define STM32_SPI_SPI1_DMA_PRIORITY         1
-#define STM32_SPI_SPI2_DMA_PRIORITY         1
-#define STM32_SPI_SPI3_DMA_PRIORITY         1
-#define STM32_SPI_SPI4_DMA_PRIORITY         1
-#define STM32_SPI_SPI5_DMA_PRIORITY         1
+#define STM32_SPI_SPI1_DMA_PRIORITY         3
+#define STM32_SPI_SPI2_DMA_PRIORITY         3
+#define STM32_SPI_SPI3_DMA_PRIORITY         3
+#define STM32_SPI_SPI4_DMA_PRIORITY         3
+#define STM32_SPI_SPI5_DMA_PRIORITY         3
 #define STM32_SPI_SPI1_IRQ_PRIORITY         10
 #define STM32_SPI_SPI2_IRQ_PRIORITY         10
 #define STM32_SPI_SPI3_IRQ_PRIORITY         10
diff --git a/tracker/software/radio.c b/tracker/software/radio.c
index 90bd8d9..d5c9aab 100644
--- a/tracker/software/radio.c
+++ b/tracker/software/radio.c
@@ -22,15 +22,12 @@ static uint32_t phase;					// Fixed point 9.7 (2PI = TABLE_SIZE)
 static uint32_t packet_pos;				// Next bit to be sent out
 static uint32_t current_sample_in_baud;	// 1 bit = SAMPLES_PER_BAUD samples
 static uint8_t current_byte;
-static radioMSG_t *tim_msg;
-static uint32_t gfsk_bit;
+static radioMSG_t tim_msg;
 
 static uint8_t txs;						// Serial maschine state
 static uint8_t txc;						// Current byte
 static uint32_t txi;					// Bitcounter of current byte
 static uint32_t txj;					// Bytecounter
-static radioMSG_t *fsk_msg;				// Current message
-static virtual_timer_t vt;				// UART timer
 
 static const char *getModulation(uint8_t key) {
 	const char *val[] = {"unknown", "OOK", "2FSK", "2GFSK", "AFSK"};
@@ -38,15 +35,15 @@ static const char *getModulation(uint8_t key) {
 };
 
 void initAFSK(radioMSG_t *msg) {
-	// Initialize radio and tune
+	// Initialize radio
 	Si4464_Init();
 	setModemAFSK();
-	radioTune(msg->freq, 0, msg->power, 0);
+	active_mod = MOD_AFSK;
 }
 
 void sendAFSK(radioMSG_t *msg) {
 	// Initialize variables for timer
-	tim_msg = msg;
+	memcpy(&tim_msg, msg, sizeof(radioMSG_t));
 
 	phase_delta = PHASE_DELTA_1200;
 	phase = 0;
@@ -54,12 +51,13 @@ void sendAFSK(radioMSG_t *msg) {
 	current_sample_in_baud = 0;
 	current_byte = 0;
 
+	// Tune
+	radioTune(msg->freq, 0, msg->power, 0);
+
 	// Initialize timer
-	uint32_t initial_interval = 250;
 	RCC->APB1ENR |= RCC_APB1ENR_TIM7EN;
 	nvicEnableVector(TIM7_IRQn, 1);
-	TIM7->ARR = initial_interval;
-	TIM7->PSC = 1;
+	TIM7->ARR = 500;
 	TIM7->CR1 &= ~STM32_TIM_CR1_ARPE;
 	TIM7->DIER |= STM32_TIM_DIER_UIE;
 
@@ -69,47 +67,91 @@ void sendAFSK(radioMSG_t *msg) {
 	// Block execution while timer is running
 	while(TIM7->CR1 & STM32_TIM_CR1_CEN)
 		chThdSleepMilliseconds(10);
+
+	shutdownRadio();
 }
 
 void init2GFSK(radioMSG_t *msg) {
 	// Initialize radio
 	Si4464_Init();
 	setModem2GFSK(msg->gfsk_conf);
+	active_mod = MOD_2GFSK;
 }
 
-void send2GFSK(radioMSG_t *msg) {
+thread_reference_t feeder_ref = NULL;
+
+/*
+ * Radio GPIO1 interrupt
+ */
+CH_IRQ_HANDLER(VectorE0) {
+	CH_IRQ_PROLOGUE();
+
+	chThdResumeS(&feeder_ref, MSG_OK);
+
+	EXTI->PR |= EXTI_PR_PR12;
+	CH_IRQ_EPILOGUE();
+}
+
+static THD_WORKING_AREA(si_fifo_feeder_wa, 10240);
+THD_FUNCTION(si_fifo_feeder_thd, arg)
+{
+	(void)arg;
+
 	uint16_t c = 64;
-	uint16_t all = (msg->bin_len+7)/8;
+	uint16_t all = (tim_msg.bin_len+7)/8;
 
 	// Initial FIFO fill
-	Si4464_writeFIFO(msg->msg, c);
+	Si4464_writeFIFO(tim_msg.msg, c);
+
+	// Initialize interrupt
+	SYSCFG->EXTICR[3] |= SYSCFG_EXTICR4_EXTI12_PC;
+	EXTI->IMR = EXTI_IMR_MR12; // Activate interrupt for chan12 (=>PC12)
+	EXTI->RTSR = EXTI_RTSR_TR12; // Listen on rising edge
+	nvicEnableVector(EXTI15_10_IRQn, 1); // Enable interrupt
 
 	// Transmit
-	radioTune(msg->freq, 0, msg->power, all);
+	radioTune(tim_msg.freq, 0, tim_msg.power, all);
 
 	while(c < all) { // Do while bytes not written into FIFO completely
+		chThdSuspendS(&feeder_ref); // Suspend until interupt resumes it
 
 		// Determine free memory in Si4464-FIFO
 		uint16_t more = Si4464_freeFIFO();
 		if(more > all-c)
 			more = all-c; // Last bytes in FIFO
 
-		Si4464_writeFIFO(&msg->msg[c], more); // Write into FIFO
-
+		//TRACE_DEBUG("fed %db %d<%d", more, c, all);
+		Si4464_writeFIFO(&tim_msg.msg[c], more); // Write into FIFO
 		c += more;
-		chThdSleepMilliseconds(1);
 	}
+
+	nvicDisableVector(EXTI15_10_IRQn); // Disable interrupt
+
+	shutdownRadio();
+}
+
+void send2GFSK(radioMSG_t *msg) {
+	// Copy data
+	memcpy(&tim_msg, msg, sizeof(radioMSG_t));
+
+	// Start FIFO feeder
+	chThdCreateStatic(si_fifo_feeder_wa, sizeof(si_fifo_feeder_wa), HIGHPRIO+1, si_fifo_feeder_thd, NULL);
+
+	// Wait for the transmitter to start (because it is used as mutex)
+	while(Si4464_getState() != SI4464_STATE_TX)
+		chThdSleepMilliseconds(1);
 }
 
 /**
-  * Fast interrupt handler for AFSK and 2GFSK modulation. It has has the
-  * highest priority in order to provide an accurate low jitter modulation.
+  * Fast interrupt handler for AFSK modulation. It has has the highest priority
+  * in order to provide an accurate low jitter modulation.
   */
 CH_FAST_IRQ_HANDLER(STM32_TIM7_HANDLER)
 {
-	if(tim_msg->mod == MOD_AFSK) {
+	if(active_mod == MOD_AFSK) // AFSK
+	{
 
-		if(packet_pos == tim_msg->bin_len) { 	// Packet transmission finished
+		if(packet_pos == tim_msg.bin_len) { 	// Packet transmission finished
 			TIM7->CR1 &= ~STM32_TIM_CR1_CEN;	// Disable timer
 			TIM7->SR &= ~STM32_TIM_SR_UIF;		// Reset interrupt flag
 			return;
@@ -117,7 +159,7 @@ CH_FAST_IRQ_HANDLER(STM32_TIM7_HANDLER)
 
 		if(current_sample_in_baud == 0) {
 			if((packet_pos & 7) == 0) { // Load up next byte
-				current_byte = tim_msg->msg[packet_pos >> 3];
+				current_byte = tim_msg.msg[packet_pos >> 3];
 			} else { // Load up next bit
 				current_byte = current_byte / 2;
 			}
@@ -127,7 +169,7 @@ CH_FAST_IRQ_HANDLER(STM32_TIM7_HANDLER)
 		phase_delta = (current_byte & 1) ? PHASE_DELTA_1200 : PHASE_DELTA_2200;
 
 		phase += phase_delta;					// Add delta-phase (delta-phase tone dependent)
-		RADIO_MOD_GPIO((phase >> 16) & 1);		// Set modulaton pin (connected to Si4464)
+		RADIO_WRITE_GPIO((phase >> 16) & 1);	// Set modulaton pin (connected to Si4464)
 
 		current_sample_in_baud++;
 
@@ -135,136 +177,129 @@ CH_FAST_IRQ_HANDLER(STM32_TIM7_HANDLER)
 			current_sample_in_baud = 0;
 			packet_pos++;
 		}
-		palToggleLine(LINE_IO_LED1);
 
-	} else if(tim_msg->mod == MOD_2GFSK) {
+	} else { // 2FSK
 
-		if(gfsk_bit >= tim_msg->bin_len) {		// Packet transmission finished
-			TIM7->CR1 &= ~STM32_TIM_CR1_CEN;	// Disable timer
-			TIM7->SR &= ~STM32_TIM_SR_UIF;		// Reset interrupt flag
-			return;
+		switch(txs)
+		{
+			case 6: // TX-delay
+				txj++;
+				if(txj > (uint32_t)(tim_msg.fsk_conf->predelay * tim_msg.fsk_conf->baud / 1000)) {
+					txj = 0;
+					txs = 7;
+				}
+				break;
+
+			case 7: // Transmit a single char
+				if(txj < tim_msg.bin_len/8) {
+					txc = tim_msg.msg[txj]; // Select char
+					txj++;
+					RADIO_WRITE_GPIO(LOW); // Start Bit (Synchronizing)
+					txi = 0;
+					txs = 8;
+				} else { // Finished to transmit string
+					RADIO_WRITE_GPIO(HIGH);
+					TIM7->CR1 &= ~STM32_TIM_CR1_CEN; // Disable timer
+					TIM7->SR &= ~STM32_TIM_SR_UIF; // Reset interrupt flag
+					return;
+				}
+				break;
+
+			case 8:
+				if(txi < tim_msg.fsk_conf->bits) {
+					txi++;
+					RADIO_WRITE_GPIO(txc & 1);
+					txc = txc >> 1;
+				} else {
+					RADIO_WRITE_GPIO(HIGH); // Stop Bit
+					txi = 0;
+					txs = 9;
+				}
+				break;
+
+			case 9:
+				if(tim_msg.fsk_conf->stopbits == 2)
+					RADIO_WRITE_GPIO(HIGH); // Stop Bit
+				txs = 7;
 		}
 
-		if((gfsk_bit & 7) == 0) { // Load up next byte
-			current_byte = tim_msg->msg[gfsk_bit >> 3];
-		} else {
-			current_byte = current_byte / 2; // Load next bit
-		}
-
-		RADIO_MOD_GPIO(current_byte & 0x1);
-		gfsk_bit++;
-
-		palToggleLine(LINE_IO_LED1);
 	}
 
+	palToggleLine(LINE_IO_LED1);
+
 	TIM7->SR &= ~STM32_TIM_SR_UIF; // Reset interrupt flag
 }
 
 void initOOK(radioMSG_t *msg) {
-	// Initialize radio and tune
+	// Initialize radio
 	Si4464_Init();
 	setModemOOK();
-	radioTune(msg->freq, 0, msg->power, 0);
+	active_mod = MOD_OOK;
 }
 
 /**
   * Transmits binary OOK message. One bit = 20ms (1: TONE, 0: NO TONE)
   */
 void sendOOK(radioMSG_t *msg) {
+	// Tune
+	radioTune(msg->freq, 0, msg->power, 0);
+
 	// Transmit data
 	uint32_t bit = 0;
 	systime_t time = chVTGetSystemTimeX();
 	while(bit < msg->bin_len) {
-		RADIO_MOD_GPIO((msg->msg[bit/8] >> (bit%8)) & 0x1);
+		RADIO_WRITE_GPIO((msg->msg[bit/8] >> (bit%8)) & 0x1);
 		bit++;
 
 		time = chThdSleepUntilWindowed(time, time + MS2ST(1200 / msg->ook_conf->speed));
 	}
-}
-
-static void serial_cb(void *arg) {
-	(void)arg;
-
-	switch(txs)
-	{
-		case 6: // TX-delay
-			txj++;
-			if(txj > (uint32_t)(fsk_msg->fsk_conf->predelay * fsk_msg->fsk_conf->baud / 1000)) {
-				txj = 0;
-				txs = 7;
-			}
-			break;
-
-		case 7: // Transmit a single char
-			if(txj < fsk_msg->bin_len/8) {
-				txc = fsk_msg->msg[txj]; // Select char
-				txj++;
-				RADIO_MOD_GPIO(LOW); // Start Bit (Synchronizing)
-				txi = 0;
-				txs = 8;
-			} else {
-				txj = 0;
-				txs = 0; // Finished to transmit string
-				RADIO_MOD_GPIO(HIGH);
-			}
-			break;
-
-		case 8:
-			if(txi < fsk_msg->fsk_conf->bits) {
-				txi++;
-				RADIO_MOD_GPIO(txc & 1);
-				txc = txc >> 1;
-			} else {
-				RADIO_MOD_GPIO(HIGH); // Stop Bit
-				txi = 0;
-				txs = 9;
-			}
-			break;
-
-		case 9:
-			if(fsk_msg->fsk_conf->stopbits == 2)
-				RADIO_MOD_GPIO(HIGH); // Stop Bit
-			txs = 7;
-	}
-
-	// Reload timer
-	if(txs) {
-		chSysLockFromISR();
-		uint32_t delay = US2ST(1000000/fsk_msg->fsk_conf->baud);
-		chVTSetI(&vt, delay, serial_cb, NULL);
-		chSysUnlockFromISR();
-	}
+	shutdownRadio();
 }
 
 void init2FSK(radioMSG_t *msg) {
-	// Initialize virtual timer
-	chVTObjectInit(&vt);
-
 	// Initialize radio and tune
 	Si4464_Init();
 	setModem2FSK();
-	radioTune(msg->freq, msg->fsk_conf->shift, msg->power, 0);
 }
 
 void send2FSK(radioMSG_t *msg) {
-	// Prepare serial machine states
+	// Initialize variables for timer
+	memcpy(&tim_msg, msg, sizeof(radioMSG_t));
+
 	txs = 6;
 	txc = 0;
 	txi = 0;
 	txj = 0;
-	fsk_msg = msg;
 
-	// Modulate
-	chVTSet(&vt, 1, serial_cb, NULL);	// Start timer
-	while(txs)
-		chThdSleepMilliseconds(1);		// Wait for routine to finish
+	// Tune
+	radioTune(msg->freq, msg->fsk_conf->shift, msg->power, 0);
+
+	// Initialize timer
+	RCC->APB1ENR |= RCC_APB1ENR_TIM7EN;
+	nvicEnableVector(TIM7_IRQn, 1);
+	TIM7->ARR = STM32_PCLK1 / 16 / msg->fsk_conf->baud; // FIXME: 5625000 should be actually STM32_PCLK1
+	TIM7->PSC = 15;
+	TIM7->CR1 &= ~STM32_TIM_CR1_ARPE;
+	TIM7->DIER |= STM32_TIM_DIER_UIE;
+
+	// Start timer
+	TIM7->CR1 |= STM32_TIM_CR1_CEN;
+
+	// Block execution while timer is running
+	while(TIM7->CR1 & STM32_TIM_CR1_CEN)
+		chThdSleepMilliseconds(10);
+
+	shutdownRadio();
 }
 
 void shutdownRadio(void)
 {
 	// Wait for PH to finish transmission for 2GFSK
 	while(active_mod == MOD_2GFSK && Si4464_getState() == SI4464_STATE_TX)
+	{
+		TRACE_DEBUG("Waiting for Si4464 (state=%d, free=%d)", Si4464_getState(), Si4464_freeFIFO());
 		chThdSleepMilliseconds(5);
+	}
 
 	Si4464_shutdown();
 	active_mod = MOD_NOT_SET;
@@ -322,56 +357,57 @@ uint32_t getAPRSRegionFrequency(void) {
 /**
   * Sends radio message into message box. This method will return false if message box is full.
   */
-bool transmitOnRadio(radioMSG_t *msg, bool shutdown) {
-	// Lock radio
-	chMtxLock(&radio_mtx);
+bool transmitOnRadio(radioMSG_t *msg, bool shutdown)
+{
+	if(inRadioBand(msg->freq)) // Frequency in radio radio band
+	{
+		if(inRadioBand(msg->freq)) // Frequency in radio radio band
+		{
 
-	if(inRadioBand(msg->freq)) { // Frequency in radio radio band
+			lockRadio(); // Lock radio
 
-		// Wait for PH to finish transmission for 2GFSK
-		while(active_mod == MOD_2GFSK && Si4464_getState() == SI4464_STATE_TX)
-			chThdSleepMilliseconds(5);
-
-		// Lock interference mutex
-		chMtxLock(&interference_mtx);
-
-		TRACE_INFO(	"RAD  > Transmit %d.%03d MHz, Pwr %d, %s, %d bits",
-					msg->freq/1000000, (msg->freq%1000000)/1000, msg->power,
-					getModulation(msg->mod), msg->bin_len
-		);
+			TRACE_INFO(	"RAD  > Transmit %d.%03d MHz, Pwr %d, %s, %d bits",
+						msg->freq/1000000, (msg->freq%1000000)/1000, msg->power,
+						getModulation(msg->mod), msg->bin_len
+			);
 		
-		switch(msg->mod) {
-			case MOD_2FSK:
-				if(active_mod != msg->mod)
-					init2FSK(msg);
-				send2FSK(msg);
-				break;
-			case MOD_2GFSK:
-				if(active_mod != msg->mod)
-					init2GFSK(msg);
-				send2GFSK(msg);
-				break;
-			case MOD_AFSK:
-				if(active_mod != msg->mod)
-					initAFSK(msg);
-				sendAFSK(msg);
-				break;
-			case MOD_OOK:
-				if(active_mod != msg->mod)
-					initOOK(msg);
-				sendOOK(msg);
-				break;
-			case MOD_NOT_SET:
-				TRACE_ERROR("RAD  > Modulation not set");
-				break;
+			switch(msg->mod)
+			{
+				case MOD_2FSK:
+					if(active_mod != msg->mod)
+						init2FSK(msg);
+					send2FSK(msg);
+					break;
+				case MOD_2GFSK:
+					if(active_mod != msg->mod)
+						init2GFSK(msg);
+					send2GFSK(msg);
+					break;
+				case MOD_AFSK:
+					if(active_mod != msg->mod)
+						initAFSK(msg);
+					sendAFSK(msg);
+					break;
+				case MOD_OOK:
+					if(active_mod != msg->mod)
+						initOOK(msg);
+					sendOOK(msg);
+					break;
+				case MOD_NOT_SET:
+					TRACE_ERROR("RAD  > Modulation not set");
+					break;
+			}
+
+			unlockRadio(); // Unlock radio
+
+		} else {
+
+			TRACE_ERROR("RAD  > It is nonsense to transmit 0 bits, %d.%03d MHz, Pwr dBm, %s, %d bits",
+						msg->freq/1000000, (msg->freq%1000000)/1000, msg->power, getModulation(msg->mod), msg->bin_len
+			);
+
 		}
 
-		active_mod = msg->mod;
-		if(shutdown)
-			shutdownRadio(); // Shutdown radio for reinitialization
-
-		chMtxUnlock(&interference_mtx); // Heavy interference finished (HF)
-
 	} else { // Frequency out of radio band
 
 		TRACE_ERROR("RAD  > Radio cant transmit on this frequency, %d.%03d MHz, Pwr dBm, %s, %d bits",
@@ -380,9 +416,6 @@ bool transmitOnRadio(radioMSG_t *msg, bool shutdown) {
 
 	}
 
-	// Unlock radio
-	chMtxUnlock(&radio_mtx);
-
 	return true;
 }
 
@@ -403,3 +436,16 @@ uint32_t getFrequency(freq_conf_t *config)
 	}
 }
 
+void lockRadio(void)
+{
+	chMtxLock(&radio_mtx);
+
+	while(active_mod != MOD_NOT_SET && Si4464_getState() == SI4464_STATE_TX)
+		chThdSleepMilliseconds(1);
+}
+
+void unlockRadio(void)
+{
+	chMtxUnlock(&radio_mtx);
+}
+
diff --git a/tracker/software/radio.h b/tracker/software/radio.h
index 1e4812a..f73cab1 100644
--- a/tracker/software/radio.h
+++ b/tracker/software/radio.h
@@ -24,6 +24,8 @@ extern mutex_t radio_mtx;
 bool transmitOnRadio(radioMSG_t *msg, bool shutdown);
 void shutdownRadio(void);
 uint32_t getFrequency(freq_conf_t *config);
+void lockRadio(void);
+void unlockRadio(void);
 
 THD_FUNCTION(moduleRADIO, arg);
 
diff --git a/tracker/software/threads/image.c b/tracker/software/threads/image.c
index 8c3827d..87eb3d4 100644
--- a/tracker/software/threads/image.c
+++ b/tracker/software/threads/image.c
@@ -273,7 +273,7 @@ const uint8_t noCameraFound[] = {
 	0xBD, 0xC0, 0x20, 0x00, 0x01, 0xFF, 0xD9
 };
 
-static uint8_t gimage_id; // Global image ID (for all image threads)
+static uint8_t gimage_id = 0; // Global image ID (for all image threads)
 mutex_t camera_mtx;
 
 void encode_ssdv(const uint8_t *image, uint32_t image_len, module_conf_t* conf, uint8_t image_id, bool redudantTx)
@@ -309,8 +309,7 @@ void encode_ssdv(const uint8_t *image, uint32_t image_len, module_conf_t* conf,
 
 			if(r <= 0)
 			{
-				if(conf->protocol == PROT_APRS_2GFSK || conf->protocol == PROT_APRS_AFSK) transmitOnRadio(&msg, true); // Empty buffer
-				shutdownRadio();
+				if(msg.bin_len > 0) transmitOnRadio(&msg, false); // Empty buffer
 				TRACE_ERROR("SSDV > Premature end of file");
 				break;
 			}
@@ -319,13 +318,11 @@ void encode_ssdv(const uint8_t *image, uint32_t image_len, module_conf_t* conf,
 
 		if(c == SSDV_EOI)
 		{
-			if(conf->protocol == PROT_APRS_2GFSK || conf->protocol == PROT_APRS_AFSK) transmitOnRadio(&msg, true); // Empty buffer
-			shutdownRadio();
+			if(msg.bin_len > 0) transmitOnRadio(&msg, false); // Empty buffer
 			TRACE_INFO("SSDV > ssdv_enc_get_packet said EOI");
 			break;
 		} else if(c != SSDV_OK) {
-			if(conf->protocol == PROT_APRS_2GFSK || conf->protocol == PROT_APRS_AFSK) transmitOnRadio(&msg, true); // Empty buffer
-			shutdownRadio();
+			if(msg.bin_len > 0) transmitOnRadio(&msg, false); // Empty buffer
 			TRACE_ERROR("SSDV > ssdv_enc_get_packet failed: %i", c);
 			return;
 		}
@@ -348,8 +345,9 @@ void encode_ssdv(const uint8_t *image, uint32_t image_len, module_conf_t* conf,
 				if(redudantTx)
 					msg.bin_len += 8 + aprs_encode_experimental('I', &msg.msg[msg.bin_len/8+1], msg.mod, &conf->aprs_conf, pkt_base91, strlen((char*)pkt_base91), false);
 
-				if(msg.bin_len > 58000 || conf->packet_spacing) { // Transmit if buffer is full or if single packet transmission activation (packet_spacing != 0)
-					transmitOnRadio(&msg, true);
+				// Transmit
+				if(msg.bin_len >= 58000 || conf->packet_spacing) { // Transmit if buffer is full or if single packet transmission activation (packet_spacing != 0)
+					transmitOnRadio(&msg, false);
 					msg.bin_len = 0;
 				}
 				break;
@@ -358,11 +356,21 @@ void encode_ssdv(const uint8_t *image, uint32_t image_len, module_conf_t* conf,
 				msg.mod = MOD_2FSK;
 				msg.fsk_conf = &(conf->fsk_conf);
 
-				memcpy(msg.msg, pkt, sizeof(pkt));
-				msg.bin_len = 8*sizeof(pkt);
+				// Encode packet
+				TRACE_INFO("IMG  > Encode 2FSK/SSDV packet");
+				memcpy(&msg.msg[msg.bin_len/8], pkt, sizeof(pkt));
+				msg.bin_len += 8*sizeof(pkt);
+				if(redudantTx)
+				{
+					memcpy(&msg.msg[msg.bin_len/8], pkt, sizeof(pkt));
+					msg.bin_len += 8*sizeof(pkt);
+				}
 
-				if(redudantTx) transmitOnRadio(&msg, false); // Redundant transmission
-				transmitOnRadio(&msg, conf->packet_spacing != 0); // Keep transmitter switched on if next packet will be sent right away
+				// Transmit
+				if(msg.bin_len >= 61440 || conf->packet_spacing) { // Transmit if buffer is full or if single packet transmission activation (packet_spacing != 0)
+					transmitOnRadio(&msg, false);
+					msg.bin_len = 0;
+				}
 				break;
 
 			default:
@@ -397,11 +405,6 @@ THD_FUNCTION(imgThread, arg) {
 			chMtxLock(&camera_mtx);
 			TRACE_INFO("IMG  > Locked camera");
 
-			// Lock RADIO from producing interferences
-			TRACE_INFO("IMG  > Lock radio");
-			chMtxLock(&interference_mtx);
-			TRACE_INFO("IMG  > Locked radio");
-
 			uint8_t tries;
 			bool status = false;
 			bool camera_found = false;
@@ -453,10 +456,10 @@ THD_FUNCTION(imgThread, arg) {
 				}
 
 				// Switch off camera
-				#if !KEEP_CAM_SWITCHED_ON
-				OV5640_deinit();
-				camInitialized = false;
-				#endif
+				if(!keep_cam_switched_on) {
+					OV5640_deinit();
+					camInitialized = false;
+				}
 
 				// Get image
 				image_len = OV5640_getBuffer(&image);
@@ -469,11 +472,6 @@ THD_FUNCTION(imgThread, arg) {
 
 			}
 
-			// Unlock radio
-			TRACE_INFO("IMG  > Unlock radio");
-			chMtxUnlock(&interference_mtx);
-			TRACE_INFO("IMG  > Unlocked radio");
-
 			// Unlock camera
 			TRACE_INFO("IMG  > Unlock camera");
 			chMtxUnlock(&camera_mtx);
@@ -504,7 +502,7 @@ void start_image_thread(module_conf_t *conf)
 	if(conf->init_delay) chThdSleepMilliseconds(conf->init_delay);
 	TRACE_INFO("IMG  > Startup image thread");
 	chsnprintf(conf->name, sizeof(conf->name), "IMG");
-	thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(6*1024+8191), "IMG", NORMALPRIO, imgThread, conf);
+	thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(50*1024), "IMG", NORMALPRIO, imgThread, conf);
 	if(!th) {
 		// Print startup error, do not start watchdog for this thread
 		TRACE_ERROR("IMG  > Could not startup thread (not enough memory available)");
diff --git a/tracker/software/threads/position.c b/tracker/software/threads/position.c
index 2541ae1..a4b516c 100644
--- a/tracker/software/threads/position.c
+++ b/tracker/software/threads/position.c
@@ -218,7 +218,7 @@ void start_position_thread(module_conf_t *conf)
 	// Start position thread
 	TRACE_INFO("POS  > Startup position thread");
 	chsnprintf(conf->name, sizeof(conf->name), "POS");
-	thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(2*1024), "POS", NORMALPRIO, posThread, conf);
+	thread_t *th = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(10*1024), "POS", NORMALPRIO, posThread, conf);
 	if(!th) {
 		// Print startup error, do not start watchdog for this thread
 		TRACE_ERROR("POS  > Could not startup thread (not enough memory available)");
diff --git a/tracker/software/threads/threads.c b/tracker/software/threads/threads.c
index 7bae335..9428044 100644
--- a/tracker/software/threads/threads.c
+++ b/tracker/software/threads/threads.c
@@ -8,7 +8,6 @@
 #include "pi2c.h"
 #include "pac1720.h"
 
-mutex_t interference_mtx;
 systime_t watchdog_tracking;
 
 void start_essential_threads(void) {
@@ -16,7 +15,6 @@ void start_essential_threads(void) {
 	init_watchdog();
 
 	// Initialize essential mutex's
-	chMtxObjectInit(&interference_mtx);
 	chMtxObjectInit(&camera_mtx);
 	chMtxObjectInit(&radio_mtx);
 
diff --git a/tracker/software/threads/threads.h b/tracker/software/threads/threads.h
index d95ed70..75977b7 100644
--- a/tracker/software/threads/threads.h
+++ b/tracker/software/threads/threads.h
@@ -11,7 +11,6 @@
 
 void start_essential_threads(void);
 
-extern mutex_t interference_mtx;	// HF interference mutex (needed to exclude radio from HF sensitiv components [Camera])
 extern systime_t watchdog_tracking;	// Last update time for module TRACKING
 
 #endif
diff --git a/tracker/software/threads/tracking.c b/tracker/software/threads/tracking.c
index db1c85c..f17c70f 100644
--- a/tracker/software/threads/tracking.c
+++ b/tracker/software/threads/tracking.c
@@ -233,7 +233,7 @@ THD_FUNCTION(trackingThread, arg) {
 
 		// Switch on GPS is enough power is available
 		uint16_t batt = getBatteryVoltageMV();
-		if(batt >= GPS_ON_VBAT)
+		if(batt >= gps_on_vbat)
 		{
 			// Switch on GPS
 			GPS_Init();
@@ -242,21 +242,21 @@ THD_FUNCTION(trackingThread, arg) {
 			do {
 				batt = getBatteryVoltageMV();
 				gps_get_fix(&gpsFix);
-			} while(!isGPSLocked(&gpsFix) && batt >= GPS_OFF_VBAT && chVTGetSystemTimeX() <= time + S2ST(TRACK_CYCLE_TIME-5)); // Do as long no GPS lock and within timeout, timeout=cycle-1sec (-1sec in order to keep synchronization)
+			} while(!isGPSLocked(&gpsFix) && batt >= gps_off_vbat && chVTGetSystemTimeX() <= time + track_cycle_time - S2ST(3)); // Do as long no GPS lock and within timeout, timeout=cycle-1sec (-3sec in order to keep synchronization)
 
-			if(batt < GPS_OFF_VBAT) // Switch off GPS at low batt
+			if(batt < gps_off_vbat) // Switch off GPS at low batt
 				GPS_Deinit();
 		}
 
 		if(isGPSLocked(&gpsFix)) { // GPS locked
 
 			// Switch off GPS (if cycle time is more than 60 seconds)
-			#if TRACK_CYCLE_TIME >= 60
-			TRACE_INFO("TRAC > Switch off GPS");
-			GPS_Deinit();
-			#else
-			TRACE_INFO("TRAC > Keep GPS switched of because cycle < 60sec");
-			#endif
+			if(track_cycle_time >= S2ST(60)) {
+				TRACE_INFO("TRAC > Switch off GPS");
+				GPS_Deinit();
+			} else {
+				TRACE_INFO("TRAC > Keep GPS switched of because cycle < 60sec");
+			}
 
 			// Debug
 			TRACE_INFO("TRAC > GPS sampling finished GPS LOCK");
@@ -283,7 +283,7 @@ THD_FUNCTION(trackingThread, arg) {
 		} else { // GPS lost (keep GPS switched on)
 
 			// Debug
-			if(batt < GPS_OFF_VBAT) {
+			if(batt < gps_off_vbat) {
 				TRACE_WARN("TRAC > GPS sampling finished GPS LOW BATT");
 			} else {
 				TRACE_WARN("TRAC > GPS sampling finished GPS LOSS");
@@ -304,7 +304,7 @@ THD_FUNCTION(trackingThread, arg) {
 			tp->gps_alt = ltp->gps_alt;
 
 			// Mark GPS loss (or low batt)
-			tp->gps_lock = batt < GPS_OFF_VBAT ? GPS_LOWBATT : GPS_LOSS;
+			tp->gps_lock = batt < gps_off_vbat ? GPS_LOWBATT : GPS_LOSS;
 			tp->gps_sats = 0;
 
 		}
@@ -353,14 +353,14 @@ THD_FUNCTION(trackingThread, arg) {
 		if(nextLogEntryTimer <= chVTGetSystemTimeX() && isGPSLocked(&gpsFix))
 		{
 			writeLogTrackPoint(tp);
-			nextLogEntryTimer += S2ST(LOG_CYCLE_TIME);
+			nextLogEntryTimer += log_cycle_time;
 		}
 
 		// Switch last recent track point
 		lastTrackPoint = tp;
 		id++;
 
-		time = chThdSleepUntilWindowed(time, time + S2ST(TRACK_CYCLE_TIME)); // Wait until time + cycletime
+		time = chThdSleepUntilWindowed(time, time + track_cycle_time); // Wait until time + cycletime
 	}
 }
 
diff --git a/tracker/software/types.h b/tracker/software/types.h
index 8597623..b18c315 100644
--- a/tracker/software/types.h
+++ b/tracker/software/types.h
@@ -99,7 +99,6 @@ typedef struct { // Radio message type
 } radioMSG_t;
 
 typedef enum {
-	RES_QCIF,
 	RES_QVGA,
 	RES_VGA,
 	RES_XGA,
@@ -108,7 +107,7 @@ typedef enum {
 } resolution_t;
 
 typedef struct {
-	char callsign[8];		// Callsign
+	char callsign[7];		// Callsign (or stream identifier)
 	resolution_t res;		// Camera resolution
 	uint8_t quality;		// JPEG quality
 	uint8_t *ram_buffer;	// Camera Buffer