We gave it a try yesterday to a primary lane of EKF with VIO as yaw source and GPS for position. We noticed that the alignment for the yaw of the VIO to be referenced with North direction as zero is done with respect to the yaw is currently in use. Before taking off, this would be, and correct me if I am wrong, the yaw estimation in the first lane (EKF3 core with index 0) since the AP_NavEKF3::updateFilter method forces it to be the primary if the vehicle has not been armed. Therefore, when we define the EK3_SRC1_YAW=6 (Extnav yaw) and RC9_OPTION=80 (VisoAlign) and flip the channel 9 before flying, the alignment does not happen. In fact, it makes sense since the method AP_VisualOdom_IntelT265::align_sensor_to_vehicle check first if the source of yaw is external navigation and does not perform the alignment if this is true.
The alignment works perfectly before taking off if the primary lane has another yaw source such compass as shown below:
And I dare say that we were able to have a very good flight with the yaw of the VIO as source without the alignment making sure that the vehicle was pointing north before taking off. However, I wonder if there is a work-around to this situation that does not imply us to point to north before taking off. The yaw of the camera is quite good in certain environments where the compass is not an option. I am going to try today a Lua script that will change the primary core before taking off, so the alignment can happen having in a secondary lane the compass and making it the primary just before taking off. Not sure if it is going to work since the AP_NavEKF3::updateFilter does not allow lane switching before arming.
Really happy that you’re testing this out and that it is working OK so far.
One clarification is the EKF “source set” is a separate feature from the “lane”. All lanes will use the active source set. As a side note we have discussed adding an option so that each lane uses a separate source set but that’s still a to-do.
As I think you’ve found, the position of the external nav system is aligned to the EKF’s position whenever the external nav is not the active position source. The external nav’s yaw alignment however only happens once at startup or when the pilot moves the “VisoAlign” aux switch (assuming it has been setup). We have an item on the to-do to align the yaw as well whenever it’s not the active yaw source but I just haven’t quite gotten to it yet.
There is unfortunately no way to manually set the external nav’s yaw to a particular value so the best you can do is, before takeoff, momentarily switch the source set to one that uses compass for yaw, move the VisoAlign switch and then switch the source set back to the external nav yaw.
I’m open to ideas to make this better though! We could add a lua script method to set the external nav yaw to a particular value.
That will be awesome, I will keep reading the source code for understanding better in order to make an educate suggestion. We tested today the lane switching between camera yaw and compass in altitude hold and loiter mode. There is something that we notice that deserves some attention, but I am going to report it in a new topic for not talking about different topics in the same stream.
Today we also tried a lane switch between a lane with the first source for yaw as GPS (EK3_SRC1_YAW=2) and a second lane with VIO as yaw source (EK3_SRC2_YAW=6). We were unable to align the yaw of the camera using a channel of the transmitter (RCx_OPTION=80) as we have done before when the source 1 is compass. I guess the initial alignment that @rmackay9 did mention did not happen in this scenario since the yaw requires RTK fixed which can several seconds to happen depending on your location, the weather, the position of the satellites, etc. In the following plot, it is clear that the alignment does not happen, and that when we switch between source 1 and source 2, there is a jump in the yaw:
Actually, because the misalignment, we get the message PreArm: VisOdom:yaw diff 81.4 deg (>10) at about 80 seconds, just after we tried to align several times with the channel that has RCx_OPTION=80. The log of this ground test can be found in the link below:
Anyway, we aligned the drone facing north before taking off, and had a pretty clean flight with manual lane switching. This new version of Arducopter is great and this source switching is a big improvement. Thank you all that make this great software possible!
… but we can also see that, as you have described, the visual odometry yaw is not being aligned to the AHRS/EKF’s yaw when visual odometry is not being used… and the issue is just that AP_AHRS::is_ext_nav_used_for_yaw() unfortunately returns “true” if GPS-for-yaw is being used.
These are the lines in AP_VisualOdom_IntelT265 that are causing the trouble:
// use sensor provided attitude to calculate rotation to align sensor with AHRS/EKF attitude
bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position, const Quaternion &attitude)
{
/// do not align to ahrs if it is using us as its yaw source
if (AP::ahrs().is_ext_nav_used_for_yaw()) {
return false;
}
This issue is already on the to-do list (see top item here) and hopefully we can get it fixed in the near-ish future. If you’re comfortable compiling the code yourself you could simply remove those lines… you will want to try and avoid aligning the yaw if the ExtNav is already the source of yaw but even if you did accidentally re-align it to itself it shouldn’t cause significant problems.
It has been a while, but we discovered what may be the issue. First, we tried removing the check @rmackay9 suggested, and still we were not able to align it. At some point I thought it was that the method AP_VisualOdom_IntelT265::align_sensor_to_vehicle was not called for some reason when the channel configured with RCx_OPTION=80 was switched. However, setting a breakpoint inside the method using GDB proved us wrong. The problem was not only on that check but also in the second check of AP_VisualOdom_IntelT265::align_sensor_to_vehicle. Apparently, when using GPS yaw, the method AP::ahrs().yaw_initialised() still returns false even if we got RTK fixed. This is the result of a ground test where we created binders for AP::ahrs().is_ext_nav_used_for_yaw() and AP::ahrs().yaw_initialised. If AP::ahrs().yaw_initialised was false, the Lua script sends the message:
“GPS yaw has not been initialized and cannot be used for alignment!”
Otherwise, the script sends the message:
“GPS yaw has been initialized and can be used for alignment!”
Similarly, with the method AP::ahrs().is_ext_nav_used_for_yaw(), if it returns true the Lua script sends the message:
“External navigation is used for yaw!”
Otherwise:
“External navigation is not used for yaw!”
Here the log, when even after getting RTK fixed the AP::ahrs().is_ext_nav_used_for_yaw() was returning false, and the method AP::ahrs().is_ext_nav_used_for_yaw() always returns true:
In conclusion, the two mentioned methods are avoiding the alignment to happen. I think for now I will be just double-check with a Lua script that the yaw has a valid value before alignment and remove the two checks. I wonder if this can be problematic for any reason.
I gave it a try with the modification I mentioned, and there are a couple things worthwhile sharing. Before sharing some discoveries, I want to point that one of my objectives was to be able to have certain logic that aligns the T265 yaw (source 1) with the GPS yaw (source 0) automatically without using a the RCx_OPTION=80 channel. These are my findings:
It was not difficult to create necessary Lua binders for aligning the visual odometry since the class AP_VisualOdom is implemented as singleton; it is my understanding that the binders generator uses the same pointer that avoids the creation of more than one object of a singleton class, very clever. Thus, for having this feature available on the Lua script it was only required to add to AP_Scripting/generator/description/bindings.desc:
include AP_VisualOdom/AP_VisualOdom.h
singleton AP_VisualOdom alias visualodom
singleton AP_VisualOdom method healthy boolean
singleton AP_VisualOdom method align_sensor_to_vehicle void
May be these are binders that can be there by default. The only problem is that there is no way to know if the alignment was successful, unless I switch sources momentarily and check that the yaw has no an abrupt jump. This is because the yaw of the T265 apparently is not saved anywhere, but it is corrected using the attribute yaw_trim of the class AP_VisualOdomT265 every time a visual_position_estimation message arrives; the align_sensor_to_vehicle is just an adjustment to yaw_trim. Thus, I could not find any method to use as a binder for the Lua script and check that the alignment happened.
The check done calling AP::ahrs().yaw_initialised inside AP_VisualOdom_IntelT265::align_sensor_to_vehicle is actually very important. Getting RTK fixed does not guarantee that the yaw has been aligned in the EKF3, it usually takes a few seconds. Thus, if the align_sensor_to_vehicle method is called in the Lua script, it may happen that the yaw is not properly align in the AHRS. This is not checked I remove this check from AP_VisualOdom_IntelT265::align_sensor_to_vehicle to make it possible the alignment with GPS yaw.
A work around was to create a binder for the method NavEKF3:yawAlignmentComplete(). Since this class is not singleton and for keeping consistency, it required me to create a virtual method in AP_AHRS that can be override by a method in AP_AHRS_NavEKF which finally calls NavEKF3:yawAlignmentComplete(). Something similar to what is done with the method AP_AHRS::get_vel_innovations_and_variances_for_source. Now this binder allows me to check if the AHRS with the first source (GPS yaw) has been aligned and align it from the Lua script.
I still have a question, since it is not possible to know if the alignment has been performed correctly or to check that it keeps aligned during flight, how critical is this for the T265 yaw? so far we have not seen any significant yaw drifting in the logs between the GPS yaw and the yaw in VISP, but do you think is a good idea to have something that check for the alignment of the camera during flight and realign or to call the align_sensor_to_vehicle binder periodically?
Great that you’re digging into the code to this depth.
Re aligning the T265 inflight, yes, I think we should re-align T265’s yaw periodically instead of only when the pilot commands it and it’s on the to-do list within the “EKF3: improve source switching (GPS<->NonGPS)” issue. I don’t think it is a really big change but I wanted to be sure that there was not large CPU performance impact (it shouldn’t be too bad I think) and I also haven’t checked whether aligning the yaw and position on every update (from the external nav system) works properly. When we adjust the external nav’s yaw trim we also update a position offset… we just need to be sure that we don’t introduce an error or a position drift. I think it is correct as-is but we need to be sure.
