Improve trigger control for survey applications #11878

LorenzMeier · 2019-04-20T08:02:23Z

QGC enables the trigger now on entering and exiting each transect without impacting the flight controller.

Rebased version of this:

DonLakeFlyer · 2019-04-21T19:21:54Z

I tested using this QGC hacked branch which add condition gate logic to surveys: https://github.com/DonLakeFlyer/qgroundcontrol/tree/ConditionGateHack and the attached plan file.

ConditionGate.plan.txt

This is a simple survey with a single transect. The new code will add the following at the end of transect survey edge:

CONDITION_GATE
TRIGGER_DISTANCE stop
Single CAMERA_TRIGGER
WAYPOINT for survey edge
WAYPOINT to turnaround point

When I tested the vehicle flew to the gate and just stopped there. At that point I think it was stuck at the gate command. The single image capture did not fire. So I don't think it ever moved past that.

LorenzMeier · 2019-04-22T12:02:33Z

I need to build QGC and test end to end

julianoes · 2019-04-23T07:57:53Z

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -86,6 +86,8 @@ param set MAV_SYS_ID $((px4_instance+1))
 simulator_tcp_port=$((4560+px4_instance))
 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
+udp_onboard_payload_port_local=$((14558+px4_instance))


14558 is not consistent with the other port numbers. However, we need to rethink these specs anyway.

@Jaeyoung-Lim What pattern do you recommend here?

@Jaeyoung-Lim ping.

This will conflict with udp_offboard_port_local after 22 instances. Would it be possible to find another slot? Regarding patterns I think the first port is defined up to the decimal, to be consistent with the others

which port do you suggest?

The used ports are only from 18570 +0~255 and 14580+0~255 so anything outside of this...so maybe 14280 would do the trick? Just a question, what is the downside of using the udp_offboard_port_local port for the onboard payload? This will only be used for development (SITL) and I cannot imagine too many people trying to use both at the sametime

The camera is a different component than actual off board == mission management interfaces. You can't mix them.

src/drivers/camera_trigger/camera_trigger.cpp

src/drivers/camera_trigger/camera_trigger_params.c

src/modules/navigator/mission_block.cpp

julianoes · 2019-04-23T08:23:13Z

src/modules/navigator/mission_block.cpp

-	if (item.nav_cmd == NAV_CMD_DO_LAND_START) {
+	if (item_contains_position(item)
+	    || item_contains_gate(item)
+	    || item_contains_marker(item)) {


src/modules/navigator/mission_block.h

LorenzMeier · 2019-04-30T11:43:52Z

@DonLakeFlyer I will re-test with your QGC branch

LorenzMeier · 2019-05-05T12:28:00Z

@DonLakeFlyer I've tested with your QGC branch:

I ran QGC on this commit: DonLakeFlyer/qgroundcontrol@bac384a
I had to fix my test setup, it broke in the meantime, my bad. I've added a commit here on this PR now, you need to update and, you need to run SITL like this:
make px4_sitl gazebo_plane_cam
I get this output from SITL in debug mode, so it seems to work:

[Msg] Took picture: frames/DSC00028.jpg
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 263 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:262] Done with image capture
[Dbg] [gazebo_geotagged_images_plugin.cpp:569] Send capture status
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 262 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 0 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:448] Handle Start Capture
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 77 1 100 to: 127.0.0.1:14558
[Msg] Took picture: frames/DSC00029.jpg
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 263 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:262] Done with image capture
[Dbg] [gazebo_geotagged_images_plugin.cpp:569] Send capture status
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 262 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 0 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 0 1 100 to: 127.0.0.1:14550
[Dbg] [gazebo_geotagged_images_plugin.cpp:448] Handle Start Capture
[Dbg] [gazebo_geotagged_images_plugin.cpp:329] Message 77 1 100 to: 127.0.0.1:14558

I get the trigger messages in QGC, but they are not displayed on the map (which makes testing / debugging hard).

Could you look into this?

DonLakeFlyer · 2019-05-06T17:39:34Z

@LorenzMeier error: unknown target 'gazebo_plane_cam', did you mean 'gazebo_plane_gdb'?

LorenzMeier · 2019-05-06T18:23:41Z

Sorry, I pushed yesterday to the wrong branch name - fixed and pushed.

DonLakeFlyer · 2019-05-07T16:26:05Z

When I do a Start Mission it always sends back Unable to start, vehicle not ready?

LorenzMeier · 2019-05-07T16:30:03Z

You need a takeoff waypoint for a fixed wing

Antiheavy · 2019-05-07T16:37:07Z

You need a takeoff waypoint for a fixed wing

there is even a (optional) check for that now! :) #11531

DonLakeFlyer · 2019-05-07T16:40:26Z

You need a takeoff waypoint for a fixed wing

Not the problem. There is some sort of problem with either takeoff waypoint height or distance from home which ends up being reported as Unable to start instead of a better message. I'll try to get a repro.

DonLakeFlyer · 2019-05-07T16:41:46Z

Camera triggers have 0 lat/lon

LorenzMeier · 2019-05-18T21:34:23Z

@DonLakeFlyer working on it.

julianoes

Requesting changes so that my previous review doesn't get ignored and lost.

stale · 2019-09-11T06:16:07Z

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

Antiheavy · 2019-09-11T16:31:39Z

Still relevant and needed! ;-)

ROMFS/px4fmu_common/init.d-posix/1030_plane

ROMFS/px4fmu_common/init.d-posix/1031_plane_cam

julianoes · 2020-03-13T09:48:36Z

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -86,6 +86,8 @@ param set MAV_SYS_ID $((px4_instance+1))
 simulator_tcp_port=$((4560+px4_instance))
 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
+udp_onboard_payload_port_local=$((14558+px4_instance))


@Jaeyoung-Lim ping.

julianoes · 2020-03-13T09:53:49Z

src/drivers/camera_trigger/camera_trigger.cpp


-			// Check that distance threshold is exceeded
-			if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
+			if (!_one_shot) {


I don't understand this _one_shot flag. Is this some sort of mode? And if so, is it orthogonal to _trigger_enabled?

It is orthogonal because it tells the system to take a single picture and disengage right after. If you wouldn't architect it this way you would need to schedule a HRT call to disengage the trigger right after, opening the door to horrible race conditions.

src/drivers/camera_trigger/camera_trigger.cpp

src/modules/navigator/mission.cpp

julianoes · 2020-03-13T10:09:18Z

src/modules/navigator/mission.cpp

+	// If the mission item under evaluation contains a gate,
+	// then we need to check if we have a next position item so
+	// the controller can fly the correct line between the
+	// current and next setpoint
+	if (item_contains_gate(_mission_item)) {
+		if (has_after_next_position_item) {


I'm trying to understand why this logic is needed twice 🤔.

I didn't want to refactor it in parallel, but basically one area is concerned with current waypoints and the one with next. This whole function needs to be broken down accordingly to make it readable again.

Right, sadly that won't be done anytime soon.

src/modules/navigator/mission_block.cpp

src/modules/navigator/mission_feasibility_checker.cpp

Jaeyoung-Lim · 2020-03-13T23:07:08Z

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -86,6 +86,8 @@ param set MAV_SYS_ID $((px4_instance+1))
 simulator_tcp_port=$((4560+px4_instance))
 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
+udp_onboard_payload_port_local=$((14558+px4_instance))


This will conflict with udp_offboard_port_local after 22 instances. Would it be possible to find another slot? Regarding patterns I think the first port is defined up to the decimal, to be consistent with the others

…rthogonal to current trajectory

This enables the vehicle to be able to wait with executing the next item until a position has been passed.

… mission This enables the navigator to wait for a specific gate coordinate to pass orthogonally to the current trajectory. This is particularly helpful for payload / camera handling in missions and avoids a dependency of payload handling on navigation waypoints.

This allows to test plane survey missions with the camera trigger included in SITL.

This is to allow us to test triggering properly in SITL

Boolean logic improvement to avoid double assignment of true

A mission with zero elements is invalid but should not lead to a warning. Previously a fixed wing aircraft without a mission did emit warnings about a missing land item.

This helps to trace the code, no functional changes.

This helps to fly smaller / faster test missions.

Previously a zero-length mission that failed checks (e.g. because a mandatory element was not present) would lead to a warning.

This diff does not contain any functional changes but just makes the code more readable and adds comments.

The default for the gate check condition is to pass and this needed to be explicitly documented

This change simplifies the gate calculation.

The mission logic depends in a number of locations on being able to calculate the direction from one waypoint to another. Missions that have waypoints that are in the same physical location do not make sense and need to be rejected (the GCS / SDK generating them needs to be fixed). By enforcing this we can work with a reasonable and simpler state machine while executing the mission.

We have a plane_cam configuration for survey applications instead.

LorenzMeier · 2020-03-14T16:50:36Z

I've accounted for all the comments - merging.

moreba1 · 2020-09-04T12:57:15Z

I can not upload a mission with QGC when mav_cmd_condition_gate enabled.
@LorenzMeier

LorenzMeier requested a review from dagar April 20, 2019 08:02

LorenzMeier mentioned this pull request Apr 20, 2019

Improve trigger control for survey applications #7739

Closed

LorenzMeier force-pushed the pr-survey-trigger-control-rebased branch from 71745a5 to 51cbe78 Compare April 20, 2019 08:22

LorenzMeier requested a review from bkueng April 20, 2019 08:24

julianoes reviewed Apr 23, 2019

View reviewed changes

weekly-digest bot mentioned this pull request May 5, 2019

Weekly Digest (28 April, 2019 - 5 May, 2019) #11965

Closed

LorenzMeier force-pushed the pr-survey-trigger-control-rebased branch from 51cbe78 to 2fba83f Compare May 6, 2019 18:23

weekly-digest bot mentioned this pull request May 12, 2019

Weekly Digest (5 May, 2019 - 12 May, 2019) #12005

Closed

LorenzMeier force-pushed the pr-survey-trigger-control-rebased branch from 2fba83f to d58f77a Compare May 18, 2019 08:27

weekly-digest bot mentioned this pull request May 19, 2019

Weekly Digest (12 May, 2019 - 19 May, 2019) #12043

Closed

julianoes requested changes May 20, 2019

View reviewed changes

weekly-digest bot mentioned this pull request May 26, 2019

Weekly Digest (19 May, 2019 - 26 May, 2019) #12076

Closed

LorenzMeier force-pushed the pr-survey-trigger-control-rebased branch from d58f77a to fe0fac1 Compare June 13, 2019 05:16

weekly-digest bot mentioned this pull request Jun 16, 2019

Weekly Digest (9 June, 2019 - 16 June, 2019) #12290

Closed

stale bot added the Admin: Wont fix label Sep 11, 2019

stale bot removed the Admin: Wont fix label Sep 11, 2019

LorenzMeier removed request for dagar and bkueng March 13, 2020 09:46

julianoes requested changes Mar 13, 2020

View reviewed changes

Jaeyoung-Lim requested changes Mar 13, 2020

View reviewed changes

LorenzMeier added 18 commits March 14, 2020 16:41

Vehicle command: Add mission item to gate mission execution on line o…

57a71dd

…rthogonal to current trajectory

Camera trigger: Handle trigger distance handling better

ff2e723

MAVLink: Add support for new gate command

ee03f9b

This enables the vehicle to be able to wait with executing the next item until a position has been passed.

Update to latest mission API

820e511

POSIX SITL: Enable trigger control in plane model

5cc7ece

This allows to test plane survey missions with the camera trigger included in SITL.

Add dedicated survey camera plane model

257321c

This is to allow us to test triggering properly in SITL

Mission block: Style improvement

07f74de

Boolean logic improvement to avoid double assignment of true

Mission feasibility checker: Handle mission with zero elements silently

7635876

A mission with zero elements is invalid but should not lead to a warning. Previously a fixed wing aircraft without a mission did emit warnings about a missing land item.

Enable Gazebo mock optical flow

fe52679

Camera trigger: Improve code readability

7d8eab0

This helps to trace the code, no functional changes.

Gazebo plane model: Enable tighter turns

a06c42a

This helps to fly smaller / faster test missions.

Navigator: Warn only for non-zero missions

7804502

Previously a zero-length mission that failed checks (e.g. because a mandatory element was not present) would lead to a warning.

Navigator: Improve code readibility

0763436

This diff does not contain any functional changes but just makes the code more readable and adds comments.

Navigator: Improve documentation of mission block

2f9a8cd

The default for the gate check condition is to pass and this needed to be explicitly documented

Mission block: Gate math cleanup and checks

41639cf

This change simplifies the gate calculation.

SITL plane: Do not configure trigger

1eafb20

We have a plane_cam configuration for survey applications instead.

LorenzMeier force-pushed the pr-survey-trigger-control-rebased branch from fb94b9a to 1eafb20 Compare March 14, 2020 15:42

LorenzMeier merged commit 4f2d39c into master Mar 14, 2020

LorenzMeier deleted the pr-survey-trigger-control-rebased branch March 14, 2020 16:50

julianoes mentioned this pull request May 14, 2020

Incompatilibilty with MAVSDK missions #14904

Closed

Antiheavy mentioned this pull request Aug 9, 2020

Survey planing : images in turnarounds default setting mavlink/qgroundcontrol#8964

Closed

moreba1 mentioned this pull request Aug 9, 2020

Does px4 works correctly with MAV_CMD_CONDITION_GATE ? #15130

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Improve trigger control for survey applications #11878

Improve trigger control for survey applications #11878

LorenzMeier commented Apr 20, 2019

DonLakeFlyer commented Apr 21, 2019

LorenzMeier commented Apr 22, 2019

julianoes Apr 23, 2019

LorenzMeier Mar 2, 2020

julianoes Mar 13, 2020

Jaeyoung-Lim Mar 13, 2020

LorenzMeier Mar 13, 2020

Jaeyoung-Lim Mar 14, 2020

LorenzMeier Mar 14, 2020

julianoes Apr 23, 2019

LorenzMeier commented Apr 30, 2019

LorenzMeier commented May 5, 2019 •

edited

DonLakeFlyer commented May 6, 2019

LorenzMeier commented May 6, 2019

DonLakeFlyer commented May 7, 2019

LorenzMeier commented May 7, 2019

Antiheavy commented May 7, 2019

DonLakeFlyer commented May 7, 2019

DonLakeFlyer commented May 7, 2019

LorenzMeier commented May 18, 2019

julianoes left a comment

stale bot commented Sep 11, 2019

Antiheavy commented Sep 11, 2019

julianoes Mar 13, 2020

julianoes Mar 13, 2020

LorenzMeier Mar 13, 2020

julianoes Mar 13, 2020

LorenzMeier Mar 13, 2020

julianoes Mar 20, 2020

Jaeyoung-Lim Mar 13, 2020

LorenzMeier commented Mar 14, 2020

moreba1 commented Sep 4, 2020 •

edited

Navigation Menu

Improve trigger control for survey applications #11878

Improve trigger control for survey applications #11878

Conversation

LorenzMeier commented Apr 20, 2019

DonLakeFlyer commented Apr 21, 2019

LorenzMeier commented Apr 22, 2019

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

LorenzMeier commented Apr 30, 2019

LorenzMeier commented May 5, 2019 • edited

DonLakeFlyer commented May 6, 2019

LorenzMeier commented May 6, 2019

DonLakeFlyer commented May 7, 2019

LorenzMeier commented May 7, 2019

Antiheavy commented May 7, 2019

DonLakeFlyer commented May 7, 2019

DonLakeFlyer commented May 7, 2019

LorenzMeier commented May 18, 2019

julianoes left a comment

Choose a reason for hiding this comment

stale bot commented Sep 11, 2019

Antiheavy commented Sep 11, 2019

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

LorenzMeier commented Mar 14, 2020

moreba1 commented Sep 4, 2020 • edited

LorenzMeier commented May 5, 2019 •

edited

moreba1 commented Sep 4, 2020 •

edited