mavtest_hackathon.txt

#!/usr/bin/env python

import sys, os, time
from optparse import OptionParser

# tell python where to find mavlink so we can import it
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '/mnt/c/Users/marce/DEV/mavlink'))
from pymavlink import mavutil
from pymavlink.dialects.v20 import aerotain as at


def handle_heartbeat(msg):
    mode = mavutil.mode_string_v10(msg)
    is_armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
    is_enabled = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED


def handle_attitude(msg):
    attitude_data = (msg.roll, msg.pitch, msg.yaw, msg.rollspeed,
                     msg.pitchspeed, msg.yawspeed)
    print "Roll\tPit\tYaw\tRSpd\tPSpd\tYSpd"
    print "%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t" % attitude_data


def loop(m):

    mav = at.MAVLink(m)

    xaxis_output = 0
    counter = 0

    while (True):

        # grab a mavlink message
        msg = m.recv_match(blocking=False)
        if not msg:
            #discard this message
            continue

        # handle the message based on its type
        msg_type = msg.get_type()
        if msg_type == "BAD_DATA":
            if mavutil.all_printable(msg.data):
                sys.stdout.write(msg.data)
                sys.stdout.flush()
        elif msg_type == "HEARTBEAT":
            handle_heartbeat(msg)
        elif msg_type == "ATTITUDE":
            handle_attitude(msg)

            if msg.roll > 0 and xaxis_output < 32757:
                xaxis_output += 10
            elif msg.roll < 0 and xaxis_output > -32757:
                xaxis_output -= 10

            mav.setpoint_6dof_send(xaxis_output, 0, 0, 0, 0, 0, counter)
            counter += 1

def main():
    # read command line options
    parser = OptionParser("readdata.py [options]")
    parser.add_option("--baudrate", dest="baudrate", type='int',
                      help="master port baud rate", default=115200)
    parser.add_option("--device", dest="device", default= "/dev/ttyUSB0", help="serial device")
    parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate")
    parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
                      default=255, help='MAVLink source system for this GCS')
    parser.add_option("--showmessages", dest="showmessages", action='store_true',
                      help="show incoming messages", default=True)
    (opts, args) = parser.parse_args()

    if opts.device is None:
        print("You must specify a serial device")
        sys.exit(1)

    # create a mavlink serial instance
    master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate)

    # wait for the heartbeat msg to find the system ID
    master.wait_heartbeat()

    # request data to be sent at the given rate
    master.mav.request_data_stream_send(master.target_system, master.target_component,
                                        mavutil.mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1)

    # enter the data loop
    loop(master)


if __name__ == '__main__':
    main()