Ask Your Question
0

MAV drifting away from its takeoff position

asked 2019-12-26 23:54:43 -0600

zaccer gravatar image

updated 2019-12-26 23:58:47 -0600

I am working on MAV control in GPS denied environment using px4flow and Lidar sensor.While in simulation and during experiments on real vehicle, MAV doesnt stay at its position and drifts in random direction. I am using /mavros/local_position/pose msg to provide setpoints to the MAV. Currently it just have to takeoff hover and land at the same position. What might be causing this? Here is the code for your reference.

`!/usr/bin/env python

import rospy

from geometry_msgs.msg import Point, PoseStamped

from mavros_msgs.msg import * from mavros_msgs.srv import *

class fcuModes: def __init__(self): pass

def setTakeoff(self):
    rospy.wait_for_service('mavros/cmd/takeoff')
    try:
        takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
        takeoffService(altitude = 3)
    except rospy.ServiceException, e:
        print "Service takeoff call failed: %s"%e

def setArm(self):
    rospy.wait_for_service('mavros/cmd/arming')
    try:
        armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        armService(True)
    except rospy.ServiceException, e:
        print "Service arming call failed: %s"%e

def setDisarm(self):
    rospy.wait_for_service('mavros/cmd/arming')
    try:
        armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        armService(False)
    except rospy.ServiceException, e:
        print "Service disarming call failed: %s"%e

def setStabilizedMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='STABILIZED')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Stabilized Mode could not be set."%e

def setOffboardMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='OFFBOARD')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Offboard Mode could not be set."%e

def setAltitudeMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='ALTCTL')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Altitude Mode could not be set."%e

def setPositionMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='POSCTL')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Position Mode could not be set."%e

def setAutoLandMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='AUTO.LAND')
    except rospy.ServiceException, e:
           print "service set_mode call failed: %s. Autoland Mode could not be set."%e

class Controller:

def __init__(self):
    # Drone state
    self.state = State()
    # Instantiate a setpoints message
    self.sp = PositionTarget()
    # set the flag to use position setpoints and yaw angle
    self.sp.type_mask = int('010111111000', 2)
    # LOCAL_NED
    self.sp.coordinate_frame = 1

    # We will fly at a fixed altitude for now
    # Altitude setpoint, [meters]
    self.ALT_SP = 1.5
    # update the setpoint message with the required altitude
    self.sp.position.z = self.ALT_SP
    # Step size for position update
    self.STEP_SIZE = 2.0
    # Fence. We will assume a square fence for now
    self.FENCE_LIMIT = 5.0

    # A Message for the current local position of the drone
    self.local_pos = Point(0.0, 0.0, 1.0)

    # initial values for setpoints
    self.sp.position.x = 0.0
    self.sp.position.y = 0.0

    # speed ...
(more)
edit retag flag offensive close merge delete

Comments

Does it stay still in the POSCTL mode?

Orhan gravatar imageOrhan ( 2019-12-27 06:22:34 -0600 )edit

yes it does.

zaccer gravatar imagezaccer ( 2020-01-03 07:21:40 -0600 )edit

Then it is capable of holding its position. If you need to control it in offboard mode, and make it act same, inspect its local position information from mavros topics and send alike commands to make it stand still. I'm writing as answer since I need to copy your codes.

Orhan gravatar imageOrhan ( 2020-01-03 08:10:24 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-01-03 08:28:10 -0600

Orhan gravatar image

updated 2020-01-06 00:25:13 -0600

If it is able to hold its position in POSCTL mode, then sensor is connected and correctly configured. If you need to control it in OFFBOARD mode, and make it act same, inspect its local position information from mavros topics while it is holding its position in POSCTL mode. And send alike commands to make it stand still.

What I see from your code is, you are reading the position information,

def posCb(self, msg):
    self.local_pos.x = msg.pose.position.x

And publishing it back:

def updateSp(self):
    self.sp.position.x = self.local_pos.x

Theoretically, it should make it stand still. But it doesn't. Because its location belief slightly drifts and not precisely stable. And you send new commands according to the drifted position by time. After a while, the error accumulates and you are sending a command that is away from the takeoff point. If you inspect it in POSCTL, you'll see that its local position information isn't changing much and x y is closer to zero.

Instead of copying position reading continuously, read it once in the beginning after the takeoff and send the same position command in the loop, since it is capable of calculating its local position correctly. This doesn't provide a perfect position hold but you will get a close one to what POSCTL provides.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-12-26 23:54:43 -0600

Seen: 22 times

Last updated: Jan 06