Pos-Vel-Attitude Controller for Guided Mode

ArduCopter
1

Hello everyone, I have a high level path planner in ROS that I would like to use on top of ArduCopter.
I know that I can communicate from my ROS node and Mavlink with mavros.

My high level planner spits out position, velocity, and attitude (represented as quaternions) trajectories.
There is SET_POSITION_TARGET_LOCAL_NED which allows me to send position and velocity setpoints. There is also SET_ATTITUDE_TARGET which allows me to send the quaternion setpoint.

However, I would like to send all three (pos, vel, and att), over Guided Mode. Is this currently possible?

Thank you!

2 Likes
2

yes it is. pos and att at least.

3

Thanks for replying @amilcarlucas can you point me to the documentation and/or code for it?
I cannot find it in the mode_guided.cpp file.

4

you need to read dronekit-python documentation, or mavros documentation. Not the ardupilot source code.

5

In SET_POSITION_TARGET_LOCAL_NED you’ve got yaw control, but if you look at the AC_PosControl design (https://ardupilot.org/dev/docs/code-overview-copter-poscontrol-and-navigation.html) it’s just a stack of PID controllers. I don’t think it’s able to handle some fancy paths from open loop planner. I think you should put some MPC controller in ROS to follow the global path and use only low level AC_AttitudeControl

6

I think mavros will allow me to send messages properly, but it still depends on how those messages (and the contents therein) get used by Ardupilot.

I want to see how Ardupilot uses the message contents to actually control to confirm if I am using the right controller mode for the job.

7

I don’t think it’s able to handle some fancy paths from open loop planner.

Yeah thinking a bit more about, you’re right. Sending pos and vel setpoints to Ardupilot won’t allow me to do fancy maneuvers. Maybe sending just attitude and thrust would be the way to go?

I actually currently have an MPC controller in ROS which outputs angular velocities and thrust.
Ideally I would like to have a mode where Ardupilot allows me to send angular velocities and thrust as setpoints.

Since I can’t do this, without adding my own controllers in the mode_guided.cpp file (maybe I should just do this), I am going one step “higher” and trying to send the corresponding pos, vel, att states based on my model.

I’ll continue looking deeper in the attitude control, and see how I can expose that functionality through mode_guided.cpp so that there is a connection between mavros and Ardupilot for that part of control.

8

Exactly! Current control is designed for missions of type fly straight from A to B, rotate, fly B to C
I even investigate that MPC topic recently and I think it’s doable (attitude + thrust) but I didn’t have time to finish it. Let us know what you get. In case you stuck I can dig in my notes

1 Like
9

BTW what framework for MPC are you using? If ACADO then I thought about implementing it directly in Ardupilot because it has fast C generated code

10

For sure, I’ll keep this thread updated with my progress. Once I start concretely working on it I’ll share a link to my fork of ardupilot. Wanna do some architecture design first.

I think right now, I’ll start with modifying mode_guided.cpp and slowly adding unraveling layers of API.

One of lowest levels would just be motor outputs, which is what the AttitudeControl outputs (I guess low level controllers that output moments would like this API)

One layer above that would be angular rates and thrust (which is what most “mid” level controllers would be interested in.

The layer above already exists with SET_ATTITUDE_TARGET

I’m afraid I can’t answer the MPC framework rn. But its too computational heavy to perform onboard so I need a companion computer.

11

I don’t think you have to change much in guided. Look into angle_control_run(), recently thrust control was added there and that is pretty much it. You’ve got there AC_AttitudeControl instance that is responsible for lower level pid control.

12

BTW do you plan static or dynamic object avoidance in your planners?

13

No object avoidance as of yet :frowning:
Btw I went searching through the issues and I found one that describes the body rate setpoints: https://github.com/ArduPilot/ardupilot/issues/14610

14

I’ve been using SET_ATTITUDE_TARGET in guided mode and although I can orient the craft where I intend, the position controller slowly drifts away even when no position change is commanded (yaw only attitude change). @acxz Have you experienced this?

1 Like
15

I actually haven’t had time to test SET_ATTITUDE_TARGET yet, I’ll probably get around to it during Thanksgiving week and let you know (no guarantees tho…)

17

Hi, any updates on your work?@acxz

18

Hi vdot, I have recently been testing SET_ATTITUDE_TARGET and the drone does fly away, at least when commanding a yaw attitude. Please see my post here for videos.