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.