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;
}
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;
}
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_);
}
/*
** 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;
}
}
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 */
}
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);
}
}
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, "}");
}
static void removekey (Node *n) {
setnilvalue(gval(n)); /* remove corresponding value ... */
if (iscollectable(gkey(n)))
setttype(gkey(n), LUA_TNONE); /* dead key; remove it */
}
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);
}
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;
}
/* 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);
}
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);
}
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);
}
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);
}
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();
}
void Relay::turn_off(void)
{
std::ofstream gval("/sys/class/gpio/gpio" + gpio + "/value");
gval << "1";
gval.close();
}
/*
** 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 */
}
/*
** 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);
}
static void removeentry (Node *n) {
lua_assert(ttisnil(gval(n)));
if (iscollectable(gkey(n)))
setttype(gkey(n), LUA_TDEADKEY); /* dead key; remove it */
}
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);
}
