Yea so the battery is at 83% when it crashes… Based on the FS_CRASH crash type checking params being at specific values more then 2 seconds I changed the MAX Angle to be 20 in QgroundControl since that FS_CRASH detection checks for a value over 30 for more then 2 seconds and now my quad did not crash…
HOWEVER the speed drops after about 500 of the 1500 meters from a speed of 5.00 to .006 and starts crawling slowly forward but never crashes do to the angle constraint… So it fixed the crash detection but not the underlying issue of whats happening at 500 meters.
Output I was monitoring below died at 1065 dropped to a speed of .006 and remained there:
AirSpeed: 5.008000373840332
HeartBeat 2227.253474317
Distance from dropoff point: 1071.0173701304914
AirSpeed: 5.005000114440918
HeartBeat 2229.28461993
Distance from dropoff point: 1065.909084532523
AirSpeed: 0.03500000014901161
HeartBeat 2231.266562261
Distance from dropoff point: 1065.773969685431
AirSpeed: 0.006000000052154064
HeartBeat 2233.276560427
Here is the code I am using:
from dronekit import Vehicle, connect, VehicleMode, LocationGlobal, APIException, LocationGlobalRelative, Command
import time
import socket
import math
import argparse
from pymavlink import mavutil
def get_distance_meters(targetLocation, currentLocation):
""" Takes a target location and current location and returns the distance"""
dlat = targetLocation.lat - currentLocation.lat
dlon = targetLocation.lon - currentLocation.lon
return math.sqrt((dlon*dlon) + (dlat*dlat))*1.113195e5
def calculate_currentDistance(targetLocation):
"""Takes a target location object and returns your distance to that location """
distanceToTargetLocation = get_distance_meters(targetLocation, vehicle.location.global_relative_frame)
return distanceToTargetLocation
def connectMyCopter():
"""Connects to your quad and returns a vehicle connection object"""
parser = argparse.ArgumentParser(description='commands')
parser.add_argument('--connect')
args = parser.parse_args()
connection_string = args.connect
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
vehicle = connect(connection_string, wait_ready=True)
return vehicle
def arm_and_takeoff(targetHeight):
"""Arms the drone and gets it up in the air to the meters height of input targetheight"""
while (vehicle.is_armable != True):
print("Waiting for vehical to become armable")
time.sleep(1)
print("--------------------------------")
print("Vehical Is armable")
print("--------------------------------")
vehicle.mode = VehicleMode("GUIDED")
while(vehicle.mode != "GUIDED"):
print(vehicle.mode)
print("Waiting for the drone to enter GUIDED flight mode")
time.sleep(1)
vehicle.armed = True
while(vehicle.armed == False):
print("Waiting to arm vehical")
time.sleep(2)
print("--------------------------------")
print("Props are spinning")
print("--------------------------------")
vehicle.simple_takeoff(targetHeight) #meters
while True:
print(f"Current Altitude: {vehicle.location.global_relative_frame.alt}")
if (vehicle.location.global_relative_frame.alt) >=.95*targetHeight:
break
time.sleep(1)
print("--------------------------------")
print("Target Altitude Reached")
print("--------------------------------")
return None
#############Start of Main#################
wp1 = LocationGlobalRelative(REMOVED,-REMOVED, 10)
vehicle = connectMyCopter()
wphome=vehicle.location.global_relative_frame
cmd1=Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,wphome.lat, wphome.lon,wphome.alt)
cmd2=Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,REMOVED,-REMOVED, 10)
cmd3=Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,0,0,0,0,0,0,0,0,0)
################# Setting Up AUTO COMMANDS####################
#Download list of commands from current drone
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
#Clear List of commands from drone:
cmds.clear()
#Add in our new commands
cmds.add(cmd1)
cmds.add(cmd2)
cmds.add(cmd3)
#upload our commands to the drone
vehicle.commands.upload()
##################END SETUP COMMANDS##########################
#Arm Your Copter#
arm_and_takeoff(10)
print("Setting to Auto Mode...")
vehicle.mode = VehicleMode("AUTO")
while(vehicle.mode != "AUTO"):
time.sleep(.2)
print("--------------------------------")
print("Vehical entered AutoFlight Mode")
print("--------------------------------")
print("---------------------------------------------------")
print("Drone is executing Auto fly to dropoff point")
print("---------------------------------------------------")
########## Mission Execution Loop###########
while (vehicle.location.global_relative_frame.alt > 2):
current_distance = calculate_currentDistance(wp1)
print(f"Distance from dropoff point: {current_distance}")
print("---------------------------------------")
print(f"AirSpeed: {vehicle.airspeed}")
print(f"HeartBeat {vehicle._heartbeat_lastreceived}")
time.sleep(2)
print("---------------------------------------")
print("---------------------------------------")
print ("Mission Completed")
print("---------------------------------------")
print("---------------------------------------")
