Indoor (GPS denied area) fly using GUIDED_NOGPS

[borhanreo]

I'm using pixhawk for indoor mission. i am familiar with the controlling with gps signal available area. My copter wrok good in gps signal available area. But i want to flying in GPS denied area. I have successfully install maxbotix range finder and get altitude value. I’ve got a very basic script that puts the drone to GUIDED_NOGPS mode, arms it and forces it to takeoff for the altitude say 1m, then it wains for a while and lands. When i armed then it takeoff then drone going straight up. Did not stay in 1m altitude. Anyone help me.

sonar_alt = -5
def range_finder():
    def rangefinder_callback(self, attr_name, value):
        global sonar_alt
        sonar_alt = value.distance
    print ("Rangefinder ", value.distance, value.voltage)

vehicle.add_attribute_listener('rangefinder', rangefinder_callback)
def main_arm_and_takeoff_nogps(aTargetAltitude):
    DEFAULT_TAKEOFF_THRUST = 0.6
    SMOOTH_TAKEOFF_THRUST = 0.5
    print("Basic pre-arm checks")
    while not vehicle.is_armable:
        print(" Waiting for vehicle to initialise...")
        time.sleep(1)
    print("Arming motors")
    vehicle.mode = VehicleMode("GUIDED_NOGPS")
    vehicle.armed = True
    while not vehicle.armed:
        print(" Waiting for arming...")
        vehicle.armed = True
        time.sleep(1)
    print("Taking off!")
    thrust = DEFAULT_TAKEOFF_THRUST
    while True:
        current_altitude = sonar_alt
        print(" Altitude: %f  Desired: %f" % (current_altitude, aTargetAltitude))
        if current_altitude >= aTargetAltitude * 0.95:
            print("Reached target altitude ")
            break
        elif current_altitude >= aTargetAltitude * 0.6:
            thrust = SMOOTH_TAKEOFF_THRUST
        main_set_attitude(thrust=thrust)
        time.sleep(0.2)
def modeLand():
    vehicle.mode = VehicleMode("LAND")
    time.sleep(5)
    print("-------------Vehicle is in:-------%s" % vehicle.mode)
def main_send_attitude_target(roll_angle = 0.0, pitch_angle = 0.0,
                     yaw_angle = None, yaw_rate = 0.0, use_yaw_rate = False,
                     thrust = 0.5):
    if yaw_angle is None:
        yaw_angle = vehicle.attitude.yaw
        msg = vehicle.message_factory.set_attitude_target_encode(
        0, # time_boot_ms
        1, # Target system
        1, # Target component
        0b00000000 if use_yaw_rate else 0b00000100,
        main_to_quaternion(roll_angle, pitch_angle, yaw_angle), # Quaternion
        0, # Body roll rate in radian
        0, # Body pitch rate in radian
        math.radians(yaw_rate), # Body yaw rate in radian/second
        thrust  # Thrust
    )
    vehicle.send_mavlink(msg)
def main_set_attitude(roll_angle = 0.0, pitch_angle = 0.0,
             yaw_angle = None, yaw_rate = 0.0, use_yaw_rate = False,
             thrust = 0.5, duration = 0):

    main_send_attitude_target(roll_angle, pitch_angle,
                     yaw_angle, yaw_rate, False,
                     thrust)
    start = time.time()
    while time.time() - start < duration:
        main_send_attitude_target(roll_angle, pitch_angle,
                         yaw_angle, yaw_rate, False,
                         thrust)
        time.sleep(0.1)
    # Reset attitude, or it will persist for 1s more due to the timeout
    main_send_attitude_target(0, 0,
                     0, 0, True,
                     thrust)
def main_to_quaternion(roll = 0.0, pitch = 0.0, yaw = 0.0):
    """
    Convert degrees to quaternions
    """
    t0 = math.cos(math.radians(yaw * 0.5))
    t1 = math.sin(math.radians(yaw * 0.5))
    t2 = math.cos(math.radians(roll * 0.5))
    t3 = math.sin(math.radians(roll * 0.5))
    t4 = math.cos(math.radians(pitch * 0.5))
    t5 = math.sin(math.radians(pitch * 0.5))

    w = t0 * t2 * t4 + t1 * t3 * t5
    x = t0 * t3 * t4 - t1 * t2 * t5
    y = t0 * t2 * t5 + t1 * t3 * t4
    z = t1 * t2 * t4 - t0 * t3 * t5

    return [w, x, y, z]
if __name__ == '__main__':
    range_finder()
    main_arm_and_takeoff_nogps(1)  ## 1 miter
    time.sleep(3)
    modeLand()`