sonar values to the FC using Arduino and inav MSP

[Tsu2022]

Hello,

I want to send sonar values to the FC using Arduino and inav MSP.
I'm considering using Arduino to send sonar values to the FC using inav MSP.

With the following sample code, I can send GPS values, but I can't send the SONAR distance.
I think that if the data is sent correctly, the values should be displayed in the sensor section of the INAV Configurator.
However, they are not being displayed, and the graph shows -1.

Could someone please advise me on how to send sonar values to the FC?

・INAV Configurator 6.1.0
・FC Fiemware 6.1.1 [KAKUTEF4V2]

---

#include <SoftwareSerial.h>
#include <stdlib.h>

#define MSP_SET_RAW_GPS 201 // MSP command code
#define MSP_SET_SONAR 210 // SONAR MSP command code ??

SoftwareSerial mspSerial(10, 11); // RX TX

// Define a structure for GPS data
typedef struct {
uint8_t fixType; // GPS fix type (0: invalid, 3: 3D fix)
uint8_t numSat; // Number of satellites in view
int32_t lat; // Latitude (1 / 10000000 deg)
int32_t lon; // Longitude (1 / 10000000 deg)
int16_t alt; // Altitude (meters)
int16_t groundSpeed; // Ground speed (cm/s)
} msp_set_raw_gps_t;

// Define a structure for SONAR data
typedef struct {
uint32_t sonarDist; // SONAR distance (cm)
} msp_set_sonar_t;

// Add prototype for sendMSP function
void sendMSP(uint8_t cmd, void* data, uint8_t n_bytes);

void setup() {
mspSerial.begin(115200);
Serial.begin(115200);
randomSeed(analogRead(0)); // Initialize random seed
}

void loop() {
// Dummy GPS information
uint8_t fixType = 3; // GPS fix type (0: invalid, 3: 3D fix)
uint8_t numSat = 6; // Number of satellites in view
int32_t lat = 355123; // Latitude (1 / 10000000 deg)
int32_t lon = 1391234; // Longitude (1 / 10000000 deg)
int16_t alt = 120; // Altitude (meters)
int16_t groundSpeed = 500; // Ground speed (cm/s)

// Create and initialize an instance of the GPS data structure
msp_set_raw_gps_t gpsData;
gpsData.fixType = fixType;
gpsData.numSat = numSat;
gpsData.lat = lat;
gpsData.lon = lon;
gpsData.alt = alt;
gpsData.groundSpeed = groundSpeed;

// Send GPS data
sendMSP(MSP_SET_RAW_GPS, &gpsData, sizeof(gpsData));

// SONAR data
uint32_t sonarDist = random(100, 121); // Random value between 100 and 120

// Create and initialize an instance of the SONAR data structure
msp_set_sonar_t sonarData;
sonarData.sonarDist = sonarDist;
// Send SONAR data
sendMSP(MSP_SET_SONAR, &sonarData, sizeof(sonarData));
delay(1000);

}

// Function to send MSP command
void sendMSP(uint8_t cmd, void* data, uint8_t n_bytes) {
uint8_t checksum = 0;
mspSerial.write('$');
mspSerial.write('M');
mspSerial.write('<');
mspSerial.write(n_bytes);
checksum ^= n_bytes;
mspSerial.write(cmd);
checksum ^= cmd;
uint8_t* payloadPtr = (uint8_t*)data;
for (uint8_t i = 0; i < n_bytes; i++) {
uint8_t b = *(payloadPtr++);
checksum ^= b;
mspSerial.write(b);
}
mspSerial.write(checksum);
}

[stronnag]

1. Check the source code for defined message IDs rather than inventing your own.
2. Use MSPv2 rather than MSPv1
3. Check the MSP return CRC and status
4. Use the sensor message IDs and structures defined in src/main/msp/msp_protocol_v2_sensor.h and src/main/msp/msp_protocol_v2_sensor_msg.h. These are designed for your application.

[Tsu2022]

Thank you.

When trying to send sonar values, I cannot find the corresponding value for MSP_SET_RAW_GPS 201.

2.Use MSPv2
I am trying to support it. So far, making the change from $M< to $X< (for v2 support) is not working.

3.,4. I haven't caught up on those yet, but sending sensor values seems quite difficult.

I will keep trying, so please let me know if you have any hints.

[Tsu2022]

I switched to v2.
It doesn't show up on the graph in the INAV Configurator, so there seems to be a mistake somewhere.

#include <SoftwareSerial.h>

// Define software serial pins
SoftwareSerial mySerial(10, 11); // RX, TX

void setup() {
Serial.begin(115200);
mySerial.begin(115200);
}

void loop() {
// Define payload arrays
uint8_t payload1[] = {255, 130, 0, 0, 0};
uint8_t payload2[] = {255, 143, 0, 0, 0};

// Send MSP V2 message for SENSOR_RANGEFINDER
sendMSPMessage(0x1F01, payload1, sizeof(payload1));

// Send MSP V2 message for SENSOR_OPTIC_FLOW
sendMSPMessage(0x1F02, payload2, sizeof(payload2));

delay(1000); // Delay 1 second before sending the next message
}

void sendMSPMessage(uint16_t function, uint8_t payload[], uint16_t payloadSize) {
// Start MSP V2 message with header
mySerial.write('$');
mySerial.write('X');
mySerial.write('<');

// Send function code
mySerial.write((uint8_t)function);
mySerial.write((uint8_t)(function >> 8));

// Send payload size
mySerial.write((uint8_t)payloadSize);
mySerial.write((uint8_t)(payloadSize >> 8));

// Send payload
for (uint16_t i = 0; i < payloadSize; i++) {
mySerial.write(payload[i]);
}

// Calculate and send checksum
uint8_t checksum = 0;
checksum ^= '<';
checksum ^= (uint8_t)function;
checksum ^= (uint8_t)(function >> 8);
checksum ^= (uint8_t)payloadSize;
checksum ^= (uint8_t)(payloadSize >> 8);
for (uint16_t i = 0; i < payloadSize; i++) {
checksum ^= payload[i];
}
mySerial.write(checksum);

// Print the sent message to serial monitor
Serial.print("Sent MSP V2 Message: < Function: 0x");
Serial.print(function, HEX);
Serial.print(" Payload : ");
for (uint16_t i = 0; i < payloadSize; i++) {
Serial.print(payload[i]);
Serial.print(" ");
}
Serial.print(" Payload Size: ");
Serial.print(payloadSize);
Serial.println(" bytes");
}

[stronnag]

The API works if you implement MSPv2 correctly, in particular the checksum (which is documented on the Wiki).

[geofrancis]

this is a useless answer, no link to where to find it or any useful information at all.

[sensei-hacker]

this is a useless answer, no link to where to find it or any useful information at all.

So you're asking where to find the MSP v2 page in the INAV wiki? The wiki would be the menu item right up there called "wiki". The page about MSP v2 is the one titled "MSP v2".

And this reply is what happens when you decide to be a jerk. You might consider a different tone next time.

[geofrancis]

That page is a joke, some basic information on the message structure and nothing else. doesnt even tell you what units to use. Inav has by far the worst documentation out of the flight control systems, most of it is years out of date.