CoCalc -- LR_MsgHandler.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/LR_MsgHandler.cpp
Views: ¹⁷⁹⁸
1
#include "LR_MsgHandler.h"
2
#include "LogReader.h"
3
#include "Replay.h"
4

5
#include <AP_DAL/AP_DAL.h>
6

7
#include <cinttypes>
8

9
extern const AP_HAL::HAL& hal;
10

11
#define MSG_CREATE(sname,msgbytes) log_ ##sname msg; memcpy((void*)&msg, (msgbytes)+3, sizeof(msg));
12

13
LR_MsgHandler::LR_MsgHandler(struct log_Format &_f) :
14
    MsgHandler(_f) {
15
}
16

17
void LR_MsgHandler_RFRH::process_message(uint8_t *msgbytes)
18
{
19
    MSG_CREATE(RFRH, msgbytes);
20
    AP::dal().handle_message(msg);
21
}
22

23
void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes)
24
{
25
    MSG_CREATE(RFRF, msgbytes);
26
#define MAP_FLAG(flag1, flag2) if (msg.frame_types & uint8_t(flag1)) msg.frame_types |= uint8_t(flag2)
27
    /*
28
      when we force an EKF we map the trigger flags over
29
     */
30
    if (replay_force_ekf2) {
31
        MAP_FLAG(AP_DAL::FrameType::InitialiseFilterEKF3, AP_DAL::FrameType::InitialiseFilterEKF2);
32
        MAP_FLAG(AP_DAL::FrameType::UpdateFilterEKF3, AP_DAL::FrameType::UpdateFilterEKF2);
33
        MAP_FLAG(AP_DAL::FrameType::LogWriteEKF3, AP_DAL::FrameType::LogWriteEKF2);
34
    }
35
    if (replay_force_ekf3) {
36
        MAP_FLAG(AP_DAL::FrameType::InitialiseFilterEKF2, AP_DAL::FrameType::InitialiseFilterEKF3);
37
        MAP_FLAG(AP_DAL::FrameType::UpdateFilterEKF2, AP_DAL::FrameType::UpdateFilterEKF3);
38
        MAP_FLAG(AP_DAL::FrameType::LogWriteEKF2, AP_DAL::FrameType::LogWriteEKF3);
39
    }
40
#undef MAP_FLAG
41
    AP::dal().handle_message(msg, ekf2, ekf3);
42
}
43

44
void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes)
45
{
46
    MSG_CREATE(RFRN, msgbytes);
47
    AP::dal().handle_message(msg);
48
}
49

50
void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes)
51
{
52
    MSG_CREATE(REV2, msgbytes);
53

54
    switch ((AP_DAL::Event)msg.event) {
55

56
    case AP_DAL::Event::resetGyroBias:
57
        ekf2.resetGyroBias();
58
        break;
59
    case AP_DAL::Event::resetHeightDatum:
60
        ekf2.resetHeightDatum();
61
        break;
62
    case AP_DAL::Event::setTerrainHgtStable:
63
        ekf2.setTerrainHgtStable(true);
64
        break;
65
    case AP_DAL::Event::unsetTerrainHgtStable:
66
        ekf2.setTerrainHgtStable(false);
67
        break;
68
    case AP_DAL::Event::requestYawReset:
69
        ekf2.requestYawReset();
70
        break;
71
    case AP_DAL::Event::checkLaneSwitch:
72
        ekf2.checkLaneSwitch();
73
        break;
74
    case AP_DAL::Event::setSourceSet0 ... AP_DAL::Event::setSourceSet2:
75
        break;
76
    }
77
    if (replay_force_ekf3) {
78
        LR_MsgHandler_REV3 h{f, ekf2, ekf3};
79
        h.process_message(msgbytes);
80
    }
81
}
82

83
void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes)
84
{
85
    MSG_CREATE(RSO2, msgbytes);
86
    Location loc;
87
    loc.lat = msg.lat;
88
    loc.lng = msg.lng;
89
    loc.alt = msg.alt;
90
    ekf2.setOriginLLH(loc);
91

92
    if (replay_force_ekf3) {
93
        LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
94
        h.process_message(msgbytes);
95
    }
96
}
97

98
void LR_MsgHandler_RWA2::process_message(uint8_t *msgbytes)
99
{
100
    MSG_CREATE(RWA2, msgbytes);
101
    ekf2.writeDefaultAirSpeed(msg.airspeed);
102
    if (replay_force_ekf3) {
103
        LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
104
        h.process_message(msgbytes);
105
    }
106
}
107

108

109
void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes)
110
{
111
    MSG_CREATE(REV3, msgbytes);
112

113
    switch ((AP_DAL::Event)msg.event) {
114

115
    case AP_DAL::Event::resetGyroBias:
116
        ekf3.resetGyroBias();
117
        break;
118
    case AP_DAL::Event::resetHeightDatum:
119
        ekf3.resetHeightDatum();
120
        break;
121
    case AP_DAL::Event::setTerrainHgtStable:
122
        ekf3.setTerrainHgtStable(true);
123
        break;
124
    case AP_DAL::Event::unsetTerrainHgtStable:
125
        ekf3.setTerrainHgtStable(false);
126
        break;
127
    case AP_DAL::Event::requestYawReset:
128
        ekf3.requestYawReset();
129
        break;
130
    case AP_DAL::Event::checkLaneSwitch:
131
        ekf3.checkLaneSwitch();
132
        break;
133
    case AP_DAL::Event::setSourceSet0 ... AP_DAL::Event::setSourceSet2:
134
        ekf3.setPosVelYawSourceSet(uint8_t(msg.event)-uint8_t(AP_DAL::Event::setSourceSet0));
135
        break;
136
    }
137

138
    if (replay_force_ekf2) {
139
        LR_MsgHandler_REV2 h{f, ekf2, ekf3};
140
        h.process_message(msgbytes);
141
    }
142
}
143

144
void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes)
145
{
146
    MSG_CREATE(RSO3, msgbytes);
147
    Location loc;
148
    loc.lat = msg.lat;
149
    loc.lng = msg.lng;
150
    loc.alt = msg.alt;
151
    ekf3.setOriginLLH(loc);
152
    if (replay_force_ekf2) {
153
        LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
154
        h.process_message(msgbytes);
155
    }
156
}
157

158
void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes)
159
{
160
    MSG_CREATE(RWA3, msgbytes);
161
    ekf3.writeDefaultAirSpeed(msg.airspeed, msg.uncertainty);
162
    if (replay_force_ekf2) {
163
        LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
164
        h.process_message(msgbytes);
165
    }
166
}
167

168
void LR_MsgHandler_REY3::process_message(uint8_t *msgbytes)
169
{
170
    MSG_CREATE(REY3, msgbytes);
171
    ekf3.writeEulerYawAngle(msg.yawangle, msg.yawangleerr, msg.timestamp_ms, msg.type);
172
}
173

174
void LR_MsgHandler_RISH::process_message(uint8_t *msgbytes)
175
{
176
    MSG_CREATE(RISH, msgbytes);
177
    AP::dal().handle_message(msg);
178
}
179
void LR_MsgHandler_RISI::process_message(uint8_t *msgbytes)
180
{
181
    MSG_CREATE(RISI, msgbytes);
182
    AP::dal().handle_message(msg);
183
}
184

185
void LR_MsgHandler_RASH::process_message(uint8_t *msgbytes)
186
{
187
    MSG_CREATE(RASH, msgbytes);
188
    AP::dal().handle_message(msg);
189
}
190
void LR_MsgHandler_RASI::process_message(uint8_t *msgbytes)
191
{
192
    MSG_CREATE(RASI, msgbytes);
193
    AP::dal().handle_message(msg);
194
}
195

196
void LR_MsgHandler_RBRH::process_message(uint8_t *msgbytes)
197
{
198
    MSG_CREATE(RBRH, msgbytes);
199
    AP::dal().handle_message(msg);
200
}
201

202
void LR_MsgHandler_RBRI::process_message(uint8_t *msgbytes)
203
{
204
    MSG_CREATE(RBRI, msgbytes);
205
    AP::dal().handle_message(msg);
206
}
207

208
void LR_MsgHandler_RRNH::process_message(uint8_t *msgbytes)
209
{
210
    MSG_CREATE(RRNH, msgbytes);
211
    AP::dal().handle_message(msg);
212
}
213

214
void LR_MsgHandler_RRNI::process_message(uint8_t *msgbytes)
215
{
216
    MSG_CREATE(RRNI, msgbytes);
217
    AP::dal().handle_message(msg);
218
}
219

220
void LR_MsgHandler_RGPH::process_message(uint8_t *msgbytes)
221
{
222
    MSG_CREATE(RGPH, msgbytes);
223
    AP::dal().handle_message(msg);
224
}
225

226
void LR_MsgHandler_RGPI::process_message(uint8_t *msgbytes)
227
{
228
    MSG_CREATE(RGPI, msgbytes);
229
    AP::dal().handle_message(msg);
230
}
231

232
void LR_MsgHandler_RGPJ::process_message(uint8_t *msgbytes)
233
{
234
    MSG_CREATE(RGPJ, msgbytes);
235
    AP::dal().handle_message(msg);
236
}
237

238
void LR_MsgHandler_RMGH::process_message(uint8_t *msgbytes)
239
{
240
    MSG_CREATE(RMGH, msgbytes);
241
    AP::dal().handle_message(msg);
242
}
243

244
void LR_MsgHandler_RMGI::process_message(uint8_t *msgbytes)
245
{
246
    MSG_CREATE(RMGI, msgbytes);
247
    AP::dal().handle_message(msg);
248
}
249

250
void LR_MsgHandler_RBCH::process_message(uint8_t *msgbytes)
251
{
252
    MSG_CREATE(RBCH, msgbytes);
253
    AP::dal().handle_message(msg);
254
}
255

256
void LR_MsgHandler_RBCI::process_message(uint8_t *msgbytes)
257
{
258
    MSG_CREATE(RBCI, msgbytes);
259
    AP::dal().handle_message(msg);
260
}
261

262
void LR_MsgHandler_RVOH::process_message(uint8_t *msgbytes)
263
{
264
    MSG_CREATE(RVOH, msgbytes);
265
    AP::dal().handle_message(msg);
266
}
267

268
void LR_MsgHandler_ROFH::process_message(uint8_t *msgbytes)
269
{
270
    MSG_CREATE(ROFH, msgbytes);
271
    AP::dal().handle_message(msg, ekf2, ekf3);
272
}
273

274
void LR_MsgHandler_RWOH::process_message(uint8_t *msgbytes)
275
{
276
    MSG_CREATE(RWOH, msgbytes);
277
    AP::dal().handle_message(msg, ekf2, ekf3);
278
}
279

280
void LR_MsgHandler_RBOH::process_message(uint8_t *msgbytes)
281
{
282
    MSG_CREATE(RBOH, msgbytes);
283
    AP::dal().handle_message(msg, ekf2, ekf3);
284
}
285

286
void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes)
287
{
288
    MSG_CREATE(REPH, msgbytes);
289
    AP::dal().handle_message(msg, ekf2, ekf3);
290
}
291

292
void LR_MsgHandler_RSLL::process_message(uint8_t *msgbytes)
293
{
294
    MSG_CREATE(RSLL, msgbytes);
295
    AP::dal().handle_message(msg, ekf2, ekf3);
296
}
297

298
void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes)
299
{
300
    MSG_CREATE(REVH, msgbytes);
301
    AP::dal().handle_message(msg, ekf2, ekf3);
302
}
303

304
#include <AP_AHRS/AP_AHRS.h>
305
#include "VehicleType.h"
306

307
bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
308
{
309
    const char *ignore_parms[] = {
310
        "LOG_FILE_BUFSIZE",
311
        "LOG_DISARMED"
312
    };
313
    for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) {
314
        if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
315
            ::printf("Ignoring set of %s to %f\n", name, value);
316
            return true;
317
        }
318
    }
319

320
    return LogReader::set_parameter(name, value);
321
}
322

323
void LR_MsgHandler_PARM::process_message(uint8_t *msg)
324
{
325
    const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
326
    char parameter_name[parameter_name_len];
327

328
    require_field(msg, "Name", parameter_name, parameter_name_len);
329

330
    float value = require_field_float(msg, "Value");
331
    set_parameter(parameter_name, value);
332
}
333

334
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.
