Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :  *   AP_Common.cpp - common utility functions
      17             :  */
      18             : 
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include "AP_Common.h"
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : /* assert that const vals are float, not double. so 100.0 means 100.0f */
      26             : static_assert(sizeof(1e6) == sizeof(float), "Compilation needs to use single-precision constants");
      27             : 
      28             : /*
      29             :   Return true if value is between lower and upper bound inclusive.
      30             :   False otherwise.
      31             : */
      32        1051 : bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound)
      33             : {
      34        1051 :     if ((lower_bound <= upper_bound) &&
      35        1049 :         (value >= lower_bound) && (value <= upper_bound)) {
      36        1043 :         return true;
      37             :     }
      38             : 
      39           8 :     return false;
      40             : 
      41             : }
      42             : 
      43             : /**
      44             :  * return the numeric value of an ascii hex character
      45             :  *
      46             :  * @param [in]  a   Hexa character
      47             :  * @param [out] res uint8 value
      48             :  * @retval true  Conversion OK
      49             :  * @retval false Input value error
      50             :  * @Note Input character is 0-9, A-F, a-f
      51             :  *  A 0x41, a 0x61, 0 0x30
      52             :  */
      53       69039 : bool hex_char_to_nibble(uint8_t a, uint8_t &res)
      54             : {
      55       69039 :     uint8_t nibble_low  = a & 0xf;
      56             : 
      57       69039 :     switch (a & 0xf0) {
      58       56292 :     case 0x30:  // 0-
      59       56292 :         if (nibble_low > 9) {
      60           2 :             return false;
      61             :         }
      62       56290 :         res = nibble_low;
      63       56290 :         break;
      64       12711 :     case 0x40:  // uppercase A-
      65             :     case 0x60:  // lowercase a-
      66       12711 :         if (nibble_low == 0 || nibble_low > 6) {
      67          26 :             return false;
      68             :         }
      69       12685 :         res = nibble_low + 9;
      70       12685 :         break;
      71          36 :     default:
      72          36 :         return false;
      73             :     }
      74       68975 :     return true;
      75             : }
      76             : 
      77             : /*
      78             :   decode two hex characters into a byte.
      79             :   e.g. hex_twochars_to_uint8("3F", res) -> res = 0x3F
      80             :  */
      81        3345 : bool hex_twochars_to_uint8(const char s[2], uint8_t &res)
      82             : {
      83             :     uint8_t hi, lo;
      84        3345 :     if (!hex_char_to_nibble(s[0], hi) || !hex_char_to_nibble(s[1], lo)) {
      85          48 :         return false;
      86             :     }
      87        3297 :     res = (hi << 4) | lo;
      88        3297 :     return true;
      89             : }
      90             : 
      91           8 : bool hex_charpairs_to_uint8s(const char *s, uint8_t num_pairs, uint8_t *out)
      92             : {
      93          20 :     for (uint8_t i = 0; i < num_pairs; i++) {
      94             :         uint8_t hi, lo;
      95          14 :         if (!hex_char_to_nibble(s[i*2], hi) || !hex_char_to_nibble(s[i*2+1], lo)) {
      96           2 :             return false;
      97             :         }
      98          12 :         out[i] = (hi << 4) | lo;
      99             :     }
     100           6 :     return true;
     101             : }
     102             : 
     103             : /*
     104             :   decode len hex characters into a uint32_t, treating each character
     105             :   as a nibble (most-significant first).
     106             :   e.g. hex_chars_to_uint32("1A2B", 4, out) -> out = 0x1A2B
     107             :  */
     108        9036 : bool hex_chars_to_uint32(const char *s, uint8_t len, uint32_t &out)
     109             : {
     110        9036 :     out = 0;
     111       45182 :     for (uint8_t i = 0; i < len; i++) {
     112             :         uint8_t nibble;
     113       36148 :         if (!hex_char_to_nibble(s[i], nibble)) {
     114           2 :             return false;
     115             :         }
     116       36146 :         out = (out << 4) | nibble;
     117             :     }
     118        9034 :     return true;
     119             : }
     120             : 
     121             : /*
     122             :   strncpy without the warning for not leaving room for nul termination
     123             :  */
     124    11608544 : size_t strncpy_noterm(char *dest, const char *src, size_t n)
     125             : {
     126    11608544 :     size_t len = strnlen(src, n);
     127    11608544 :     size_t ret = len; // return value is length of src
     128    11608544 :     if (len < n) {
     129             :         // include nul term if it fits
     130     9484104 :         len++;
     131             :     }
     132    11608544 :     memcpy(dest, src, len);
     133    11608544 :     return ret;
     134             : }