Copter: Add support for full attitude control by companion computer

[ajshank]

Copter modes GUIDED and GUIDED_NOGPS currently convert the setpoint thrust in the Mavlink SET_ATTITUDE_TARGET message to a climb rate GCS_Mavlink.cpp, Line 1094. A lot of offboard control relies on being able to command direct thrust (and attitude) values to the copter. Similar to this issue, but with the addition of thrust control.

I'm running a fork with a new mode (simply called "Computer Control", for lack of a better name!), that interprets the setpoint thrust directly as a thrust value (normalized between 0~1). Tested fine** in an indoor mo-cap environment to control an off-the-shelf frame with a px4-v2 board over SiK radios. A desktop computer generates target quaternions, thrust and body-rate yaw values (roll, pitch, thrust, yawrate), and sends them over Mavros. File with the new mode is here for reference.

I can create a pull request after adding in some checks and doing some clean-ups, if this could be a useful feature in general.

Platform [ ] All [ ] AntennaTracker [x] Copter [ ] Plane [ ] Rover [ ] Submarine

** The mode calls the set_throttle_out() function. In the actual flights, thrust seems to be affected by battery voltage (the target thrust values need to increase over time in order to hover at the same location). I'm trying to figure out the methods that do voltage compensation for thrust/throttle, something to the effect of apply_thrust_curve_and_volt_scaling() Line 101 (which is a protected member..). Or some alternative function call for thrust.

Apologies if a similar feature already exists elsewhere that I could not find.

[rmackay9]

@ajshank, thanks for this.

When adding support for the message, the thrust field in particular was a difficult choice because most users don't want to write their own altitude controller. instead of writing a whole new mode, I'd prefer if we can add some method so the caller can specify which they are providing. Worst case let's add a parameter to Copter to control this. We could hide it in the DEV_OPTIONS parameter perhaps.

[ajshank]

@rmackay9 yeah, having the caller specify this is a good idea. My first preference was also to avoid writing a new mode. However, I found two forms of conflicts there:

1. The existing modes GUIDED and GUIDED_NoGPS have their meanings, and wasn't sure if changing some underlying scheme would break existing workflows.
2. Mavlink seems to have no more room in its message definitions.

Adding a new mode was just easier in this sense. It would be ideal to use something like a bit field in the SET_ATTITUDE_TARGET message to affect this behavior (bit 4~6 are "reserved", not sure what exactly for). Or if there's some other way, I'd be happy to look into it. Were you thinking of creating a parameter that should be set in a groundstation?

[ajshank]

@rmackay9 Sorry for reposting on this thread - but could you offer a resolution to my second comment (**) in the original post? That is: applying battery voltage scaling for z controller's thrust. Because, directly passing in throttle values (0..1) results in a voltage-dependent behavior ..

[DaneSlattery]

@ajshank, has there been any progress on this PR? I'm in the same mind of being able to directly control thrust.

[ajshank]

@DaneSlattery I basically ended up making a custom mode, which is almost similar to GUIDED_NoGPS except that it handles raw thrust from mavlink's setpoint_attitude. Masking this behind a dev parameter seemed a little more confusing than switching into a mode (a personal preference, really), and doesn't break anyone's existing workflow. I maintain a fork which you are welcome to check out!

Two notes:

1. Using raw thrust [0..1] is a little tricky without battery compensation (which I haven't had the time to factor in yet). So while my vehicle hovers at roughly 0.12, as the voltage drops this will go up. Likewise, a different battery will lead to different values.
2. My fork is from December 2018, so (a) I've only tested mine on NuttX and not ChibiOS, and (b) if you want the newest features, you might try merging with master. I plan on doing this, but sorry I'm a little time-constrained for the next 2-3 weeks :x

[DaneSlattery]

@ajshank many thanks. I started flying recently in AltHold mode with a MaxBotix sensor. The ArduCopter altitude hold really is quite good, and while I, like you, wanted to have much finer altitude control (setting a height setpoint rather than a velocity setpoint), I really do feel like the built in height controller is quite good, and will stick with it.

As for battery compensation, I do think a PID or some linear controller will manage this. I'd say it is definitely safe to assume that if someone is flying with Guided_NoGPS or your custom mode, they will be comfortable with designing controllers around some custom hardware/companion computer/ROS. I had success previously with a PID being able to compensate for the decreasing battery voltage.

[ajshank]

@DaneSlattery I'm glad AltHold is great for you. I've normally had good luck with it too. Sadly, feeding external sensor data to Copter isn't always an option for me, and hence the need for thrust control.

I was afraid you'd say PID :) The only catch with that is how a large variety of state-space/model predictive control works well with a PD control (no integrator). If the Copter compensated for battery voltage, I could rely on thrust=0.2 to mean the same (or roughly the same) physical thrust throughout. But without that, the model will have bias/offset scalars that are unknown, and a PD loop will not compensate for it. Sure there are ways to continuously estimate these quantities online; I just prefer if the Copter exposed the functions for it (because it does that magic in STABILIZE mode).

Sorry - didn't mean to keep posting on this thread without having made more progress on it. Just thought someone might find the same motivation :)

[DaneSlattery]

@ajshank I only say PID because I'm lazy. Although without the Integrator term, you could eliminate the steady state error that would slowly but surely grow as your voltage drops, I think.

If you have a state space model of your system, you could design a controller (linear quadratic regulator) based on the voltage (given that you've modelled the effect of the voltage on the rotor torque). I suspect that the thrust-vs-battery curve is mostly linear, but as you say, the bias/offset are unknown. It would be nice if the internal Copter thrust compensation functionality was available, although I had no idea that STABILIZE mode did such magic.

By the way, do you use ROS/mavros?

[ajshank]

@DaneSlattery I am lazy too :) And as a result, the state-space model that I use for LQR does not account for voltage changes (and modeling that thrust+torque curve is not quite what I want to do at the moment). I ended up writing a state observer that captures these offsets and feeds them to the model - seems to work. Yes, all of it is ROS+mavros.

Note my first post on this thread - Copter artificially changes the throttle value such that center-stick is "roughly hover" even in STABILIZE mode. It is clever how it does that (think it uses voltage, and observes for reported climb_rate almost 0), but it slightly changes the response around the center-stick on offboard control (makes it damped). I did consider using this as a proxy for scaling the commanded throttle from GCS, but I haven't moved forward with that yet.

[amilcarlucas]

Any progress on the Pull request?

[BosM16]

@ajshank Thanks for sharing your fork! I'm just struggling with activating this custom mode. When I clone your fork and build it the same way as in the instructions for the 'standard' ArduPilot, the custom mode (which is named COMP_CTRL if I'm not mistaken?) is not available. In Mavproxy, the command mode COMP_CTRL simply returns Unknown mode COMP_CTRL:

Is there something specific I need to pay attention to while building? Or is there a different way to activate this custom mode? (Running sitl in Ubuntu 18.04)

[BosM16]

@ajshank Thanks for sharing your fork! I'm just struggling with activating this custom mode. When I clone your fork and build it the same way as in the instructions for the 'standard' ArduPilot, the custom mode (which is named COMP_CTRL if I'm not mistaken?) is not available. In Mavproxy, the command mode COMP_CTRL simply returns Unknown mode COMP_CTRL:

Is there something specific I need to pay attention to while building? Or is there a different way to activate this custom mode? (Running sitl in Ubuntu 18.04)

I found that calling the mode by its number (in this case 25) does activate it. It works properly for me now!

[ajshank]

@BosMathias Thanks for sharing that method - I typically don't use MavProxy so I've not had the chance to look in there (I usually affect mode changes through pilot/RC switch). I wonder if MavProxy uses the 4 letter code for COMP_CTRL? In which case it should be CCTL.

@amilcarlucas Nothing on the PR yet, although I'm still happy to work on it if there's some interest in it. We've been using the fork extensively for over a year, and it has worked great so far - albeit with the caveats I mentioned prior in this thread. I still prefer having a different mode for this behavior (adds clarity), but as @rmackay9 pointed out, it might be overkill for general users.

[BosM16]

@ajshank mode CCTL returns the "Unknown mode" message as well. Seems like if you want to use a name rather than a number it's not enough to have the name in ArduPilot, but you should add it to MavProxy in some way such that he recognizes it as a command. Anyway name or number doesn't really matter to me personally, as long as it works :wink:

[rmackay9]

I've created a PR which might resolve this issue. I've added a new DEV_OPTIONS bit to allow changing how the thrust field is interpreted. https://github.com/ArduPilot/ardupilot/pull/14918. Hope this helps.

[ajshank]

@rmackay9 This is fantastic, thanks! A couple of things I've not managed to comment here:

1. I've noticed that multiplying with lift_max is usually a better approach than just passing the thrust directly to set_throttle_out(). This helps with the problem of a dropping throttle with a drop in battery voltage.
2. I've looked over this PR, but I can't recall if this helps in the Guided_NoGPS mode as well..

[lthall]

@ajshank

I've noticed that multiplying with lift_max is usually a better approach than just passing the thrust directly to set_throttle_out(). This helps with the problem of a dropping throttle with a drop in battery voltage.

We do this in the motor mixer so any throttle input is scaled to achieve the same thrust. This is why hover throttle does not vary significantly over battery voltage. So this PR is already doing that.

[ajshank]

@lthall I'm a little confused by this, because AP_Motors does seem to indicate so. But I've maintained and extensively used a fork that addresses the contents of this PR for about a year and a half (mentioned earlier in this thread). I've tested literally hundreds of flights in motion-capture rooms, and pre-multiplying by 1.0/lift_max is the only way I get the same physical thrust from, say, set_throttle_out( 0.2 ), for instance. The state-space-model outer-loop controller we use has no compensation for thrust scaling, so the effect is plainly visible.

There's a possibility that I might be missing some parameter/setting, or something else. Could there be something in the logs that could show the commanded raw vs the mixer's scaled thrust?

[malfarizit]

Hello. I'm try using SET_ATTITUDE_TARGET in while loop, it's like : while 1: attitude_target(roll_angle= 1, duration 1) ....

But, this message only run once time, anyone knows? Thanks.