diff --git a/.travis.yml b/.travis.yml
index 93b3c15b3..3a9ecd43c 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -39,5 +39,5 @@ addons:
       - kalakris-cmake
   artifacts:
     paths:
-      - $(ls build/firmware/portapack-h1-firmware-*.tar.bz2 | tr "\n" ":")
-      - $(ls build/firmware/portapack-h1-firmware-*.zip | tr "\n" ":")
+      - $(ls build/firmware/portapack-h1-havoc-*.tar.bz2 | tr "\n" ":")
+      - $(ls build/firmware/portapack-h1-havoc-*.zip | tr "\n" ":")
diff --git a/firmware/application/main.cpp b/firmware/application/main.cpp
index 630aca26a..f17fe906b 100755
--- a/firmware/application/main.cpp
+++ b/firmware/application/main.cpp
@@ -23,7 +23,7 @@
 // Color bitmaps generated with:
 // Gimp image > indexed colors (16), then "xxd -i *.bmp"
 
-//TODO: De bruijn sequence scanner for encoders
+//TEST: ADS-B tx manchester encoder, velocity and squawk frames
 //TEST: Mic tx
 //TEST: Menuview refresh, seems to blink a lot
 //TEST: Check AFSK transmit end, skips last bits ?
@@ -37,6 +37,7 @@
 //BUG: REPLAY freezes when SD card not present
 //BUG: RDS doesn't stop baseband when stopping tx ?
 
+//TODO: De bruijn sequence scanner for encoders
 //TODO: Make freqman refresh simpler (use previous black rectangle method)
 //TODO: Merge AFSK and TONES procs ?
 //TODO: NFM RX mode: nav.pop on squelch
diff --git a/firmware/application/portapack.cpp b/firmware/application/portapack.cpp
index 5e1c51917..98d971294 100644
--- a/firmware/application/portapack.cpp
+++ b/firmware/application/portapack.cpp
@@ -279,9 +279,9 @@ bool init() {
 		return false;
 	}
 
-	/*if( !hackrf::cpld::load_sram() ) {
+	if( !hackrf::cpld::load_sram() ) {
 		chSysHalt();
-	}*/
+	}
 
 	portapack::io.init();
 
diff --git a/firmware/application/ui_adsb_tx.cpp b/firmware/application/ui_adsb_tx.cpp
index 6445aa278..4e63e5bbf 100644
--- a/firmware/application/ui_adsb_tx.cpp
+++ b/firmware/application/ui_adsb_tx.cpp
@@ -23,6 +23,7 @@
 #include "ui_adsb_tx.hpp"
 #include "ui_alphanum.hpp"
 
+#include "manchester.hpp"
 #include "string_format.hpp"
 #include "portapack.hpp"
 #include "baseband_api.hpp"
@@ -35,6 +36,47 @@ using namespace portapack;
 
 namespace ui {
 
+Compass::Compass(
+	const Point parent_pos
+) : Widget { { parent_pos, { 64, 64 } } }
+{
+	set_focusable(false);	// Useless ?
+}
+
+void Compass::set_value(uint32_t new_value) {
+	Point center = screen_pos() + Point(32, 32);
+	
+	new_value = clamp_value(new_value);
+	
+	display.draw_line(
+		center,
+		center + Point(sin_f32(DEG_TO_RAD(value_) + (pi / 2)) * 28, -sin_f32(DEG_TO_RAD(value_)) * 28),
+		Color::dark_grey()
+	);
+	
+	display.draw_line(
+		center,
+		center + Point(sin_f32(DEG_TO_RAD(new_value) + (pi / 2)) * 28, -sin_f32(DEG_TO_RAD(new_value)) * 28),
+		Color::green()
+	);
+
+	value_ = new_value;
+}
+
+void Compass::paint(Painter&) {
+	display.fill_circle(screen_pos() + Point(32, 32), 32, Color::dark_grey(), Color::black());
+	display.fill_rectangle({ screen_pos() + Point(32 - 2, 0), { 4, 4 } }, Color::black());	// N
+	display.fill_rectangle({ screen_pos() + Point(32 - 2, 64 - 4), { 4, 4 } }, Color::black());	// S
+	display.fill_rectangle({ screen_pos() + Point(0, 32 - 2), { 4, 4 } }, Color::black());	// W
+	display.fill_rectangle({ screen_pos() + Point(64 - 4, 32 - 2), { 4, 4 } }, Color::black());	// E
+	
+	set_value(value_);
+}
+
+uint32_t Compass::clamp_value(uint32_t value) {
+	return range.clip(value);
+}
+
 void ADSBTxView::focus() {
 	tx_view.focus();
 }
@@ -48,22 +90,25 @@ void ADSBTxView::paint(Painter&) {
 	button_callsign.set_text(callsign);
 }
 
-void ADSBTxView::generate_frame() {
-	uint32_t c;
+void ADSBTxView::generate_frames() {
 	uint8_t * bin_ptr = shared_memory.bb_data.data;
 	
-	generate_frame_id(frames[0], sym_icao.value_hex_u64(), callsign);
-	generate_frame_pos(frames[1], sym_icao.value_hex_u64(), 5000, field_lat_degrees.value(), field_lon_degrees.value(), 0);
-	generate_frame_pos(frames[2], sym_icao.value_hex_u64(), 5000, field_lat_degrees.value(), field_lon_degrees.value(), 1);
+	encode_frame_id(frames[0], sym_icao.value_hex_u64(), callsign);
+	
+	encode_frame_pos(frames[1], sym_icao.value_hex_u64(), field_altitude.value(),
+		field_lat_degrees.value(), field_lon_degrees.value(), 0);
+	encode_frame_pos(frames[2], sym_icao.value_hex_u64(), field_altitude.value(),
+		field_lat_degrees.value(), field_lon_degrees.value(), 1);
 
 	memset(bin_ptr, 0, 240);
 
 	auto raw_ptr = frames[0].get_raw_data();
 	
+	// The preamble isn't manchester encoded
 	memcpy(bin_ptr, adsb_preamble, 16);
 	
-	// Convert to binary (1 byte per bit, faster for baseband code)
-	for (c = 0; c < 112; c++) {
+	// Convert to binary with manchester encoding (1 byte per bit, faster for baseband code)
+	/*for (c = 0; c < 112; c++) {
 		if ((raw_ptr[c >> 3] << (c & 7)) & 0x80) {
 			bin_ptr[(c * 2) + 16] = 1;
 			bin_ptr[(c * 2) + 16 + 1] = 0;
@@ -71,19 +116,20 @@ void ADSBTxView::generate_frame() {
 			bin_ptr[(c * 2) + 16] = 0;
 			bin_ptr[(c * 2) + 16 + 1] = 1;
 		}
-	}
+	}*/
+	
+	manchester_encode(bin_ptr + 16, raw_ptr, 112, 0);
 	
 	// Display in hex for debug
-	text_frame_a.set(to_string_hex_array(frames[0].get_raw_data(), 7));
-	text_frame_b.set(to_string_hex_array(frames[0].get_raw_data() + 7, 7));
+	text_frame.set(to_string_hex_array(frames[0].get_raw_data(), 14));
 
 	button_callsign.set_text(callsign);
 }
 
-bool ADSBTxView::start_tx() {
-	generate_frame();
+void ADSBTxView::start_tx() {
+	generate_frames();
 	
-	transmitter_model.set_tuning_frequency(434000000);
+	transmitter_model.set_tuning_frequency(434000000);	// DEBUG
 	transmitter_model.set_sampling_rate(4000000U);
 	transmitter_model.set_rf_amp(true);
 	transmitter_model.set_vga(40);
@@ -91,8 +137,6 @@ bool ADSBTxView::start_tx() {
 	transmitter_model.enable();
 	
 	baseband::set_adsb();
-	
-	return true;
 }
 
 void ADSBTxView::on_txdone(const bool v) {
@@ -102,7 +146,7 @@ void ADSBTxView::on_txdone(const bool v) {
 	}
 }
 
-void ADSBTxView::rotate_frames() {
+/*void ADSBTxView::rotate_frames() {
 	// DEBUG
 	uint8_t * bin_ptr = shared_memory.bb_data.data;
 	uint8_t * raw_ptr;
@@ -114,10 +158,10 @@ void ADSBTxView::rotate_frames() {
 		if (!regen) {
 			regen = 10;
 			
-			generate_frame_id(frames[0], plane_index, "DEMO" + to_string_dec_uint(plane_index));
-			generate_frame_pos(frames[1], plane_index, 5000, plane_lats[plane_index]/8 + offs + 38.5, plane_lons[plane_index]/8 + 125.8, 0);
-			generate_frame_pos(frames[2], plane_index, 5000, plane_lats[plane_index]/8 + offs + 38.5, plane_lons[plane_index]/8 + 125.8, 1);
-			generate_frame_identity(frames[3], plane_index, 1337);
+			encode_frame_id(frames[0], plane_index, "DEMO" + to_string_dec_uint(plane_index));
+			encode_frame_pos(frames[1], plane_index, 5000, plane_lats[plane_index]/8 + offs + 38.5, plane_lons[plane_index]/8 + 125.8, 0);
+			encode_frame_pos(frames[2], plane_index, 5000, plane_lats[plane_index]/8 + offs + 38.5, plane_lons[plane_index]/8 + 125.8, 1);
+			encode_frame_identity(frames[3], plane_index, 1337);
 			
 			if (plane_index == 11)
 				plane_index = 0;
@@ -156,7 +200,7 @@ void ADSBTxView::rotate_frames() {
 			frame_index++;
 		}
 	}
-}
+}*/
 
 ADSBTxView::ADSBTxView(NavigationView& nav) {
 	uint32_t c;
@@ -165,7 +209,7 @@ ADSBTxView::ADSBTxView(NavigationView& nav) {
 
 	add_children({
 		&labels,
-		&options_format,
+		//&options_format,
 		&sym_icao,
 		&button_callsign,
 		&field_altitude,
@@ -175,50 +219,67 @@ ADSBTxView::ADSBTxView(NavigationView& nav) {
 		&field_lon_degrees,
 		&field_lon_minutes,
 		&field_lon_seconds,
+		&compass,
+		&field_angle,
+		&field_speed,
 		&check_emergency,
 		&field_squawk,
-		&text_frame_a,
-		&text_frame_b,
+		&text_frame,
 		&tx_view
 	});
 	
-	options_format.set_by_value(17);	// Mode S
+	/*options_format.set_by_value(17);	// Mode S
 	
 	options_format.on_change = [this](size_t, int32_t) {
-		generate_frame();
-	};
+		generate_frames();
+	};*/
+	
 	sym_icao.on_change = [this]() {
-		generate_frame();
+		generate_frames();
 	};
 	button_callsign.on_select = [this, &nav](Button&) {
 		text_prompt(nav, &callsign, 8);
 	};
 	
-	field_altitude.set_value(11000);
+	field_altitude.set_value(36000);
 	field_lat_degrees.set_value(0);
 	field_lat_minutes.set_value(0);
 	field_lat_seconds.set_value(0);
 	field_lon_degrees.set_value(0);
 	field_lon_minutes.set_value(0);
 	field_lon_seconds.set_value(0);
+	field_angle.set_value(0);
+	field_speed.set_value(0);
 	
+	field_altitude.on_change = [this](int32_t) {
+		generate_frames();
+	};
 	field_lat_degrees.on_change = [this](int32_t) {
-		generate_frame();
+		generate_frames();
 	};
 	field_lon_degrees.on_change = [this](int32_t) {
-		generate_frame();
+		generate_frames();
+	};
+	
+	field_angle.on_change = [this](int32_t v) {
+		compass.set_value(v);
+		generate_frames();
+	};
+	
+	field_speed.on_change = [this](int32_t) {
+		generate_frames();
 	};
 	
 	for (c = 0; c < 4; c++)
 		field_squawk.set_sym(c, 0);
 	
-	generate_frame();
+	generate_frames();
 	
 	receiver_model.set_tuning_frequency(434000000);		// DEBUG
 	
 	tx_view.on_start = [this]() {
-		if (start_tx())
-			tx_view.set_transmitting(true);
+		start_tx();
+		tx_view.set_transmitting(true);
 		//rotate_frames();
 	};
 	
diff --git a/firmware/application/ui_adsb_tx.hpp b/firmware/application/ui_adsb_tx.hpp
index ee63f2a48..d741f7379 100644
--- a/firmware/application/ui_adsb_tx.hpp
+++ b/firmware/application/ui_adsb_tx.hpp
@@ -22,10 +22,11 @@
 
 #include "ui.hpp"
 #include "adsb.hpp"
+#include "utility.hpp"
+#include "sine_table.hpp"
 #include "ui_textentry.hpp"
 #include "ui_widget.hpp"
 #include "ui_navigation.hpp"
-#include "ui_font_fixed_8x16.hpp"
 #include "ui_transmitter.hpp"
 
 #include "message.hpp"
@@ -36,6 +37,21 @@ using namespace adsb;
 
 namespace ui {
 
+class Compass : public Widget {
+public:
+	Compass(const Point parent_pos);
+
+	void set_value(uint32_t new_value);
+
+	void paint(Painter&) override;
+
+private:
+	const range_t<uint32_t> range { 0, 359 };
+	uint32_t value_ { 0 };
+
+	uint32_t clamp_value(uint32_t value);
+};
+
 class ADSBTxView : public View {
 public:
 	ADSBTxView(NavigationView& nav);
@@ -85,37 +101,28 @@ private:
 	
 	tx_modes tx_mode = IDLE;
 	
-	std::string callsign = "PORTAPAC";
+	std::string callsign = "TEST1234";
 	
 	ADSBFrame frames[4] { };
 	
-	bool start_tx();
-	void generate_frame();
-	void rotate_frames();
+	void start_tx();
+	void generate_frames();
+	//void rotate_frames();
 	void on_txdone(const bool v);
 	
-	const Style style_val {
-		.font = font::fixed_8x16,
-		.background = Color::black(),
-		.foreground = Color::green(),
-	};
-	const Style style_cancel {
-		.font = font::fixed_8x16,
-		.background = Color::black(),
-		.foreground = Color::red(),
-	};
-	
 	Labels labels {
-		{ { 2 * 8, 2 * 8 }, "Format:", Color::light_grey() },
+		{ { 2 * 8, 2 * 8 }, "Format: 17 (ADS-B)", Color::light_grey() },
 		{ { 2 * 8, 4 * 8 }, "ICAO24:", Color::light_grey() },
-		{ { 2 * 8, 7 * 8 }, "ID:", Color::light_grey() },
-		{ { 2 * 8, 10 * 8 }, "Altitude:       feet", Color::light_grey() },
-		{ { 2 * 8, 12 * 8 }, "Latitude:     *  '  \"", Color::light_grey() },	// No ° symbol in 8x16 font
-		{ { 2 * 8, 14 * 8 }, "Longitude:    *  '  \"", Color::light_grey() },	// No ° symbol in 8x16 font
-		{ { 15 * 8, 18 * 8 }, "Squawk", Color::light_grey() }
+		{ { 2 * 8, 7 * 8 }, "Callsign:", Color::light_grey() },
+		{ { 1 * 8, 11 * 8 }, "Alt:       feet", Color::light_grey() },
+		{ { 1 * 8, 13 * 8 }, "Lat:     *  '  \"", Color::light_grey() },	// No ° symbol in 8x16 font
+		{ { 1 * 8, 15 * 8 }, "Lon:     *  '  \"", Color::light_grey() },
+		{ { 1 * 8, 18 * 8 }, "Speed:    kn  Bearing:    *", Color::light_grey() },
+		{ { 16 * 8, 22 * 8 }, "Squawk:", Color::light_grey() }
 	};
 	
-	OptionsField options_format {
+	// Only ADS-B is implemented right now
+	/*OptionsField options_format {
 		{ 10 * 8, 1 * 16 },
 		9,
 		{
@@ -123,7 +130,7 @@ private:
 			{ "18: TIS-B", 18 },
 			{ "19: MIL  ", 19 },
 		}
-	};
+	};*/
 	
 	SymField sym_icao {
 		{ 10 * 8, 2 * 16 },
@@ -132,12 +139,12 @@ private:
 	};
 
 	Button button_callsign {
-		{ 6 * 8, 3 * 16 + 4, 10 * 8, 24 },
+		{ 12 * 8, 3 * 16 + 4, 10 * 8, 24 },
 		""
 	};
 	
 	NumberField field_altitude {
-		{ 12 * 8, 5 * 16 },
+		{ 6 * 8, 11 * 8 },
 		5,
 		{ -1000, 50000 },
 		250,
@@ -145,44 +152,50 @@ private:
 	};
 	
 	NumberField field_lat_degrees {
-		{ 12 * 8, 6 * 16 }, 4, { -90, 90 }, 1, ' '
+		{ 6 * 8, 13 * 8 }, 4, { -90, 90 }, 1, ' '
 	};
 	NumberField field_lat_minutes {
-		{ 17 * 8, 6 * 16 }, 2, { 0, 59 }, 1, ' '
+		{ 11 * 8, 13 * 8 }, 2, { 0, 59 }, 1, ' '
 	};
 	NumberField field_lat_seconds {
-		{ 20 * 8, 6 * 16 }, 2, { 0, 59 }, 1, ' '
+		{ 14 * 8, 13 * 8 }, 2, { 0, 59 }, 1, ' '
 	};
 	
 	NumberField field_lon_degrees {
-		{ 12 * 8, 7 * 16 }, 4, { -180, 180 }, 1, ' '
+		{ 6 * 8, 15 * 8 }, 4, { -180, 180 }, 1, ' '
 	};
 	NumberField field_lon_minutes {
-		{ 17 * 8, 7 * 16 }, 2, { 0, 59 }, 1, ' '
+		{ 11 * 8, 15 * 8 }, 2, { 0, 59 }, 1, ' '
 	};
 	NumberField field_lon_seconds {
-		{ 20 * 8, 7 * 16 }, 2, { 0, 59 }, 1, ' '
+		{ 14 * 8, 15 * 8 }, 2, { 0, 59 }, 1, ' '
+	};
+	
+	Compass compass {
+		{ 21 * 8, 5 * 16 }
+	};
+	NumberField field_angle {
+		{ 21 * 8 + 20, 9 * 16 }, 3, { 0, 359 }, 1, ' '
+	};
+	
+	NumberField field_speed {
+		{ 8 * 8, 18 * 8 }, 3, { 0, 999 }, 5, ' '
 	};
 	
 	Checkbox check_emergency {
-		{ 2 * 8, 9 * 16 - 4 },
+		{ 2 * 8, 11 * 16 - 4 },
 		9,
 		"Emergency",
 		false
 	};
-	
 	SymField field_squawk {
-		{ 22 * 8, 9 * 16 },
+		{ 24 * 8, 11 * 16 },
 		4,
 		SymField::SYMFIELD_OCT
 	};
 	
-	Text text_frame_a {
-		{ 2 * 8, 13 * 16, 14 * 8, 16 },
-		"-"
-	};
-	Text text_frame_b {
-		{ 2 * 8, 14 * 16, 14 * 8, 16 },
+	Text text_frame {
+		{ 1 * 8, 29 * 8, 14 * 8, 16 },
 		"-"
 	};
 	
diff --git a/firmware/common/adsb.cpp b/firmware/common/adsb.cpp
index eda163760..629631bbc 100644
--- a/firmware/common/adsb.cpp
+++ b/firmware/common/adsb.cpp
@@ -21,28 +21,29 @@
  */
 
 #include "adsb.hpp"
+#include "sine_table.hpp"
 
 #include <math.h>
 
 namespace adsb {
 
-void make_frame_mode_s(ADSBFrame& frame, const uint32_t ICAO_address) {
+void make_frame_adsb(ADSBFrame& frame, const uint32_t ICAO_address) {
 	frame.clear();
-	frame.push_byte((17 << 3) | 5);			// DF17 and CA
+	frame.push_byte((DF_ADSB << 3) | 5);	// DF and CA
 	frame.push_byte(ICAO_address >> 16);
 	frame.push_byte(ICAO_address >> 8);
 	frame.push_byte(ICAO_address & 0xFF);
 }
 
-void generate_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std::string& callsign) {
+void encode_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std::string& callsign) {
 	std::string callsign_formatted(8, '_');
 	uint64_t callsign_coded = 0;
 	uint32_t c, s;
 	char ch;
 	
-	make_frame_mode_s(frame, ICAO_address);
+	make_frame_adsb(frame, ICAO_address);
 	
-	frame.push_byte(4 << 3);	// TC = 4: Aircraft ID
+	frame.push_byte(TC_IDENT << 3);		// No aircraft category
 	
 	// Translate and encode callsign
 	for (c = 0; c < 8; c++) {
@@ -78,15 +79,39 @@ void generate_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std:
 	frame.make_CRC();
 }*/
 
-void generate_frame_identity(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t code) {
+void encode_frame_squawk(ADSBFrame& frame, const uint32_t squawk) {
+	uint32_t squawk_coded;
+	
 	frame.clear();
 	
-	frame.push_byte(21 << 3);		// DF = 21
+	frame.push_byte(DF_EHS_SQUAWK << 3);	// DF
 	frame.push_byte(0);
 	
-	// 1337:
-	frame.push_byte(0b00011100);
-	frame.push_byte(0b00111101);
+	// 12 11 10  9  8  7  6  5  4  3  2  1  0
+	// 31 30 29 28 27 26 25 24 23 22 21 20 19
+	// D4 B4 D2 B2 D1 B1 __ A4 C4 A2 C2 A1 C1
+	// ABCD = code (octal, 0000~7777)
+	
+	// FEDCBA9876543210
+	// xAAAxBBBxCCCxDDD
+	// x421x421x421x421
+	
+	squawk_coded = ((squawk << 10) & 0x1000) |	// D4
+					((squawk << 1) & 0x0800) |	// B4
+					((squawk << 9) & 0x0400) |	// D2
+					((squawk << 0) & 0x0200) |	// B2
+					((squawk << 8) & 0x0100) |	// D1
+					((squawk >> 1) & 0x0080) |	// B1
+					
+					((squawk >> 9) & 0x0020) |	// A4
+					((squawk >> 2) & 0x0010) |	// C4
+					((squawk >> 10) & 0x0008) |	// A2
+					((squawk >> 3) & 0x0004) |	// C2
+					((squawk >> 11) & 0x0002) |	// A1
+					((squawk >> 4) & 0x0001);	// C1
+	
+	frame.push_byte(squawk_coded >> 5);
+	frame.push_byte(squawk_coded << 3);
 	
 	frame.make_CRC();
 }
@@ -116,41 +141,44 @@ int cpr_N(float lat, int is_odd) {
     return nl;
 }
 
-void generate_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t altitude,
+// Decoding method (from dump1090):
+// index int j = floor(((59 * latcprE - 60 * latcprO) / 131072) + 0.50)
+// latE = DlatE * (cpr_mod(j, 60) + (latcprE / 131072))
+// latO = DlatO * (cpr_mod(j, 59) + (latcprO / 131072))
+// if latE >= 270 -> latE -= 360
+// if latO >= 270 -> latO -= 360
+// if (cpr_NL(latE) != cpr_NL(latO)) return;
+
+// int ni = cpr_N(latE ,0);
+// int m = floor((((loncprE * (cpr_NL(latE) - 1)) - (loncprO * cpr_NL(latE))) / 131072) + 0.5)
+// lon = cpr_Dlon(latE, 0) * (cpr_mod(m, ni) + loncprE / 131072);
+// lat = latE;
+// ... or ...
+// int ni = cpr_N(latO ,0);
+// int m = floor((((loncprE * (cpr_NL(latO) - 1)) - (loncprO * cpr_NL(latO))) / 131072) + 0.5)
+// lon = cpr_Dlon(latO, 0) * (cpr_mod(m, ni) + loncprO / 131072);
+// lat = latO;
+// ... and ...
+// if (lon > 180) lon -= 360;
+
+void encode_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t altitude,
 	const float latitude, const float longitude, const uint32_t time_parity) {
+	
 	uint32_t altitude_coded;
 	uint32_t lat, lon;
 	float delta_lat, yz, rlat, delta_lon, xz;
 	
-	make_frame_mode_s(frame, ICAO_address);
+	make_frame_adsb(frame, ICAO_address);
 	
-	frame.push_byte(11 << 3);		// TC = 11 (Airborne position)
+	frame.push_byte(TC_AIRBORNE_POS << 3);		// Bits 2~1: Surveillance Status, bit 0: NICsb
+	
+	altitude_coded = (altitude + 1000) / 25;	// 25ft precision, insert Q-bit (1)
+	altitude_coded = ((altitude_coded & 0x7F0) << 1) | 0x10 | (altitude_coded & 0x0F);
 	
-	altitude_coded = (altitude + 1000) / 25;	// Can be coded in 100ft steps also
 	frame.push_byte(altitude_coded >> 4);		// Top-most altitude bits
 	
-	// Decoding method (from dump1090):
-	// index int j = floor(((59 * latcprE - 60 * latcprO) / 131072) + 0.50)
-	// latE = DlatE * (cpr_mod(j, 60) + (latcprE / 131072))
-	// latO = DlatO * (cpr_mod(j, 59) + (latcprO / 131072))
-	// if latE >= 270 -> latE -= 360
-	// if latO >= 270 -> latO -= 360
-	// if (cpr_NL(latE) != cpr_NL(latO)) return;
-	
-	// int ni = cpr_N(latE ,0);
-	// int m = floor((((loncprE * (cpr_NL(latE) - 1)) - (loncprO * cpr_NL(latE))) / 131072) + 0.5)
-	// lon = cpr_Dlon(latE, 0) * (cpr_mod(m, ni) + loncprE / 131072);
-	// lat = latE;
-	// ... or ...
-	// int ni = cpr_N(latO ,0);
-	// int m = floor((((loncprE * (cpr_NL(latO) - 1)) - (loncprO * cpr_NL(latO))) / 131072) + 0.5)
-	// lon = cpr_Dlon(latO, 0) * (cpr_mod(m, ni) + loncprO / 131072);
-	// lat = latO;
-	// ... and ...
-	// if (lon > 180) lon -= 360;
-	
 	// CPR encoding
-	// Info: http://antena.fe.uni-lj.si/literatura/Razno/Avionika/modes/CPRencoding.pdf
+	// Info from: http://antena.fe.uni-lj.si/literatura/Razno/Avionika/modes/CPRencoding.pdf
 	
 	delta_lat = 360.0 / ((4.0 * 15.0) - time_parity);		// NZ = 15
 	yz = floor(131072.0 * (cpr_mod(latitude, delta_lat) / delta_lat) + 0.5);
@@ -165,7 +193,7 @@ void generate_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const uin
 	lat = cpr_mod(yz, 131072.0);
 	lon = cpr_mod(xz, 131072.0);
 	
-	frame.push_byte((altitude_coded << 4) | ((uint32_t)time_parity << 2) | (lat >> 15));
+	frame.push_byte((altitude_coded << 4) | ((uint32_t)time_parity << 2) | (lat >> 15));	// T = 0
 	frame.push_byte(lat >> 7);
 	frame.push_byte((lat << 1) | (lon >> 16));
 	frame.push_byte(lon >> 8);
@@ -174,4 +202,35 @@ void generate_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const uin
 	frame.make_CRC();
 }
 
+// speed is in knots
+// vertical rate is in ft/min
+void encode_frame_velo(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t speed,
+	const float angle, const int32_t v_rate) {
+	
+	int32_t velo_ew, velo_ns, v_rate_coded;
+	uint32_t velo_ew_abs, velo_ns_abs, v_rate_coded_abs;
+	
+	// To get NS and EW speeds from speed and bearing, a polar to cartesian conversion is enough
+	velo_ew = static_cast<int32_t>(sin_f32(DEG_TO_RAD(angle) + (pi / 2)) * speed);
+	velo_ns = static_cast<int32_t>(sin_f32(DEG_TO_RAD(angle)) * speed);
+	
+	v_rate_coded = (v_rate / 64) + 1;
+	
+	velo_ew_abs = abs(velo_ew);
+	velo_ns_abs = abs(velo_ns);
+	v_rate_coded_abs = abs(v_rate_coded);
+	
+	make_frame_adsb(frame, ICAO_address);
+	
+	frame.push_byte((TC_AIRBORNE_VELO << 3) | 1);		// Subtype: 1 (subsonic)
+	frame.push_byte(((velo_ew < 0 ? 1 : 0) << 2) | (velo_ew_abs >> 8));
+	frame.push_byte(velo_ew_abs);
+	frame.push_byte(((velo_ns < 0 ? 1 : 0) << 7) | (velo_ns_abs >> 3));
+	frame.push_byte((velo_ns_abs << 5) | ((v_rate_coded < 0 ? 1 : 0) << 3) | (v_rate_coded_abs >> 6));	// VrSrc = 0
+	frame.push_byte(v_rate_coded_abs << 2);
+	frame.push_byte(0);
+	
+	frame.make_CRC();
+}
+
 } /* namespace adsb */
diff --git a/firmware/common/adsb.hpp b/firmware/common/adsb.hpp
index 7f31c42d4..332f2d741 100644
--- a/firmware/common/adsb.hpp
+++ b/firmware/common/adsb.hpp
@@ -30,6 +30,26 @@
 
 namespace adsb {
 
+#define DEG_TO_RAD(d) (d * (2 * pi) / 360.0)
+
+enum downlink_format {
+	DF_ADSB = 17,
+	DF_EHS_SQUAWK = 21
+};
+
+enum type_code {
+	TC_IDENT = 4,
+	TC_AIRBORNE_POS = 11,
+	TC_AIRBORNE_VELO = 19
+};
+
+enum data_selector {
+	BDS_ID = 0x20,
+	BDS_ID_MARKS = 0x21,
+	BDS_INTENT = 0x40,
+	BDS_HEADING = 0x60
+};
+
 const float adsb_lat_lut[58] = {
 	10.47047130,    14.82817437,    18.18626357,    21.02939493,
     23.54504487,    25.82924707,    27.93898710,    29.91135686,
@@ -48,12 +68,14 @@ const float adsb_lat_lut[58] = {
     86.53536998,    87.00000000
 };
 
-void make_frame_mode_s(ADSBFrame& frame, const uint32_t ICAO_address);
-void generate_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std::string& callsign);
-void generate_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t altitude,
+void make_frame_adsb(ADSBFrame& frame, const uint32_t ICAO_address);
+void encode_frame_id(ADSBFrame& frame, const uint32_t ICAO_address, const std::string& callsign);
+void encode_frame_pos(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t altitude,
 	const float latitude, const float longitude, const uint32_t time_parity);
-void generate_frame_emergency(ADSBFrame& frame, const uint32_t ICAO_address, const uint8_t code);
-void generate_frame_identity(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t code);
+void encode_frame_velo(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t speed,
+	const float angle, const int32_t v_rate);
+void encode_frame_emergency(ADSBFrame& frame, const uint32_t ICAO_address, const uint8_t code);
+void encode_frame_identity(ADSBFrame& frame, const uint32_t ICAO_address, const uint32_t code);
 
 } /* namespace adsb */
 
diff --git a/firmware/common/manchester.cpp b/firmware/common/manchester.cpp
index 15196191c..100710db0 100644
--- a/firmware/common/manchester.cpp
+++ b/firmware/common/manchester.cpp
@@ -69,3 +69,14 @@ FormattedSymbols format_symbols(
 
 	return { hex_data, hex_error };
 }
+
+void manchester_encode(uint8_t * dest, uint8_t * src, size_t length, const size_t sense) {
+	uint_fast8_t part = sense ? 0 : 0xFF;
+	
+	for (size_t c = 0; c < length; c++) {
+		if ((src[c >> 3] << (c & 7)) & 0x80) {
+			*(dest++) = part;
+			*(dest++) = ~part;
+		}
+	}
+}
diff --git a/firmware/common/manchester.hpp b/firmware/common/manchester.hpp
index 6a2376a98..72f3cc8c3 100644
--- a/firmware/common/manchester.hpp
+++ b/firmware/common/manchester.hpp
@@ -67,4 +67,6 @@ FormattedSymbols format_symbols(
 	const ManchesterDecoder& decoder
 );
 
+void manchester_encode(uint8_t * dest, uint8_t * src, size_t length, const size_t sense = 0);
+
 #endif/*__MANCHESTER_H__*/