Integrate optical navigation system 🥲

ArduPlane Plane 4.5
1

Hello, I am a young programmer with a passion for aviation! And I ask you to help me with integration of optical navigation system! I didn’t know where to write about the problem, so I decided to write here.

I want to integrate an Optical Navigation System that sends longitude and latitude with the following command in a Pytohn script over MavLink:

while True:
    mavserial.mav.command_int_send(
        target_system=1,
        target_component=1,
        frame=MAV_FRAME_GLOBAL,
        command=MAV_CMD_EXTERNAL_POSITION_ESTIMATE,
        current=1,
        autocontinue=1,
        param1=time.time(),
        param2=0.0,
        param3=1.0,
        param4=0.0,
        x=int(56.0 * 1e7),
        y=int(34.0 * 1e7),
        z=float(“nan”),
    )

I went through the standard mavlink commands and thought of accepting SON’s mavlink messages in the standard way and changing the position accordingly, but so far I’m stumped: the values don’t change. Although debug messages indicate that all processing steps have been passed (although I’ve set some of them by force)

Link to the branch: GitHub - VermennikovMV/ardupilot at EK3_ALL_ENABLE_+_Debug

Now I will describe my ideas and what I did:

  1. First of all (as I found out) the message starts processing with the command handle_command_int_external_position_estimate from the file GCS_Common.cpp. And here I noticed something wrong at once, because the AP_AHRS_POSITION_RESET_ENABLED macro was shown inactive in VSCode because of HAL_PROGRAM_SIZE_LIMIT_KB <= 1024. And in the code HAL_PROGRAM_SIZE_LIMIT_KB always seems to be greater than 1024. In general, I decided to forcibly enable the macro to make the function of reading mavlink-package work.
  2. Then the message was processed in handle_external_position_estimate and then in EKF3.setLatLng(loc, pos_accuracy, timestamp_ms);. The NavEKF3::setLatLng function again contained the EK3_FEATURE_POSITION_RESET macro, which was specified as inactive. To make this macro active, I forcibly set EK3_FEATURE_ALL = 1 (it was enabled by default from the replay or standalone build condition, but we don’t build that).
  3. After that the message was processed in the NavEKF3_core::setLatLng( function from AP_NavEKF3_PosVelFusion.cpp, where it returned false. I decided to set PV_AidingMode and initialize validOrigin through appropriate functions. And I made deadReckonDeclare_ms 0 so that the flase condition would not occur.

But all this didn’t give any result: the message passes all the processing stages, but the coordinates don’t change. Perhaps I missed something. I will be very grateful for your help!!!