From 6e5bb7911c448dbd5de38aa5ec686c86bc47e017 Mon Sep 17 00:00:00 2001
From: Brumi-2021 <ea3hqj@gmail.com>
Date: Thu, 5 Jan 2023 00:14:11 +0100
Subject: [PATCH] Added Vertical rate field ADSB-TX

---
 firmware/application/apps/ui_adsb_tx.cpp | 7 +++++--
 firmware/application/apps/ui_adsb_tx.hpp | 8 ++++++++
 firmware/common/adsb.cpp                 | 9 ++++-----
 3 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/firmware/application/apps/ui_adsb_tx.cpp b/firmware/application/apps/ui_adsb_tx.cpp
index 997bd65d..be41c7aa 100644
--- a/firmware/application/apps/ui_adsb_tx.cpp
+++ b/firmware/application/apps/ui_adsb_tx.cpp
@@ -162,13 +162,16 @@ ADSBSpeedView::ADSBSpeedView(
 	
 	add_children({
 		&labels_speed,
+		&labels_vert_rate,
 		&compass,
 		&field_angle,
-		&field_speed
+		&field_speed,
+		&field_vert_rate
 	});
 	
 	field_angle.set_value(0);
 	field_speed.set_value(400);
+	field_vert_rate.set_value(0);
 	
 	field_angle.on_change = [this](int32_t v) {
 		compass.set_value(v);
@@ -181,7 +184,7 @@ void ADSBSpeedView::collect_frames(const uint32_t ICAO_address, std::vector<ADSB
 	ADSBFrame temp_frame;
 	
 	encode_frame_velo(temp_frame, ICAO_address, field_speed.value(),
-		field_angle.value(), 0);	// TODO: v_rate
+		field_angle.value(), field_vert_rate.value());				// Added v_rate ,  ft/min  ,  (+) climb ,  (-) descend .
 	
 	frame_list.emplace_back(temp_frame);
 }
diff --git a/firmware/application/apps/ui_adsb_tx.hpp b/firmware/application/apps/ui_adsb_tx.hpp
index 8556f6e8..3f275074 100644
--- a/firmware/application/apps/ui_adsb_tx.hpp
+++ b/firmware/application/apps/ui_adsb_tx.hpp
@@ -96,6 +96,10 @@ private:
 		{ { 1 * 8, 6 * 16 }, "Speed:    kn  Bearing:    *", Color::light_grey() }
 	};
 	
+	Labels labels_vert_rate {
+		{ { 1 * 8, 8 * 16 }, "Vert. rate:    ft/min, (+/-)", Color::light_grey() }
+	};
+
 	Compass compass {
 		{ 21 * 8, 2 * 16 }
 	};
@@ -107,6 +111,10 @@ private:
 	NumberField field_speed {
 		{ 8 * 8, 6 * 16 }, 3, { 0, 999 }, 5, ' '
 	};
+	
+	NumberField field_vert_rate {
+		{ 11 * 8, 8 * 16 }, 5, { -4096, 4096 }, 64, ' '			// Let's limit to +/-5k aprox , Ex. max safe descent vertical rate aprox -1000 ft/min on an instrument approach. , std step is 64  
+	};
 };
 
 class ADSBSquawkView : public OptionTabView {
diff --git a/firmware/common/adsb.cpp b/firmware/common/adsb.cpp
index e8481f6f..c209cdf8 100644
--- a/firmware/common/adsb.cpp
+++ b/firmware/common/adsb.cpp
@@ -307,19 +307,18 @@ adsb_pos decode_frame_pos(ADSBFrame& frame_even, ADSBFrame& frame_odd) {
 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;
+	int32_t velo_ew, velo_ns;
 	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)  ) * speed);	            // East direction, is the projection from West -> East is directly sin(angle=Compas Bearing) , (90º is the max +1, EAST) max velo_EW 	
 	velo_ns = static_cast<int32_t>(sin_f32( (pi/2 - DEG_TO_RAD(angle) )  ) * speed);    // North direction,is the projection of North = cos(angle=Compas Bearing), cos(angle)= sen(90-angle) (0º is the max +1 NORTH) max velo_NS                                    
 	
-	v_rate_coded = (v_rate / 64) + 1;											//encoding vertical rate source.  (Decoding, VR ft/min = (Decimal v_rate_value - 1)* 64) 
+	v_rate_coded_abs = (abs(v_rate) / 64) + 1;									//encoding vertical rate source.  (Decoding, VR ft/min = (Decimal v_rate_value - 1)* 64) 
 	
 	velo_ew_abs = abs(velo_ew) + 1; 											// encoding Velo speed EW , when sign Direction is 0 (+): West->East, (-) 1: East->West
 	velo_ns_abs = abs(velo_ns) + 1;												// encoding Velo speed NS , when sign Direction is 0 (+): South->North , (-) 1: North->South
-	v_rate_coded_abs = abs(v_rate_coded);
-	
+		
 	make_frame_adsb(frame, ICAO_address);
 	
 	// Airborne velocities are all transmitted with Type Code 19 ( TC=19, using 5 bits ,TC=19 [Binary: 10011]), the following 3 bits are Subt-type Code ,SC= 1,2,3,4 
@@ -334,7 +333,7 @@ void encode_frame_velo(ADSBFrame& frame, const uint32_t ICAO_address, const uint
 	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((velo_ns_abs << 5) | ((v_rate < 0 ? 1 : 0) << 3) | (v_rate_coded_abs >> 6));	// VrSrc = 0
 	frame.push_byte(v_rate_coded_abs << 2);
 	frame.push_byte(0);