Hello,
I'm trying to change from Arduino Uno R4 Wifi to an Arduino Mega 2560, as i need the extra serial ports for a sonar which is working only on hardware serial, and the same program that runs on Uno is not working on Mega, although i made the same connections on Serial, meaning RX0 and TX0.
The program requests data from Pixhawk flight controller, thru Serial, by sending a heartbeat message and receiving the data requested, in this particular program i am requesting unix time, latitude, longitude, elevation, roll, pitch, yaw, which are being received correctly as i'm connected to the Pixhawk with Mission Planner and reading the values (same in the planner as in what i'm receiving as in the uploaded screenshot, mind you I was inside and the unix time and position are not being correct).
This screenshot is from the Mega
Below you can see the code:
/* MAVLInk_DroneLights
by Juan Pedro López
*/
// In case we need a second serial port for debugging
#define SOFT_SERIAL_DEBUGGING // Comment this line if no serial debugging is needed
#ifdef SOFT_SERIAL_DEBUGGING
// Library to use serial debugging with a second board
#include <SoftwareSerial.h>
#include <SPI.h>
#include <SD.h>
#include "SoftwareSerial.h"
#include "ping1d.h"
#include <Int64String.h>
#define RxPin0 1
#define TxPin0 0
int request = 0;
int receive = 0;
const int chipSelect = 53;
float distantaMetri;
SoftwareSerial pxSerial = SoftwareSerial(RxPin0, TxPin0); // RX, TX || UNO - PX4
#endif
#include "mavlink.h"
//#include "common/mavlink_msg_request_data_stream.h"
static const uint8_t ledPin = 13;
// Mavlink variables
unsigned long previousMillisMAVLink = 0; // will store last time MAVLink was transmitted and listened
unsigned long next_interval_MAVLink = 100; // next interval to count
const int num_hbs = 10; // # of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int num_hbs_pasados = num_hbs;
float mesajRoll;
float mesajPitch;
float mesajYaw;
int32_t gpsLat;
int32_t gpsLong;
int32_t gpsElev;
uint8_t gpsSolution;
uint64_t timeUNIX;
String dataString = "";
String timeUNIXstring = "";
void setup() {
pinMode(RxPin0, INPUT);
pinMode(TxPin0, OUTPUT);
// MAVLink interface start
// Serial.begin(57600);
// [DEB] Soft serial port start
Serial.begin(115200);
pxSerial.begin(115200);
Serial.println("MAVLink starting.");
Serial.print("Initializing SD card...");
// see if the card is present and can be initialized:
/*if (!SD.begin(chipSelect)) {
Serial.println("Card failed, or not present");
// don't do anything more:
while (1)
;
}
*/
Serial.println("card initialized.");
//inceput scrie capat fisier~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
File dataFile = SD.open("sonar.txt", FILE_WRITE);
dataFile.println("unix_time,lat,long,elev,roll,pitch,yaw,distanta");
dataFile.close();
//sfarsit scrie capat fisier~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// sfarsit setari card SD ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// PANA AICI E OK MOMENTAN
}
void loop() {
String dataString = "";
// MAVLink
/* The default UART header for your MCU */
int sysid = 1; ///< ID 20 for this airplane. 1 PX, 255 ground station
int compid = 158; ///< The component sending the message
int type = MAV_TYPE_QUADROTOR; ///< This system is an airplane / fixed wing
// Define the system type, in this case an airplane -> on-board controller
// uint8_t system_type = MAV_TYPE_FIXED_WING;
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;
uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
uint32_t custom_mode = 0; ///< Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
//mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_heartbeat_pack(1, 0, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message with the standard UART send function
// uart0_send might be named differently depending on
// the individual microcontroller / library in use.
unsigned long currentMillisMAVLink = millis();
if (currentMillisMAVLink - previousMillisMAVLink >= next_interval_MAVLink) {
previousMillisMAVLink = currentMillisMAVLink;
#ifdef SOFT_SERIAL_DEBUGGING
pxSerial.write(buf, len);
// Serial.println("Ardu HB");
#else
Serial.write(buf, len);
#endif
//Mav_Request_Data();
num_hbs_pasados++;
}
// Check reception buffer
comm_receive();
//Serial.println(timeUNIX);
//Serial.println(gpsLat);
//Serial.println(gpsLong);
//Serial.println(gpsElev);
//Serial.println(gpsSolution);
//Serial.println(mesajRoll,10);
//Serial.println(mesajPitch,10);
//Serial.println(mesajYaw,10);
timeUNIXstring = int64String(timeUNIX);
dataString = timeUNIXstring + "," + String(gpsLat) + "," + String(gpsLong) + "," + String(gpsElev) + "," + String(mesajRoll,10) + "," + String(mesajPitch,10) + "," + String(mesajYaw,10) + ","; // + String(distantaMetri);
Serial.println(dataString);
if (gpsSolution > 3) {
// INCEPUT PARTE DE SCRIS PE CARD SD
// open the file. note that only one file can be open at a time,
// so you have to close this one before opening another.
File dataFile = SD.open("sonar.txt", FILE_WRITE);
// if the file is available, write to it:
if (dataFile) {
dataFile.println(dataString);
dataFile.close();
// print to the serial port too:
Serial.println(dataString);
}
// if the file isn't open, pop up an error:
else {
Serial.println("Eroare fisier sonar.txt");
}
// SFARSIT PARTE DE SCRIS PE CARD SD
// Toggle the LED to show that the program is running
digitalWrite(ledPin, !digitalRead(ledPin));
}
delay(100);
} // SFARSIT LOOP
void Mav_Request_Data()
{
request = request + 1;
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// STREAMS that can be requested
/*
Definitions are in common.h: enum MAV_DATA_STREAM
MAV_DATA_STREAM_ALL=0, // Enable all data streams
MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.
MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot
MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot
MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot
MAV_DATA_STREAM_ENUM_END=13,
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
*/
// To be setup according to the needed information to be requested from the Pixhawk
const int maxStreams = 3;
const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_EXTENDED_STATUS, MAV_DATA_STREAM_POSITION, MAV_DATA_STREAM_EXTRA1};
const uint16_t MAVRates[maxStreams] = {0x10, 0x10, 0x10};
for (int i = 0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
#ifdef SOFT_SERIAL_DEBUGGING
pxSerial.write(buf, len);
#else
Serial.write(buf, len);
#endif
}
}
void comm_receive() {
receive = receive + 1;
mavlink_message_t msg;
mavlink_status_t status;
#ifdef SOFT_SERIAL_DEBUGGING
while(pxSerial.available()>0) {
uint8_t c = pxSerial.read();
#else
while(Serial.available()>0) {
uint8_t c = Serial.read();
#endif
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
// Handle message
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat
{
// E.g. read GCS heartbeat and go into
// comm lost mode if timer times out
}
break;
case MAVLINK_MSG_ID_ATTITUDE: // #30
{
/* Message decoding: PRIMITIVE
mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
*/
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&msg, &attitude);
mesajRoll = attitude.roll;
mesajPitch = attitude.pitch;
mesajYaw = attitude.yaw;
}
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: // #30
{
/* Message decoding: PRIMITIVE
mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
*/
mavlink_gps_raw_int_t gps_raw_int;
mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
gpsLat = gps_raw_int.lat; // seteaza variabila globala cu latitudine GPS Pixhawk
gpsLong = gps_raw_int.lon; // seteaza variabila globala cu longitudine GPS Pixhawk
gpsElev = gps_raw_int.alt; // seteaza variabila globala cu altitudine GPS Pixhawk
gpsSolution = gps_raw_int.satellites_visible; // seteaza variabila globala cu numar sateliti
}
break;
case MAVLINK_MSG_ID_SYSTEM_TIME: // #30
{
mavlink_system_time_t system_time;
mavlink_msg_system_time_decode(&msg, &system_time);
timeUNIX = system_time.time_unix_usec; // seteaza variabila globala cu timp unix
}
break;
}
}
}
}
The code above is stitched together from MAVLInk_DroneLights by Juan Pedro López and the SDcard library (which is commented as i keep switching between boards to see if something changes)
I tried to change from Serial0 to Serial1, Serial2, together with changing the cables on their respective pins and changing where there is Serial.write(), Serial.available() and Serial.read() to Serial1.write(), Serial1.available() and Serial1.read() ....
I don't know what exactly i'm missing.
Please be kind to me as i only started with Arduino, and implicitly C language only a month ago. I have some programming experience with Vlisp.
Thank you.
