public void CopyFrom(DebugPacket _packet)
{
module_count = _packet.module_count;
for (int i = 0; i < module_count; i++)
{
DebugModule cur_module = null;
int cur_module_serial = _packet.modules[i].serial;
foreach (DebugModule module in modules)
{
if (module.serial == cur_module_serial)
{
cur_module = module;
break;
}
}
if (cur_module == null)
{
cur_module = new DebugModule
{
serial = cur_module_serial
};
modules.Add(cur_module);
}
cur_module.sensors_active = _packet.modules[i].sensors_active;
for (int s = 0; s < 6; s++)
{
DebugSensor cur_sensor = null;
int cur_sensor_id = _packet.modules[i].sensors[s].id;
foreach (var sensor in cur_module.sensors)
{
if (sensor.id == cur_sensor_id)
{
cur_sensor = sensor;
break;
}
}
if (cur_sensor == null)
{
cur_sensor = new DebugSensor
{
id = cur_sensor_id
};
cur_module.sensors.Add(cur_sensor);
}
cur_sensor.active = _packet.modules[i].sensors[s].active;
if (cur_sensor.active)
{
cur_sensor.acc_x = _packet.modules[i].sensors[s].acc_x;
cur_sensor.acc_y = _packet.modules[i].sensors[s].acc_y;
cur_sensor.acc_z = _packet.modules[i].sensors[s].acc_z;
cur_sensor.gyro_x = _packet.modules[i].sensors[s].gyro_x;
cur_sensor.gyro_y = _packet.modules[i].sensors[s].gyro_y;
cur_sensor.gyro_z = _packet.modules[i].sensors[s].gyro_z;
cur_sensor.mag_x = _packet.modules[i].sensors[s].mag_x;
cur_sensor.mag_y = _packet.modules[i].sensors[s].mag_y;
cur_sensor.mag_z = _packet.modules[i].sensors[s].mag_z;
cur_sensor.mag_terra_x = _packet.modules[i].sensors[s].mag_terra_x;
cur_sensor.mag_terra_y = _packet.modules[i].sensors[s].mag_terra_y;
cur_sensor.mag_terra_z = _packet.modules[i].sensors[s].mag_terra_z;
cur_sensor.mag_coil_x = _packet.modules[i].sensors[s].mag_coil_x;
cur_sensor.mag_coil_y = _packet.modules[i].sensors[s].mag_coil_y;
cur_sensor.mag_coil_z = _packet.modules[i].sensors[s].mag_coil_z;
}
}
}
initialized = true;
}
public void ParseUpdate(BinaryReader br)
{
module_count = br.ReadByte();
for (int i = 0; i < module_count; i++)
{
DebugModule cur_module = null;
int cur_module_serial = br.ReadUInt16();
foreach (DebugModule module in modules)
{
if (module.serial == cur_module_serial)
{
cur_module = module;
break;
}
}
if (cur_module == null)
{
cur_module = new DebugModule
{
serial = cur_module_serial
};
modules.Add(cur_module);
}
cur_module.sensors_active = br.ReadByte();
foreach (var sensor in cur_module.sensors)
{
sensor.active = false;
}
for (int s = 0; s < 6; s++)
{
DebugSensor cur_sensor = null;
int cur_sensor_id = cur_module.serial * 16 + s;
foreach (var sensor in cur_module.sensors)
{
if (sensor.id == cur_sensor_id)
{
cur_sensor = sensor;
break;
}
}
if (cur_sensor == null)
{
cur_sensor = new DebugSensor
{
id = cur_sensor_id
};
cur_module.sensors.Add(cur_sensor);
}
cur_sensor.active = (cur_module.sensors_active & (1 << s)) > 0;
if (cur_sensor.active)
{
cur_sensor.acc_x = br.ReadSingle();
cur_sensor.acc_y = br.ReadSingle();
cur_sensor.acc_z = br.ReadSingle();
cur_sensor.gyro_x = br.ReadSingle();
cur_sensor.gyro_y = br.ReadSingle();
cur_sensor.gyro_z = br.ReadSingle();
cur_sensor.mag_x = br.ReadSingle();
cur_sensor.mag_y = br.ReadSingle();
cur_sensor.mag_z = br.ReadSingle();
cur_sensor.mag_terra_x = br.ReadSingle();
cur_sensor.mag_terra_y = br.ReadSingle();
cur_sensor.mag_terra_z = br.ReadSingle();
cur_sensor.mag_coil_x = br.ReadSingle();
cur_sensor.mag_coil_y = br.ReadSingle();
cur_sensor.mag_coil_z = br.ReadSingle();
}
}
}
initialized = true;
}
