src/main/io/osd_grid.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  *
  24  * @author Alberto Garcia Hierro <alberto@garciahierro.com>
  25  */
  26
  27 #include <math.h>
  28
  29 #include "platform.h"
  30
  31 #if defined(USE_OSD)
  32
  33 #include "common/maths.h"
  34 #include "common/utils.h"
  35
  36 #include "drivers/display.h"
  37 #include "drivers/osd.h"
  38 #include "drivers/osd_symbols.h"
  39 #include "drivers/time.h"
  40
  41 #include "io/osd.h"
  42 #include "io/osd_common.h"
  43
  44 #include "navigation/navigation.h"
  45
  46 typedef enum {
  47     OSD_SIDEBAR_ARROW_NONE,
  48     OSD_SIDEBAR_ARROW_UP,
  49     OSD_SIDEBAR_ARROW_DOWN,
  50 } osd_sidebar_arrow_e;
  51
  52 typedef struct osd_sidebar_s {
  53     int32_t offset;
  54     timeMs_t updated;
  55     osd_sidebar_arrow_e arrow;
  56     uint8_t idle;
  57 } osd_sidebar_t;
  58
  59 void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel)
  60 {
  61     int v = zvel / OSD_VARIO_CM_S_PER_ARROW;
  62     uint16_t vchars[] = {SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK};
  63
  64     if (v >= 6)
  65         vchars[0] = SYM_VARIO_UP_2A;
  66     else if (v == 5)
  67         vchars[0] = SYM_VARIO_UP_1A;
  68     if (v >=4)
  69         vchars[1] = SYM_VARIO_UP_2A;
  70     else if (v == 3)
  71         vchars[1] = SYM_VARIO_UP_1A;
  72     if (v >=2)
  73         vchars[2] = SYM_VARIO_UP_2A;
  74     else if (v == 1)
  75         vchars[2] = SYM_VARIO_UP_1A;
  76     if (v <= -2)
  77         vchars[2] = SYM_VARIO_DOWN_2A;
  78     else if (v == -1)
  79         vchars[2] = SYM_VARIO_DOWN_1A;
  80     if (v <= -4)
  81         vchars[3] = SYM_VARIO_DOWN_2A;
  82     else if (v == -3)
  83         vchars[3] = SYM_VARIO_DOWN_1A;
  84     if (v <= -6)
  85         vchars[4] = SYM_VARIO_DOWN_2A;
  86     else if (v == -5)
  87         vchars[4] = SYM_VARIO_DOWN_1A;
  88
  89     displayWriteChar(display, gx, gy, vchars[0]);
  90     displayWriteChar(display, gx, gy + 1, vchars[1]);
  91     displayWriteChar(display, gx, gy + 2, vchars[2]);
  92     displayWriteChar(display, gx, gy + 3, vchars[3]);
  93     displayWriteChar(display, gx, gy + 4, vchars[4]);
  94 }
  95
  96 void osdGridDrawDirArrow(displayPort_t *display, unsigned gx, unsigned gy, float degrees)
  97 {
  98     // There are 16 orientations for the direction arrow.
  99     // so we use 22.5deg per image, where the 1st image is used
 100     // for [349, 11], the 2nd for [12, 33], etc...
 101     // Add 11 to the angle, so first character maps to [349, 11]
 102     int dir = osdGetHeadingAngle(degrees + 11);
 103     unsigned arrowOffset = dir * 2 / 45;
 104     displayWriteChar(display, gx, gy, SYM_ARROW_UP + arrowOffset);
 105 }
 106
 107 static float osdGetAspectRatioCorrection(void)
 108 {
 109     return osdDisplayIsPAL() ? 12.0f/15.0f : 12.0f/18.46f;
 110 }
 111
 112 void osdGridDrawArtificialHorizon(displayPort_t *display, unsigned gx, unsigned gy, float pitchAngle, float rollAngle)
 113 {
 114     UNUSED(gx);
 115     UNUSED(gy);
 116
 117     uint8_t elemPosX;
 118     uint8_t elemPosY;
 119
 120     osdCrosshairPosition(&elemPosX, &elemPosY);
 121
 122     // Store the positions we draw over to erase only these at the next iteration
 123     static int8_t previous_written[OSD_AHI_PREV_SIZE];
 124     static int8_t previous_orient = -1;
 125
 126     const float pitch_rad_to_char = (float)(OSD_AHI_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch);
 127
 128     const float ky = sin_approx(rollAngle);
 129     const float kx = cos_approx(rollAngle);
 130     const float ratio = osdGetAspectRatioCorrection();
 131
 132     if (previous_orient != -1) {
 133         for (int i = 0; i < OSD_AHI_PREV_SIZE; ++i) {
 134             if (previous_written[i] > -1) {
 135                 int8_t dx = (previous_orient ? previous_written[i] : i) - OSD_AHI_PREV_SIZE / 2;
 136                 int8_t dy = (previous_orient ? i : previous_written[i]) - OSD_AHI_PREV_SIZE / 2;
 137                 displayWriteChar(display, elemPosX + dx, elemPosY - dy, SYM_BLANK);
 138                 previous_written[i] = -1;
 139             }
 140         }
 141     }
 142
 143     int8_t ahiPitchAngleDatum;     // sets the pitch datum AHI is drawn relative to (degrees)
 144     int8_t ahiLineEndPitchOffset;  // AHI end of line offset in degrees when ahiPitchAngleDatum > 0
 145
 146     if (osdConfig()->ahi_pitch_interval) {
 147         ahiPitchAngleDatum = osdConfig()->ahi_pitch_interval * (int8_t)(RADIANS_TO_DEGREES(pitchAngle) / osdConfig()->ahi_pitch_interval);
 148         pitchAngle -= DEGREES_TO_RADIANS(ahiPitchAngleDatum);
 149     } else {
 150         ahiPitchAngleDatum = 0;
 151     }
 152
 153     if (fabsf(ky) < fabsf(kx)) {
 154
 155         previous_orient = 0;
 156
 157         /* ahi line ends drawn with 3 deg offset when ahiPitchAngleDatum > 0
 158          * Line end offset increased by 1 deg with every 20 deg pitch increase */
 159         const int8_t ahiLineEndOffsetFactor = ahiPitchAngleDatum / 20;
 160
 161         for (int8_t dx = -OSD_AHI_WIDTH / 2; dx <= OSD_AHI_WIDTH / 2; dx++) {
 162             ahiLineEndPitchOffset = ahiPitchAngleDatum && (dx == -OSD_AHI_WIDTH / 2 || dx == OSD_AHI_WIDTH / 2) ? -(ahiLineEndOffsetFactor + 3 * ABS(ahiPitchAngleDatum) / ahiPitchAngleDatum) : 0;
 163             float fy = (ratio * dx) * (ky / kx) + (pitchAngle + DEGREES_TO_RADIANS(ahiLineEndPitchOffset)) * pitch_rad_to_char + 0.49f;
 164             int8_t dy = floorf(fy);
 165             const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
 166             uint16_t c;
 167
 168             if ((dy >= -OSD_AHI_HEIGHT / 2) && (dy <= OSD_AHI_HEIGHT / 2) && displayReadCharWithAttr(display, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
 169                 c = SYM_AH_H_START + ((OSD_AHI_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * OSD_AHI_H_SYM_COUNT));
 170                 displayWriteChar(display, elemPosX + dx, elemPosY - dy, c);
 171                 previous_written[dx + OSD_AHI_PREV_SIZE / 2] = dy + OSD_AHI_PREV_SIZE / 2;
 172             }
 173         }
 174
 175     } else {
 176
 177         previous_orient = 1;
 178
 179         for (int8_t dy = -OSD_AHI_HEIGHT / 2; dy <= OSD_AHI_HEIGHT / 2; dy++) {
 180             const float fx = ((dy / ratio) - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f;
 181             const int8_t dx = floorf(fx);
 182             const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
 183             uint16_t c;
 184
 185             if ((dx >= -OSD_AHI_WIDTH / 2) && (dx <= OSD_AHI_WIDTH / 2) && displayReadCharWithAttr(display, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
 186                 c = SYM_AH_V_START + (fx - dx) * OSD_AHI_V_SYM_COUNT;
 187                 displayWriteChar(display, chX, chY, c);
 188                 previous_written[dy + OSD_AHI_PREV_SIZE / 2] = dx + OSD_AHI_PREV_SIZE / 2;
 189             }
 190         }
 191     }
 192 }
 193
 194 void osdGridDrawHeadingGraph(displayPort_t *display, unsigned gx, unsigned gy, int heading)
 195 {
 196     static const uint8_t graph[] = {
 197         SYM_HEADING_LINE,
 198         SYM_HEADING_E,
 199         SYM_HEADING_LINE,
 200         SYM_HEADING_DIVIDED_LINE,
 201         SYM_HEADING_LINE,
 202         SYM_HEADING_S,
 203         SYM_HEADING_LINE,
 204         SYM_HEADING_DIVIDED_LINE,
 205         SYM_HEADING_LINE,
 206         SYM_HEADING_W,
 207         SYM_HEADING_LINE,
 208         SYM_HEADING_DIVIDED_LINE,
 209         SYM_HEADING_LINE,
 210         SYM_HEADING_N,
 211         SYM_HEADING_LINE,
 212         SYM_HEADING_DIVIDED_LINE,
 213         SYM_HEADING_LINE,
 214         SYM_HEADING_E,
 215         SYM_HEADING_LINE,
 216         SYM_HEADING_DIVIDED_LINE,
 217         SYM_HEADING_LINE,
 218         SYM_HEADING_S,
 219         SYM_HEADING_LINE,
 220         SYM_HEADING_DIVIDED_LINE,
 221         SYM_HEADING_LINE,
 222         SYM_HEADING_W,
 223         SYM_HEADING_LINE,
 224     };
 225     char buf[OSD_HEADING_GRAPH_WIDTH + 1];
 226     int16_t h = DECIDEGREES_TO_DEGREES(heading);
 227     if (h >= 180) {
 228         h -= 360;
 229     }
 230     int hh = h * 4;
 231     hh = hh + 720 + 45;
 232     hh = hh / 90;
 233     memcpy_fn(buf, graph + hh + 1, sizeof(buf) - 1);
 234     buf[sizeof(buf) - 1] = '\0';
 235     displayWrite(display, gx, gy, buf);
 236 }
 237
 238 static uint16_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs)
 239 {
 240     // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX.
 241     // Zero scrolling should draw SYM_AH_DECORATION.
 242     uint16_t decoration = SYM_AH_DECORATION;
 243     int offset = 0;
 244     int steps;
 245     switch (scroll) {
 246         case OSD_SIDEBAR_SCROLL_NONE:
 247             sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
 248             sidebar->offset = 0;
 249             return decoration;
 250         case OSD_SIDEBAR_SCROLL_ALTITUDE:
 251             // Move 1 char for every 20cm
 252             offset = osdGetAltitude();
 253             steps = offset / 20;
 254             break;
 255         case OSD_SIDEBAR_SCROLL_SPEED:
 256 #if defined(USE_GPS)
 257             offset = gpsSol.groundSpeed;
 258 #endif
 259             // Move 1 char for every 20 cm/s
 260             steps = offset / 20;
 261             break;
 262         case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
 263 #if defined(USE_GPS)
 264             offset = GPS_distanceToHome;
 265 #endif
 266             // Move 1 char for every 5m
 267             steps = offset / 5;
 268             break;
 269     }
 270     if (offset) {
 271         decoration -= steps % SYM_AH_DECORATION_COUNT;
 272         if (decoration > SYM_AH_DECORATION_MAX) {
 273             decoration -= SYM_AH_DECORATION_COUNT;
 274         } else if (decoration < SYM_AH_DECORATION_MIN) {
 275             decoration += SYM_AH_DECORATION_COUNT;
 276         }
 277     }
 278     if (currentTimeMs - sidebar->updated > 100) {
 279         if (offset > sidebar->offset) {
 280             sidebar->arrow = OSD_SIDEBAR_ARROW_UP;
 281             sidebar->idle = 0;
 282         } else if (offset < sidebar->offset) {
 283             sidebar->arrow = OSD_SIDEBAR_ARROW_DOWN;
 284             sidebar->idle = 0;
 285         } else {
 286             if (sidebar->idle > 3) {
 287                 sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
 288             } else {
 289                 sidebar->idle++;
 290             }
 291         }
 292         sidebar->offset = offset;
 293         sidebar->updated = currentTimeMs;
 294     }
 295     return decoration;
 296 }
 297
 298 void osdGridDrawSidebars(displayPort_t *display)
 299 {
 300     uint8_t elemPosX;
 301     uint8_t elemPosY;
 302
 303     osdCrosshairPosition(&elemPosX, &elemPosY);
 304
 305     static osd_sidebar_t left;
 306     static osd_sidebar_t right;
 307
 308     timeMs_t currentTimeMs = millis();
 309     uint16_t leftDecoration = osdUpdateSidebar(osdConfig()->left_sidebar_scroll, &left, currentTimeMs);
 310     uint16_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
 311
 312     const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS;
 313     const int hudheight = osdConfig()->sidebar_height;
 314
 315     // Arrows
 316     if (osdConfig()->sidebar_scroll_arrows) {
 317         displayWriteChar(display, elemPosX - hudwidth, elemPosY - hudheight - 1,
 318             left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK);
 319
 320         displayWriteChar(display, elemPosX + hudwidth, elemPosY - hudheight - 1,
 321             right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK);
 322
 323         displayWriteChar(display, elemPosX - hudwidth, elemPosY + hudheight + 1,
 324             left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK);
 325
 326         displayWriteChar(display, elemPosX + hudwidth, elemPosY + hudheight + 1,
 327             right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK);
 328     }
 329
 330     // Draw AH sides
 331     int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0);
 332     int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1);
 333     if (osdConfig()->sidebar_height) {
 334         for (int y = -hudheight; y <= hudheight; y++) {
 335             displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
 336             displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
 337         }
 338     }
 339     // AH level indicators
 340     displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT);
 341     displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT);
 342 }
 343
 344 #endif