WIP: circle mode for multicopters

[dagar]

#4792 continued

TODO

• review controller changes
• how should stick input work in circle mode? Right now it's unintuitive.
• how should this work with QGC? It currently wants to switch to "Guided mode" first.
• user escape back to regular position control
• enable/disable with RC switch?

@aysh-shariff this is functional, but needs some work. I wouldn't test it outside of a simulator quite yet.
@Tumbili FYI

[Stifael]

I have a question about the general structure. I have also implemented a smoothing for waypoints in the position controller because with the current structure it is the most obvious thing to do (the feature is not in master). However, I did not really like to put that stuff into the position controller, since it just adds more complexity to that class. Therefore, I was wondering if we could try to avoid adding more and more features to the position controller, but rather start a new class or classes which are called before the position controller and map the inputs to desired position setpoints.
In my opinion shrinking the position controller (this actually applies to all PX4 apps and classes) would make it much easier to read the code and write tests for legacy code. Because right now it is very difficult to test anything because most methods are private and too complex and sometimes the main functionality of the class cannot be called since it is merged with the app thread.

[dagar]

That sounds good to me. These monster classes are hard to navigate.
As a first step should we finally do the stick mapper and get the manual input out of the controllers? #2822

[Stifael]

@dagar:
I think it would be great to have first tests for the class itself to make sure that if we move anything out of the class, everything still stays the same. Unless it is very obvious how to to move stuff outside. kind of funny that #2822 has been opened for such a long time. Those things are exactly what I would like to do, but I think #2822 shows that major changes to the current code base is very difficult to do because it very difficult to verify that nothing breaks without a good test base. I shared a document with you. it would be great if you can add suggestions to it

[aysh-shariff]

@dagar
I am not able to build the code after checking out pr/6306

I even tried make submodules clean and git clean -dfx . Nothing helps.

[LorenzMeier]

Since the continuous integration is passing here you did not check out this branch. Make sure git status is not showing any stale sub modules.

[aysh-shariff]

@LorenzMeier I cloned the latest master. Then ran the below commands before building the code.

git fetch origin pull/6306/head:pr-6306
git checkout pr-6306

malte@malte-Latitude-E6400:~/orbit/src/Firmware$ git status
On branch pr-6306
Changes not staged for commit:
(use "git add ..." to update what will be committed)
(use "git checkout -- ..." to discard changes in working directory)

modified:   Tools/sitl_gazebo (new commits)
modified:   src/lib/ecl (new commits)
modified:   src/modules/uavcan/libuavcan (new commits)

no changes added to commit (use "git add" and/or "git commit -a")
Am I missing something here?

[LorenzMeier]

Yep. That status output looks wrong. It should have catched it on building automatically, but what you need to do is git submodule update --recursive.

[aysh-shariff]

@LorenzMeier Thanx! Tat worked.
@dagar How can I test orbit mode in SITL ? I am using jmavsim SITL. It just gets into 'Hold' mode and does nothing.

[dagar]

Right now the orbit button in QGC switches to hold first. Do you know how to build QGC? Otherwise I could make this accessible via px4 command line.

[aysh-shariff]

@dagar Yes. I have been building qGC from source. But I'm not using the latest code. The orbit button does switch to hold. But what is the expected behaviour? It just seems to hold the altitude and nothing else.

[dagar]

@aysh-shariff I updated commander so you can switch to orbit from the px4 command line.

pxh> commander mode posctl_circle

I'll update QGC once the firmware side settles.
Any opinion on how sticks should work in this mode? You can can properties of the circle, or we move the center.

[dagar]

Do people want multicopter orbiting in a mission context? We could do it with loiters that have a radius set.

[aysh-shariff]

@dagar I couldn't test it. I fly the UAV in Mission mode and then switch to Circle from the command line. But the command gets rejected.
pxh> commander mode posctl_circle
WARN [commander] mode change failed

[dagar]

Did you update the branch? I amended the commit and rebased on master, so you might be out of sync.
You could fetch it again and reset.

git checkout pr-circleMode_rebased
git fetch --all
git reset --hard origin/pr-circleMode_rebased

make distclean

[aysh-shariff]

@dagar
It still doesn't work for me.

pxh> commander takeoff
pxh> INFO [commander] Taking off
INFO [tone_alarm] positive
INFO [commander] home: 47.3977421, 8.5455944, 488.00
INFO [tone_alarm] arming
INFO [logger] Start file log
INFO [logger] Opened log file: rootfs/fs/microsd/log/2017-01-16/12_55_45.ulg
INFO [commander] Takeoff detected
INFO [local_position_estimator] [lpe] land timeout

pxh>
pxh> commander mode posctl_circle
WARN [commander] mode change failed
pxh> shutdown
Shutting down
Restoring terminal
[100%] Built target jmavsim
malte@malte-Latitude-E6400:~/orbit/src/Firmware$ git branch
master

• pr-circleMode_rebased

[dagar]

I'm pretty sure it's your local branch not matching the upstream version. Did you try the reset described above? Or you could just do a fresh clone.

[aysh-shariff]

@dagar I did follow all the steps you mentioned.

[dagar]

@aysh-shariff so it seems to be a bit stubborn getting into position control circle mode the way I hacked the change via commander. Update the branch again and try asking twice.

commander mode posctl_circle
commander mode posctl_circle

It's still going to give the wrong error, but I can fix it later.

[aysh-shariff]

@dagar This command puts the drone in Position mode and causes it to land.
pxh> commander mode posctl_circle
WARN [commander] mode change failed
pxh> INFO [commander] Position mode
INFO [tone_alarm] positive
INFO [commander] Landing detected
INFO [local_position_estimator] [lpe] land init

[dagar]

Position control mode needs manual input (gamepad, onscreen sticks, etc) and the throttle needs to be in the middle to maintain altitude,

[aysh-shariff]

@dagar Ok. So this needs to be tested in HITL ?
I was trying pure software simulation till now!

I am having issues testing this in jmavsim HITL. Pixhawk led blinks purple and HITL fails.
Is there a way to test this in jmavsim SITL. I tried enabling virtual joysticks in qGC, but not sure how tat's supposed to work.

[dagar]

No, not necessarily. You just need manual control from some source. In SITL this could be QGC and a gamepad/joystick or the on screen thumbsticks.

[aysh-shariff]

@dagar I could finally test this.
The drone seems to circle with some predefined radius and holds the altitude if I don't touch any of the input sticks. By using Pitch, the radius seems to be increasing. The tests were not always consistent. I had to repeatedly run commander mode posctl_circle to initiate the orbiting.

I came across how Circle Mode works in Ardupilot. I guess it would be nice to be able to set the circle radius in qGC, and have min and max radius parameters . I am intending to use this mode when I use a camera on my UAV to get good 360° videos.

Also, will this mode be available only through qGC?
How long is the drone going to keep circling ?
Is it equivalent to putting the drone to Loiter but with some pre-defined radius?

[dagar]

I think the next piece that's missing for more testing is the QGC side.

[dagar]

@aysh-shariff I've resolved the conflicts and rebased.

I'll push a QGC test branch with better orbit support shortly and link to the binary.

[dagar]

Issues

• need to send circle mode twice to enter (first always goes to regular position control)
• doesn't cleanly enter the circle

@aysh-shariff QGC here https://s3-us-west-2.amazonaws.com/qgroundcontrol/builds/orbit/QGroundControl.AppImage
Allows you to click to set the orbit center. You'll have to enter position mode first or click orbit twice.

I don't really have good answers to some of the outstanding design issues. What should be controllable from RC and from QGC, should it work as an AUTO mode, what the sticks should do. If someone would like to take the lead on figuring out these parts I'm happy to help finish this off, but otherwise this isn't really solving a problem I have, so it's hard to know what's wanted.

[dagar]

Considering redoing this from scratch using ecl L1.

[deksprime]

@dagar
A suggestion to the outstanding design issues would be

On the RC/position control mode side

1. Yaw and altitude

• Yaw angle of the vehicle during circling may be determined by the RC transmitter's left stick's horizontal movements like normal position control mode,
• Altitude of the vehicle during circling may be determined by the RC transmitter's left stick's vertical movements like normal position control mode,
• While changing yaw and altitude, circling movement should stop for safety reasons and should continue only while the sticks are in the middle(or in the deadzone)

2. Radius and velocity

• Circle radius might be determined by the RC transmitter's right stick's vertical movements which is fairly intuitive
• I don't know whether it is really necessary to offer the option to control velocity (in tangential direction) since it would require the introduction of new parameters such as Minimum Circle Velocity and Maximum Circle Velocity (or it could just use 0 as the minimum velocity, MPC_XY_CRUISE as the default velocity and MPC_XY_VEL_MAX as the maximum velocity)

On the QGC/mission mode side

• I think one of the most useful functions would be circling around a certain waypoint with given latitude, longitude and altitude values. It can be added as a mission item to the waypoint file by hand or via QGC

MAV_CMD_NAV_LOITER_TURNS can be implemented as follows (I think it is not implemented on PX4 as a mission item correct me if I'm wrong)

Param 1 = Turns
Param 2 = Velocity maybe???
Param 3 = Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise
Param 4 = Desired yaw angle relative to the tangential velocity vector
Param 5 = Latitude of the point of interest
Param 6 = Longitude of the point of interest
Param 7 = Altitude of the point of interest

So there can be two separate sub-modes of position control, one being the mission item and one being the RC controlled version. But I think implementing the mission item would be much more useful given the fact that this is functionality which would be mostly used outdoors, and with GPS. Sorry if any of this sounds stupid or if anyone has already said all these, just trying to be useful :)

[dagar]

Thanks @deksprime, that's very helpful. I'm interested in implementing this, but as I don't actually use multicopters for filming I didn't want to create something useless.

My thought was if I structure the code properly we'll be able to get multicopter loiter with radius as part of missions for free.

I also haven't figured out how we can get into and leave position control submodes entirely from a transmitter.

[LorenzMeier]

We should discuss on the dev call. Rather than solving circle one-off I think we need a trajectory / task interface that allows to define curves and how sticks map to them.

[dagar]

Sounds good to me.

[superware]

Any plans to implement this in the most basic way? (e.g. center position + radius, forward flight)

[dagar]

Yes, this is going to be done within the new FlightTasks architecture. @MaEtUgR FYI

[MaEtUgR]

@dagar Yes thanks, I'm currently bringing it up to good usability level here: #9434
Message suggestion is being discussed here: mavlink/mavlink#924 (comment)