CoCalc -- LogReader.cpp

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Tools/Replay/LogReader.cpp
Views: ¹⁷⁹⁸
1
#include "LogReader.h"
2

3
#include "MsgHandler.h"
4
#include "Replay.h"
5

6
#include <stdio.h>
7
#include <unistd.h>
8
#include <sys/types.h>
9
#include <sys/stat.h>
10
#include <sys/types.h>
11
#include <signal.h>
12

13

14
#define DEBUG 1
15
#if DEBUG
16
# define debug(fmt, args...)     printf(fmt "\n", ##args)
17
#else
18
# define debug(fmt, args...)
19
#endif
20

21
#define streq(x, y) (!strcmp(x, y))
22

23
extern struct user_parameter *user_parameters;
24

25
LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
26
    AP_LoggerFileReader(),
27
    ekf2(_ekf2),
28
    ekf3(_ekf3),
29
    _log_structure(log_structure)
30
{
31
}
32

33
/*
34
  see if a type is in a list of types
35
 */
36
bool LogReader::in_list(const char *type, const char *list[])
37
{
38
    if (list == NULL) {
39
        return false;
40
    }
41
    for (uint8_t i=0; list[i] != NULL; i++) {
42
        if (strcmp(type, list[i]) == 0) {
43
            return true;
44
        }
45
    }
46
    return false;
47
}
48

49
bool LogReader::handle_log_format_msg(const struct log_Format &f)
50
{
51
    // emit the output as we receive it:
52
    AP::logger().WriteBlock((void*)&f, sizeof(f));
53

54
	char name[5];
55
	memset(name, '\0', 5);
56
	memcpy(name, f.name, 4);
57

58
    if (msgparser[f.type] != NULL) {
59
        return true;
60
    }
61

62
    // map from format name to a parser subclass:
63
	if (streq(name, "PARM")) {
64
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_PARM(formats[f.type]);
65
    } else if (streq(name, "RFRH")) {
66
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRH(formats[f.type]);
67
    } else if (streq(name, "RFRF")) {
68
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3);
69
    } else if (streq(name, "RFRN")) {
70
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RFRN(formats[f.type]);
71
    } else if (streq(name, "REV2")) {
72
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV2(formats[f.type], ekf2, ekf3);
73
	} else if (streq(name, "RSO2")) {
74
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO2(formats[f.type], ekf2, ekf3);
75
	} else if (streq(name, "RWA2")) {
76
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA2(formats[f.type], ekf2, ekf3);
77
	} else if (streq(name, "REV3")) {
78
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REV3(formats[f.type], ekf2, ekf3);
79
	} else if (streq(name, "RSO3")) {
80
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSO3(formats[f.type], ekf2, ekf3);
81
	} else if (streq(name, "RWA3")) {
82
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWA3(formats[f.type], ekf2, ekf3);
83
	} else if (streq(name, "REY3")) {
84
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REY3(formats[f.type], ekf2, ekf3);
85
	} else if (streq(name, "RISH")) {
86
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RISH(formats[f.type]);
87
	} else if (streq(name, "RISI")) {
88
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RISI(formats[f.type]);
89
    } else if (streq(name, "RASH")) {
90
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RASH(formats[f.type]);
91
	} else if (streq(name, "RASI")) {
92
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RASI(formats[f.type]);
93
	} else if (streq(name, "RBRH")) {
94
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBRH(formats[f.type]);
95
	} else if (streq(name, "RBRI")) {
96
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBRI(formats[f.type]);
97
	} else if (streq(name, "RRNH")) {
98
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RRNH(formats[f.type]);
99
	} else if (streq(name, "RRNI")) {
100
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RRNI(formats[f.type]);
101
	} else if (streq(name, "RGPH")) {
102
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RGPH(formats[f.type]);
103
	} else if (streq(name, "RGPI")) {
104
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RGPI(formats[f.type]);
105
    } else if (streq(name, "RGPJ")) {
106
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RGPJ(formats[f.type]);
107
	} else if (streq(name, "RMGH")) {
108
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RMGH(formats[f.type]);
109
	} else if (streq(name, "RMGI")) {
110
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RMGI(formats[f.type]);
111
	} else if (streq(name, "RBCH")) {
112
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBCH(formats[f.type]);
113
	} else if (streq(name, "RBCI")) {
114
	    msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBCI(formats[f.type]);
115
    } else if (streq(name, "RVOH")) {
116
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RVOH(formats[f.type]);
117
    } else if (streq(name, "ROFH")) {
118
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
119
    } else if (streq(name, "REPH")) {
120
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3);
121
	} else if (streq(name, "RSLL")) {
122
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3);
123
    } else if (streq(name, "REVH")) {
124
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3);
125
    } else if (streq(name, "RWOH")) {
126
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3);
127
    } else if (streq(name, "RBOH")) {
128
        msgparser[f.type] = NEW_NOTHROW LR_MsgHandler_RBOH(formats[f.type], ekf2, ekf3);
129
	} else {
130
        // debug("  No parser for (%s)\n", name);
131
    }
132

133
    return true;
134
}
135

136
bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
137
    // emit the output as we receive it:
138
    AP::logger().WriteBlock(msg, f.length);
139

140
    LR_MsgHandler *p = msgparser[f.type];
141
    if (p == NULL) {
142
        return true;
143
    }
144

145
    p->process_message(msg);
146

147
    return true;
148
}
149

150
/*
151
  see if a user parameter is set
152
 */
153
bool LogReader::check_user_param(const char *name)
154
{
155
    for (struct user_parameter *u=user_parameters; u; u=u->next) {
156
        if (strcmp(name, u->name) == 0) {
157
            return true;
158
        }
159
    }
160
    return false;
161
}
162

163
bool LogReader::set_parameter(const char *name, float value, bool force)
164
{
165
    if (!force && check_user_param(name)) {
166
        // ignore user set parameters
167
        return false;
168
    }
169
    enum ap_var_type var_type;
170
    AP_Param *vp = AP_Param::find(name, &var_type);
171
    if (vp == NULL) {
172
        // a lot of parameters will not be found - e.g. FORMAT_VERSION
173
        // and all of the vehicle-specific parameters, ....
174
        return false;
175
    }
176
    float old_value = 0;
177
    if (var_type == AP_PARAM_FLOAT) {
178
        old_value = ((AP_Float *)vp)->cast_to_float();
179
        ((AP_Float *)vp)->set(value);
180
    } else if (var_type == AP_PARAM_INT32) {
181
        old_value = ((AP_Int32 *)vp)->cast_to_float();
182
        ((AP_Int32 *)vp)->set(value);
183
    } else if (var_type == AP_PARAM_INT16) {
184
        old_value = ((AP_Int16 *)vp)->cast_to_float();
185
        ((AP_Int16 *)vp)->set(value);
186
    } else if (var_type == AP_PARAM_INT8) {
187
        old_value = ((AP_Int8 *)vp)->cast_to_float();
188
        ((AP_Int8 *)vp)->set(value);
189
    } else {
190
        AP_HAL::panic("What manner of evil is var_type=%u", var_type);
191
    }
192
    if (fabsf(old_value - value) > 1.0e-12) {
193
        ::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
194
    }
195
    return true;
196
}
197

198

199
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
