Attempt to provide vision coordinate

ArduCopter Copter 4.6
1

I am trying to provide vision coordinates and covariance from the onboard computer (raspberry) using mavlink to FC (MATEK H743 SLIM). I tried all three methods (with c++):

mavlink_msg_vision_position_estimate_encode
mavlink_msg_global_vision_position_estimate_encode
mavlink_msg_odometry_encode

I use gcs id = 200

I also set up a home point with

mavlink_msg_set_gps_global_origin_pack

The problem is that all this works ONLY if I connect to FC with the mission planner (in addition to Raspberry). Otherwise, all my sent data was ignored.

2

You need to send heartbeat packets from your RPi as well as vision position estimates. And you need to sync the timebases for good measure as well.

3

I tried, but no changes:

mavlink_heartbeat_t hb;
hb.type = MAV_TYPE_ONBOARD_CONTROLLER;
hb.autopilot = MAV_AUTOPILOT_INVALID;
hb.base_mode = 0;
hb.custom_mode = 0;
hb.system_status = MAV_STATE_ACTIVE;
hb.mavlink_version = 3;
mavlink_msg_heartbeat_encode(sysid, compid, &message, &hb);

Btw, if I connect with mission planner, disconnect, then go to fly, the provided coordinate works ideally, better that usual gps-based loiter. But I still can’t avoid the Mission Planner!

4

And this is how I send coordinate:

    mavlink_global_vision_position_estimate_t vpd;
    memset(&vpd, 0, sizeof(vpd));
    vpd.usec = get_synchronized_time_usec();
    vpd.x = pos.x;
    vpd.y = pos.y;
    vpd.z = pos.z;
    vpd.roll = ang.x;
    vpd.pitch = ang.y;
    vpd.yaw = ang.z;

    std::fill_n(vpd.covariance, 21, 0);
    float coor_uncertainty = error;
    coor_uncertainty *= coor_uncertainty;
    // Set position covariance to 1.0 (1m standard deviation)
    vpd.covariance[0] = coor_uncertainty;  // x
    vpd.covariance[6] = coor_uncertainty;  // y
    vpd.covariance[11] = 1e8; // z

    // Set angular covariance to a large angular value, so that the EKF does not use the vision estimate for angles
    vpd.covariance[15] = 100; // roll
    vpd.covariance[18] = 100; // pitch
    vpd.covariance[20] = 100; // yaw

    mavlink_msg_global_vision_position_estimate_encode(sysid, compid, &message, &vpd);
5

Some additional details:

when Mission Planner connects I immediately getting tonn of messages

00:00:43.654 INFO: EKF3 IMU0 is using external nav data
00:00:43.654 INFO: EKF3 IMU0 initial pos NED = 0.1,-0.2,0.4 (m)
00:00:43.654 INFO: EKF3 IMU0 stopped aiding

Also I start getting messages

LOCAL_POSITION_NED

Of course, I require that messages at some FPS, but never get it till I connect to Mission Planner. I need to know what sort of magic does the MP that enables that.

6

Hi @AndrewShpagin,

I think that the only thing that MP could be doing is setting the stream rates (dev wiki is here). I wonder if you’ve confirmed that the RPI is actually sending the position estimates to the autopilot before MP is connected?

BTW, I’ve recently created this developer wiki page to help with setup of non-GPS navigation. You’re already past what the wiki page shows but I hope it will be a good reference for others at least.

I don’t actually think it’s necessary to sync the times between companion computer and autopilot.

2 Likes
7

Thanks! After some debugging, I found the reason. It was my fault, I was waiting for timesync with tc1 != 0 before passing coordinates. That was wrong. Now I start sending coordinates constantly, 20fps. And it works perfectly for another drone (of my companion in another city), but not for me. That (working) drone has MATEK H743, I test on SpeedyBee F405V3. I made special firmware with support of odometry, external nav/ahrs. But when I turn on loiter FC tells that coordinate required. The Mission Planner shows that coordinate changes, in inspector I see lat/lon changes (when I move drome manually) but I can’t get loiter to work.
Btw, does the timestamp I send with global_vision_position really used?

1 Like
8

I see from the doc that GLOBAL_VISION_POSITION_ESTIMATE is not recommended, but this is the only one that has given me some actual results; I tested VISION_POSITION_ESTIMATE, ODOMETRY. When I send an odometry message (using mavlink_msg_odometry_encode), the message does not even appear in Mavlink Inspector.

Actually, I attempted to provide vision coordinate many times (at least 4 serious attempts). No success each time, so ended up using set attitude target and solved the task up to some degree (optical stab on any height, tested up to 1km). But I don’t like this solution, I want more consistent integration.

Build settings:

{“selected_features”: [“AHRS_EXT”, “AHRS_EXT_VECTORNAV”, “BARO_WIND_COMP”, “EKF3”, “EKF3_EXTNAV”, “EKF3_OPTFLOW”, “EKF3_WINDEST”, “VISUALODOM”, “BARO_TEMPCAL”, “MSP_BARO”, “SPL06”, “BATTERY_SUM”, “Camera”, “Camera_Relay”, “Camera_Servo”, “RUNCAM”, “AK09916”, “BMM150”, “BMM350”, “CompassLearn”, “FixedYawCal”, “HMC5843”, “ICM20948”, “IST8308”, “IST8310”, “LIS3MDL”, “MMC3416”, “QMC5883L”, “RM3100”, “MODE_BRAKE”, “MODE_FLIP”, “MODE_GUIDED_NOGPS”, “MODE_TURTLE”, “FILESYSTEM_MISSION”, “FILESYSTEM_PARAM”, “FILESYSTEM_ROMFS”, “FILESYSTEM_SYS”, “NMEA_GPS”, “NMEA_UNICORE”, “UBLOX”, “BatchSampler”, “HarmonicNotches”, “AP_MAVLINK_FTP_ENABLED”, “FENCEPOINT_PROTOCOL”, “MAVLINK_MSG_MISSION_REQUEST”, “MAVLINK_MSG_RC_CHANNELS_RAW”, “MAV_DEVICE_OP”, “MAV_MSG_SERIAL_CONTROL”, “MAV_SERVO_RELAY”, “RALLYPOINT_PROTOCOL”, “REQUEST_AUTOPILOT_CAPA”, “MSP”, “MSP_COMPASS”, “MSP_DISPLAYPORT”, “MSP_GPS”, “MSP_RANGEFINDER”, “LED_CONTROL”, “NOTIFY_NEOPIXEL”, “NOTIFY_PROFILED”, “PLAY_TUNE”, “TONEALARM”, “OSD”, “OSD_PARAM”, “OSD_SIDEBARS”, “Buttons”, “COMPASS_CAL”, “CUSTOM_ROTATIONS”, “Logging”, “SDCARD_FORMATTING”, “LANDING_GEAR”, “RELAY”, “SERVORELAY_EVENTS”, “RC_CRSF”, “RC_GHST”, “RC_IBUS”, “RC_PPMSUM”, “RC_Protocol”, “RC_SBUS”, “RC_SRXL”, “RC_SRXL2”, “RC_ST24”, “RC_SUMD”, “RSSI”, “RANGEFINDER”, “RANGEFINDER_ANALOG”, “RANGEFINDER_BENEWAKE_TF02”, “RANGEFINDER_BENEWAKE_TF03”, “RANGEFINDER_BLPING”, “RANGEFINDER_GYUS42V2”, “RANGEFINDER_HC_SR04”, “RANGEFINDER_LANBAO”, “RANGEFINDER_LEDDARONE”, “RANGEFINDER_LEDDARVU8”, “RANGEFINDER_LIGHTWARE_SERIAL”, “RANGEFINDER_LWI2C”, “RANGEFINDER_MAVLINK”, “RANGEFINDER_MAXBOTIX_SERIAL”, “RANGEFINDER_MAXSONARI2CXL”, “RANGEFINDER_NMEA”, “RANGEFINDER_PULSEDLIGHTLRF”, “RANGEFINDER_PWM”, “RANGEFINDER_TRI2C”, “RANGEFINDER_TR_SERIAL”, “RANGEFINDER_USD1_SERIAL”, “RANGEFINDER_VL53L0X”, “RANGEFINDER_VL53L1X”, “RANGEFINDER_WASP”, “RFND_BENEWAKE_TFMINI”, “RFND_BENEWAKE_TFMINIPLUS”, “AC_AVOID”, “FENCE”, “PARACHUTE”, “RALLY”, “AIRSPEED”, “OPTICALFLOW”, “RPM”, “RPM_ESC_TELEM”, “RPM_HARMONIC_NOTCH”, “RPM_PIN”, “Bidirectional FrSky Telemetry”, “CRSF”, “CRSFText”, “FrSky”, “FrSky D”, “FrSky SPort”, “FrSky SPort PassThrough”, “GHST”, “SPEKTRUM”, “HEXA”, “OCTA”, “QUAD”, “SMARTAUDIO”, “TRAMP”, “VIDEO_TX”], “git_hash_short”: “b094390943”}

9

Ok, issue solved. There was no compass, None in yaw source. I have set External there and all started to work. Of course I still pass huge yaw error. I understood this from this comment in sources:

// Handle the special case where we are on ground and disarmed without a yaw measurement
    // and navigating. This can occur if not using a magnetometer and yaw was aligned using GPS
    // during the previous flight.
    if (yaw_source_last == AP_NavEKF_Source::SourceYaw::NONE &&
        !motorsArmed &&
        onGround &&
        PV_AidingMode != AID_NONE)
    {
        PV_AidingMode = AID_NONE;
...
10

As a result, I got a nicely working optical loiter even on 1mb fc-s.

1 Like
11

There is still one problem. I send message GLOBAL_VISION_POSITION_ESTIMATE and it comes not only to flight controller itself, but to other connected devices (QGround) and this way creates additional traffic that partially blocks the carry channel. Why? How to disable it? Serious problem…

image
image1920Ă—1220 68 KB

12

Currently I consider as solution modifying this routing code, adding global vision as well:

if (msg.msgid == MAVLINK_MSG_ID_RADIO ||
    msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
    // don't forward RADIO packets
     return true;
}
13

Hi @AndrewShpagin,

There’s an SERIALx_OPTION bit that can stop forward of mavlink packets to/from that serial port. If you only want to disable one direction then a trick I’ve used is to again use the SERIALx_OPTION parameter but set one of the bits to invert either the RX or TX pin. That’s a bit naughty because it means garbled messages will still be sent but they end up being ignored by the other side.