public void ReceiverThread() { IPEndPoint remote = new IPEndPoint(IPAddress.Any, UDP_PORT); byte[] data = null; while (runReceiverThread) { try { data = udpSocket.Receive(ref remote); if (remote.Port != UDP_PORT) { continue; } } catch (System.Net.Sockets.SocketException e) { continue; //socket closed, finish execution if run is false } if (data.Length != Marshal.SizeOf(frames[0])) { MessageBox.Show(".Net frame=" + Marshal.SizeOf(frames[0]).ToString() + " received=" + data.Length.ToString()); continue; } GCHandle handle = GCHandle.Alloc(data, GCHandleType.Pinned); RobotFrame frame = new RobotFrame(); try { IntPtr ptr = handle.AddrOfPinnedObject(); frame = (RobotFrame)Marshal.PtrToStructure(ptr, typeof(RobotFrame)); } finally { handle.Free(); } laserCanvas.Dispatcher.BeginInvoke(new RefresherHandler(RefreshReadings), null, frame); } }
public void RefreshReadings(object sender, RobotFrame frame) { map_point_to_update = 0; const double deg2rad = Math.PI / 180.0; double centerx = laserCanvas.ActualWidth / 2.0; double centery = laserCanvas.ActualHeight / 2.0; double middlex = centerx - POINT_RADIUS; double middley = centery - POINT_RADIUS; laserRPMTextBlock.Text = frame.laser_speed.ToString(); double packet_time_sec = (double)frame.elapsed_ms / 100000.0; const double ENCODER_SCALE_FACTOR = Math.PI * 4.32 / 360.0; double ldiff = frame.left_stop - frame.left_start; double rdiff = frame.right_stop - frame.right_start; double displacement_cm = (ldiff + rdiff) * ENCODER_SCALE_FACTOR / 2.0; speedBuffer.Add(displacement_cm / packet_time_sec); speedTextBlock.Text = (-speedBuffer.Average).ToString("F2"); double gyro_start = frame.angle_start / 100.0; double gyro_stop = frame.angle_stop / 100.0; double angle_difference = (gyro_stop - gyro_start); if (Math.Abs(angle_difference) > 180.0) { angle_difference = Math.Sign(angle_difference) * 360.0 - angle_difference; } angleTextBlock.Text = gyro_stop.ToString(); double gyro_rad = gyro_stop * deg2rad; double gyro_average_rad = (gyro_start + angle_difference / 2.0) * deg2rad; double max_mapping_distance = 10000; if (maxMappingDistanceDoubleUpDown.Value.HasValue) { max_mapping_distance = maxMappingDistanceDoubleUpDown.Value.Value; } int point_index; LaserReading[] readings = frame.laser_readings; for (int i = 0; i < readings.Length; ++i) { point_index = i + frame.laser_angle * 4; if (readings[i].unknown_warning == 1) { Console.Out.WriteLine("Uknown warning flag triggered! "); } if (readings[i].invalid_data == 1) { points[point_index].Visibility = Visibility.Hidden; continue;//readings[i].distance = 160; } else { points[point_index].Visibility = Visibility.Visible; } points[point_index].Stroke = DeterminePointColor(readings[i]); double angle = point_index; double angle_offset = gyro_start + i * angle_difference / (readings.Length - 1); if (absoluteRotationCheckBox.IsChecked == true) { angle -= angle_offset; } double rad = deg2rad * angle; double y, x; if (correctGeometryCheckBox.IsChecked == false) { y = -Math.Cos(rad) * readings[i].distance / 10.0; x = -Math.Sin(rad) * readings[i].distance / 10.0; } else //(correctGeometryCheckBox.IsChecked == true) { double yp = -Math.Cos(rad) * readings[i].distance; double xp = -Math.Sin(rad) * readings[i].distance; double alpha = 180 - 82 + angle; double alpharad = deg2rad * alpha; double b = 25; x = (xp + b * Math.Sin(alpharad)) / 10; y = (yp + b * Math.Cos(alpharad)) / 10; } if (absolutePositionCheckBox.IsChecked == true) { double angle_offset_rad = deg2rad * angle_offset; double displacement_offset = displacement_cm * (double)i / (double)(readings.Length - 1); x += positionEstimate.X - displacement_offset * Math.Sin(gyro_average_rad); y += positionEstimate.Y + displacement_offset * Math.Cos(gyro_average_rad); if (mapEstimateCheckBox.IsChecked == true && ((double)readings[i].distance / 10.0) < max_mapping_distance) { map_update[map_point_to_update].X = x + map_center_x; map_update[map_point_to_update].Y = y + map_center_y; ++map_point_to_update; } } Canvas.SetLeft(points[point_index], x + middlex + viewOffset.X); Canvas.SetTop(points[point_index], y + middley + viewOffset.Y); } RefreshPath(displacement_cm, gyro_average_rad, gyro_stop, centerx, centery); RefreshPointer(centerx, centery, gyro_rad, speedBuffer.Average); if (mapEstimateCheckBox.IsChecked == true && map_point_to_update > 0) { if (mapMovingCheckBox.IsChecked == true || displacement_cm == 0 && gyro_start == gyro_stop) { UpdateMap(); } } }