Servers by jDrones

FC Not getting Mavlink(dronekit) Commands but SITL does

(Conrad) #1

Hi all,
My setup is :
FC = F405 with 3.6.9 arducopter
sensors = Optical flow and tfmini (for GPS denied flight)
Companion computer = Pi Zero connected through uart to serial 1

Target:
Be able to take off and fly using the optical flow and lidar to get position estimates rather than GPS.

My Python3 script uses dronekit to connect to mavproxy both running on the pi zero. It connects, uses ‘mavutil.mavlink.MAV_CMD_DO_SET_HOME’ to give it coordinates (I’m told I need to do this in GPS denied flight if I then want to use ‘mavutil.mavlink.MAV_FRAME_LOCAL_NED’ to give reference coordinates/velocities.

When used on SITL the code seems to work (I’m using a bluetooth controller to give input) and moves the quad around like it should, but I think the SITL is generated with GPS so that might cause extra problems.

When I try and fly on the actual quad nothing happens, it seems the mavlink messages are being sent, because the code carries on but the quad doesnt take any action on them. For instance the set mode command is sent and the program goes onto the next loop but the quad doesnt arm.

From this I have come to the conclusion:
The Pi IS sending commands to the FC.
The commands are correct in as much as they work with SITL
The FC is not acting on the commands it is given

`from future import print_function

from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative
from pymavlink import mavutil # Needed for command message definitions
import time
import math

def connect_to_vehicle():
# Connect to Vehicle
connection_string = ‘10.3.141.255:14550’
print(‘Connecting to vehicle on: %s’ % connection_string)
return connect(connection_string, wait_ready=True)
print(‘Connected’)

def cmdBodyPosition(lat, lon, alt):
msg = vehicle.message_factory.command_long_encode(
0, 0, # target system, target component
mavutil.mavlink.MAV_CMD_DO_SET_HOME, # command
0, # confirmation
0, # param 1, (1=use current location, 0=use specified location)
0, # param 2, unused
0, # param 3,unused
0, # param 4, unused
lat, lon, alt) # param 5 ~ 7 latitude, longitude, altitude
vehicle.send_mavlink(msg)
vehicle.flush()

def arm_and_takeoff(aTargetAltitude):
“”"
Arms vehicle and fly to aTargetAltitude.
“”"

print("Basic pre-arm checks")
# Don't let the user try to arm until autopilot is ready
while not vehicle.is_armable_indoor:
    print(" Waiting for vehicle to initialise...")
    time.sleep(1)


print("Arming motors")
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True

while not vehicle.armed:
    print(" Waiting for arming...")
    time.sleep(1)

print("Taking off!")
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude

# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
#  after Vehicle.simple_takeoff will execute immediately).
while True:
    print(" Altitude: ", vehicle.location.global_relative_frame.alt)
    if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Trigger just below target alt.
        print("Reached target altitude")
        break
    time.sleep(1)

def cmdBodyPosition(x, y, z):
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0b110111111000, # type_mask
x, y, -z, # x, y, z positions
0, 0, 0, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
vehicle.send_mavlink(msg)
vehicle.flush()

def send_ned_velocity(velocity_x, velocity_y, velocity_z, duration):
“”"
Move vehicle in direction based on specified velocity vectors and
for the specified duration.

This uses the SET_POSITION_TARGET_LOCAL_NED command with a type mask enabling only
velocity components
(http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_local_ned).

Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
velocity persists until it is canceled. The code below should work on either version
(sending the message multiple times does not cause problems).

See the above link for information on the type_mask (0=enable, 1=ignore).
At time of writing, acceleration and yaw bits are ignored.
"""
msg = vehicle.message_factory.set_position_target_local_ned_encode(
    0,       # time_boot_ms (not used)
    0, 0,    # target system, target component
    mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
    0b0000111111000111, # type_mask (only speeds enabled)
    0, 0, 0, # x, y, z positions (not used)
    velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
    0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
    0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
for x in range(0,duration):
    vehicle.send_mavlink(msg)
    time.sleep(1)

vehicle = connect_to_vehicle()

cmdBodyPosition(51.500544, -2.547071, 0) #Set home position otherwise unarmable

arm_and_takeoff(1.5)

`.