MSP2 implementation in ESP8266 chip

[MahmoudHamandiNYUAD]

We are attempting to replicate your work on MSP based communication between PC and an inav/betaflight FC, while replacing the PC with an ESP8266 chip.
The ESP8266 chip is receiving optitrack data, and transforming it into GPS and Compass data, which is then sent to the FC through a UART port.
Currently we are testing with an T-motor F7 pro FC, running INAV (latest version).
While we were able to communicate the GPS as MSP v1 message, the communication as GPS MSP2 or Compass MSP2 is failing.
We would highly appreciate any intuition about what we are doing wrong.
The following is our Compass code:

#define MSP2_SENSOR_COMPASS 0x1F04

typedef struct MspSensorCompassDataMessage {
uint8_t instance;
uint32_t timeMs;
int16_t magX;
int16_t magY;
int16_t magZ;
}t_compass;
t_compass currentcompass = {
.instance = 1,
.timeMs = 0,
.magX = 0,
.magY = 0,
.magZ = 0
};

void sendMSPSetCompass(t_compass currentcompass){
uint8_t flag = 0;
uint8_t compasschecksum = 0;
uint16_t function = MSP2_SENSOR_COMPASS;
uint16_t payload_size = 11;
espSerial.write('$');
espSerial.write('X');
espSerial.write('<');
espSerial.write(flag);
espSerial.write(function & 0xFF);
espSerial.write((function & 0xFF00) >> 8);
espSerial.write(payload_size & 0xFF);
espSerial.write((payload_size & 0xFF00) >> 8);
espSerial.write((uint8_t*)&currentcompass.instance, sizeof(currentcompass.instance));
espSerial.write((uint8_t*)&currentcompass.timeMs, sizeof(currentcompass.timeMs));
espSerial.write((uint8_t*)&currentcompass.magX, sizeof(currentcompass.magX));
espSerial.write((uint8_t*)&currentcompass.magY, sizeof(currentcompass.magY));
espSerial.write((uint8_t*)&currentcompass.magZ, sizeof(currentcompass.magZ));

compasschecksum ^= flag;
uint8_t* ptrr = (uint8_t*)&function;
for (size_t i = 0; i < sizeof(function); i++) {
compasschecksum ^= ptrr[i];
}
ptrr = (uint8_t*)&payload_size;
for (size_t i = 0; i < sizeof(payload_size); i++) {
compasschecksum ^= ptrr[i];
}
ptrr = (uint8_t*)&currentcompass.instance;
for (size_t i = 0; i < sizeof(currentcompass.instance); i++) {
compasschecksum ^= ptrr[i];
}
ptrr = (uint8_t*)&currentcompass.timeMs;
for (size_t i = 0; i < sizeof(currentcompass.timeMs); i++) {
compasschecksum ^= ptrr[i];
}
ptrr = (uint8_t*)&currentcompass.magX;
for (size_t i = 0; i < sizeof(currentcompass.magX); i++) {
compasschecksum ^= ptrr[i];
}
ptrr = (uint8_t*)&currentcompass.magY;
for (size_t i = 0; i < sizeof(currentcompass.magY); i++) {
compasschecksum ^= ptrr[i];
}
ptrr = (uint8_t*)&currentcompass.magZ;
for (size_t i = 0; i < sizeof(currentcompass.magZ); i++) {
compasschecksum ^= ptrr[i];
}
espSerial.write(compasschecksum);
}

The data in currentcompass is populated with dummy date as follows:

// Get the current time
uint32_t currentTime = millis();
// Calculate the time difference in milliseconds
mydata.currentcompasss.timeMs = currentTime - initialTime;

mydata.currentcompasss.instance = 1;
mydata.currentcompasss.timeMs = 0.1;
mydata.currentcompasss.magX = 0.2; //TO BE MODIFIED
mydata.currentcompasss.magY = 0.3; //TO BE MODIFIED
mydata.currentcompasss.magZ = 0.7; //TO BE MODIFIED

[yannbouteiller]

Hi, I am not sure about the MSP protocol part as @ricardodeazambuja handles this directly in the YAMSPy library (yb/stable branch), but the way we fake these sensors in cognifly-python are these lines of code

[ricardodeazambuja]

My suggestion is to communicate with YAMSPy first (ESP to PC) as a way to easily debug things since YAMSPy will be able to receive and decode the messages.