Tools/Replay/MsgHandler.h

   1 #pragma once
   2
   3 #include <AP_Logger/AP_Logger.h>
   4 #include "VehicleType.h"
   5
   6 #include <stdio.h>
   7
   8 // ignore cast errors in this case to keep complexity down
   9 // on x86 where replay is run we don't care about cast alignment
  10 #pragma GCC diagnostic ignored "-Wcast-align"
  11
  12 #define LOGREADER_MAX_FIELDS 30
  13
  14 #define streq(x, y) (!strcmp(x, y))
  15
  16 class MsgHandler {
  17 public:
  18     // constructor - create a parser for a MavLink message format
  19     MsgHandler(const struct log_Format &f);
  20
  21     // retrieve a comma-separated list of all labels
  22     void string_for_labels(char *buffer, uint32_t bufferlen);
  23
  24     // field_value - retrieve the value of a field from the supplied message
  25     // these return false if the field was not found
  26     template<typename R>
  27     bool field_value(uint8_t *msg, const char *label, R &ret);
  28
  29     bool field_value(uint8_t *msg, const char *label, Vector3f &ret);
  30     bool field_value(uint8_t *msg, const char *label,
  31                      char *buffer, uint8_t bufferlen);
  32
  33     template <typename R>
  34     void require_field(uint8_t *msg, const char *label, R &ret)
  35         {
  36             if (! field_value(msg, label, ret)) {
  37                 field_not_found(msg, label);
  38             }
  39         }
  40     void require_field(uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen);
  41     float require_field_float(uint8_t *msg, const char *label);
  42     uint8_t require_field_uint8_t(uint8_t *msg, const char *label);
  43     int32_t require_field_int32_t(uint8_t *msg, const char *label);
  44     uint16_t require_field_uint16_t(uint8_t *msg, const char *label);
  45     int16_t require_field_int16_t(uint8_t *msg, const char *label);
  46
  47 private:
  48
  49     void add_field(const char *_label, uint8_t _type, uint8_t _offset,
  50                    uint8_t length);
  51
  52     template<typename R>
  53     void field_value_for_type_at_offset(uint8_t *msg, uint8_t type,
  54                                         uint8_t offset, R &ret);
  55
  56     struct format_field_info { // parsed field information
  57         char *label;
  58         uint8_t type;
  59         uint8_t offset;
  60         uint8_t length;
  61     };
  62     struct format_field_info field_info[LOGREADER_MAX_FIELDS];
  63
  64     uint8_t next_field;
  65     size_t size_for_type_table[52]; // maps field type (e.g. 'f') to e.g 4 bytes
  66
  67     struct format_field_info *find_field_info(const char *label);
  68
  69     void parse_format_fields();
  70     void init_field_types();
  71     void add_field_type(char type, size_t size);
  72     uint8_t size_for_type(char type);
  73
  74 protected:
  75     struct log_Format f; // the format we are a parser for
  76
  77     void location_from_msg(uint8_t *msg, Location &loc, const char *label_lat,
  78                            const char *label_long, const char *label_alt);
  79
  80     void ground_vel_from_msg(uint8_t *msg,
  81                              Vector3f &vel,
  82                              const char *label_speed,
  83                              const char *label_course,
  84                              const char *label_vz);
  85
  86     void attitude_from_msg(uint8_t *msg,
  87                            Vector3f &att,
  88                            const char *label_roll,
  89                            const char *label_pitch,
  90                            const char *label_yaw);
  91     [[noreturn]] void field_not_found(uint8_t *msg, const char *label);
  92 };
  93
  94 template<typename R>
  95 bool MsgHandler::field_value(uint8_t *msg, const char *label, R &ret)
  96 {
  97     struct format_field_info *info = find_field_info(label);
  98     if (info == NULL) {
  99         return false;
 100     }
 101
 102     uint8_t offset = info->offset;
 103     if (offset == 0) {
 104         return false;
 105     }
 106
 107     field_value_for_type_at_offset(msg, info->type, offset, ret);
 108
 109     return true;
 110 }
 111
 112
 113 template<typename R>
 114 inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg,
 115                                                       uint8_t type,
 116                                                       uint8_t offset,
 117                                                       R &ret)
 118 {
 119     /* we register the types - add_field_type - so can we do without
 120      * this switch statement somehow? */
 121     switch (type) {
 122     case 'B':
 123         ret = (R)(((uint8_t*)&msg[offset])[0]);
 124         break;
 125     case 'c':
 126     case 'h':
 127         ret = (R)(((int16_t*)&msg[offset])[0]);
 128         break;
 129     case 'H':
 130         ret = (R)(((uint16_t*)&msg[offset])[0]);
 131         break;
 132     case 'C':
 133         ret = (R)(((uint16_t*)&msg[offset])[0]);
 134         break;
 135     case 'f':
 136         ret = (R)(((float*)&msg[offset])[0]);
 137         break;
 138     case 'I':
 139     case 'E':
 140         ret = (R)(((uint32_t*)&msg[offset])[0]);
 141         break;
 142     case 'L':
 143     case 'e':
 144         ret = (R)(((int32_t*)&msg[offset])[0]);
 145         break;
 146     case 'q':
 147         ret = (R)(((int64_t*)&msg[offset])[0]);
 148         break;
 149     case 'Q':
 150         ret = (R)(((uint64_t*)&msg[offset])[0]);
 151         break;
 152     default:
 153         ::printf("Unhandled format type (%c)\n", type);
 154         exit(1);
 155     }
 156 }