/
FileMAVLinkReader.h
61 lines (48 loc) · 1.66 KB
/
FileMAVLinkReader.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
#pragma once
// FileMAVLinkReader.h
#ifndef _FILEMAVLINKREADER_h
#define _FILEMAVLINKREADER_h
#if defined(ARDUINO) && ARDUINO >= 100
#include "arduino.h"
#else
#include "WProgram.h"
#endif
#include "MAVLinkReader.h"
#include <SD.h>
/**
* @brief This class reads a telemetry file from SD card for testing. Mission Planner .tlog has been tested.
*/
class FileMAVLinkReader : public MAVLinkReader
{
public:
/**
* @brief Constructor.
* @param mavlinkLogFilePath The file to MAVLink file. Use 8.3 file naming convention to support SD API.
* @param mavlinkEvebtReceiver The event receiver that will capture MAVLink messages read from the file.
* @param fileSpeedMilliseconds The speed at which to read the MAVLink file during testing.
*
*/
FileMAVLinkReader( const char* mavlinkLogFilePath, MAVLinkEventReceiver* mavlinkEvebtReceiver, uint8_t fileSpeedMilliseconds );
/**
* @brief Read a single byte from the MAVLink source.
* @param buffer A buffer to read the byte into.
* @return True if a byte was read.
*/
virtual bool readByte( uint8_t* buffer );
/**
* @brief Used by the scheduling system to give FileMAVLinkReader execution time.
*/
virtual void tick();
/**
* @brief Returns the latest run time as reported from the flight controller messages in the recorded MAVLink file.
* Local millis cannot be used because the file is read at a speed that doesn't represent real time.
* @return Milliseconds since flight controller started.
*/
virtual uint32_t getMissionTime();
protected:
const char* _mavlinkLogFilePath;
File _mavlinkFile;
unsigned long _previousMAVLinkMilliseconds = 0;
unsigned long _nextIntervalMAVLinkMilliseconds = 1;
};
#endif