calculate EKF position when EKF disabled
965448
def ekf1_pos(EKF1):
  ''''''
  global ekf_home
  from . import mavutil
  self = mavutil.mavfile_global
  if ekf_home is None:
      if not 'GPS' in self.messages or self.messages['GPS'].Status != 3:
          return None
      ekf_home = self.messages['GPS']
      (ekf_home.Lat, ekf_home.Lng) = gps_offset(ekf_home.Lat, ekf_home.Lng, -EKF1.PE, -EKF1.PN)
  (lat,lon) = gps_offset(ekf_home.Lat, ekf_home.Lng, EKF1.PE, EKF1.PN)
  return (lat, lon)
1290350
def get_position(self):
        
        with self._lock:
            return self.x, self.y, self.angle
719924
def get_position_i(self):
        
        data = []
        data.append(0x09)
        data.append(self.servoid)
        data.append(RAM_READ_REQ)
        data.append(POSITION_KI_RAM)
        data.append(BYTE2)
        send_data(data)
        rxdata = []
        try:
            rxdata = SERPORT.read(13)
            return (ord(rxdata[10])*256)+(ord(rxdata[9])&0xff)
        except HerkulexError:
            raise HerkulexError("Could not read from motors")
21537
def get_scaled_state(self):
        ''''''
        return [0.5 * (self.x + self.position_limit) / self.position_limit,
                (self.dx + 0.75) / 1.5,
                0.5 * (self.theta + self.angle_limit_radians) / self.angle_limit_radians,
                (self.dtheta + 1.0) / 2.0]
1219584
def on_motion(self, event):
        ''
        if self.press is None:
            return

        if event.inaxes != self.ax:
            return
        # print(event.inaxes)

        x0, y0, btn = self.press
        x0.append(event.xdata)
        y0.append(event.ydata)
1370634
def stationarystate(self, k):
        """"""
        assert 0 <= k < self.ncats
        return self._models[k].stationarystate
1194659
def set_default_pos(self, defaultPos):
        """"""
        self.coords = defaultPos
        self.velocity = r.Vector2()
        self.desired_position = defaultPos
        r.Ragnarok.get_world().Camera.pan = self.coords
        r.Ragnarok.get_world().Camera.desired_pan = self.coords
91998
def get_controller_state(self):
        """"""
        dpos = self.control[:3] * 0.005
        roll, pitch, yaw = self.control[3:] * 0.005
        self.grasp = self.control_gripper

        # convert RPY to an absolute orientation
        drot1 = rotation_matrix(angle=-pitch, direction=[1., 0, 0], point=None)[:3, :3]
        drot2 = rotation_matrix(angle=roll, direction=[0, 1., 0], point=None)[:3, :3]
        drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.], point=None)[:3, :3]

        self.rotation = self.rotation.dot(drot1.dot(drot2.dot(drot3)))

        return dict(
            dpos=dpos, rotation=self.rotation, grasp=self.grasp, reset=self._reset_state
        )
408998
function () {
                return this._attachedCamera.inertialAlphaOffset !== 0 ||
                    this._attachedCamera.inertialBetaOffset !== 0 ||
                    this._attachedCamera.inertialRadiusOffset !== 0 ||
                    this._attachedCamera.inertialPanningX !== 0 ||
                    this._attachedCamera.inertialPanningY !== 0 ||
                    this._isPointerDown;
            }
852589
private boolean estimateMotion() {
		List<PointTrack> active = tracker.getActiveTracks(null);
		List<Point2D3D> obs = new ArrayList<>();

		for( PointTrack t : active ) {
			Point2D3D p = t.getCookie();
			pixelToNorm.compute( t.x , t.y , p.observation );
			obs.add( p );
		}

		// estimate the motion up to a scale factor in translation
		if( !motionEstimator.process( obs ) )
			return false;

		if( doublePass ) {
			if (!performSecondPass(active, obs))
				return false;
		}
		tracker.finishTracking();

		Se3_F64 keyToCurr;

		if( refine != null ) {
			keyToCurr = new Se3_F64();
			refine.fitModel(motionEstimator.getMatchSet(), motionEstimator.getModelParameters(), keyToCurr);
		} else {
			keyToCurr = motionEstimator.getModelParameters();
		}

		keyToCurr.invert(currToKey);

		// mark tracks as being inliers and add to inlier list
		int N = motionEstimator.getMatchSet().size();
		for( int i = 0; i < N; i++ ) {
			int index = motionEstimator.getInputIndex(i);
			Point2D3DTrack t = active.get(index).getCookie();
			t.lastInlier = tick;
			inlierTracks.add( t );
		}

		return true;
	}
984397
def on_for_degrees(self, steering, speed, degrees, brake=True, block=True):
        
        (left_speed, right_speed) = self.get_speed_steering(steering, speed)
        MoveTank.on_for_degrees(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), degrees, brake, block)
1134901
function (raw) {
  var data = {};

  // consolidate various sensor data into more localized representations
  data.wheels = {
    right: {
      dropped: raw.bump_and_wheel_drop.wheel_drop.right,
      overcurrent: raw.low_side_driver_and_wheel_overcurrents.wheel.right
    },
    left: {
      dropped: raw.bump_and_wheel_drop.wheel_drop.left,
      overcurrent: raw.low_side_driver_and_wheel_overcurrents.wheel.left
    },
    caster: {
      dropped: raw.bump_and_wheel_drop.wheel_drop.caster
    }
  };

  // give the low side drivers explicit overcurrent properties
  data.low_side_drivers = _.map(raw.low_side_driver_and_wheel_overcurrents.low_side_drivers, function (lsd) {
    return  { overcurrent: lsd }
  });

  data.bumpers = {
    left: { activated: raw.bump_and_wheel_drop.bump.left },
    right: { activated: raw.bump_and_wheel_drop.bump.right },
    both: {
      // true if both bumpers are activated at once
      activated: (raw.bump_and_wheel_drop.bump.right &&
                  raw.bump_and_wheel_drop.bump.left)
    }
  };

  data.cliff_sensors = {
    left: {
      detecting: raw.cliff_left,
      signal: raw.cliff_left_signal
    },
    front_left: {
      detecting: raw.cliff_front_left,
      signal: raw.cliff_front_left_signal
    },
    front_right: {
      detecting: raw.cliff_front_right,
      signal: raw.cliff_front_right_signal
    },
    right: {
      detecting: raw.cliff_right,
      signal: raw.cliff_right_signal
    },
  };

  data.wall_sensor = {
    detecting: raw.wall,
    signal: raw.wall_signal
  };

  data.virtual_wall_sensor = {
    detecting: raw.virtual_wall
  };

  // TODO: verify that this works
  data.ir = {
    receiving: raw.infrared_byte.receiving,
    received_value: raw.infrared_byte.receiving ? raw.infrared_byte.value : null
  };

  data.buttons = {
    advance: { pressed: raw.buttons.advance },
    play: { pressed: raw.buttons.play }
  };

  data.battery = {
    charging: {
      recharging: !raw.charging_state.not_charging,
      from: raw.charging_sources_available,
      type: {
        reconditioning: raw.charging_state.reconditioning_charging,
        full: raw.charging_state.full_charging,
        trickle: raw.charging_state.trickle_charging,
        waiting: raw.charging_state.waiting,
        fault: raw.charging_state.charging_fault_condition
      }
    },

    voltage: {
      volts: raw.voltage.millivolts / 1000,
      millivolts: raw.voltage.millivolts
    },

    current: {
      amps: raw.current.milliamps / 1000,
      milliamps: raw.current.milliamps
    },

    temperature: {
      celsius: raw.battery_temperature.celsius,
      fahrenheit: raw.battery_temperature.celsius * 9 / 5 + 32,
    },

    capacity: {
      percent: (raw.battery_charge.milliamp_hours /
                raw.battery_capacity.milliamp_hours),
      current: raw.battery_charge,
      max: raw.battery_capacity,
    }
  };

  data.cargo_bay = {
    // these are both the same value since they come to the same cargo bay pin,
    // but it will make for more readable client code to separate them.
    device_detected: raw.cargo_bay_digital_inputs.device_detect_baudrate_change,
    baudrate_change_requested: raw.cargo_bay_digital_inputs.device_detect_baudrate_change,

    digital_input: raw.cargo_bay_digital_inputs.digital_inputs,
    analog_signal: raw.cargo_bay_analog_signal
  };

  data.song = {
    playing: raw.song_playing,
    number: raw.song_number
  };

  data.state = {
    mode: raw.oi_mode,
    distance: raw.distance,
    angle: raw.angle,

    requested_velocity: raw.requested_velocity,
    requested_radius: raw.requested_radius,
    requested_right_velocity: raw.requested_right_velocity,
    requested_left_velocity: raw.requested_left_velocity,
  };

  // NOTE: we ignore "number_stream_packets" (who would need that here anyway?)

  return data;
}
1019421
def _at(self, t):
        """"""
        rITRF, vITRF, error = self.ITRF_position_velocity_error(t)
        rGCRS, vGCRS = ITRF_to_GCRS2(t, rITRF, vITRF)
        return rGCRS, vGCRS, rGCRS, error
232528
def on_for_rotations(self, steering, speed, rotations, brake=True, block=True):
        
        (left_speed, right_speed) = self.get_speed_steering(steering, speed)
        MoveTank.on_for_rotations(self, SpeedNativeUnits(left_speed), SpeedNativeUnits(right_speed), rotations, brake, block)
71977
def get_azimuth_plunge(vect, degrees=True):
    '''
    
    '''
    if vect[0] > 0:
        vect = -1. * np.copy(vect)
    vect_hor = sqrt(vect[1] ** 2. + vect[2] ** 2.)
    plunge = atan2(-vect[0], vect_hor)
    azimuth = atan2(vect[2], -vect[1])
    if degrees:
        icr = 180. / pi
        return icr * azimuth % 360., icr * plunge
    else:
        return azimuth % (2. * pi), plunge
1043314
def parse_motion_state(val):
        """"""
        number = val & 0b00111111
        unit = (val & 0b11000000) >> 6
        if unit == 1:
            number *= 60 # minutes
        elif unit == 2:
            number *= 60 * 60 # hours
        elif unit == 3 and number < 32:
            number *= 60 * 60 * 24 # days
        elif unit == 3:
            number -= 32
            number *= 60 * 60 * 24 * 7 # weeks
        return number
593695
def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7):
    ''''''
    if hasattr(RAW_IMU, 'xacc'):
        rx = RAW_IMU.xacc * 9.81 / 1000.0
        ry = RAW_IMU.yacc * 9.81 / 1000.0
        rz = RAW_IMU.zacc * 9.81 / 1000.0
    else:
        rx = RAW_IMU.AccX
        ry = RAW_IMU.AccY
        rz = RAW_IMU.AccZ
    if SENSOR_OFFSETS is not None and ofs is not None:
        rx += SENSOR_OFFSETS.accel_cal_x
        ry += SENSOR_OFFSETS.accel_cal_y
        rz += SENSOR_OFFSETS.accel_cal_z
        rx -= ofs[0]
        ry -= ofs[1]
        rz -= ofs[2]
        if mul is not None:
            rx *= mul[0]
            ry *= mul[1]
            rz *= mul[2]
    return sqrt(rx**2+ry**2+rz**2)