Skip to content

Commit

Permalink
Version bump to 0.4.3-alpha
Browse files Browse the repository at this point in the history 
	- Added basic support for WT901 sensor, tested
	- Added controller application for WT901 sensor
	- Added unblock configuration packet ID to types.h
	- Validated controller application for WT901 sensor
	- TODO: Bias setup for sensor from series 9
	- To be mentioned in #2: @tohax сенсор WT901 протестирован на
	  частотах до 200 Гц, фактическая частота отдачи показаний
	  составляет около 180 Гц, так как устанавливается частота
	  выдачи пакетов на UART, а пакеты отдаются последовательно.
  • Loading branch information
@twdragon
twdragon committed Jul 28, 2022
1 parent 9452621 commit d4d4930
Show file tree
Hide file tree
Showing 6 changed files with 264 additions and 4 deletions.
4 changes: 3 additions & 1 deletion include/witmotion/types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,9 @@ enum witmotion_config_register_id
ridSetOrientationZ = 0x53,
ridSetOrientationW = 0x54,

ridGyroscopeAutoCalibrate = 0x63
ridGyroscopeAutoCalibrate = 0x63,

ridUnlockConfiguration = 0x69
};

/*!
Expand Down
1 change: 1 addition & 0 deletions include/witmotion/wt901-uart.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ class QWitmotionWT901Sensor: public QAbstractWitmotionSensorController
public:
virtual const std::set<witmotion_packet_id>* RegisteredPacketTypes();
virtual void Start();
virtual void UnlockConfiguration();
virtual void Calibrate();
virtual void SetBaudRate(const QSerialPort::BaudRate& rate);
virtual void SetPollingRate(const int32_t hz);
Expand Down
4 changes: 4 additions & 0 deletions src/wt31n-control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -233,6 +233,10 @@ int main(int argc, char** args)
std::cout << "Waiting for the first packet acquired..." << std::endl;
int result = app.exec();

std::cout << "Average sensor return rate "
<< std::accumulate(times.begin(), times.end(), 0.f) / times.size()
<< " s" << std::endl << std::endl;

if(covariance)
{
std::cout << "Calculating noise covariance matrices..." << std::endl
Expand Down
235 changes: 233 additions & 2 deletions src/wt901-control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ void handle_shutdown(int s)

int main(int argc, char** args)
{
bool maintenance = false;

struct sigaction sigIntHandler;
sigIntHandler.sa_handler = handle_shutdown;
sigemptyset(&sigIntHandler.sa_mask);
Expand Down Expand Up @@ -104,11 +106,109 @@ int main(int argc, char** args)
QCoreApplication::exit(1);
});

std::vector<float> accels_x, accels_y, accels_z, vels_x, vels_y, vels_z, rolls, pitches, yaws, temps, times, mags_x, mags_y, mags_z;
QObject::connect(&sensor, &QWitmotionWT901Sensor::Acquired,
[maintenance,
&accels_x,
&accels_y,
&accels_z,
&vels_x,
&vels_y,
&vels_z,
&rolls,
&pitches,
&yaws,
&temps,
&times,
&mags_x,
&mags_y,
&mags_z](const witmotion::witmotion_datapacket& packet)
{
if(maintenance)
return;

std::cout.precision(5);
std::cout << std::fixed;

float ax, ay, az, wx, wy, wz, roll, pitch, yaw, t, mx, my, mz;
static size_t packets = 1;
static auto time_start = std::chrono::system_clock::now();
auto time_acquisition = std::chrono::system_clock::now();
std::chrono::duration<float> elapsed_seconds = time_acquisition - time_start;
switch (static_cast<witmotion::witmotion_packet_id>(packet.id_byte))
{
case witmotion::pidAcceleration:
witmotion::decode_accelerations(packet, ax, ay, az, t);
accels_x.push_back(ax);
accels_y.push_back(ay);
accels_z.push_back(az);
std::cout << packets << "\t"
<< "Accelerations [X|Y|Z]:\t[ "
<< ax << " | "
<< ay << " | "
<< az << " ], temp "
<< t << " degrees,"
<< " in " << elapsed_seconds.count() << " s"
<< std::endl;
break;
case witmotion::pidAngularVelocity:
witmotion::decode_angular_velocities(packet, wx, wy, wz, t);
vels_x.push_back(wx);
vels_y.push_back(wy);
vels_z.push_back(wz);
std::cout << packets << "\t"
<< "Angular velocities [X|Y|Z]:\t[ "
<< wx << " | "
<< wy << " | "
<< wz << " ], temp "
<< t << " degrees,"
<< " in " << elapsed_seconds.count() << " s"
<< std::endl;
break;
case witmotion::pidAngles:
witmotion::decode_angles(packet, roll, pitch, yaw, t);
rolls.push_back(roll);
pitches.push_back(pitch);
yaws.push_back(yaw);
std::cout << packets << "\t"
<< "Euler angles [R|P|Y]:\t[ "
<< roll << " | "
<< pitch << " | "
<< yaw << " ], temp "
<< t << " degrees, "
<< " in " << elapsed_seconds.count() << " s"
<< std::endl;
break;
case witmotion::pidMagnetometer:
witmotion::decode_magnetometer(packet, mx, my ,mz, t);
mags_x.push_back(mx);
mags_y.push_back(my);
mags_z.push_back(mz);
temps.push_back(t);
std::cout << packets << "\t"
<< "Magnetic field [X|Y|Z]:\t[ "
<< mx << " | "
<< my << " | "
<< mz << " ], temp "
<< t << " degrees, "
<< " in " << elapsed_seconds.count() << " s"
<< std::endl;
break;
default:
break;
}
times.push_back(elapsed_seconds.count());

packets++;
time_start = time_acquisition;
});


// Start acquisition
sensor.Start();

// Rendering control packets
maintenance = true;
sleep(1);
if(parser.isSet(CalibrateOption))
{
Expand All @@ -124,6 +224,7 @@ int main(int argc, char** args)
sleep(1);
}
std::cout << std::endl << "Calibrating..." << std::endl;
sensor.UnlockConfiguration();
sensor.Calibrate();
sensor.ConfirmConfiguration();
sleep(1);
Expand All @@ -144,10 +245,10 @@ int main(int argc, char** args)
std::cout << "ERROR: Wrong baudrate setting (use --help for detailed information). Ignoring baudrate reconfiguration request." << std::endl;
else
{
std::cout << "Configuring baudrate. NOTE: Please reconnect the sensor after this operation with the proper baudrate setting!" << std::endl;
std::cout << "Configuring baudrate for " << new_rate << " baud. NOTE: Please reconnect the sensor after this operation with the proper baudrate setting!" << std::endl;
sensor.UnlockConfiguration();
sensor.SetBaudRate(static_cast<QSerialPort::BaudRate>(new_rate));
sensor.ConfirmConfiguration();
sleep(1);
std::cout << "Reconfiguration completed. Please reconnect now" << std::endl;
std::exit(0);
}
Expand All @@ -173,15 +274,145 @@ int main(int argc, char** args)
else
{
std::cout << "Configuring output frequency. NOTE: Please reconnect the sensor after this operation with the proper setting!" << std::endl;
sensor.UnlockConfiguration();
sensor.SetPollingRate(new_poll);
sensor.ConfirmConfiguration();
sleep(1);
std::cout << "Reconfiguration completed. Please reconnect now" << std::endl;
std::exit(0);
}
}
maintenance = false;

int result = app.exec();

std::cout << "Average sensor return rate "
<< std::accumulate(times.begin(), times.end(), 0.f) / times.size()
<< " s" << std::endl << std::endl;

if(parser.isSet(CovarianceOption))
{
std::cout << "Calculating noise covariance matrices..." << std::endl
<< std::endl
<< "Accelerations (total for " << accels_x.size() << " measurements): " << std::endl
<< "[\t" << variance(accels_x) << "\t0.00000\t0.00000" << std::endl
<< "\t0.00000\t" << variance(accels_y) << "\t0.00000" << std::endl
<< "\t0.00000\t0.00000\t" << variance(accels_z) << "\t]" << std::endl
<< std::endl
<< "Angular velocities (total for " << vels_x.size() << " measurements): " << std::endl
<< "[\t" << variance(vels_x) << "\t0.00000\t0.00000" << std::endl
<< "\t0.00000\t" << variance(vels_y) << "\t0.00000" << std::endl
<< "\t0.00000\t0.00000\t" << variance(vels_z) << "\t]" << std::endl
<< std::endl
<< "Angles (total for " << pitches.size() << " measurements): " << std::endl
<< "[\t" << variance(rolls) << "\t0.00000\t0.00000" << std::endl
<< "\t0.00000\t" << variance(pitches) << "\t0.00000" << std::endl
<< "\t0.00000\t0.00000\t" << variance(yaws) << "\t]" << std::endl
<< std::endl
<< "Temperature (total for " << temps.size() << " measurements): " << std::endl
<< "[\t" << variance(temps) << "\t]" << std::endl
<< std::endl
<< "Magnetometer (total for " << mags_x.size() << " measurements): " << std::endl
<< "[\t" << variance(mags_x) << "\t00.00000\t00.00000" << std::endl
<< "\t00.00000\t" << variance(mags_y) << "\t00.00000" << std::endl
<< "\t00.00000\t00.00000\t" << variance(mags_z) << "\t]" << std::endl
<< std::endl;
}

if(parser.isSet(LogOption))
{
std::cout << "Writing log file to sensor.log" << std::endl;
std::fstream logfile;
logfile.open("sensor.log", std::ios::out|std::ios::trunc);
logfile.precision(5);
logfile << std::fixed;
logfile << "WITMOTION WT901 STANDALONE SENSOR CONTROLLER/MONITOR" << std::endl << std::endl;
auto time_start = std::chrono::system_clock::now();
std::time_t timestamp_start = std::chrono::system_clock::to_time_t(time_start);
logfile << "Device /dev/" << device.toStdString() << " opened at " << static_cast<int32_t>(rate) << " baud" << std::endl;
if(!accels_x.empty())
{
logfile << std::endl << "Spatial measurements (only full packets logged):" << std::endl;
for(size_t i = 0; i < std::min(accels_x.size(),
std::min(vels_x.size(), rolls.size()) ); i++)
{
logfile << i + 1 << "\t"
<< "Accelerations [X|Y|Z]:\t[ "
<< accels_x[i] << " | "
<< accels_y[i] << " | "
<< accels_z[i] << " ]"
<< std::endl;
logfile << i + 1 << "\t"
<< "Angular velocities [X|Y|Z]:\t[ "
<< vels_x[i] << " | "
<< vels_y[i] << " | "
<< vels_z[i] << " ]"
<< std::endl;
logfile << "\t"
<< "Euler angles [R|P|Y]:\t[ "
<< rolls[i] << " | "
<< pitches[i] << " | "
<< yaws[i] << " ]"
<< std::endl;
}
}
logfile << std::endl;
if(!mags_x.empty())
{
logfile << std::endl << "Magnetic measurements:" << std::endl;
for(size_t i = 0; i < mags_x.size(); i++)
{
logfile << i + 1 << "\t"
<< "Accelerations [X|Y|Z]:\t[ "
<< mags_x[i] << " | "
<< mags_y[i] << " | "
<< mags_z[i] << " ]"
<< std::endl;
}
logfile << std::endl;
logfile << std::endl << "Temperature measurements:" << std::endl;
for(size_t i = 0; i < mags_x.size(); i++)
logfile << i + 1 << "\t" << temps[i] << std::endl;
}
logfile << std::endl
<< "Acquired "
<< std::min(accels_x.size(),
std::min(vels_x.size(), rolls.size()) )
<< " measurements, average reading time "
<< std::accumulate(times.begin(), times.end(), 0.f) / times.size()
<< " s"
<< std::endl;
if(parser.isSet(CovarianceOption))
{
logfile << "Calculating noise covariance matrices..." << std::endl
<< std::endl
<< "Accelerations (total for " << accels_x.size() << " measurements): " << std::endl
<< "[\t" << variance(accels_x) << "\t0.00000\t0.00000" << std::endl
<< "\t0.00000\t" << variance(accels_y) << "\t0.00000" << std::endl
<< "\t0.00000\t0.00000\t" << variance(accels_z) << "\t]" << std::endl
<< std::endl
<< "Angular velocities (total for " << vels_x.size() << " measurements): " << std::endl
<< "[\t" << variance(vels_x) << "\t0.00000\t0.00000" << std::endl
<< "\t0.00000\t" << variance(vels_y) << "\t0.00000" << std::endl
<< "\t0.00000\t0.00000\t" << variance(vels_z) << "\t]" << std::endl
<< std::endl
<< "Angles (total for " << pitches.size() << " measurements): " << std::endl
<< "[\t" << variance(rolls) << "\t0.00000\t0.00000" << std::endl
<< "\t0.00000\t" << variance(pitches) << "\t0.00000" << std::endl
<< "\t0.00000\t0.00000\t" << variance(yaws) << "\t]" << std::endl
<< std::endl
<< "Temperature (total for " << temps.size() << " measurements): " << std::endl
<< "[\t" << variance(temps) << "\t]" << std::endl
<< std::endl
<< "Magnetometer (total for " << mags_x.size() << " measurements): " << std::endl
<< "[\t" << variance(mags_x) << "\t00.00000\t00.00000" << std::endl
<< "\t00.00000\t" << variance(mags_y) << "\t00.00000" << std::endl
<< "\t00.00000\t00.00000\t" << variance(mags_z) << "\t]" << std::endl
<< std::endl;
}
logfile << "Acquisition performed at " << std::ctime(&timestamp_start) << std::endl;
logfile.close();
}

return result;
}
22 changes: 22 additions & 0 deletions src/wt901-uart.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,19 @@ const std::set<witmotion_packet_id> QWitmotionWT901Sensor::registered_types =
pidMagnetometer
};

void QWitmotionWT901Sensor::UnlockConfiguration()
{
witmotion_config_packet config_packet;
config_packet.header_byte = WITMOTION_CONFIG_HEADER;
config_packet.key_byte = WITMOTION_CONFIG_KEY;
config_packet.address_byte = ridUnlockConfiguration;
config_packet.setting.raw[0] = 0x88;
config_packet.setting.raw[1] = 0xB5;
ttyout << "Configuration ROM: lock removal started" << ENDL;
emit SendConfig(config_packet);
sleep(1);
}

const std::set<witmotion_packet_id> *QWitmotionWT901Sensor::RegisteredPacketTypes()
{
return &registered_types;
Expand All @@ -34,7 +47,13 @@ void QWitmotionWT901Sensor::Calibrate()
config_packet.address_byte = ridCalibrate;
config_packet.setting.raw[0] = 0x01;
config_packet.setting.raw[1] = 0x00;
ttyout << "Entering spatial calibration, please hold the sensor in fixed position for 5 seconds" << ENDL;
emit SendConfig(config_packet);
sleep(5);
config_packet.setting.raw[0] = 0x00;
ttyout << "Exiting spatial calibration mode" << ENDL;
emit SendConfig(config_packet);
sleep(1);
}

void QWitmotionWT901Sensor::SetBaudRate(const QSerialPort::BaudRate &rate)
Expand All @@ -47,6 +66,7 @@ void QWitmotionWT901Sensor::SetBaudRate(const QSerialPort::BaudRate &rate)
config_packet.setting.raw[0] = witmotion_baud_rate(port_rate);
config_packet.setting.raw[1] = 0x00;
emit SendConfig(config_packet);
sleep(1);
}

void QWitmotionWT901Sensor::SetPollingRate(const int32_t hz)
Expand All @@ -58,6 +78,7 @@ void QWitmotionWT901Sensor::SetPollingRate(const int32_t hz)
config_packet.setting.raw[0] = witmotion_output_frequency(hz);
config_packet.setting.raw[1] = 0x00;
emit SendConfig(config_packet);
sleep(1);
}

void QWitmotionWT901Sensor::ConfirmConfiguration()
Expand All @@ -69,6 +90,7 @@ void QWitmotionWT901Sensor::ConfirmConfiguration()
config_packet.setting.raw[0] = 0x00;
config_packet.setting.raw[1] = 0x00;
emit SendConfig(config_packet);
sleep(1);
}

QWitmotionWT901Sensor::QWitmotionWT901Sensor(const QString device,
Expand Down
2 changes: 1 addition & 1 deletion version
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
0.3.7-alpha
0.4.3-alpha

0 comments on commit d4d4930

Please sign in to comment.