Add simple FSM support to ORBIT_EXECUTION_STATUS message in common.xml

[tahsinkose]

This is a simple feature extension on orbit mode that allows the implementation of a very simple finite state machine structure in autopilot software. Normally orbit executes indefinitely, however the use-case I'm currently working on requires a finite orbit behavior for a certain amount of angles.

Following is a diagram of the added states when the orbit is requested to happen only for 360 degrees.

Based on use-cases, this state machine can be populated with more fine-grained states. Nevertheless this alone enables many capabilities.

[hamishwillee]

So the ORBIT_EXECUTION_STATUS is only sent out while the orbit is in progress. So this will be sent out at most one time (and of course the recipient might miss it). Is the inference that ORBIT_EXECUTION_STATUS will always be sent out at the point it leaves the orbit? IF so the protocol would need to state this.

[tahsinkose]

So the ORBIT_EXECUTION_STATUS is only sent out while the orbit is in progress

Yes.

So this will be sent out at most one time (and of course the recipient might miss it). Is the inference that ORBIT_EXECUTION_STATUS will always be sent out at the point it leaves the orbit?

A proper implementation should send telemetry of orbit status as long as the vehicle is conducting orbit action. Therefore it will not be sent out at most one time. The vehicle should start hovering on wherever it is once the orbiting action finishes, i.e. in ORBIT_EXECUTION_STATE_FINISHED, but orbit flight task would be still active. Between FINISHED and flight task update time window, telemetry is still kept published with the state info.

[hamishwillee]

@tahsinkose The problem is that there is no such thing/concept as the orbit "finishing" - at least not related to these messages or PX4 (and I am pretty sure ArduPilot too). The only concept I know in mavlink of an orbit finishing is the loiter command for a fixed wing vehicle, which will finish after either time or a certain number of turns.

What we have is two WIP messages:

• MAV_CMD_DO_ORBIT - This starts the orbit. There is no mechanism to stop the orbit. This does not mention ORBIT_EXECUTION_STATUS
• ORBIT_EXECUTION_STATUS - says it is emitted while orbit is happening.

I would expect ORBIT_EXECUTION_STATUS to stop being emitted when you stop orbiting. When that happens would depend on how the orbit is implemented in a particular flight stack - so if the flight stack supports the idea of loitering after one turn (say) then yes, perhaps what you are describing might happen. But that's not how PX4 orbit mode works (for example) and we need to try think of messages that are agnostic of a particular stack.

Thoughts?

[tahsinkose]

@hamishwillee PX4 orbit mode is a time-uncapped flight mode that would execute as long as autopilot is not requested to transition into another flight mode. It doesn't prevent a stateful finite orbit flight mode to be added such as FlightTaskFiniteOrbit. Actually this is the name of flight task I'm currently using in my project and this extension on message makes both flight tasks (FlightTaskOrbit and FlightTaskFiniteOrbit) gracefully work without any interference.

I can't see how these messages are not stack-agnostic. The particular flight modes that use this message who don't care (or unable to reflect) about the state of orbit can simply neglect those fields.

[hamishwillee]

This is addition of a state enum to ORBIT_EXECUTION_STATUS to indicate whether you're approaching or "in" the orbit, from the perspective of the flight stack. The use case is to make it easier for a ROS system (linked PR) to work out the point at which the orbit started, in order to choose an exit angle.

Any opinions? I'm not opposed in that a GCS can now also know the autopilot thinks it is circling, rather than just guessing based on it overlaying the planned path.

[tahsinkose]

@hamishwillee any updates on this?

[hamishwillee]

@tahsinkose Sorry for delay - I typically address the queue on Weds/Thurs every week but am sporadic between.

I added a comment and will discuss in the dev call (feel free to attend - it is in about 5 hours from now - see dronecode calendar).

We should probably remove the WIP on these messages and add info about ORBIT_EXECUTION_STATUS to the orbit command. I THINK this has to be an extension field too - depends on whether there is a plan in place to allow breaking the message?

[tahsinkose]

@hamishwillee thanks for the due diligence. Unfortunately I can't commit to removing WIP on the messages yet (not before Sept 2021). Nevertheless this can wait until then - or someone else finishes WIP statuses on them and this would become ready for free.

[hamishwillee]

Hi @tahsinkose ,

You could already achieve what (we think) you want using the existing messages. Simply set a target on the point on the radius that you want to enter, fly there, start orbit, then track the position on the radius. Then send new command when you reach the point on the radius you want to exit. This way you know the starting point and end point because you set rather than get them.

We had a look at the command and message in the dev call. The general feeling was that perhaps

• there may be better ways of achieving what you want than exposing this state.
• the messages are a bit limited, and while this might address your case, they aren't a good generalization of the constraints that we might want to set on an orbit.
• We don't really understand your use case well enough to provide a good design. Can you please provide more background on the flight you envisage. Is this for use in a mission or is it command driven. How is it planned? Where would you be coming from or going to - does the path matter?

I don't want to go into much detail on command design without a better view on your use case, but our thinking was:

• Desirable to command orbit forever, for time, for number of orbits (in the same way as for plane loiter commands (MAV_CMD_NAV_LOITER_UNLIM, MAV_CMD_NAV_LOITER_TURNS, MAV_CMD_NAV_LOITER_TIME).
• We might want to specify that number of turns in radians to allow it to exit after a particular angle of rotation - which is what you wanted.
• We might further want to command the exit path https://mavlink.io/en/services/mission.html#mission-command-detail or an entry path.
• An orbit in a mission would progress to the next waypoint on completion. An orbit sent as a command might simply loiter on completion. Which is where your state would potentially be useful to emit.
• For mission estimation it is better to separate your commands for getting to the orbit location and your commands for doing the orbit itself [This was someone elses point - I don't understand why].
• For a smart shot, usually you want to fly to orbit location, do the orbit, then fly back to original position where orbit shot was done.
• Desirable to be able to specify centre, radius, direction and speed of rotation, direction of vehicle yaw, direction of yaw on exit in a mission.

Anything else?

To progress this can you outline a little more about your setup. It would help to have a discussion at the dev call. I know that might be hard for you - is there a good time that overlaps with Zurich work hours? If so, then I can arrange a discussion with someone in the team.

[tahsinkose]

Hey @hamishwillee.

You could already achieve what (we think) you want using the existing messages. Simply set a target on the point on the radius that you want to enter, fly there, start orbit, then track the position on the radius. Then send new command when you reach the point on the radius you want to exit. This way you know the starting point and end point because you set rather than get them.

This is still limited for orbits that require more than 1 tour, e.g. orbit of 720 degree. Besides it relies on a perfect localization which is not the case for my application, i.e. a minor localization jump could cause a possible if-check to miss that the robot was on the orbit. I actually faced this issue with PX4 and solved it by projecting the current position of the robot to orbit circle in flight task.

I'm not sure this can be achieved with the same precision by using the telemetry published over ROS. Even if it could be done, I like the idea of deferring the internals to flight task and monitoring orbit FSM until it says "Finished".

there may be better ways of achieving what you want than exposing this state.

I can't see why it would be bad to expose this state. Telemetry itself is already exposed.

the messages are a bit limited, and while this might address your case, they aren't a good generalization of the constraints that we might want to set on an orbit.

I'm all ears for the suggestions on additional states one might be interested in orbit behavior 🙂 Although I don't agree that they aren't a good generalization. They are few in numbers, but still comprehensive in the sense that all discrete states are exposed. For instance, I'm using 3 additional states on my local PX4 clone that are not exposed to this telemetry message, since they are fairly application-specific.

We don't really understand your use case well enough to provide a good design. Can you please provide more background on the flight you envisage.

I solved my use-case already with an adequate design. Let's don't waste time on solving it in better ways, because it is irrelevant in the sense that MAVLink format specification should be autopilot-agnostic. Therefore we should deliberately constrain ourselves to the particular question: Is it meaningful to have stateful orbit behavior?

Is this for use in a mission or is it command driven. How is it planned?

It is command driven.

How is it planned? Where would you be coming from or going to - does the path matter?

Exactly as in FlightTaskOrbit of PX4.

---

OK most of your bullet points consider also the fixed wing frames, which makes it naturally more convoluted than the simple extension this PR is aiming to bring in for rotary wings. There is one thing though:

We might want to specify that number of turns in radians to allow it to exit after a particular angle of rotation - which is what you wanted.

Yes this. However [APPROACHING,STARTED,FINISHED] state list is still enough for rendering it. Also it requires extension on MAV_CMD_DO_ORBIT rather than ORBIT_EXECUTION_STATUS.

For a smart shot, usually you want to fly to orbit location, do the orbit, then fly back to original position where orbit shot was done.

This is invalid when you specify the exact orbit distance in radians. This is extremely advanced when compared to what I'm aspiring to bring in and therefore should be avoided until an actual use-case arrives.

An orbit in a mission would progress to the next waypoint on completion. An orbit sent as a command might simply loiter on completion. Which is where your state would potentially be useful to emit.

I believe it would be useful to emit in both cases. Otherwise how would the mission executor know that the orbit has finished in former case?

We might further want to command the exit path https://mavlink.io/en/services/mission.html#mission-command-detail or an entry path.
Desirable to be able to specify centre, radius, direction and speed of rotation, direction of vehicle yaw, direction of yaw on exit in a mission.

These seem like fixed-wing considerations to me. If that is the case, let's constrain this only for rotary wings where loiter on position is achievable.

For mission estimation it is better to separate your commands for getting to the orbit location and your commands for doing the orbit itself [This was someone elses point - I don't understand why].

This is autopilot, even user-code impl. details and has nothing to do with MAVLink message contents right? Again, please don't try to come up with a good overall design for my problem that has to inevitably involve autopilot/user code, which is much more than the content of ORBIT_EXECUTION_STATUS.

For a last but not least argument for discouraging the consideration of impl. details, please do consider a spiral orbit behavior wherein the altitude gradually increases in the consecutive stages of orbit arc. This extension still supports it in current status 🙂

To progress this can you outline a little more about your setup. It would help to have a discussion at the dev call. I know that might be hard for you - is there a good time that overlaps with Zurich work hours? If so, then I can arrange a discussion with someone in the team.

It is really tentative, but I can attend a meeting of 1-2 hrs in May 2 between CEST 12.00-5 PM 🤔

[hamishwillee]

It is really tentative, but I can attend a meeting of 1-2 hrs in May 2 between CEST 12.00-5 PM 🤔

Sorry, I missed your response!

@julianoes Can you liase with @tahsinkose to discuss the needs/requirements here?

[MaEtUgR]

Good idea.

In the minimum, we should clearly define finished as one turn. Optionally we support commanding multiple turns and reporting a progress. I think that would make it much more flexible.

What do you think? If we see the use case I think we should extend right away.

[hamishwillee]

In the minimum, we should clearly define finished as one turn. Optionally we support commanding multiple turns and reporting a progress. I think that would make it much more flexible.

@MaEtUgR So it depends a bit on what happens after finishing. Right now I think this is a command and the orbit basically goes on forever. So finished would mean "I got another command that made me leave this mode". There isn't value unless you can be in the mode and not exiting.

In that case I see it as useful, but again, it would mean "we stopped orbiting". I wouldn't base that on complete turns since we might want to exit on partial turns - and we'd still be finished. Hope that makes sense. I plan on looking at the responses on this tomorrow. But I really think you guys should all have a meeting to nut out the different use cases we'd like an orbit to address.

[tahsinkose]

@MaEtUgR @hamishwillee Here is a video in Gazebo where I command the robot to orbit for 3pi around each plant. As soon as 3pi radian is achieved, orbit execution state machine transitions into FINISHED, effectively causing the robot to hover at its current pose. Since my ROS application subscribes to orbit execution status telemetry topic (which I added in mavros with mavlink/mavros#1569), it commands the robot to start the next orbit if there are more plants to monitor, otherwise it commands "Return Home".

In the minimum, we should clearly define finished as one turn

I can't see any particular reason for such a constraint. I currently hardcode 3pi radians to my custom flight task implementation, but that can be easily made an argument. One can even specify, e.g. pi/9 to have a small arched motion around a center point for just showing off 🙂

[hamishwillee]

TYI I turned this into a uint8 - much smaller and granularity of 1 is fine for %

[hamishwillee]

Thanks @tahsinkose . Looking good. There are probably a couple of other minor things I should do to it (E.g. allow default value for radius, velocity etc if called when in already in orbit). Will discuss with dev call tonight and hopefully merge tomorrow.

[hamishwillee]

@tahsinkose We had a discussion in the dev call on this. There was concern that the progress update is something we need to have more generically - ie all commands should report their progress in some way. As a result I have pulled that out of this request (and will submit attempt at generic message shortly in a separate PR).

We are broadly agreed on the turns addition though. I will do some other minor fixes on this and merge (as it is still WIP).

This is what I removed.

    <enum name="ORBIT_EXECUTION_STATE">
      <description>State of execution during orbit flight.</description>
      <entry value="0" name="ORBIT_EXECUTION_STATE_UNKNOWN">
        <description>Vehicle state is unknown according to orbit action.</description>
      </entry>
      <entry value="1" name="ORBIT_EXECUTION_STATE_APPROACH_TO_CIRCLE">
        <description>Vehicle is approaching to orbit track.</description>
      </entry>
      <entry value="2" name="ORBIT_EXECUTION_STATE_STARTED">
        <description>Vehicle started following orbit track.</description>
      </entry>
      <entry value="3" name="ORBIT_EXECUTION_STATE_FINISHED">
        <description>Vehicle finished following orbit track.</description>
      </entry>
    </enum>

      <field type="uint8_t" name="state" enum="ORBIT_EXECUTION_STATE">State of execution during the orbit action.</field>
      <field type="uint8_t" units="%" name="progress">Progress of orbit. UINT_MAX8 if orbit is indefinite.</field>

[tahsinkose]

Orbit execution status is already a published telemetry info. How the new generic method will apply to that then without the explicit state field?

[hamishwillee]

@tahsinkose That is a very good question - or at least as I am interpreting it "Should the new mesage also send positional information, if present". If not, what do you mean? The new message will has state information in it.

[tahsinkose]

Your interpretation seems correct AFAIU. I like discussing over real examples though. Consider https://mavlink.io/en/messages/common.html#ORBIT_EXECUTION_STATUS. My proposal was to add state field in it so that it can be published over MAVLink in the context of ORBIT_EXECUTION_STATUS. But you have deleted it for the sake of COMMAND_PROGRESS message. I suppose, this COMMAND_PROGRESS message will be a field of ORBIT_EXECUTION_STATUS. Otherwise how COMMAND_PROGRESS should be published?

[hamishwillee]

@tahsinkose The COMMAND_PROGRESS is still in discussion (as you know) and I hope we can answer those questions in that PR.

I felt it useful to merge this because it was the part of the command we were generally in agreement on. It is possible that the COMMAND_PROGRESS doesn't work as a general command and we end up with a new PR on ORBIT_EXECUTION_STATUS.

Just FMI, if you know you can command a particular arc what do you actually need in the way of additional information for your use case? i.e. you no longer need the entry and exit status to calculate your arc. Perhaps you need to know when the arc is finished to send the next command?