CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

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/libraries/AP_DAL/AP_DAL.h
Views: 1798
1
#pragma once

2


3
#include "AP_DAL_InertialSensor.h"

4
#include "AP_DAL_Baro.h"

5
#include "AP_DAL_GPS.h"

6
#include "AP_DAL_RangeFinder.h"

7
#include "AP_DAL_Compass.h"

8
#include "AP_DAL_Airspeed.h"

9
#include "AP_DAL_Beacon.h"

10
#include "AP_DAL_VisualOdom.h"

11


12
#include "LogStructure.h"

13


14
#include <stdio.h>

15
#include <stdint.h>

16
#include <cstddef>

17


18


19
#define DAL_CORE(c) AP::dal().logging_core(c)

20


21
class NavEKF2;

22
class NavEKF3;

23


24
class AP_DAL {

25
public:

26


27
    enum class FrameType : uint8_t {

28
        InitialiseFilterEKF2 = 1U<<0,

29
        UpdateFilterEKF2 = 1U<<1,

30
        InitialiseFilterEKF3 = 1<<2,

31
        UpdateFilterEKF3 = 1<<3,

32
        LogWriteEKF2 = 1<<4,

33
        LogWriteEKF3 = 1<<5,

34
    };

35


36
    enum class Event : uint8_t {

37
        resetGyroBias             =  0,

38
        resetHeightDatum          =  1,

39
        //setInhibitGPS           =  2, // removed

40
        //setTakeoffExpected        =  3, // removed

41
        //unsetTakeoffExpected      =  4, // removed

42
        //setTouchdownExpected      =  5, // removed

43
        //unsetTouchdownExpected    =  6, // removed

44
        //setInhibitGpsVertVelUse   =  7, // removed

45
        //unsetInhibitGpsVertVelUse =  8, // removed

46
        setTerrainHgtStable       =  9,

47
        unsetTerrainHgtStable     = 10,

48
        requestYawReset           = 11,

49
        checkLaneSwitch           = 12,

50
        setSourceSet0             = 13,

51
        setSourceSet1             = 14,

52
        setSourceSet2             = 15,

53
    };

54


55
    // must remain the same as AP_AHRS_VehicleClass numbers-wise

56
    enum class VehicleClass : uint8_t {

57
        UNKNOWN,

58
        GROUND,

59
        COPTER,

60
        FIXED_WING,

61
        SUBMARINE,

62
    };

63


64
    AP_DAL() {}

65


66
    static AP_DAL *get_singleton() {

67
        if (!_singleton) {

68
            _singleton = NEW_NOTHROW AP_DAL();

69
        }

70
        return _singleton;

71
    }

72


73
    void start_frame(FrameType frametype);

74
    void end_frame(void);

75
    uint64_t micros64() const { return _RFRH.time_us; }

76
    uint32_t micros() const { return _micros; }

77
    uint32_t millis() const { return _millis; }

78


79
    void log_event2(Event event);

80
    void log_SetOriginLLH2(const Location &loc);

81
    void log_writeDefaultAirSpeed2(const float aspeed, const float uncertainty);

82


83
    void log_event3(Event event);

84
    void log_SetOriginLLH3(const Location &loc);

85
    void log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);

86


87
    void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty);

88
    void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);

89


90
    enum class StateMask {

91
        ARMED = (1U<<0),

92
    };

93


94
    // EKF ID for timing checks

95
    enum class EKFType : uint8_t {

96
        EKF2 = 0,

97
        EKF3 = 1,

98
    };

99


100
    // check if we are low on CPU for this core

101
    bool ekf_low_time_remaining(EKFType etype, uint8_t core);

102
    

103
    // returns armed state for the current frame

104
    bool get_armed() const { return _RFRN.armed; }

105


106
    // memory available at start of current frame.  While this could

107
    // potentially change as we go through the frame, the

108
    // ramifications of being out of memory are that you don't start

109
    // the EKF, so the simplicity of having one value for the entire

110
    // frame is worthwhile.

111
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

112
    uint32_t available_memory() const { return _RFRN.available_memory + 10240; }

113
#else

114
    uint32_t available_memory() const { return _RFRN.available_memory; }

115
#endif

116


117
    int8_t get_ekf_type(void) const {

118
        return _RFRN.ekf_type;

119
    }

120


121
    int snprintf(char* str, size_t size, const char *format, ...) const;

122


123
    // copied in AP_HAL/Util.h

124
    enum class MemoryType : uint8_t {

125
        DMA_SAFE = 0,

126
        FAST     = 1,

127
    };

128
    void *malloc_type(size_t size, MemoryType mem_type) const;

129
    void free_type(void *ptr, size_t size, MemoryType memtype) const;

130


131
    AP_DAL_InertialSensor &ins() { return _ins; }

132
    AP_DAL_Baro &baro() { return _baro; }

133
    AP_DAL_GPS &gps() { return _gps; }

134


135
#if AP_RANGEFINDER_ENABLED

136
    AP_DAL_RangeFinder *rangefinder() {

137
        return _rangefinder;

138
    }

139
#endif

140


141
    AP_DAL_Airspeed *airspeed() {

142
        return _airspeed;

143
    }

144
#if AP_BEACON_ENABLED

145
    AP_DAL_Beacon *beacon() {

146
        return _beacon;

147
    }

148
#endif

149
#if HAL_VISUALODOM_ENABLED

150
    AP_DAL_VisualOdom *visualodom() {

151
        return _visualodom;

152
    }

153
#endif

154


155
    AP_DAL_Compass &compass() { return _compass; }

156


157
    // random methods that AP_NavEKF3 wants to call on AHRS:

158
    bool airspeed_sensor_enabled(void) const {

159
        return _RFRN.ahrs_airspeed_sensor_enabled;

160
    }

161


162
    // this replaces AP::ahrs()->EAS2TAS(), which should probably go

163
    // away in favour of just using the Baro method.

164
    // get apparent to true airspeed ratio

165
    float get_EAS2TAS(void) const {

166
        return _RFRN.EAS2TAS;

167
    }

168


169
    VehicleClass get_vehicle_class(void) const {

170
        return (VehicleClass)_RFRN.vehicle_class;

171
    }

172


173
    bool get_fly_forward(void) const {

174
        return _RFRN.fly_forward;

175
    }

176


177
    bool get_takeoff_expected(void) const {

178
        return _RFRN.takeoff_expected;

179
    }

180


181
    bool get_touchdown_expected(void) const {

182
        return _RFRN.touchdown_expected;

183
    }

184


185
    // for EKF usage to enable takeoff expected to true

186
    void set_takeoff_expected();

187


188
    // get ahrs trim

189
    const Vector3f &get_trim() const {

190
        return _RFRN.ahrs_trim;

191
    }

192


193
    const Matrix3f &get_rotation_vehicle_body_to_autopilot_body(void) const {

194
        return _rotation_vehicle_body_to_autopilot_body;

195
    }

196


197
    // get the home location. This is const to prevent any changes to

198
    // home without telling AHRS about the change

199
    const class Location &get_home(void) const {

200
        return _home;

201
    }

202


203
    uint32_t get_time_flying_ms(void) const {

204
        return _RFRH.time_flying_ms;

205
    }

206


207
    bool opticalflow_enabled(void) const {

208
        return _RFRN.opticalflow_enabled;

209
    }

210


211
    bool wheelencoder_enabled(void) const {

212
        return _RFRN.wheelencoder_enabled;

213
    }

214


215
    // log optical flow data

216
    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);

217


218
    // log external nav data

219
    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);

220
    void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);

221


222
    // log wheel odometry data

223
    void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);

224
    void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);

225


226
    // Replay support:

227
    void handle_message(const log_RFRH &msg) {

228
        _RFRH = msg;

229
        _micros = _RFRH.time_us;

230
        _millis = _RFRH.time_us / 1000UL;

231
    }

232
    void handle_message(const log_RFRN &msg) {

233
        _RFRN = msg;

234
        _home.lat = msg.lat;

235
        _home.lng = msg.lng;

236
        _home.alt = msg.alt;

237
    }

238
    void handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

239


240
    void handle_message(const log_RISH &msg) {

241
        _ins.handle_message(msg);

242
    }

243
    void handle_message(const log_RISI &msg) {

244
        _ins.handle_message(msg);

245
    }

246


247
    void handle_message(const log_RASH &msg) {

248
        if (_airspeed == nullptr) {

249
            _airspeed = NEW_NOTHROW AP_DAL_Airspeed;

250
        }

251
        _airspeed->handle_message(msg);

252
    }

253
    void handle_message(const log_RASI &msg) {

254
        if (_airspeed == nullptr) {

255
            _airspeed = NEW_NOTHROW AP_DAL_Airspeed;

256
        }

257
        _airspeed->handle_message(msg);

258
    }

259


260
    void handle_message(const log_RBRH &msg) {

261
        _baro.handle_message(msg);

262
    }

263
    void handle_message(const log_RBRI &msg) {

264
        _baro.handle_message(msg);

265
    }

266


267
    void handle_message(const log_RRNH &msg) {

268
#if AP_RANGEFINDER_ENABLED

269
        if (_rangefinder == nullptr) {

270
            _rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;

271
        }

272
        _rangefinder->handle_message(msg);

273
#endif

274
    }

275
    void handle_message(const log_RRNI &msg) {

276
#if AP_RANGEFINDER_ENABLED

277
        if (_rangefinder == nullptr) {

278
            _rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;

279
        }

280
        _rangefinder->handle_message(msg);

281
#endif

282
    }

283


284
    void handle_message(const log_RGPH &msg) {

285
        _gps.handle_message(msg);

286
    }

287
    void handle_message(const log_RGPI &msg) {

288
        _gps.handle_message(msg);

289
    }

290
    void handle_message(const log_RGPJ &msg) {

291
        _gps.handle_message(msg);

292
    }

293


294
    void handle_message(const log_RMGH &msg) {

295
        _compass.handle_message(msg);

296
    }

297
    void handle_message(const log_RMGI &msg) {

298
        _compass.handle_message(msg);

299
    }

300


301
    void handle_message(const log_RBCH &msg) {

302
#if AP_BEACON_ENABLED

303
        if (_beacon == nullptr) {

304
            _beacon = NEW_NOTHROW AP_DAL_Beacon;

305
        }

306
        _beacon->handle_message(msg);

307
#endif

308
    }

309
    void handle_message(const log_RBCI &msg) {

310
#if AP_BEACON_ENABLED

311
        if (_beacon == nullptr) {

312
            _beacon = NEW_NOTHROW AP_DAL_Beacon;

313
        }

314
        _beacon->handle_message(msg);

315
#endif

316
    }

317
    void handle_message(const log_RVOH &msg) {

318
#if HAL_VISUALODOM_ENABLED

319
        if (_visualodom == nullptr) {

320
            _visualodom = NEW_NOTHROW AP_DAL_VisualOdom;

321
        }

322
        _visualodom->handle_message(msg);

323
#endif

324
    }

325
    void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

326
    void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

327
    void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

328
    void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

329
    void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

330
    void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

331


332
    // map core number for replay

333
    uint8_t logging_core(uint8_t c) const;

334


335
#if HAL_LOGGING_ENABLED

336
    // write out a DAL log message. If old_msg is non-null, then

337
    // only write if the content has changed

338
    static void WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size);

339
#endif

340


341
private:

342


343
    static AP_DAL *_singleton;

344


345
    // framing structures

346
    struct log_RFRH _RFRH;

347
    struct log_RFRF _RFRF;

348
    struct log_RFRN _RFRN;

349


350
    // push-based sensor structures

351
    struct log_ROFH _ROFH;

352
    struct log_REPH _REPH;

353
    struct log_REVH _REVH;

354
    struct log_RWOH _RWOH;

355
    struct log_RBOH _RBOH;

356
    struct log_RSLL _RSLL;

357


358
    // cached variables for speed:

359
    uint32_t _micros;

360
    uint32_t _millis;

361


362
    Matrix3f _rotation_vehicle_body_to_autopilot_body;

363
    Location _home;

364
    uint32_t _last_imu_time_us;

365


366
    AP_DAL_InertialSensor _ins;

367
    AP_DAL_Baro _baro;

368
    AP_DAL_GPS _gps;

369
#if AP_RANGEFINDER_ENABLED

370
    AP_DAL_RangeFinder *_rangefinder;

371
#endif

372
    AP_DAL_Compass _compass;

373
    AP_DAL_Airspeed *_airspeed;

374
#if AP_BEACON_ENABLED

375
    AP_DAL_Beacon *_beacon;

376
#endif

377
#if HAL_VISUALODOM_ENABLED

378
    AP_DAL_VisualOdom *_visualodom;

379
#endif

380


381
    static bool logging_started;

382
    static bool force_write;

383


384
    bool ekf2_init_done;

385
    bool ekf3_init_done;

386


387
    void init_sensors(void);

388
    bool init_done;

389
};

390


391
#if HAL_LOGGING_ENABLED

392
#define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))

393
#define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { static_assert(sizeof(v) == sizeof(old), "types must match"); \

394
                                                      AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)); } \

395
                                                 while (0)

396
#else

397
#define WRITE_REPLAY_BLOCK(sname,v) do { (void)v; } while (false)

398
#define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { (void)old; } while (false)

399
#endif

400


401
namespace AP {

402
    AP_DAL &dal();

403
};

404


405
// replay printf for debugging

406
void rprintf(const char *format, ...);

407


408


409