-
Notifications
You must be signed in to change notification settings - Fork 0
/
02_request_datastream.ino
20 lines (17 loc) · 1.08 KB
/
02_request_datastream.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void request_datastream() {
//Request Data from Pixhawk
uint8_t _system_id = 255; // id of computer which is sending the command (ground control software has id of 255)
uint8_t _component_id = 2; // seems like it can be any # except the number of what Pixhawk sys_id is
uint8_t _target_system = 1; // Id # of Pixhawk (should be 1)
uint8_t _target_component = 0; // Target component, 0 = all (seems to work with 0 or 1)
uint8_t _req_stream_id = MAV_DATA_STREAM_ALL;
uint16_t _req_message_rate = 0x01; //number of times per second to request the data in hex
uint8_t _start_stop = 1; // 1 = start, 0 = stop
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_request_data_stream_pack(_system_id, _component_id, &msg, _target_system, _target_component, _req_stream_id, _req_message_rate, _start_stop);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes)
Serial1.write(buf, len); //Write data to serial port
}