src/main/io/osd_utils.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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "io/osd_utils.h"
  19
  20 #include "common/maths.h"
  21 #include "common/typeconversion.h"
  22 #include "drivers/osd_symbols.h"
  23 #include "io/displayport_msp_dji_compat.h"
  24
  25 #if defined(USE_OSD) || defined(OSD_UNIT_TEST)
  26
  27 int digitCount(int32_t value)
  28 {
  29     int digits = 1;
  30     while(1) {
  31         value = value / 10;
  32         if (value == 0) {
  33             break;
  34         }
  35         digits++;
  36     }
  37     return digits;
  38 }
  39
  40
  41 bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros)
  42 {
  43     char *ptr = buff;
  44     char *dec;
  45     int decimals = maxDecimals;
  46     bool negative = false;
  47     bool scaled = false;
  48     bool explicitDecimal = isDJICompatibleVideoSystem(osdConfig());
  49
  50     buff[length] = '\0';
  51
  52     if (centivalue < 0) {
  53         negative = true;
  54         centivalue = -centivalue;
  55         length--;
  56     }
  57
  58     int32_t integerPart = centivalue / 100;
  59     // 3 decimal digits
  60     int32_t millis = (centivalue % 100) * 10;
  61
  62     int digits = digitCount(integerPart);
  63     int remaining = length - digits;
  64     if (explicitDecimal) {
  65         remaining--;
  66     }
  67
  68     if (remaining < 0 && scale > 0) {
  69         // Reduce by scale
  70         scaled = true;
  71         decimals = maxScaledDecimals;
  72         integerPart = integerPart / scale;
  73         // Multiply by 10 to get 3 decimal digits
  74         millis = ((centivalue % (100 * scale)) * 10) / scale;
  75         digits = digitCount(integerPart);
  76         remaining = length - digits;
  77         if (explicitDecimal) {
  78             remaining--;
  79         }
  80     }
  81
  82     // 3 decimals at most
  83     decimals = MIN(remaining, MIN(decimals, 3));
  84     remaining -= decimals;
  85
  86     // Done counting. Time to write the characters.
  87     // Write spaces at the start
  88     while (remaining > 0) {
  89         if (leadingZeros)
  90             *ptr = '0';
  91         else
  92             *ptr = SYM_BLANK;
  93
  94         ptr++;
  95         remaining--;
  96     }
  97
  98     // Keep number right aligned and correct length
  99     if(explicitDecimal && decimals == 0) {
 100         uint8_t blank_spaces = ptr - buff;
 101         int8_t rem_spaces = length - (digits + blank_spaces);
 102         // Add any needed remaining leading spaces
 103         while(rem_spaces > 0)
 104         {
 105             if (leadingZeros)
 106                 *ptr = '0';
 107             else
 108                 *ptr = SYM_BLANK;
 109
 110             ptr++;
 111             remaining--;
 112             rem_spaces--;
 113         }
 114     }
 115
 116     // Write the minus sign if required
 117     if (negative) {
 118         *ptr = '-';
 119         ptr++;
 120     }
 121     // Now write the digits.
 122     ui2a(integerPart, 10, 0, ptr);
 123     ptr += digits;
 124
 125     if (decimals > 0) {
 126         if (explicitDecimal) {
 127             *ptr = '.';
 128             ptr++;
 129         } else {
 130             *(ptr - 1) += SYM_ZERO_HALF_TRAILING_DOT - '0';
 131         }
 132         dec = ptr;
 133         int factor = 3; // we're getting the decimal part in millis first
 134         while (decimals < factor) {
 135             factor--;
 136             millis /= 10;
 137         }
 138         int decimalDigits = digitCount(millis);
 139         while (decimalDigits < decimals) {
 140             decimalDigits++;
 141             *ptr = '0';
 142             ptr++;
 143         }
 144         ui2a(millis, 10, 0, ptr);
 145         if (!explicitDecimal) {
 146             *dec += SYM_ZERO_HALF_LEADING_DOT - '0';
 147         }
 148     }
 149     return scaled;
 150 }
 151 #endif