Indoor flight with external navigation data

chobitsfan · November 29, 2018, 5:16am

Lat, Lon, and Alt values do not change.

You may need to use SET_GPS_GLOBAL_ORIGIN to set lat, lon of EKF origin (any valid lat, lon will do)

xfly · December 5, 2018, 9:02pm

Hey @XerxMaleki,
did you manage to only update position and yaw values? I’m also interested in this.

Thanks,
Fabian

chobitsfan · December 6, 2018, 3:15am

hi @XerxMaleki @xfly
I am not the anther of ekf2 lib, but according to following code, only yaw is used in heading fuse

github.com

ArduPilot/ardupilot/blob/997d07b694b7fa549afd167f0002af666fe76f73/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp#L820


    stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
    predicted_yaw = euler321.z;
    if (use_compass() && yawAlignComplete && magStateInitComplete) {
        // Use measured mag components rotated into earth frame to measure yaw
        Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
        Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
        measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
    } else if (extNavUsedForYaw) {
        // Get the yaw angle  from the external vision data
        extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
        measured_yaw =  euler321.z;
    } else {
        // no data so use predicted to prevent unconstrained variance growth
        measured_yaw = predicted_yaw;
    }
} else {
    // calculate observaton jacobian when we are observing a rotation in a 312 sequence
    float t2 = q0*q0;
    float t3 = q1*q1;
    float t4 = q2*q2;
    float t5 = q3*q3;

XerxMaleki · March 8, 2019, 11:27pm

Thank you for the help.
It works now

Soon I will publish some video of what I did.

grayleow · March 31, 2019, 1:06am

Hi Niki @XerxMaleki,

I am doing something similar as your above mentioned project. Mind sharing with me your software setup for the system and procedure? Your assistance is deeply appreciated.

guglie · April 24, 2019, 2:49pm

Hi @chobitsfan,

I’ve seen from your parameter list that you, like me, are using WP_NAV_RADIUS values below 10cm, but it seems that WP_NAV_RADIUS cannot be set lower than 10cm .

chobitsfan · April 25, 2019, 2:04am

hi @guglie

Yes, set WP_NAV_RADIUS less than 10 has no effect because _wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN); So I change WPNAV_WP_RADIUS_MIN to 1.0f and re-build the firmware myself

guglie · April 25, 2019, 1:42pm

This is exactly what I’m currently doing (except that I set WPNAV_WP_RADIUS_MIN to 0.1f), let us know if that enhances your tests.

I’ve created a PR on this

ziranwu · August 10, 2019, 2:42am

Got the same error message as below when trying to build skyviper v2450 and the Journey GPS -f412 firmware:

ArduCopter/toy_mode.cpp:939:47: error: cannot convert ‘const mavlink_message_t {aka const __mavlink_message}’ to ‘const mavlink_message_t* {aka const __mavlink_message*}’ for argument ‘1’ to ‘void mavlink_msg_named_value_int_decode(const mavlink_message_t*, mavlink_named_value_int_t*)’

Any thoughts?

Joel_Fankam · August 10, 2019, 9:14pm

Hi , did u also set the home_position , if yes what about the orientation , i mean the quaternion ?
I am also try to send external position coming from pozyx. i think i did not send the right mocap since i did not get the quaternion from the pozyx tag . A little question is waht do i need to change if the yaw agnle is different to zero ?
any help will be welcome

XerxMaleki · August 11, 2019, 1:43am

I am facing a new problem in Arducopter 3.6.10. @rmackay9 @chobitsfan @tiffo

I am feeding the Black Cube equipped with Arducopter 3.6.10 with mocap data (as I did before on earlier versions) but after a while (2~5 minutes) it automatically disregards the external navigation data and loses its (Alt, Lat, Lon, and yaw)!!!
I checked the connection and everything appears to be OK and I cannot figure out what is going on in the arducopter.
you can find the log file here: https://drive.google.com/file/d/18HMvk6NWLArGBOR1yICU18ljswp7dOS2/view?usp=sharing

I also attached an interesting graph I got from my telemetry log which kinda shows ekf status when everything goes wrong.

Also, note that in this test, the quad is on the ground and not moving (motors are off) the whole time.

chobitsfan · August 12, 2019, 1:29pm

hi @ziranwu did you git submodule update --init --recursive?

ziranwu · August 12, 2019, 5:37pm

Yes, did the submodule update for both the Ardupilot master and the Chobitsfan/ardupilot master. Both repo gave this same error message when compiling skyviper-v2450,
skyviper-f412, and skyviper-f412-rev1.

Looks like it involves dealing with this ToyMode on Skyviper drones.

Thanks,

Ziran

chobitsfan · August 28, 2019, 2:58am

did you do ./waf clean and ./waf distclean?

ziranwu · August 28, 2019, 6:40pm

Tried and still got the same error message regarding toy_mode.cpp:939:47.

My input of command lines in the /ardupilot repo are:

./waf clean

./waf distclean

./waf configure --board skyviper-f412 (since I am using a Skyviper Journey Pro drone)

./waf copter

Thanks,

Ziran

chobitsfan · August 29, 2019, 1:38am

you can edit libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat
#define TOY_MODE_ENABLED ENABLED

ziranwu · August 29, 2019, 2:31am

This option was originally ENABLED. I disabled it and the copter build passed. Will try the firmware.

Thanks,

Ziran

Runze · December 7, 2019, 10:38am

Hello Chobitsfan,

Your great job is exactly what I want to do. I have bebop2 running the ardupilot, and have a optitrack. I followed your tutorial and modified the SampleClientML.exe. I think it keep sending feedback to bebop(192.168.42.1:14550). But when I connect with bebop and let it arm in loiter mode, the pre-arm keeps warning that EKF2 still initializing. I don’t know what to do with this situation. I have tried GPS_TYPE=0, or GPS_TYPE=14.

roackb2 · December 5, 2020, 10:56am

For those who’s also looking for this feature, I created a new PR based on @chobitsfan 's work, and rebase on master branch, the PR is here.

Also, I’ve tested this feature with a copter for many times, and the result is quite satisfying for me.
I would share some information to help those who might need it as following:

Here’s the video
This is the flight plan
And this is the parameters I use
Hardware: Pixhawk 2.4
Vision: Intel Realsense T265
Companion Computer: Raspberry Pi 3B+
script running on companion computer to feed position data to flight control is here

churl · December 1, 2021, 2:42am

When we put the drone in the origin point (0,0,0) of the optitrack system. The drone have a proper position on MP, but if we put the drone in the other place of the optitrack system ( like -3m, -2m, 0 ), The drone will lost proper position on MP, the phenomenon is like the drone’s position is added with the offset distance (-3m,-2m) of the optitrack system. in addition, the offset distance in MP is enlarged compared with the distance in the optitack system. The drone cannot fly properly with bad position health. Can anyone help? thanks