ardupilot/libraries/APO/examples/MavlinkTest/MavlinkTest.pde

50 lines
1.2 KiB
Plaintext

/*
AnalogReadSerial
Reads an analog input on pin 0, prints the result to the serial monitor
This example code is in the public domain.
*/
#include <GCS_MAVLink.h>
int packetDrops = 0;
void handleMessage(mavlink_message_t * msg) {
Serial.print(", received mavlink message: ");
Serial.print(msg->msgid, DEC);
}
void setup() {
Serial.begin(57600);
Serial3.begin(57600);
mavlink_comm_0_port = &Serial3;
packetDrops = 0;
}
void loop() {
mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
Serial.print("heartbeat sent");
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
Serial.print(", bytes available: ");
Serial.print(comm_get_available(MAVLINK_COMM_0));
while (comm_get_available( MAVLINK_COMM_0)) {
uint8_t c = comm_receive_ch(MAVLINK_COMM_0);
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
handleMessage(&msg);
}
// Update packet drops counter
packetDrops += status.packet_rx_drop_count;
Serial.print(", dropped packets: ");
Serial.println(packetDrops);
delay(1000);
}