restraining_bolt/SerialMAVLinkReader.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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
/**
 * Copyright (C) 2020 Vincent Miceli - All Rights Reserved.
 * You may use, distribute, and modify this code under the
 * terms of the CC-BY-4.0 license. The author and publisher make no claims to the
 * suitability of this software for any application. By using this
 * software the end-user accepts all responsibility and liability for its use.
 * Please visit: https://creativecommons.org/licenses/by/4.0/ to get the latest version of this license.
 *
 *
 * \author Vincent Miceli
 *
 * \copyright  CC-BY-4.0
 *
 */


#include "SerialMAVLinkReader.h"
#include <ArduinoLog.h>


SerialMAVLinkReader::SerialMAVLinkReader( HardwareSerial* serial, MAVLinkEventReceiver* mavlinkEvebtReceiver )
	: MAVLinkReader( mavlinkEvebtReceiver )
{
	_serial = serial;

	Log.trace( "Starting MAVLink serial reader" );
	_serial->begin( 57600, SERIAL_8N1 );
}


bool SerialMAVLinkReader::readByte( uint8_t* buffer )
{

	if ( _serial->available() > 0 )
	{
		return _serial->readBytes( buffer, 1 ) == 1;
	}

	return false;
}

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

	receiveMAVLinkMessages();

	// If ready to send heartbeat
	if ( currentMillisMAVLink - _previousMAVLinkMilliseconds >= _nextIntervalMAVLinkMilliseconds )
	{

		_previousMAVLinkMilliseconds = currentMillisMAVLink;

		sendMAVLinkHeartbeat();

		_cycleCount += 1;

		// If ready to send data request
		if ( _cycleCount >= _numberOfCyclesToWait )
		{
			// Request streams from Pixhawk
			Log.trace( "Requesting stream data" );
			requestMAVLinkStreams();
			_cycleCount = 0;
		}


	}

}

void SerialMAVLinkReader::requestMAVLinkStreams()
{
	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
	uint16_t messageLength = 0;
	const int maxStreams = 1;
	uint8_t MAVStreams[maxStreams] = { MAV_DATA_STREAM_ALL };
	uint16_t MAVRates[maxStreams] = { 0x02 };
	mavlink_message_t mavlinkMessage;

	/*
	 * Definitions are in common.h: enum MAV_DATA_STREAM
	 *
	 *
	 * Data in PixHawk available in:
	 *  - Battery, amperage and voltage (SYS_STATUS) in MAV_DATA_STREAM_EXTENDED_STATUS
	 *  - Gyro info (IMU_SCALED) in MAV_DATA_STREAM_EXTRA1
	 */
	for ( int i = 0; i < maxStreams; i++ )
	{
		mavlink_msg_request_data_stream_pack( _sysid, _compid, &mavlinkMessage, 1, 0, MAVStreams[i], MAVRates[i], 1 );
		messageLength = mavlink_msg_to_send_buffer( buffer, &mavlinkMessage );
		_serial->write( buffer, messageLength );
	}
}

void SerialMAVLinkReader::sendMAVLinkHeartbeat()
{
	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
	uint16_t messageLength = 0;
	mavlink_message_t mavlinkMessage;

	//Log.trace( "Sending heartbeat message" );

	// Pack the MAVLink heartbeat message
	mavlink_msg_heartbeat_pack( _sysid, _compid, &mavlinkMessage, _type, _autopilotType, _systemMode, _customMode, _systemState );

	// Copy the message to the send buffer
	messageLength = mavlink_msg_to_send_buffer( buffer, &mavlinkMessage );

	// Write buffer containing heartbeat message
	_serial->write( buffer, messageLength );


}

void SerialMAVLinkReader::sendChangeMode( ROVER_MODE roverMode)
{
	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
	uint16_t messageLength = 0;
	mavlink_message_t mavlinkMessage;

	//Log.trace( "Sending heartbeat message" );

	// Pack the MAVLink change mode message
	mavlink_msg_set_mode_pack( _sysid, _compid, &mavlinkMessage, _flight_controller_sysid,  roverMode, 1 );

	// Copy the message to the send buffer
	messageLength = mavlink_msg_to_send_buffer( buffer, &mavlinkMessage );

	// Write buffer containing heartbeat message
	_serial->write( buffer, messageLength );


}