# How to use rotation method in ATX

Best Python code snippet using ATX

magfit_rotation_gyro.py

Source:magfit_rotation_gyro.py

`1#!/usr/bin/env python2'''3fit best estimate of magnetometer rotation to gyro data4'''5import sys, time, os, math6from argparse import ArgumentParser7parser = ArgumentParser(description=__doc__)8parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")9parser.add_argument("--verbose", action='store_true', help="verbose output")10parser.add_argument("--min-rotation", default=5.0, type=float, help="min rotation to add point")11parser.add_argument("logs", metavar="LOG", nargs="+")12args = parser.parse_args()13from pymavlink import mavutil14from pymavlink.rotmat import Vector3, Matrix315from math import radians, degrees16class Rotation(object):17 def __init__(self, name, roll, pitch, yaw):18 self.name = name19 self.roll = roll20 self.pitch = pitch21 self.yaw = yaw22 self.r = Matrix3()23 self.r.from_euler(self.roll, self.pitch, self.yaw)24 def is_90_degrees(self):25 return (self.roll % 90 == 0) and (self.pitch % 90 == 0) and (self.yaw % 90 == 0)26 def __str__(self):27 return self.name28# the rotations used in APM29rotations = [30 Rotation("ROTATION_NONE", 0, 0, 0),31 Rotation("ROTATION_YAW_45", 0, 0, 45),32 Rotation("ROTATION_YAW_90", 0, 0, 90),33 Rotation("ROTATION_YAW_135", 0, 0, 135),34 Rotation("ROTATION_YAW_180", 0, 0, 180),35 Rotation("ROTATION_YAW_225", 0, 0, 225),36 Rotation("ROTATION_YAW_270", 0, 0, 270),37 Rotation("ROTATION_YAW_315", 0, 0, 315),38 Rotation("ROTATION_ROLL_180", 180, 0, 0),39 Rotation("ROTATION_ROLL_180_YAW_45", 180, 0, 45),40 Rotation("ROTATION_ROLL_180_YAW_90", 180, 0, 90),41 Rotation("ROTATION_ROLL_180_YAW_135", 180, 0, 135),42 Rotation("ROTATION_PITCH_180", 0, 180, 0),43 Rotation("ROTATION_ROLL_180_YAW_225", 180, 0, 225),44 Rotation("ROTATION_ROLL_180_YAW_270", 180, 0, 270),45 Rotation("ROTATION_ROLL_180_YAW_315", 180, 0, 315),46 Rotation("ROTATION_ROLL_90", 90, 0, 0),47 Rotation("ROTATION_ROLL_90_YAW_45", 90, 0, 45),48 Rotation("ROTATION_ROLL_90_YAW_90", 90, 0, 90),49 Rotation("ROTATION_ROLL_90_YAW_135", 90, 0, 135),50 Rotation("ROTATION_ROLL_270", 270, 0, 0),51 Rotation("ROTATION_ROLL_270_YAW_45", 270, 0, 45),52 Rotation("ROTATION_ROLL_270_YAW_90", 270, 0, 90),53 Rotation("ROTATION_ROLL_270_YAW_135", 270, 0, 135),54 Rotation("ROTATION_PITCH_90", 0, 90, 0),55 Rotation("ROTATION_PITCH_270", 0, 270, 0),56 Rotation("ROTATION_PITCH_180_YAW_90", 0, 180, 90),57 Rotation("ROTATION_PITCH_180_YAW_270", 0, 180, 270),58 Rotation("ROTATION_ROLL_90_PITCH_90", 90, 90, 0),59 Rotation("ROTATION_ROLL_180_PITCH_90", 180, 90, 0),60 Rotation("ROTATION_ROLL_270_PITCH_90", 270, 90, 0),61 Rotation("ROTATION_ROLL_90_PITCH_180", 90, 180, 0),62 Rotation("ROTATION_ROLL_270_PITCH_180", 270, 180, 0),63 Rotation("ROTATION_ROLL_90_PITCH_270", 90, 270, 0),64 Rotation("ROTATION_ROLL_180_PITCH_270", 180, 270, 0),65 Rotation("ROTATION_ROLL_270_PITCH_270", 270, 270, 0),66 Rotation("ROTATION_ROLL_90_PITCH_180_YAW_90", 90, 180, 90),67 Rotation("ROTATION_ROLL_90_YAW_270", 90, 0, 270)68 ]69def mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL):70 '''fixup a mag vector back to original value using AHRS and Compass orientation parameters'''71 if COMPASS_EXTERNAL == 0 and AHRS_ORIENTATION != 0:72 # undo any board orientation73 mag = rotations[AHRS_ORIENTATION].r.transposed() * mag74 # undo any compass orientation75 if COMPASS_ORIENT != 0:76 mag = rotations[COMPASS_ORIENT].r.transposed() * mag77 return mag78def add_errors(mag, gyr, last_mag, deltat, total_error, rotations):79 for i in range(len(rotations)):80 if not rotations[i].is_90_degrees():81 continue82 r = rotations[i].r83 m = Matrix3()84 m.rotate(gyr * deltat)85 rmag1 = r * last_mag86 rmag2 = r * mag87 rmag3 = m.transposed() * rmag188 err = rmag3 - rmag289 total_error[i] += err.length()90def magfit(logfile):91 '''find best magnetometer rotation fit to a log file'''92 print("Processing log %s" % filename)93 mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)94 last_mag = None95 last_usec = 096 count = 097 total_error = [0]*len(rotations)98 AHRS_ORIENTATION = 099 COMPASS_ORIENT = 0100 COMPASS_EXTERNAL = 0101 last_gyr = None102 # now gather all the data103 while True:104 m = mlog.recv_match()105 if m is None:106 break107 if m.get_type() in ["PARAM_VALUE", "PARM"]:108 if m.get_type() == "PARM":109 name = str(m.Name)110 value = int(m.Value)111 else:112 name = str(m.param_id)113 value = int(m.param_value)114 if name == "AHRS_ORIENTATION":115 AHRS_ORIENTATION = value116 if name == 'COMPASS_ORIENT':117 COMPASS_ORIENT = value118 if name == 'COMPASS_EXTERNAL':119 COMPASS_EXTERNAL = value120 if m.get_type() in ["RAW_IMU", "MAG","IMU"]:121 if m.get_type() == "RAW_IMU":122 mag = Vector3(m.xmag, m.ymag, m.zmag)123 gyr = Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001124 usec = m.time_usec125 elif m.get_type() == "IMU":126 last_gyr = Vector3(m.GyrX,m.GyrY,m.GyrZ)127 continue128 elif last_gyr is not None:129 mag = Vector3(m.MagX,m.MagY,m.MagZ)130 gyr = last_gyr131 usec = m.TimeMS * 1000132 mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL)133 if last_mag is not None and gyr.length() > radians(args.min_rotation):134 add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations)135 count += 1136 last_mag = mag137 last_usec = usec138 best_i = 0139 best_err = total_error[0]140 for i in range(len(rotations)):141 r = rotations[i]142 if not r.is_90_degrees():143 continue144 if args.verbose:145 print("%s err=%.2f" % (r, total_error[i]/count))146 if total_error[i] < best_err:147 best_i = i148 best_err = total_error[i]149 r = rotations[best_i]150 print("Current rotation is AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=%u" % (151 rotations[AHRS_ORIENTATION],152 rotations[COMPASS_ORIENT],153 COMPASS_EXTERNAL))154 print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count))155 print("Please set AHRS_ORIENTATION=%s COMPASS_ORIENT=%s COMPASS_EXTERNAL=1" % (156 rotations[AHRS_ORIENTATION],157 r))158for filename in args.logs:...`

ori_filter.py

Source:ori_filter.py

`1import cv22import numpy as np3# http://campar.in.tum.de/Chair/KalmanFilter4"""5This is a Kalman filter implementation for rotation6To solve issues with wrapping at 0deg-360deg, total rotations are tracked7"""8class OrientationFilter:9 def __init__(self, orientation):10 self.rotation_value = orientation * np.pi / 180.011 self.orientation = orientation12 self.kf = cv2.KalmanFilter(3, 1, 0)13 self.kf.measurementMatrix = np.array([[1., 0., 0.]], np.float32)14 # Q: process noise covariance15 self.kf.processNoiseCov = cv2.setIdentity(self.kf.processNoiseCov, 0.1)16 # R: measurement noise covariance17 self.kf.measurementNoiseCov = cv2.setIdentity(self.kf.measurementNoiseCov, 0.1)18 # self.kf.errorCovPost = cv2.setIdentity(self.kf.errorCovPost, 1e-4)19 # Q, the process noise covariance, contributes to the overall uncertainty.20 # When Q is large, the Kalman Filter tracks large changes in the data more closely than for smaller Q.21 # R, the measurement noise covariance, determines how much information from the measurement is used.22 # If R is high, the Kalman Filter considers the measurements as not very accurate. For smaller R it will follow the measurements more closely.23 def predict(self, dt):24 self.kf.transitionMatrix = np.array(25 [[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]], np.float3226 )27 prediction = self.kf.predict()28 self.rotation_value = prediction[0][0]29 temp_rotation_value = self.rotation_value30 if temp_rotation_value > 0:31 while temp_rotation_value > (2 * np.pi):32 temp_rotation_value -= 2 * np.pi33 if temp_rotation_value < 0:34 while temp_rotation_value < 0:35 temp_rotation_value += 2 * np.pi36 self.orientation = int(temp_rotation_value * 180 / np.pi)37 def correct(self, orientation):38 measured_orientation_rad = orientation * np.pi / 180.039 temp_rotation_value = self.rotation_value40 if temp_rotation_value > 0:41 while temp_rotation_value > (2 * np.pi):42 temp_rotation_value -= 2 * np.pi43 if temp_rotation_value < 0:44 while temp_rotation_value < 0:45 temp_rotation_value += 2 * np.pi46 rotation_diff = measured_orientation_rad - temp_rotation_value47 # * this is between -2PI and 2PI, but if it is more than PI, it is closer from the other direction48 if rotation_diff < -1 * np.pi:49 rotation_diff += (2 * np.pi)50 if rotation_diff > np.pi:51 rotation_diff -= (2 * np.pi)52 measurement = self.rotation_value + rotation_diff53 corrected = self.kf.correct(np.array([measurement], np.float32))54 self.rotation_value = corrected[0][0]55 # convert to degrees to draw in opencv56 temp_rotation_value = self.rotation_value57 if temp_rotation_value > 0:58 while temp_rotation_value > (2 * np.pi):59 temp_rotation_value -= 2 * np.pi60 if temp_rotation_value < 0:61 while temp_rotation_value < 0:62 temp_rotation_value += 2 * np.pi...`

## Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.