calculate magnetic field strength from raw magnetometer
13631
def get_motor_offsets(SERVO_OUTPUT_RAW, ofs, motor_ofs):
    ''''''
    import mavutil
    self = mavutil.mavfile_global

    m = SERVO_OUTPUT_RAW
    motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw
    motor_pwm *= 0.25
    rc3_min = self.param('RC3_MIN', 1100)
    rc3_max = self.param('RC3_MAX', 1900)
    motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min)
    if motor > 1.0:
        motor = 1.0
    if motor < 0.0:
        motor = 0.0

    motor_offsets0 = motor_ofs[0] * motor
    motor_offsets1 = motor_ofs[1] * motor
    motor_offsets2 = motor_ofs[2] * motor
    ofs = (ofs[0] + motor_offsets0, ofs[1] + motor_offsets1, ofs[2] + motor_offsets2)

    return ofs
442663
def _g(self, h, xp, s):
        """"""

        nphi = sum(self.phi)

        return (nphi / 2.0) * log(2 * pi) + nphi * \
            log(s) + 0.5 * sum((h - xp) ** 2) / (s ** 2)
747021
def mag_discrepancy(RAW_IMU, ATTITUDE, inclination, declination=None):
    ''''''
    if declination is None:
        import mavutil
        declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
    expected = expected_mag(RAW_IMU, ATTITUDE, inclination, declination)
    mag = Vector3(RAW_IMU.xmag, RAW_IMU.ymag, RAW_IMU.zmag)
    return degrees(expected.angle(mag))
472098
def instantaneous_tpole(self):
        
        logger.debug("{}.instantaneous_tpole".format(self.component))

        if 'tpole' not in self.inst_vals.keys():
            logger.debug("{}.instantaneous_tpole COMPUTING".format(self.component))

            if self.mesh is None:
                raise ValueError("mesh must be computed before determining tpole")
            # Convert from mean to polar by dividing total flux by gravity darkened flux (Ls drop out)
            # see PHOEBE Legacy scientific reference eq 5.20
            self.inst_vals['tpole'] = self.teff*(np.sum(self.mesh.areas) / np.sum(self.mesh.gravs.centers*self.mesh.areas))**(0.25)

        return self.inst_vals['tpole']
1193238
def mag_heading(RAW_IMU, ATTITUDE, declination=None, SENSOR_OFFSETS=None, ofs=None):
    ''''''
    if declination is None:
        import mavutil
        declination = degrees(mavutil.mavfile_global.param('COMPASS_DEC', 0))
    mag_x = RAW_IMU.xmag
    mag_y = RAW_IMU.ymag
    mag_z = RAW_IMU.zmag
    if SENSOR_OFFSETS is not None and ofs is not None:
        mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
        mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
        mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z

    # go via a DCM matrix to match the APM calculation
    dcm_matrix = rotation(ATTITUDE)
    cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x)
    headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y
    headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z)

    heading = degrees(atan2(-headY,headX)) + declination
    if heading < 0:
        heading += 360
    return heading
116907
def get(self, nb=0):
        """"""
        return {i: self.stats_history[i].history_raw(nb=nb) for i in self.stats_history}
121910
def _calculate_ms_from_base(self, size):
        """"""
        hw = size.x * 0.5
        hh = size.y * 0.5

        a = datatypes.Point(hw, hh).get_rotated(self.angle)
        b = datatypes.Point(-hw, hh).get_rotated(self.angle)
        c = datatypes.Point(hw, -hh).get_rotated(self.angle)
        d = datatypes.Point(-hw, -hh).get_rotated(self.angle)

        minp = a.get_minimum(b).get_minimum(c).get_minimum(d)
        maxp = a.get_maximum(b).get_maximum(c).get_maximum(d)
        return maxp - minp
522745
def magfit(logfile):
    ''''''

    print("Processing log %s" % filename)
    mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)

    data = []

    last_t = 0
    offsets = Vector3(0,0,0)

    # now gather all the data
    while True:
        m = mlog.recv_match(condition=args.condition)
        if m is None:
            break
        if m.get_type() == "SENSOR_OFFSETS":
            # update current offsets
            offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
        if m.get_type() == "RAW_IMU":
            mag = Vector3(m.xmag, m.ymag, m.zmag)
            # add data point after subtracting the current offsets
            data.append(mag - offsets + noise())
        if m.get_type() == "MAG" and not args.mag2:
            offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
            mag = Vector3(m.MagX,m.MagY,m.MagZ)
            data.append(mag - offsets + noise())
        if m.get_type() == "MAG2" and args.mag2:
            offsets = Vector3(m.OfsX,m.OfsY,m.OfsZ)
            mag = Vector3(m.MagX,m.MagY,m.MagZ)
            data.append(mag - offsets + noise())

    print("Extracted %u data points" % len(data))
    print("Current offsets: %s" % offsets)

    orig_data = data

    data = select_data(data)

    # remove initial outliers
    data.sort(lambda a,b : radius_cmp(a,b,offsets))
    data = data[len(data)/16:-len(data)/16]

    # do an initial fit
    (offsets, field_strength) = fit_data(data)

    for count in range(3):
        # sort the data by the radius
        data.sort(lambda a,b : radius_cmp(a,b,offsets))

        print("Fit %u    : %s  field_strength=%6.1f to %6.1f" % (
            count, offsets,
            radius(data[0], offsets), radius(data[-1], offsets)))

        # discard outliers, keep the middle 3/4
        data = data[len(data)/8:-len(data)/8]

        # fit again
        (offsets, field_strength) = fit_data(data)

    print("Final    : %s  field_strength=%6.1f to %6.1f" % (
        offsets,
        radius(data[0], offsets), radius(data[-1], offsets)))

    if args.plot:
        plot_data(orig_data, data)
122946
def calibrate_bt(array, attributes, index, band_name):
    """"""
    offset = np.float32(attributes["radiance_offsets"][index])
    scale = np.float32(attributes["radiance_scales"][index])

    array = (array - offset) * scale

    # Planck constant (Joule second)
    h__ = np.float32(6.6260755e-34)

    # Speed of light in vacuum (meters per second)
    c__ = np.float32(2.9979246e+8)

    # Boltzmann constant (Joules per Kelvin)
    k__ = np.float32(1.380658e-23)

    # Derived constants
    c_1 = 2 * h__ * c__ * c__
    c_2 = (h__ * c__) / k__

    # Effective central wavenumber (inverse centimeters)
    cwn = np.array([
        2.641775E+3, 2.505277E+3, 2.518028E+3, 2.465428E+3,
        2.235815E+3, 2.200346E+3, 1.477967E+3, 1.362737E+3,
        1.173190E+3, 1.027715E+3, 9.080884E+2, 8.315399E+2,
        7.483394E+2, 7.308963E+2, 7.188681E+2, 7.045367E+2],
        dtype=np.float32)

    # Temperature correction slope (no units)
    tcs = np.array([
        9.993411E-1, 9.998646E-1, 9.998584E-1, 9.998682E-1,
        9.998819E-1, 9.998845E-1, 9.994877E-1, 9.994918E-1,
        9.995495E-1, 9.997398E-1, 9.995608E-1, 9.997256E-1,
        9.999160E-1, 9.999167E-1, 9.999191E-1, 9.999281E-1],
        dtype=np.float32)

    # Temperature correction intercept (Kelvin)
    tci = np.array([
        4.770532E-1, 9.262664E-2, 9.757996E-2, 8.929242E-2,
        7.310901E-2, 7.060415E-2, 2.204921E-1, 2.046087E-1,
        1.599191E-1, 8.253401E-2, 1.302699E-1, 7.181833E-2,
        1.972608E-2, 1.913568E-2, 1.817817E-2, 1.583042E-2],
        dtype=np.float32)

    # Transfer wavenumber [cm^(-1)] to wavelength [m]
    cwn = 1. / (cwn * 100)

    # Some versions of the modis files do not contain all the bands.
    emmissive_channels = ["20", "21", "22", "23", "24", "25", "27", "28", "29",
                          "30", "31", "32", "33", "34", "35", "36"]
    global_index = emmissive_channels.index(band_name)

    cwn = cwn[global_index]
    tcs = tcs[global_index]
    tci = tci[global_index]
    array = c_2 / (cwn * xu.log(c_1 / (1000000 * array * cwn ** 5) + 1))
    array = (array - tci) / tcs
    return array
501714
def expected_magy(RAW_IMU, ATTITUDE, inclination, declination):
    ''''''
    v = expected_mag(RAW_IMU, ATTITUDE, inclination, declination)
    return v.y
273166
def evaluate(self, x, *args):
        """"""
        xx = x / self.x_0
        y = (self.amplitude * xx ** (-self.alpha)) * self._flux_unit
        flux = units.convert_flux(x, y, units.PHOTLAM)
        return flux.value
770581
def calculate_mag_calibration(self, mag_samples):
        """"""

        max_vals = [mag_samples[0][0], mag_samples[2][1], mag_samples[3][2]]
        min_vals = [mag_samples[1][0], mag_samples[3][1], mag_samples[5][2]]

        magbiases = [int((max_vals[i] + min_vals[i]) / 2.0) for i in range(3)]
        magscalings = [(max_vals[i] - min_vals[i]) / 2.0 for i in range(3)]
        avg_rads = sum(magscalings) / 3.0
        magscalings = [avg_rads / magscalings[i] for i in range(3)]

        data = self.calibration_data[self.current_imuid]

        data[self.MAGX_OFFSET] = str(int(magbiases[0]))
        data[self.MAGY_OFFSET] = str(int(magbiases[1]))
        data[self.MAGZ_OFFSET] = str(int(magbiases[2]))
        
        data[self.MAGX_SCALE] = str(magscalings[0])
        data[self.MAGY_SCALE] = str(magscalings[1])
        data[self.MAGZ_SCALE] = str(magscalings[2])

        data[self.MAG_TIMESTAMP] = datetime.now().isoformat()

        self.write_calibration_data()
        self.update_data_display(self.calibration_data[self.current_imuid])
        self.calibration_state = self.CAL_NONE
1037641
def _loglr(self):
        
        """
        # calculate <d-h|d-h> = <h|h> - 2<h|d> + <d|d> up to a constant
        p = self.current_params.copy()
        p.update(self.static_params)

        if self.time is None:
            self.time = p['tc']

        shloglr = hhloglr = 0
        for ifo in self.sh:
            fp, fc = self.det[ifo].antenna_pattern(p['ra'], p['dec'],
                                                   p['polarization'],
                                                   self.time)
            dt = self.det[ifo].time_delay_from_earth_center(p['ra'],
                                                            p['dec'],
                                                            self.time)
            ip = numpy.cos(p['inclination'])
            ic = 0.5 * (1.0 + ip * ip)
            htf = (fp * ip + 1.0j * fc * ic) / p['distance']

            sh = self.sh[ifo].at_time(p['tc'] + dt) * htf
            shloglr += sh
            hhloglr += self.hh[ifo] * abs(htf) ** 2.0

        vloglr = numpy.log(scipy.special.i0e(abs(shloglr)))
        vloglr += abs(shloglr) + hhloglr

        return float(vloglr)
611427
def rawData(self, fileAdres=None):
        # skip skiprows
        skiprows = None
        skip_from = [b'Field', b'Moment']
        with open(fileAdres, 'rb') as fr:
            #f = fr.read()
            for i, line in enumerate(fr, 1):
                # print(line.split())
                if skip_from == line.split():
                    skiprows = i+2
                    break
                # else:
                #    print('file format wrong, cannot find the data row.')
        skiprows = 34 if skiprows == None else skiprows
        df = pd.read_csv(fileAdres, skiprows=skiprows, sep='\s+',
                         delimiter=',', names=['H', 'M'], skipfooter=1,
                         engine='python')

        H = df.H  # measured field
        M = df.M  # measured magnetic moment
        '''
        
        '''
        dataInterval_H = []
        dataInterval_M = []
        # print(H)
        cretia = df.H.mean()  # edge of linear programing for selecting data
        H0 = df.H.max()  # the maximum field
        self.x, self.y, self.z = [[], [], []]
        for i in np.arange(1, len(H)):
            dataInterval_H.append(H[i])
            dataInterval_M.append(M[i])
            if abs(H[i]-H0) <= 0.001:  # when the filed reach the max, a new forc
                if len(dataInterval_H) >= 0 and len(dataInterval_H) <= 200:
                    # print(dataInterval_H)
                    Ha = dataInterval_H[0]
                    dataInterval_H.pop(-1)
                    dataInterval_M.pop(-1)
                    Hb = dataInterval_H[1:-1]
                    Hm = dataInterval_M[1:-1]
                    for t in np.arange(len(Hb)):
                        self.x.append(Hb[t])
                        self.y.append(Ha)
                        self.z.append(Hm[t])
                        # print(Ha)
                dataInterval_H = []
                dataInterval_M = []
        self.rawdf = df
        '''
        #=================================================
        transfer the data set to matrix as len(x)*len(y) with z value
        /mesh up the rawdata
        /select the data area by X,Y ranges
        /obtain regular spaced data potins by np.linspace
        /use interplote to caculate the Hm values
        /loop Ha(Y),Hb(X)
        /fill every position with Hm, else with np.nan
        #=================================================
        '''
        self.z = self.z/np.max(self.z)
        # print(int(np.min(self.x)*100)/100,np.max(self.x))
        xrange = [int((np.min(self.x)-0.1)*10)/10,
                  int((np.max(self.x)+0.1)*10)/10]
        yrange = [int((np.min(self.y)-0.1)*10)/10,
                  int((np.max(self.y)+0.1)*10)/10]
        X = np.linspace(xrange[0], xrange[1], 200)
        Y = np.linspace(yrange[0], yrange[1], 200)
        yi, xi = np.mgrid[yrange[0]:yrange[1]:200j, xrange[0]:xrange[1]:200j]

        #X = np.linspace(-0.2,0.3,200)
        #Y = np.linspace(-0.2,0.3,200)
        #xi,yi = np.mgrid[-0.2:0.3:200j,-0.2:0.3:200j]

        zi = griddata((self.x, self.y), self.z, (xi, yi),
                      method='linear')  # !!! must linear
        self.matrix_z = zi
        self.x_range = X
        self.y_range = Y
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;
}
476042
def read_pressure(self):
        """"""
        UT = self.read_raw_temp()
        UP = self.read_raw_pressure()
        # Datasheet values for debugging:
        #UT = 27898
        #UP = 23843
        # Calculations below are taken straight from section 3.5 of the datasheet.
        # Calculate true temperature coefficient B5.
        X1 = ((UT - self.cal_AC6) * self.cal_AC5) >> 15
        X2 = (self.cal_MC << 11) // (X1 + self.cal_MD)
        B5 = X1 + X2
        self.logger.debug('B5 = {0}', B5)
        # Pressure Calculations
        B6 = B5 - 4000
        self.logger.debug('B6 = {0}', B6)
        X1 = (self.cal_B2 * (B6 * B6) >> 12) >> 11
        X2 = (self.cal_AC2 * B6) >> 11
        X3 = X1 + X2
        B3 = (((self.cal_AC1 * 4 + X3) << self._mode) + 2) // 4
        self.logger.debug('B3 = {0}', B3)
        X1 = (self.cal_AC3 * B6) >> 13
        X2 = (self.cal_B1 * ((B6 * B6) >> 12)) >> 16
        X3 = ((X1 + X2) + 2) >> 2
        B4 = (self.cal_AC4 * (X3 + 32768)) >> 15
        self.logger.debug('B4 = {0}', B4)
        B7 = (UP - B3) * (50000 >> self._mode)
        self.logger.debug('B7 = {0}', B7)
        if B7 < 0x80000000:
            p = (B7 * 2) // B4
        else:
            p = (B7 // B4) * 2
        X1 = (p >> 8) * (p >> 8)
        X1 = (X1 * 3038) >> 16
        X2 = (-7357 * p) >> 16
        p = p + ((X1 + X2 + 3791) >> 4)
        self.logger.debug('Pressure {0} Pa', p)
        return p/100
1112188
def roll_estimate(RAW_IMU,GPS_RAW_INT=None,ATTITUDE=None,SENSOR_OFFSETS=None, ofs=None, mul=None,smooth=0.7):
    ''''''
    rx = RAW_IMU.xacc * 9.81 / 1000.0
    ry = RAW_IMU.yacc * 9.81 / 1000.0
    rz = RAW_IMU.zacc * 9.81 / 1000.0
    if ATTITUDE is not None and GPS_RAW_INT is not None:
        ry -= ATTITUDE.yawspeed * GPS_RAW_INT.vel*0.01
        rz += ATTITUDE.pitchspeed * GPS_RAW_INT.vel*0.01
    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 lowpass(degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))),'_roll',smooth)