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