void erts_destroy_monitor(ErtsMonitor *mon)
{
Uint mon_size = ERTS_MONITOR_SIZE;
ErlNode *node;
ASSERT(!IS_CONST(mon->ref));
mon_size += NC_HEAP_SIZE(mon->ref);
if (is_external(mon->ref)) {
node = external_thing_ptr(mon->ref)->node;
erts_deref_node_entry(node);
}
if (!IS_CONST(mon->pid)) {
mon_size += NC_HEAP_SIZE(mon->pid);
if (is_external(mon->pid)) {
node = external_thing_ptr(mon->pid)->node;
erts_deref_node_entry(node);
}
}
if (mon_size <= ERTS_MONITOR_SH_SIZE) {
erts_free(ERTS_ALC_T_MONITOR_SH, (void *) mon);
} else {
erts_free(ERTS_ALC_T_MONITOR_LH, (void *) mon);
erts_smp_atomic_add_nob(&tot_link_lh_size, -1*mon_size*sizeof(Uint));
}
}
static void doit_insert_monitor(ErtsMonitor *monitor, void *p)
{
Eterm *idp = p;
if(is_external(monitor->pid))
insert_node(external_thing_ptr(monitor->pid)->node, MONITOR_REF, *idp);
if(is_external(monitor->ref))
insert_node(external_thing_ptr(monitor->ref)->node, MONITOR_REF, *idp);
}
void change_visibility(DexMethod* method) {
auto code = method->get_code();
always_assert(code != nullptr);
editable_cfg_adapter::iterate(code, [](MethodItemEntry& mie) {
auto insn = mie.insn;
if (insn->has_field()) {
auto cls = type_class(insn->get_field()->get_class());
if (cls != nullptr && !cls->is_external()) {
set_public(cls);
}
auto field =
resolve_field(insn->get_field(), is_sfield_op(insn->opcode())
? FieldSearch::Static : FieldSearch::Instance);
if (field != nullptr && field->is_concrete()) {
set_public(field);
set_public(type_class(field->get_class()));
// FIXME no point in rewriting opcodes in the method
insn->set_field(field);
}
} else if (insn->has_method()) {
auto cls = type_class(insn->get_method()->get_class());
if (cls != nullptr && !cls->is_external()) {
set_public(cls);
}
auto current_method = resolve_method(
insn->get_method(), opcode_to_search(insn));
if (current_method != nullptr && current_method->is_concrete()) {
set_public(current_method);
set_public(type_class(current_method->get_class()));
// FIXME no point in rewriting opcodes in the method
insn->set_method(current_method);
}
} else if (insn->has_type()) {
auto type = insn->get_type();
auto cls = type_class(type);
if (cls != nullptr && !cls->is_external()) {
set_public(cls);
}
}
return editable_cfg_adapter::LOOP_CONTINUE;
});
std::vector<DexType*> types;
if (code->editable_cfg_built()) {
code->cfg().gather_catch_types(types);
} else {
code->gather_catch_types(types);
}
for (auto type : types) {
auto cls = type_class(type);
if (cls != nullptr && !cls->is_external()) {
set_public(cls);
}
}
}
void execute(char **cmd)
{
int status;
int i = 0;
pid_t pid;
if(is_built_in(cmd[0]))
{
execute_built_in(cmd);
}
else if (is_external(cmd[0]))
{
pid = fork();
/* error */
if (pid == -1)
exit(1);
/* child */
else if (pid == 0)
{
execv(cmd[0], cmd);
exit(0);
}
/* parent */
else
waitpid(pid, &status, 0);
}
else
printf("%s: Command not found.\n", cmd[0]);
}
static void doit_insert_link(ErtsLink *lnk, void *p)
{
Eterm *idp = p;
if(is_external(lnk->pid))
insert_node(external_thing_ptr(lnk->pid)->node, LINK_REF,
*idp);
}
/**
* The callee contains an invoke to a virtual method we either do not know
* or it's not public. Given the caller may not be in the same
* hierarchy/package we cannot inline it unless we make the method public.
* But we need to make all methods public across the hierarchy and for methods
* we don't know we have no idea whether the method was public or not anyway.
*/
bool MultiMethodInliner::unknown_virtual(DexInstruction* insn, DexMethod* context) {
if (insn->opcode() == OPCODE_INVOKE_VIRTUAL ||
insn->opcode() == OPCODE_INVOKE_VIRTUAL_RANGE) {
auto method = static_cast<DexOpcodeMethod*>(insn)->get_method();
auto res_method = resolver(method, MethodSearch::Virtual);
if (res_method == nullptr) {
// if it's not known to redex but it's a common java/android API method
if (method_ok(method->get_class(), method)) {
return false;
}
auto type = method->get_class();
if (type_ok(type)) return false;
// the method ref is bound to a type known to redex but the method does
// not exist in the hierarchy known to redex. Essentially the method
// is from an external type i.e. A.equals(Object)
auto cls = type_class(type);
while (cls != nullptr) {
type = cls->get_super_class();
cls = type_class(type);
}
if (type_ok(type)) return false;
if (method_ok(type, method)) return false;
assert(track(method));
info.escaped_virtual++;
return true;
}
if (res_method->is_external() && !is_public(res_method)) {
info.non_pub_virtual++;
return true;
}
}
return false;
}
/*
* Parse a Cypress IIC image file and invoke the poke() function on the
* various segments for writing to RAM
*
* image - the IIC image file
* context - for use by poke()
* is_external - if non-null, used to check which segments go into
* external memory (writable only by software loader)
* poke - called with each memory segment; errors indicated
* by returning negative values.
*
* Caller is responsible for halting CPU as needed, such as when
* overwriting a second stage loader.
*/
static int parse_iic(FILE *image, void *context,
bool (*is_external)(uint32_t addr, size_t len),
int (*poke)(void *context, uint32_t addr, bool external, const unsigned char *data, size_t len))
{
unsigned char data[4096];
uint32_t data_addr = 0;
size_t data_len = 0, read_len;
uint8_t block_header[4];
int rc;
bool external = false;
long file_size, initial_pos;
initial_pos = ftell(image);
if (initial_pos < 0) {
return -1;
}
if (fseek(image, 0L, SEEK_END) < 0) {
return -1;
}
file_size = ftell(image);
if (file_size < 0) {
return -1;
}
if (fseek(image, initial_pos, SEEK_SET) < 0) {
return -1;
}
for (;;) {
/* Ignore the trailing reset IIC data (5 bytes) */
if (ftell(image) >= (file_size - 5))
break;
if (fread(&block_header, 1, sizeof(block_header), image) != 4) {
log_debug("Unable to read IIC block header\n");
return -1;
}
data_len = (block_header[0] << 8) + block_header[1];
data_addr = (block_header[2] << 8) + block_header[3];
if (data_len > sizeof(data)) {
/* If this is ever reported as an error, switch to using malloc/realloc */
log_debug("IIC data block too small - please report this error to libusbx.org\n");
return -1;
}
read_len = fread(data, 1, data_len, image);
if (read_len != data_len) {
log_debug("Read error: %s\n", strerror(errno));
return -1;
}
if (is_external)
external = is_external(data_addr, data_len);
rc = poke(context, data_addr, external, data, data_len);
if (rc < 0)
return -1;
}
return 0;
}
/* returns 2 if cd is the command, 1 if external command, 0 if other */
int is_command(char **args, int i)
{
if (strcmp(args[0],"cd") == 0)
{
return 2;
}
else if (is_external(args[i]) == 1)
{
return 1;
}
return 0;
}
int
IIS328DQ::init()
{
int ret = ERROR;
if (probe() != OK)
goto out;
/* do SPI init (and probe) first */
if (CDev::init() != OK)
goto out;
/* allocate basic report buffers */
if (_reports != NULL) {
ringbuf_deinit(_reports);
vPortFree(_reports);
_reports = NULL;
}
/* allocate basic report buffers */
_reports = (ringbuf_t *) pvPortMalloc (sizeof(ringbuf_t));
ringbuf_init(_reports, 2, sizeof(accel_report));
if (_reports == NULL)
goto out;
_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
reset();
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
ringbuf_get(_reports, &arp, sizeof(arp));
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic == NULL) {
DEVICE_DEBUG("failed to create sensor_accel publication");
}
ret = OK;
out:
return ret;
}
/**
* Change the visibility of members accessed in a callee as they are moved
* to the caller context.
* We make everything public but we could be more precise and only
* relax visibility as needed.
*/
void MultiMethodInliner::change_visibility(DexMethod* callee) {
TRACE(MMINL, 6, "checking visibility usage of members in %s\n",
SHOW(callee));
for (auto insn : callee->get_code()->get_instructions()) {
if (insn->has_fields()) {
auto fop = static_cast<DexOpcodeField*>(insn);
auto field = fop->field();
field = resolve_field(field, is_sfield_op(insn->opcode())
? FieldSearch::Static : FieldSearch::Instance);
if (field != nullptr && field->is_concrete()) {
TRACE(MMINL, 6, "changing visibility of %s.%s %s\n",
SHOW(field->get_class()), SHOW(field->get_name()),
SHOW(field->get_type()));
set_public(field);
set_public(type_class(field->get_class()));
fop->rewrite_field(field);
}
continue;
}
if (insn->has_methods()) {
auto mop = static_cast<DexOpcodeMethod*>(insn);
auto method = mop->get_method();
method = resolver(method, opcode_to_search(insn));
if (method != nullptr && method->is_concrete()) {
TRACE(MMINL, 6, "changing visibility of %s.%s: %s\n",
SHOW(method->get_class()), SHOW(method->get_name()),
SHOW(method->get_proto()));
set_public(method);
set_public(type_class(method->get_class()));
mop->rewrite_method(method);
}
continue;
}
if (insn->has_types()) {
auto type = static_cast<DexOpcodeType*>(insn)->get_type();
auto cls = type_class(type);
if (cls != nullptr && !cls->is_external()) {
TRACE(MMINL, 6, "changing visibility of %s\n", SHOW(type));
set_public(cls);
}
continue;
}
}
}
unsigned list_cases_on(vm_obj const & o, buffer<vm_obj> & data) {
if (is_simple(o)) {
return 0;
} else if (is_constructor(o)) {
data.append(csize(o), cfields(o));
return 1;
} else {
lean_assert(is_external(o));
if (auto l = dynamic_cast<vm_list<name>*>(to_external(o))) {
return list_cases_on_core(l->m_val, data);
} else if (auto l = dynamic_cast<vm_list<expr>*>(to_external(o))) {
return list_cases_on_core(l->m_val, data);
} else if (auto l = dynamic_cast<vm_list<level>*>(to_external(o))) {
return list_cases_on_core(l->m_val, data);
} else {
lean_unreachable();
}
}
}
void
M2c::process_sym_table(SymTable *st)
{
bool st_is_private = is_private(st);
Iter<SymbolTableObject*> iter = st->get_symbol_table_object_iterator();
for (/* iter */; iter.is_valid(); iter.next()) {
SymbolTableObject *the_sto = iter.current();
if (is_kind_of<VariableSymbol>(the_sto)) {
if (is_kind_of<ParameterSymbol>(the_sto))
continue; // don't shadow an arg!
VarSym *v = to<VariableSymbol>(the_sto);
if (!is_global(st)) {
printer->print_var_def(v, false);
}
else if (v->get_definition() != NULL) {
postponed_vars.push_back(v);
printer->print_var_def(v, true);
}
else {
claim(is_external(v));
fprintf(out, "extern ");
printer->print_sym_decl(v);
fprintf(out, ";\n");
}
} else if (is_kind_of<ProcedureSymbol>(the_sto)) {
if (st_is_private) fputs("static ", out);
printer->print_proc_decl(to<ProcedureSymbol>(the_sto));
} else if (is_kind_of<Type>(the_sto)) {
if (is_kind_of<EnumeratedType>(the_sto) ||
(is_kind_of<GroupType>(the_sto) &&
to<GroupType>(the_sto)->get_is_complete())) {
printer->print_type(to<Type>(the_sto));
fprintf(out, ";\n");
}
}
}
}
void erts_destroy_link(ErtsLink *lnk)
{
Uint lnk_size = ERTS_LINK_SIZE;
ErlNode *node;
ASSERT(lnk->type == LINK_NODE || ERTS_LINK_ROOT(lnk) == NULL);
if (!IS_CONST(lnk->pid)) {
lnk_size += NC_HEAP_SIZE(lnk->pid);
if (is_external(lnk->pid)) {
node = external_thing_ptr(lnk->pid)->node;
erts_deref_node_entry(node);
}
}
if (lnk_size <= ERTS_LINK_SH_SIZE) {
erts_free(ERTS_ALC_T_NLINK_SH, (void *) lnk);
} else {
erts_free(ERTS_ALC_T_NLINK_LH, (void *) lnk);
erts_smp_atomic_add_nob(&tot_link_lh_size, -1*lnk_size*sizeof(Uint));
}
}
int
L3GD20::init()
{
int ret = ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
reset();
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
DEVICE_DEBUG("failed to create sensor_gyro publication");
}
ret = OK;
out:
return ret;
}
/*
* Parse a binary image file and write it as is to the target.
* Applies to Cypress BIX images for RAM or Cypress IIC images
* for EEPROM.
*
* image - the BIX image file
* context - for use by poke()
* is_external - if non-null, used to check which segments go into
* external memory (writable only by software loader)
* poke - called with each memory segment; errors indicated
* by returning negative values.
*
* Caller is responsible for halting CPU as needed, such as when
* overwriting a second stage loader.
*/
static int parse_bin(FILE *image, void *context,
bool (*is_external)(uint32_t addr, size_t len), int (*poke)(void *context,
uint32_t addr, bool external, const unsigned char *data, size_t len))
{
unsigned char data[4096];
uint32_t data_addr = 0;
size_t data_len = 0;
int rc;
bool external = false;
for (;;) {
data_len = fread(data, 1, 4096, image);
if (data_len == 0)
break;
if (is_external)
external = is_external(data_addr, data_len);
rc = poke(context, data_addr, external, data, data_len);
if (rc < 0)
return -1;
data_addr += (uint32_t)data_len;
}
return feof(image)?0:-1;
}
// Check that visibility / accessibility changes to the current method
// won't need to change a referenced method into a virtual or static one.
bool gather_invoked_methods_that_prevent_relocation(
const DexMethod* method,
std::unordered_set<DexMethodRef*>* methods_preventing_relocation) {
auto code = method->get_code();
always_assert(code);
bool can_relocate = true;
for (const auto& mie : InstructionIterable(code)) {
auto insn = mie.insn;
auto opcode = insn->opcode();
if (is_invoke(opcode)) {
auto meth = resolve_method(insn->get_method(), opcode_to_search(insn));
if (!meth && opcode == OPCODE_INVOKE_VIRTUAL &&
unknown_virtuals::is_method_known_to_be_public(insn->get_method())) {
continue;
}
if (meth) {
always_assert(meth->is_def());
if (meth->is_external() && !is_public(meth)) {
meth = nullptr;
} else if (opcode == OPCODE_INVOKE_DIRECT && !is_init(meth)) {
meth = nullptr;
}
}
if (!meth) {
can_relocate = false;
if (!methods_preventing_relocation) {
break;
}
methods_preventing_relocation->emplace(insn->get_method());
}
}
}
return can_relocate;
}
int
MPU9250::init()
{
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
int ret = probe();
if (ret != OK) {
DEVICE_DEBUG("MPU9250 probe failed");
return ret;
}
/* do init */
ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
#ifdef USE_I2C
if (!_mag->is_passthrough() && _mag->_interface->init() != OK) {
warnx("failed to setup ak8963 interface");
}
#endif
/* do CDev init for the mag device node, keep it optional */
ret = _mag->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
void execute_built_in(char **cmd)
{
int i;
struct timeval start, end;
pid_t pid;
FILE *file;
DIR *dir;
char str1[256] = "/proc/";
char *str2 = "/limits";
char str3[256];
char pidstr[9];
int status;
int result;
char *same;
char *lastOccur;
char *nextSpace, *nextTab, *endofstr;
int len;
if (strcmp(cmd[0], "exit") == 0)
{
printf("Exiting Shell...\n");
exit(1);
}
else if (strcmp(cmd[0], "cd") == 0)
{
/* if more than one argument is present, trigger error */
if (cmd[2] != 0)
{
printf("cd: Too many arguments.\n");
return;
}
/* if no arguments, treat as if $HOME is the argument */
if (cmd[1] == 0)
cmd[1] = getenv("HOME");
result = chdir(cmd[1]);
if (result == 0) /* successful directory change */
setenv("PWD", cmd[1], 1);
else /* directory doesn't exist */
printf("%s: No such file or directory.\n", cmd[1]);
}
else if (strcmp(cmd[0], "echo") == 0)
{
i = 1;
while(cmd[i] != 0)
{
/* print argument if it isn't an external command */
if (!is_external(cmd[i]))
printf("%s ", cmd[i]);
/* if external, find original statement and print that
echo ls will print "ls" instead of "/bin/ls"
echo /bin/ls will print "/bin/ls" */
else
{
same = strstr(uneditedLine, cmd[i]);
/* if cmd matches thing in uneditedLine, print cmd[i] and change uneditedline */
if (same != NULL)
{
/* find next whitespace character or '\0'*/
nextSpace = strchr(same, ' ');
nextTab = strchr(same, '\t');
endofstr = strchr(same, '\0');
if (nextSpace != NULL)
{
if (nextTab != NULL && nextTab > nextSpace)
nextSpace = nextTab;
}
else if (nextTab != NULL)
nextSpace = nextTab;
else
nextSpace = endofstr;
len = 0;
while (nextSpace != same)
{
nextSpace--;
len++;
}
printf("%s ", cmd[i]);
strncpy(same, "-", len);
}
/* else if it doesn't match, but cmd is an external command */
else if ((dir = opendir(cmd[i])) == 0)
{
lastOccur = strrchr(cmd[i], '/');
strcpy(str3, lastOccur + 1);
printf("%s ", str3);
}
/* else if the cmd[i] is an expanded env variable */
else
{
closedir(dir);
printf("%s ", cmd[i]);
}
}
i++;
}
printf("\n");
}
else if (strcmp(cmd[0], "etime") == 0)
{
if (cmd[1] == 0)
{
printf("Usage: etime requires an argument.\n");
return;
}
/* remove etime from cmd */
for (i = 1; cmd[i] != 0; i++)
{
cmd[i - 1] = cmd[i];
}
cmd[i - 1] = NULL;
gettimeofday(&start, NULL);
my_execute(cmd);
gettimeofday(&end, NULL);
printf("Elapsed Time: %.6fs\n", (double)((end.tv_sec * 1000000 + end.tv_usec) - (start.tv_sec * 1000000 + start.tv_usec)) / 1000000.0);
}
else if (strcmp(cmd[0], "limits") == 0)
{
if (cmd[1] == 0)
{
printf("Usage: limits requires an argument.\n");
return;
}
/* remove limits from cmd */
for (i = 1; cmd[i] != 0; i++)
{
cmd[i - 1] = cmd[i];
}
cmd[i - 1] = NULL;
/* child */
if ((pid = fork()) == 0)
{
my_execute(cmd);
sprintf(pidstr, "%d", i);
strcat(str1, pidstr);
strcat(str1, str2);
file = fopen(str1, "r");
if (file != NULL)
{
i = 1;
while (fgets(str1, sizeof str1, file) != NULL)
{
if (i == 3)
printf("\n%s", str1);
if (i == 8)
strcpy(str3, str1);
if (i == 9)
printf("%s%s", str1, str3);
if (i == 13)
printf("%s", str1);
i = i + 1;
}
fclose(file);
}
exit(1);
}
/* parent */
else
{
i = pid;
waitpid(pid, &status, 0);
}
}
}
void
FXOS8700CQ::mag_measure()
{
/* status register and data as read back from the device */
mag_report mag_report {};
/* start the performance counter */
perf_begin(_mag_sample_perf);
/*
* 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.
*/
mag_report.timestamp = hrt_absolute_time();
mag_report.is_external = is_external();
mag_report.x_raw = _last_raw_mag_x;
mag_report.y_raw = _last_raw_mag_y;
mag_report.z_raw = _last_raw_mag_z;
float xraw_f = mag_report.x_raw;
float yraw_f = mag_report.y_raw;
float zraw_f = mag_report.z_raw;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.scaling = _mag_range_scale;
mag_report.range_ga = (float)_mag_range_ga;
mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
mag_report.temperature = _last_temperature;
mag_report.device_id = _mag->_device_id.devid;
_mag_reports->force(&mag_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
/* stop the perf counter */
perf_end(_mag_sample_perf);
}
/*
* Accumulate a reading from the magnetometer
*
* bus semaphore must not be taken
*/
void AP_Compass_HMC5843::accumulate()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return;
}
uint32_t tnow = AP_HAL::micros();
if (_accum_count != 0 && (tnow - _last_accum_time) < 13333) {
// the compass gets new data at 75Hz
return;
}
if (!_bus->get_semaphore()->take(1)) {
// the bus is busy - try again later
return;
}
bool result = _read_sample();
_bus->get_semaphore()->give();
if (!result) {
return;
}
// the _mag_N values are in the range -2048 to 2047, so we can
// accumulate up to 15 of them in an int16_t. Let's make it 14
// for ease of calculation. We expect to do reads at 10Hz, and
// we get new data at most 75Hz, so we don't expect to
// accumulate more than 8 before a read
// get raw_field - sensor frame, uncorrected
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
raw_field *= _gain_scale;
// rotate to the desired orientation
if (is_external(_compass_instance) &&
_product_id == AP_COMPASS_TYPE_HMC5883L) {
raw_field.rotate(ROTATION_YAW_90);
}
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _compass_instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, tnow, _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 14) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 7;
}
_last_accum_time = tnow;
}
int
BMI160::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
int
FXOS8700CQ::init()
{
int ret = PX4_ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
goto out;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
}
reset();
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
PX4_ERR("MAG init failed");
goto out;
}
/* fill report structures */
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic == nullptr) {
PX4_ERR("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT ERR");
}
out:
return ret;
}
int
MPU9250::init()
{
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
/*
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
* make the integration autoreset faster so that we integrate just one
* sample since the sampling rate is already low.
*/
if (is_i2c()) {
_sample_rate = 200;
_accel_int.set_autoreset_interval(1000000 / 1000);
_gyro_int.set_autoreset_interval(1000000 / 1000);
}
int ret = probe();
if (ret != OK) {
DEVICE_DEBUG("MPU9250 probe failed");
return ret;
}
/* do init */
ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
ret = -ENOMEM;
if (_accel_reports == nullptr) {
return ret;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
return ret;
}
if (reset_mpu() != OK) {
PX4_ERR("Exiting! Device failed to take initialization");
return ret;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
#ifdef USE_I2C
if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak8963 interface");
}
#endif /* USE_I2C */
/* do CDev init for the mag device node, keep it optional */
if (_whoami == MPU_WHOAMI_9250) {
ret = _mag->init();
}
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("mag init failed");
return ret;
}
if (_whoami == MPU_WHOAMI_9250) {
ret = _mag->ak8963_reset();
}
if (ret != OK) {
DEVICE_DEBUG("mag reset failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
return ret;
}
int
MPU9250::init()
{
irqstate_t state;
#if defined(USE_I2C)
use_i2c(_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
#endif
/*
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
* make the integration autoreset faster so that we integrate just one
* sample since the sampling rate is already low.
*/
if (is_i2c() && !_magnetometer_only) {
_sample_rate = 200;
_accel_int.set_autoreset_interval(1000000 / 1000);
_gyro_int.set_autoreset_interval(1000000 / 1000);
}
int ret = probe();
if (ret != OK) {
PX4_DEBUG("MPU9250 probe failed");
return ret;
}
state = px4_enter_critical_section();
_reset_wait = hrt_absolute_time() + 100000;
px4_leave_critical_section(state);
if (reset_mpu() != OK) {
PX4_ERR("Exiting! Device failed to take initialization");
return ret;
}
if (!_magnetometer_only) {
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
ret = -ENOMEM;
if (_accel_reports == nullptr) {
return ret;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
if (_gyro_reports == nullptr) {
return ret;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
}
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
}
/* do CDev init for the accel device node */
ret = _accel->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
PX4_DEBUG("accel init failed");
return ret;
}
/* do CDev init for the gyro device node */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
PX4_DEBUG("gyro init failed");
return ret;
}
}
/* Magnetometer setup */
if (_whoami == MPU_WHOAMI_9250) {
#ifdef USE_I2C
up_udelay(100);
if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak8963 interface");
}
#endif /* USE_I2C */
/* do CDev init for the mag device node */
ret = _mag->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
PX4_DEBUG("mag init failed");
return ret;
}
ret = _mag->ak8963_reset();
if (ret != OK) {
PX4_DEBUG("mag reset failed");
return ret;
}
}
measure();
if (!_magnetometer_only) {
/* advertise sensor topic, measure manually to initialize valid report */
sensor_accel_s arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel->_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
/* advertise sensor topic, measure manually to initialize valid report */
sensor_gyro_s grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return ret;
}
}
return ret;
}
bool is_format(vm_obj const & o) {
return is_external(o) && dynamic_cast<vm_format*>(to_external(o));
}
level const & to_level(vm_obj const & o) {
lean_assert(is_external(o));
lean_assert(dynamic_cast<vm_level*>(to_external(o)));
return static_cast<vm_level*>(to_external(o))->m_val;
}
int
BAROSIM::init()
{
int ret;
DEVICE_DEBUG("BAROSIM::init");
ret = VDev::init();
if (ret != OK) {
DEVICE_DEBUG("VDev init failed");
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
DEVICE_DEBUG("can't get memory for reports");
ret = -ENOMEM;
goto out;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
_measure_phase = 0;
_reports->flush();
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == nullptr) {
PX4_ERR("failed to create sensor_baro publication");
}
/* this do..while is goto without goto */
do {
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
PX4_ERR("temp measure failed");
break;
}
usleep(BAROSIM_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
PX4_ERR("temp collect failed");
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
PX4_ERR("pressure collect failed");
break;
}
usleep(BAROSIM_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
PX4_ERR("pressure collect failed");
break;
}
/* state machine will have generated a report, copy it out */
_reports->get(&brp);
ret = OK;
//PX4_WARN("sensor_baro publication %ld", _baro_topic);
} while (0);
out:
return ret;
}
/*
* Parse an Intel HEX image file and invoke the poke() function on the
* various segments to implement policies such as writing to RAM (with
* a one or two stage loader setup, depending on the firmware) or to
* EEPROM (two stages required).
*
* image - the hex image file
* context - for use by poke()
* is_external - if non-null, used to check which segments go into
* external memory (writable only by software loader)
* poke - called with each memory segment; errors indicated
* by returning negative values.
*
* Caller is responsible for halting CPU as needed, such as when
* overwriting a second stage loader.
*/
static int parse_ihex(FILE *image, void *context,
bool (*is_external)(uint32_t addr, size_t len),
int (*poke) (void *context, uint32_t addr, bool external,
const unsigned char *data, size_t len))
{
unsigned char data[1023];
uint32_t data_addr = 0;
size_t data_len = 0;
int rc;
int first_line = 1;
bool external = false;
/* Read the input file as an IHEX file, and report the memory segments
* as we go. Each line holds a max of 16 bytes, but uploading is
* faster (and EEPROM space smaller) if we merge those lines into larger
* chunks. Most hex files keep memory segments together, which makes
* such merging all but free. (But it may still be worth sorting the
* hex files to make up for undesirable behavior from tools.)
*
* Note that EEPROM segments max out at 1023 bytes; the upload protocol
* allows segments of up to 64 KBytes (more than a loader could handle).
*/
for (;;) {
char buf[512], *cp;
char tmp, type;
size_t len;
unsigned idx, off;
cp = fgets(buf, sizeof(buf), image);
if (cp == NULL) {
logerror("EOF without EOF record!\n");
break;
}
/* EXTENSION: "# comment-till-end-of-line", for copyrights etc */
if (buf[0] == '#')
continue;
if (buf[0] != ':') {
logerror("not an ihex record: %s", buf);
return -2;
}
/* ignore any newline */
cp = strchr(buf, '\n');
if (cp)
*cp = 0;
if (verbose >= 3)
logerror("** LINE: %s\n", buf);
/* Read the length field (up to 16 bytes) */
tmp = buf[3];
buf[3] = 0;
len = strtoul(buf+1, NULL, 16);
buf[3] = tmp;
/* Read the target offset (address up to 64KB) */
tmp = buf[7];
buf[7] = 0;
off = (int)strtoul(buf+3, NULL, 16);
buf[7] = tmp;
/* Initialize data_addr */
if (first_line) {
data_addr = off;
first_line = 0;
}
/* Read the record type */
tmp = buf[9];
buf[9] = 0;
type = (char)strtoul(buf+7, NULL, 16);
buf[9] = tmp;
/* If this is an EOF record, then make it so. */
if (type == 1) {
if (verbose >= 2)
logerror("EOF on hexfile\n");
break;
}
if (type != 0) {
logerror("unsupported record type: %u\n", type);
return -3;
}
if ((len * 2) + 11 > strlen(buf)) {
logerror("record too short?\n");
return -4;
}
/* FIXME check for _physically_ contiguous not just virtually
* e.g. on FX2 0x1f00-0x2100 includes both on-chip and external
* memory so it's not really contiguous */
/* flush the saved data if it's not contiguous,
* or when we've buffered as much as we can.
*/
if (data_len != 0
&& (off != (data_addr + data_len)
/* || !merge */
|| (data_len + len) > sizeof(data))) {
if (is_external)
external = is_external(data_addr, data_len);
rc = poke(context, data_addr, external, data, data_len);
if (rc < 0)
return -1;
data_addr = off;
data_len = 0;
}
/* append to saved data, flush later */
for (idx = 0, cp = buf+9 ; idx < len ; idx += 1, cp += 2) {
tmp = cp[2];
cp[2] = 0;
data[data_len + idx] = (uint8_t)strtoul(cp, NULL, 16);
cp[2] = tmp;
}
data_len += len;
}
/* flush any data remaining */
if (data_len != 0) {
if (is_external)
external = is_external(data_addr, data_len);
rc = poke(context, data_addr, external, data, data_len);
if (rc < 0)
return -1;
}
return 0;
}
vm_obj_map const & to_map(vm_obj const & o) {
lean_assert(is_external(o));
lean_assert(dynamic_cast<vm_rb_map*>(to_external(o)));
return static_cast<vm_rb_map*>(to_external(o))->m_map;
}
bool is_options(vm_obj const & o) {
return is_external(o) && dynamic_cast<vm_options*>(to_external(o));
}