public RawImu(MavLinkMessage message) : base(null)
{
if (message.messid == this.MessageID)
{
MAVLink.mavlink_raw_imu_t raw_data = (MAVLink.mavlink_raw_imu_t)message.data_struct;
this.xacc = raw_data.xacc;
this.yacc = raw_data.yacc;
this.zacc = raw_data.zacc;
this.xgyro = raw_data.xgyro;
this.ygyro = raw_data.ygyro;
this.zgyro = raw_data.zgyro;
this.xmag = raw_data.xmag;
this.ymag = raw_data.ymag;
this.zmag = raw_data.zmag;
}
}
public void CheckRawImuObject()
{
MAVLink.mavlink_raw_imu_t data = new MAVLink.mavlink_raw_imu_t();
data.xacc = 1;
data.xgyro = 2;
data.xmag = 3;
data.yacc = 4;
data.ygyro = 5;
data.ymag = 6;
data.zacc = 7;
data.zgyro = 8;
data.zmag = 9;
MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.RAW_IMU, data);
RawImu obj = new RawImu(message);
Assert.AreEqual(data.xacc, obj.xacc);
Assert.AreEqual(data.yacc, obj.yacc);
Assert.AreEqual(data.zacc, obj.zacc);
Assert.AreEqual(data.xgyro, obj.xgyro);
Assert.AreEqual(data.ygyro, obj.ygyro);
Assert.AreEqual(data.zgyro, obj.zgyro);
Assert.AreEqual(data.xmag, obj.xmag);
Assert.AreEqual(data.ymag, obj.ymag);
Assert.AreEqual(data.zmag, obj.zmag);
RawImuDTO dto = DTOFactory.createRawImuDTO(obj);
Assert.AreEqual(dto.xacc, obj.xacc);
Assert.AreEqual(dto.yacc, obj.yacc);
Assert.AreEqual(dto.zacc, obj.zacc);
Assert.AreEqual(dto.xgyro, obj.xgyro);
Assert.AreEqual(dto.ygyro, obj.ygyro);
Assert.AreEqual(dto.zgyro, obj.zgyro);
Assert.AreEqual(dto.xmag, obj.xmag);
Assert.AreEqual(dto.ymag, obj.ymag);
Assert.AreEqual(dto.zmag, obj.zmag);
}
static bool ReceviedPacket(byte[] rawpacket)
{
MAVLink.mavlink_raw_imu_t packet = rawpacket.ByteArrayToStructure <MAVLink.mavlink_raw_imu_t>();
// filter dataset
string item = (int)(packet.xmag / div) + "," +
(int)(packet.ymag / div) + "," +
(int)(packet.zmag / div);
if (filter.ContainsKey(item))
{
filter[item] = (int)filter[item] + 1;
if ((int)filter[item] > 3)
{
return(false);
}
}
else
{
filter[item] = 1;
}
// values
float rawmx = packet.xmag - (float)MainV2.comPort.MAV.cs.mag_ofs_x;
float rawmy = packet.ymag - (float)MainV2.comPort.MAV.cs.mag_ofs_y;
float rawmz = packet.zmag - (float)MainV2.comPort.MAV.cs.mag_ofs_z;
// add data
lock (locker)
{
data.Add(new Tuple <float, float, float>(rawmx, rawmy, rawmz));
}
return(true);
}
static bool ReceviedPacket(byte[] rawpacket)
{
if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.SCALED_IMU2)
{
MAVLink.mavlink_scaled_imu2_t packet = rawpacket.ByteArrayToStructure <MAVLink.mavlink_scaled_imu2_t>();
// filter dataset
string item = (int)(packet.xmag / div) + "," +
(int)(packet.ymag / div) + "," +
(int)(packet.zmag / div);
if (filtercompass2.ContainsKey(item))
{
filtercompass2[item] = (int)filtercompass2[item] + 1;
if ((int)filtercompass2[item] > 3)
{
return(false);
}
}
else
{
filtercompass2[item] = 1;
}
// values - offsets are 0
float rawmx = packet.xmag;
float rawmy = packet.ymag;
float rawmz = packet.zmag;
// add data
lock (datacompass2)
{
datacompass2.Add(new Tuple <float, float, float>(rawmx, rawmy, rawmz));
}
return(true);
}
else if (rawpacket[5] == (byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU)
{
MAVLink.mavlink_raw_imu_t packet = rawpacket.ByteArrayToStructure <MAVLink.mavlink_raw_imu_t>();
// filter dataset
string item = (int)(packet.xmag / div) + "," +
(int)(packet.ymag / div) + "," +
(int)(packet.zmag / div);
if (filtercompass1.ContainsKey(item))
{
filtercompass1[item] = (int)filtercompass1[item] + 1;
if ((int)filtercompass1[item] > 3)
{
return(false);
}
}
else
{
filtercompass1[item] = 1;
}
// values
float rawmx = packet.xmag - (float)MainV2.comPort.MAV.cs.mag_ofs_x;
float rawmy = packet.ymag - (float)MainV2.comPort.MAV.cs.mag_ofs_y;
float rawmz = packet.zmag - (float)MainV2.comPort.MAV.cs.mag_ofs_z;
// add data
lock (datacompass1)
{
datacompass1.Add(new Tuple <float, float, float>(rawmx, rawmy, rawmz));
}
return(true);
}
return(true);
}
static string process(MAVLinkInterface mavint)
{
DateTime Deadline = DateTime.Now.AddSeconds(60);
Vector3 last_mag = null;
double last_usec = 0;
double count = 0;
double[] total_error = new double[rotations.Length];
float AHRS_ORIENTATION = 0;
float COMPASS_ORIENT = 0;
float COMPASS_EXTERNAL = 0;
//# now gather all the data
while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
{
MAVLink.MAVLinkMessage packetbytes = mavint.readPacket();
if (packetbytes.Length < 5)
{
continue;
}
object packet = packetbytes.data;
if (packet == null)
{
continue;
}
if (packet is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t)packet;
if (str(m.param_id) == "AHRS_ORIENTATION")
{
AHRS_ORIENTATION = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_ORIENT")
{
COMPASS_ORIENT = (int)(m.param_value);
}
if (str(m.param_id) == "COMPASS_EXTERNAL")
{
COMPASS_EXTERNAL = (int)(m.param_value);
}
}
if (packet is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t)packet;
Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro) * 0.001;
double usec = m.time_usec;
if (last_mag != null && gyr.length() > radians(5.0))
{
add_errors(mag, gyr, last_mag, (usec - last_usec) * 1.0e-6, total_error, rotations);
count += 1;
}
last_mag = mag;
last_usec = usec;
}
}
int best_i = 0;
double best_err = total_error[0];
foreach (var i in range(len(rotations)))
{
Rotation r = rotations[i];
// if (!r.is_90_degrees())
// continue;
//if ( opts.verbose) {
// print("%s err=%.2f" % (r, total_error[i]/count));}
if (total_error[i] < best_err)
{
best_i = i;
best_err = total_error[i];
}
}
Rotation rans = rotations[best_i];
Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err / count, count);
//print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
return(rans.name);
}
public static void ScanAccel()
{
string[] files = Directory.GetFiles(MainV2.LogDir, "*.tlog", SearchOption.AllDirectories);
List <string> badfiles = new List <string>();
foreach (var file in files)
{
bool board = false;
Console.WriteLine(file);
using (MAVLinkInterface mavi = new MAVLinkInterface())
using (mavi.logplaybackfile = new BinaryReader(File.OpenRead(file)))
{
mavi.logreadmode = true;
try
{
while (mavi.logplaybackfile.BaseStream.Position < mavi.logplaybackfile.BaseStream.Length)
{
byte[] packet = mavi.readPacket();
if (packet.Length == 0)
{
break;
}
var objectds = mavi.DebugPacket(packet, false);
if (objectds is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t param = (MAVLink.mavlink_param_value_t)objectds;
if (ASCIIEncoding.ASCII.GetString(param.param_id).Contains("INS_PRODUCT_ID"))
{
if (param.param_value == 0 || param.param_value == 5)
{
board = true;
}
else
{
break;
}
}
}
if (objectds is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t rawimu = (MAVLink.mavlink_raw_imu_t)objectds;
if (board && Math.Abs(rawimu.xacc) > 2000 && Math.Abs(rawimu.yacc) > 2000 &&
Math.Abs(rawimu.zacc) > 2000)
{
//CustomMessageBox.Show("Bad Log " + file);
badfiles.Add(file);
break;
}
}
}
}
catch
{
}
}
}
if (badfiles.Count > 0)
{
FileStream fs = File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + "SearchResults.zip",
FileMode.Create);
ZipOutputStream zipStream = new ZipOutputStream(fs);
zipStream.SetLevel(9); //0-9, 9 being the highest level of compression
zipStream.UseZip64 = UseZip64.Off; // older zipfile
foreach (var file in badfiles)
{
// entry 2
string entryName = ZipEntry.CleanName(Path.GetFileName(file));
// Removes drive from name and fixes slash direction
ZipEntry newEntry = new ZipEntry(entryName);
newEntry.DateTime = DateTime.Now;
zipStream.PutNextEntry(newEntry);
// Zip the file in buffered chunks
// the "using" will close the stream even if an exception occurs
byte[] buffer = new byte[4096];
using (FileStream streamReader = File.OpenRead(file))
{
StreamUtils.Copy(streamReader, zipStream, buffer);
}
zipStream.CloseEntry();
}
zipStream.IsStreamOwner = true; // Makes the Close also Close the underlying stream
zipStream.Close();
CustomMessageBox.Show("Added " + badfiles.Count + " logs to " + MainV2.LogDir +
Path.DirectorySeparatorChar +
"SearchResults.zip\n Please send this file to Craig Elder <*****@*****.**>");
}
else
{
CustomMessageBox.Show("No Bad Logs Found");
}
}
