How to TakeOff a Quadcopter steadly in stabilized mode with using gazebo and sitl?

ArduCopter
1

In our project we want to design a software about indoor autonomous drone. I want to test my softares on gazebo so I use sitl for that. But when I give takeoff command in stabilize mode with using rc_channels_override message , the drone is moving unsteadly and somersaults. What should I do to takeoff and control the drone steadly without using GPS data?

You can find the code below;

from pymavlink import mavutil
import sys
import time
autotune_bool = False

def mod_change(master , mode_name): #this function changes fligth mode of the UAV.
if mode_name not in master.mode_mapping():
print(‘Unknown mode : {}’.format(mode_name))
print(‘Try:’, list(master.mode_mapping().keys()))
sys.exit(1)

Get mode ID

mode_id = master.mode_mapping()[mode_name]
# Set new mode
# master.mav.command_long_send(
#    master.target_system, master.target_component,
#    mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
#    0, mode_id, 0, 0, 0, 0, 0) or:
# master.set_mode(mode_id) or:
master.mav.set_mode_send(
    master.target_system,
    mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
    mode_id)

def arming_motors():
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0)

def autotune_axis():
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component,
mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, 1, 3 , 0 , 0,0,0,0,0)
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component,
mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, 1, 1 , 0 , 0,0,0,0,0)
the_connection.mav.command_long_send(the_connection.target_system, the_connection.target_component,
mavutil.mavlink.MAV_CMD_DO_AUTOTUNE_ENABLE, 1, 2 , 0 , 0,0,0,0,0)

def set_rc_channel_pwm(master , id, pwm=1500):

if id < 1:
    print("Channel does not exist.")
    return

if id < 9:
    rc_channel_values = [65535 for _ in range(8)]
    rc_channel_values[id - 1] = pwm
    rc_channel_values[4] = 1900
    master.mav.rc_channels_override_send(
        master.target_system,                # target_system
        master.target_component,             # target_component
        *rc_channel_values)                  # RC channel list, in microseconds.

the_connection = mavutil.mavlink_connection(‘tcp:localhost:5762’)
the_connection.wait_heartbeat()

mod_change(the_connection , ‘STABILIZE’)

time.sleep(2)

time.sleep(3)
arming_motors()
time.sleep(2)

msg = the_connection.recv_match(type=‘COMMAND_ACK’, blocking=True)
print(msg)

bas_time = time.time()
while True:

code_time = time.time()

if code_time - bas_time <= 2.0:
    set_rc_channel_pwm(the_connection ,  3, 1525)
2

As per definition, stabilize mode requires a pilot. The pilot is part of the control loop and does the “steadying” of the system. So, if no pilot in stabilize, then no steadying.

The solutions:

  1. implement a control algorithm in your pymavlink code or,
  2. use another flight mode (like guided) to do the job. But this does require GPS, or optical flow, or vision odometry
3

The iris quadcopter example provided with the ardupilot gazebo plugins will take off without flipping. It will not maintain altitude or position in stabilize, but should maintain attitude.

This using Gazebo Garden and the version of the plugins at ArduPilot/ardupilot_gazebo. If you are using a different vehicle it may be an issue with the initial tune.

Gazebo:

gz sim -v4 -r iris_runway.sdf

SITL:

sim_vehicle.py -D -v ArduCopter -f gazebo-iris --model JSON --console

At the MAVProxy command prompt:

STABILIZE> arm throttle
STABILIZE> rc 3 1600

The iris will takeoff and start to drift.