src/main/blackbox/blackbox_encoding.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight 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  * Cleanflight 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 <stdlib.h>
  19 #include <stdarg.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #ifdef USE_BLACKBOX
  25
  26 #include "blackbox_encoding.h"
  27 #include "blackbox_io.h"
  28
  29 #include "common/encoding.h"
  30 #include "common/printf.h"
  31
  32
  33 static void _putc(void *p, char c)
  34 {
  35     (void)p;
  36     blackboxWrite(c);
  37 }
  38
  39 static int blackboxPrintfv(const char *fmt, va_list va)
  40 {
  41     return tfp_format(NULL, _putc, fmt, va);
  42 }
  43
  44
  45 //printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!)
  46 int blackboxPrintf(const char *fmt, ...)
  47 {
  48     va_list va;
  49
  50     va_start(va, fmt);
  51
  52     const int written = blackboxPrintfv(fmt, va);
  53
  54     va_end(va);
  55
  56     return written;
  57 }
  58
  59 /*
  60  * printf a Blackbox header line with a leading "H " and trailing "\n" added automatically. blackboxHeaderBudget is
  61  * decreased to account for the number of bytes written.
  62  */
  63 void blackboxPrintfHeaderLine(const char *name, const char *fmt, ...)
  64 {
  65     va_list va;
  66
  67     blackboxWrite('H');
  68     blackboxWrite(' ');
  69     blackboxPrint(name);
  70     blackboxWrite(':');
  71
  72     va_start(va, fmt);
  73
  74     const int written = blackboxPrintfv(fmt, va);
  75
  76     va_end(va);
  77
  78     blackboxWrite('\n');
  79
  80     blackboxHeaderBudget -= written + 3;
  81 }
  82
  83 /**
  84  * Write an unsigned integer to the blackbox serial port using variable byte encoding.
  85  */
  86 void blackboxWriteUnsignedVB(uint32_t value)
  87 {
  88     //While this isn't the final byte (we can only write 7 bits at a time)
  89     while (value > 127) {
  90         blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow"
  91         value >>= 7;
  92     }
  93     blackboxWrite(value);
  94 }
  95
  96 /**
  97  * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding.
  98  */
  99 void blackboxWriteSignedVB(int32_t value)
 100 {
 101     //ZigZag encode to make the value always positive
 102     blackboxWriteUnsignedVB(zigzagEncode(value));
 103 }
 104
 105 void blackboxWriteSignedVBArray(int32_t *array, int count)
 106 {
 107     for (int i = 0; i < count; i++) {
 108         blackboxWriteSignedVB(array[i]);
 109     }
 110 }
 111
 112 void blackboxWriteSigned16VBArray(int16_t *array, int count)
 113 {
 114     for (int i = 0; i < count; i++) {
 115         blackboxWriteSignedVB(array[i]);
 116     }
 117 }
 118
 119 void blackboxWriteS16(int16_t value)
 120 {
 121     blackboxWrite(value & 0xFF);
 122     blackboxWrite((value >> 8) & 0xFF);
 123 }
 124
 125 /**
 126  * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits
 127  */
 128 void blackboxWriteTag2_3S32(int32_t *values)
 129 {
 130     static const int NUM_FIELDS = 3;
 131
 132     //Need to be enums rather than const ints if we want to switch on them (due to being C)
 133     enum {
 134         BITS_2  = 0,
 135         BITS_4  = 1,
 136         BITS_6  = 2,
 137         BITS_32 = 3
 138     };
 139
 140     enum {
 141         BYTES_1  = 0,
 142         BYTES_2  = 1,
 143         BYTES_3  = 2,
 144         BYTES_4  = 3
 145     };
 146
 147     int selector = BITS_2, selector2;
 148
 149     /*
 150      * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes
 151      * below:
 152      *
 153      * Selector possibilities
 154      *
 155      * 2 bits per field  ss11 2233,
 156      * 4 bits per field  ss00 1111 2222 3333
 157      * 6 bits per field  ss11 1111 0022 2222 0033 3333
 158      * 32 bits per field sstt tttt followed by fields of various byte counts
 159      */
 160     for (int x = 0; x < NUM_FIELDS; x++) {
 161         //Require more than 6 bits?
 162         if (values[x] >= 32 || values[x] < -32) {
 163             selector = BITS_32;
 164             break;
 165         }
 166
 167         //Require more than 4 bits?
 168         if (values[x] >= 8 || values[x] < -8) {
 169              if (selector < BITS_6) {
 170                  selector = BITS_6;
 171              }
 172         } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits?
 173             if (selector < BITS_4) {
 174                 selector = BITS_4;
 175             }
 176         }
 177     }
 178
 179     switch (selector) {
 180     case BITS_2:
 181         blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03));
 182         break;
 183     case BITS_4:
 184         blackboxWrite((selector << 6) | (values[0] & 0x0F));
 185         blackboxWrite((values[1] << 4) | (values[2] & 0x0F));
 186         break;
 187     case BITS_6:
 188         blackboxWrite((selector << 6) | (values[0] & 0x3F));
 189         blackboxWrite((uint8_t)values[1]);
 190         blackboxWrite((uint8_t)values[2]);
 191         break;
 192     case BITS_32:
 193         /*
 194          * Do another round to compute a selector for each field, assuming that they are at least 8 bits each
 195          *
 196          * Selector2 field possibilities
 197          * 0 - 8 bits
 198          * 1 - 16 bits
 199          * 2 - 24 bits
 200          * 3 - 32 bits
 201          */
 202         selector2 = 0;
 203
 204         //Encode in reverse order so the first field is in the low bits:
 205         for (int x = NUM_FIELDS - 1; x >= 0; x--) {
 206             selector2 <<= 2;
 207
 208             if (values[x] < 128 && values[x] >= -128) {
 209                 selector2 |= BYTES_1;
 210             } else if (values[x] < 32768 && values[x] >= -32768) {
 211                 selector2 |= BYTES_2;
 212             } else if (values[x] < 8388608 && values[x] >= -8388608) {
 213                 selector2 |= BYTES_3;
 214             } else {
 215                 selector2 |= BYTES_4;
 216             }
 217         }
 218
 219         //Write the selectors
 220         blackboxWrite((selector << 6) | selector2);
 221
 222         //And now the values according to the selectors we picked for them
 223         for (int x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) {
 224             switch (selector2 & 0x03) {
 225             case BYTES_1:
 226                 blackboxWrite(values[x]);
 227                 break;
 228             case BYTES_2:
 229                 blackboxWrite(values[x]);
 230                 blackboxWrite(values[x] >> 8);
 231                 break;
 232             case BYTES_3:
 233                 blackboxWrite(values[x]);
 234                 blackboxWrite(values[x] >> 8);
 235                 blackboxWrite(values[x] >> 16);
 236                 break;
 237             case BYTES_4:
 238                 blackboxWrite(values[x]);
 239                 blackboxWrite(values[x] >> 8);
 240                 blackboxWrite(values[x] >> 16);
 241                 blackboxWrite(values[x] >> 24);
 242                 break;
 243             }
 244         }
 245         break;
 246     }
 247 }
 248
 249 /**
 250  * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
 251  */
 252 void blackboxWriteTag8_4S16(int32_t *values)
 253 {
 254
 255     //Need to be enums rather than const ints if we want to switch on them (due to being C)
 256     enum {
 257         FIELD_ZERO  = 0,
 258         FIELD_4BIT  = 1,
 259         FIELD_8BIT  = 2,
 260         FIELD_16BIT = 3
 261     };
 262
 263     uint8_t selector = 0;
 264     //Encode in reverse order so the first field is in the low bits:
 265     for (int x = 3; x >= 0; x--) {
 266         selector <<= 2;
 267
 268         if (values[x] == 0) {
 269             selector |= FIELD_ZERO;
 270         } else if (values[x] < 8 && values[x] >= -8) {
 271             selector |= FIELD_4BIT;
 272         } else if (values[x] < 128 && values[x] >= -128) {
 273             selector |= FIELD_8BIT;
 274         } else {
 275             selector |= FIELD_16BIT;
 276         }
 277     }
 278
 279     blackboxWrite(selector);
 280
 281     int nibbleIndex = 0;
 282     uint8_t buffer = 0;
 283     for (int x = 0; x < 4; x++, selector >>= 2) {
 284         switch (selector & 0x03) {
 285         case FIELD_ZERO:
 286             //No-op
 287             break;
 288         case FIELD_4BIT:
 289             if (nibbleIndex == 0) {
 290                 //We fill high-bits first
 291                 buffer = values[x] << 4;
 292                 nibbleIndex = 1;
 293             } else {
 294                 blackboxWrite(buffer | (values[x] & 0x0F));
 295                 nibbleIndex = 0;
 296             }
 297             break;
 298         case FIELD_8BIT:
 299             if (nibbleIndex == 0) {
 300                 blackboxWrite(values[x]);
 301             } else {
 302                 //Write the high bits of the value first (mask to avoid sign extension)
 303                 blackboxWrite(buffer | ((values[x] >> 4) & 0x0F));
 304                 //Now put the leftover low bits into the top of the next buffer entry
 305                 buffer = values[x] << 4;
 306             }
 307             break;
 308         case FIELD_16BIT:
 309             if (nibbleIndex == 0) {
 310                 //Write high byte first
 311                 blackboxWrite(values[x] >> 8);
 312                 blackboxWrite(values[x]);
 313             } else {
 314                 //First write the highest 4 bits
 315                 blackboxWrite(buffer | ((values[x] >> 12) & 0x0F));
 316                 // Then the middle 8
 317                 blackboxWrite(values[x] >> 4);
 318                 //Only the smallest 4 bits are still left to write
 319                 buffer = values[x] << 4;
 320             }
 321             break;
 322         }
 323     }
 324     //Anything left over to write?
 325     if (nibbleIndex == 1) {
 326         blackboxWrite(buffer);
 327     }
 328 }
 329
 330 /**
 331  * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is
 332  * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero).
 333  *
 334  * valueCount must be 8 or less.
 335  */
 336 void blackboxWriteTag8_8SVB(int32_t *values, int valueCount)
 337 {
 338     uint8_t header;
 339
 340     if (valueCount > 0) {
 341         //If we're only writing one field then we can skip the header
 342         if (valueCount == 1) {
 343             blackboxWriteSignedVB(values[0]);
 344         } else {
 345             //First write a one-byte header that marks which fields are non-zero
 346             header = 0;
 347
 348             // First field should be in low bits of header
 349             for (int i = valueCount - 1; i >= 0; i--) {
 350                 header <<= 1;
 351
 352                 if (values[i] != 0) {
 353                     header |= 0x01;
 354                 }
 355             }
 356
 357             blackboxWrite(header);
 358
 359             for (int i = 0; i < valueCount; i++) {
 360                 if (values[i] != 0) {
 361                     blackboxWriteSignedVB(values[i]);
 362                 }
 363             }
 364         }
 365     }
 366 }
 367
 368 /** Write unsigned integer **/
 369 void blackboxWriteU32(int32_t value)
 370 {
 371     blackboxWrite(value & 0xFF);
 372     blackboxWrite((value >> 8) & 0xFF);
 373     blackboxWrite((value >> 16) & 0xFF);
 374     blackboxWrite((value >> 24) & 0xFF);
 375 }
 376
 377 /** Write float value in the integer form **/
 378 void blackboxWriteFloat(float value)
 379 {
 380     blackboxWriteU32(castFloatBytesToInt(value));
 381 }
 382 #endif // BLACKBOX