Double flip on Clover drone

double_flip.py

import math

import rospy

from clover import srv
from std_srvs.srv import Trigger
from sensor_msgs.msg import Range
from mavros_msgs.srv import SetMode

rospy.init_node('fly')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry, persistent=True)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
land = rospy.ServiceProxy('land', Trigger)
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)


def flip():
    first_flip = 0
    second_ready = 0
    debug_flip = "start"

    start = get_telemetry()  # memorize starting position

    set_rates(thrust=1)  # bump up
    rospy.sleep(0.2)

    set_rates(roll_rate=30, thrust=0.2)  # maximum roll rate

    while True:
        telem = get_telemetry()
        a = telem.roll

        if -math.pi + 0.1 < a < -0.2:
            first_flip = 1

        if (math.pi * 0.9) / 6 < a < math.pi and first_flip:
            print(telem.roll)
            break

    while True:
        telem = get_telemetry()

        if -math.pi < telem.roll < -0.1 or math.radians(134) < telem.roll < math.pi:
            set_rates(roll_rate=-50, thrust=0.8)
            rospy.sleep(0.15)
            break

    print 'set position'
    set_position(x=start.x, y=start.y, z=start.z, yaw=start.yaw)  # finish flip


navigate(x=0, y=0, z=1.5, speed=1.3, auto_arm=True, frame_id='body')
rospy.sleep(3)

navigate(x=0, y=0, z=2, yaw=math.radians(90), speed=1, frame_id='aruco_map')
rospy.sleep(6)

flip()

rospy.sleep(9)

flip()

rospy.sleep(6)

land()