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