static int numusehash (const Table *t, int *nums, int *pnasize)
{
    int totaluse = 0;  /* total number of elements */
    int ause = 0;  /* summation of `nums' */
    int i = sizenode(t);
    while (i--) {
        Node *n = &t->node[i];
        if (!ttisnil(gval(n))) {
            ause += countint(key2tval(n), nums);
            totaluse++;
        }
    }
    *pnasize += ause;
    return totaluse;
}
Exemple #2
0
static int traverseephemeron (global_State *g, Table *h) {
  int marked = 0;  /* true if an object is marked in this traversal */
  int hasclears = 0;  /* true if table has white keys */
  int prop = 0;  /* true if table has entry "white-key -> white-value" */
  Node *n, *limit = gnodelast(h);
  int i;
  /* traverse array part (numeric keys are 'strong') */
  for (i = 0; i < h->sizearray; i++) {
    if (valiswhite(&h->array[i])) {
      marked = 1;
      reallymarkobject(g, gcvalue(&h->array[i]));
    }
  }
  /* traverse hash part */
  for (n = gnode(h, 0); n < limit; n++) {
    checkdeadkey(n);
    if (ttisnil(gval(n)))  /* entry is empty? */
      removeentry(n);  /* remove it */
    else if (iscleared(g, gkey(n))) {  /* key is not marked (yet)? */
      hasclears = 1;  /* table must be cleared */
      if (valiswhite(gval(n)))  /* value not marked yet? */
        prop = 1;  /* must propagate again */
    }
    else if (valiswhite(gval(n))) {  /* value not marked yet? */
      marked = 1;
      reallymarkobject(g, gcvalue(gval(n)));  /* mark it now */
    }
  }
  if (prop)
    linktable(h, &g->ephemeron);  /* have to propagate again */
  else if (hasclears)  /* does table have white keys? */
    linktable(h, &g->allweak);  /* may have to clean white keys */
  else  /* no white keys */
    linktable(h, &g->grayagain);  /* no need to clean */
  return marked;
}
Exemple #3
0
const Tvalue *kp_table_getint(Table *t, int key)
{
	Node *n;

	if ((unsigned int)(key - 1) < (unsigned int)t->sizearray)
		return &t->array[key - 1];

	n = hashnum(t, key);
	do {
		if (ttisnumber(gkey(n)) && nvalue(gkey(n)) == key)
			return gval(n);
		else
			n = gnext(n);
	} while (n);

	return ktap_nilobject;
}
  void getGradient(Vector<Real> &g, SampleGenerator<Real> &sampler) {
    RiskVector<Real> &gs = Teuchos::dyn_cast<RiskVector<Real> >(g);

    std::vector<Real> mygval(3,0.0), gval(3,0.0);
    mygval[0] = RiskMeasure<Real>::val_;
    mygval[1] = valLam_;
    mygval[2] = valMu_;
    sampler.sumAll(&mygval[0],&gval[0],3);

    std::vector<Real> stat(2,0.0);
    stat[0] = thresh_ + gval[0] + gval[1];
    stat[1] = (Real)1 + gval[2];
    gs.setStatistic(stat);

    sampler.sumAll(*(RiskMeasure<Real>::g_),*dualVector_);
    gs.setVector(*dualVector_);
  }
Exemple #5
0
/*
** clear collected values from weaktables
*/
static void cleartablevalues (lua_State *L, GCObject *l) {
  while (l) {
    Table *h = gcotoh(l);
    int i = h->sizearray;
    lua_assert(h->marked & VALUEWEAK);
    while (i--) {
      TObject *o = &h->array[i];
      if (!valismarked(o))  /* value was collected? */
        setnilvalue(o);  /* remove value */
    }
    i = sizenode(h);
    while (i--) {
      Node *n = gnode(h, i);
      if (!valismarked(gval(n)))  /* value was collected? */
        removekey(L, n);  /* remove entry from table */
    }
    l = h->gclist;
  }
}
Exemple #6
0
NAMESPACE_LUA_BEGIN

#define GCSTEPSIZE	1024u
#define GCSWEEPMAX	40
#define GCSWEEPCOST	10
#define GCFINALIZECOST	100


#define maskmarks	cast_byte(~(bitmask(BLACKBIT)|WHITEBITS))

#define makewhite(g,x)	\
   ((x)->gch.marked = cast_byte(((x)->gch.marked & maskmarks) | luaC_white(g)))

#define white2gray(x)	reset2bits((x)->gch.marked, WHITE0BIT, WHITE1BIT)
#define black2gray(x)	resetbit((x)->gch.marked, BLACKBIT)

#define stringmark(s)	reset2bits((s)->tsv.marked, WHITE0BIT, WHITE1BIT)


#define isfinalized(u)		testbit((u)->marked, FINALIZEDBIT)
#define markfinalized(u)	l_setbit((u)->marked, FINALIZEDBIT)


#define KEYWEAK         bitmask(KEYWEAKBIT)
#define VALUEWEAK       bitmask(VALUEWEAKBIT)



#define markvalue(g,o) { checkconsistency(o); \
  if (iscollectable(o) && iswhite(gcvalue(o))) reallymarkobject(g,gcvalue(o)); }

#define markobject(g,t) { if (iswhite(obj2gco(t))) \
		reallymarkobject(g, obj2gco(t)); }


#define setthreshold(g)  (g->GCthreshold = (g->estimate/100) * g->gcpause)


static void removeentry (Node *n) {
  lua_assert(ttisnil(gval(n)));
  if (iscollectable(gkey(n)))
    setttype(gkey(n), LUA_TDEADKEY);  /* dead key; remove it */
}
Exemple #7
0
void kp_tab_setvalue(ktap_tab_t *t, const ktap_val_t *key, ktap_val_t *val)
{
	const ktap_val_t *v = kp_tab_get(t, key);

	if (v != niltv) {
		set_obj((ktap_val_t *)v, val);
	} else {
		if (t->freetop == t->node) {
			int size = (t->hmask + 1) * sizeof(ktap_node_t);
			t->node = realloc(t->node, size * 2);
			memset(t->node + t->hmask + 1, 0, size);
			t->freetop = t->node + (t->hmask + 1) * 2;
			t->hmask = (t->hmask + 1) * 2 - 1;
		}

		ktap_node_t *n = --t->freetop;
		set_obj(gkey(n), key);
		set_obj(gval(n), val);
	}
}
Exemple #8
0
void kp_table_dump(ktap_State *ks, Table *t)
{
	int i, count = 0;

	kp_printf(ks, "{");
	for (i = 0; i < t->sizearray; i++) {
		Tvalue *v = &t->array[i];

		if (isnil(v))
			continue;

		if (count)
			kp_printf(ks, ", ");

		kp_printf(ks, "(%d: ", i + 1);
		kp_showobj(ks, v);
		kp_printf(ks, ")");
		count++;
	}

	for (i = 0; i < sizenode(t); i++) {
		Node *n = &t->node[i];

		if (isnil(gkey(n)))
			continue;

		if (count)
			kp_printf(ks, ", ");

		kp_printf(ks, "(");
		kp_showobj(ks, gkey(n));
		kp_printf(ks, ": ");
		kp_showobj(ks, gval(n));
		kp_printf(ks, ")");
		count++;
	}
	kp_printf(ks, "}");
}
Exemple #9
0
static void removekey (Node *n) {
    setnilvalue(gval(n));  /* remove corresponding value ... */
    if (iscollectable(gkey(n)))
        setttype(gkey(n), LUA_TNONE);  /* dead key; remove it */
}
Exemple #10
0
void
L3GD20::measure()
{
	/* status register and data as read back from the device */
#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		int8_t		temp;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_report;
#pragma pack(pop)

	gyro_report report;

	/* start the performance counter */
	perf_begin(_sample_perf);

	check_registers();

	/* fetch data from the sensor */
	memset(&raw_report, 0, sizeof(raw_report));
	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));

	if (!(raw_report.status & STATUS_ZYXDA)) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		return;
	}

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_bad_registers);

	switch (_orientation) {

	case SENSOR_BOARD_ROTATION_000_DEG:
		/* keep axes in place */
		report.x_raw = raw_report.x;
		report.y_raw = raw_report.y;
		break;

	case SENSOR_BOARD_ROTATION_090_DEG:
		/* swap x and y */
		report.x_raw = raw_report.y;
		report.y_raw = raw_report.x;
		break;

	case SENSOR_BOARD_ROTATION_180_DEG:
		/* swap x and y and negate both */
		report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
		report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
		break;

	case SENSOR_BOARD_ROTATION_270_DEG:
		/* swap x and y and negate y */
		report.x_raw = raw_report.y;
		report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
		break;
	}

	report.z_raw = raw_report.z;

#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
	int16_t tx = -report.y_raw;
	int16_t ty = -report.x_raw;
	int16_t tz = -report.z_raw;
	report.x_raw = tx;
	report.y_raw = ty;
	report.z_raw = tz;
#endif




	report.temperature_raw = raw_report.temp;

	float xraw_f = report.x_raw;
	float yraw_f = report.y_raw;
	float zraw_f = report.z_raw;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	report.x = _gyro_filter_x.apply(xin);
	report.y = _gyro_filter_y.apply(yin);
	report.z = _gyro_filter_z.apply(zin);

	math::Vector<3> gval(xin, yin, zin);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
	report.x_integral = gval_integrated(0);
	report.y_integral = gval_integrated(1);
	report.z_integral = gval_integrated(2);

	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;

	report.scaling = _gyro_range_scale;
	report.range_rad_s = _gyro_range_rad_s;

	_reports->force(&report);

	if (gyro_notify) {
		/* notify anyone waiting for data */
		poll_notify(POLLIN);

		/* publish for subscribers */
		if (!(_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
		}
	}

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
Exemple #11
0
static void cleartable (lua_State *L, GCObject *l) {
#else
static void cleartable (GCObject *l) {
#endif /* LUA_REFCOUNT */
  while (l) {
    Table *h = gco2h(l);
    int i = h->sizearray;
    lua_assert(testbit(h->marked, VALUEWEAKBIT) ||
               testbit(h->marked, KEYWEAKBIT));
    if (testbit(h->marked, VALUEWEAKBIT)) {
      while (i--) {
        TValue *o = &h->array[i];
#if LUA_REFCOUNT
        if (iscleared(o, 0)) { /* value was collected? */
          if (iscollectable(o))
            o->value.gc->gch.ref--;
          setnilvalue2n(l, o);  /* remove value */
        }
#else
        if (iscleared(o, 0))  /* value was collected? */
          setnilvalue(o);  /* remove value */
#endif /* LUA_REFCOUNT */
      }
    }
    i = sizenode(h);
    while (i--) {
      Node *n = gnode(h, i);
      if (!ttisnil(gval(n)) &&  /* non-empty entry? */
          (iscleared(key2tval(n), 1) || iscleared(gval(n), 0))) {
#if LUA_REFCOUNT
        if (iscollectable(gval(n)))
          gval(n)->value.gc->gch.ref--;
        setnilvalue2n(L, gval(n));  /* remove value ... */
#else
        setnilvalue(gval(n));  /* remove value ... */
#endif /* LUA_REFCOUNT */
        removeentry(n);  /* remove entry from table */
      }
    }
    l = h->gclist;
  }
}


static void freeobj (lua_State *L, GCObject *o) {
  switch (o->gch.tt) {
    case LUA_TPROTO: luaF_freeproto(L, gco2p(o)); break;
    case LUA_TFUNCTION: luaF_freeclosure(L, gco2cl(o)); break;
    case LUA_TUPVAL: luaF_freeupval(L, gco2uv(o)); break;
    case LUA_TTABLE: luaH_free(L, gco2h(o)); break;
    case LUA_TTHREAD: {
      lua_assert(gco2th(o) != L && gco2th(o) != G(L)->mainthread);
      luaE_freethread(L, gco2th(o));
      break;
    }
    case LUA_TSTRING: {
      G(L)->strt.nuse--;
      luaM_freemem(L, o, sizestring(gco2ts(o)));
      break;
    }
#if LUA_WIDESTRING
    case LUA_TWSTRING: {
      G(L)->strt.nuse--;
      luaM_freemem(L, o, sizestring(gco2ts(o)));
      break;
    }
#endif /* LUA_WIDESTRING */
    case LUA_TUSERDATA: {
      luaM_freemem(L, o, sizeudata(gco2u(o)));
      break;
    }
    default: lua_assert(0);
  }
}



#define sweepwholelist(L,p)	sweeplist(L,p,MAX_LUMEM)


static GCObject **sweeplist (lua_State *L, GCObject **p, lu_mem count) {
  GCObject *curr;
  global_State *g = G(L);
  int deadmask = otherwhite(g);
  while ((curr = *p) != NULL && count-- > 0) {
    if (curr->gch.tt == LUA_TTHREAD)  /* sweep open upvalues of each thread */
      sweepwholelist(L, &gco2th(curr)->openupval);
    if ((curr->gch.marked ^ WHITEBITS) & deadmask) {  /* not dead? */
      lua_assert(!isdead(g, curr) || testbit(curr->gch.marked, FIXEDBIT));
      makewhite(g, curr);  /* make it white (for next cycle) */
      p = &curr->gch.next;
    }
    else {  /* must erase `curr' */
      lua_assert(isdead(g, curr) || deadmask == bitmask(SFIXEDBIT));
#if LUA_REFCOUNT
      if (curr->gch.prev)
        curr->gch.prev->gch.next = curr->gch.next;
      if (curr->gch.next)
        curr->gch.next->gch.prev = (GCObject*)p;
#endif /* LUA_REFCOUNT */
      *p = curr->gch.next;
      if (curr == g->rootgc)  /* is the first element of the list? */
        g->rootgc = curr->gch.next;  /* adjust first */
      freeobj(L, curr);
    }
  }
  return p;
}
Exemple #12
0
/* histogram: key should be number or string, value must be number */
void kp_table_histogram(ktap_State *ks, Table *t)
{
	struct table_hist_record *thr;
	char dist_str[40];
	int i, ratio, total = 0, count = 0;

	thr = kp_malloc(ks, sizeof(*thr) * (t->sizearray + sizenode(t)));

	for (i = 0; i < t->sizearray; i++) {
		Tvalue *v = &t->array[i];

		if (isnil(v))
			continue;

		if (!ttisnumber(v))
			goto error;

		setnvalue(&thr[count++].key, i + 1);
		total += nvalue(v);
	}

	for (i = 0; i < sizenode(t); i++) {
		Node *n = &t->node[i];
		int num;

		if (isnil(gkey(n)))
			continue;

		if (!ttisnumber(gval(n)))
			goto error;

		num = nvalue(gval(n));
		setobj(ks, &thr[count].key, gkey(n));
		setobj(ks, &thr[count].val, gval(n));
		count++;
		total += nvalue(gval(n));
	}

	sort(thr, count, sizeof(struct table_hist_record), hist_record_cmp, NULL);

	kp_printf(ks, "%32s%s%s\n", "value ", DISTRIBUTION_STR, " count");
	dist_str[sizeof(dist_str) - 1] = '\0';
	for (i = 0; i < count; i++) {
		Tvalue *key = &thr[i].key;
		Tvalue *val = &thr[i].val;

		memset(dist_str, ' ', sizeof(dist_str) - 1);
		ratio = (nvalue(val) * (sizeof(dist_str) - 1)) / total;
		memset(dist_str, '@', ratio);

		if (ttisstring(key)) {
			char buf[32 + 1] = {0};
			char *keystr;

			if (strlen(svalue(key)) > 32) {
				strncpy(buf, svalue(key), 32-4);
				memset(buf + 32-4, '.', 3);
				keystr = buf;
			} else
				keystr = svalue(key);

			kp_printf(ks, "%32s |%s%-10d\n", keystr, dist_str,
					nvalue(val));
		} else
			kp_printf(ks, "%32d | %s%-10d\n", nvalue(key),
					dist_str, nvalue(val));
	}

	goto out;

 error:
	kp_printf(ks, "error: table histogram only handle "
			" (key: string/number val: number)\n");
 out:
	kp_free(ks, thr);
}
Exemple #13
0
void
BMI160::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct BMIReport bmi_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the BMI160 in one pass.
	 */
	bmi_report.cmd = BMIREG_GYR_X_L | DIR_READ;

	uint8_t		status = read_reg(BMIREG_STATUS);

	if (OK != transfer((uint8_t *)&bmi_report, ((uint8_t *)&bmi_report), sizeof(bmi_report))) {
		return;
	}

	check_registers();

	if ((!(status & (0x80))) && (!(status & (0x04)))) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		_got_duplicate = true;
		return;
	}

	_last_accel[0] = bmi_report.accel_x;
	_last_accel[1] = bmi_report.accel_y;
	_last_accel[2] = bmi_report.accel_z;
	_got_duplicate = false;

	uint8_t temp_l = read_reg(BMIREG_TEMP_0);
	uint8_t temp_h = read_reg(BMIREG_TEMP_1);

	report.temp = ((temp_h << 8) + temp_l);

	report.accel_x = bmi_report.accel_x;
	report.accel_y = bmi_report.accel_y;
	report.accel_z = bmi_report.accel_z;

	report.gyro_x = bmi_report.gyro_x;
	report.gyro_y = bmi_report.gyro_y;
	report.gyro_z = bmi_report.gyro_z;

	if (report.accel_x == 0 &&
	    report.accel_y == 0 &&
	    report.accel_z == 0 &&
	    report.temp == 0 &&
	    report.gyro_x == 0 &&
	    report.gyro_y == 0 &&
	    report.gyro_z == 0) {
		// all zero data - probably a SPI bus error
		perf_count(_bad_transfers);
		perf_end(_sample_perf);
		// note that we don't call reset() here as a reset()
		// costs 20ms with interrupts disabled. That means if
		// the bmi160 does go bad it would cause a FMU failure,
		// regardless of whether another sensor is available,
		return;
	}

	perf_count(_good_transfers);

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again. We still increment
		// _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Report buffers.
	 */
	accel_report		arb;
	gyro_report		grb;

	/*
	 * Adjust and scale results to m/s^2.
	 */
	grb.timestamp = arb.timestamp = hrt_absolute_time();

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */


	/* NOTE: Axes have been swapped to match the board a few lines above. */

	arb.x_raw = report.accel_x;
	arb.y_raw = report.accel_y;
	arb.z_raw = report.accel_z;

	float xraw_f = report.accel_x;
	float yraw_f = report.accel_y;
	float zraw_f = report.accel_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	arb.x = _accel_filter_x.apply(x_in_new);
	arb.y = _accel_filter_y.apply(y_in_new);
	arb.z = _accel_filter_z.apply(z_in_new);

	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> aval_integrated;

	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
	arb.x_integral = aval_integrated(0);
	arb.y_integral = aval_integrated(1);
	arb.z_integral = aval_integrated(2);

	arb.scaling = _accel_range_scale;
	arb.range_m_s2 = _accel_range_m_s2;

	_last_temperature = 23 + report.temp * 1.0f / 512.0f;

	arb.temperature_raw = report.temp;
	arb.temperature = _last_temperature;

	/* return device ID */
	arb.device_id = _device_id.devid;

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	xraw_f = report.gyro_x;
	yraw_f = report.gyro_y;
	zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;

	/* return device ID */
	grb.device_id = _gyro->_device_id.devid;

	_accel_reports->force(&arb);
	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (accel_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify) {
		_gyro->parent_poll_notify();
	}

	if (accel_notify && !(_pub_blocked)) {
		/* log the time of this report */
		perf_begin(_controller_latency_perf);
		/* publish it */
		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
Exemple #14
0
static void marktmu (GCState *st) {
  GCObject *u;
#if LUA_REFCOUNT
  for (u = st->g->tmudata_head.next; u != (GCObject*)&st->g->tmudata_tail; u = u->gch.next) {
#else !LUA_REFCOUNT
  for (u = st->g->tmudata; u; u = u->gch.next) {
#endif LUA_REFCOUNT
    unmark(u);  /* may be marked, if left from previous GC */
    reallymarkobject(st, u);
  }
}


/* move `dead' udata that need finalization to list `tmudata' */
size_t luaC_separateudata (lua_State *L) {
  size_t deadmem = 0;
#if LUA_REFCOUNT
  GCObject **p = &G(L)->rootudata_head.next;
#else !LUA_REFCOUNT
  GCObject **p = &G(L)->rootudata;
#endif LUA_REFCOUNT
  GCObject *curr;
  GCObject *collected = NULL;  /* to collect udata with gc event */
#if !LUA_REFCOUNT
  GCObject **lastcollected = &collected;
  while ((curr = *p) != NULL) {
    lua_assert(curr->gch.tt == LUA_TUSERDATA);
#else LUA_REFCOUNT
  while ((curr = *p) != (GCObject*)&G(L)->rootudata_tail) {
#endif LUA_REFCOUNT
    if (ismarked(curr) || isfinalized(gcotou(curr)))
      p = &curr->gch.next;  /* don't bother with them */

    else if (fasttm(L, gcotou(curr)->uv.metatable, TM_GC) == NULL) {
      markfinalized(gcotou(curr));  /* don't need finalization */
      p = &curr->gch.next;
    }
    else {  /* must call its gc method */
      deadmem += sizeudata(gcotou(curr)->uv.len);
      *p = curr->gch.next;
#if LUA_REFCOUNT
	  Unlink(curr);
	  curr->gch.next = (GCObject*)&G(L)->tmudata_tail;  /* link `curr' at the end of `collected' list */
	  curr->gch.prev = G(L)->tmudata_tail.prev;
	  G(L)->tmudata_tail.prev->gch.next = curr;
      G(L)->tmudata_tail.prev = curr;
#else !LUA_REFCOUNT
	  curr->gch.next = NULL;  /* link `curr' at the end of `collected' list */
      *lastcollected = curr;
      lastcollected = &curr->gch.next;
#endif LUA_REFCOUNT
    }
  }
  /* insert collected udata with gc event into `tmudata' list */
#if LUA_REFCOUNT
//  *lastcollected = G(L)->tmudata_head.next;
//  G(L)->tmudata_head.next = collected;
#else !LUA_REFCOUNT
  *lastcollected = G(L)->tmudata;
  G(L)->tmudata = collected;
#endif LUA_REFCOUNT
  return deadmem;
}


static void removekey (lua_State *L, Node *n) {
  (void)L;
  setnilvalue(gval(n));  /* remove corresponding value ... */
  if (iscollectable(gkey(n)))
    setttype(gkey(n), LUA_TNONE);  /* dead key; remove it */
}
void
BMI055_gyro::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct BMI_GyroReport bmi_gyroreport;

	struct Report {
		int16_t     temp;
		int16_t     gyro_x;
		int16_t     gyro_y;
		int16_t     gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the BMI055 gyro in one pass.
	 */
	bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ;


	if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
		return;
	}

	check_registers();

	uint8_t temp = read_reg(BMI055_ACC_TEMP);

	report.temp = temp;

	report.gyro_x = bmi_gyroreport.gyro_x;
	report.gyro_y = bmi_gyroreport.gyro_y;
	report.gyro_z = bmi_gyroreport.gyro_z;

	if (report.temp == 0 &&
	    report.gyro_x == 0 &&
	    report.gyro_y == 0 &&
	    report.gyro_z == 0) {
		// all zero data - probably a SPI bus error
		perf_count(_bad_transfers);
		perf_end(_sample_perf);
		// note that we don't call reset() here as a reset()
		// costs 20ms with interrupts disabled. That means if
		// the bmi055 does go bad it would cause a FMU failure,
		// regardless of whether another sensor is available,
		return;
	}

	perf_count(_good_transfers);

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again. We still increment
		// _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Report buffers.
	 */
	gyro_report     grb;


	grb.timestamp =  hrt_absolute_time();

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 *   at a nominally 'zero' input. Therefore the offset has to
	 *   be subtracted.
	 *
	 *   Example: A gyro outputs a value of 74 at zero angular rate
	 *        the offset is 74 from the origin and subtracting
	 *        74 from all measurements centers them around zero.
	 */

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	float xraw_f = report.gyro_x;
	float yraw_f = report.gyro_y;
	float zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	matrix::Vector3f gval_integrated;

	bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;
	grb.device_id = _device_id.devid;

	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (gyro_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
Exemple #16
0
void
FXAS21002C::measure()
{
	/* status register and data as read back from the device */

#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_gyro_report;
#pragma pack(pop)

	struct gyro_report gyro_report;

	/* start the performance counter */
	perf_begin(_sample_perf);

	check_registers();

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again.
		_register_wait--;
		perf_end(_sample_perf);
		return;
	}

	/* fetch data from the sensor */
	memset(&raw_gyro_report, 0, sizeof(raw_gyro_report));
	raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS);
	transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report));

	if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		return;
	}

	/*
	 * The TEMP register contains an 8-bit 2's complement temperature value with a range
	 * of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only
	 * compensated (factory trim values applied) when the device is operating in the Active
	 * mode and actively measuring the angular rate.
	 */

	if ((_read % _current_rate) == 0) {
		_last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
		gyro_report.temperature = _last_temperature;
	}

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	gyro_report.timestamp = hrt_absolute_time();

	// report the error count as the number of bad
	// register reads. This allows the higher level
	// code to decide if it should use this sensor based on
	// whether it has had failures
	gyro_report.error_count = perf_event_count(_bad_registers);

	gyro_report.x_raw = swap16(raw_gyro_report.x);
	gyro_report.y_raw = swap16(raw_gyro_report.y);
	gyro_report.z_raw = swap16(raw_gyro_report.z);

	float xraw_f = gyro_report.x_raw;
	float yraw_f = gyro_report.y_raw;
	float zraw_f = gyro_report.z_raw;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	gyro_report.x = _gyro_filter_x.apply(x_in_new);
	gyro_report.y = _gyro_filter_y.apply(y_in_new);
	gyro_report.z = _gyro_filter_z.apply(z_in_new);

	matrix::Vector3f gval(x_in_new, y_in_new, z_in_new);
	matrix::Vector3f gval_integrated;

	bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
	gyro_report.x_integral = gval_integrated(0);
	gyro_report.y_integral = gval_integrated(1);
	gyro_report.z_integral = gval_integrated(2);

	gyro_report.scaling = _gyro_range_scale;

	/* return device ID */
	gyro_report.device_id = _device_id.devid;


	_reports->force(&gyro_report);

	/* notify anyone waiting for data */
	if (gyro_notify) {
		poll_notify(POLLIN);

		if (!(_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
		}
	}

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
Exemple #17
0
void
MPU9250::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct MPUReport mpu_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the MPU9250 in one pass.
	 */
	if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
				   (uint8_t *)&mpu_report,
				   sizeof(mpu_report))) {
		return;
	}

	check_registers();

	if (check_duplicate(&mpu_report.accel_x[0])) {
		return;
	}

#ifdef USE_I2C

	if (_mag->is_passthrough()) {
#endif
		_mag->_measure(mpu_report.mag);
#ifdef USE_I2C

	} else {
		_mag->measure();
	}

#endif

	/*
	 * Convert from big to little endian
	 */
	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
	report.temp    = int16_t_from_bytes(mpu_report.temp);
	report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
	report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
	report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);

	if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) {
		return;
	}

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using the sensor again
		// We still increment _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Swap axes and negate y
	 */
	int16_t accel_xt = report.accel_y;
	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);

	int16_t gyro_xt = report.gyro_y;
	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);

	/*
	 * Apply the swap
	 */
	report.accel_x = accel_xt;
	report.accel_y = accel_yt;
	report.gyro_x = gyro_xt;
	report.gyro_y = gyro_yt;

	/*
	 * Report buffers.
	 */
	accel_report		arb;
	gyro_report		grb;

	/*
	 * Adjust and scale results to m/s^2.
	 */
	grb.timestamp = arb.timestamp = hrt_absolute_time();

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	/* NOTE: Axes have been swapped to match the board a few lines above. */

	arb.x_raw = report.accel_x;
	arb.y_raw = report.accel_y;
	arb.z_raw = report.accel_z;

	float xraw_f = report.accel_x;
	float yraw_f = report.accel_y;
	float zraw_f = report.accel_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	arb.x = _accel_filter_x.apply(x_in_new);
	arb.y = _accel_filter_y.apply(y_in_new);
	arb.z = _accel_filter_z.apply(z_in_new);

	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> aval_integrated;

	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
	arb.x_integral = aval_integrated(0);
	arb.y_integral = aval_integrated(1);
	arb.z_integral = aval_integrated(2);

	arb.scaling = _accel_range_scale;
	arb.range_m_s2 = _accel_range_m_s2;

	_last_temperature = (report.temp) / 361.0f + 35.0f;

	arb.temperature_raw = report.temp;
	arb.temperature = _last_temperature;

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	xraw_f = report.gyro_x;
	yraw_f = report.gyro_y;
	zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;

	_accel_reports->force(&arb);
	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (accel_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify) {
		_gyro->parent_poll_notify();
	}

	if (accel_notify && !(_pub_blocked)) {
		/* log the time of this report */
		perf_begin(_controller_latency_perf);
		/* publish it */
		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
/* This function could be optimized by directly using the property values 
 * array, hence bypassing the virtual function calls in Grid_continuous_property.
 */
void Rgrid_ellips_neighborhood::find_neighbors( const Geovalue& center ) {
  neighbors_.clear();
  neigh_filter_->clear();
  if( !property_ ) return;

  center_ = center;
//  center_.set_property_array( property_ );

  // "already_found" is the number of neighbors already found
  int already_found=0;

  // loc will store the i,j,k coordinates of the center, node_id is the 
  // center's node-id. They will be computed differently, whether "center"
  // and *this both refer to the same grid or not.
  GsTLGridNode loc;
  GsTLInt node_id = -1;

  if( center.grid() != grid_ ) {
    // "center" and "*this" do not refer to the same grid
    bool ok = grid_->geometry()->grid_coordinates( loc, center.location() );
    if( !ok ) return;

    if( includes_center_ ) 
      node_id = cursor_.node_id( loc[0], loc[1], loc[2] );
  }
  else {
    // "center" and "*this" both refer to the same grid
    cursor_.coords( center.node_id(), loc[0], loc[1], loc[2] ); 
    node_id = center.node_id();      
  }
  
  if( includes_center_ && property_->is_informed( node_id ) ) {
    Geovalue gval( grid_, property_, node_id );
    if(neigh_filter_->is_admissible(gval, center)) {
      neighbors_.push_back( gval );
      already_found++;
    }
  }
 
  
  // Visit each node defined by the window ("geom_")
  // For each node, check if the node is inside the grid.
  // If it is and it contains a data value, add it to the list of
  // neighbors
  Grid_template::const_iterator it = geom_.begin();
  Grid_template::const_iterator end = geom_.end();

  while( it != end && already_found < max_neighbors_ ) {
    GsTLGridNode node = loc + (*it);
    GsTLInt node_id = cursor_.node_id( node[0], node[1], node[2] );
    
    if( node_id < 0 ) {
      // The node does not belong to the grid: skip it
      it++;
      continue;
    }

    if( property_->is_informed( node_id )  ) {
      if(region_ &&  !region_->is_inside_region(node_id) ) continue;
      // The node is informed: get the corresponding geovalue and add it
      // to the list of neighbors
      Geovalue gval( grid_, property_, node_id );
      if(neigh_filter_->is_admissible(gval, center)) {
        neighbors_.push_back( gval );
        already_found++;
      }
//      neighbors_.push_back( Geovalue( grid_, property_, node_id ) );
//      already_found++;
    }

    it++;
  }
 // if(!neigh_filter_->is_neighborhood_valid()) neighbors_.clear();

}
Exemple #19
0
void Relay::turn_off(void)
{
	std::ofstream gval("/sys/class/gpio/gpio" + gpio + "/value");
	gval << "1";
	gval.close();
}
Exemple #20
0
/*
** if key is not marked, mark its entry as dead (therefore removing it
** from the table)
*/
static void removeentry (Node *n) {
  lua_assert(ttisnil(gval(n)));
  if (valiswhite(gkey(n)))
    setdeadvalue(wgkey(n));  /* unused and unmarked key; remove it */
}
Exemple #21
0
/*
** open parts that may cause memory-allocation errors
*/
static void f_luaopen (lua_State *L, void *ud) {
  int i;
  global_State globalState;
  lua_State luaState;
  global_State *g;
#ifdef _DEBUG
  luaState.allocName = "Lua_global_State";
#endif _DEBUG
  luaState.l_G = &globalState;
  globalState.reallocFunc = luaHelper_Realloc;
  globalState.freeFunc = luaHelper_Free;
  globalState.memData = luaHelper_memData;
  globalState.nblocks = sizeof(lua_State) + sizeof(global_State);	// Bogus.
  /* create a new global state */
  g = luaM_new(&luaState, global_State);
  UNUSED(ud);
  if (g == NULL) luaD_throw(L, LUA_ERRMEM);
  L->l_G = g;
  g->mainthread = L;
  g->GCthreshold = 0;  /* mark it as unfinished state */
  g->strt.size = 0;
  g->strt.nuse = 0;
  g->strt.hash = NULL;
  setnilvalue2n(defaultmeta(L));
  setnilvalue2n(registry(L));
  luaZ_initbuffer(L, &g->buff);
  g->panic = default_panic;
#if !LUA_REFCOUNT
  g->rootgc = NULL;
  g->rootudata = NULL;
  g->tmudata = NULL;
#else LUA_REFCOUNT
  g->rootgc_head.next = (GCObject*)&g->rootgc_tail;
  g->rootgc_head.prev = NULL;
  g->rootgc_tail.next = NULL;
  g->rootgc_tail.prev = (GCObject*)&g->rootgc_head;
  g->rootgc_head.tt = LUA_TNIL;
  g->rootgc_head.marked = 0;
  g->rootgc_head.ref = 0;
  g->rootgc_tail.tt = LUA_TNIL;
  g->rootgc_tail.marked = 0;
  g->rootgc_tail.ref = 0;

  g->rootudata_head.next = (GCObject*)&g->rootudata_tail;
  g->rootudata_head.prev = NULL;
  g->rootudata_tail.next = NULL;
  g->rootudata_tail.prev = (GCObject*)&g->rootudata_head;
  g->rootudata_head.tt = LUA_TNIL;
  g->rootudata_head.marked = 0;
  g->rootudata_head.ref = 0;
  g->rootudata_tail.tt = LUA_TNIL;
  g->rootudata_tail.marked = 0;
  g->rootudata_tail.ref = 0;

  g->tmudata_head.next = (GCObject*)&g->tmudata_tail;
  g->tmudata_head.prev = NULL;
  g->tmudata_tail.next = NULL;
  g->tmudata_tail.prev = (GCObject*)&g->tmudata_head;
  g->tmudata_head.tt = LUA_TNIL;
  g->tmudata_head.marked = 0;
  g->tmudata_head.ref = 0;
  g->tmudata_tail.tt = LUA_TNIL;
  g->tmudata_tail.marked = 0;
  g->tmudata_tail.ref = 0;
#endif LUA_REFCOUNT
  setnilvalue2n(gkey(g->dummynode));
  setnilvalue2n(gval(g->dummynode));
  g->dummynode->next = NULL;
  g->nblocks = sizeof(lua_State) + sizeof(global_State);

  g->reallocFunc = luaHelper_Realloc;
  g->freeFunc = luaHelper_Free;
  g->memData = luaHelper_memData;
  g->fatalErrorFunc = defaultFatalErrorFunc;
#ifdef LUA_MTSUPPORT
  g->lockData = NULL;
  g->lockFunc = NULL;
  g->unlockFunc = NULL;
#endif LUA_MTSUPPORT
  g->userGCFunction = NULL;
  g->globalUserData = NULL;

  stack_init(L, L);  /* init stack */

  for (i = 0; i < LUA_NTYPES; i++)
  {
    defaultmetatypes(L, i)->value.gc = NULL;
  }

  /* create default meta table with a dummy table, and then close the loop */
  defaultmeta(L)->tt = LUA_TNUMBER;
  defaultmeta(L)->value.gc = NULL;
  sethvalue2n(defaultmeta(L), luaH_new(L, 0, 0));
  __AddRefDirect(hvalue(defaultmeta(L)));
  hvalue(defaultmeta(L))->metatable = hvalue(defaultmeta(L));
  __AddRefDirect(hvalue(defaultmeta(L))->metatable);

  /* build meta tables */
  for (i = 0; i < LUA_NTYPES; i++)
  {
    luaM_setname(L, "Lua_defaultMetaTypes");
    sethvalue2n(defaultmetatypes(L, i), luaH_new(L, 0, 0));
    hvalue(defaultmetatypes(L, i))->metatable = hvalue(defaultmeta(L));
  }

  luaM_setname(L, "Lua_Globals");
  sethvalue(gt(L), luaH_new(L, 0, 4));  /* table of globals */
  __AddRefDirect(hvalue(gt(L)));
  luaM_setname(L, "Lua_Registry");
  sethvalue(registry(L), luaH_new(L, 4, 4));  /* registry */
  __AddRef(registry(L));
  g->minimumstrings = lua_minimumnumstrings;
  luaS_resize(L, MINSTRTABSIZE);  /* initial size of string table */
  luaT_init(L);
  luaX_init(L);
  luaS_fix(luaS_newliteral(L, MEMERRMSG));
  g->GCthreshold = 4*G(L)->nblocks;

  luaZ_openspace(L, &g->buff, lua_minimumauxspace);
}
Exemple #22
0
static void removeentry (Node *n) {
  lua_assert(ttisnil(gval(n)));
  if (iscollectable(gkey(n)))
    setttype(gkey(n), LUA_TDEADKEY);  /* dead key; remove it */
}
Exemple #23
0
void
MPU9250::measure()
{

	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct MPUReport mpu_report;

	struct ICMReport icm_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the MPU9250 in one pass
	 */

	if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) {
		if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
			if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
				perf_end(_sample_perf);
				return;
			}

		} else {    // ICM20948
			select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H));

			if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report,
						 sizeof(icm_report))) {
				perf_end(_sample_perf);
				return;
			}
		}

		check_registers();

		if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) {
			return;
		}
	}

	/*
	 * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
	 * try to read a magnetometer report.
	 */

#   ifdef USE_I2C

	if (_mag->is_passthrough()) {
#   endif

		_mag->_measure(mpu_report.mag);

#   ifdef USE_I2C

	} else {
		_mag->measure();
	}

#   endif

	/*
	 * Continue evaluating gyro and accelerometer results
	 */
	if (!_magnetometer_only && _register_wait == 0) {

		/*
		 * Convert from big to little endian
		 */
		if (_whoami == ICM_WHOAMI_20948) {
			report.accel_x = int16_t_from_bytes(icm_report.accel_x);
			report.accel_y = int16_t_from_bytes(icm_report.accel_y);
			report.accel_z = int16_t_from_bytes(icm_report.accel_z);
			report.temp    = int16_t_from_bytes(icm_report.temp);
			report.gyro_x  = int16_t_from_bytes(icm_report.gyro_x);
			report.gyro_y  = int16_t_from_bytes(icm_report.gyro_y);
			report.gyro_z  = int16_t_from_bytes(icm_report.gyro_z);

		} else { // MPU9250/MPU6500
			report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
			report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
			report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
			report.temp    = int16_t_from_bytes(mpu_report.temp);
			report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
			report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
			report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);
		}

		if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
			return;
		}
	}

	if (_register_wait != 0) {
		/*
		 * We are waiting for some good transfers before using the sensor again.
		 * We still increment _good_transfers, but don't return any data yet.
		 *
		*/
		_register_wait--;
		return;
	}

	/*
	 * Get sensor temperature
	 */
	_last_temperature = (report.temp) / 333.87f + 21.0f;


	/*
	 * Convert and publish accelerometer and gyrometer data.
	 */

	if (!_magnetometer_only) {

		/*
		 * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation
		 */
		if (_whoami != ICM_WHOAMI_20948) {
			/*
			 * Swap axes and negate y
			 */

			int16_t accel_xt = report.accel_y;
			int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);

			int16_t gyro_xt = report.gyro_y;
			int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);

			/*
			 * Apply the swap
			 */
			report.accel_x = accel_xt;
			report.accel_y = accel_yt;
			report.gyro_x = gyro_xt;
			report.gyro_y = gyro_yt;
		}

		/*
		 * Report buffers.
		 */
		sensor_accel_s		arb;
		sensor_gyro_s			grb;

		/*
		 * Adjust and scale results to m/s^2.
		 */
		grb.timestamp = arb.timestamp = hrt_absolute_time();

		// report the error count as the sum of the number of bad
		// transfers and bad register reads. This allows the higher
		// level code to decide if it should use this sensor based on
		// whether it has had failures
		grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

		/*
		 * 1) Scale raw value to SI units using scaling from datasheet.
		 * 2) Subtract static offset (in SI units)
		 * 3) Scale the statically calibrated values with a linear
		 *    dynamically obtained factor
		 *
		 * Note: the static sensor offset is the number the sensor outputs
		 * 	 at a nominally 'zero' input. Therefore the offset has to
		 * 	 be subtracted.
		 *
		 *	 Example: A gyro outputs a value of 74 at zero angular rate
		 *	 	  the offset is 74 from the origin and subtracting
		 *		  74 from all measurements centers them around zero.
		 */

		/* NOTE: Axes have been swapped to match the board a few lines above. */

		arb.x_raw = report.accel_x;
		arb.y_raw = report.accel_y;
		arb.z_raw = report.accel_z;

		float xraw_f = report.accel_x;
		float yraw_f = report.accel_y;
		float zraw_f = report.accel_z;

		// apply user specified rotation
		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

		float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
		float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
		float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

		arb.x = _accel_filter_x.apply(x_in_new);
		arb.y = _accel_filter_y.apply(y_in_new);
		arb.z = _accel_filter_z.apply(z_in_new);

		matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
		matrix::Vector3f aval_integrated;

		bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
		arb.x_integral = aval_integrated(0);
		arb.y_integral = aval_integrated(1);
		arb.z_integral = aval_integrated(2);

		arb.scaling = _accel_range_scale;

		arb.temperature = _last_temperature;

		/* return device ID */
		arb.device_id = _accel->_device_id.devid;

		grb.x_raw = report.gyro_x;
		grb.y_raw = report.gyro_y;
		grb.z_raw = report.gyro_z;

		xraw_f = report.gyro_x;
		yraw_f = report.gyro_y;
		zraw_f = report.gyro_z;

		// apply user specified rotation
		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

		float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
		float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
		float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

		grb.x = _gyro_filter_x.apply(x_gyro_in_new);
		grb.y = _gyro_filter_y.apply(y_gyro_in_new);
		grb.z = _gyro_filter_z.apply(z_gyro_in_new);

		matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
		matrix::Vector3f gval_integrated;

		bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
		grb.x_integral = gval_integrated(0);
		grb.y_integral = gval_integrated(1);
		grb.z_integral = gval_integrated(2);

		grb.scaling = _gyro_range_scale;

		grb.temperature = _last_temperature;

		/* return device ID */
		grb.device_id = _gyro->_device_id.devid;

		_accel_reports->force(&arb);
		_gyro_reports->force(&grb);

		/* notify anyone waiting for data */
		if (accel_notify) {
			_accel->poll_notify(POLLIN);
		}

		if (gyro_notify) {
			_gyro->parent_poll_notify();
		}

		if (accel_notify && !(_accel->_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
		}

		if (gyro_notify && !(_gyro->_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
		}
	}

	/* stop measuring */
	perf_end(_sample_perf);
}