Strange Turbulent Flight Behavior when transitioning into Offboard Mode (Please Help!!)

ggiacalone

I am attempting to use my Sentinel drone with offboard control and I am running into some undesirable behaviors.

Here is a video that shows the behavior: https://www.youtube.com/watch?v=NoKqImDZU4A

This is the code I am running from my laptop while running the Mavros node on the Voxl 2.

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest

current_state = State()

def state_cb(msg):
    global current_state
    current_state = msg


if __name__ == "__main__":
    rospy.init_node("offb_node_py")

    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)

    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)

    rospy.wait_for_service("/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)

    rospy.wait_for_service("/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)


    # Setpoint publishing MUST be faster than 2Hz
    rate = rospy.Rate(20)

    # Wait for Flight Controller connection
    while(not rospy.is_shutdown() and not current_state.connected):
        rate.sleep()

    initial_pose = PoseStamped()
    initial_pose.pose.position.x = 0
    initial_pose.pose.position.y = 0
    initial_pose.pose.position.z = 1.3
    initial_pose.pose.orientation.x = 0.0
    initial_pose.pose.orientation.y = 0.0
    initial_pose.pose.orientation.z = -0.7071
    initial_pose.pose.orientation.w = -0.7071

    land_pose = PoseStamped()
    land_pose.pose.position.x = 0
    land_pose.pose.position.y = 0
    land_pose.pose.position.z = 0
    land_pose.pose.orientation.x = 0.0
    land_pose.pose.orientation.y = 0.0
    land_pose.pose.orientation.z = -0.7071
    land_pose.pose.orientation.w = -0.7071

    flight_over = False

    # Variable to hold control timing for simple hop test 
    # TODO replace with better controll later
    control_time_start = None

    # Send a few setpoints before starting
    for i in range(100):
        if(rospy.is_shutdown()):
            break

        local_pos_pub.publish(initial_pose)
        rate.sleep()

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = 'OFFBOARD'

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    disarm_cmd = CommandBoolRequest()
    disarm_cmd.value = False

    last_req = rospy.Time.now()

    while(not rospy.is_shutdown()):

        if control_time_start is None:
            control_time_start = rospy.Time.now()

        if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            if(set_mode_client.call(offb_set_mode).mode_sent == True):
                rospy.loginfo("OFFBOARD enabled")

            last_req = rospy.Time.now()
        else:
            if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0) and not flight_over):
                if(arming_client.call(arm_cmd).success == True):
                    rospy.loginfo("Vehicle armed")

                last_req = rospy.Time.now()

        # Time Based Control for Hop Routine
        # TODO Make better later

        if (rospy.Time.now() - control_time_start) < rospy.Duration(15.0):
            local_pos_pub.publish(initial_pose)
        elif (rospy.Time.now() - control_time_start) < rospy.Duration(15.5):
            local_pos_pub.publish(land_pose)
        else:
            # Disarm the drone
            if(current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
                if(arming_client.call(disarm_cmd).success == True):
                    rospy.loginfo("Vehicle Disarmed")
                last_req = rospy.Time.now()
                flight_over = True
        rate.sleep()

I am using this mavros code to control it to go to a predefined setpoint. Unfortunately, on the first flight of every power cycle, it has a little bit of turbulence and it bounces up and down and shakes in the air a little bit before quickly and accurately moving to the desired setpoint I commanded it to go to. If the flight finishes and I run the code on my computer again to rerun the flight, it executes perfectly with no oscillations or other issues. It almost looks like the drone is fighting with itself until it makes it's mind up about where it needs to go.

I also noticed that every time the drone powercycles, it thinks that its initial /mavros/local_position_pose has an orientation of approximately

initial_pose.pose.orientation.x = 0.0
initial_pose.pose.orientation.y = 0.0
initial_pose.pose.orientation.z = -0.7071
initial_pose.pose.orientation.w = -0.7071

hence why I ensure to include this point in my setpoint pose. Although, QVIO shows the correct "zeroed" pose.

Again, everytime I re run the code without turning the drone off and powering on again, it goes straight to the setpoint without any turbulence or shaking (except for the first flight) which makes me believe it's not an issue with the code.

Why might it be having this unexpected and unwelcomed behavior?

system-image: 1.7.4-M0054-14.1a-perf
kernel: #1 SMP PREEMPT Fri Feb 9 21:59:24 UTC 2024 4.19.125

hw platform: M0054
mach.var: 1.0

voxl-suite: 1.1.3-1

Service Name | Enabled | Running | CPU Usage

Dawid Mościcki

In my case similar behavior was fixed by :
"The thing that is most likely the issue I believe is voxl-vision-hub. There is a program in vision-hub that will automatically do a figure 8 in offboard when it gets kicked off programatically and this is most likely conflicting with your takeoff. The parameters you needs to disable in /etc/modalai/voxl-vision-hub.conf are:

offboard_mode: off"

ggiacalone

@Dawid-Mościcki Wow amazing I will give this a try very soon. Thank you so much!

Strange Turbulent Flight Behavior when transitioning into Offboard Mode (Please Help!!)

system-image: 1.7.4-M0054-14.1a-perf kernel: #1 SMP PREEMPT Fri Feb 9 21:59:24 UTC 2024 4.19.125

hw platform: M0054 mach.var: 1.0

voxl-suite: 1.1.3-1

Service Name | Enabled | Running | CPU Usage

system-image: 1.7.4-M0054-14.1a-perf
kernel: #1 SMP PREEMPT Fri Feb 9 21:59:24 UTC 2024 4.19.125

hw platform: M0054
mach.var: 1.0