at jp after adding images.cpp and trying to reconcile with eos which has later development, e.g. PORT handling
Merge branch 'master' of https://salemdata.net/repo/jlpoole/MotionCal
This commit is contained in:
commit
c8c903c141
8 changed files with 474 additions and 56 deletions
3
Makefile
3
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
|
||||
|
||||
|
|
|
|||
263
README.md
Normal file
263
README.md
Normal file
|
|
@ -0,0 +1,263 @@
|
|||
# MotionCal
|
||||
|
||||
|
||||
|
||||
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.
|
||||
141
gui.cpp
141
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
BIN
img/MotionCal_2026-04-25_11-22.png
Normal file
BIN
img/MotionCal_2026-04-25_11-22.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 148 KiB |
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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()
|
|||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
61
rawdata.c
61
rawdata.c
|
|
@ -1,4 +1,5 @@
|
|||
#include "imuread.h"
|
||||
#include <time.h>
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
58
serialdata.c
58
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;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue