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()`