void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // list of x,y,z 's List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >(); // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz DateTime deadline = DateTime.Now.AddSeconds(60); float oldmx = 0; float oldmy = 0; float oldmz = 0; while (deadline > DateTime.Now) { double timeremaining = (deadline - DateTime.Now).TotalSeconds; ((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds - got " + data.Count + " Samples"); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = true; return; } if (oldmx != MainV2.comPort.MAV.cs.mx && oldmy != MainV2.comPort.MAV.cs.my && oldmz != MainV2.comPort.MAV.cs.mz) { data.Add(new Tuple <float, float, float>( MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z)); oldmx = MainV2.comPort.MAV.cs.mx; oldmy = MainV2.comPort.MAV.cs.my; oldmz = MainV2.comPort.MAV.cs.mz; } } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (data.Count < 10) { e.ErrorMessage = "Log does not contain enough data"; ans = null; return; } ans = MagCalib.LeastSq(data); }
private void pdr_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { var fw = new Firmware(); fw.Progress -= fw_Progress1; fw.Progress += fw_Progress; softwares = fw.getFWList(firmwareurl); foreach (var soft in softwares) { updateDisplayNameInvoke(soft); } }
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // turn learning off MainV2.comPort.setParam("COMPASS_LEARN", 0); bool havecompass2 = false; //compass2 get mag2 offsets if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X")) { //com2ofsx = MainV2.comPort.GetParam("COMPASS_OFS2_X"); //com2ofsy = MainV2.comPort.GetParam("COMPASS_OFS2_Y"); //com2ofsz = MainV2.comPort.GetParam("COMPASS_OFS2_Z"); MainV2.comPort.setParam("COMPASS_OFS2_X", 0); MainV2.comPort.setParam("COMPASS_OFS2_Y", 0); MainV2.comPort.setParam("COMPASS_OFS2_Z", 0); havecompass2 = true; } // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket); var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket); string extramsg = ""; ((ProgressReporterSphere)sender).sphere1.Clear(); ((ProgressReporterSphere)sender).sphere2.Clear(); int lastcount = 0; DateTime lastlsq = DateTime.MinValue; DateTime lastlsq2 = DateTime.MinValue; HIL.Vector3 centre = new HIL.Vector3(); while (true) { // slow down execution System.Threading.Thread.Sleep(20); ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + datacompass1.Count + " Samples " + extramsg); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = false; e.CancelRequested = false; break; } if (datacompass1.Count == 0) { continue; } // dont use dup data if (lastcount == datacompass1.Count) { continue; } lastcount = datacompass1.Count; float rawmx = datacompass1[datacompass1.Count - 1].Item1; float rawmy = datacompass1[datacompass1.Count - 1].Item2; float rawmz = datacompass1[datacompass1.Count - 1].Item3; // for old method setMinorMax(rawmx, ref minx, ref maxx); setMinorMax(rawmy, ref miny, ref maxy); setMinorMax(rawmz, ref minz, ref maxz); // get the current estimated centerpoint //new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2)); // run lsq every second when more than 100 datapoints if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second) { lastlsq = DateTime.Now; lock (datacompass1) { var lsq = MagCalib.LeastSq(datacompass1, false); // simple validation if (Math.Abs(lsq[0]) < 999) { centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]); log.Info("new centre " + centre.ToString()); ((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3((float)centre.x, (float)centre.y, (float)centre.z); } } } // run lsq every second when more than 100 datapoints if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second) { lastlsq2 = DateTime.Now; lock (datacompass2) { var lsq = MagCalib.LeastSq(datacompass2, false); // simple validation if (Math.Abs(lsq[0]) < 999) { HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]); log.Info("new centre2 " + centre2.ToString()); ((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3((float)centre2.x, (float)centre2.y, (float)centre2.z); } } } // add to sphere with center correction ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz)); ((ProgressReporterSphere)sender).sphere1.AimClear(); if (havecompass2 && datacompass2.Count > 0) { float raw2mx = datacompass2[datacompass2.Count - 1].Item1; float raw2my = datacompass2[datacompass2.Count - 1].Item2; float raw2mz = datacompass2[datacompass2.Count - 1].Item3; ((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz)); ((ProgressReporterSphere)sender).sphere2.AimClear(); } HIL.Vector3 point; point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre; //find the mean radius float radius = 0; for (int i = 0; i < datacompass1.Count; i++) { point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3); radius += (float)(point + centre).length(); } radius /= datacompass1.Count; //test that we can find one point near a set of points all around the sphere surface string displayresult = ""; int factor = 3; // 4 point check 16 points float max_distance = radius / 3; //pretty generouse for (int j = 0; j <= factor; j++) { double theta = (Math.PI * (j + 0)) / factor; for (int i = 0; i <= factor; i++) { double phi = (2 * Math.PI * i) / factor; HIL.Vector3 point_sphere = new HIL.Vector3( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) - centre; //log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance); bool found = false; for (int k = 0; k < datacompass1.Count; k++) { point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3); double d = (point_sphere - point).length(); if (d < max_distance) { found = true; break; } } // draw them all //((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z)); if (!found) { displayresult = "more data needed " + (theta * rad2deg).ToString("0") + " " + (phi * rad2deg).ToString("0"); ((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z)); //j = factor; //break; } } } extramsg = displayresult; } MainV2.comPort.UnSubscribeToPacketType(sub); MainV2.comPort.UnSubscribeToPacketType(sub2); if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0) { e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance."; ans = null; ans2 = null; return; } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (extramsg != "") { if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No) { e.CancelAcknowledged = true; e.CancelRequested = true; ans = null; ans2 = null; return; } } // remove outlyers RemoveOutliers(ref datacompass1); if (havecompass2 && datacompass2.Count > 0) { RemoveOutliers(ref datacompass2); } if (datacompass1.Count < 10) { e.ErrorMessage = "Log does not contain enough data"; ans = null; ans2 = null; return; } bool ellipsoid = false; if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA")) { ellipsoid = true; } log.Info("Compass 1"); ans = MagCalib.LeastSq(datacompass1, ellipsoid); if (havecompass2 && datacompass2.Count > 0) { log.Info("Compass 2"); ans2 = MagCalib.LeastSq(datacompass2, ellipsoid); } }
void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { this.ConvertBini(inputfn, outputfn, true); }
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // list of x,y,z 's List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >(); // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz float oldmx = 0; float oldmy = 0; float oldmz = 0; // filter data points to only x number per quadrant int div = 20; Hashtable filter = new Hashtable(); string extramsg = ""; ((ProgressReporterSphere)sender).sphere1.Clear(); while (true) { ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = false; e.CancelRequested = false; break; } if (oldmx != MainV2.comPort.MAV.cs.mx && oldmy != MainV2.comPort.MAV.cs.my && oldmz != MainV2.comPort.MAV.cs.mz) { // for new lease sq string item = (int)(MainV2.comPort.MAV.cs.mx / div) + "," + (int)(MainV2.comPort.MAV.cs.my / div) + "," + (int)(MainV2.comPort.MAV.cs.mz / div); if (filter.ContainsKey(item)) { filter[item] = (int)filter[item] + 1; if ((int)filter[item] > 3) { continue; } } else { filter[item] = 1; } // add data data.Add(new Tuple <float, float, float>( MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z)); oldmx = MainV2.comPort.MAV.cs.mx; oldmy = MainV2.comPort.MAV.cs.my; oldmz = MainV2.comPort.MAV.cs.mz; ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(oldmx, oldmy, oldmz)); // for old method setMinorMax(MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, ref minx, ref maxx); setMinorMax(MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, ref miny, ref maxy); setMinorMax(MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z, ref minz, ref maxz); } //find the mean radius HIL.Vector3 centre = new HIL.Vector3(); //new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2)); HIL.Vector3 point; float radius = 0; for (int i = 0; i < data.Count; i++) { point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3); radius += (float)(point - centre).length(); } radius /= data.Count; //test that we can find one point near a set of points all around the sphere surface int factor = 4; // 4 point check 2x2 float max_distance = radius / 3; //pretty generouse for (int j = 0; j < factor; j++) { double theta = (Math.PI * (j + 0.5)) / factor; for (int i = 0; i < factor; i++) { double phi = (2 * Math.PI * i) / factor; HIL.Vector3 point_sphere = new HIL.Vector3( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) + centre; //Console.WriteLine("{0} {1}", theta * rad2deg, phi * rad2deg); bool found = false; for (int k = 0; k < data.Count; k++) { point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3); double d = (point_sphere - point).length(); if (d < max_distance) { found = true; break; } } if (!found) { extramsg = "more data needed"; //e.ErrorMessage = "Data missing for some directions"; //ans = null; //return; j = factor; break; } else { extramsg = ""; } } } } if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0) { e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance."; ans = null; return; } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (data.Count < 10 || extramsg != "") { e.ErrorMessage = "Log does not contain enough data"; ans = null; return; } bool ellipsoid = false; if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA")) { ellipsoid = true; } ans = MagCalib.LeastSq(data, ellipsoid); }
void DoCalibration(object sender, ProgressWorkerEventArgs e, object passdata = null) { var prd = ((ProgressReporterDialogue)sender); prd.UpdateProgressAndStatus(-1, "Starting Compass Mot"); int8_t comp_type; // throttle or current based compensation Vector3f compass_base = new Vector3f(); // compass vector when throttle is zero Vector3f motor_impact = new Vector3f(); // impact of motors on compass vector Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle Vector3f motor_compensation; // final compensation to be stored to eeprom float throttle_pct; // throttle as a percentage 0.0 ~ 1.0 float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0) float current_amps_max = 0.0f; // maximum current reached float interference_pct; // interference as a percentage of total mag field (for reporting purposes only) //uint32_t last_run_time; uint8_t print_counter = 49; bool updated = false; // have we updated the compensation vector at least once if ((float)MainV2.comPort.MAV.param["BATT_MONITOR"] == 4f) // volt and current { comp_type = (sbyte)comptype.Current; prd.UpdateProgressAndStatus(-1, "Compass Mot using current"); } else { comp_type = (sbyte)comptype.Throttle; prd.UpdateProgressAndStatus(-1, "Compass Mot using throttle"); } if ((float)MainV2.comPort.MAV.param["COMPASS_USE"] != 1) { e.ErrorMessage = "Compass is disabled"; return; } // request streams MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 10); // rc out MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); // mag out // reset compass mot MainV2.comPort.setParam("COMPASS_MOTCT ", 0.0f); MainV2.comPort.setParam("COMPASS_MOT_X", 0.000000f); MainV2.comPort.setParam("COMPASS_MOT_Y", 0.000000f); MainV2.comPort.setParam("COMPASS_MOT_Z", 0.000000f); // store initial x,y,z compass values compass_base.x = MainV2.comPort.MAV.cs.mx; compass_base.y = MainV2.comPort.MAV.cs.my; compass_base.z = MainV2.comPort.MAV.cs.mz; // initialise motor compensation motor_compensation = new Vector3f(0, 0, 0); int magseen = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU]; int rcseen = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.HIL_RC_INPUTS_RAW]; DateTime deadline = DateTime.Now.AddSeconds(10); prd.UpdateProgressAndStatus(-1, "Waiting for Mag and RC data"); while (true) { if (magseen > (magseen + 100) && rcseen > (rcseen + 20)) { break; } if (e.CancelRequested) { e.CancelAcknowledged = true; return; } if (DateTime.Now > deadline) { e.ErrorMessage = "Not enough packets where received\n" + magseen + " mag " + rcseen + " rc"; return; } } while (true) { if (prd.doWorkArgs.CancelRequested) { prd.doWorkArgs.CancelAcknowledged = true; break; } // radio // passthorugh - cant do. // compass read // battery read // calculate scaling for throttle int checkme; throttle_pct = (float)MainV2.comPort.MAV.cs.ch3percent / 100.0f; throttle_pct = constrain_float(throttle_pct, 0.0f, 1.0f); // if throttle is zero, update base x,y,z values if (throttle_pct == 0.0f) { compass_base.x = compass_base.x * 0.99f + (float)MainV2.comPort.MAV.cs.mx * 0.01f; compass_base.y = compass_base.y * 0.99f + (float)MainV2.comPort.MAV.cs.my * 0.01f; compass_base.z = compass_base.z * 0.99f + (float)MainV2.comPort.MAV.cs.mz * 0.01f; // causing printing to happen as soon as throttle is lifted print_counter = 49; } else { // calculate diff from compass base and scale with throttle motor_impact.x = MainV2.comPort.MAV.cs.mx - compass_base.x; motor_impact.y = MainV2.comPort.MAV.cs.my - compass_base.y; motor_impact.z = MainV2.comPort.MAV.cs.mz - compass_base.z; // throttle based compensation if (comp_type == (byte)comptype.Throttle) { // scale by throttle motor_impact_scaled = motor_impact / throttle_pct; // adjust the motor compensation to negate the impact motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; updated = true; } else { // current based compensation if more than 3amps being drawn motor_impact_scaled = motor_impact / MainV2.comPort.MAV.cs.current; // adjust the motor compensation to negate the impact if drawing over 3amps if (MainV2.comPort.MAV.cs.current >= 3.0f) { motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; updated = true; } } // record maximum throttle and current throttle_pct_max = max(throttle_pct_max, throttle_pct); current_amps_max = max(current_amps_max, MainV2.comPort.MAV.cs.current); // display output at 1hz if throttle is above zero print_counter++; if (print_counter >= 50) { print_counter = 0; var line = String.Format("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n", (int)MainV2.comPort.MAV.cs.ch3percent, (float)MainV2.comPort.MAV.cs.current, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); Console.Write(line); prd.UpdateProgressAndStatus(-1, line); } } } MainV2.comPort.doARM(false); // set and save motor compensation if (updated) { MainV2.comPort.setParam("COMPASS_MOTCT ", comp_type); MainV2.comPort.setParam("COMPASS_MOT_X", (float)motor_compensation.x); MainV2.comPort.setParam("COMPASS_MOT_Y", (float)motor_compensation.y); MainV2.comPort.setParam("COMPASS_MOT_Z", (float)motor_compensation.z); // calculate and display interference compensation at full throttle as % of total mag field if (comp_type == (byte)comptype.Throttle) { // interference is impact@fullthrottle / mag field * 100 interference_pct = (float)motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; } else { // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100 interference_pct = (float)motor_compensation.length() * (current_amps_max / throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; } string line = String.Format("\nInterference at full throttle is {0}% of mag field\n\n", (int)interference_pct); Console.Write(line); prd.UpdateProgressAndStatus(100, line); } else { prd.UpdateProgressAndStatus(100, "Failed"); } MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 2); }
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // turn learning off MainV2.comPort.setParam("COMPASS_LEARN", 0); bool havecompass2 = false; //compass2 get mag2 offsets if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X")) { //com2ofsx = MainV2.comPort.GetParam("COMPASS_OFS2_X"); //com2ofsy = MainV2.comPort.GetParam("COMPASS_OFS2_Y"); //com2ofsz = MainV2.comPort.GetParam("COMPASS_OFS2_Z"); MainV2.comPort.setParam("COMPASS_OFS2_X", 0, true); MainV2.comPort.setParam("COMPASS_OFS2_Y", 0, true); MainV2.comPort.setParam("COMPASS_OFS2_Z", 0, true); havecompass2 = true; } int hittarget = 14;// int.Parse(File.ReadAllText("magtarget.txt")); // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; // backup current rate and set byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; byte backuprateatt = MainV2.comPort.MAV.cs.rateattitude; byte backupratepos = MainV2.comPort.MAV.cs.rateposition; MainV2.comPort.MAV.cs.ratesensors = 2; MainV2.comPort.MAV.cs.rateattitude = 0; MainV2.comPort.MAV.cs.rateposition = 0; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0); MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); // subscribe to data packets var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket); var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket); string extramsg = ""; // clear any old data ((ProgressReporterSphere)sender).sphere1.Clear(); ((ProgressReporterSphere)sender).sphere2.Clear(); // keep track of data count and last lsq run int lastcount = 0; DateTime lastlsq = DateTime.MinValue; DateTime lastlsq2 = DateTime.MinValue; HIL.Vector3 centre = new HIL.Vector3(); while (true) { // slow down execution System.Threading.Thread.Sleep(10); ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + datacompass1.Count + " Samples\ncompass 1 error:" + error + "\ncompass 2 error:" + error2 + "\n" + extramsg); if (e.CancelRequested) { e.CancelAcknowledged = false; e.CancelRequested = false; break; } if (datacompass1.Count == 0) { continue; } float rawmx = datacompass1[datacompass1.Count - 1].Item1; float rawmy = datacompass1[datacompass1.Count - 1].Item2; float rawmz = datacompass1[datacompass1.Count - 1].Item3; // for old method setMinorMax(rawmx, ref minx, ref maxx); setMinorMax(rawmy, ref miny, ref maxy); setMinorMax(rawmz, ref minz, ref maxz); // get the current estimated centerpoint //new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2)); //Console.WriteLine("1 " + DateTime.Now.Millisecond); // run lsq every second when more than 100 datapoints if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second) { MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0); MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); lastlsq = DateTime.Now; lock (datacompass1) { var lsq = MagCalib.LeastSq(datacompass1, false); // simple validation if (Math.Abs(lsq[0]) < 999) { centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]); log.Info("new centre " + centre.ToString()); ((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3((float)centre.x, (float)centre.y, (float)centre.z); } } } // run lsq every second when more than 100 datapoints if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second) { lastlsq2 = DateTime.Now; lock (datacompass2) { var lsq = MagCalib.LeastSq(datacompass2, false); // simple validation if (Math.Abs(lsq[0]) < 999) { HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]); log.Info("new centre2 " + centre2.ToString()); ((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3((float)centre2.x, (float)centre2.y, (float)centre2.z); } } } //Console.WriteLine("1a " + DateTime.Now.Millisecond); // dont use dup data if (lastcount == datacompass1.Count) { continue; } lastcount = datacompass1.Count; // add to sphere with center correction ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz)); ((ProgressReporterSphere)sender).sphere1.AimClear(); if (datacompass2.Count > 30) { float raw2mx = datacompass2[datacompass2.Count - 1].Item1; float raw2my = datacompass2[datacompass2.Count - 1].Item2; float raw2mz = datacompass2[datacompass2.Count - 1].Item3; ((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz)); ((ProgressReporterSphere)sender).sphere2.AimClear(); } //Console.WriteLine("2 " + DateTime.Now.Millisecond); HIL.Vector3 point; point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre; //find the mean radius float radius = 0; for (int i = 0; i < datacompass1.Count; i++) { point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3); radius += (float)(point + centre).length(); } radius /= datacompass1.Count; //test that we can find one point near a set of points all around the sphere surface int pointshit = 0; string displayresult = ""; int factor = 3; // pitch int factor2 = 4; // yaw float max_distance = radius / 3; //pretty generouse for (int j = 0; j <= factor; j++) { double theta = (Math.PI * (j + 0.5)) / factor; for (int i = 0; i <= factor2; i++) { double phi = (2 * Math.PI * i) / factor2; HIL.Vector3 point_sphere = new HIL.Vector3( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) - centre; //log.InfoFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance); bool found = false; for (int k = 0; k < datacompass1.Count; k++) { point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3); double d = (point_sphere - point).length(); if (d < max_distance) { pointshit++; found = true; break; } } // draw them all //((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z)); if (!found) { displayresult = "more data needed Aim For " + GetColour((int)(theta * rad2deg), (int)(phi * rad2deg)); ((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z)); //j = factor; //break; } } } extramsg = displayresult; //Console.WriteLine("3 "+ DateTime.Now.Millisecond); // check primary compass error if (error < 0.2 && pointshit > hittarget && ((ProgressReporterSphere)sender).autoaccept) { extramsg = ""; break; } } MainV2.comPort.UnSubscribeToPacketType(sub); MainV2.comPort.UnSubscribeToPacketType(sub2); // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.MAV.cs.rateattitude = backuprateatt; MainV2.comPort.MAV.cs.rateposition = backupratepos; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MainV2.comPort.MAV.cs.rateposition); // request gps MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MainV2.comPort.MAV.cs.rateattitude); // request attitude MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MainV2.comPort.MAV.cs.rateattitude); // request vfr MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MainV2.comPort.MAV.cs.ratesensors); // request extra stuff - tridge MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // request raw sensor MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MainV2.comPort.MAV.cs.raterc); // request rc info if (MainV2.speechEnable) { MainV2.speechEngine.SpeakAsync("Compass Calibration Complete"); } else { Console.Beep(); } if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0) { e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance."; ans = null; ans2 = null; return; } if (extramsg != "") { if (CustomMessageBox.Show(Strings.MissingDataPoints, Strings.RunAnyway, MessageBoxButtons.YesNo) == DialogResult.No) { e.CancelAcknowledged = true; e.CancelRequested = true; ans = null; ans2 = null; return; } } // remove outlyers RemoveOutliers(ref datacompass1); if (havecompass2 && datacompass2.Count > 0) { RemoveOutliers(ref datacompass2); } if (datacompass1.Count < 10) { e.ErrorMessage = "Log does not contain enough data"; ans = null; ans2 = null; return; } bool ellipsoid = false; if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA")) { ellipsoid = true; } log.Info("Compass 1"); ans = MagCalib.LeastSq(datacompass1, ellipsoid); if (havecompass2 && datacompass2.Count > 0) { log.Info("Compass 2"); ans2 = MagCalib.LeastSq(datacompass2, ellipsoid); } }
private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs) { frmProgressReporter.UpdateProgressAndStatus(-1, "正在连接....."); giveComport = true; byte[] buffer = new byte[1024]; if (BaseStream is SerialPort) { // allow settings to settle - previous dtr Thread.Sleep(1000); } //Terrain = new TerrainFollow(this); try { BaseStream.ReadBufferSize = 1024; lock (objlock) // so we dont have random traffic { log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate); if (BaseStream is UdpSerial) { progressWorkerEventArgs.CancelRequestChanged += (o, e) => { ((UdpSerial)BaseStream).CancelConnect = true; ((ProgressWorkerEventArgs)o) .CancelAcknowledged = true; }; } BaseStream.Open(); // 读取数据 待修改 //BaseStream.DiscardInBuffer(); // other boards seem to have issues if there is no delay? posible bootloader timeout issue if (BaseStream is SerialPort) { Thread.Sleep(1000); } } //List<MAVLinkMessage> hbhistory = new List<MAVLinkMessage>(); DateTime start = DateTime.Now; DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS); var countDown = new Timer { Interval = 1000, AutoReset = false }; countDown.Elapsed += (sender, e) => { int secondsRemaining = (deadline - e.SignalTime).Seconds; frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining)); if (secondsRemaining > 0) { countDown.Start(); } }; countDown.Start(); int count = 0; while (true) { if (progressWorkerEventArgs.CancelRequested) { progressWorkerEventArgs.CancelAcknowledged = true; countDown.Stop(); if (BaseStream.IsOpen) { BaseStream.Close(); } giveComport = false; return; } log.Info(DateTime.Now.Millisecond + " Start connect loop "); if (DateTime.Now > deadline) { //if (Progress != null) // Progress(-1, "No Heartbeat Packets"); countDown.Stop(); this.Close(); } Thread.Sleep(1); //BaseStream.Read(buffer, count, 1); count++; // 解析数据,获取 sysid ,comid sysidcurrent = 1; compidcurrent = 50; // if we get no data, try enableing rts/cts //if (buffer.Length == 0 && BaseStream is SerialPort) //{ // BaseStream.RtsEnable = !BaseStream.RtsEnable; //} if (count > 2) { break; } SetupMavConnect(1, 50, "AIR"); } countDown.Stop(); if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true) { giveComport = false; if (BaseStream.IsOpen) { BaseStream.Close(); } return; } } catch (Exception e) { try { BaseStream.Close(); } catch { } giveComport = false; if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage)) { progressWorkerEventArgs.ErrorMessage = Strings.ConnectFailed; } log.Error(e); Console.WriteLine(e.ToString()); throw; } //frmProgressReporter.Close(); giveComport = false; frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done); //log.Info("Done open " + MAV.sysid + " " + MAV.compid); //MAV.packetslost = 0; //MAV.synclost = 0; }
void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // list of x,y,z 's List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >(); // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz DateTime deadline = DateTime.Now.AddSeconds(60); float oldmx = 0; float oldmy = 0; float oldmz = 0; while (deadline > DateTime.Now) { double timeremaining = (deadline - DateTime.Now).TotalSeconds; ((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds - got " + data.Count + " Samples"); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = true; return; } if (oldmx != MainV2.comPort.MAV.cs.mx && oldmy != MainV2.comPort.MAV.cs.my && oldmz != MainV2.comPort.MAV.cs.mz) { data.Add(new Tuple <float, float, float>( MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z)); oldmx = MainV2.comPort.MAV.cs.mx; oldmy = MainV2.comPort.MAV.cs.my; oldmz = MainV2.comPort.MAV.cs.mz; } } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (data.Count < 10) { e.ErrorMessage = "Log does not contain enough data"; ans = null; return; } ans = MagCalib.LeastSq(data); //find the mean radius Vector3f centre = new Vector3f((float)-ans[0], (float)-ans[1], (float)-ans[2]); Vector3f point; float radius = 0; for (int i = 0; i < data.Count; i++) { point = new Vector3f(data[i].Item1, data[i].Item2, data[i].Item3); radius += Vector3f.Distance(point, centre); } radius /= data.Count; //test that we can find one point near a set of points all around the sphere surface int factor = 10; float max_distance = radius / 3; //pretty generouse for (int j = 0; j < factor; j++) { double theta = (Math.PI * (j + 0.5)) / factor; for (int i = 0; i < factor; i++) { double phi = (2 * Math.PI * i) / factor; Vector3f point_sphere = new Vector3f( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) + centre; bool found = false; for (int k = 0; k < data.Count; k++) { point = new Vector3f(data[i].Item1, data[i].Item2, data[i].Item3); double d = Vector3f.Distance(point_sphere, point); if (d < max_distance) { found = true; break; } } if (!found) { e.ErrorMessage = "Data missing for some directions"; ans = null; return; } } } }
private void FrmProgressReporterDoWorkNOParams(object sender, ProgressWorkerEventArgs e, object passdata = null) { OpenBg(sender, false, e); }
void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // list of x,y,z 's List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>(); // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz DateTime deadline = DateTime.Now.AddSeconds(60); float oldmx = 0; float oldmy = 0; float oldmz = 0; while (deadline > DateTime.Now) { double timeremaining = (deadline - DateTime.Now).TotalSeconds; ((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds"); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = true; return; } if (oldmx != MainV2.comPort.MAV.cs.mx && oldmy != MainV2.comPort.MAV.cs.my && oldmz != MainV2.comPort.MAV.cs.mz) { data.Add(new Tuple<float, float, float>( MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z)); oldmx = MainV2.comPort.MAV.cs.mx; oldmy = MainV2.comPort.MAV.cs.my; oldmz = MainV2.comPort.MAV.cs.mz; } } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (data.Count < 10) { CustomMessageBox.Show("Log does not contain enough data"); return; } double[] ans = MagCalib.LeastSq(data); MagCalib.SaveOffsets(ans); }
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket); string extramsg = ""; ((ProgressReporterSphere)sender).sphere1.Clear(); int lastcount = 0; while (true) { // slow down execution System.Threading.Thread.Sleep(20); ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = false; e.CancelRequested = false; break; } if (data.Count == 0) { continue; } // dont use dup data if (lastcount == data.Count) { continue; } lastcount = data.Count; float rawmx = data[data.Count - 1].Item1; float rawmy = data[data.Count - 1].Item2; float rawmz = data[data.Count - 1].Item3; // for old method setMinorMax(rawmx, ref minx, ref maxx); setMinorMax(rawmy, ref miny, ref maxy); setMinorMax(rawmz, ref minz, ref maxz); // get the current estimated centerpoint HIL.Vector3 centre = new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2)); HIL.Vector3 point; // add to sphere with center correction point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre; ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3((float)point.x, (float)point.y, (float)point.z)); //find the mean radius float radius = 0; for (int i = 0; i < data.Count; i++) { point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3); radius += (float)(point + centre).length(); } radius /= data.Count; //test that we can find one point near a set of points all around the sphere surface int factor = 4; // 4 point check 16 points float max_distance = radius / 3; //pretty generouse for (int j = 0; j < factor; j++) { double theta = (Math.PI * (j + 0.5)) / factor; for (int i = 0; i < factor; i++) { double phi = (2 * Math.PI * i) / factor; HIL.Vector3 point_sphere = new HIL.Vector3( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) - centre; //log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance); bool found = false; for (int k = 0; k < data.Count; k++) { point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3); double d = (point_sphere - point).length(); if (d < max_distance) { found = true; break; } } if (!found) { extramsg = "more data needed"; //e.ErrorMessage = "Data missing for some directions"; //ans = null; //return; j = factor; break; } else { extramsg = ""; } } } } MainV2.comPort.UnSubscribeToPacketType(sub); if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0) { e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance."; ans = null; return; } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (extramsg != "") { if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No) { e.CancelAcknowledged = true; e.CancelRequested = true; ans = null; return; } } // remove outlyers RemoveOutliers(ref data); if (data.Count < 10) { e.ErrorMessage = "Log does not contain enough data"; ans = null; return; } bool ellipsoid = false; if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA")) { ellipsoid = true; } ans = MagCalib.LeastSq(data, ellipsoid); }
static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { bool havecompass2 = false; havecompass2 = true; int hittarget = 14; // old method float minx = 0; float maxx = 0; float miny = 0; float maxy = 0; float minz = 0; float maxz = 0; MavLinkInterface mavLinkInterface = new MavLinkInterface(); MavLink.mavlink_request_data_stream_t req = new MavLink.mavlink_request_data_stream_t(); mavLinkInterface = new MavLinkInterface(); req = new MavLink.mavlink_request_data_stream_t(); req.target_system = Global.sysID; req.target_component = Global.compID; req.req_message_rate = 30; //50 req.start_stop = 1; // start req.req_stream_id = (byte)1; // id 1 // send each one twice. mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); string extramsg = ""; // clear any old data ((ProgressReporterSphere)sender).sphere1.Clear(); ((ProgressReporterSphere)sender).sphere2.Clear(); // keep track of data count and last lsq run int lastcount = 0; DateTime lastlsq = DateTime.MinValue; DateTime lastlsq2 = DateTime.MinValue; DateTime lastlsq3 = DateTime.MinValue; HI.Vector3 centre = new HI.Vector3(); int pointshitnum = 0; while (true) { // slow down execution System.Threading.Thread.Sleep(10); string str = " 共有 " + datacompass1.Count + " 份数据\n\n" + " 罗盘 1 误差:" + error + "\n" + " 罗盘 2 误差:" + error2 + "\n" + " 结束条件,罗盘 1 误差不大于0.2"; str += "\n " + extramsg; ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, str); addcompass1list(); addcompass2list(); if (e.CancelRequested) { e.CancelAcknowledged = false; e.CancelRequested = false; break; } if (datacompass1.Count == 0) { continue; } float rawmx = datacompass1[datacompass1.Count - 1].Item1; float rawmy = datacompass1[datacompass1.Count - 1].Item2; float rawmz = datacompass1[datacompass1.Count - 1].Item3; // for old method setMinorMax(rawmx, ref minx, ref maxx); setMinorMax(rawmy, ref miny, ref maxy); setMinorMax(rawmz, ref minz, ref maxz); if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second) //w { //req = new MavLink.mavlink_request_data_stream_t(); //req.target_system = Global.sysID; //req.target_component = Global.compID; //req.req_message_rate = 30; //50 //req.start_stop = 1; // start //req.req_stream_id = (byte)1; // id 1 //// send each one twice. //mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); //mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); lastlsq = DateTime.Now; lock (datacompass1) { var lsq = MagCalib.LeastSq(datacompass1, false); // simple validation if (Math.Abs(lsq[0]) < 999) { centre = new HI.Vector3(lsq[0], lsq[1], lsq[2]); ((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3( (float)centre.x, (float)centre.y, (float)centre.z); } } } // run lsq every second when more than 100 datapoints if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)//w { lastlsq2 = DateTime.Now; lock (datacompass2) { var lsq = MagCalib.LeastSq(datacompass2, false); // simple validation if (Math.Abs(lsq[0]) < 999) { HI.Vector3 centre2 = new HI.Vector3(lsq[0], lsq[1], lsq[2]); ((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3( (float)centre2.x, (float)centre2.y, (float)centre2.z); } } } if (lastcount == datacompass1.Count) { continue; } lastcount = datacompass1.Count; // add to sphere with center correction ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz)); ((ProgressReporterSphere)sender).sphere1.AimClear(); if (datacompass2.Count > 30)//w { float raw2mx = datacompass2[datacompass2.Count - 1].Item1; float raw2my = datacompass2[datacompass2.Count - 1].Item2; float raw2mz = datacompass2[datacompass2.Count - 1].Item3; ((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz)); ((ProgressReporterSphere)sender).sphere2.AimClear(); } HI.Vector3 point; point = new HI.Vector3(rawmx, rawmy, rawmz) + centre; //find the mean radius float radius = 0; for (int i = 0; i < datacompass1.Count; i++) { point = new HI.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3); radius += (float)(point + centre).length(); } radius /= datacompass1.Count; //test that we can find one point near a set of points all around the sphere surface int pointshit = 0; string displayresult = ""; int factor = 3; // pitch int factor2 = 4; // yaw float max_distance = radius / 3; //pretty generouse for (int j = 0; j <= factor; j++) { double theta = (Math.PI * (j + 0.5)) / factor; for (int i = 0; i <= factor2; i++) { double phi = (2 * Math.PI * i) / factor2; HI.Vector3 point_sphere = new HI.Vector3( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) - centre; //log.InfoFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance); bool found = false; for (int k = 0; k < datacompass1.Count; k++) { point = new HI.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3); double d = (point_sphere - point).length(); if (d < max_distance) { pointshit++; found = true; break; } } // draw them all //((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z)); //if (!found) //{ // displayresult = "需要更多的数据 " + // GetColour((int)(theta * rad2deg), (int)(phi * rad2deg)); // ((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, // (float)point_sphere.y, (float)point_sphere.z)); // //j = factor; // //break; //} } } extramsg = displayresult; if (error < 0.2 && pointshit > hittarget && ((ProgressReporterSphere)sender).autoaccept) { extramsg = ""; MessageBox.Show("旋转完成\r\n请自行关闭罗盘校准窗口"); break; } } req = new MavLink.mavlink_request_data_stream_t(); req.target_system = Global.sysID; req.target_component = Global.compID; req.req_message_rate = 2; //50 req.start_stop = 1; // start req.req_stream_id = (byte)1; // id 1 // send each one twice. mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); mavLinkInterface.AssembleAndSendFrame((byte)MavLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req); ans = MagCalib.LeastSq(datacompass1, false); ans2 = MagCalib.LeastSq(datacompass2, false); }
void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null) { // list of x,y,z 's List <Tuple <float, float, float> > data = new List <Tuple <float, float, float> >(); // backup current rate and set to 10 hz byte backupratesens = MainV2.comPort.MAV.cs.ratesensors; MainV2.comPort.MAV.cs.ratesensors = 10; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz float oldmx = 0; float oldmy = 0; float oldmz = 0; ((ProgressReporterSphere)sender).sphere1.Clear(); while (true) { ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples"); if (e.CancelRequested) { // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); e.CancelAcknowledged = false; e.CancelRequested = false; break; } if (oldmx != MainV2.comPort.MAV.cs.mx && oldmy != MainV2.comPort.MAV.cs.my && oldmz != MainV2.comPort.MAV.cs.mz) { data.Add(new Tuple <float, float, float>( MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z)); oldmx = MainV2.comPort.MAV.cs.mx; oldmy = MainV2.comPort.MAV.cs.my; oldmz = MainV2.comPort.MAV.cs.mz; ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(oldmx, oldmy, oldmz)); } } // restore old sensor rate MainV2.comPort.MAV.cs.ratesensors = backupratesens; MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); if (data.Count < 10) { e.ErrorMessage = "Log does not contain enough data"; ans = null; return; } bool ellipsoid = false; if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA")) { ellipsoid = true; } ans = MagCalib.LeastSq(data, ellipsoid); //find the mean radius HIL.Vector3 centre = new HIL.Vector3((float)-ans[0], (float)-ans[1], (float)-ans[2]); HIL.Vector3 point; float radius = 0; for (int i = 0; i < data.Count; i++) { point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3); radius += (float)(point - centre).length(); } radius /= data.Count; //test that we can find one point near a set of points all around the sphere surface int factor = 3; // 9 point check 3x3 float max_distance = radius / 3; //pretty generouse for (int j = 0; j < factor; j++) { double theta = (Math.PI * (j + 0.5)) / factor; for (int i = 0; i < factor; i++) { double phi = (2 * Math.PI * i) / factor; HIL.Vector3 point_sphere = new HIL.Vector3( (float)(Math.Sin(theta) * Math.Cos(phi) * radius), (float)(Math.Sin(theta) * Math.Sin(phi) * radius), (float)(Math.Cos(theta) * radius)) + centre; Console.WriteLine("{0} {1}", theta * rad2deg, phi * rad2deg); bool found = false; for (int k = 0; k < data.Count; k++) { point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3); double d = (point_sphere - point).length(); if (d < max_distance) { found = true; break; } } if (!found) { e.ErrorMessage = "Data missing for some directions"; ans = null; return; } } } }