how to hold height while controling attitude in offboard mode?

[shupx]

I use "mavros" to control px4, and I wonder if there is a way that I can control drone's attitude(roll,pitch) while holding height? I know there are ROS topics named "mavros/setpoint_attitude" and "mavros/setpoint_raw/local" that allow me to control drone's attitude, but they require "thrust" input as well, which means i need to write a height controller on companion computer by myself. In "Altitude" mode, I can control drone's roll and pitch angle while holding height, so I guess it may also work in "offboard" mode. But I just don't find the proper ROS topic to do it. Is anyone who faces the same question with me? thx!

[antonio-sc66]

Hi @shupx , I might be wrong but thrust is a normalized value between 0-1. If your drone is configured properly 0.5 thrust should keep the altitude, just like when you apply 50% throttle in altitude or position with your RC. Then PX4 will transform that normalized thrust to obtain the PWM

[thitapi]

Reffer to my code: https://github.com/thitapi/Drone-sim/blob/main/exp1/src/px4/offboard.py it requires a teleop node what publishes cmd_vel messages. Explaination for altitude control part: where I takeoff by continuously publishing [0.0, 0.0, 3.0] to mavros/setpoint_raw/local topic of type PositionTarget.

self.sp = PositionTarget()
 self.sp.position.x = 0.0
 self.sp.position.y = 0.0
 self.sp.position.z = 3.0

Keep a function to update the position values at >2Hz, else it will go to auto land mode and land it in the takeoff position from a safely height(30m). thus use this function to update the position data

def updateSp(self):
        self.sp.position.x = self.local_pos.x
        self.sp.position.y = self.local_pos.y
        self.sp.position.z = 3.0
        self.vel_sp = self.vel_local

I am updating the velocity because I intend to control its velocity as well, you can omit that part. Keep publishing the [0, 0, 3] position till it reaches the required height and check for the same. But keep in mind: keep a threshold height lower than the intended height else it will start vibrating.

Before change the mode to offboard mode you need some points to be published already

    k=0
    while k<100:
        sp_pub.publish(cnt.sp)
        rate.sleep()
        k = k + 1
    # offboard mode
   modes.setOffboardMode()

 last_request=rospy.Time.now()
 while not rospy.is_shutdown():
        cnt.updateSp()
        ''' for testing position stream control
        if rospy.Time.now() - last_request < rospy.Duration(20.0):
            cnt.x_dir()
            rospy.loginfo("^^^^^^Going positive x + 5^^^^^^^") 
        elif rospy.Time.now() - last_request < rospy.Duration(23.0):
            cnt.y_dir() 
            rospy.loginfo("^^^^^^Going positive y + 5^^^^^^^")
        '''
        if cnt.local_pos.z <=2.5:
            sp_pub.publish(cnt.sp)
        vel_pub.publish(cnt.vel_sp)
        rate.sleep()

In my case I have set it to 2.5 (<3.0). You can coment out the if statement and uncoment the position stream control part to test if the position is being published properly or not. After this whatever you plan to do just make sure you are continuously publishing either poistion data or velocity data.

Hope this solves your problem.

[cesar-source]

@thitapi I am experiencing the same issue as @shupx. The issue with what you shared is that I also have to publish a thrust command to control the vehicle's attitude. For whatever reason, I am unable to find the thrust at hover. To me, it seems that the throttle command is extremely finicky. It either plummets or rises quickly and I am unable to get the vehicle to hover. I am using ~/setpoint_attitude/thrust and ~/setpoint_attitude/attitude to do this

I guess all that was a long winded way of saying that I am unable to use what you shared because the INSTANT I move on to attitude and thrust commands, the vehicle is unable to hold altitude. EVEN if I use the throttle that was previously being used to hold that altitude (I do this by saving the throttle msg from mavros/vfr_hud). Would anyone have any idea how to solve this issue? Thanks in advance!

[beginner-shuo]

@thitapi I am experiencing the same issue as @shupx. The issue with what you shared is that I also have to publish a thrust command to control the vehicle's attitude. For whatever reason, I am unable to find the thrust at hover. To me, it seems that the throttle command is extremely finicky. It either plummets or rises quickly and I am unable to get the vehicle to hover. I am using ~/setpoint_attitude/thrust and ~/setpoint_attitude/attitude to do this

I guess all that was a long winded way of saying that I am unable to use what you shared because the INSTANT I move on to attitude and thrust commands, the vehicle is unable to hold altitude. EVEN if I use the throttle that was previously being used to hold that altitude (I do this by saving the throttle msg from mavros/vfr_hud). Would anyone have any idea how to solve this issue? Thanks in advance! Have you solved this problem? I am going to control my UAV using the thrust and body_rate of the topic~/setpoint_attitude/attitude, but i can not find out the relationship between the real thrust and mavros message "thrust". I look forward to hearing back from you. lizhishuo2019@ia.ac.cn

[cesar-source]

@beginner-shuo I was able to fix it. I had to create a vertical position controller to maintain altitude. I used this paper to help me building the controller. The position controller takes into account the attitude of the drone to determine the thrust necessary to maintain its altitude. Since the output of the controller is a force, it therefore needs to be converted to a throttle command. I did this by multiplying by a 'motor constant'. I found (roughly) what the throttle of my vehicle was at hover and then compared that to the weight of the vehicle. Evidently, this throttle maps to the force of m_vehicle * gravity and can then find the motor constant knowing that k_f = thottle_hover / (m_vehicle * gravity)

[beginner-shuo]

@beginner-shuo I was able to fix it. I had to create a vertical position controller to maintain altitude. I used this paper to help me building the controller. The position controller takes into account the attitude of the drone to determine the thrust necessary to maintain its altitude. Since the output of the controller is a force, it therefore needs to be converted to a throttle command. I did this by multiplying by a 'motor constant'. I found (roughly) what the throttle of my vehicle was at hover and then compared that to the weight of the vehicle. Evidently, this throttle maps to the force of m_vehicle * gravity and can then find the motor constant knowing that k_f = thottle_hover / (m_vehicle * gravity)

Thanks for your reply. I have tried to map the thrust to thottle in this linear relationship, but it is not good for real time trajectory tracking.

I'd like to introduce my work. I am trying to do path planning using MPC, and use the fist step input (thrust, body_rate_x,y,z) of mpc to control the uav. Then get state feedback from the uav sensors, and replan the trajectory. However, with the linear relationship assumption, the control effect is not good.

I think it's possible that my control framework is infeasible, since the model of uav can not be established very accurately. Assumping the uav is at (0,0,0), and the reference state is (0,0,1), the first step thrust of mpc may be 10N, when the thrust is sent to the uav, it don't move because of the inaccurate model or the resistance. Then, the mpc replan the trajectory, and the first step thrust is still 10N...... Evently, the uav stays in the start place.

Can you give me some advice for real-time trajectory tracking? And how the control accuracy using the linear mapping in your work?

Thanks!

[cesar-source]

@beginner-shuo Hmm I see. I also am using real-time trajectory tracking, but I am using a backstepping control method. For my application I actually want to maintain my altitude throughout the flight. This means that the throttle does not change too much and will work for me even if the relationship is not exactly linear. To be clear, I found the hover throttle separately without using my controller.

What is the output of your MPC? For my controller, I get Fx, Fy, and Fz. I ignore the Fz from the controller since I am using a separate controller to maintain altitude. This has worked well for me, again, since my application has low vertical accelerations. I then find the attitude that Fx, Fy, and Fz=0.0 maps to using information from this paper. Finally, I send my desired attitude and thrust (from altitude controller) to MAVROS to set the vehicle in Offboard mode and perform the mission.

[beginner-shuo]

We use the dynamic model proposed in this paper. The output of my MPC is total thrust, body_rate of three-axis. For my application I want the hybrid robot to cross the obstacle on the powerline, so the uav has to take off and land on the powerline. So the control effect is not good unless the thrust-throttle model is very accurate. Now I am looking for other control methods that can take the control command from the MPC and translate to the commands for Mavros. I am going to reading the paper you mentioned above. Thanks again!

[shupx]

@beginner-shuo For MPC control of quadrotors, maybe you can refer to https://github.com/ethz-asl/mav_control_rw.git

If you know any other good mpc work (code or paper), please let me know, thanks

[beginner-shuo]

感谢！请问这个mpc的控制指令可以直接通过mavros让PX4无人机执行么？