src/main/io/osd_hud.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19
  20 #include "platform.h"
  21
  22 #include "common/constants.h"
  23 #include "common/printf.h"
  24
  25 #include "flight/imu.h"
  26
  27 #include "io/osd.h"
  28 #include "io/osd_hud.h"
  29
  30 #include "drivers/display.h"
  31 #include "drivers/display_canvas.h"
  32 #include "drivers/osd.h"
  33 #include "drivers/osd_symbols.h"
  34 #include "drivers/time.h"
  35
  36 #include "navigation/navigation.h"
  37
  38 #ifdef USE_OSD
  39
  40 #define HUD_DRAWN_MAXCHARS 54 // 8 POI (1 home, 4 radar, 3 WP) x 7 chars max for each, minus 2 for H
  41
  42 static int8_t hud_drawn[HUD_DRAWN_MAXCHARS][2];
  43 static int8_t hud_drawn_pt;
  44
  45 /*
  46  * Overwrite all previously written positions on the OSD with a blank
  47  */
  48 void osdHudClear(void)
  49 {
  50     for (int i = 0; i < HUD_DRAWN_MAXCHARS; i++) {
  51         if (hud_drawn[i][0] > -1) {
  52             displayWriteChar(osdGetDisplayPort(), hud_drawn[i][0], hud_drawn[i][1], SYM_BLANK);
  53             hud_drawn[i][0] = -1;
  54         }
  55     }
  56     hud_drawn_pt = 0;
  57 }
  58
  59 /*
  60  * Write a single char on the OSD, and record the position for the next loop
  61  */
  62 static int osdHudWrite(uint8_t px, uint8_t py, uint16_t symb, bool crush)
  63 {
  64     if (!crush) {
  65         uint16_t c;
  66         if (displayReadCharWithAttr(osdGetDisplayPort(), px, py, &c, NULL) && c != SYM_BLANK) {
  67             return false;
  68         }
  69     }
  70
  71     displayWriteChar(osdGetDisplayPort(), px, py, symb);
  72     hud_drawn[hud_drawn_pt][0] = px;
  73     hud_drawn[hud_drawn_pt][1] = py;
  74     hud_drawn_pt++;
  75     if (hud_drawn_pt >= HUD_DRAWN_MAXCHARS) hud_drawn_pt = 0;
  76     return true;
  77 }
  78
  79 /*
  80  * Constrain an angle between -180 and +180°
  81  */
  82 static int16_t hudWrap180(int16_t angle)
  83 {
  84     while (angle < -179) angle += 360;
  85     while (angle > 180) angle -= 360;
  86     return angle;
  87 }
  88
  89 /*
  90  * Constrain an angle between 0 and +360°
  91  */
  92 static int16_t hudWrap360(int16_t angle)
  93 {
  94     while (angle < 0) angle += 360;
  95     while (angle > 360) angle -= 360;
  96     return angle;
  97 }
  98
  99 /*
 100  * Radar, get the nearest POI
 101  */
 102 int8_t radarGetNearestPOI(void)
 103 {
 104     int8_t poi = -1;
 105     uint16_t min = 10000; // 10kms
 106
 107     for (int i = 0; i < RADAR_MAX_POIS; i++) {
 108         if ((radar_pois[i].distance > 0) && (radar_pois[i].distance < min) && (radar_pois[i].state != 2)) {
 109             min = radar_pois[i].distance;
 110             poi = i;
 111         }
 112     }
 113     return poi;
 114 }
 115
 116 /*
 117  * Display a POI as a 3D-marker on the hud
 118  * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°),
 119  * Type = 0 : Home point
 120  * Type = 1 : Radar POI, P1: Relative heading, P2: Signal, P3 Cardinal direction
 121  * Type = 2 : Waypoint, P1: WP number, P2: 1=WP+1, 2=WP+2, 3=WP+3
 122  */
 123 void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2)
 124 {
 125     int poi_x = -1;
 126     int poi_y = -1;
 127     uint8_t center_x;
 128     uint8_t center_y;
 129     bool poi_is_oos = 0;
 130     char buff[4];
 131     int altc = 0;
 132
 133     uint8_t minX = osdConfig()->hud_margin_h + 2;
 134     uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 3;
 135     uint8_t minY = osdConfig()->hud_margin_v;
 136     uint8_t maxY = osdGetDisplayPort()->rows - osdConfig()->hud_margin_v - 2;
 137
 138     osdCrosshairPosition(&center_x, &center_y);
 139
 140     if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
 141         poiDirection = poiDirection + osdGetPanServoOffset();
 142     }
 143
 144     int16_t error_x = hudWrap180(poiDirection - DECIDEGREES_TO_DEGREES(osdGetHeading()));
 145
 146     if ((error_x > -(osdConfig()->camera_fov_h / 2)) && (error_x < osdConfig()->camera_fov_h / 2)) { // POI might be in sight, extra geometry needed
 147         float scaled_x = sin_approx(DEGREES_TO_RADIANS(error_x)) / sin_approx(DEGREES_TO_RADIANS(osdConfig()->camera_fov_h / 2));
 148         poi_x = center_x + 15 * scaled_x;
 149
 150         if (poi_x < minX || poi_x > maxX ) { // In camera view, but out of the hud area
 151             poi_is_oos = 1;
 152         } else { // POI is on sight, compute the vertical
 153             float poi_angle = atan2_approx(-poiAltitude, poiDistance);
 154             poi_angle = RADIANS_TO_DEGREES(poi_angle);
 155             int16_t plane_angle = attitude.values.pitch / 10;
 156             int camera_angle = osdConfig()->camera_uptilt;
 157             int16_t error_y = poi_angle - plane_angle + camera_angle;
 158             float scaled_y = sin_approx(DEGREES_TO_RADIANS(error_y)) / sin_approx(DEGREES_TO_RADIANS(osdConfig()->camera_fov_v / 2));
 159             poi_y = constrain(center_y + (osdGetDisplayPort()->rows / 2) * scaled_y, minY, maxY - 1);
 160         }
 161     } else {
 162         poi_is_oos = 1; // POI is out of camera view for sure
 163     }
 164
 165     // Out-of-sight arrows and stacking
 166     // Always show with ESP32 Radar
 167
 168     if (poi_is_oos || poiType == 1) {
 169         uint16_t d;
 170         uint16_t c;
 171
 172         if (poi_is_oos) {
 173             poi_x = (error_x > 0 ) ? maxX : minX;
 174             poi_y = center_y - 1;
 175         }
 176
 177         if (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK) {
 178             poi_y = center_y - 3;
 179             while (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK && poi_y < maxY - 3) { // Stacks the out-of-sight POI from top to bottom
 180                 poi_y += 2;
 181             }
 182         }
 183
 184         if (poiType == 1) { // POI from the ESP radar
 185             d = constrain(((error_x + 180) / 30), 0, 12);
 186             if (d == 12) {
 187                 d = 0; // Directly behind
 188             }
 189
 190             d = SYM_HUD_CARDINAL + d;
 191             osdHudWrite(poi_x + 2, poi_y, d, 1);
 192         } else {
 193             if (error_x > 0 ) {
 194                 d = SYM_HUD_ARROWS_R3 - constrain((180 - error_x) / 45, 0, 2);
 195                 osdHudWrite(poi_x + 2, poi_y, d, 1);
 196             }
 197             else {
 198                 d = SYM_HUD_ARROWS_L3 - constrain((180 + error_x) / 45, 0, 2);
 199                 osdHudWrite(poi_x - 2, poi_y, d, 1);
 200             }
 201         }
 202     }
 203
 204     // Markers
 205
 206     osdHudWrite(poi_x, poi_y, poiSymbol, 1);
 207
 208     if (poiType == 1) { // POI from the ESP radar
 209         error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading()));
 210         osdHudWrite(poi_x - 1, poi_y, SYM_DECORATION + ((error_x + 22) / 45) % 8, 1);
 211         osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1);
 212     }
 213     else if (poiType == 2) { // Waypoint,
 214         osdHudWrite(poi_x - 1, poi_y, SYM_HUD_ARROWS_U1 + poiP2, 1);
 215         osdHudWrite(poi_x + 1, poi_y, poiP1, 1);
 216     }
 217
 218     // Distance
 219
 220     if (poiType > 0 &&
 221         ((millis() / 1000) % (osdConfig()->hud_radar_alt_difference_display_time + osdConfig()->hud_radar_distance_display_time) < (osdConfig()->hud_radar_alt_difference_display_time % (osdConfig()->hud_radar_alt_difference_display_time + osdConfig()->hud_radar_distance_display_time)))
 222        ) { // For Radar and WPs, display the difference in altitude, then distance. Time is pilot defined
 223         altc = poiAltitude;
 224
 225         switch ((osd_unit_e)osdConfig()->units) {
 226             case OSD_UNIT_UK:
 227                 FALLTHROUGH;
 228             case OSD_UNIT_GA:
 229                 FALLTHROUGH;
 230             case OSD_UNIT_IMPERIAL:
 231                 // Convert to feet
 232                 altc = CENTIMETERS_TO_FEET(poiAltitude * 100);
 233                 break;
 234             default:
 235                 FALLTHROUGH;
 236             case OSD_UNIT_METRIC_MPH:
 237                 FALLTHROUGH;
 238             case OSD_UNIT_METRIC:
 239                 // Already in metres
 240                 break;
 241         }
 242
 243         if (poiType == 1) {
 244             altc = ABS(constrain(altc, -999, 999));
 245             tfp_sprintf(buff+1, "%3d", altc);
 246         } else {
 247             altc = constrain(altc, -99, 99);
 248             tfp_sprintf(buff, "%3d", altc);
 249         }
 250
 251         buff[0] = (poiAltitude >= 0) ? SYM_AH_DECORATION_UP : SYM_AH_DECORATION_DOWN;
 252     } else { // Display the distance by default
 253         switch ((osd_unit_e)osdConfig()->units) {
 254             case OSD_UNIT_UK:
 255                 FALLTHROUGH;
 256             case OSD_UNIT_IMPERIAL:
 257                 {
 258                     if (poiType == 1) {
 259                         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4, false);
 260                     } else {
 261                         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3, false);
 262                     }
 263                 }
 264                 break;
 265             case OSD_UNIT_GA:
 266                 {
 267                     if (poiType == 1) {
 268                         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4, false);
 269                     } else {
 270                         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3, false);
 271                     }
 272                 }
 273                 break;
 274             default:
 275                 FALLTHROUGH;
 276             case OSD_UNIT_METRIC_MPH:
 277                 FALLTHROUGH;
 278             case OSD_UNIT_METRIC:
 279                 {
 280                     if (poiType == 1) {
 281                         osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4, false);
 282                     } else {
 283                         osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3, false);
 284                     }
 285                 }
 286                 break;
 287         }
 288     }
 289
 290     osdHudWrite(poi_x - 1, poi_y + 1, buff[0], 1);
 291     osdHudWrite(poi_x , poi_y + 1, buff[1], 1);
 292     osdHudWrite(poi_x + 1, poi_y + 1, buff[2], 1);
 293     if (poiType == 1) {
 294         osdHudWrite(poi_x + 2, poi_y + 1, buff[3], 1);
 295     }
 296 }
 297
 298 /*
 299  * Draw the crosshair
 300  */
 301 void osdHudDrawCrosshair(displayCanvas_t *canvas, uint8_t px, uint8_t py)
 302 {
 303     static const uint16_t crh_style_all[] = {
 304         SYM_AH_CH_LEFT, SYM_AH_CH_CENTER, SYM_AH_CH_RIGHT,
 305         SYM_AH_CH_AIRCRAFT1, SYM_AH_CH_AIRCRAFT2, SYM_AH_CH_AIRCRAFT3,
 306         SYM_AH_CH_TYPE3, SYM_AH_CH_TYPE3 + 1, SYM_AH_CH_TYPE3 + 2,
 307         SYM_AH_CH_TYPE4, SYM_AH_CH_TYPE4 + 1, SYM_AH_CH_TYPE4 + 2,
 308         SYM_AH_CH_TYPE5, SYM_AH_CH_TYPE5 + 1, SYM_AH_CH_TYPE5 + 2,
 309         SYM_AH_CH_TYPE6, SYM_AH_CH_TYPE6 + 1, SYM_AH_CH_TYPE6 + 2,
 310         SYM_AH_CH_TYPE7, SYM_AH_CH_TYPE7 + 1, SYM_AH_CH_TYPE7 + 2,
 311         SYM_AH_CH_TYPE8, SYM_AH_CH_TYPE8 + 1, SYM_AH_CH_TYPE8 + 2,
 312     };
 313
 314     // Center on the screen
 315     if (canvas) {
 316         displayCanvasContextPush(canvas);
 317         displayCanvasCtmTranslate(canvas, -canvas->gridElementWidth / 2, -canvas->gridElementHeight / 2);
 318     }
 319
 320     uint8_t crh_crosshair = (osd_crosshairs_style_e)osdConfig()->crosshairs_style;
 321
 322     displayWriteChar(osdGetDisplayPort(), px - 1, py,crh_style_all[crh_crosshair * 3]);
 323     displayWriteChar(osdGetDisplayPort(), px, py, crh_style_all[crh_crosshair * 3 + 1]);
 324     displayWriteChar(osdGetDisplayPort(), px + 1, py, crh_style_all[crh_crosshair * 3 + 2]);
 325
 326     if ((crh_style_all[crh_crosshair * 3]) == SYM_AH_CH_AIRCRAFT1) {
 327         displayWriteChar(osdGetDisplayPort(), px - 2, py, SYM_AH_CH_AIRCRAFT0);
 328         displayWriteChar(osdGetDisplayPort(), px + 2, py, SYM_AH_CH_AIRCRAFT4);
 329     }
 330
 331     if (canvas) {
 332         displayCanvasContextPop(canvas);
 333     }
 334 }
 335
 336
 337 /*
 338  * Draw the homing arrows around the crosshair
 339  */
 340 void osdHudDrawHoming(uint8_t px, uint8_t py)
 341 {
 342     int crh_l = SYM_BLANK;
 343     int crh_r = SYM_BLANK;
 344     int crh_u = SYM_BLANK;
 345     int crh_d = SYM_BLANK;
 346
 347     int16_t crh_diff_head = hudWrap180(GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()));
 348
 349     if (crh_diff_head <= -162 || crh_diff_head >= 162) {
 350         crh_l = SYM_HUD_ARROWS_L3;
 351         crh_r = SYM_HUD_ARROWS_R3;
 352     } else if (crh_diff_head > -162 && crh_diff_head <= -126) {
 353         crh_l = SYM_HUD_ARROWS_L3;
 354         crh_r = SYM_HUD_ARROWS_R2;
 355     } else if (crh_diff_head > -126 && crh_diff_head <= -90) {
 356         crh_l = SYM_HUD_ARROWS_L3;
 357         crh_r = SYM_HUD_ARROWS_R1;
 358     } else if (crh_diff_head > -90 && crh_diff_head <= -OSD_HOMING_LIM_H3) {
 359         crh_l = SYM_HUD_ARROWS_L3;
 360     } else if (crh_diff_head > -OSD_HOMING_LIM_H3 && crh_diff_head <= -OSD_HOMING_LIM_H2) {
 361         crh_l = SYM_HUD_ARROWS_L2;
 362     } else if (crh_diff_head > -OSD_HOMING_LIM_H2 && crh_diff_head <= -OSD_HOMING_LIM_H1) {
 363         crh_l = SYM_HUD_ARROWS_L1;
 364     } else if (crh_diff_head >= OSD_HOMING_LIM_H1 && crh_diff_head < OSD_HOMING_LIM_H2) {
 365         crh_r = SYM_HUD_ARROWS_R1;
 366     } else if (crh_diff_head >= OSD_HOMING_LIM_H2 && crh_diff_head < OSD_HOMING_LIM_H3) {
 367         crh_r = SYM_HUD_ARROWS_R2;
 368     } else if (crh_diff_head >= OSD_HOMING_LIM_H3 && crh_diff_head < 90) {
 369         crh_r = SYM_HUD_ARROWS_R3;
 370     } else if (crh_diff_head >= 90 && crh_diff_head < 126) {
 371         crh_l = SYM_HUD_ARROWS_L1;
 372         crh_r = SYM_HUD_ARROWS_R3;
 373     } else if (crh_diff_head >= 126 && crh_diff_head < 162) {
 374         crh_l = SYM_HUD_ARROWS_L2;
 375         crh_r = SYM_HUD_ARROWS_R3;
 376     }
 377
 378     if (ABS(crh_diff_head) < 90) {
 379
 380         int32_t crh_altitude = osdGetAltitude() / 100;
 381         int32_t crh_distance = GPS_distanceToHome;
 382
 383         float crh_home_angle = atan2_approx(crh_altitude, crh_distance);
 384         crh_home_angle = RADIANS_TO_DEGREES(crh_home_angle);
 385         int crh_plane_angle = attitude.values.pitch / 10;
 386         int crh_camera_angle = osdConfig()->camera_uptilt;
 387         int crh_diff_vert = crh_home_angle - crh_plane_angle + crh_camera_angle;
 388
 389         if (crh_diff_vert > -OSD_HOMING_LIM_V2 && crh_diff_vert <= -OSD_HOMING_LIM_V1 ) {
 390             crh_u = SYM_HUD_ARROWS_U1;
 391         } else if (crh_diff_vert > -OSD_HOMING_LIM_V3 && crh_diff_vert <= -OSD_HOMING_LIM_V2) {
 392             crh_u = SYM_HUD_ARROWS_U2;
 393         } else if (crh_diff_vert <= -OSD_HOMING_LIM_V3) {
 394             crh_u = SYM_HUD_ARROWS_U3;
 395         } else if (crh_diff_vert >= OSD_HOMING_LIM_V1  && crh_diff_vert < OSD_HOMING_LIM_V2) {
 396             crh_d = SYM_HUD_ARROWS_D1;
 397         } else if (crh_diff_vert >= OSD_HOMING_LIM_V2 && crh_diff_vert < OSD_HOMING_LIM_V3) {
 398             crh_d = SYM_HUD_ARROWS_D2;
 399         } else if (crh_diff_vert >= OSD_HOMING_LIM_V3) {
 400             crh_d = SYM_HUD_ARROWS_D3;
 401         }
 402     }
 403
 404     displayWriteChar(osdGetDisplayPort(), px - 2, py, crh_l);
 405     displayWriteChar(osdGetDisplayPort(), px + 2, py, crh_r);
 406     displayWriteChar(osdGetDisplayPort(), px, py - 1, crh_u);
 407     displayWriteChar(osdGetDisplayPort(), px, py + 1, crh_d);
 408 }
 409
 410 #endif // USE_OSD