magtest.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 rotate APMs on bench to test magnetometers
00005 
00006 '''
00007 
00008 import sys, os, time
00009 from math import radians
00010 
00011 from pymavlink import mavutil
00012 
00013 from argparse import ArgumentParser
00014 parser = ArgumentParser(description=__doc__)
00015 
00016 parser.add_argument("--device1", required=True, help="mavlink device1")
00017 parser.add_argument("--device2", required=True, help="mavlink device2")
00018 parser.add_argument("--baudrate", type=int,
00019                   help="master port baud rate", default=115200)
00020 args = parser.parse_args()
00021 
00022 def set_attitude(rc3, rc4):
00023     global mav1, mav2
00024     values = [ 65535 ] * 8
00025     values[2] = rc3
00026     values[3] = rc4
00027     mav1.mav.rc_channels_override_send(mav1.target_system, mav1.target_component, *values)
00028     mav2.mav.rc_channels_override_send(mav2.target_system, mav2.target_component, *values)
00029 
00030 
00031 # create a mavlink instance
00032 mav1 = mavutil.mavlink_connection(args.device1, baud=args.baudrate)
00033 
00034 # create a mavlink instance
00035 mav2 = mavutil.mavlink_connection(args.device2, baud=args.baudrate)
00036 
00037 print("Waiting for HEARTBEAT")
00038 mav1.wait_heartbeat()
00039 mav2.wait_heartbeat()
00040 print("Heartbeat from APM (system %u component %u)" % (mav1.target_system, mav1.target_system))
00041 print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system))
00042 
00043 print("Waiting for MANUAL mode")
00044 mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)
00045 mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)
00046 
00047 print("Setting declination")
00048 mav1.mav.param_set_send(mav1.target_system, mav1.target_component,
00049                        'COMPASS_DEC', radians(12.33))
00050 mav2.mav.param_set_send(mav2.target_system, mav2.target_component,
00051                        'COMPASS_DEC', radians(12.33))
00052 
00053 
00054 set_attitude(1060, 1160)
00055 
00056 event = mavutil.periodic_event(30)
00057 pevent = mavutil.periodic_event(0.3)
00058 rc3_min = 1060
00059 rc3_max = 1850
00060 rc4_min = 1080
00061 rc4_max = 1500
00062 rc3 = rc3_min
00063 rc4 = 1160
00064 delta3 = 2
00065 delta4 = 1
00066 use_pitch = 1
00067 
00068 MAV_ACTION_CALIBRATE_GYRO = 17
00069 mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO)
00070 mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO)
00071 
00072 print("Waiting for gyro calibration")
00073 mav1.recv_match(type='ACTION_ACK')
00074 mav2.recv_match(type='ACTION_ACK')
00075 
00076 print("Resetting mag offsets")
00077 mav1.mav.set_mag_offsets_send(mav1.target_system, mav1.target_component, 0, 0, 0)
00078 mav2.mav.set_mag_offsets_send(mav2.target_system, mav2.target_component, 0, 0, 0)
00079 
00080 def TrueHeading(SERVO_OUTPUT_RAW):
00081     p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min)
00082     return 172 + p*(326 - 172)
00083 
00084 while True:
00085     mav1.recv_msg()
00086     mav2.recv_msg()
00087     if event.trigger():
00088         if not use_pitch:
00089             rc4 = 1160
00090         set_attitude(rc3, rc4)
00091         rc3 += delta3
00092         if rc3 > rc3_max or rc3 < rc3_min:
00093             delta3 = -delta3
00094             use_pitch ^= 1
00095         rc4 += delta4
00096         if rc4 > rc4_max or rc4 < rc4_min:
00097             delta4 = -delta4
00098     if pevent.trigger():
00099         print("hdg1: %3u hdg2: %3u  ofs1: %4u, %4u, %4u  ofs2: %4u, %4u, %4u" % (
00100             mav1.messages['VFR_HUD'].heading,
00101             mav2.messages['VFR_HUD'].heading,
00102             mav1.messages['SENSOR_OFFSETS'].mag_ofs_x,
00103             mav1.messages['SENSOR_OFFSETS'].mag_ofs_y,
00104             mav1.messages['SENSOR_OFFSETS'].mag_ofs_z,
00105             mav2.messages['SENSOR_OFFSETS'].mag_ofs_x,
00106             mav2.messages['SENSOR_OFFSETS'].mag_ofs_y,
00107             mav2.messages['SENSOR_OFFSETS'].mag_ofs_z,
00108             ))
00109     time.sleep(0.01)
00110 
00111 # 314M 326G
00112 # 160M 172G
00113 


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43