d3ngit / djihdfpv_mavlink_to_msp_V2

DJI_mavlink_to_MSP_V2
57 stars 23 forks source link

No home data nor gps speed #4

Closed naitse closed 4 years ago

naitse commented 4 years ago

Home heading and distance does not work as well as gps speed, those remain in 0 all the time

I did some debugging and noticed this message is never pressent MAVLINK_MSG_ID_GLOBAL_POSITION_INT when receiving mavlink telemetry from INAV.

My setup: INAV 2.3.0 Matek405-wing GPS hglrc ublox M8

naitse commented 4 years ago

I used the data at MAVLINK_MSG_ID_GPS_RAW_INT to have a home set, but I dont know if it is the correct approach

        gps_lat = gps_raw_int.lat;
        gps_lon = gps_raw_int.lon;
        gps_alt = gps_raw_int.alt;
        altitude_mav = (uint16_t)(gps_raw_int.alt / 1000);

still missing the heading information though and the "altitude_mav" used relative_alt which the raw message dont seem to have

d3ngit commented 4 years ago

I don't exactly understand what you're trying to say. Does GPS lock or not? I'm not using MAVLINK_MSG_ID_GLOBAL_POSITION_INT because reqiures to request it from the FC and this adds a lot of additional code.

EDIT: I misread here. I thought you were talking about MAVLINK_MSG_ID_GLOBAL_ORIGIN

naitse commented 4 years ago

I have GPS lock in INAV

the issue is: in the OSD at the pitch and roll placeholders I dont have any reading for home distance, heading and speed, also altitude updates veeery slow like almost never.

now my futile tries to understand what is wrong: what I observed (and I can be super wrong about it since I dont like very much arduino debugging) I saw in the code at here https://github.com/d3ngit/djihdfpv_mavlink_to_msp_V2/blob/master/djihdfpv_mavlink_to_msp_V2.ino#L294

that you use MAVLINK_MSG_ID_GLOBAL_POSITION_INT message to stablish gps_lat lon and alt.

I connected the arduino to see the serial output and saw that the switch never goes into MAVLINK_MSG_ID_GLOBAL_POSITION_INT case

so I used the data from the other message but it is not correct

d3ngit commented 4 years ago

inav is sending it to MAVLINK_MSG_ID_GLOBAL_POSITION_INT https://github.com/iNavFlight/inav/blob/master/src/main/telemetry/mavlink.c#L329

you can try to move gps_lat = global_position_int.lat; gps_lon = global_position_int.lon; gps_alt = global_position_int.alt;

to MAVLINK_MSG_ID_GPS_RAW_INT and test if it makes any difference, but I doubt it.

and rename of course gps_lat = gps_raw_int.lat; gps_lon = gps_raw_int.lon; gps_alt = gps_raw_int.alt;

d3ngit commented 4 years ago

You also reported that the recording is not starting for you when arming. It's possible this is also related. Do you have another microcontroller with only hardware serial ports to test? Did you copy the inlcuded Softwareserial Arduino library in the Arudino libraries folder? Also you could try to change the softwareserial port, now set to 8,9. Maybe try 10,11 or something else.

You could also try to invert the mavlink and msp serial ports, and switching cables. Just for testing the softserial port:

include

HardwareSerial &mspSerial = Serial; SoftwareSerial mavlinkSerial(8, 9); // RX, TX

to

include

HardwareSerial &mavlinkSerial= Serial; SoftwareSerial mspSerial(8, 9); // RX, TX

d3ngit commented 4 years ago

I was doing some tests today and it looks like it's really slower reacting in Inav. The same functionality is almost instant in ArduPlane. Seems to get slower and slower Mavlink output the more flight modes you add. If I only put Arm mode and everything else disabled seems to be ok. I'm not sure what is going on there, but it's some bug in Inav mavlink code, not in my code.

naitse commented 4 years ago

I was able to get all the messages by running the mavlink serial at 19200 bauds, dont know if it is just with my arduino or it is as you mention an INAV issue. I´ll close this issue since the problem seems to be configuration related to the serial speed, many thanks for your time!