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