libraries/AP_OSD/AP_OSD_Backend.cpp

   1 /*
   2  * This file is free software: you can redistribute it and/or modify it
   3  * under the terms of the GNU General Public License as published by the
   4  * Free Software Foundation, either version 3 of the License, or
   5  * (at your option) any later version.
   6  *
   7  * This file is distributed in the hope that it will be useful, but
   8  * WITHOUT ANY WARRANTY; without even the implied warranty of
   9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  10  * See the GNU General Public License for more details.
  11  *
  12  * You should have received a copy of the GNU General Public License along
  13  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  14  *
  15  */
  16
  17 #include <AP_OSD/AP_OSD_Backend.h>
  18 #include <AP_HAL/Util.h>
  19 #include <GCS_MAVLink/GCS.h>
  20 #include <ctype.h>
  21
  22 extern const AP_HAL::HAL& hal;
  23 constexpr uint8_t AP_OSD_Backend::symbols[AP_OSD_NUM_SYMBOLS];
  24
  25 #define SYM_DIG_OFS_1 0x90
  26 #define SYM_DIG_OFS_2 0xA0
  27
  28
  29 uint8_t AP_OSD_Backend::convert_to_decimal_packed_characters(char* buff, uint8_t size) {
  30     if (size == 0) {
  31         return 0;
  32     }
  33 #if OSD_ENABLED
  34     // use packed decimal characters based on fiam idea implemented in inav osd
  35     // search the decimal separator with a bound and always terminate the string
  36     char* p = (char*)memchr(&buff[1],'.',size-1);
  37     if (p && isdigit(p[1]) && isdigit(p[-1])) {
  38         // remove the decimal separator and replace the digit before and after
  39         p[-1] += SYM_DIG_OFS_1;
  40         p[1] += SYM_DIG_OFS_2;
  41         // shift anything after p[1] 1 character to the left
  42         const char* move_start = p+1;
  43         const uint8_t move_size = size-(move_start-buff);
  44         memmove(p, move_start, move_size);
  45         p[move_size] = 0x00; // terminate
  46         return size-1;
  47     }
  48 #else
  49     // we guarantee string is terminated
  50     buff[size-1] = 0x00;
  51 #endif
  52     return size;
  53 }
  54
  55 uint8_t AP_OSD_Backend::format_string_for_osd(char* buff, uint8_t size, bool decimal_packed, const char *fmt, va_list ap)
  56 {
  57     if (size == 0) {
  58         return 0;
  59     }
  60 #if OSD_ENABLED
  61     // note: vsnprintf() always terminates the string
  62     int res = hal.util->vsnprintf(buff, size, fmt, ap);
  63     res = MIN(res, size);
  64     if (res > 0 && decimal_packed) {
  65         // note: convert_to_decimal_packed_characters() always terminates the string
  66         res = convert_to_decimal_packed_characters(buff, res);
  67     }
  68     return res;
  69 #else
  70     // we guarantee string is terminated
  71     buff[0] = 0x00;
  72     // and notify the caller we actually failed
  73     return 0;
  74 #endif
  75 }
  76
  77 void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...)
  78 {
  79 #if OSD_ENABLED
  80     if (blink && (blink_phase < 2)) {
  81         return;
  82     }
  83     va_list ap;
  84     va_start(ap, fmt);
  85     char buff[32+1]; // +1 for null-termination
  86     // note: format_string_for_osd() always terminates the string
  87     IGNORE_RETURN(format_string_for_osd(buff, sizeof(buff), check_option(AP_OSD::OPTION_DECIMAL_PACK), fmt, ap));
  88     va_end(ap);
  89     // buff is null terminated, this call should be safe without further checks
  90     write(x, y, buff);
  91 #endif
  92 }
  93
  94 /*
  95   load a font from sdcard or ROMFS
  96  */
  97 FileData *AP_OSD_Backend::load_font_data(uint8_t font_num)
  98 {
  99     FileData *fd;
 100
 101     // first try from microSD
 102     char fontname[] = "font0.bin";
 103     fontname[4] = font_num + '0';
 104
 105     fd = AP::FS().load_file(fontname);
 106     if (fd == nullptr) {
 107         char fontname_romfs[] = "@ROMFS/font0.bin";
 108         fontname_romfs[7+4] = font_num + '0';
 109         fd = AP::FS().load_file(fontname_romfs);
 110     }
 111     if (fd == nullptr) {
 112         GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "OSD: Failed to load font %u", font_num);
 113         if (font_num != 0) {
 114             // fallback to font0.bin. This allows us to reduce the
 115             // number of fonts we include in flash without breaking
 116             // user setups
 117             return load_font_data(0);
 118         }
 119     }
 120     return fd;
 121 }
 122
 123 void AP_OSD_Backend::init_symbol_set(uint8_t *lookup_table, const uint8_t size)
 124 {
 125     memcpy(lookup_table, symbols, size);
 126 }