void RTFusionKalman4::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings) { if (m_enableGyro) m_gyro = data.gyro; else m_gyro = RTVector3(); m_accel = data.accel; m_compass = data.compass; m_compassValid = data.compassValid; if (m_firstTime) { m_lastFusionTime = data.timestamp; calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination); m_Fk.fill(0); // init covariance matrix to something m_Pkk.fill(0); for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) m_Pkk.setVal(i,j, 0.5); // initialize the observation model Hk // Note: since the model is the state vector, this is an identity matrix so it won't be used // initialize the poses m_stateQ.fromEuler(m_measuredPose); m_fusionQPose = m_stateQ; m_fusionPose = m_measuredPose; m_firstTime = false; } else { m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000; m_lastFusionTime = data.timestamp; if (m_timeDelta <= 0) return; if (m_debug) { HAL_INFO("\n------\n"); HAL_INFO1("IMU update delta time: %f\n", m_timeDelta); } calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination); predict(); update(); m_stateQ.toEuler(m_fusionPose); m_fusionQPose = m_stateQ; if (m_debug | settings->m_fusionDebug) { HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose)); HAL_INFO(RTMath::displayRadians("Kalman pose", m_fusionPose)); HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose)); HAL_INFO(RTMath::display("Kalman quat", m_stateQ)); HAL_INFO(RTMath::display("Error quat", m_stateQError)); } } data.fusionPoseValid = true; data.fusionQPoseValid = true; data.fusionPose = m_fusionPose; data.fusionQPose = m_fusionQPose; }
bool RTIMUSettings::loadSettings() { char buf[200]; char key[200]; char val[200]; RTFLOAT ftemp; // preset general defaults m_imuType = RTIMU_TYPE_AUTODISCOVER; m_I2CSlaveAddress = 0; m_I2CBus = 1; m_fusionType = RTFUSION_TYPE_RTQF; m_compassCalValid = false; // MPU9150 defaults m_MPU9150GyroAccelSampleRate = 50; m_MPU9150CompassSampleRate = 25; m_MPU9150GyroAccelLpf = MPU9150_LPF_20; m_MPU9150GyroFsr = MPU9150_GYROFSR_1000; m_MPU9150AccelFsr = MPU9150_ACCELFSR_8; // GD20HM303D defaults m_GD20HM303DGyroSampleRate = L3GD20H_SAMPLERATE_50; m_GD20HM303DGyroBW = L3GD20H_BANDWIDTH_1; m_GD20HM303DGyroHpf = L3GD20H_HPF_4; m_GD20HM303DGyroFsr = L3GD20H_FSR_500; m_GD20HM303DAccelSampleRate = LSM303D_ACCEL_SAMPLERATE_50; m_GD20HM303DAccelFsr = LSM303D_ACCEL_FSR_8; m_GD20HM303DAccelLpf = LSM303D_ACCEL_LPF_50; m_GD20HM303DCompassSampleRate = LSM303D_COMPASS_SAMPLERATE_50; m_GD20HM303DCompassFsr = LSM303D_COMPASS_FSR_2; // GD20M303DLHC defaults m_GD20M303DLHCGyroSampleRate = L3GD20_SAMPLERATE_95; m_GD20M303DLHCGyroBW = L3GD20_BANDWIDTH_1; m_GD20M303DLHCGyroHpf = L3GD20_HPF_4; m_GD20M303DLHCGyroFsr = L3GD20H_FSR_500; m_GD20M303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50; m_GD20M303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8; m_GD20M303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30; m_GD20M303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3; // LSM9DS0 defaults m_LSM9DS0GyroSampleRate = LSM9DS0_GYRO_SAMPLERATE_95; m_LSM9DS0GyroBW = LSM9DS0_GYRO_BANDWIDTH_1; m_LSM9DS0GyroHpf = LSM9DS0_GYRO_HPF_4; m_LSM9DS0GyroFsr = LSM9DS0_GYRO_FSR_500; m_LSM9DS0AccelSampleRate = LSM9DS0_ACCEL_SAMPLERATE_50; m_LSM9DS0AccelFsr = LSM9DS0_ACCEL_FSR_8; m_LSM9DS0AccelLpf = LSM9DS0_ACCEL_LPF_50; m_LSM9DS0CompassSampleRate = LSM9DS0_COMPASS_SAMPLERATE_50; m_LSM9DS0CompassFsr = LSM9DS0_COMPASS_FSR_2; // check to see if settings file exists if (!(m_fd = fopen(m_filename, "r"))) { HAL_INFO("Settings file not found. Using defaults and creating settings file\n"); return saveSettings(); } while (fgets(buf, 200, m_fd)) { if ((buf[0] == '#') || (buf[0] == ' ') || (buf[0] == '\n')) // just a comment continue; if (sscanf(buf, "%[^=]=%s", key, val) != 2) { HAL_ERROR1("Bad line in settings file: %s\n", buf); fclose(m_fd); return false; } // now decode keys // general config if (strcmp(key, RTIMULIB_IMU_TYPE) == 0) { m_imuType = atoi(val); } else if (strcmp(key, RTIMULIB_FUSION_TYPE) == 0) { m_fusionType = atoi(val); } else if (strcmp(key, RTIMULIB_I2C_BUS) == 0) { m_I2CBus = atoi(val); } else if (strcmp(key, RTIMULIB_I2C_SLAVEADDRESS) == 0) { m_I2CSlaveAddress = atoi(val); // compass calibration } else if (strcmp(key, RTIMULIB_COMPASSCAL_VALID) == 0) { m_compassCalValid = strcmp(val, "true") == 0; } else if (strcmp(key, RTIMULIB_COMPASSCAL_MINX) == 0) { sscanf(val, "%f", &ftemp); m_compassCalMin.setX(ftemp); } else if (strcmp(key, RTIMULIB_COMPASSCAL_MINY) == 0) { sscanf(val, "%f", &ftemp); m_compassCalMin.setY(ftemp); } else if (strcmp(key, RTIMULIB_COMPASSCAL_MINZ) == 0) { sscanf(val, "%f", &ftemp); m_compassCalMin.setZ(ftemp); } else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXX) == 0) { sscanf(val, "%f", &ftemp); m_compassCalMax.setX(ftemp); } else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXY) == 0) { sscanf(val, "%f", &ftemp); m_compassCalMax.setY(ftemp); } else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXZ) == 0) { sscanf(val, "%f", &ftemp); m_compassCalMax.setZ(ftemp); // MPU9150 settings } else if (strcmp(key, RTIMULIB_MPU9150_GYROACCEL_SAMPLERATE) == 0) { m_MPU9150GyroAccelSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_MPU9150_COMPASS_SAMPLERATE) == 0) { m_MPU9150CompassSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_MPU9150_GYROACCEL_LPF) == 0) { m_MPU9150GyroAccelLpf = atoi(val); } else if (strcmp(key, RTIMULIB_MPU9150_GYRO_FSR) == 0) { m_MPU9150GyroFsr = atoi(val); } else if (strcmp(key, RTIMULIB_MPU9150_ACCEL_FSR) == 0) { m_MPU9150AccelFsr = atoi(val); // GD20HM303D settings } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_SAMPLERATE) == 0) { m_GD20HM303DGyroSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_FSR) == 0) { m_GD20HM303DGyroFsr = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_HPF) == 0) { m_GD20HM303DGyroHpf = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_GYRO_BW) == 0) { m_GD20HM303DGyroBW = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_ACCEL_SAMPLERATE) == 0) { m_GD20HM303DAccelSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_ACCEL_FSR) == 0) { m_GD20HM303DAccelFsr = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_ACCEL_LPF) == 0) { m_GD20HM303DAccelLpf = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_COMPASS_SAMPLERATE) == 0) { m_GD20HM303DCompassSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_GD20HM303D_COMPASS_FSR) == 0) { m_GD20HM303DCompassFsr = atoi(val); // GD20M303DLHC settings } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_SAMPLERATE) == 0) { m_GD20M303DLHCGyroSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_FSR) == 0) { m_GD20M303DLHCGyroFsr = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_HPF) == 0) { m_GD20M303DLHCGyroHpf = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_GYRO_BW) == 0) { m_GD20M303DLHCGyroBW = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_ACCEL_SAMPLERATE) == 0) { m_GD20M303DLHCAccelSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_ACCEL_FSR) == 0) { m_GD20M303DLHCAccelFsr = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_COMPASS_SAMPLERATE) == 0) { m_GD20M303DLHCCompassSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_GD20M303DLHC_COMPASS_FSR) == 0) { m_GD20M303DLHCCompassFsr = atoi(val); // LSM9DS0 settings } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_SAMPLERATE) == 0) { m_LSM9DS0GyroSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_FSR) == 0) { m_LSM9DS0GyroFsr = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_HPF) == 0) { m_LSM9DS0GyroHpf = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_GYRO_BW) == 0) { m_LSM9DS0GyroBW = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_ACCEL_SAMPLERATE) == 0) { m_LSM9DS0AccelSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_ACCEL_FSR) == 0) { m_LSM9DS0AccelFsr = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_ACCEL_LPF) == 0) { m_LSM9DS0AccelLpf = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_COMPASS_SAMPLERATE) == 0) { m_LSM9DS0CompassSampleRate = atoi(val); } else if (strcmp(key, RTIMULIB_LSM9DS0_COMPASS_FSR) == 0) { m_LSM9DS0CompassFsr = atoi(val); // Handle unrecognized key } else { HAL_ERROR1("Unrecognized key in settings file: %s\n", buf); } } HAL_INFO1("Settings file %s loaded\n", m_filename); fclose(m_fd); return saveSettings(); // make sure settings file is correct and complete }
static gboolean part_add_change_partition (char *device_file, guint64 start, guint64 size, guint64 new_start, guint64 new_size, guint64 *out_start, guint64 *out_size, char *type, char *label, char **flags, int geometry_hps, int geometry_spt) { int n; gboolean is_change; gboolean res; PedDevice *device; PedDisk *disk; PedPartition *part; PedConstraint* constraint; PedPartitionType ped_type; guint64 start_sector; guint64 end_sector; guint64 new_start_sector; guint64 new_end_sector; PartitionTable *p; PartitionTable *container_p; int container_entry; PartitionScheme scheme; guint8 mbr_flags = 0; guint8 mbr_part_type = 0; char *endp; guint64 gpt_attributes = 0; guint32 apm_status = 0; res = FALSE; is_change = FALSE; if (size == 0) { is_change = TRUE; } if (is_change) { HAL_INFO (("In part_change_partition: device_file=%s, start=%lld, new_start=%lld, new_size=%lld, type=%s", device_file, start, new_start, new_size, type)); } else { HAL_INFO (("In part_add_partition: device_file=%s, start=%lld, size=%lld, type=%s", device_file, start, size, type)); } /* first, find the kind of (embedded) partition table the new partition is going to be part of */ p = part_table_load_from_disk (device_file); if (p == NULL) { HAL_INFO (("Cannot load partition table from %s", device_file)); goto out; } part_table_find (p, start + 512, &container_p, &container_entry); scheme = part_table_get_scheme (container_p); if (is_change) { /* if changing, make sure there is a partition to change */ if (container_entry < 0) { HAL_INFO (("Couldn't find partition to change")); goto out; } } else { /* if adding, make sure there is no partition in the way... */ if (container_entry >= 0) { char *part_type; /* this might be Apple_Free if we're on PART_TYPE_APPLE */ part_type = part_table_entry_get_type (p, container_entry); if (! (p->scheme == PART_TYPE_APPLE && part_type != NULL && (strcmp (part_type, "Apple_Free") == 0))) { part_table_free (p); HAL_INFO (("There is a partition in the way on %s", device_file)); goto out; } } } HAL_INFO (("containing partition table scheme = %d", scheme)); part_table_free (p); p = NULL; if (!is_change) { if (type == NULL) { HAL_INFO (("No type specified")); goto out; } } /* now that we know the partitoning scheme, sanity check type and flags */ switch (scheme) { case PART_TYPE_MSDOS: case PART_TYPE_MSDOS_EXTENDED: mbr_flags = 0; if (flags != NULL) { for (n = 0; flags[n] != NULL; n++) { if (strcmp (flags[n], "boot") == 0) { mbr_flags |= 0x80; } else { HAL_INFO (("unknown flag '%s'", flags[n])); goto out; } } } if (type != NULL) { mbr_part_type = (guint8) (strtol (type, &endp, 0)); if (*endp != '\0') { HAL_INFO (("invalid type '%s' given", type)); goto out; } } if (label != NULL) { HAL_INFO (("labeled partitions not supported on MSDOS or MSDOS_EXTENDED")); goto out; } break; case PART_TYPE_GPT: gpt_attributes = 0; if (flags != NULL) { for (n = 0; flags[n] != NULL; n++) { if (strcmp (flags[n], "required") == 0) { gpt_attributes |= 1; } else { HAL_INFO (("unknown flag '%s'", flags[n])); goto out; } } } break; case PART_TYPE_APPLE: apm_status = 0; if (flags != NULL) { for (n = 0; flags[n] != NULL; n++) { if (strcmp (flags[n], "allocated") == 0) { apm_status |= (1<<1); } else if (strcmp (flags[n], "in_use") == 0) { apm_status |= (1<<2); } else if (strcmp (flags[n], "boot") == 0) { apm_status |= (1<<3); } else if (strcmp (flags[n], "allow_read") == 0) { apm_status |= (1<<4); } else if (strcmp (flags[n], "allow_write") == 0) { apm_status |= (1<<5); } else if (strcmp (flags[n], "boot_code_is_pic") == 0) { apm_status |= (1<<6); } else { HAL_INFO (("unknown flag '%s'", flags[n])); goto out; } } } break; default: HAL_INFO (("partitioning scheme %d not supported", scheme)); goto out; } switch (scheme) { case PART_TYPE_MSDOS: if (mbr_part_type == 0x05 || mbr_part_type == 0x85 || mbr_part_type == 0x0f) { ped_type = PED_PARTITION_EXTENDED; } else { ped_type = PED_PARTITION_NORMAL; } break; case PART_TYPE_MSDOS_EXTENDED: ped_type = PED_PARTITION_LOGICAL; if (mbr_part_type == 0x05 || mbr_part_type == 0x85 || mbr_part_type == 0x0f) { HAL_INFO (("Cannot create an extended partition inside an extended partition")); goto out; } break; default: ped_type = PED_PARTITION_NORMAL; break; } /* now, create the partition */ start_sector = start / 512; end_sector = (start + size) / 512 - 1; new_start_sector = new_start / 512; new_end_sector = (new_start + new_size) / 512 - 1; device = ped_device_get (device_file); if (device == NULL) { HAL_INFO (("ped_device_get() failed")); goto out; } HAL_INFO (("got it")); /* set drive geometry on libparted object if the user requested it */ if (geometry_hps > 0 && geometry_spt > 0 ) { /* not sure this is authorized use of libparted, but, eh, it seems to work */ device->hw_geom.cylinders = device->bios_geom.cylinders = device->length / geometry_hps / geometry_spt; device->hw_geom.heads = device->bios_geom.heads = geometry_hps; device->hw_geom.sectors = device->bios_geom.sectors = geometry_spt; } disk = ped_disk_new (device); if (disk == NULL) { HAL_INFO (("ped_disk_new() failed")); goto out_ped_device; } HAL_INFO (("got disk")); if (!is_change) { part = ped_partition_new (disk, ped_type, NULL, start_sector, end_sector); if (part == NULL) { HAL_INFO (("ped_partition_new() failed")); goto out_ped_disk; } HAL_INFO (("new partition")); } else { part = ped_disk_get_partition_by_sector (disk, start_sector); if (part == NULL) { HAL_INFO (("ped_partition_get_by_sector() failed")); goto out_ped_disk; } HAL_INFO (("got partition")); } /* TODO HACK XXX FIXME UGLY BAD: This is super ugly abuse of * libparted - we poke at their internal data structures - but * there ain't nothing we can do about it until libparted * provides API for this... */ if (scheme == PART_TYPE_GPT) { struct { efi_guid type; efi_guid uuid; char name[37]; int lvm; int raid; int boot; int hp_service; int hidden; /* more stuff */ } *gpt_data = (void *) part->disk_specific; if (type != NULL) { if (!set_le_guid ((guint8*) &gpt_data->type, type)) { HAL_INFO (("type '%s' for GPT appear to be malformed", type)); goto out_ped_partition; } } if (flags != NULL) { if (gpt_attributes & 1) { gpt_data->hidden = 1; } else { gpt_data->hidden = 0; } } } else if (scheme == PART_TYPE_MSDOS || scheme == PART_TYPE_MSDOS_EXTENDED) { struct { unsigned char system; int boot; /* more stuff */ } *dos_data = (void *) part->disk_specific; if (type != NULL) { dos_data->system = mbr_part_type; } if (flags != NULL) { if (mbr_flags & 0x80) { dos_data->boot = 1; } else { dos_data->boot = 0; } } } else if (scheme == PART_TYPE_APPLE) { struct { char volume_name[33]; /* eg: "Games" */ char system_name[33]; /* eg: "Apple_Unix_SVR2" */ char processor_name[17]; int is_boot; int is_driver; int has_driver; int is_root; int is_swap; int is_lvm; int is_raid; PedSector data_region_length; PedSector boot_region_length; guint32 boot_base_address; guint32 boot_entry_address; guint32 boot_checksum; guint32 status; /* more stuff */ } *mac_data = (void *) part->disk_specific; if (type != NULL) { memset (mac_data->system_name, 0, 33); strncpy (mac_data->system_name, type, 32); } if (flags != NULL) { mac_data->status = apm_status; } } if (label != NULL) { ped_partition_set_name (part, label); } if (geometry_hps > 0 && geometry_spt > 0 ) { /* respect drive geometry */ constraint = ped_constraint_any (device); } else if (geometry_hps == -1 && geometry_spt == -1 ) { /* undocumented (or is it?) libparted usage again.. it appears that * the probed geometry is stored in hw_geom */ device->bios_geom.cylinders = device->hw_geom.cylinders; device->bios_geom.heads = device->hw_geom.heads; device->bios_geom.sectors = device->hw_geom.sectors; constraint = ped_constraint_any (device); } else { PedGeometry *geo_start; PedGeometry *geo_end; /* ignore drive geometry */ if (is_change) { geo_start = ped_geometry_new (device, new_start_sector, 1); geo_end = ped_geometry_new (device, new_end_sector, 1); } else { geo_start = ped_geometry_new (device, start_sector, 1); geo_end = ped_geometry_new (device, end_sector, 1); } constraint = ped_constraint_new (ped_alignment_any, ped_alignment_any, geo_start, geo_end, 1, device->length); } try_change_again: if (is_change) { if (ped_disk_set_partition_geom (disk, part, constraint, new_start_sector, new_end_sector) == 0) { HAL_INFO (("ped_disk_set_partition_geom() failed")); goto out_ped_constraint; } } else { if (ped_disk_add_partition (disk, part, constraint) == 0) { HAL_INFO (("ped_disk_add_partition() failed")); goto out_ped_constraint; } } *out_start = part->geom.start * 512; *out_size = part->geom.length * 512; if (is_change) { /* make sure the resulting size is never smaller than requested * (this is because one will resize the FS and *then* change the partition table) */ if (*out_size < new_size) { HAL_INFO (("new_size=%lld but resulting size, %lld, smaller than requested", new_size, *out_size)); new_end_sector++; goto try_change_again; } else { HAL_INFO (("changed partition to start=%lld size=%lld", *out_start, *out_size)); } } else { HAL_INFO (("added partition start=%lld size=%lld", *out_start, *out_size)); } /* hmm, if we don't do this libparted crashes.. I assume that * ped_disk_add_partition assumes ownership of the * PedPartition when adding it... sadly this is not documented * anywhere.. sigh.. */ part = NULL; /* use commit_to_dev rather than just commit to avoid * libparted sending BLKRRPART to the kernel - we want to do * this ourselves... */ if (ped_disk_commit_to_dev (disk) == 0) { HAL_INFO (("ped_disk_commit_to_dev() failed")); goto out_ped_constraint; } HAL_INFO (("committed to disk")); res = TRUE; ped_constraint_destroy (constraint); ped_disk_destroy (disk); ped_device_destroy (device); goto out; out_ped_constraint: ped_constraint_destroy (constraint); out_ped_partition: if (part != NULL) { ped_partition_destroy (part); } out_ped_disk: ped_disk_destroy (disk); out_ped_device: ped_device_destroy (device); out: return res; }
bool RTIMUMPU9250::IMURead() { unsigned char fifoCount[2]; unsigned int count; unsigned char fifoData[MPU9250_FIFO_CHUNK_SIZE]; #if MPU9250_FIFO_WITH_COMPASS == 0 unsigned char compassData[8]; // compass data goes here if it is not coming in through FIFO #endif #if MPU9250_FIFO_WITH_TEMP == 0 unsigned char temperatureData[2]; // if temperature data is not coming in through FIFO #endif if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count")) return false; count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1]; // Debug // printf("FIFO Count: %d, Cache Count: %d, FIFO Chunk Length: %d, Max Cache Size: %d\n",count, m_cacheCount, MPU9250_FIFO_CHUNK_SIZE, MPU9250_CACHE_SIZE); if (count == 512) { HAL_INFO("MPU9250 fifo has overflowed"); resetFifo(); m_imuData.timestamp += m_sampleInterval * (512 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp return false; } #ifdef MPU9250_CACHE_MODE if ( (m_cacheCount == 0) && (count < MPU9250_FIFO_CHUNK_SIZE) ) return false; // no new set of data available if ((m_cacheCount == 0) && (count >= MPU9250_FIFO_CHUNK_SIZE) && (count < (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE)) ) { // special case of a small fifo and nothing cached - just handle as simple read if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) return false; #if MPU9250_FIFO_WITH_TEMP == 0 // read temp from registers if (!m_settings->HALRead(m_slaveAddr, MPU9250_TEMP_OUT_H, 2, temperatureData, "Failed to read temperature data")) return false; #endif #if MPU9250_FIFO_WITH_COMPASS == 0 // read compass without fifo if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data")) return false; #endif } else { if (count >= (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE)) { if (m_cacheCount == MPU9250_CACHE_BLOCK_COUNT) { // all cache blocks are full - discard oldest and update timestamp to account for lost samples m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count; if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT) m_cacheOut = 0; m_cacheCount--; } int blockCount = count / MPU9250_FIFO_CHUNK_SIZE; // number of chunks in fifo if (blockCount > MPU9250_CACHE_SIZE) blockCount = MPU9250_CACHE_SIZE; if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE * blockCount, m_cache[m_cacheIn].data, "Failed to read fifo data")) return false; #if MPU9250_FIFO_WITH_TEMP == 0 // read temp from registers if (!m_settings->HALRead(m_slaveAddr, MPU9250_TEMP_OUT_H, 2, m_cache[m_cacheIn].temperature, "Failed to read temperature data")) return false; #endif #if MPU9250_FIFO_WITH_COMPASS == 0 // read compass from register if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data")) return false; #endif m_cache[m_cacheIn].count = blockCount; m_cache[m_cacheIn].index = 0; m_cacheCount++; if (++m_cacheIn == MPU9250_CACHE_BLOCK_COUNT) m_cacheIn = 0; } // now fifo has been read if necessary, get something to process if (m_cacheCount == 0) return false; memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9250_FIFO_CHUNK_SIZE); #if MPU9250_FIFO_WITH_COMPASS == 0 memcpy(compassData, m_cache[m_cacheOut].compass, 8); #endif #if MPU9250_FIFO_WITH_TEMP == 0 memcpy(temperatureData, m_cache[m_cacheOut].temperature, 2); #endif m_cache[m_cacheOut].index += MPU9250_FIFO_CHUNK_SIZE; if (--m_cache[m_cacheOut].count == 0) { // this cache block is now empty if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT) m_cacheOut = 0; m_cacheCount--; } } #else if (count > MPU9250_FIFO_CHUNK_SIZE * 40) { // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) { if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) return false; count -= MPU9250_FIFO_CHUNK_SIZE; m_imuData.timestamp += m_sampleInterval; } } if (count < MPU9250_FIFO_CHUNK_SIZE) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) return false; #if MPU9250_FIFO_WITH_TEMP == 0 if (!m_settings->HALRead(m_slaveAddr, MPU9250_TEMP_OUT_H, 2, temperatureData, "Failed to read temperature data")) return false; #endif #if MPU9250_FIFO_WITH_COMPASS == 0 if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data")) return false; #endif #endif // Accelerometer RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true); #if MPU9250_FIFO_WITH_TEMP == 1 // Temperature m_imuData.IMUtemperature = ((RTFLOAT)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])) / 333.87f ) + 21.0f; // combined registers and convert to temperature m_imuData.IMUtemperatureValid = true; // Gyroscope RTMath::convertToVector(fifoData + 8, m_imuData.gyro, m_gyroScale, true); // Compass #if MPU9250_FIFO_WITH_COMPASS == 1 RTMath::convertToVector(fifoData + 14 + 1, m_imuData.compass, 0.6f, false); #else RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false); #endif #else // no temp in fifo // Temperature m_imuData.IMUtemperature = ((RTFLOAT)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])) / 333.87f ) + 21.0f; // combined registers and convert to temperature m_imuData.IMUtemperatureValid = true; // ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity) + 21degC // 333.87 = sensitivity // 0 = room temp offset at 21 // Gyroscope RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true); // Compass #if MPU9250_FIFO_WITH_COMPASS == 1 // without temp but with compass in FIFO RTMath::convertToVector(fifoData + 12 + 1, m_imuData.compass, 0.6f, false); #else RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false); #endif #endif // FIFO contains data from register 59 up to register 96 in that order // (given sensor path was reset, otherwise the data order is not correct) // ACC (6), TEMP(2), GYRO(6), EXT SENS(8, up to 24)) // Debug /* Serial.print("FIFO: "); for (unsigned int i=0; i < MPU9250_FIFO_CHUNK_SIZE; i++) { Serial.printf("%x, ", fifoData[i] ); } #if MPU9250_FIFO_WITH_COMPASS == 0 Serial.print("Compass: "******"%x, ", compassData[i] ); } #endif #if MPU9250_FIFO_WITH_TEMP == 0 Serial.print("Temperature: "); Serial.printf("%x, ", temperatureData[0] ); Serial.printf("%x, ", temperatureData[1] ); #endif Serial.print("\n"); Serial.printf("T valid %x \n", m_imuData.IMUtemperatureValid); Serial.printf("T %i \n", (int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])); float TEMP_OUT = (float)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])); Serial.printf("T %f \n", (TEMP_OUT/333.87f)+21.0f); */ // sort out gyro axes m_imuData.gyro.setX(m_imuData.gyro.x()); m_imuData.gyro.setY(-m_imuData.gyro.y()); m_imuData.gyro.setZ(-m_imuData.gyro.z()); // sort out accel data; m_imuData.accel.setX(-m_imuData.accel.x()); // use the compass fuse data adjustments m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]); m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]); m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]); // sort out compass axes float temp; temp = m_imuData.compass.x(); m_imuData.compass.setX(m_imuData.compass.y()); m_imuData.compass.setY(-temp); if (m_firstTime) m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); else m_imuData.timestamp += m_sampleInterval; m_firstTime = false; // now do standard processing if (m_imuData.IMUtemperatureValid == true) { // Check if temperature changed if (fabs(m_imuData.IMUtemperature - m_IMUtemperature_previous) >= TEMPERATURE_DELTA) { // If yes, update bias updateTempBias(m_imuData.IMUtemperature); m_IMUtemperature_previous = m_imuData.IMUtemperature; } // Then do handleTempBias(); // temperature Correction } handleGyroBias(); calibrateAverageCompass(); calibrateAccel(); // now update the filter updateFusion(); return true; }
bool RTIMUBNO055::IMUInit() { unsigned char result; m_slaveAddr = m_settings->m_I2CSlaveAddress; m_lastReadTime = RTMath::currentUSecsSinceEpoch(); // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.motion = true; m_imuData.IMUtemperatureValid = false; m_imuData.IMUtemperature = 0.0; m_imuData.humidityValid = false; m_imuData.humidity = -1.0; m_imuData.humidityTemperatureValid = false; m_imuData.humidityTemperature = 0.0; m_imuData.pressureValid = false; m_imuData.pressure = 0.0; m_imuData.pressureTemperatureValid = false; m_imuData.pressureTemperature = 0.0; if (!m_settings->HALRead(m_slaveAddr, BNO055_WHO_AM_I, 1, &result, "Failed to read BNO055 id")) return false; if (result != BNO055_ID) { HAL_ERROR1("Incorrect IMU id %d", result); return false; } if (!m_settings->HALWrite(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_CONFIG, "Failed to set BNO055 into config mode")) return false; m_settings->delayMs(50); if (!m_settings->HALWrite(m_slaveAddr, BNO055_SYS_TRIGGER, 0x20, "Failed to reset BNO055")) return false; m_settings->delayMs(50); while (1) { if (!m_settings->HALRead(m_slaveAddr, BNO055_WHO_AM_I, 1, &result, "")) continue; if (result == BNO055_ID) break; m_settings->delayMs(50); } m_settings->delayMs(50); if (!m_settings->HALWrite(m_slaveAddr, BNO055_PWR_MODE, BNO055_PWR_MODE_NORMAL, "Failed to set BNO055 normal power mode")) return false; m_settings->delayMs(50); if (!m_settings->HALWrite(m_slaveAddr, BNO055_PAGE_ID, 0, "Failed to set BNO055 page 0")) return false; m_settings->delayMs(50); if (!m_settings->HALWrite(m_slaveAddr, BNO055_SYS_TRIGGER, 0x00, "Failed to start BNO055")) return false; m_settings->delayMs(50); if (!m_settings->HALWrite(m_slaveAddr, BNO055_UNIT_SEL, 0x87, "Failed to set BNO055 units")) return false; m_settings->delayMs(50); if (!m_settings->HALWrite(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_NDOF, "Failed to set BNO055 into 9-dof mode")) return false; m_settings->delayMs(50); HAL_INFO("BNO055 init complete\n"); return true; }
void drvctl_fini(void) { HAL_INFO (("drvctl_fini")); }
static void runner_server_unregister_handler (DBusConnection *connection, void *user_data) { HAL_INFO (("unregistered")); }
static PartitionTable * part_table_parse_gpt (int fd, guint64 offset, guint64 size) { int n; PartitionTable *p; guint8 buf[16]; guint64 partition_entry_lba; int num_entries; int size_of_entry; HAL_INFO (("Entering EFI GPT parser")); /* by way of getting here, we've already checked for a protective MBR */ p = NULL; /* Check GPT signature */ if (lseek (fd, offset + 512 + 0, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, buf, 8) != 8) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } if (memcmp (buf, GPT_MAGIC, 8) != 0) { HAL_INFO (("No GPT_MAGIC found")); goto out; } HAL_INFO (("GPT magic found")); /* Disk UUID */ if (lseek (fd, offset + 512 + 56, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, buf, 16) != 16) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } //hexdump ((guint8*) buf, 16); if (lseek (fd, offset + 512 + 72, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, buf, 8) != 8) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } partition_entry_lba = get_le64 (buf); if (lseek (fd, offset + 512 + 80, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, buf, 4) != 4) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } num_entries = get_le32 (buf); if (lseek (fd, offset + 512 + 84, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, buf, 4) != 4) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } size_of_entry = get_le32(buf); p = part_table_new_empty (PART_TYPE_GPT); p->offset = offset; p->size = size; HAL_INFO (("partition_entry_lba=%d", partition_entry_lba)); HAL_INFO (("num_entries=%d", num_entries)); HAL_INFO (("size_of_entry=%d", size_of_entry)); for (n = 0; n < num_entries; n++) { PartitionEntry *pe; struct { guint8 partition_type_guid[16]; guint8 partition_guid[16]; guint8 starting_lba[8]; guint8 ending_lba[8]; guint8 attributes[8]; guint8 partition_name[72]; } gpt_part_entry; char *partition_type_guid; if (lseek (fd, offset + partition_entry_lba * 512 + n * size_of_entry, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, &gpt_part_entry, 128) != 128) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } partition_type_guid = get_le_guid (gpt_part_entry.partition_type_guid); if (strcmp (partition_type_guid, GPT_PART_TYPE_GUID_EMPTY) == 0) continue; pe = part_entry_new (NULL, (guint8*) &gpt_part_entry, 128, offset + partition_entry_lba * 512 + n * size_of_entry); p->entries = g_slist_append (p->entries, pe); g_free (partition_type_guid); //hexdump ((guint8 *) &gpt_part_entry, 128); } out: HAL_INFO (("Leaving EFI GPT parser")); return p; }
static PartitionTable * part_table_parse_apple (int fd, guint64 offset, guint64 size) { int n; PartitionTable *p; struct { guint16 signature; guint16 block_size; guint32 block_count; /* more stuff */ } __attribute__ ((packed)) mac_header; struct { guint16 signature; guint16 res1; guint32 map_count; guint32 start_block; guint32 block_count; char name[32]; char type[32]; guint32 data_start; guint32 data_count; guint32 status; guint32 boot_start; guint32 boot_size; guint32 boot_load; guint32 boot_load2; guint32 boot_entry; guint32 boot_entry2; guint32 boot_cksum; char processor[16]; /* identifies ISA of boot */ /* more stuff */ } __attribute__ ((packed)) mac_part; int block_size; int block_count; int map_count; HAL_INFO (("Entering Apple parser")); p = NULL; /* Check Mac start of disk signature */ if (lseek (fd, offset + 0, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, &mac_header, sizeof (mac_header)) != sizeof (mac_header)) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } if (memcmp (&(mac_header.signature), MAC_MAGIC, 2) != 0) { HAL_INFO (("No MAC_MAGIC found")); goto out; } block_size = GUINT16_FROM_BE (mac_header.block_size); block_count = GUINT32_FROM_BE (mac_header.block_count); /* num blocks on whole disk */ HAL_INFO (("Mac MAGIC found, block_size=%d", block_size)); p = part_table_new_empty (PART_TYPE_APPLE); p->offset = offset; p->size = size; /* get number of entries from first entry */ if (lseek (fd, offset + block_size, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, &mac_part, sizeof (mac_part)) != sizeof (mac_part)) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } map_count = GUINT32_FROM_BE (mac_part.map_count); /* num blocks in part map */ HAL_INFO (("map_count = %d", map_count)); for (n = 0; n < map_count; n++) { PartitionEntry *pe; if (memcmp (&(mac_part.signature), MAC_PART_MAGIC, 2) != 0) { HAL_INFO (("No MAC_PART_MAGIC found")); break; } if (lseek (fd, offset + (n + 1) * block_size, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, &mac_part, sizeof (mac_part)) != sizeof (mac_part)) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } pe = part_entry_new (NULL, (guint8*) &mac_part, sizeof (mac_part), offset + (n + 1) * block_size); p->entries = g_slist_append (p->entries, pe); } out: HAL_INFO (("Leaving Apple parser")); return p; }
static PartitionTable * part_table_parse_msdos_extended (int fd, guint64 offset, guint64 size) { int n; PartitionTable *p; guint64 next; //HAL_INFO (("Entering MS-DOS extended parser")); p = NULL; next = offset; while (next != 0) { guint64 readfrom; const guint8 embr[512]; readfrom = next; next = 0; //HAL_INFO (("readfrom = %lld", readfrom)); if (lseek (fd, readfrom, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, &embr, sizeof (embr)) != sizeof (embr)) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } if (memcmp (&embr[MSDOS_SIG_OFF], MSDOS_MAGIC, 2) != 0) { HAL_INFO (("No MSDOS_MAGIC found")); goto out; } //HAL_INFO (("MSDOS_MAGIC found")); if (p == NULL) { p = part_table_new_empty (PART_TYPE_MSDOS_EXTENDED); p->offset = offset; p->size = size; } for (n = 0; n < 2; n++) { PartitionEntry *pe; guint64 pstart; guint64 psize; pstart = 0x200 * ((guint64) get_le32 (&(embr[MSDOS_PARTTABLE_OFFSET + n * 16 + 8]))); psize = 0x200 * ((guint64) get_le32 (&(embr[MSDOS_PARTTABLE_OFFSET + n * 16 + 12]))); if (psize == 0) continue; pe = NULL; if (n == 0) { //HAL_INFO (("part %d (offset %lld, size %lld, type 0x%02x)", // n, readfrom + pstart, psize, ptype)); //HAL_INFO (("pstart = %lld", pstart)); //hexdump (&(embr[MSDOS_PARTTABLE_OFFSET + n * 16]), 16); pe = part_entry_new (NULL, &(embr[MSDOS_PARTTABLE_OFFSET + n * 16]), 16, readfrom + MSDOS_PARTTABLE_OFFSET + n * 16); } else { if (pstart != 0) { //HAL_INFO (("found chain at offset %lld", offset + pstart); next = offset + pstart; } } //HAL_INFO (("pe = %p", pe)); if (pe != NULL) { p->entries = g_slist_append (p->entries, pe); } } } out: //HAL_INFO (("Exiting MS-DOS extended parser")); return p; }
static PartitionTable * part_table_parse_msdos (int fd, guint64 offset, guint64 size, gboolean *found_gpt) { int n; const guint8 mbr[512]; PartitionTable *p; //HAL_INFO (("Entering MS-DOS parser")); *found_gpt = FALSE; p = NULL; if (lseek (fd, offset, SEEK_SET) < 0) { HAL_INFO (("lseek failed (%s)", strerror (errno))); goto out; } if (read (fd, &mbr, sizeof (mbr)) != sizeof (mbr)) { HAL_INFO (("read failed (%s)", strerror (errno))); goto out; } if (memcmp (&mbr[MSDOS_SIG_OFF], MSDOS_MAGIC, 2) != 0) { HAL_INFO (("No MSDOS_MAGIC found")); goto out; } //HAL_INFO (("MSDOS_MAGIC found")); /* sanity checks */ for (n = 0; n < 4; n++) { if (mbr[MSDOS_PARTTABLE_OFFSET + n * 16 + 0] != 0 && mbr[MSDOS_PARTTABLE_OFFSET + n * 16 + 0] != 0x80) { HAL_INFO (("partitioning flag for part %d is not 0x00 or 0x80", n)); goto out; } /* protective MBR for GPT => GPT, not MS-DOS */ if (mbr[MSDOS_PARTTABLE_OFFSET + n * 16 + 4] == 0xee) { HAL_INFO (("found partition type 0xee => protective MBR for GPT", n)); *found_gpt = TRUE; goto out; } } p = part_table_new_empty (PART_TYPE_MSDOS); p->offset = offset; p->size = size; /* we _always_ want to create four partitions */ for (n = 0; n < 4; n++) { PartitionEntry *pe; guint64 pstart; guint64 psize; guint8 ptype; PartitionTable *e_part_table; pstart = 0x200 * ((guint64) get_le32 (&(mbr[MSDOS_PARTTABLE_OFFSET + n * 16 + 8]))); psize = 0x200 * ((guint64) get_le32 (&(mbr[MSDOS_PARTTABLE_OFFSET + n * 16 + 12]))); ptype = mbr[MSDOS_PARTTABLE_OFFSET + n * 16 + 4]; //HAL_INFO (("looking at part %d (offset %lld, size %lld, type 0x%02x)", n, pstart, psize, ptype)); pe = NULL; e_part_table = NULL; /* look for embedded partition tables */ switch (ptype) { /* extended partitions */ case 0x05: /* MS-DOS */ case 0x0f: /* Win95 */ case 0x85: /* Linux */ e_part_table = part_table_parse_msdos_extended (fd, pstart, psize); if (e_part_table != NULL) { pe = part_entry_new (e_part_table, &(mbr[MSDOS_PARTTABLE_OFFSET + n * 16]), 16, offset + MSDOS_PARTTABLE_OFFSET + n * 16); } break; case 0xa5: /* FreeBSD */ case 0xa6: /* OpenBSD */ case 0xa9: /* NetBSD */ //e_part_table = part_table_parse_bsd (fd, pstart, psize); //break; default: //HAL_INFO (("new part entry")); pe = part_entry_new (NULL, &(mbr[MSDOS_PARTTABLE_OFFSET + n * 16]), 16, offset + MSDOS_PARTTABLE_OFFSET + n * 16); break; } //HAL_INFO (("pe = %p", pe)); p->entries = g_slist_append (p->entries, pe); } out: //HAL_INFO (("Exiting MS-DOS parser")); return p; }
static gboolean set_le_guid (guint8 *buf, const char *source) { efi_guid *guid = (efi_guid *) buf; guint32 __attribute__((__unused__)) data1; guint16 __attribute__((__unused__)) data2; guint16 __attribute__((__unused__)) data3; guint8 __attribute__((__unused__)) data4[8]; gboolean ret; int n; n = sscanf (source, "%x-%hx-%hx-%02hhx%02hhx-%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", &guid->data1, &guid->data2, &guid->data3, &(guid->data4[0]), &(guid->data4[1]), &(guid->data4[2]), &(guid->data4[3]), &(guid->data4[4]), &(guid->data4[5]), &(guid->data4[6]), &(guid->data4[7])); if (n != 11) { HAL_INFO (("guid '%s' is not valid")); goto out; } #if 0 HAL_INFO (("source = %s", source)); HAL_INFO (("data1 = %08x", guid->data1)); HAL_INFO (("data2 = %04x", guid->data2)); HAL_INFO (("data3 = %04x", guid->data3)); HAL_INFO (("data4[0] = %02x", guid->data4[0])); HAL_INFO (("data4[1] = %02x", guid->data4[1])); HAL_INFO (("data4[2] = %02x", guid->data4[2])); HAL_INFO (("data4[3] = %02x", guid->data4[3])); HAL_INFO (("data4[4] = %02x", guid->data4[4])); HAL_INFO (("data4[5] = %02x", guid->data4[5])); HAL_INFO (("data4[6] = %02x", guid->data4[6])); HAL_INFO (("data4[7] = %02x", guid->data4[7])); #endif guid->data1 = GUINT32_TO_LE (guid->data1); guid->data2 = GUINT16_TO_LE (guid->data2); guid->data3 = GUINT16_TO_LE (guid->data3); ret = TRUE; out: return ret; }
gboolean part_create_partition_table (char *device_file, PartitionScheme scheme) { PedDevice *device; PedDisk *disk; PedDiskType *disk_type; gboolean ret; ret = FALSE; HAL_INFO (("In part_create_partition_table: device_file=%s, scheme=%d", device_file, scheme)); device = ped_device_get (device_file); if (device == NULL) { HAL_INFO (("ped_device_get() failed")); goto out; } HAL_INFO (("got it")); switch (scheme) { case PART_TYPE_MSDOS: disk_type = ped_disk_type_get ("msdos"); break; case PART_TYPE_APPLE: disk_type = ped_disk_type_get ("mac"); break; case PART_TYPE_GPT: disk_type = ped_disk_type_get ("gpt"); break; default: disk_type = NULL; break; } if (disk_type == NULL) { HAL_INFO (("Unknown or unsupported partitioning scheme %d", scheme)); goto out; } disk = ped_disk_new_fresh (device, disk_type); if (disk == NULL) { HAL_INFO (("ped_disk_new_fresh() failed")); goto out_ped_device; } HAL_INFO (("got disk")); if (ped_disk_commit_to_dev (disk) == 0) { HAL_INFO (("ped_disk_commit_to_dev() failed")); goto out_ped_disk; } HAL_INFO (("committed to disk")); ret = TRUE; ped_disk_destroy (disk); ped_device_destroy (device); goto out; out_ped_disk: ped_disk_destroy (disk); out_ped_device: ped_device_destroy (device); out: return ret; }
gboolean part_del_partition (char *device_file, guint64 offset) { gboolean ret; PedDevice *device; PedDisk *disk; PedPartition *part; PartitionTable *p; gboolean is_extended; int n; HAL_INFO (("In part_del_partition: device_file=%s, offset=%lld", device_file, offset)); ret = FALSE; /* sigh.. one would think that if you passed the sector of where the * the beginning of the extended partition starts, then _by_sector * would return the same as _extended_partition. * * Sadly it's not so.. * * So, check if the passed offset actually corresponds to a nested * partition table... */ is_extended = FALSE; p = part_table_load_from_disk (device_file); if (p == NULL) { HAL_INFO (("Cannot load partition table from %s", device_file)); goto out; } for (n = 0; n < part_table_get_num_entries (p); n++) { PartitionTable *nested; nested = part_table_entry_get_nested (p, n); if (nested != NULL) { if (part_table_get_offset (nested) == offset) { HAL_INFO (("partition to delete is an extended partition")); is_extended = TRUE; } } } part_table_free (p); device = ped_device_get (device_file); if (device == NULL) { HAL_INFO (("ped_device_get() failed")); goto out; } HAL_INFO (("got it")); disk = ped_disk_new (device); if (disk == NULL) { HAL_INFO (("ped_disk_new() failed")); goto out_ped_device; } HAL_INFO (("got disk")); if (is_extended) { part = ped_disk_extended_partition (disk); } else { part = ped_disk_get_partition_by_sector (disk, offset / 512); } if (part == NULL) { HAL_INFO (("ped_disk_get_partition_by_sector() failed")); goto out_ped_disk; } HAL_INFO (("got partition - part->type=%d", part->type)); /* allow only to delete primary, logical and extended partitions */ if (! ((part->type == PED_PARTITION_NORMAL) || (part->type == PED_PARTITION_LOGICAL) || (part->type == PED_PARTITION_EXTENDED))) { HAL_INFO (("no data partition at given offset %lld for device %s", offset, device_file)); goto out_ped_disk; } if (ped_disk_delete_partition (disk, part) == 0) { HAL_INFO (("ped_disk_delete_partition() failed")); goto out_ped_disk; } /* use commit_to_dev rather than just commit to avoid * libparted sending BLKRRPART to the kernel - we want to do * this ourselves... */ if (ped_disk_commit_to_dev (disk) == 0) { HAL_INFO (("ped_disk_commit_to_dev() failed")); goto out_ped_disk; } HAL_INFO (("committed to disk")); ret = TRUE; ped_disk_destroy (disk); ped_device_destroy (device); goto out; out_ped_disk: ped_disk_destroy (disk); out_ped_device: ped_device_destroy (device); out: return ret; }
bool RTIMUGD20M303DLHC::IMURead() { unsigned char status; unsigned char gyroData[6]; unsigned char accelData[6]; unsigned char compassData[6]; #ifdef GD20M303DLHC_CACHE_MODE int count; if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_FIFO_SRC, 1, &status, "Failed to read L3GD20 fifo status")) return false; if ((status & 0x40) != 0) { HAL_INFO("L3GD20 fifo overrun\n"); if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL5, 0x10, "Failed to set L3GD20 CTRL5")) return false; if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x0, "Failed to set L3GD20 FIFO mode")) return false; if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode")) return false; if (!setGyroCTRL5()) return false; m_imuData.timestamp += m_sampleInterval * 32; return false; } // get count of samples in fifo count = status & 0x1f; if ((m_cacheCount == 0) && (count > 0) && (count < GD20M303DLHC_FIFO_THRESH)) { // special case of a small fifo and nothing cached - just handle as simple read if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data")) return false; if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data")) return false; if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data")) return false; if (m_firstTime) m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); else m_imuData.timestamp += m_sampleInterval; m_firstTime = false; } else { if (count >= GD20M303DLHC_FIFO_THRESH) { // need to create a cache block if (m_cacheCount == GD20M303DLHC_CACHE_BLOCK_COUNT) { // all cache blocks are full - discard oldest and update timestamp to account for lost samples m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count; if (++m_cacheOut == GD20M303DLHC_CACHE_BLOCK_COUNT) m_cacheOut = 0; m_cacheCount--; } if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, GD20M303DLHC_FIFO_CHUNK_SIZE * GD20M303DLHC_FIFO_THRESH, m_cache[m_cacheIn].data, "Failed to read L3GD20 fifo data")) return false; if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, m_cache[m_cacheIn].accel, "Failed to read LSM303DLHC accel data")) return false; if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, m_cache[m_cacheIn].compass, "Failed to read LSM303DLHC compass data")) return false; m_cache[m_cacheIn].count = GD20M303DLHC_FIFO_THRESH; m_cache[m_cacheIn].index = 0; m_cacheCount++; if (++m_cacheIn == GD20M303DLHC_CACHE_BLOCK_COUNT) m_cacheIn = 0; } // now fifo has been read if necessary, get something to process if (m_cacheCount == 0) return false; memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20M303DLHC_FIFO_CHUNK_SIZE); memcpy(accelData, m_cache[m_cacheOut].accel, 6); memcpy(compassData, m_cache[m_cacheOut].compass, 6); m_cache[m_cacheOut].index += GD20M303DLHC_FIFO_CHUNK_SIZE; if (--m_cache[m_cacheOut].count == 0) { // this cache block is now empty if (++m_cacheOut == GD20M303DLHC_CACHE_BLOCK_COUNT) m_cacheOut = 0; m_cacheCount--; } if (m_firstTime) m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); else m_imuData.timestamp += m_sampleInterval; m_firstTime = false; } #else if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_STATUS, 1, &status, "Failed to read L3GD20 status")) return false; if ((status && 0x8) == 0) return false; if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data")) return false; m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data")) return false; if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data")) return false; #endif RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false); RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false); m_imuData.compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY); m_imuData.compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY); m_imuData.compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ); // sort out gyro axes m_imuData.gyro.setX(m_imuData.gyro.x()); m_imuData.gyro.setY(-m_imuData.gyro.y()); m_imuData.gyro.setZ(-m_imuData.gyro.z()); // sort out accel data; m_imuData.accel.setX(-m_imuData.accel.x()); // sort out compass axes RTFLOAT temp; temp = m_imuData.compass.z(); m_imuData.compass.setZ(-m_imuData.compass.y()); m_imuData.compass.setY(-temp); // now do standard processing handleGyroBias(); calibrateAverageCompass(); calibrateAccel(); // now update the filter updateFusion(); return true; }
HalDevice * devinfo_mass_add(HalDevice *parent, const char *devnode, char *devfs_path, char *device_type) { HalDevice *d = NULL, *gparent = NULL; prop_dictionary_t dict; struct disklabel label; struct stat st; const char *driver; char *rdevpath, *devpath; char *childnode; char *parent_devnode, *gparent_devnode = NULL; char *gparent_udi; int16_t unit; int i, fd; struct scsipi_inquiry_data inqbuf; struct scsipi_inquiry cmd; bool scsiinq_status; char *storage_model = NULL, *storage_vendor = NULL; if (drvctl_find_device (devnode, &dict) == FALSE || dict == NULL) return NULL; if (prop_dictionary_get_int16 (dict, "device-unit", &unit) == false || prop_dictionary_get_cstring_nocopy (dict, "device-driver", &driver) == false) { prop_object_release (dict); return NULL; } if (strcmp (driver, "wd") != 0 && strcmp (driver, "sd") != 0 && strcmp (driver, "ld") != 0) { prop_object_release (dict); return NULL; } sleep (1); devpath = g_strdup_printf ("/dev/%s%c", devnode, RAW_PART + 'a'); rdevpath = g_strdup_printf ("/dev/r%s%c", devnode, RAW_PART + 'a'); HAL_INFO ((" going to open %s", rdevpath)); fd = open (rdevpath, O_RDONLY); if (fd < 0) { HAL_WARNING (("couldn't open %s: %s", rdevpath, strerror (errno))); g_free (rdevpath); g_free (devpath); return NULL; } HAL_INFO ((" going to DIOCGDINFO %s", rdevpath)); if (ioctl (fd, DIOCGDINFO, &label) == -1) { HAL_WARNING (("DIOCGDINFO failed on %s: %s", rdevpath, strerror (errno))); g_free (rdevpath); g_free (devpath); close (fd); return NULL; } if (strcmp (driver, "sd") == 0) { memset(&cmd, 0, sizeof (cmd)); memset(&inqbuf, 0, sizeof (inqbuf)); cmd.opcode = INQUIRY; cmd.length = sizeof (inqbuf); scsiinq_status = scsi_command (fd, &cmd, sizeof (cmd), &inqbuf, sizeof (inqbuf), 10000, SCCMD_READ); } else scsiinq_status = false; close (fd); d = hal_device_new (); devinfo_set_default_properties (d, parent, devnode, devfs_path); hal_device_add_capability (d, "block"); hal_device_property_set_string (d, "info.subsystem", "block"); hal_device_property_set_string (d, "block.device", devpath); if (stat (devpath, &st) == 0) { hal_device_property_set_int (d, "block.major", major (st.st_rdev)); hal_device_property_set_int (d, "block.minor", minor (st.st_rdev)); } hal_device_property_set_bool (d, "block.is_volume", FALSE); hal_device_property_set_bool (d, "block.no_partitions", FALSE); hal_device_property_set_bool (d, "block.have_scanned", TRUE); hal_device_add_capability (d, "storage"); hal_device_property_set_string (d, "info.category", "storage"); parent_devnode = hal_device_property_get_string (parent, "netbsd.device"); gparent_udi = hal_device_property_get_string (parent, "info.parent"); if (gparent_udi) { gparent = hal_device_store_find (hald_get_gdl (), gparent_udi); if (gparent) gparent_devnode = hal_device_property_get_string (gparent, "netbsd.device"); } if (gparent_devnode && strstr (gparent_devnode, "umass") == gparent_devnode) hal_device_property_set_string (d, "storage.bus", "usb"); else if (parent_devnode && strstr (parent_devnode, "atabus") == parent_devnode) hal_device_property_set_string (d, "storage.bus", "ide"); else hal_device_property_set_string (d, "storage.bus", "scsi"); hal_device_property_set_string (d, "storage.device_type", "disk"); hal_device_property_set_bool (d, "storage.removable", label.d_flags & D_REMOVABLE ? TRUE : FALSE); if (label.d_flags & D_REMOVABLE) { hal_device_property_set_bool (d, "storage.removable.media_available", TRUE); hal_device_property_set_uint64 (d, "storage.removable.media_size", (uint64_t)label.d_secsize * (uint64_t)label.d_secperunit); hal_device_property_set_uint64 (d, "storage.size", 0); hal_device_property_set_bool (d, "storage.hotpluggable", TRUE); hal_device_property_set_bool (d, "storage.automount_enabled_hint", TRUE); } else { hal_device_property_set_bool (d, "storage.removable.media_available", FALSE); hal_device_property_set_uint64 (d, "storage.removable.media_size", 0); hal_device_property_set_uint64 (d, "storage.size", (uint64_t)label.d_secsize * (uint64_t)label.d_secperunit); hal_device_property_set_bool (d, "storage.hotpluggable", FALSE); hal_device_property_set_bool (d, "storage.automount_enabled_hint", FALSE); } hal_device_property_set_bool (d, "storage.no_partitions_hint", FALSE); hal_device_property_set_bool (d, "storage.requires_eject", FALSE); hal_device_property_set_bool (d, "storage.removable.support_async_notification", FALSE); hal_device_property_set_string (d, "storage.partitioning_scheme", "mbr"); hal_device_property_set_string (d, "storage.originating_device", hal_device_property_get_string (d, "info.udi")); if (scsiinq_status == true) { storage_model = rtrim_copy(inqbuf.product, sizeof (inqbuf.product)); storage_vendor = rtrim_copy(inqbuf.vendor, sizeof (inqbuf.vendor)); } if (storage_model) { hal_device_property_set_string (d, "storage.model", storage_model); free (storage_model); } else hal_device_property_set_string (d, "storage.model", label.d_packname); if (storage_vendor) { hal_device_property_set_string (d, "storage.vendor", storage_vendor); free (storage_vendor); } else hal_device_property_set_string (d, "storage.vendor", label.d_typename); devinfo_add_enqueue (d, devfs_path, &devinfo_mass_handler); for (i = 0; i < MAXPARTITIONS; i++) { const char *fstype; fstype = devinfo_mass_get_fstype(label.d_partitions[i].p_fstype); if (fstype == NULL) continue; childnode = g_strdup_printf ("%s%c", devnode, 'a' + i); HAL_INFO ((" adding %s on %s", childnode, rdevpath)); devinfo_mass_disklabel_add (d, childnode, childnode, childnode); g_free (childnode); } HAL_INFO ((" returning")); g_free (rdevpath); g_free (devpath); done: prop_object_release (dict); return d; }
bool RTIMUGD20M303DLHC::IMUInit() { unsigned char result; #ifdef GD20M303DLHC_CACHE_MODE m_firstTime = true; m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.pressureValid = false; m_imuData.temperatureValid = false; m_imuData.humidityValid = false; // configure IMU m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS; m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS; setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_CTRL5, 0x80, "Failed to boot L3GD20")) return false; if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20_WHO_AM_I, 1, &result, "Failed to read L3GD20 id")) return false; if (result != L3GD20_ID) { HAL_ERROR1("Incorrect L3GD20 id %d\n", result); return false; } if (!setGyroSampleRate()) return false; if (!setGyroCTRL2()) return false; if (!setGyroCTRL4()) return false; // Set up the accel if (!setAccelCTRL1()) return false; if (!setAccelCTRL4()) return false; // Set up the compass if (!setCompassCRA()) return false; if (!setCompassCRB()) return false; if (!setCompassCRM()) return false; #ifdef GD20M303DLHC_CACHE_MODE // turn on gyro fifo if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode")) return false; #endif if (!setGyroCTRL5()) return false; gyroBiasInit(); HAL_INFO("GD20M303DLHC init complete\n"); return true; }
HalDevice * devinfo_mass_disklabel_add(HalDevice *parent, const char *devnode, char *devfs_path, char *device_type) { HalDevice *d = NULL; struct disklabel label; struct partition *part; struct stat st; const char *driver; char *devpath, *rdevpath, *partname; char *childnode; char unit; struct volume_id *vid; uint64_t psize, msize; int i, fd; partname = devnode; unit = partname[strlen (partname) - 1] - 'a'; if (unit >= MAXPARTITIONS) return NULL; devpath = g_strdup_printf ("/dev/%s", partname); rdevpath = g_strdup_printf ("/dev/r%s", partname); fd = open (rdevpath, O_RDONLY); if (fd < 0) { HAL_WARNING (("couldn't open %s: %s", rdevpath, strerror (errno))); g_free (rdevpath); return NULL; } if (ioctl (fd, DIOCGDINFO, &label) == -1) { HAL_WARNING (("DIOCGDINFO failed on %s: %s", rdevpath, strerror (errno))); g_free (rdevpath); close (fd); return NULL; } part = &label.d_partitions[unit]; d = hal_device_new (); devinfo_set_default_properties (d, parent, devnode, devfs_path); hal_device_add_capability (d, "block"); hal_device_property_set_string (d, "info.subsystem", "block"); hal_device_property_set_string (d, "info.category", "volume"); hal_device_property_set_string (d, "block.device", devpath); hal_device_property_set_string (d, "block.storage_device", hal_device_property_get_string (parent, "info.udi")); if (stat (devpath, &st) == 0) { hal_device_property_set_int (d, "block.major", major (st.st_rdev)); hal_device_property_set_int (d, "block.minor", minor (st.st_rdev)); } hal_device_property_set_bool (d, "block.is_volume", TRUE); hal_device_property_set_bool (d, "block.no_partitions", FALSE); hal_device_property_set_bool (d, "block.have_scanned", TRUE); hal_device_add_capability (d, "volume"); hal_device_property_set_bool (d, "volume.ignore", FALSE); hal_device_property_set_bool (d, "volume.is_mounted", FALSE); hal_device_property_set_bool (d, "volume.is_mounted_read_only", FALSE); hal_device_property_set_string (d, "volume.mount_point", ""); hal_device_property_set_string (d, "volume.fsusage", "filesystem"); hal_device_property_set_string (d, "volume.fstype", devinfo_mass_get_fstype (part->p_fstype)); hal_device_property_set_bool (d, "volume.is_disc", FALSE); hal_device_property_set_string (d, "volume.label", ""); hal_device_property_set_string (d, "volume.partition.label", ""); hal_device_property_set_string (d, "volume.uuid", ""); hal_device_property_set_string (d, "volume.partition.uuid", ""); psize = (uint64_t)part->p_size * (uint64_t)label.d_secsize; msize = (uint64_t)label.d_secsize * (uint64_t)label.d_secperunit; hal_device_property_set_uint64 (d, "volume.size", psize); hal_device_property_set_int (d, "volume.block_size", label.d_secsize); hal_device_property_set_uint64 (d, "volume.num_blocks", part->p_size); hal_device_property_set_uint64 (d, "volume.partition.media_size", msize); hal_device_property_set_bool (d, "volume.is_partition", TRUE); hal_device_property_set_int (d, "volume.partition.number", unit); hal_device_property_set_string (d, "volume.partition.scheme", "mbr"); vid = volume_id_open_fd (fd); if (vid) { if (volume_id_probe_all (vid, 0, psize) == 0) { const char *type,*fstype; hal_device_property_set_string (d, "volume.label", vid->label); hal_device_property_set_string (d, "volume.partition.label", vid->label); hal_device_property_set_string (d, "volume.uuid", vid->uuid); hal_device_property_set_string (d, "volume.partition.uuid", vid->uuid); if ( type && volume_id_get_type (vid, &type)) { fstype=devinfo_mass_get_fstype (part->p_fstype); if (strcmp (type, fstype)) { HAL_INFO (("%s disklabel reports [%s] but libvolume_id says it is " "[%s], assuming disklabel is incorrect", devpath, fstype, type)); hal_device_property_set_string (d, "volume.fstype", type); } } } volume_id_close (vid); } devinfo_add_enqueue (d, devfs_path, &devinfo_mass_disklabel_handler); close (fd); g_free (rdevpath); g_free (devpath); return d; }
static void force_unmount (LibHalContext *ctx, const char *udi) { DBusError error; DBusMessage *msg = NULL; DBusMessage *reply = NULL; char **options = NULL; unsigned int num_options = 0; DBusConnection *dbus_connection; char *device_file; dbus_connection = libhal_ctx_get_dbus_connection (ctx); msg = dbus_message_new_method_call ("org.freedesktop.Hal", udi, "org.freedesktop.Hal.Device.Volume", "Unmount"); if (msg == NULL) { HAL_ERROR (("Could not create dbus message for %s", udi)); goto out; } options = calloc (1, sizeof (char *)); if (options == NULL) { HAL_ERROR (("Could not allocate options array")); goto out; } options[0] = "lazy"; num_options = 1; device_file = libhal_device_get_property_string (ctx, udi, "block.device", NULL); if (device_file != NULL) { HAL_INFO(("forcibly attempting to lazy unmount %s as media was removed", device_file)); libhal_free_string (device_file); } if (!dbus_message_append_args (msg, DBUS_TYPE_ARRAY, DBUS_TYPE_STRING, &options, num_options, DBUS_TYPE_INVALID)) { HAL_ERROR (("Could not append args to dbus message for %s", udi)); goto out; } dbus_error_init (&error); if (!(reply = dbus_connection_send_with_reply_and_block (dbus_connection, msg, -1, &error))) { HAL_ERROR (("Unmount failed for %s: %s : %s\n", udi, error.name, error.message)); dbus_error_free (&error); goto out; } if (dbus_error_is_set (&error)) { HAL_ERROR (("Unmount failed for %s\n%s : %s\n", udi, error.name, error.message)); dbus_error_free (&error); goto out; } HAL_DEBUG (("Succesfully unmounted udi '%s'", udi)); out: if (options != NULL) free (options); if (msg != NULL) dbus_message_unref (msg); if (reply != NULL) dbus_message_unref (reply); }
static gboolean validate_bus_name (const char *name) { int len; const char *s; const char *end; const char *last_dot; gboolean ret; s = name; len = strlen (name); end = name + len; last_dot = NULL; ret = FALSE; /* check special cases of first char so it doesn't have to be done * in the loop. Note we know len > 0 */ if (*s == ':') { /* unique name */ ++s; while (s != end) { if (*s == '.') { if (G_UNLIKELY ((s + 1) == end)) goto error; if (G_UNLIKELY (!VALID_BUS_NAME_CHARACTER (*(s + 1)))) goto error; ++s; /* we just validated the next char, so skip two */ } else if (G_UNLIKELY (!VALID_BUS_NAME_CHARACTER (*s))) { goto error; } ++s; } return TRUE; } else if (G_UNLIKELY (*s == '.')) /* disallow starting with a . */ { goto error; } else if (G_UNLIKELY (!VALID_INITIAL_BUS_NAME_CHARACTER (*s))) { goto error; } else { ++s; } while (s != end) { if (*s == '.') { if (G_UNLIKELY ((s + 1) == end)) goto error; else if (G_UNLIKELY (!VALID_INITIAL_BUS_NAME_CHARACTER (*(s + 1)))) goto error; last_dot = s; ++s; /* we just validated the next char, so skip two */ } else if (G_UNLIKELY (!VALID_BUS_NAME_CHARACTER (*s))) { goto error; } ++s; } if (G_UNLIKELY (last_dot == NULL)) goto error; ret = TRUE; error: if (!ret) HAL_INFO (("name '%s' did not validate", name)); return ret; }
gboolean hald_runner_start_runner(void) { DBusServer *server = NULL; DBusError err; GError *error = NULL; GPid pid; char *argv[] = { NULL, NULL}; char *env[] = { NULL, NULL, NULL, NULL}; const char *hald_runner_path; char *server_addr; running_processes = g_hash_table_new (g_direct_hash, g_direct_equal); dbus_error_init(&err); server = dbus_server_listen(DBUS_SERVER_ADDRESS, &err); if (server == NULL) { HAL_ERROR (("Cannot create D-BUS server for the runner")); goto error; } dbus_server_setup_with_g_main(server, NULL); dbus_server_set_new_connection_function(server, handle_connection, NULL, NULL); argv[0] = "hald-runner"; server_addr = dbus_server_get_address (server); env[0] = g_strdup_printf("HALD_RUNNER_DBUS_ADDRESS=%s", server_addr); dbus_free (server_addr); hald_runner_path = g_getenv("HALD_RUNNER_PATH"); if (hald_runner_path != NULL) { env[1] = g_strdup_printf ("PATH=%s:" PACKAGE_LIBEXEC_DIR ":" PACKAGE_SCRIPT_DIR ":" PACKAGE_BIN_DIR, hald_runner_path); } else { env[1] = g_strdup_printf ("PATH=" PACKAGE_LIBEXEC_DIR ":" PACKAGE_SCRIPT_DIR ":" PACKAGE_BIN_DIR); } /*env[2] = "DBUS_VERBOSE=1";*/ if (!g_spawn_async(NULL, argv, env, G_SPAWN_DO_NOT_REAP_CHILD|G_SPAWN_SEARCH_PATH, NULL, NULL, &pid, &error)) { HAL_ERROR (("Could not spawn runner : '%s'", error->message)); g_error_free (error); goto error; } g_free(env[0]); g_free(env[1]); HAL_INFO (("Runner has pid %d", pid)); g_child_watch_add(pid, runner_died, NULL); while (runner_connection == NULL) { /* Wait for the runner */ g_main_context_iteration(NULL, TRUE); } return TRUE; error: if (server != NULL) dbus_server_unref(server); return FALSE; }
bool RTIMUMPU9150::IMUInit() { unsigned char result; unsigned char asa[3]; m_firstTime = true; #ifdef MPU9150_CACHE_MODE m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.pressureValid = false; m_imuData.temperatureValid = false; m_imuData.humidityValid = false; // configure IMU m_slaveAddr = m_settings->m_I2CSlaveAddress; setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate); setCompassRate(m_settings->m_MPU9150CompassSampleRate); setLpf(m_settings->m_MPU9150GyroAccelLpf); setGyroFsr(m_settings->m_MPU9150GyroFsr); setAccelFsr(m_settings->m_MPU9150AccelFsr); setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // reset the MPU9150 if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset")) return false; m_settings->delayMs(100); if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id")) return false; if (result != MPU9150_ID) { HAL_ERROR1("Incorrect MPU9150 id %d\n", result); return false; } // now configure the various components if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf")) return false; if (!setSampleRate()) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr")) return false; // now configure compass bypassOn(); // get fuse ROM data if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) { bypassOff(); return false; } if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) { bypassOff(); return false; } if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "Failed to read compass fuse ROM")) { bypassOff(); return false; } // convert asa to usable scale factor m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f; m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f; m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f; if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) { bypassOff(); return false; } bypassOff(); // now set up MPU9150 to talk to the compass chip if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc")) return false; if (!setCompassRate()) return false; // enable the sensors if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2")) return false; // select the data to go into the FIFO and enable if (!resetFifo()) return false; gyroBiasInit(); HAL_INFO("MPU9150 init complete\n"); return true; }
bool RTIMULSM9DS1::IMUInit() { unsigned char result; // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.motion = true; m_imuData.IMUtemperatureValid = false; m_imuData.IMUtemperature = 0.0; m_imuData.humidityValid = false; m_imuData.humidity = -1.0; m_imuData.humidityTemperatureValid = false; m_imuData.humidityTemperature = 0.0; m_imuData.pressureValid = false; m_imuData.pressure = 0.0; m_imuData.pressureTemperatureValid = false; m_imuData.pressureTemperature = 0.0; // configure IMU m_accelGyroSlaveAddr = m_settings->m_I2CSlaveAddress; // work outmag address if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS0, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS0; } } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS1, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS1; } } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS2, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS2; } } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS3, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS3; } } setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro/accel if (!m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL8, 0x80, "Failed to boot LSM9DS1")) return false; m_settings->delayMs(100); if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/gyro id")) return false; if (result != LSM9DS1_ID) { HAL_ERROR1("Incorrect LSM9DS1 gyro id %d\n", result); return false; } if (!setGyroSampleRate()) return false; if (!setGyroCTRL3()) return false; // Set up the mag if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/mag id")) return false; if (result != LSM9DS1_MAG_ID) { HAL_ERROR1("Incorrect LSM9DS1 accel/mag id %d\n", result); return false; } if (!setAccelCTRL6()) return false; if (!setAccelCTRL7()) return false; if (!setCompassCTRL1()) return false; if (!setCompassCTRL2()) return false; if (!setCompassCTRL3()) return false; gyroBiasInit(); HAL_INFO("LSM9DS1 init complete\n"); return true; }
bool RTIMULSM9DS0::IMUInit() { unsigned char result; #ifdef LSM9DS0_CACHE_MODE m_firstTime = true; m_cacheIn = m_cacheOut = m_cacheCount = 0; #endif // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.pressureValid = false; m_imuData.temperatureValid = false; m_imuData.humidityValid = false; // configure IMU m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; // work out accelmag address if (m_settings->HALRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS0_ACCELMAG_ID) { m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0; } } else { m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1; } setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0")) return false; if (!m_settings->HALRead(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 gyro id")) return false; if (result != LSM9DS0_GYRO_ID) { HAL_ERROR1("Incorrect LSM9DS0 gyro id %d\n", result); return false; } if (!setGyroSampleRate()) return false; if (!setGyroCTRL2()) return false; if (!setGyroCTRL4()) return false; // Set up the accel if (!m_settings->HALRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id")) return false; if (result != LSM9DS0_ACCELMAG_ID) { HAL_ERROR1("Incorrect LSM9DS0 accel/mag id %d\n", result); return false; } if (!setAccelCTRL1()) return false; if (!setAccelCTRL2()) return false; if (!setCompassCTRL5()) return false; if (!setCompassCTRL6()) return false; if (!setCompassCTRL7()) return false; #ifdef LSM9DS0_CACHE_MODE // turn on gyro fifo if (!m_settings->HALWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode")) return false; #endif if (!setGyroCTRL5()) return false; gyroBiasInit(); HAL_INFO("LSM9DS0 init complete\n"); return true; }
bool RTIMUMPU9250::IMURead() { unsigned char fifoCount[2]; unsigned int count; unsigned char fifoData[12]; unsigned char compassData[8]; if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count")) return false; count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1]; if (count == 512) { HAL_INFO("MPU-9250 fifo has overflowed"); resetFifo(); m_imuData.timestamp += m_sampleInterval * (512 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp return false; } #ifdef MPU9250_CACHE_MODE if ((m_cacheCount == 0) && (count >= MPU9250_FIFO_CHUNK_SIZE) && (count < (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE))) { // special case of a small fifo and nothing cached - just handle as simple read if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data")) return false; } else { if (count >= (MPU9250_CACHE_SIZE * MPU9250_FIFO_CHUNK_SIZE)) { if (m_cacheCount == MPU9250_CACHE_BLOCK_COUNT) { // all cache blocks are full - discard oldest and update timestamp to account for lost samples m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count; if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT) m_cacheOut = 0; m_cacheCount--; } int blockCount = count / MPU9250_FIFO_CHUNK_SIZE; // number of chunks in fifo if (blockCount > MPU9250_CACHE_SIZE) blockCount = MPU9250_CACHE_SIZE; if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE * blockCount, m_cache[m_cacheIn].data, "Failed to read fifo data")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data")) return false; m_cache[m_cacheIn].count = blockCount; m_cache[m_cacheIn].index = 0; m_cacheCount++; if (++m_cacheIn == MPU9250_CACHE_BLOCK_COUNT) m_cacheIn = 0; } // now fifo has been read if necessary, get something to process if (m_cacheCount == 0) return false; memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9250_FIFO_CHUNK_SIZE); memcpy(compassData, m_cache[m_cacheOut].compass, 8); m_cache[m_cacheOut].index += MPU9250_FIFO_CHUNK_SIZE; if (--m_cache[m_cacheOut].count == 0) { // this cache block is now empty if (++m_cacheOut == MPU9250_CACHE_BLOCK_COUNT) m_cacheOut = 0; m_cacheCount--; } } #else if (count > MPU9250_FIFO_CHUNK_SIZE * 40) { // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) { if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) return false; count -= MPU9250_FIFO_CHUNK_SIZE; m_imuData.timestamp += m_sampleInterval; } } if (count < MPU9250_FIFO_CHUNK_SIZE) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data")) return false; #endif RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true); RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true); RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false); // sort out gyro axes m_imuData.gyro.setX(m_imuData.gyro.x()); m_imuData.gyro.setY(-m_imuData.gyro.y()); m_imuData.gyro.setZ(-m_imuData.gyro.z()); // sort out accel data; m_imuData.accel.setX(-m_imuData.accel.x()); // use the compass fuse data adjustments m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]); m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]); m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]); // sort out compass axes float temp; temp = m_imuData.compass.x(); m_imuData.compass.setX(m_imuData.compass.y()); m_imuData.compass.setY(-temp); // now do standard processing handleGyroBias(); calibrateAverageCompass(); calibrateAccel(); if (m_firstTime) m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); else m_imuData.timestamp += m_sampleInterval; m_firstTime = false; // now update the filter updateFusion(); return true; }
void RTFusionAHRS::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings) { if (m_debug) { HAL_INFO("\n------\n"); HAL_INFO1("IMU update delta time: %f\n", m_timeDelta); } m_gyro = data.gyro; m_accel = data.accel; m_compass = data.compass; m_compassValid = data.compassValid; if (m_firstTime) { // adjust for compass declination, compute the correction data only at beginning m_sin_theta_half = sin(-settings->m_compassAdjDeclination/2.0f); m_cos_theta_half = cos(-settings->m_compassAdjDeclination/2.0f); m_lastFusionTime = data.timestamp; calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination); // initialize the poses m_stateQ.fromEuler(m_measuredPose); m_fusionQPose = m_stateQ; m_fusionPose = m_measuredPose; m_firstTime = false; } else { // not first time m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000; m_lastFusionTime = data.timestamp; if (m_timeDelta <= 0) return; calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination); // ================================================= // AHRS // // Previous Q Pose; short name local variables for readability float q1 = m_stateQ.scalar(); float q2 = m_stateQ.x(); float q3 = m_stateQ.y(); float q4 = m_stateQ.z(); float ax, ay, az, mx, my, mz, gx, gy, gz; // accelerometer, magnetometer, gyroscope float gerrx, gerry, gerrz; // gyro bias error float norm; float hx, hy, _2bx, _2bz; float s1, s2, s3, s4; float qDot1, qDot2, qDot3, qDot4; float _2q1mx,_2q1my, _2q1mz,_2q2mx; float _4bx, _4bz; float _2q1, _2q2, _2q3, _2q4; float q1q1, q1q2, q1q3, q1q4, q2q2, q2q3, q2q4, q3q4, q3q3, q4q4; float _4q1, _4q2, _4q3, _8q2, _8q3; float _2q3q4, _2q1q3; if (m_enableCompass) { mx = m_compass.x(); my = m_compass.y(); mz = m_compass.z(); } else { mx = 0.0f; my = 0.0f; mz = 0.0f; } if (m_enableAccel) { ax = m_accel.x(); ay = m_accel.y(); az = m_accel.z(); } else { ax = 0.0f; ay = 0.0f; az = 0.0f; } if (m_enableGyro) { gx=m_gyro.x(); gy=m_gyro.y(); gz=m_gyro.z(); } else { return; } // We need to have valid gyroscope data //////////////////////////////////////////////////////////////////////////// // Regular Algorithm // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz); // s qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy); // x qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx); // y qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx); // z if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Use this algorithm if accelerometer is valid // If accelerometer is not valid, update pose based on previous qDot // Normalise accelerometer measurement norm = sqrt(ax * ax + ay * ay + az * az); if (norm == 0.0) return; // handle NaN ax /= norm; ay /= norm; az /= norm; // Auxiliary variables to avoid repeated arithmetic _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3; _2q4 = 2.0f * q4; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; q4q4 = q4 * q4; if(((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { // If magnetometer is invalid // Auxiliary variables to avoid repeated arithmetic _4q1 = 4.0f * q1; _4q2 = 4.0f * q2; _4q3 = 4.0f * q3; _8q2 = 8.0f * q2; _8q3 = 8.0f * q3; // Gradient decent algorithm corrective step s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay; s2 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az; s3 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az; s4 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay; } else { // Valid magnetometer available use this code // Normalise magnetometer measurement norm = sqrt(mx * mx + my * my + mz * mz); if (norm == 0.0) return; // handle NaN mx /= norm; my /= norm; mz /= norm; // Auxiliary variables to avoid repeated arithmetic q1q2 = q1 * q2; q1q3 = q1 * q3; q1q4 = q1 * q4; q2q3 = q2 * q3; q2q4 = q2 * q4; q3q4 = q3 * q4; _2q1q3 = 2.0f * q1q3; _2q3q4 = 2.0f * q3q4; _2q1mx = 2.0f * q1 * mx; _2q1my = 2.0f * q1 * my; _2q1mz = 2.0f * q1 * mz; _2q2mx = 2.0f * q2 * mx; // Reference direction of Earth's magnetic field hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; _2bx = sqrt(hx * hx + hy * hy); _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; // Gradient decent algorithm corrective step s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); } // end valid magnetometer norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude if (norm == 0.0) return; // handle NaN s1 /= norm; s2 /= norm; s3 /= norm; s4 /= norm; // Compute estimated gyroscope biases gerrx = _2q1 * s2 - _2q2 * s1 - _2q3 * s4 + _2q4 * s3; gerry = _2q1 * s3 + _2q2 * s4 - _2q3 * s1 - _2q4 * s2; gerrz = _2q1 * s4 - _2q2 * s3 + _2q3 * s2 - _2q4 * s1; // Compute and remove gyroscope biases m_gbiasx += gerrx * m_timeDelta * m_zeta; m_gbiasy += gerry * m_timeDelta * m_zeta; m_gbiasz += gerrz * m_timeDelta * m_zeta; gx -= m_gbiasx; gy -= m_gbiasy; gz -= m_gbiasz; // Apply feedback step qDot1 -= m_beta * s1; qDot2 -= m_beta * s2; qDot3 -= m_beta * s3; qDot4 -= m_beta * s4; } // end if valid accelerometer // Integrate to yield quaternion q1 += qDot1 * m_timeDelta; q2 += qDot2 * m_timeDelta; q3 += qDot3 * m_timeDelta; q4 += qDot4 * m_timeDelta; // normalise quaternion norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); if (norm == 0.0) return; // handle NaN q1 /= norm; q2 /= norm; q3 /= norm; q4 /= norm; m_stateQ.setScalar(q1); m_stateQ.setX(q2); m_stateQ.setY(q3); m_stateQ.setZ(q4); // Rotate Quaternion by Magnetic Declination // m_stateQ = q_declination * m_statqQ; /* SAGE N.<c,d,q1,q2,q3,q4,cos_theta_half, sin_theta_half> = QQ[] H.<i,j,k> = QuaternionAlgebra(c,d) q = q1 + q2 * i + q3 * j + q4 * k // here rotation is around gravity vector by theta mag_declination = cos_theta_half + sin_theta_half * (0 * i + 0 * j+ 1*k) q * mag_declination s : -q4*sin_theta_half + q1*cos_theta_half x : q3*sin_theta_half + q2*cos_theta_half y : -q2*sin_theta_half + q3*cos_theta_half z : q4*cos_theta_half + q1*sin_theta_half */ m_stateQdec.setScalar(q1*m_cos_theta_half - q4*m_sin_theta_half); m_stateQdec.setX(q3*m_sin_theta_half + q2*m_cos_theta_half); m_stateQdec.setY(q3*m_cos_theta_half - q2*m_sin_theta_half); m_stateQdec.setZ(q4*m_cos_theta_half + q1*m_sin_theta_half); } // end not first time // ================================================= if (m_enableCompass || m_enableAccel) { m_stateQError = m_measuredQPose - m_stateQ; } else { m_stateQError = RTQuaternion(); } m_stateQdec.toEuler(m_fusionPose); m_fusionQPose = m_stateQdec; if (m_debug | settings->m_fusionDebug) { HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose)); HAL_INFO(RTMath::displayRadians("AHRS pose", m_fusionPose)); HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose)); HAL_INFO(RTMath::display("AHRS quat", m_stateQ)); HAL_INFO(RTMath::display("Error quat", m_stateQError)); HAL_INFO3("AHRS Gyro Bias: %+f, %+f, %+f\n", m_gbiasx, m_gbiasy, m_gbiasz); } data.fusionPoseValid = true; data.fusionQPoseValid = true; data.fusionPose = m_fusionPose; data.fusionQPose = m_fusionQPose; } //
bool RTIMUSettings::discoverIMU(int& imuType, unsigned char& slaveAddress) { unsigned char result; unsigned char altResult; setI2CBus(m_I2CBus); if (!I2COpen()) { HAL_ERROR1("Failed to open I2C bus %d\n", m_I2CBus); return false; } if (I2CRead(MPU9150_ADDRESS0, MPU9150_WHO_AM_I, 1, &result, "")) { if (result == MPU9150_ID) { imuType = RTIMU_TYPE_MPU9150; slaveAddress = MPU9150_ADDRESS0; I2CClose(); HAL_INFO("Detected MPU9150 at standard address\n"); return true; } } if (I2CRead(MPU9150_ADDRESS1, MPU9150_WHO_AM_I, 1, &result, "")) { if (result == MPU9150_ID) { imuType = RTIMU_TYPE_MPU9150; slaveAddress = MPU9150_ADDRESS1; I2CClose(); HAL_INFO("Detected MPU9150 at option address\n"); return true; } } if (I2CRead(L3GD20H_ADDRESS0, L3GD20H_WHO_AM_I, 1, &result, "")) { if (result == L3GD20H_ID) { imuType = RTIMU_TYPE_GD20HM303D; slaveAddress = L3GD20H_ADDRESS0; I2CClose(); HAL_INFO("Detected L3GD20H at standard address\n"); return true; } else if (result == LSM9DS0_GYRO_ID) { if (I2CRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &altResult, "")) { if (altResult == LSM9DS0_ACCELMAG_ID) { imuType = RTIMU_TYPE_LSM9DS0; slaveAddress = LSM9DS0_GYRO_ADDRESS0; I2CClose(); HAL_INFO("Detected LSM9DS0 at standard address\n"); return true; } } } } if (I2CRead(L3GD20H_ADDRESS1, L3GD20H_WHO_AM_I, 1, &result, "")) { if (result == L3GD20H_ID) { imuType = RTIMU_TYPE_GD20HM303D; slaveAddress = L3GD20H_ADDRESS1; I2CClose(); HAL_INFO("Detected L3GD20H at option address\n"); return true; } else if (result == LSM9DS0_GYRO_ID) { if (I2CRead(LSM9DS0_ACCELMAG_ADDRESS1, LSM9DS0_WHO_AM_I, 1, &altResult, "")) { if (altResult == LSM9DS0_ACCELMAG_ID) { imuType = RTIMU_TYPE_LSM9DS0; slaveAddress = LSM9DS0_GYRO_ADDRESS1; I2CClose(); HAL_INFO("Detected LSM9DS0 at standard address\n"); return true; } } } } if (I2CRead(L3GD20_ADDRESS0, L3GD20_WHO_AM_I, 1, &result, "")) { if (result == L3GD20_ID) { imuType = RTIMU_TYPE_GD20M303DLHC; slaveAddress = L3GD20_ADDRESS0; I2CClose(); HAL_INFO("Detected L3GD20 at standard address\n"); return true; } } if (I2CRead(L3GD20_ADDRESS1, L3GD20_WHO_AM_I, 1, &result, "")) { if (result == L3GD20_ID) { imuType = RTIMU_TYPE_GD20M303DLHC; slaveAddress = L3GD20_ADDRESS1; I2CClose(); HAL_INFO("Detected L3GD20 at option address\n"); return true; } } I2CClose(); HAL_ERROR("No IMU detected\n"); return false; }
bool RTIMUBMX055::IMUInit() { unsigned char result; m_firstTime = true; // set validity flags m_imuData.fusionPoseValid = false; m_imuData.fusionQPoseValid = false; m_imuData.gyroValid = true; m_imuData.accelValid = true; m_imuData.compassValid = true; m_imuData.motion = true; m_imuData.IMUtemperatureValid = false; m_imuData.IMUtemperature = 0.0; m_imuData.humidityValid = false; m_imuData.humidity = -1.0; m_imuData.humidityTemperatureValid = false; m_imuData.humidityTemperature = 0.0; m_imuData.pressureValid = false; m_imuData.pressure = 0.0; m_imuData.pressureTemperatureValid = false; m_imuData.pressureTemperature = 0.0; m_imuData.tTemperatureValid = false; m_imuData.tTemperature = 0.0; // configure IMU m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_WHO_AM_I, 1, &result, "Failed to read BMX055 gyro id")) return false; if (result != BMX055_GYRO_ID) { HAL_ERROR1("Incorrect BMX055 id %d\n", result); return false; } // work out accel address if (m_settings->HALRead(BMX055_ACCEL_ADDRESS0, BMX055_ACCEL_WHO_AM_I, 1, &result, "")) { if (result == BMX055_ACCEL_ID) { m_accelSlaveAddr = BMX055_ACCEL_ADDRESS0; } else { m_accelSlaveAddr = BMX055_ACCEL_ADDRESS1; } } // work out mag address int magAddr; for (magAddr = BMX055_MAG_ADDRESS0; magAddr <= BMX055_MAG_ADDRESS3; magAddr++) { // have to enable chip to get id... m_settings->HALWrite(magAddr, BMX055_MAG_POWER, 1, ""); m_settings->delayMs(50); if (m_settings->HALRead(magAddr, BMX055_MAG_WHO_AM_I, 1, &result, "")) { if (result == BMX055_MAG_ID) { m_magSlaveAddr = magAddr; break; } } } if (magAddr > BMX055_MAG_ADDRESS3) { HAL_ERROR("Failed to find BMX055 mag\n"); return false; } setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config")) return false; if (!setGyroSampleRate()) return false; if (!setGyroFSR()) return false; gyroBiasInit(); // set up the accel if (!setAccelSampleRate()) return false; if (!setAccelFSR()) return false; // set up the mag magInitTrimRegisters(); setMagPreset(); HAL_INFO("BMX055 init complete\n"); return true; }