Correct home yaw when a valid heading is acquired #3247

fiam · 2018-05-20T15:10:14Z

This is specially relevant in FW without compass, since the home
location will typically be stored while the craft hasn't yet moved
and a CoG derived heading is not determined yet.

The navigation code checks when the heading transitions from invalid
to valid in updateActualHeading(), calculates the rotation from the old
invalid heading (using bootup heading as 0º) to the real one and
applies to posControl.homePosition.yaw.

In order to save a bit of memory, isHomeValid has been replaced
with a bitmask which stored several flags relative to the home
location.

digitalentity · 2018-05-21T08:05:08Z

src/main/navigation/navigation.c

+        (posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
+
+        int32_t yawRotation = newHeading - posControl.actualState.yaw;
+        posControl.homePosition.yaw += yawRotation;


Shouldn't we simply rewrite posControl.homePosition.yaw here? Why the yawRotation magic?

You need the rotation because you might arm, turn and then launch. If you turned 90º before launching the "fake" yaw at that point will be 90º and home will be at 0º (assuming you armed without rotating since battery was plugged in). Now you take off and once you get the real heading yaw is becoming 180º. That means your fake yaw was 90º off to the left, hence your home yaw needs that same correction since it was calculated with the fake one.

The problem with storing the first acquired yaw as home yaw is that you might unintentionally throw the plane a bit off from the direction you intended or even the pitch/roll stabilisation might send it a bit to one side (launch mode doesn't try to keep heading, but level). Using the yaw when you arm allows more predictable values for the pilot.

Ok, now I understand. Still, this code looks somewhat "hacky" to me, but I can't think about a way of improving it.

Maybe we can rename yawRotation to fakeToRealYawOffset?

Agreed, it will make things easier to understand. A comment, explaining the code, will also be quite helpful.

digitalentity · 2018-05-21T08:07:21Z

src/main/navigation/navigation.c

    }

    // Heading
    if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
        // Heading
        posControl.homePosition.yaw = yaw;
+        if (posControl.flags.estHeadingStatus >= EST_USABLE) {


setHomePosition may be called with arbitrary yaw. We shouldn't check for estHeadingStatus in this function. We could check for valid heading before calling setHomePosition though.

Opps. Nice catch! Thing is, for the best result we actually want to store it with the "fake" yaw, then correct it, since we don't don't have a valid yaw at arming time in crafts without mag. Maybe we can pass the homeFlags to setHomePosition()?

Not sure, need to think about it. setHomePosition has access to homeFlags anyway

I mean we can let the caller tell setHomePosition() which validity flags it should set for the home position.

Ah, that's definitely a good idea

digitalentity · 2018-05-22T04:16:00Z

src/main/navigation/navigation.c

+        (posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {
+
+        int32_t yawRotation = newHeading - posControl.actualState.yaw;
+        posControl.homePosition.yaw += yawRotation;


Agreed, it will make things easier to understand. A comment, explaining the code, will also be quite helpful.

This is specially relevant in FW without compass, since the home location will typically be stored while the craft hasn't yet moved and a CoG derived heading is not determined yet. The navigation code checks when the heading transitions from invalid to valid in updateActualHeading(), calculates the rotation from the old invalid heading (using bootup heading as 0º) to the real one and applies to posControl.homePosition.yaw. In order to save a bit of memory, isHomeValid has been replaced with a bitmask which stored several flags relative to the home location.

Callers can specify wether XY, Z and HEADING are valid for the new home position.

Also, add a small comment explaining the logic behind the home yaw adjustment.

commit 8e419c6 Author: Alberto García Hierro <[email protected]> Date: Thu Jun 7 16:34:16 2018 +0100 Round rather than truncate when updating GPS_directionToHome Makes taking off in straight line north produce a home direction of 180º rather than 179º. commit 002ebbd Merge: ec31f99 e4b99ba Author: Alberto García Hierro <[email protected]> Date: Thu Jun 7 16:32:56 2018 +0100 Merge pull request iNavFlight#3330 from iNavFlight/agh_maps_draw_over_center When drawing a POI over the map center, alternate commit ec31f99 Merge: 4b95c2e c8b26b4 Author: Paweł Spychalski <[email protected]> Date: Thu Jun 7 15:03:59 2018 +0200 Merge pull request iNavFlight#3331 from iNavFlight/dzikuvx-rangefinder-cleanup Cleanup of unused functions commit 4b95c2e Merge: f7c2824 d452344 Author: Konstantin Sharlaimov <[email protected]> Date: Wed Jun 6 20:03:43 2018 +0200 Merge pull request iNavFlight#3314 from iNavFlight/de_remove_midrc Remove mid_rc setting commit c8b26b4 Author: Pawel Spychalski (DzikuVx) <[email protected]> Date: Wed Jun 6 15:16:11 2018 +0200 Cleanup of unused functions commit e4b99ba Author: Alberto García Hierro <[email protected]> Date: Wed Jun 6 12:58:43 2018 +0100 When drawing a POI over the map center, alternate Draw the map center symbol for 1s, then the POI symbol for 1s. This feels more natural than not drawing the POI when both are very close, since flying over the home location would make the craft disappear from the map. commit f7c2824 Merge: 544e03b d624f65 Author: Alberto García Hierro <[email protected]> Date: Wed Jun 6 12:33:18 2018 +0100 Merge pull request iNavFlight#3247 from iNavFlight/agh_home_heading_correction Correct home yaw when a valid heading is acquired commit 544e03b Merge: 0e0dba5 616ccaa Author: Alberto García Hierro <[email protected]> Date: Wed Jun 6 12:32:56 2018 +0100 Merge pull request iNavFlight#3305 from iNavFlight/agh_fix_map_radar_directions Fix directions of drawing for symbols in map and radar mode commit 0e0dba5 Merge: 50847b9 85c690a Author: Paweł Spychalski <[email protected]> Date: Wed Jun 6 09:50:44 2018 +0200 Merge pull request iNavFlight#3328 from iNavFlight/dzikuvx-rangefinder-rename I2C rangefinder rename and docs update commit 85c690a Author: Pawel Spychalski (DzikuVx) <[email protected]> Date: Wed Jun 6 08:50:54 2018 +0200 Fixed build commit 2b7b47e Author: Pawel Spychalski (DzikuVx) <[email protected]> Date: Wed Jun 6 08:43:45 2018 +0200 I2C rangefinder rename commit 50847b9 Merge: 4374471 a63049d Author: Alberto García Hierro <[email protected]> Date: Mon Jun 4 21:47:12 2018 +0100 Merge pull request iNavFlight#3320 from iNavFlight/agh_update_gitignore Remove /src/main/fc/settings_generated.{h, c} from .gitignore commit a63049d Author: Alberto García Hierro <[email protected]> Date: Mon Jun 4 18:46:33 2018 +0100 Remove /src/main/fc/settings_generated.{h, c} from .gitignore The generated files are now in obj/, so there's no need to ignore the ones in the old location. Also, having the old generated files in place can make the compiler use the old outdated files if someone built the firmware before, so it's better to make them easily noticeable via git status. commit d452344 Author: Konstantin (DigitalEntity) Sharlaimov <[email protected]> Date: Sun Jun 3 20:15:57 2018 +0200 Remove MIDRC setting commit 4374471 Merge: 59864b8 dd86a0b Author: Konstantin Sharlaimov <[email protected]> Date: Sat Jun 2 19:24:00 2018 +0200 Merge pull request iNavFlight#3296 from shellixyz/restore_osd_sw Restore OSD SW commit 59864b8 Merge: ff60ce8 25697c0 Author: Konstantin Sharlaimov <[email protected]> Date: Sat Jun 2 19:23:40 2018 +0200 Merge pull request iNavFlight#3265 from teckel12/te_make-current-meter-feature-work make isAmperageConfigured respect FEATURE_CURRENT_METER and not just current_meter_type commit ff60ce8 Merge: e28f7fa 680bf8b Author: Konstantin Sharlaimov <[email protected]> Date: Sat Jun 2 19:22:51 2018 +0200 Merge pull request iNavFlight#3304 from giacomo892/arming_checks_rework Disable arming in AH for FW unless launch mode is enabled commit e28f7fa Merge: a2a60d4 05c5f0f Author: Konstantin Sharlaimov <[email protected]> Date: Sat Jun 2 19:21:56 2018 +0200 Merge pull request iNavFlight#3303 from giacomo892/nav_launch_safety_fix NAV_LAUNCH extra safety commit a2a60d4 Merge: 4d5cc76 bf0f05a Author: Konstantin Sharlaimov <[email protected]> Date: Sat Jun 2 19:21:08 2018 +0200 Merge pull request iNavFlight#3275 from iNavFlight/agh_parallel_build Move build-stamp and generated files to obj/main/$(TARGET) commit 616ccaa Author: Alberto García Hierro <[email protected]> Date: Thu May 31 21:38:27 2018 +0100 Fix directions of drawing for symbols in map and radar mode - X axis was reversed in map mode - Home direction rotation was not corrected for the current heading in radar mode commit 680bf8b Author: giacomo892 <[email protected]> Date: Thu May 31 17:19:01 2018 +0200 Disable arming in AH for FW unless launch mode is enabled commit 05c5f0f Author: giacomo892 <[email protected]> Date: Thu May 31 16:29:21 2018 +0200 Extra safety for NAV_LAUNCH if enabled via BOX commit dd86a0b Author: Michel Pastor <[email protected]> Date: Thu May 31 01:12:44 2018 +0200 Restore OSD SW It is nice to be able to be able to hide the OSD without wasting an OSD layout commit bf0f05a Author: Alberto García Hierro <[email protected]> Date: Fri May 25 11:00:12 2018 +0100 Move build-stamp and generatic files to obj/main/$(TARGET) This allows target level parallel builds, since now there are no shared files between targets. Fixes iNavFlight#3261 commit 25697c0 Author: Tim Eckel <[email protected]> Date: Tue May 22 22:35:37 2018 -0400 make isAmperageConfigured respect FEATURE_CURRENT_METER and not just current_meter_type commit d624f65 Author: Alberto García Hierro <[email protected]> Date: Tue May 22 11:05:00 2018 +0100 Rename yawRotation to fakeToRealYawOffset Also, add a small comment explaining the logic behind the home yaw adjustment. commit 200a85a Author: Alberto García Hierro <[email protected]> Date: Tue May 22 11:01:34 2018 +0100 Make setHomePosition() accept a new argument indicating the home flags Callers can specify wether XY, Z and HEADING are valid for the new home position. commit 3e0ae8b Author: Alberto García Hierro <[email protected]> Date: Sun May 20 16:04:19 2018 +0100 Correct home yaw when a valid heading is acquired This is specially relevant in FW without compass, since the home location will typically be stored while the craft hasn't yet moved and a CoG derived heading is not determined yet. The navigation code checks when the heading transitions from invalid to valid in updateActualHeading(), calculates the rotation from the old invalid heading (using bootup heading as 0º) to the real one and applies to posControl.homePosition.yaw. In order to save a bit of memory, isHomeValid has been replaced with a bitmask which stored several flags relative to the home location.

fiam added this to the 2.0 milestone May 20, 2018

fiam requested a review from digitalentity May 20, 2018 15:10

digitalentity reviewed May 21, 2018

View reviewed changes

digitalentity approved these changes May 22, 2018

View reviewed changes

digitalentity added the Rebase Required label May 22, 2018

fiam force-pushed the agh_home_heading_correction branch from 2ff90a3 to 413453f Compare May 22, 2018 12:02

fiam force-pushed the agh_home_heading_correction branch from 413453f to 131213f Compare May 22, 2018 12:08

fiam removed the Rebase Required label May 22, 2018

fiam force-pushed the agh_home_heading_correction branch 2 times, most recently from 5b6f98f to 39fe28a Compare May 22, 2018 12:26

fiam added 2 commits May 22, 2018 13:27

Make setHomePosition() accept a new argument indicating the home flags

200a85a

Callers can specify wether XY, Z and HEADING are valid for the new home position.

Rename yawRotation to fakeToRealYawOffset

d624f65

Also, add a small comment explaining the logic behind the home yaw adjustment.

fiam force-pushed the agh_home_heading_correction branch from 39fe28a to d624f65 Compare May 22, 2018 12:27

fiam merged commit f7c2824 into development Jun 6, 2018

fiam deleted the agh_home_heading_correction branch June 6, 2018 11:33

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Correct home yaw when a valid heading is acquired #3247

Correct home yaw when a valid heading is acquired #3247

fiam commented May 20, 2018

digitalentity May 21, 2018

fiam May 21, 2018

digitalentity May 21, 2018

fiam May 21, 2018

digitalentity May 22, 2018

digitalentity May 21, 2018

fiam May 21, 2018

digitalentity May 21, 2018

fiam May 21, 2018

digitalentity May 21, 2018

digitalentity May 22, 2018

Correct home yaw when a valid heading is acquired #3247

Correct home yaw when a valid heading is acquired #3247

Conversation

fiam commented May 20, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment