[WIP] FW navigation first order hold move to position controller #8883

dagar · 2018-02-14T03:02:22Z

Navigator mission first order hold (FOH) altitude setpoint was limited to non-rotary wing missions within navigator. Given those restrictions it's a much better fit within the fixed wing position controller.

fixes Yaw timeout in missions doesn't work anymore #6758
fixes NAV_CMD_LOITER_TIME_LIMIT never reaches when altitude is different from last waypoint #8852
fixes loiter to alt for MC
fixes FOH changing altitude during FW loiter within mission

dagar · 2018-02-14T06:52:30Z

TODO: unify FW position + loiter handling in position controller

loiter to climb or descend
position for large xy (FOH altitude)

dagar · 2018-04-03T22:00:48Z

@Stifael this kills off the navigator FOH entirely.

The MissionBlock change is rather large. Additional review would be greatly appreciated.

Stifael · 2018-04-04T07:33:02Z

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

+					     _global_pos.lat, _global_pos.lon, _global_pos.alt,
+					     &dist_xy, &dist_z);
+
+			/* close to waypoint, but altitude error greater than twice acceptance */


Because I just recently started to change comments type in another PR, I was wondering why you choose here /* but above you chose the if // -comment?

It's a carry over from moving the old code. I use c++ style // for any new comments.

Stifael · 2018-04-04T07:35:08Z

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

+			if ((dist >= 0.0f)
+			    && (dist_z > 2 * _parameters.climbout_diff)
+			    && (dist_xy < 2 * pos_sp_curr.acceptance_radius)) {
+


why two times the acceptance radius?

Enough distance to cleanly enter the loiter radius. It's a bit arbitrary.

@tstastny might be cleaner to use L1 distance here?

yes I would. This is anyway when the controller would start commanding a turn.

Stifael · 2018-04-04T07:40:00Z

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

+					const float d_current = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, curr_pos(0), curr_pos(1));
+
+					/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
+					 * min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */


What do you mean with

Save distance to waypoint if it is the smallest ever achieved

min_current_sp_distance _xy is always d_current as long as the distance is below distance_current_previous

It's possible for a plane to miss waypoint acceptance and have to turn around and come back.

Stifael · 2018-04-04T07:45:07Z

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

+					if (min_current_sp_distance_xy > pos_sp_curr.acceptance_radius) {
+
+						/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
+						 * of the mission item as it is used to check if the mission item is reached


I assume this comment is a leftover from the navigator, since mission_item does not exist in the position-controller.

Stifael · 2018-04-04T07:57:04Z

src/modules/navigator/mission.cpp

@@ -198,10 +198,6 @@ Mission::on_active()
 			set_mission_items();
 		}

-	} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {


~~I assume this param_altmode parameter is now not used anymore~~

Param no longer exists

Stifael · 2018-04-04T07:58:55Z

src/modules/navigator/mission.cpp

@@ -1058,14 +1032,6 @@ Mission::heading_sp_update()
 					_mission_item.yaw = _wrap_pi(yaw + M_PI_F);
 					pos_sp_triplet->current.yaw = _mission_item.yaw;

-				} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT


whay is this one gone?

I was assuming/anticipating you handling it entirely in the MC position controller. This was done before #9146 and needs review.

The ROI portion should have been kept.

Stifael · 2018-04-04T08:14:06Z

src/modules/navigator/mission_block.cpp

-					// set required yaw from bearing to the next mission item
-					if (_mission_item.force_heading) {
-						struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next;
+	if (!_navigator->get_vstatus()->is_rotary_wing) {


i think it should be without negation

Yes, good catch.

Stifael · 2018-04-04T08:22:06Z

src/modules/navigator/mission_block.cpp


+				if (fabsf(yaw_err) > math::radians(_navigator->get_yaw_threshold())) {
+					success = false;


The success and yaw_required boolean are a bit confusing to me.
Should not first be checked that yaw is required, and if required (https://github.com/PX4/Firmware/pull/8883/files#diff-7a824e3d1e9a5420071c8c9bbe27b8e7R353) it then needs to be checked for success?

dagar · 2018-04-04T13:57:43Z

Testing required

fixed wing mission testing
- TAKEOFF
- WAYPOINT
- LOITER (timed, unlimited, to alt)
  - timed acceptance or successful timeout
- test changing the active waypoint (especially back to takeoff, or interrupting a loiter and coming back to it)
- loiter acceptance/exit when heading is aligned
multicopter
- LOITER (timed, unlimited, to alt)
- anything with yaw acceptance
- yaw acceptance timeout

Antiheavy · 2018-04-04T16:56:53Z

@dagar would addressing the min pitch angle on FW takeoff as a PX4 parameter (instead of a MAVlink param) be appropriate for this PR. see issue here: #6787

Antiheavy · 2018-04-07T23:36:26Z

I just started SITL testing this on FW. ~~So far haven't noticed anything bad, but I have a lot more testing to do.~~ edit: i spoke too soon. It's having a very hard time with trying to achieve a large altitude change with two waypoints that are close together:

Question: is there such a thing as tangent exit on time based loiters? I'm guessing not because that wouldn't make sense anyway.

dagar · 2018-04-09T15:36:27Z

@Antiheavy thanks for the thorough test. We need to think about how the switch between regular waypoint navigation and achieve waypoint altitude via loiter (once arriving) is handled. It might need to latch between one mode or the other a little more carefully.

dagar · 2018-07-01T19:28:50Z

I'd like to pick this up again, starting by reviewing actual FW requirements.

@philipoe

dagar · 2020-08-25T15:10:53Z

@RomanBapst @sfuhrer let's take another look at this one.

sfuhrer · 2020-12-11T10:05:39Z

@dagar I guess we can close this one since #15677 is in?

LorenzMeier · 2021-01-10T17:52:15Z

Closing as stale.

dagar · 2021-01-10T17:53:39Z

This was finally completed in #15677.

dagar added the Fixed Wing 🛩️ label Feb 14, 2018

dagar force-pushed the pr-nav_foh_loiter branch from 92dd5c0 to fb5f092 Compare February 14, 2018 04:06

dagar changed the title ~~FW navigation first order hold move to position controller~~ [WIP] FW navigation first order hold move to position controller Feb 14, 2018

dagar force-pushed the pr-nav_foh_loiter branch 3 times, most recently from 973ed76 to fc98262 Compare February 17, 2018 16:18

dagar force-pushed the pr-nav_foh_loiter branch from fc98262 to 53955b3 Compare March 7, 2018 01:20

This was referenced Mar 20, 2018

mission yaw #9114

Open

Waypoint time inside #9171

Closed

dagar force-pushed the pr-nav_foh_loiter branch from 53955b3 to 42619d8 Compare April 3, 2018 21:59

dagar added bug Admin: Enhancement (improvement) 💡 labels Apr 3, 2018

dagar requested review from RomanBapst and Stifael April 3, 2018 21:59

Stifael reviewed Apr 4, 2018

View reviewed changes

dagar force-pushed the pr-nav_foh_loiter branch from 42619d8 to 07947c3 Compare April 4, 2018 13:45

dagar force-pushed the pr-nav_foh_loiter branch 2 times, most recently from 93ba6f1 to 953c4e9 Compare April 4, 2018 22:03

dagar force-pushed the pr-nav_foh_loiter branch from 953c4e9 to 4b57be8 Compare May 23, 2018 01:11

dagar force-pushed the pr-nav_foh_loiter branch from 4b57be8 to b24801f Compare July 1, 2018 19:26

dagar force-pushed the pr-nav_foh_loiter branch from b24801f to 990d394 Compare July 1, 2018 19:32

dagar mentioned this pull request Aug 30, 2018

**BUG** Timed Loiter running indefinitely #10376

Closed

dagar added 2 commits September 10, 2018 15:29

Navigator MissionBlock cast double conversion

4ded523

fw_pos_control_l1 cast double conversion

885ee4f

dagar force-pushed the pr-nav_foh_loiter branch from 8c92f1a to 885ee4f Compare September 10, 2018 19:33

FW restore FOH min distance latch and cleanup

54ad73d

dagar force-pushed the pr-nav_foh_loiter branch from 40b47d5 to 54ad73d Compare September 10, 2018 20:26

dagar mentioned this pull request Oct 16, 2018

FW altitude control overshoot #10495

Closed

dagar mentioned this pull request Dec 18, 2018

Fix to first order altitude hold gradient calculation #11063

Closed

stale bot added the Admin: Wont fix label Jan 20, 2019

weekly-digest bot mentioned this pull request Jan 27, 2019

Weekly Digest (20 January, 2019 - 27 January, 2019) #11307

Closed

stale bot closed this Feb 4, 2019

dagar reopened this Aug 25, 2020

PX4 deleted a comment from stale bot Aug 25, 2020

dagar added Hybrid VTOL 🛩️🚁 Multirotor + Fixedwing! and removed Admin: Wont fix labels Aug 25, 2020

dagar assigned dagar, RomanBapst and sfuhrer Aug 25, 2020

dagar added this to the Release v1.12.0 milestone Aug 25, 2020

dagar mentioned this pull request Sep 2, 2020

FW navigation first order hold move to position controller #15677

Merged

LorenzMeier closed this Jan 10, 2021

LorenzMeier deleted the pr-nav_foh_loiter branch January 10, 2021 17:52

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP] FW navigation first order hold move to position controller #8883

[WIP] FW navigation first order hold move to position controller #8883

dagar commented Feb 14, 2018 •

edited by AuterionWrikeBot

Loading

dagar commented Feb 14, 2018 •

edited

Loading

dagar commented Apr 3, 2018

Stifael Apr 4, 2018

dagar Apr 4, 2018

Stifael Apr 4, 2018

dagar Apr 4, 2018

tstastny Apr 9, 2018

Stifael Apr 4, 2018

dagar Apr 4, 2018

Stifael Apr 4, 2018

Stifael Apr 4, 2018

Stifael Apr 4, 2018

dagar Apr 4, 2018

dagar Apr 4, 2018

Stifael Apr 4, 2018

dagar Apr 4, 2018

Stifael Apr 4, 2018

dagar commented Apr 4, 2018 •

edited

Loading

Antiheavy commented Apr 4, 2018

Antiheavy commented Apr 7, 2018 •

edited

Loading

dagar commented Apr 9, 2018 •

edited

Loading

dagar commented Jul 1, 2018

dagar commented Aug 25, 2020

sfuhrer commented Dec 11, 2020

LorenzMeier commented Jan 10, 2021

dagar commented Jan 10, 2021


		if (fabsf(yaw_err) > math::radians(_navigator->get_yaw_threshold())) {
		success = false;

[WIP] FW navigation first order hold move to position controller #8883

[WIP] FW navigation first order hold move to position controller #8883

Conversation

dagar commented Feb 14, 2018 • edited by AuterionWrikeBot Loading

dagar commented Feb 14, 2018 • edited Loading

dagar commented Apr 3, 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

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

dagar commented Apr 4, 2018 • edited Loading

Antiheavy commented Apr 4, 2018

Antiheavy commented Apr 7, 2018 • edited Loading

dagar commented Apr 9, 2018 • edited Loading

dagar commented Jul 1, 2018

dagar commented Aug 25, 2020

sfuhrer commented Dec 11, 2020

LorenzMeier commented Jan 10, 2021

dagar commented Jan 10, 2021

dagar commented Feb 14, 2018 •

edited by AuterionWrikeBot

Loading

dagar commented Feb 14, 2018 •

edited

Loading

dagar commented Apr 4, 2018 •

edited

Loading

Antiheavy commented Apr 7, 2018 •

edited

Loading

dagar commented Apr 9, 2018 •

edited

Loading