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