restraining_bolt/FileMAVLinkReader.cpp at master · vincemic/restraining_bolt · GitHub

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
  #include "FileMAVLinkReader.h"
#include <ArduinoLog.h>

/**
 * @brief FileMAVLinkReader constructor
 *
 *
 * @param mavlinkLogFilePath File path on SD card to MAVLink log file like what is created by Mission Planner
 *
 * @param mavlinkEvebtReceiver Custom MAVLink receiver to capture incomming events
 *
 *
*/
FileMAVLinkReader::FileMAVLinkReader( const char* mavlinkLogFilePath, MAVLinkEventReceiver* mavlinkEvebtReceiver, uint8_t fileSpeedMilliseconds )
	: MAVLinkReader( mavlinkEvebtReceiver )
{
	_nextIntervalMAVLinkMilliseconds = fileSpeedMilliseconds;
	_mavlinkLogFilePath = mavlinkLogFilePath;

	if ( !SD.exists( _mavlinkLogFilePath ) )
	{
		Log.trace( "Cannot find MAVLink file: %s", _mavlinkLogFilePath );
	}
	else
	{
		Log.trace( "Reading MAVLink log file: %s", _mavlinkLogFilePath );
		_mavlinkFile = SD.open( _mavlinkLogFilePath, FILE_READ );
	}
}


bool FileMAVLinkReader::readByte( uint8_t* buffer )
{
	return _mavlinkFile.readBytes( buffer, 1 ) == 1;
}

void FileMAVLinkReader::tick()
{
	unsigned long currentMillisMAVLink = millis();

	// If ready to read next message
	if ( currentMillisMAVLink - _previousMAVLinkMilliseconds >= _nextIntervalMAVLinkMilliseconds )
	{
		receiveMAVLinkMessages();
		_previousMAVLinkMilliseconds = currentMillisMAVLink;


	}

}


uint32_t FileMAVLinkReader::getMissionTime()
{

	 return _systemBootTimeMilliseconds;

}