src/main/io/osd_common.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 "platform.h"
  28
  29 #include "common/utils.h"
  30
  31 #include "config/parameter_group.h"
  32 #include "config/parameter_group_ids.h"
  33
  34 #include "drivers/display.h"
  35 #include "drivers/display_canvas.h"
  36 #include "drivers/osd.h"
  37
  38 #include "fc/settings.h"
  39
  40 #include "io/osd_canvas.h"
  41 #include "io/osd_common.h"
  42 #include "io/osd_grid.h"
  43
  44 #include "navigation/navigation.h"
  45 #include "sensors/pitotmeter.h"
  46
  47 #if defined(USE_OSD) || defined(USE_DJI_HD_OSD)
  48
  49 PG_REGISTER_WITH_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig, PG_OSD_COMMON_CONFIG, 0);
  50
  51 PG_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig,
  52     .speedSource = SETTING_OSD_SPEED_SOURCE_DEFAULT
  53 );
  54
  55 int16_t osdGetSpeedFromSelectedSource(void) {
  56     int speed = 0;
  57     switch (osdCommonConfig()->speedSource) {
  58         case OSD_SPEED_SOURCE_GROUND:
  59             speed = gpsSol.groundSpeed;
  60             break;
  61         case OSD_SPEED_SOURCE_3D:
  62             speed = osdGet3DSpeed();
  63             break;
  64         case OSD_SPEED_SOURCE_AIR:
  65             #ifdef USE_PITOT
  66             speed = (int16_t)getAirspeedEstimate();
  67             #endif
  68             break;
  69     }
  70     return speed;
  71 }
  72
  73 #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD)
  74
  75 #if defined(USE_OSD)
  76
  77 #define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH
  78 #define CANVAS_DEFAULT_GRID_ELEMENT_HEIGHT OSD_CHAR_HEIGHT
  79
  80 void osdDrawPointGetGrid(uint8_t *gx, uint8_t *gy, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p)
  81 {
  82     UNUSED(display);
  83
  84     switch (p->type) {
  85         case OSD_DRAW_POINT_TYPE_GRID:
  86             *gx = p->grid.gx;
  87             *gy = p->grid.gy;
  88             break;
  89         case OSD_DRAW_POINT_TYPE_PIXEL:
  90             *gx = p->pixel.px / (canvas ? canvas->gridElementWidth : CANVAS_DEFAULT_GRID_ELEMENT_WIDTH);
  91             *gy = p->pixel.py / (canvas ? canvas->gridElementHeight : OSD_CHAR_HEIGHT);
  92             break;
  93     }
  94 }
  95
  96 void osdDrawPointGetPixels(int *px, int *py, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p)
  97 {
  98     UNUSED(display);
  99
 100     switch (p->type) {
 101         case OSD_DRAW_POINT_TYPE_GRID:
 102             *px = p->grid.gx * (canvas ? canvas->gridElementWidth : CANVAS_DEFAULT_GRID_ELEMENT_WIDTH);
 103             *py = p->grid.gy * (canvas ? canvas->gridElementHeight : CANVAS_DEFAULT_GRID_ELEMENT_HEIGHT);
 104             break;
 105         case OSD_DRAW_POINT_TYPE_PIXEL:
 106             *px = p->pixel.px;
 107             *py = p->pixel.py;
 108             break;
 109     }
 110 }
 111
 112 void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel)
 113 {
 114     uint8_t gx;
 115     uint8_t gy;
 116
 117 #if defined(USE_CANVAS)
 118     if (canvas) {
 119         osdCanvasDrawVario(display, canvas, p, zvel);
 120     } else {
 121 #endif
 122         osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
 123         osdGridDrawVario(display, gx, gy, zvel);
 124 #if defined(USE_CANVAS)
 125     }
 126 #endif
 127 }
 128
 129 void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees)
 130 {
 131     uint8_t gx;
 132     uint8_t gy;
 133
 134 #if defined(USE_CANVAS)
 135     if (canvas) {
 136         osdCanvasDrawDirArrow(display, canvas, p, degrees);
 137     } else {
 138 #endif
 139         osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
 140         osdGridDrawDirArrow(display, gx, gy, degrees);
 141 #if defined(USE_CANVAS)
 142     }
 143 #endif
 144 }
 145
 146 void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float rollAngle, float pitchAngle)
 147 {
 148     uint8_t gx;
 149     uint8_t gy;
 150
 151 #if defined(USE_CANVAS)
 152     if (canvas) {
 153         osdCanvasDrawArtificialHorizon(display, canvas, p, pitchAngle, rollAngle);
 154     } else {
 155 #endif
 156         // Correct pitch when inverted
 157         if (rollAngle < -1.570796f || rollAngle > 1.570796f) {
 158             pitchAngle = -pitchAngle;
 159         }
 160
 161         osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
 162         osdGridDrawArtificialHorizon(display, gx, gy, pitchAngle, rollAngle);
 163 #if defined(USE_CANVAS)
 164     }
 165 #endif
 166 }
 167
 168 void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading)
 169 {
 170     uint8_t gx;
 171     uint8_t gy;
 172 #if defined(USE_CANVAS)
 173     if (canvas) {
 174         osdCanvasDrawHeadingGraph(display, canvas, p, heading);
 175     } else {
 176 #endif
 177         osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
 178         osdGridDrawHeadingGraph(display, gx, gy, heading);
 179 #if defined(USE_CANVAS)
 180     }
 181 #endif
 182 }
 183
 184 void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
 185 {
 186 #if defined(USE_CANVAS)
 187     if (osdCanvasDrawSidebars(display, canvas))  {
 188         return;
 189     }
 190 #else
 191     UNUSED(canvas);
 192 #endif
 193     osdGridDrawSidebars(display);
 194 }
 195
 196 #endif
 197
 198 #ifdef USE_GPS
 199 int16_t osdGet3DSpeed(void)
 200 {
 201     int16_t vert_speed = getEstimatedActualVelocity(Z);
 202     int16_t hor_speed = gpsSol.groundSpeed;
 203     return (int16_t)calc_length_pythagorean_2D(hor_speed, vert_speed);
 204 }
 205 #endif