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); }