public static string magfit(string logfile) { //'''find best magnetometer rotation fit to a log file''' // print("Processing log %s" % filename); // mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps); ArdupilotMega.MAVLink mavint = new ArdupilotMega.MAVLink(); try { mavint.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { return(""); } mavint.logreadmode = true; 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 (mavint.logplaybackfile.BaseStream.Position < mavint.logplaybackfile.BaseStream.Length) { byte[] packetbytes = mavint.readPacket(); if (packetbytes.Length < 5) { continue; } object packet = mavint.GetPacket(packetbytes); if (packet == null) { continue; } if (packet is ArdupilotMega.MAVLink.mavlink_param_value_t) { ArdupilotMega.MAVLink.mavlink_param_value_t m = (ArdupilotMega.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 ArdupilotMega.MAVLink.mavlink_raw_imu_t) { ArdupilotMega.MAVLink.mavlink_raw_imu_t m = (ArdupilotMega.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 string magfit(string logfile) { //'''find best magnetometer rotation fit to a log file''' // print("Processing log %s" % filename); // mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps); ArdupilotMega.MAVLink mavint = new ArdupilotMega.MAVLink(); try { mavint.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); } catch (Exception ex) { return ""; } mavint.logreadmode = true; 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 (mavint.logplaybackfile.BaseStream.Position < mavint.logplaybackfile.BaseStream.Length) { byte[] packetbytes = mavint.readPacket(); if (packetbytes.Length < 5) continue; object packet = mavint.GetPacket(packetbytes); if (packet == null) continue; if (packet is ArdupilotMega.MAVLink.mavlink_param_value_t) { ArdupilotMega.MAVLink.mavlink_param_value_t m = (ArdupilotMega.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 ArdupilotMega.MAVLink.mavlink_raw_imu_t) { ArdupilotMega.MAVLink.mavlink_raw_imu_t m = (ArdupilotMega.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; }