Indoor flight with external navigation data

Subodh_Mishra · October 14, 2018, 9:23pm

I manually made the changes mentioned in the pull request https://github.com/ArduPilot/ardupilot/pull/9485 to the arducopter repo I am using and the build was successful. Is it necessary to implement the changes made in the second pull request you have mentioned here? I don’t think it is necessary because I am not using any vision based speed data.

chobitsfan · October 14, 2018, 11:51pm

I don’t think it is necessary because I am not using any vision based speed data.

Sure

XerxMaleki · November 28, 2018, 10:26pm

Hey @chobitsfan,

Thank you for sharing your work. I am also trying to use Optitrack data for indoor flight. Here is my setup:
Optitrack - Flex 13 Cameras
Motive 2.1
NatNet based software >> wifi >>
Raspberry Pi 3 >> Serial >> Telemetry 2 port
Pixhawk - ArduCopter 3.6.1
Custom Built Quad

I am Using (dronekit and pymavlink) libraries on Raspberry Pi and use ( att_pos_mocap_send ) function to send orientation and location data to pixhawk. Now here is my problem:

Although I see the message “EKF2 IMU0 is using external nav data” and “EKF2 IMU0 ext nav yaw alignment complete” and “EKF2 IMU0 initial pos NED = 1.0,51.0,26.0 (m)” on Mission planner (connected via Telemetry 1), Lat, Lon, and Alt values do not change. It looks like that the values that I send are ignored by autopilot. (I made sure that the GPS_Type is 3 and COMPASS_USE is 0 - I even downloaded your params and uploaded them on my pixhawk, same result!) Interestingly, my Yaw value initially changes to the value given by RPI but then if I rotate the quad it will change!! I have no idea what should I do.
I do not want to update Pitch and Roll values with optitrack values. Is there any setting that I just update yaw, x, y, z values and let internal IMU update pitch and roll?

I really appreciate if you (or anyone else familiar with the subject) can help me.

Thank you,
Niki

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