250 lines
6.9 KiB
C++
250 lines
6.9 KiB
C++
#include <cstring>
|
|
#include <stdarg.h>
|
|
#include <thread>
|
|
#include <iostream>
|
|
#include <sstream>
|
|
|
|
#include "LpmsIG1I.h"
|
|
#include "SensorDataI.h"
|
|
#include "LpmsIG1Registers.h"
|
|
|
|
using namespace std;
|
|
string TAG("MAIN");
|
|
IG1I* sensor1;
|
|
// Start print data thread
|
|
std::thread *printThread;
|
|
static bool printThreadIsRunning = false;
|
|
|
|
void logd(std::string tag, const char* str, ...)
|
|
{
|
|
va_list a_list;
|
|
va_start(a_list, str);
|
|
if (!tag.empty())
|
|
printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
|
|
vprintf(str, a_list);
|
|
va_end(a_list);
|
|
}
|
|
|
|
void printTask()
|
|
{
|
|
if (sensor1->getStatus() != STATUS_CONNECTED)
|
|
{
|
|
printThreadIsRunning = false;
|
|
logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
|
|
logd(TAG, "printTask terminated\n");
|
|
return;
|
|
}
|
|
|
|
sensor1->commandGotoStreamingMode();
|
|
|
|
printThreadIsRunning = true;
|
|
while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
|
|
{
|
|
IG1ImuDataI sd;
|
|
if (sensor1->hasImuData())
|
|
{
|
|
sensor1->getImuData(sd);
|
|
float freq = sensor1->getDataFrequency();
|
|
logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n",
|
|
sd.timestamp*0.002f,
|
|
sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
|
|
sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
|
|
sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],
|
|
freq);
|
|
}
|
|
this_thread::sleep_for(chrono::milliseconds(2));
|
|
}
|
|
|
|
printThreadIsRunning = false;
|
|
logd(TAG, "printTask terminated\n");
|
|
}
|
|
|
|
void printMenu()
|
|
{
|
|
cout << endl;
|
|
cout << "===================" << endl;
|
|
cout << "Main Menu" << endl;
|
|
cout << "===================" << endl;
|
|
cout << "[c] Connect sensor" << endl;
|
|
cout << "[d] Disconnect sensor" << endl;
|
|
cout << "[0] Goto command mode" << endl;
|
|
cout << "[1] Goto streaming mode" << endl;
|
|
cout << "[h] Reset sensor heading" << endl;
|
|
cout << "[i] Print sensor info" << endl;
|
|
cout << "[s] Print sensor settings" << endl;
|
|
cout << "[p] Print sensor data" << endl;
|
|
cout << "[a] Toggle enable/disable auto reconnect" << endl;
|
|
cout << "[q] quit" << endl;
|
|
cout << endl;
|
|
}
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
string comportNo = "/dev/ttyTHS2";
|
|
int baudrate = 115200;
|
|
|
|
if (argc == 2)
|
|
{
|
|
comportNo = string(argv[1]);
|
|
}
|
|
else if (argc == 3)
|
|
{
|
|
comportNo = string(argv[1]);
|
|
baudrate = atoi(argv[2]);
|
|
}
|
|
|
|
logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
|
|
|
|
// Create LpmsIG1 object with corresponding comport and baudrate
|
|
sensor1 = IG1Factory();
|
|
|
|
// Set verbosity to INFO level to output info messages
|
|
// Use VERBOSE_NONE to turn off all messages from library
|
|
// Use VERBOSE_DEBUG to output debug messages
|
|
sensor1->setVerbose(VERBOSE_INFO);
|
|
|
|
// Enable auto reconnect
|
|
sensor1->setAutoReconnectStatus(true);
|
|
|
|
// Set connection interface to RS485
|
|
sensor1->setConnectionInterface(CONNECTION_INTERFACE_RS485);
|
|
|
|
// Set GPIO pin toggle wait time to 2ms
|
|
sensor1->setControlGPIOToggleWaitMs(2);
|
|
|
|
// Use pin 388 as RS485 flow control pin
|
|
sensor1->setControlGPIOForRs485(388);
|
|
|
|
// Connects to sensor
|
|
if (!sensor1->connect(comportNo, baudrate))
|
|
{
|
|
sensor1->release();
|
|
logd(TAG, "Error connecting to sensor\n");
|
|
logd(TAG, "bye\n");
|
|
return 1;
|
|
}
|
|
|
|
do
|
|
{
|
|
logd(TAG, "Waiting for sensor to connect\r\n");
|
|
this_thread::sleep_for(chrono::milliseconds(1000));
|
|
} while (
|
|
!(sensor1->getStatus() == STATUS_CONNECTED) &&
|
|
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
|
|
);
|
|
|
|
if (sensor1->getStatus() != STATUS_CONNECTED)
|
|
{
|
|
logd(TAG, "Sensor connection error: %d\n", sensor1->getStatus());
|
|
sensor1->release();
|
|
logd(TAG, "bye\n");
|
|
return 1;
|
|
}
|
|
|
|
|
|
logd(TAG, "Sensor connected\n");
|
|
printMenu();
|
|
|
|
bool quit = false;
|
|
while (!quit)
|
|
{
|
|
char cmd = '\0';
|
|
string input;
|
|
getline(cin, input);
|
|
|
|
if (!input.empty())
|
|
cmd = input.at(0);
|
|
|
|
switch (cmd)
|
|
{
|
|
case 'c':
|
|
logd(TAG, "Connect sensor\n");
|
|
if (!sensor1->connect(comportNo, baudrate))
|
|
logd(TAG, "Error connecting to sensor\n");
|
|
break;
|
|
|
|
case 'd':
|
|
logd(TAG, "Disconnect sensor\n");
|
|
sensor1->disconnect();
|
|
break;
|
|
|
|
case '0':
|
|
logd(TAG, "Goto command mode\n");
|
|
sensor1->commandGotoCommandMode();
|
|
break;
|
|
|
|
case '1':
|
|
logd(TAG, "Goto streaming mode\n");
|
|
sensor1->commandGotoStreamingMode();
|
|
break;
|
|
|
|
case 'h':
|
|
logd(TAG, "Reset heading\n");
|
|
// reset sensor heading
|
|
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
|
|
break;
|
|
|
|
case 'i': {
|
|
logd(TAG, "Print sensor info\n");
|
|
// Print sensor info
|
|
IG1InfoI info;
|
|
sensor1->getInfo(info);
|
|
cout << info.toString() << endl;
|
|
break;
|
|
}
|
|
|
|
case 's': {
|
|
logd(TAG, "Print sensor settings\n");
|
|
// Print sensor settings
|
|
IG1SettingsI settings;
|
|
sensor1->getSettings(settings);
|
|
cout << settings.toString() << endl;
|
|
break;
|
|
}
|
|
|
|
case 'p': {
|
|
logd(TAG, "Print sensor data\n");
|
|
// Print sensor data
|
|
if (!printThreadIsRunning)
|
|
printThread = new std::thread(printTask);
|
|
break;
|
|
}
|
|
|
|
case 'a': {
|
|
logd(TAG, "Toggle auto reconnect\n");
|
|
bool b = sensor1->getAutoReconnectStatus();
|
|
sensor1->setAutoReconnectStatus(!b);
|
|
logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
|
|
break;
|
|
}
|
|
|
|
case 'q':
|
|
logd(TAG, "quit\n");
|
|
// disconnect sensor
|
|
sensor1->disconnect();
|
|
quit = true;
|
|
break;
|
|
|
|
default:
|
|
printThreadIsRunning = false;
|
|
if (printThread && printThread->joinable()) {
|
|
printThread->join();
|
|
printThread = NULL;
|
|
}
|
|
printMenu();
|
|
break;
|
|
}
|
|
this_thread::sleep_for(chrono::milliseconds(100));
|
|
}
|
|
|
|
|
|
if (printThread && printThread->joinable())
|
|
printThread->join();
|
|
|
|
// release sensor resources
|
|
sensor1->release();
|
|
logd(TAG, "Bye\n");
|
|
|
|
return 0;
|
|
}
|