Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Correct home yaw when a valid heading is acquired #3247

Merged
merged 3 commits into from Jun 6, 2018

Conversation

Projects
None yet
2 participants
@fiam
Copy link
Member

commented May 20, 2018

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 fiam added this to the 2.0 milestone May 20, 2018

@fiam fiam requested a review from digitalentity May 20, 2018

(posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {

int32_t yawRotation = newHeading - posControl.actualState.yaw;
posControl.homePosition.yaw += yawRotation;

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 21, 2018

Member

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

This comment has been minimized.

Copy link
@fiam

fiam May 21, 2018

Author Member

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.

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 21, 2018

Member

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

This comment has been minimized.

Copy link
@fiam

fiam May 21, 2018

Author Member

Maybe we can rename yawRotation to fakeToRealYawOffset?

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 22, 2018

Member

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

}

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

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 21, 2018

Member

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.

This comment has been minimized.

Copy link
@fiam

fiam May 21, 2018

Author Member

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()?

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 21, 2018

Member

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

This comment has been minimized.

Copy link
@fiam

fiam May 21, 2018

Author Member

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

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 21, 2018

Member

Ah, that's definitely a good idea

(posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) {

int32_t yawRotation = newHeading - posControl.actualState.yaw;
posControl.homePosition.yaw += yawRotation;

This comment has been minimized.

Copy link
@digitalentity

digitalentity May 22, 2018

Member

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

@fiam fiam force-pushed the agh_home_heading_correction branch from 2ff90a3 to 413453f May 22, 2018

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 fiam force-pushed the agh_home_heading_correction branch from 413453f to 131213f May 22, 2018

@fiam fiam removed the Rebase Required label May 22, 2018

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

fiam added some commits May 22, 2018

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.
Rename yawRotation to fakeToRealYawOffset
Also, add a small comment explaining the logic behind the home
yaw adjustment.

@fiam fiam force-pushed the agh_home_heading_correction branch from 39fe28a to d624f65 May 22, 2018

@fiam fiam merged commit f7c2824 into development Jun 6, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

@fiam fiam deleted the agh_home_heading_correction branch Jun 6, 2018

shellixyz added a commit to shellixyz/inav that referenced this pull request Jun 8, 2018

Squashed commit of the following: upstream/agh_round_home_direction
commit 8e419c6
Author: Alberto García Hierro <alberto@garciahierro.com>
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 <fiam@rm-fr.net>
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 <pspychalski@gmail.com>
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 <konstantin.sharlaimov@gmail.com>
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) <pspychalski@gmail.com>
Date:   Wed Jun 6 15:16:11 2018 +0200

    Cleanup of unused functions

commit e4b99ba
Author: Alberto García Hierro <alberto@garciahierro.com>
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 <fiam@rm-fr.net>
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 <fiam@rm-fr.net>
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 <pspychalski@gmail.com>
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) <pspychalski@gmail.com>
Date:   Wed Jun 6 08:50:54 2018 +0200

    Fixed build

commit 2b7b47e
Author: Pawel Spychalski (DzikuVx) <pspychalski@gmail.com>
Date:   Wed Jun 6 08:43:45 2018 +0200

    I2C rangefinder rename

commit 50847b9
Merge: 4374471 a63049d
Author: Alberto García Hierro <fiam@rm-fr.net>
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 García Hierro <alberto@garciahierro.com>
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 <konstantin.sharlaimov@gmail.com>
Date:   Sun Jun 3 20:15:57 2018 +0200

    Remove MIDRC setting

commit 4374471
Merge: 59864b8 dd86a0b
Author: Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
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 <konstantin.sharlaimov@gmail.com>
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 <konstantin.sharlaimov@gmail.com>
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 <konstantin.sharlaimov@gmail.com>
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 <konstantin.sharlaimov@gmail.com>
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 García Hierro <alberto@garciahierro.com>
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 <giacomo892@users.noreply.github.com>
Date:   Thu May 31 17:19:01 2018 +0200

    Disable arming in AH for FW unless launch mode is enabled

commit 05c5f0f
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Thu May 31 16:29:21 2018 +0200

    Extra safety for NAV_LAUNCH if enabled via BOX

commit dd86a0b
Author: Michel Pastor <shellixyz@gmail.com>
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 García Hierro <alberto@garciahierro.com>
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 <tim@leethost.com>
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 García Hierro <alberto@garciahierro.com>
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 García Hierro <alberto@garciahierro.com>
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 García Hierro <alberto@garciahierro.com>
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.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.