Line data    Source code

       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       18409 : uint8_t AP_OSD_Backend::convert_to_decimal_packed_characters(char* buff, uint8_t size) {
      30       18409 :     if (size == 0) {
      31           0 :         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       18409 :     char* p = (char*)memchr(&buff[1],'.',size-1);
      37       18409 :     if (p && isdigit(p[1]) && isdigit(p[-1])) {
      38             :         // remove the decimal separator and replace the digit before and after
      39        3552 :         p[-1] += SYM_DIG_OFS_1;
      40        3552 :         p[1] += SYM_DIG_OFS_2;
      41             :         // shift anything after p[1] 1 character to the left
      42        3552 :         const char* move_start = p+1;
      43        3552 :         const uint8_t move_size = size-(move_start-buff);
      44        3552 :         memmove(p, move_start, move_size);
      45        3552 :         p[move_size] = 0x00; // terminate
      46        3552 :         return size-1;
      47             :     }
      48             : #else
      49             :     // we guarantee string is terminated
      50             :     buff[size-1] = 0x00;
      51             : #endif
      52       14857 :     return size;
      53             : }
      54             : 
      55       18409 : 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       18409 :     if (size == 0) {
      58           0 :         return 0;
      59             :     }
      60             : #if OSD_ENABLED
      61             :     // note: vsnprintf() always terminates the string
      62       18409 :     int res = hal.util->vsnprintf(buff, size, fmt, ap);
      63       18409 :     res = MIN(res, size);
      64       18409 :     if (res > 0 && decimal_packed) {
      65             :         // note: convert_to_decimal_packed_characters() always terminates the string
      66       18409 :         res = convert_to_decimal_packed_characters(buff, res);
      67             :     }
      68       18409 :     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       18922 : void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...)
      78             : {
      79             : #if OSD_ENABLED
      80       18922 :     if (blink && (blink_phase < 2)) {
      81         513 :         return;
      82             :     }
      83             :     va_list ap;
      84       18409 :     va_start(ap, fmt);
      85             :     char buff[32+1]; // +1 for null-termination
      86             :     // note: format_string_for_osd() always terminates the string
      87       18409 :     IGNORE_RETURN(format_string_for_osd(buff, sizeof(buff), check_option(AP_OSD::OPTION_DECIMAL_PACK), fmt, ap));
      88       18409 :     va_end(ap);
      89             :     // buff is null terminated, this call should be safe without further checks
      90       18409 :     write(x, y, buff);
      91             : #endif
      92             : }
      93             : 
      94             : /*
      95             :   load a font from sdcard or ROMFS
      96             :  */
      97           0 : FileData *AP_OSD_Backend::load_font_data(uint8_t font_num)
      98             : {
      99             :     FileData *fd;
     100             : 
     101             :     // first try from microSD
     102           0 :     char fontname[] = "font0.bin";
     103           0 :     fontname[4] = font_num + '0';
     104             : 
     105           0 :     fd = AP::FS().load_file(fontname);
     106           0 :     if (fd == nullptr) {
     107           0 :         char fontname_romfs[] = "@ROMFS/font0.bin";
     108           0 :         fontname_romfs[7+4] = font_num + '0';
     109           0 :         fd = AP::FS().load_file(fontname_romfs);
     110             :     }
     111           0 :     if (fd == nullptr) {
     112           0 :         GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "OSD: Failed to load font %u", font_num);
     113           0 :         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           0 :             return load_font_data(0);
     118             :         }
     119             :     }
     120           0 :     return fd;
     121             : }
     122             : 
     123           0 : void AP_OSD_Backend::init_symbol_set(uint8_t *lookup_table, const uint8_t size)
     124             : {
     125           0 :     memcpy(lookup_table, symbols, size);
     126           0 : }