diff --git a/Makefile b/Makefile index 83a8d7a..e964793 100644 --- a/Makefile +++ b/Makefile @@ -66,7 +66,7 @@ IMGS = checkgreen.png checkempty.png checkemptygray.png all: $(ALL) MotionCal: gui.o portlist.o images.o $(OBJS) - $(CXX) $(SFLAG) $(CFLAGS) $(LDFLAGS) -o $@ $^ `$(WXCONFIG) --libs all,opengl` + $(CXX) $(SFLAG) $(CFLAGS) $(LDFLAGS) -o $@ $^ `$(WXCONFIG) --libs all,opengl` $(CLILIBS) MotionCal.exe: resource.o gui.o portlist.o images.o $(OBJS) $(CXX) $(SFLAG) $(CFLAGS) $(LDFLAGS) -o $@ $^ `$(WXCONFIG) --libs all,opengl` @@ -112,4 +112,3 @@ matrix.o: matrix.c imuread.h Makefile fusion.o: fusion.c imuread.h Makefile quality.o: quality.c imuread.h Makefile mahony.o: mahony.c imuread.h Makefile - diff --git a/README.md b/README.md new file mode 100644 index 0000000..659e444 --- /dev/null +++ b/README.md @@ -0,0 +1,263 @@ +# MotionCal + +![](img/MotionCal_2026-04-25_11-22.png) + +MotionCal is a desktop magnetometer calibration utility originally written by Paul Stoffregen. This fork is maintained at: + +```text +https://salemdata.net/repo/jlpoole/MotionCal +``` + +Paul's upstream source: + +```text +https://github.com/PaulStoffregen/MotionCal +``` + +The application reads IMU-style serial data, builds a 3D magnetometer point cloud, and estimates magnetic calibration parameters: + +```text +hard iron offset magnetic_offset_uT +soft iron correction magnetic_mapping_matrix +field strength magnetic_field_uT +``` + +This fork is being used with a LilyGO T-Beam Supreme / QMC6310 bridge firmware that streams MotionCal-compatible `Raw:` lines over USB serial. + +## What MotionCal Produces + +A saved calibration file contains values like: + +```text +magnetic_offset_uT=-172.96843,43.0260162,78.8941956 +magnetic_field_uT=52.4668198 +magnetic_mapping_matrix= + 0.943139076 0.0439298451 0.0595370531 + 0.0439298451 1.04979992 -0.0347476006 + 0.0595370531 -0.0347476006 1.01706612 +``` + +The hard-iron offset moves the center of the magnetometer cloud back to zero. The soft-iron matrix transforms an ellipsoid-shaped cloud back toward a sphere. + +The correction model is: + +```text +mag_uT = raw_motioncal_counts * 0.1 +centered = mag_uT - magnetic_offset_uT +corrected = magnetic_mapping_matrix * centered +``` + +## Serial Input Format + +MotionCal expects ASCII serial lines like this: + +```text +Raw:accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z +``` + +The T-Beam bridge firmware currently sends placeholder accelerometer and gyro values, plus live magnetometer readings: + +```text +Raw:0,0,8192,0,0,0,mag_x,mag_y,mag_z +``` + +Example: + +```text +Raw:0,0,8192,0,0,0,-1578,447,1266 +``` + +For this workflow, the magnetometer values are MotionCal integer counts: + +```text +1 count = 0.1 microtesla +uT = counts * 0.1 +``` + +These are not the QMC sensor's original register counts. The T-Beam bridge reads SensorLib Gauss values, converts Gauss to microtesla, then converts microtesla to MotionCal counts. + +## T-Beam Bridge Firmware + +The companion firmware project lives at: + +```text +/usr/local/src/microReticulumTbeam/exercises/25_motioncal_tbeam +``` + +Typical bridge workflow: + +```sh +source /home/jlpoole/pioenv/bin/activate +cd /usr/local/src/microReticulumTbeam/exercises/25_motioncal_tbeam +pio run -e dan -t upload +pio device monitor -b 115200 --port /dev/ttytDAN +``` + +Close the serial monitor before opening the same port in MotionCal. + +## Build On Linux + +MotionCal uses wxWidgets and OpenGL. On this system, the working build command is: + +```sh +cd /usr/local/src/MotionCal +make WXCONFIG=wx-config LDFLAGS="-lglut -lGLU -lGL -lm" +``` + +If the linker cannot find OpenGL/GLUT libraries, install the development packages for wxWidgets, OpenGL, GLU, and freeglut using your distribution's package manager. + +The Makefile default references a local wxWidgets path: + +```make +WXCONFIG = ~/wxwidgets/3.0.2.gtk2-opengl/bin/wx-config +``` + +Overriding `WXCONFIG=wx-config` on the make command line uses the system wxWidgets configuration instead. + +## Run + +Run the GUI with: + +```sh +cd /usr/local/src/MotionCal +./MotionCal +``` + +Then select the T-Beam USB serial port in the MotionCal window. + +To include a manually-created PTY, such as a `socat` split stream, pass it with `--port`: + +```sh +./MotionCal --port /tmp/ttyMotionCal +``` + +The port will appear in the Port menu and the Port dropdown even if it is not discovered by MotionCal's automatic `/dev/tty*` scan. + +If running under Wayland and the GUI has trouble starting or rendering, `GDK_BACKEND=x11` forces the wx/GTK path through X11 compatibility. + +```sh +cd /usr/local/src/MotionCal +GDK_BACKEND=x11 ./MotionCal --port /tmp/ttyMotionCal +``` + +## Calibration Procedure + +1. Flash and start the T-Beam bridge firmware. +2. Confirm the serial stream contains `Raw:` lines. +3. Close any terminal monitor using the serial port. +4. Start MotionCal. +5. Select the T-Beam serial port. +6. Rotate the board through as many 3D orientations as possible. +7. Continue until the point cloud has good sphere coverage and fit metrics are stable. +8. Save the calibration settings from MotionCal. + +Good calibration requires broad 3D motion. Do not only rotate the board flat on a table. Roll, pitch, yaw, invert, and sweep through orientations so the cloud fills the sphere. + +## Saved Settings Files + +This fork includes a modified save path that writes calibration settings to timestamped text files such as: + +```text +MotionCal_settings_20260425_114546.txt +MotionCal_KitchenMagnet_settings_20260426_080729.txt +``` + +Saved files include: + +```text +valid_points +fit_error_percent +surface_gap_error_percent +magnitude_variance_error_percent +wobble_error_percent +magnetic_offset_uT +magnetic_field_uT +magnetic_mapping_matrix +cal1_echo_line +cal2_echo_line +raw_points +``` + +The `raw_points` section is useful for later analysis because it preserves the magnetometer point cloud that produced the calibration. + +## Calibration Packet Echo + +MotionCal can send a 68-byte calibration packet back to the device. The T-Beam bridge accepts that packet and echoes human-readable values: + +```text +Cal1:... +Cal2:... +``` + +The `Cal1` line contains hard-iron offsets and field strength. The `Cal2` line contains the 3x3 soft-iron mapping matrix. + +## Interpreting Hard And Soft Iron + +Hard iron is a fixed magnetic bias. It shifts the center of the point cloud. + +Soft iron is field distortion. It stretches, squashes, shears, or rotates the cloud into an ellipsoid. MotionCal estimates an inverse soft-iron matrix that maps the ellipsoid back toward a sphere. + +A matrix close to identity means small soft-iron distortion: + +```text +1 0 0 +0 1 0 +0 0 1 +``` + +Example modest soft-iron correction: + +```text +0.9431 0.0439 0.0595 +0.0439 1.0498 -0.0347 +0.0595 -0.0347 1.0171 +``` + +## Troubleshooting + +If MotionCal does not show incoming points: + +```text +Confirm the correct serial port is selected. +Confirm the device is streaming at 115200 baud. +Confirm the stream contains Raw: lines. +Close pio device monitor before opening the port in MotionCal. +``` + +If calibration values appear 10x different from a notebook or script: + +```text +MotionCal saved offsets are in microtesla. +The Raw: magnetometer values are MotionCal counts. +Convert counts to uT with: uT = counts * 0.1 +``` + +If the fit is poor: + +```text +Collect more orientations. +Keep the board away from steel, magnets, speakers, motors, and high-current wiring. +Try the calibration in a different physical location. +Watch for read_fail or overflow messages from the bridge firmware. +``` + +If MotionCal crashes after a long run with Mesa or AMDGPU allocation errors: + +```text +MotionCal's magnetometer sample buffer is bounded at 650 points, so it is not +intended to grow without limit. Errors such as "MESA: error: amdgpu: failed to +allocate ... from the 32-bit address space" point to the OpenGL/Mesa rendering +stack rather than the calibration data buffer. + +Save calibration once the fit is stable, then restart MotionCal for another run. +Avoid running multiple OpenGL-heavy viewers at the same time if this reproduces. +For troubleshooting, try software rendering: + +LIBGL_ALWAYS_SOFTWARE=1 GDK_BACKEND=x11 ./MotionCal --port /tmp/ttyMotionCal +``` + +## Repository Notes + +The upstream project did not include a README at the time this fork was created. This document records the local build workflow and the T-Beam/QMC6310 calibration workflow used with this fork. + +Generated build outputs such as `MotionCal`, `imuread`, `*.o`, debug logs, and timestamped calibration captures are local artifacts unless intentionally committed for documentation. diff --git a/gui.cpp b/gui.cpp index f39f6a7..9e9ab5a 100644 --- a/gui.cpp +++ b/gui.cpp @@ -3,7 +3,31 @@ wxString port_name; +static wxArrayString requested_ports; static bool show_calibration_confirmed = false; +static bool calibration_saved = false; + +static wxArrayString available_port_list(void) +{ + wxArrayString list = serial_port_list(); + for (size_t i=0; i < requested_ports.GetCount(); i++) { + if (list.Index(requested_ports[i]) == wxNOT_FOUND) { + list.Add(requested_ports[i]); + } + } + list.Sort(); + return list; +} + +static int valid_mag_point_count(void) +{ + int i, count=0; + + for (i=0; i < MAGBUFFSIZE; i++) { + if (magcal.valid[i]) count++; + } + return count; +} wxBEGIN_EVENT_TABLE(MyCanvas, wxGLCanvas) @@ -90,7 +114,7 @@ MyFrame::MyFrame(wxWindow *parent, wxWindowID id, const wxString &title, menuBar = new wxMenuBar; menu = new wxMenu; - menu->Append(ID_SENDCAL_MENU, wxT("Send Calibration")); + menu->Append(ID_SENDCAL_MENU, wxT("Save Calibration")); m_sendcal_menu = menu; m_sendcal_menu->Enable(ID_SENDCAL_MENU, false); menu->Append(wxID_EXIT, wxT("Quit")); @@ -115,12 +139,13 @@ MyFrame::MyFrame(wxWindow *parent, wxWindowID id, const wxString &title, vsizer = new wxBoxSizer(wxVERTICAL); leftsizer->Add(vsizer, 0, wxALL, 8); + leftsizer->SetMinSize(wxSize(170, -1)); text = new wxStaticText(panel, wxID_ANY, "Port"); vsizer->Add(text, 0, wxTOP|wxBOTTOM, 4); m_port_list = new wxComboBox(panel, ID_PORTLIST, "", - wxDefaultPosition, wxDefaultSize, 0, NULL, wxCB_READONLY); + wxDefaultPosition, wxSize(140, -1), 0, NULL, wxCB_READONLY); + m_port_list->SetMinSize(wxSize(140, -1)); m_port_list->Append("(none)"); - m_port_list->Append(SAMPLE_PORT_NAME); // never seen, only for initial size m_port_list->SetSelection(0); vsizer->Add(m_port_list, 1, wxEXPAND, 0); @@ -130,7 +155,7 @@ MyFrame::MyFrame(wxWindow *parent, wxWindowID id, const wxString &title, m_button_clear = new wxButton(panel, ID_CLEAR_BUTTON, "Clear"); m_button_clear->Enable(false); vsizer->Add(m_button_clear, 1, wxEXPAND, 0); - m_button_sendcal = new wxButton(panel, ID_SENDCAL_BUTTON, "Send Cal"); + m_button_sendcal = new wxButton(panel, ID_SENDCAL_BUTTON, "Save Cal"); vsizer->Add(m_button_sendcal, 1, wxEXPAND, 0); m_button_sendcal->Enable(false); vsizer->AddSpacer(16); @@ -240,7 +265,7 @@ MyFrame::MyFrame(wxWindow *parent, wxWindowID id, const wxString &title, raw_data_reset(); //open_port(PORT); m_timer = new wxTimer(this, ID_TIMER); - m_timer->Start(14, wxTIMER_CONTINUOUS); + m_timer->Start(33, wxTIMER_CONTINUOUS); } void MyFrame::OnTimer(wxTimerEvent &event) @@ -248,42 +273,48 @@ void MyFrame::OnTimer(wxTimerEvent &event) static int firstrun=1; float gaps, variance, wobble, fiterror; char buf[32]; - int i, j; + int i, j, bytes_read, saveable; //printf("OnTimer\n"); if (port_is_open()) { - read_serial_data(); + bytes_read = read_serial_data(); if (firstrun && m_canvas->IsShown()) { //int h, w; //m_canvas->GetSize(&w, &h); //printf("Canvas initial size = %d, %d\n", w, h); firstrun = 0; } - m_canvas->Refresh(); - gaps = quality_surface_gap_error(); - variance = quality_magnitude_variance_error(); - wobble = quality_wobble_error(); - fiterror = quality_spherical_fit_error(); - if (gaps < 15.0f && variance < 4.5f && wobble < 4.0f && fiterror < 5.0f) { - if (!m_sendcal_menu->IsEnabled(ID_SENDCAL_MENU) || !m_button_sendcal->IsEnabled()) { - m_sendcal_menu->Enable(ID_SENDCAL_MENU, true); - m_button_sendcal->Enable(true); + if (bytes_read > 0) { + m_canvas->Refresh(false); + } + gaps = quality_surface_gap_error(); + variance = quality_magnitude_variance_error(); + wobble = quality_wobble_error(); + fiterror = quality_spherical_fit_error(); + saveable = (magcal.ValidMagCal > 0 && + valid_mag_point_count() >= 120 && + fiterror < 20.0f); + if (m_sendcal_menu->IsEnabled(ID_SENDCAL_MENU) != saveable) { + m_sendcal_menu->Enable(ID_SENDCAL_MENU, saveable); + } + if (m_button_sendcal->IsEnabled() != saveable) { + m_button_sendcal->Enable(saveable); + } + if (!saveable) { + calibration_saved = false; + m_confirm_icon->SetBitmap(MyBitmap("checkemptygray.png")); + } else if (calibration_saved) { + m_confirm_icon->SetBitmap(MyBitmap("checkgreen.png")); + } else { m_confirm_icon->SetBitmap(MyBitmap("checkempty.png")); } - } else if (gaps > 20.0f && variance > 5.0f && wobble > 5.0f && fiterror > 6.0f) { - if (m_sendcal_menu->IsEnabled(ID_SENDCAL_MENU) || m_button_sendcal->IsEnabled()) { - m_sendcal_menu->Enable(ID_SENDCAL_MENU, false); - m_button_sendcal->Enable(false); - m_confirm_icon->SetBitmap(MyBitmap("checkemptygray.png")); - } - } - snprintf(buf, sizeof(buf), "%.1f%%", quality_surface_gap_error()); - m_err_coverage->SetLabelText(buf); - snprintf(buf, sizeof(buf), "%.1f%%", quality_magnitude_variance_error()); - m_err_variance->SetLabelText(buf); - snprintf(buf, sizeof(buf), "%.1f%%", quality_wobble_error()); - m_err_wobble->SetLabelText(buf); - snprintf(buf, sizeof(buf), "%.1f%%", quality_spherical_fit_error()); + snprintf(buf, sizeof(buf), "%.1f%%", gaps); + m_err_coverage->SetLabelText(buf); + snprintf(buf, sizeof(buf), "%.1f%%", variance); + m_err_variance->SetLabelText(buf); + snprintf(buf, sizeof(buf), "%.1f%%", wobble); + m_err_wobble->SetLabelText(buf); + snprintf(buf, sizeof(buf), "%.1f%%", fiterror); m_err_fit->SetLabelText(buf); for (i=0; i < 3; i++) { snprintf(buf, sizeof(buf), "%.2f", magcal.V[i]); @@ -311,7 +342,8 @@ void MyFrame::OnTimer(wxTimerEvent &event) m_sendcal_menu->Enable(ID_SENDCAL_MENU, false); m_button_clear->Enable(false); m_button_sendcal->Enable(false); - m_confirm_icon->SetBitmap(MyBitmap("checkemptygray.png")); + m_confirm_icon->SetBitmap(MyBitmap("checkemptygray.png")); + calibration_saved = false; m_port_list->Clear(); m_port_list->Append("(none)"); m_port_list->SetSelection(0); @@ -328,21 +360,23 @@ void MyFrame::OnClear(wxCommandEvent &event) { //printf("OnClear\n"); raw_data_reset(); + calibration_saved = false; + m_confirm_icon->SetBitmap(MyBitmap("checkemptygray.png")); } void MyFrame::OnSendCal(wxCommandEvent &event) { - /*printf("OnSendCal\n"); - printf("Magnetic Calibration: (%.1f%% fit error)\n", magcal.FitError); - printf(" %7.2f %6.3f %6.3f %6.3f\n", - magcal.V[0], magcal.invW[0][0], magcal.invW[0][1], magcal.invW[0][2]); - printf(" %7.2f %6.3f %6.3f %6.3f\n", - magcal.V[1], magcal.invW[1][0], magcal.invW[1][1], magcal.invW[1][2]); - printf(" %7.2f %6.3f %6.3f %6.3f\n", - magcal.V[2], magcal.invW[2][0], magcal.invW[2][1], magcal.invW[2][2]); - */ - m_confirm_icon->SetBitmap(MyBitmap("checkempty.png")); - send_calibration(); + char filename[128]; + + if (save_calibration(filename, sizeof(filename))) { + calibration_saved = true; + m_confirm_icon->SetBitmap(MyBitmap("checkgreen.png")); + fprintf(stderr, "MotionCal saved calibration to %s\n", filename); + } else { + calibration_saved = false; + m_confirm_icon->SetBitmap(MyBitmap("checkemptygray.png")); + fprintf(stderr, "MotionCal failed to save calibration\n"); + } } void calibration_confirmed(void) @@ -362,7 +396,7 @@ void MyFrame::OnShowMenu(wxMenuEvent &event) menu->AppendRadioItem(9000, " (none)"); bool isopen = port_is_open(); if (!isopen) menu->Check(9000, true); - wxArrayString list = serial_port_list(); + wxArrayString list = available_port_list(); int num = list.GetCount(); for (int i=0; i < num; i++) { menu->AppendRadioItem(9001 + i, list[i]); @@ -378,7 +412,7 @@ void MyFrame::OnShowPortList(wxCommandEvent& event) //printf("OnShowPortList\n"); m_port_list->Clear(); m_port_list->Append("(none)"); - wxArrayString list = serial_port_list(); + wxArrayString list = available_port_list(); int num = list.GetCount(); for (int i=0; i < num; i++) { m_port_list->Append(list[i]); @@ -392,6 +426,7 @@ void MyFrame::OnPortMenu(wxCommandEvent &event) wxString name = m_port_menu->FindItem(id)->GetItemLabelText(); close_port(); + calibration_saved = false; //printf("OnPortMenu, id = %d, name = %s\n", id, (const char *)name); port_name = name; m_port_list->Clear(); @@ -410,6 +445,7 @@ void MyFrame::OnPortList(wxCommandEvent& event) wxString name = m_port_list->GetString(selected); //printf("OnPortList, %s\n", (const char *)name); close_port(); + calibration_saved = false; port_name = name; if (name == "(none)") return; raw_data_reset(); @@ -457,6 +493,19 @@ bool MyApp::OnInit() // make sure we exit properly on macosx SetExitOnFrameDelete(true); + for (int i=1; i < argc; i++) { + wxString arg(argv[i]); + if ((arg == "--port" || arg == "-p") && i + 1 < argc) { + wxString requested(argv[++i]); + if (!requested.IsEmpty() && requested_ports.Index(requested) == wxNOT_FOUND) { + requested_ports.Add(requested); + } + } else if (arg == "--help" || arg == "-h") { + fprintf(stderr, "Usage: MotionCal [--port /path/to/tty]\n"); + return false; + } + } + wxPoint pos(100, 100); MyFrame *frame = new MyFrame(NULL, -1, "Motion Sensor Calibration Tool", @@ -472,7 +521,3 @@ int MyApp::OnExit() { return 0; } - - - - diff --git a/img/MotionCal_2026-04-25_11-22.png b/img/MotionCal_2026-04-25_11-22.png new file mode 100644 index 0000000..3c28097 Binary files /dev/null and b/img/MotionCal_2026-04-25_11-22.png differ diff --git a/imuread.h b/imuread.h index 9be6ce1..f390e9b 100644 --- a/imuread.h +++ b/imuread.h @@ -72,6 +72,7 @@ void cal2_data(const float *data); void calibration_confirmed(void); void raw_data(const int16_t *data); int send_calibration(void); +int save_calibration(char *filename, size_t filename_size); void visualize_init(void); void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out); void display_callback(void); diff --git a/portlist.cpp b/portlist.cpp index be16b5f..77b99c8 100644 --- a/portlist.cpp +++ b/portlist.cpp @@ -49,6 +49,7 @@ // is a long list, but each entry takes only a few bytes and a quick strcmp() static const char *devnames[] = { "S", // "normal" Serial Ports - MANY drivers using this +"t", // local udev symlinks, /dev/ttyt* "USB", // USB to serial converters "ACM", // USB serial modem, CDC class, Abstract Control Model "MI", // MOXA Smartio/Industio family multiport serial... nice card, I have one :-) @@ -289,5 +290,3 @@ wxArrayString serial_port_list() - - diff --git a/rawdata.c b/rawdata.c index caaf063..b5af728 100644 --- a/rawdata.c +++ b/rawdata.c @@ -1,4 +1,5 @@ #include "imuread.h" +#include static int rawcount=OVERSAMPLE_RATIO; @@ -351,3 +352,63 @@ int send_calibration(void) return write_serial_data(buf, 68); } +int save_calibration(char *filename, size_t filename_size) +{ + FILE *fp; + time_t now; + struct tm *tm; + int i, j, valid_count=0; + + if (filename == NULL || filename_size == 0) return 0; + now = time(NULL); + tm = localtime(&now); + if (tm == NULL) return 0; + if (strftime(filename, filename_size, "MotionCal_settings_%Y%m%d_%H%M%S.txt", tm) == 0) return 0; + + fp = fopen(filename, "w"); + if (fp == NULL) return 0; + + for (i=0; i < MAGBUFFSIZE; i++) { + if (magcal.valid[i]) valid_count++; + } + + fprintf(fp, "MotionCal magnetic calibration settings\n"); + fprintf(fp, "saved_local_time=%04d-%02d-%02d %02d:%02d:%02d\n", + tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday, + tm->tm_hour, tm->tm_min, tm->tm_sec); + fprintf(fp, "valid_points=%d\n", valid_count); + fprintf(fp, "fit_error_percent=%.6f\n", quality_spherical_fit_error()); + fprintf(fp, "surface_gap_error_percent=%.6f\n", quality_surface_gap_error()); + fprintf(fp, "magnitude_variance_error_percent=%.6f\n", quality_magnitude_variance_error()); + fprintf(fp, "wobble_error_percent=%.6f\n", quality_wobble_error()); + fprintf(fp, "\n"); + + fprintf(fp, "magnetic_offset_uT=%.9g,%.9g,%.9g\n", + magcal.V[0], magcal.V[1], magcal.V[2]); + fprintf(fp, "magnetic_field_uT=%.9g\n", magcal.B); + fprintf(fp, "magnetic_mapping_matrix=\n"); + for (i=0; i < 3; i++) { + fprintf(fp, " %.9g %.9g %.9g\n", + magcal.invW[i][0], magcal.invW[i][1], magcal.invW[i][2]); + } + fprintf(fp, "\n"); + + fprintf(fp, "cal1_echo_line=Cal1:0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,%.6f,%.6f,%.6f,%.6f\n", + magcal.V[0], magcal.V[1], magcal.V[2], magcal.B); + fprintf(fp, "cal2_echo_line=Cal2:%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\n", + magcal.invW[0][0], magcal.invW[0][1], magcal.invW[0][2], + magcal.invW[1][0], magcal.invW[1][1], magcal.invW[1][2], + magcal.invW[2][0], magcal.invW[2][1], magcal.invW[2][2]); + fprintf(fp, "\n"); + + fprintf(fp, "raw_points=count,x,y,z\n"); + for (i=0, j=0; i < MAGBUFFSIZE; i++) { + if (magcal.valid[i]) { + fprintf(fp, "%d,%d,%d,%d\n", j++, + magcal.BpFast[0][i], magcal.BpFast[1][i], magcal.BpFast[2][i]); + } + } + + if (fclose(fp) != 0) return 0; + return 1; +} diff --git a/serialdata.c b/serialdata.c index bdd2695..1ac65ba 100644 --- a/serialdata.c +++ b/serialdata.c @@ -1,5 +1,44 @@ #include "imuread.h" +#ifndef MOTIONCAL_DEBUG +#define MOTIONCAL_DEBUG 0 +#endif + +#ifndef MOTIONCAL_DEBUG_BYTES +#define MOTIONCAL_DEBUG_BYTES 0 +#endif + +#if MOTIONCAL_DEBUG +#define DEBUG_PRINTF(...) fprintf(stderr, __VA_ARGS__) +#else +#define DEBUG_PRINTF(...) do { } while (0) +#endif + +static void debug_ascii_preview(const unsigned char *data, int len) +{ +#if MOTIONCAL_DEBUG_BYTES + int i; + + fprintf(stderr, "MotionCal read %d bytes: ", len); + for (i = 0; i < len && i < 96; i++) { + unsigned char c = data[i]; + if (c == '\r') { + fprintf(stderr, "\\r"); + } else if (c == '\n') { + fprintf(stderr, "\\n"); + } else if (isprint(c)) { + fputc(c, stderr); + } else { + fprintf(stderr, "\\x%02X", c); + } + } + if (len > 96) fprintf(stderr, "..."); + fprintf(stderr, "\n"); +#else + (void)data; + (void)len; +#endif +} void print_data(const char *name, const unsigned char *data, int len) { @@ -265,7 +304,7 @@ static int ascii_parse(const unsigned char *data, int len) } else if (*p == ',') { //printf("ascii_parse comma, %d\n", ascii_num); if (ascii_neg) ascii_num = -ascii_num; - if (ascii_num < -32768 && ascii_num > 32767) goto fail; + if (ascii_num < -32768 || ascii_num > 32767) goto fail; if (ascii_raw_data_count >= 8) goto fail; ascii_raw_data[ascii_raw_data_count++] = ascii_num; ascii_num = 0; @@ -274,10 +313,14 @@ static int ascii_parse(const unsigned char *data, int len) } else if (*p == 13) { //printf("ascii_parse newline\n"); if (ascii_neg) ascii_num = -ascii_num; - if (ascii_num < -32768 && ascii_num > 32767) goto fail; + if (ascii_num < -32768 || ascii_num > 32767) goto fail; if (ascii_raw_data_count != 8) goto fail; ascii_raw_data[ascii_raw_data_count] = ascii_num; raw_data(ascii_raw_data); + DEBUG_PRINTF("MotionCal Raw parsed: %d,%d,%d,%d,%d,%d,%d,%d,%d\n", + ascii_raw_data[0], ascii_raw_data[1], ascii_raw_data[2], + ascii_raw_data[3], ascii_raw_data[4], ascii_raw_data[5], + ascii_raw_data[6], ascii_raw_data[7], ascii_raw_data[8]); ret = 1; ascii_raw_data_count = 0; ascii_num = 0; @@ -340,7 +383,7 @@ static int ascii_parse(const unsigned char *data, int len) } return ret; fail: - //printf("ascii FAIL\n"); + DEBUG_PRINTF("MotionCal ascii parser reset\n"); ascii_state = ASCII_STATE_WORD; ascii_raw_data_count = 0; ascii_num = 0; @@ -375,9 +418,13 @@ int open_port(const char *name) int r; portfd = open(name, O_RDWR | O_NONBLOCK); - if (portfd < 0) return 0; + if (portfd < 0) { + DEBUG_PRINTF("MotionCal open failed for %s: %s\n", name, strerror(errno)); + return 0; + } r = tcgetattr(portfd, &termsettings); if (r < 0) { + DEBUG_PRINTF("MotionCal tcgetattr failed for %s: %s\n", name, strerror(errno)); close_port(); return 0; } @@ -385,9 +432,11 @@ int open_port(const char *name) cfsetspeed(&termsettings, B115200); r = tcsetattr(portfd, TCSANOW, &termsettings); if (r < 0) { + DEBUG_PRINTF("MotionCal tcsetattr failed for %s: %s\n", name, strerror(errno)); close_port(); return 0; } + DEBUG_PRINTF("MotionCal opened %s at 115200\n", name); return 1; } @@ -401,6 +450,7 @@ int read_serial_data(void) while (1) { n = read(portfd, buf, sizeof(buf)); if (n > 0 && n <= sizeof(buf)) { + debug_ascii_preview(buf, n); newdata(buf, n); nodata_count = 0; return n;