Hardware serial unknow interruption.

Blaise2410

New member
Good afternoon,

I managed to find the problem by myself, by searching in this forum and even via chatGPT without success. As last chance, I try here.
If my post is done in the wrong section of the forum, do not hesitate to move it.


Understanding the problem I have to solve requires a lot of explanations, so I would go from the most general to the most specific.

1) General context:
The program in question concerns an open-source RTK auto-guidance called AGopenGps whose purpose is to direct agricultural tractors with precision in order to reduce costs in terms of inputs and working time.
It consists in particular of an antenna and a GPS receiver board responsible for establishing the position, an IMU responsible for establishing the exact heading and roll of the machine and a guidance part generally composed of an arduino nano responsible for receiving via a sensor and an ADS1115 the current steering angle of the tractor wheels and rotating the steering to the desired angle.
The whole being connected to a Windows tablet containing the AGopenGps program which receives all the information from the components and gives the direction orders.
The designers have undertaken to unify the various programs concerning each element into a single, adaptive one, which automatically detects the gps receiver(s) (2 for dual antenna system’s), the imu and the guidance part if they are all connected to the Teensy 4.1 card and in the opposite case disables unaffected parts of the code if not connected.
In addition, this new program integrates the so-called “Panda” system which aims to synchronize accurately the informations from the GPS receiver and the IMU in order to obtain the greatest possible precision.
You will find this original program in the “A_Teensy_v56” folder located in the .zip files available here (https://github.com/BL08FR/PandaTPU).

2) My personal goal:
Adapt the program to follow the same logic as my previous system (https://www.thingiverse.com/thing:5182231), a positioning system separated from the guidance part for the purpose of versatility. The most compact possible and transferable from one tractor to another as quickly as possible, involving the least possible physical connections and therefore the use of a wireless connection (bluetooth in this case).
The advantage being that the program mentioned above is already designed to fit.

3) Hardware environment:
- Teensy 4.0 (similar to Teensy 4.1 but without Ethernet which I don't intend to use anyway and above all more compact).
- IMU type Adafruit BNO 085, connected to the Teensy 4.0 via the SCL and SDA pins.
- Ardusimple RTK2B type GPS receiver whose TX1 and RX1 pins are connected to the Teensy 4.0 via TX3 and RX3 pins.
- Bluetooth module type HC-05 whose TX and RX pins are connected to the Teensy 4.0 via TX4 and RX4. Previously configured as follows.
VERSION:3.0-20170601
AT+ROLE=0 (mean slave mode)
CLASS:0 (mean visible for other devices if I remenber well)
CMODE:0 (mean with maximum 1 other device if I remenber well)
UART:115200,0,0 (baudrate, stop bit, parity bit)
PIN:"1234"

4) Software environment:
The programming is done with Arduino IDE 1.8.19 and Teensyduino 1.57 (version 1.58 causes a compilation problem which causes the Teensy 4.0 to restart regularly for an unknown reason).

5) My changes:

5.1) My first modification was to adapt the code mentioned above, intended for a Teensy 4.1 so that it could be used with a Teensy 4.0. The modification was quite simple and you will find it in the “A_Teensy_v56_essaiModifpourTeensy4_0_v2” folder in the .zip files available here (https://github.com/BL08FR/PandaTPU), it’s mainly about ethernet part of code. It allows to receive synchronized data from the GPS receiver and the Imu via the serial connection named "SerialAOG" in the code, in clear, the Teensy 4.0 USB, that worked succesfully ! (see picture below)
V2USBok.JPG


5.2) From there, the modification to obtain this same data by bluetooth wireless connections via the HC-05 module seems quite simple, by changing the line “#define SerialAOG Serial” to “#define SerialAOG Serial4”.
But for some reason that I can't identify, it doesn't work, nothing showed up on the bluetooth serial connection.
5.3) So I undertook to check the operation of the HC-05 module with different programs on arduino UNO and on the Teensy 4.0, it seems to work normally at the expected 115200 speed.


6) You will find my attempt to identify the origin of the problem in the folder “A_Teensy_v56_Modif_Teensy40_HC05_v4” appearing in the .zip files available here (https://github.com/BL08FR/PandaTPU). Lines ending with “//test” are intended to attempt to identify the problem.
By monitoring the serial connection of the bluetooth module (COM7 in my case) after uploading the program to the Teensy 4.0 via USB (COM3 in my case), the last message transmitted on the bluetooth serial connection COM7 is "after serialGPS2 begin" around line 223 of the code.
Removing this part of the code (which I don't use since I'm using only one GPS receiver), just moves the interruption of data transmission.

I tried to change the UART setting of HC-05 to add a stop byte, result is Equal, but which configuration is really correct?
I also tried to power supply the system with 5v 2A phone charger in caser the USB of my pc was not enough powerful, result is Equal.


Do you have an idea why it work with Serial(USB) "#define SerialAOG Serial" and not with Serial4 "#define SerialAOG Serial4"?
If you need more informations, do not hesitate to ask.

Here below :
- Schema of the system (at the moment it’s 5V power supplied by USB)
- Main tab of the A_Teensy_v56_Modif_Teensy40_HC05_v4.ino (my last attemps to try to find the problem source).


Thanks a lot for your attention.

Panda TPU Schematic.jpg

Code:
// PANDA Single antenna, IMU.
// Modified by Blaise Lapierre 30/05/2023 to suit Teensy 4.0 (more compact) and HC-05 (bluetooth connection).
// DON'T use Teensyduino 1.58 to upload the sketch, it won't work, 1.57 is ok (future versions?).
// As it's originally made for Teensy 4.1, some code is useless but it work like this (at least in uSB).
//
// connection plan:
// Teensy Serial 3 RX (15) to F9P Position receiver TX1 (Position data)
// Teensy Serial 3 TX (14) to F9P Position receiver RX1 (RTCM data for RTK)
// Teensy Serial 4 RX (16) to HC-05 TX
// Teensy Serial 4 TX (17) to HC-05 RX (RTCM data for Moving Base)
//
// Configuration of receiver
// Position F9P
// CFG-RATE-MEAS - 100 ms -> 10 Hz
// CFG-UART1-BAUDRATE 115200
// Serial 1 In - RTCM (Correction Data from AGO)
// Serial 1 Out - NMEAs GGA
// CFG-UART2-BAUDRATE 460800
// Serial 2 Out - RTCM 1074,1084,1094,1230,4072.0 (Correction data for Heading F9P, Moving Base)  
// 1124 is not needed (China’s BeiDou system) - Save F9P brain power 
//
// Heading F9P
// CFG-RATE-MEAS - 100 ms -> 10 Hz
// CFG-UART1-BAUDRATE 115200
// Serial 1 Out - UBX-NAV-RELPOSNED
// CFG-UART2-BAUDRATE 460800
// Serial 2 In RTCM

/************************* User Settings *************************/
// Serial Ports
#define SerialAOG Serial4
#define SerialRTK Serial5

HardwareSerial* SerialGPS = &Serial3;   //Main postion receiver (GGA) (Serial2 must be used here with T4.0 / Basic Panda boards - Should auto swap)
HardwareSerial* SerialGPS2 = &Serial2;  //Dual heading receiver 
HardwareSerial* SerialGPSTmp = NULL;
//HardwareSerial* SerialAOG = &Serial;

const int32_t baudAOG = 115200;
const int32_t baudGPS = 115200;
const int32_t baudRTK = 9600;

#define ImuWire Wire        //SCL=19:A5 SDA=18:A4

// Swap BNO08x roll & pitch?
const bool swapRollPitch = false;
//const bool swapRollPitch = true;

// send GPS data via  0 = USB, 1 = Ethernet 
int send_Data_Via = 0;
int GGAReceivedLED = 13;

/*****************************************************************/

// Ethernet Options (Teensy 4.1 Only)
#ifdef ARDUINO_TEENSY41
#include <NativeEthernet.h>
#include <NativeEthernetUdp.h>

// IP & MAC address of this module of this module
byte Eth_myip[4] = { 192, 168, 0, 120 };
byte mac[] = {0x00, 0x00, 0x56, 0x00, 0x00, 0x78}; // original

byte Eth_ipDest_ending = 255;           // ending of IP address to send UDP data to
unsigned int portMy = 5120;             // port of this module
unsigned int AOGNtripPort = 2233;       // port NTRIP data from AOG comes in
unsigned int AOGAutoSteerPort = 8888;   // port Autosteer data from AOG comes in
unsigned int portDestination = 9999;    // Port of AOG that listens
char Eth_NTRIP_packetBuffer[512];       // buffer for receiving ntrip data

// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Eth_udpPAOGI;     //Out port 5544
EthernetUDP Eth_udpNtrip;     //In port 2233
EthernetUDP Eth_udpAutoSteer; //In & Out Port 8888

IPAddress Eth_ipDestination;
#endif // ARDUINO_TEENSY41

byte CK_A = 0;
byte CK_B = 0;

constexpr int serial_buffer_size = 512;
uint8_t GPSrxbuffer[serial_buffer_size];    //Extra serial rx buffer
uint8_t GPStxbuffer[serial_buffer_size];    //Extra serial tx buffer
uint8_t GPS2rxbuffer[serial_buffer_size];   //Extra serial rx buffer
uint8_t GPS2txbuffer[serial_buffer_size];   //Extra serial tx buffer
uint8_t RTKrxbuffer[serial_buffer_size]; //Extra serial rx buffer

uint8_t AOGrxbuffer[serial_buffer_size]; //test
uint8_t AOGtxbuffer[serial_buffer_size]; //Extra serial rx buffer


bool Autosteer_running = false; //Auto set off in autosteer setup
bool Ethernet_running = false; //Auto set on in ethernet setup

//Speed pulse output
unsigned long prev_PWM_Millis = 0;
byte velocityPWM_Pin = 36;      // Velocity (MPH speed) PWM pin

#include "zNMEAParser.h"
#include <Wire.h>
#include "BNO08x_AOG.h"

// Remote Settings ------------------------------------------------------

// Buffer to read chars from Serial, to check if "!AOG" is found
uint8_t aogSerialCmd[4] = { '!', 'A', 'O', 'G'};
uint8_t aogSerialCmdBuffer[6];
uint8_t aogSerialCmdCounter = 0;

// Booleans to indictate to passthrough GPS or GPS2
bool passhtroughGPS = false;
bool passhtroughGPS2 = false;

//Dual --------------------------------------------------------------------
bool useDual = false;
bool isDualNewGGA = false;
bool isDualNewRelPos = false;
int relposnedByteCount = 0;
double headingCorrection = 900;  //90deg heading correction (90deg*10)

float baseline;
double baseline2;
float rollDual;
float rollDualRaw;
double relPosD;
double relPosDH;
double heading = 0;

byte ackPacket[72] = {0xB5, 0x62, 0x01, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

// GPS -----------------------------------------------------------------

/* A parser is declared with 3 handlers at most */
NMEAParser<2> parser;

bool isNewGGA = false;
bool blink = false;

uint32_t PortSwapTime = 0;

bool GGA_Available = false;     //Do we have GGA on correct port?

// IMU
char imuHeading[6];
char imuRoll[6];
char imuPitch[6];
char imuYawRate[6];

// IMU --------------------------------------------------------------------

// booleans to see if we are using CMPS or BNO08x
bool useCMPS = false;
bool useBNO = true;

//CMPS always x60
#define CMPS14_ADDRESS 0x60

// BNO08x address variables to check where it is
const uint8_t bnoAddresses[] = { 0x4A, 0x4B };
const int16_t bnoAdresses = sizeof(bnoAddresses) / sizeof(bnoAddresses[0]);
uint8_t bnoAddress;
BNO080 BNO;

//BNO timing settings

//Time to wait after fix to create report
const uint32_t BNO_SENSOR_REPORT_TIME = 90; 

//Time after last GPS to read GameVector Packet (must be few more then Report Time) (milliseconds)
const uint16_t BNO_READ_DELAY_TIME = 94; 

//CMPS14, how long should we wait with GPS before reading data from IMU then takeoff with Panda
const uint16_t CMPS_READ_DELAY_TIME = 95;  //Best results seem to be around 5ms

float yaw;
float roll;
float pitch;

uint32_t gpsCurrentTime = BNO_READ_DELAY_TIME;
uint32_t ggaLastUpdatedTime = CMPS_READ_DELAY_TIME;


// Setup procedure ------------------------
void setup()
{
	pinMode(GGAReceivedLED, OUTPUT);

	// the dash means wildcard
	parser.setErrorHandler(errorHandler);
	parser.addHandler("G-GGA", GGA_Handler);

	// Disabled VTG for now because speed is calculated by AOG
	//parser.addHandler("G-VTG", VTG_Handler);

	delay(10000);// test original 10
	Serial.begin(baudAOG);
	delay(10);
	Serial.println("Start setup");

  delay(10);
  SerialAOG.begin(baudAOG);
  //SerialAOG.addMemoryForRead(AOGrxbuffer, serial_buffer_size);// useful?
  //SerialAOG.addMemoryForRead(AOGtxbuffer, serial_buffer_size);
  delay(10);
  SerialAOG.println("SerialAOG, Teensy serial4 to HC-05 to COM7 started!");//serial4 work till here

	SerialGPS->begin(baudGPS);
	SerialGPS->addMemoryForRead(GPSrxbuffer, serial_buffer_size);
	SerialGPS->addMemoryForWrite(GPStxbuffer, serial_buffer_size);

  SerialAOG.println("after serialGPS initialised");//test

	delay(10);
  SerialAOG.println("after delayRTK");//test
	SerialRTK.begin(baudRTK);
 SerialAOG.println("after serialRTK begin");//test
	SerialRTK.addMemoryForRead(RTKrxbuffer, serial_buffer_size);

  SerialAOG.println("after serial RTK memory");//test

	delay(10);
  SerialAOG.println("after delayGPS2");//test
  delay(10);
	SerialGPS2->begin(baudGPS);
  SerialAOG.println("after serialGPS2 begin");//test
	SerialGPS2->addMemoryForRead(GPS2rxbuffer, serial_buffer_size);
	SerialGPS2->addMemoryForWrite(GPS2txbuffer, serial_buffer_size);
  SerialAOG.println("ap. GPS2 mem");//test

SerialAOG.println("after serialGPS2 initialised");//test
	Serial.println("SerialAOG, SerialRTK, SerialGPS and SerialGPS2 initialized");

	Serial.println("\r\nStarting Ethernet...");
	EthernetStart();

	Serial.println("\r\nStarting AutoSteer...");
	autosteerSetup();

	Serial.println("\r\nStarting IMU...");
	//test if CMPS working
	uint8_t error;

	ImuWire.begin();

	//Serial.println("Checking for CMPS14");
	ImuWire.beginTransmission(CMPS14_ADDRESS);
	error = ImuWire.endTransmission();

	if (error == 0)
	{
		//Serial.println("Error = 0");
		Serial.print("CMPS14 I2C Address: 0x");
		Serial.println(CMPS14_ADDRESS, HEX);
		Serial.println("CMPS14 Ok.");
		Serial.println("-------------------------------------");
		useCMPS = true;
	}
	else
	{
		//Serial.println("Error = 4");
		Serial.println("CMPS not Connected or Found");
		Serial.println("**************************************");
	}

	if (!useCMPS)
	{
		for (int16_t i = 0; i < bnoAdresses; i++)
		{
			bnoAddress = bnoAddresses[i];

			//Serial.print("\r\nChecking for BNO08X on ");
			//Serial.println(bno08xAddress, HEX);
			ImuWire.beginTransmission(bnoAddress);
			error = ImuWire.endTransmission();

			if (error == 0)
			{
				// Initialize BNO080 lib
				if (BNO.begin(bnoAddress, ImuWire))
				{
					ImuWire.setClock(400000L); //Increase I2C data rate to 400kHz

					//Set the report time (sent in microseconds)
					BNO.sensorRate = BNO_SENSOR_REPORT_TIME * 1000;

					//hang a bit
					delay(200);

					// Break out of loop
					useBNO = true;
					break;

				}
				else
				{
					Serial.println("BNO080 not detected at given I2C address.");
				}
			}
			else
			{
				//Serial.println("Error = 4");
				Serial.print("0x");
				Serial.print(bnoAddress, HEX);
				Serial.println(" BNO08X not Connected or Found");
			}
		}
	}

	//debug enable for packet print
	//BNO.enableDebugging(Serial);

	delay(100);

	if (useCMPS)
	{
		Serial.println("\r\n ** Using CMPS IMU ");
	}
	if (useBNO)
	{
		Serial.println("\r\n ** Using BNO08x IMU ");
		Serial.print("I2C Adress -> 0x");
		Serial.print(bnoAddress, HEX);
		Serial.println(" BNO08X Ok.");
	}
	Serial.println();

 delay(15000);//test
	Serial.println("End setup, waiting for GPS...\r\n");
  SerialAOG.println("End setup, waiting for GPS...\r\n");// test

	delay(2000);
  
}

void loop()
{
  Serial.println("loop com3");//test
  Serial4.write("loop com7");//test
	//Serial Group
	{
		if (GGA_Available == false && !passhtroughGPS && !passhtroughGPS2)
		{
			if (systick_millis_count - PortSwapTime >= 10000)
			{
				Serial.println("Swapping GPS ports...\r\n");
        SerialAOG.println("S4 Swapping GPS ports...\r\n");// test
				SerialGPSTmp = SerialGPS;
				SerialGPS = SerialGPS2;
				SerialGPS2 = SerialGPSTmp;
				PortSwapTime = systick_millis_count;
			}
		}

		// Pass NTRIP etc to GPS
		if (SerialAOG.available())
		{
      SerialAOG.println("entering if SerialAOG.available");// test
      Serial.println("entering if SerialAOG.available");// test
			uint8_t incoming_char = SerialAOG.read();

			// Check incoming char against the aogSerialCmd array
			// The configuration utility will send !AOGR1, !AOGR2 or !AOGED (close/end)
			if (aogSerialCmdCounter < 4 && aogSerialCmd[aogSerialCmdCounter] == incoming_char)
			{
				aogSerialCmdBuffer[aogSerialCmdCounter] = incoming_char;
				aogSerialCmdCounter++;
			}
			// Whole command prefix is in, handle it
			else if (aogSerialCmdCounter == 4)
			{
				aogSerialCmdBuffer[aogSerialCmdCounter] = incoming_char;
				aogSerialCmdBuffer[aogSerialCmdCounter + 1] = SerialAOG.read();

				if (aogSerialCmdBuffer[aogSerialCmdCounter] == 'R')
				{
					if (aogSerialCmdBuffer[aogSerialCmdCounter + 1] == '1')
					{
						passhtroughGPS = true;
						passhtroughGPS2 = false;
					}
					else if (aogSerialCmdBuffer[aogSerialCmdCounter + 1] == '2')
					{
						passhtroughGPS = false;
						passhtroughGPS2 = true;
					}

					// Rest SerialGPS and SerialGPS2
					SerialGPS = &Serial3;
					SerialGPS2 = &Serial2;
				}
				// END command. maybe think of a different abbreviation
				else if (aogSerialCmdBuffer[aogSerialCmdCounter] == 'E' && aogSerialCmdBuffer[aogSerialCmdCounter + 1] == 'D')
				{
					passhtroughGPS = false;
					passhtroughGPS2 = false;
				}
			}
			else
			{
				aogSerialCmdCounter = 0;
			}

			if (passhtroughGPS)
			{
				SerialGPS->write(incoming_char);
			}
			else if (passhtroughGPS2)
			{
				SerialGPS2->write(incoming_char);
			}
			else
			{
				SerialGPS->write(incoming_char);
			}
		}

		// Read incoming nmea from GPS
		if (SerialGPS->available())
		{
			if (passhtroughGPS)
			{
				SerialAOG.write(SerialGPS->read());
       SerialAOG.println("passhtroughGPS if");// test
			}
			else
			{
				parser << SerialGPS->read();
			}
		}

		udpNtrip();

		// Check for RTK Radio
		if (SerialRTK.available())
		{
			SerialGPS->write(SerialRTK.read());
		}

		// If both dual messages are ready, send to AgOpen
		if (isDualNewGGA == true && isDualNewRelPos == true)
		{
			BuildNmea();
			isDualNewGGA = false;
			isDualNewRelPos = false;
		}

		// If anything comes in SerialGPS2 RelPos data
		if (SerialGPS2->available())
		{
			uint8_t incoming_char = SerialGPS2->read();  //Read RELPOSNED from F9P

			if (passhtroughGPS2)
			{
				SerialAOG.write(incoming_char);
       SerialAOG.println("passhtroughGPS2 if");// test
			}
			else
			{
				// Just increase the byte counter for the first 3 bytes
				if (relposnedByteCount < 4 && incoming_char == ackPacket[relposnedByteCount])
				{
					relposnedByteCount++;
				}
				else if (relposnedByteCount > 3)
				{
					// Real data, put the received bytes in the buffer
					ackPacket[relposnedByteCount] = incoming_char;
					relposnedByteCount++;
				}
				else
				{
					// Reset the counter, becaues the start sequence was broken
					relposnedByteCount = 0;
				}
			}
		}

		// Check the message when the buffer is full
		if (relposnedByteCount > 71)
		{
			if (calcChecksum())
			{
				//if(deBug) Serial.println("RelPos Message Recived");
				useDual = true;
				relPosDecode();
			}
			/*  else {
			  if(deBug) Serial.println("ACK Checksum Failure: ");
			  }
			*/
			relposnedByteCount = 0;
		}
	}

	//IMU timming section
	gpsCurrentTime = systick_millis_count;

	if (useBNO)
	{
		if (isNewGGA && gpsCurrentTime - ggaLastUpdatedTime >= BNO_READ_DELAY_TIME)
		{
			//fill the buffer with sampled data to be sent when GGA arrives
			imuReader();

			// Wait for new fix
			isNewGGA = false;
		}
	}

	if (useCMPS)
	{
		if (isNewGGA && gpsCurrentTime - ggaLastUpdatedTime >= CMPS_READ_DELAY_TIME)
		{
			//Send Panda
			BuildNmea();

			//reset the timer
			isNewGGA = false;
		}
	}

	if (Autosteer_running) autosteerLoop();


	//if (gpsCurrentTime - lastGyroTime >= GYRO_LOOP_TIME)
	//{
	//  // Get data from BNO08x (Gyro, Heading, Roll, Pitch)
	//  // Get data from CMPS14 (Gyro Only)
	//  GyroHandler(gpsCurrentTime - lastGyroTime);
	//}


}//End Loop

//**************************************************************************

bool calcChecksum()
{
  CK_A = 0;
  CK_B = 0;

  for (int i = 2; i < 70; i++)
  {
    CK_A = CK_A + ackPacket[i];
    CK_B = CK_B + CK_A;
  }

  return (CK_A == ackPacket[70] && CK_B == ackPacket[71]);
}
 
OMG.

After imagining another way to find the faultive code part (if it was code!), I found it.
I totally ignore why and honestly don't care because this part of code is useless for me, but the autosteerSetup() function in autosteer tab interfere with Serial4.

Sorry for disturbing you uselessly, this post should be detele (I didn't found yet how to do it).
 
Back
Top