From 3a0329822ac691b1ac7fc26cb76a4d8cfd8c3cac Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 20 Jul 2020 11:48:37 +0100
Subject: [PATCH] AP_OSD: apply aspect ratio correction to horizon

---
 libraries/AP_OSD/AP_OSD_Backend.h   |  9 +++++++++
 libraries/AP_OSD/AP_OSD_MAX7456.cpp | 17 +++++++++++++++++
 libraries/AP_OSD/AP_OSD_MAX7456.h   |  3 +++
 libraries/AP_OSD/AP_OSD_Screen.cpp  |  6 ++++--
 4 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_OSD/AP_OSD_Backend.h b/libraries/AP_OSD/AP_OSD_Backend.h
index 44258495e9..afc3913e66 100644
--- a/libraries/AP_OSD/AP_OSD_Backend.h
+++ b/libraries/AP_OSD/AP_OSD_Backend.h
@@ -42,6 +42,9 @@ public:
     //update screen
     virtual void flush() = 0;
 
+    // return a correction factor used to display angles correctly
+    virtual float get_aspect_ratio_correction() const {return 1;}
+
     //clear screen
     //should match hw blink
     virtual void clear()
@@ -70,6 +73,12 @@ protected:
     }
 
     int8_t blink_phase;
+
+    enum vid_format {
+        FORMAT_UNKNOWN = 0,
+        FORMAT_NTSC = 1,
+        FORMAT_PAL = 2,
+    } _format;
 };
 
 
diff --git a/libraries/AP_OSD/AP_OSD_MAX7456.cpp b/libraries/AP_OSD/AP_OSD_MAX7456.cpp
index c7f8632f3d..25d4f5cee7 100644
--- a/libraries/AP_OSD/AP_OSD_MAX7456.cpp
+++ b/libraries/AP_OSD/AP_OSD_MAX7456.cpp
@@ -322,9 +322,11 @@ void AP_OSD_MAX7456::reinit()
     if (VIN_IS_PAL(sense)) {
         video_signal_reg = VIDEO_MODE_PAL | OSD_ENABLE;
         video_lines = video_lines_pal;
+        _format = FORMAT_PAL;
     } else {
         video_signal_reg = VIDEO_MODE_NTSC | OSD_ENABLE;
         video_lines = video_lines_ntsc;
+        _format = FORMAT_NTSC;
     }
 
     // set all rows to same character black/white level
@@ -461,3 +463,18 @@ void AP_OSD_MAX7456::write(uint8_t x, uint8_t y, const char* text)
         ++x;
     }
 }
+
+// return a correction factor used to display angles correctly
+float AP_OSD_MAX7456::get_aspect_ratio_correction() const
+{
+    switch (_format) {
+    case FORMAT_NTSC:
+        return 12.0f/18.46f;
+
+    case FORMAT_PAL:
+        return 12.0f/15.0f;
+
+    default:
+        return 1.0f;
+    };
+}
diff --git a/libraries/AP_OSD/AP_OSD_MAX7456.h b/libraries/AP_OSD/AP_OSD_MAX7456.h
index 7f5ca4d54d..655e698651 100644
--- a/libraries/AP_OSD/AP_OSD_MAX7456.h
+++ b/libraries/AP_OSD/AP_OSD_MAX7456.h
@@ -37,6 +37,9 @@ public:
     //clear framebuffer
     void clear() override;
 
+    // return a correction factor used to display angles correctly
+   float get_aspect_ratio_correction() const override;
+
 private:
 
     //constructor
diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp
index 619d30239a..b5d53fa9bd 100644
--- a/libraries/AP_OSD/AP_OSD_Screen.cpp
+++ b/libraries/AP_OSD/AP_OSD_Screen.cpp
@@ -1101,9 +1101,11 @@ void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
     float ky = sinf(roll);
     float kx = cosf(roll);
 
+    float ratio = backend->get_aspect_ratio_correction();
+
     if (fabsf(ky) < fabsf(kx)) {
         for (int dx = -4; dx <= 4; dx++) {
-            float fy =  dx * (ky/kx) + pitch * ah_pitch_rad_to_char + 0.5f;
+            float fy = (ratio * dx) * (ky/kx) + pitch * ah_pitch_rad_to_char + 0.5f;
             int dy = floorf(fy);
             char c = (fy - dy) * SYM_AH_H_COUNT;
             //chars in font in reversed order
@@ -1114,7 +1116,7 @@ void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
         }
     } else {
         for (int dy=-4; dy<=4; dy++) {
-            float fx = (dy - pitch * ah_pitch_rad_to_char) * (kx/ky) + 0.5f;
+            float fx = ((dy / ratio) - pitch * ah_pitch_rad_to_char) * (kx/ky) + 0.5f;
             int dx = floorf(fx);
             char c = (fx - dx) * SYM_AH_V_COUNT;
             c = SYM_AH_V_START + c;