Skip to content

Commit 0f66d42

Browse files
committed
Version bump to 0.9.17-alpha
- types.h partially documented - Added native support for RTC origin setup - Tested convenience delays for RTC origin setup
1 parent be7ebb7 commit 0f66d42

File tree

6 files changed

+127
-15
lines changed

6 files changed

+127
-15
lines changed

include/witmotion/types.h

+54-10
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,11 @@ enum witmotion_packet_id
5555
pidGPSAccuracy = 0x5A ///< GPS: visible satellites + variance vector [East-North-Up] (16-bit binary normalized quasi-floats)
5656
};
5757

58+
/*!
59+
\brief Packet ID set to retrieve descriptions via \ref witmotion_packet_descriptions.
60+
61+
Contains values referenced in \ref witmotion_packet_id enumeration to explicitly determine a set of currently supported packet IDs. The packet IDs not referenced here sould not be considered supported.
62+
*/
5863
static const std::set<size_t> witmotion_registered_ids = {
5964
0x50,
6065
0x51,
@@ -69,6 +74,11 @@ static const std::set<size_t> witmotion_registered_ids = {
6974
0x5A
7075
};
7176

77+
/*!
78+
\brief Packet ID string set to store built-in descriptions for \ref message_enumerator.
79+
80+
Contains values referenced in \ref witmotion_packet_id enumeration with corresponding description strings used by \ref message_enumerator application.
81+
*/
7282
static const std::map<uint8_t, std::string> witmotion_packet_descriptions = {
7383
{0x50, "Real Time Clock"},
7484
{0x51, "Accelerations"},
@@ -103,7 +113,8 @@ struct witmotion_datapacket
103113
/*!
104114
* \brief List of configuration slots (registers) available for the library.
105115
*
106-
* List of configuration slots (registers) available for the library. The actual availability depends from the actual sensor and installation circuit. Please refer to the official documentation for detailed explanation.
116+
* List of configuration slots (registers) available for the library. The actual availability depends from the actual sensor and installation circuit.
117+
* Please refer to the official documentation for detailed explanation. **NOTE**: values which are currently untested/unsupported by the certain sensors or known malfunction producers are marked here as following:
107118
*/
108119
enum witmotion_config_register_id
109120
{
@@ -116,18 +127,51 @@ enum witmotion_config_register_id
116127
- `0x03` - Altitude reset (only for barometric altimeter)
117128
*/
118129
ridCalibrate = 0x01,
130+
/*!
131+
Regulates sensor output. The value stored in \ref witmotion_config_packet.`raw` determines packet ID selection to output from low to high bits by offset. `0` means disabling of the selected data packet output.
132+
133+
|`raw[0]` offset|Packet type|`raw[1]` offset| Packet type|
134+
|:-------------:|----------:|:-------------:|-----------:|
135+
|0|\ref pidRTC|0|\ref pidGPSGroundSpeed|
136+
|1|\ref pidAcceleration|1|\ref pidOrientation|
137+
|2|\ref pidAngularVelocity|2|\ref pidGPSAccuracy|
138+
|3|\ref pidAngles|3|Reserved|
139+
|4|\ref pidMagnetometer|4|Reserved|
140+
|5|\ref pidDataPortStatus|5|Reserved|
141+
|6|\ref pidAltimeter|6|Reserved|
142+
|7|\ref pidGPSCoordinates|7|Reserved|
143+
*/
119144
ridOutputValueSet = 0x02,
145+
/*!
146+
Regulates output frequency. **NOTE**: the maximum available frequency is determined internally by the available bandwidth obtained from \ref ridPortBaudRate.
147+
The actual value stored in \ref witmotion_config_packet.`raw[0]` can be determined from the following table. \ref witmotion_config_packet.`raw[1]` is set to `0x00`. Also the table contains argument value for \ref witmotion_output_frequency helper function which is used by the controller applications.
148+
149+
|**Frequency, Hz**|0 (shutdown)|0 (single measurement)|0.1 |0.5 |1 |2 |5 |10 |20 |50 |100 |125 |200 |
150+
|:----------------|:----------:|:--------------------:|:----:|:----:|:----:|:----:|:----:|:----:|:----:|:----:|:----:|:----:|:----:|
151+
|**Value** |`0x0D` |`0x0C` |`0x01`|`0x02`|`0x03`|`0x04`|`0x05`|`0x06`|`0x07`|`0x08`|`0x09`|`0x0A`|`0x0B`|
152+
|**Argument** | 0 | -1 | -10 | -2 | 1 | 2 | 5 | 10 | 20 | 50 | 100 | 125 | 200 |
153+
*/
120154
ridOutputFrequency = 0x03,
155+
/*!
156+
Regulates port baud rate. **NOTE**: the sensor has no possibility of hardware flow control and it cannot report to the system what baud rate should be explicitly used!
157+
The actual value stored in \ref witmotion_config_packet.`raw[0]` can be determined from the following table. \ref witmotion_config_packet.`raw[1]` is set to `0x00`.
158+
The \ref witmotion_baud_rate helper function argument is accepted as `QSerialPort::BaudRate` enumeration member, so only the speed inticated in that enumeration are explicitly supported.
159+
|**Rate, baud**|1200/1400|4800 |9600 |19200 |38400 |57600 |115200|
160+
|:-------------|:-------:|:----:|:----:|:----:|:----:|:----:|:----:|
161+
|**Value** |`0x00` |`0x01`|`0x02`|`0x03`|`0x04`|`0x05`|`0x06`|
162+
163+
This parameter also implicitly sets \ref ridOutputFrequency to the maximal feasible value for the available bandwidth.
164+
*/
121165
ridPortBaudRate = 0x04,
122-
ridAccelerationBiasX = 0x05,
123-
ridAccelerationBiasY = 0x06,
124-
ridAccelerationBiasZ = 0x07,
125-
ridAngularVelocityBiasX = 0x08,
126-
ridAngularVelocityBiasY = 0x09,
127-
ridAngularVelocityBiasZ = 0x0A,
128-
ridMagnetometerBiasX = 0x0B,
129-
ridMagnetometerBiasY = 0x0C,
130-
ridMagnetometerBiasZ = 0x0D,
166+
ridAccelerationBiasX = 0x05, ///< Sets acceleration zero point bias for X axis
167+
ridAccelerationBiasY = 0x06, ///< Sets acceleration zero point bias for Y axis
168+
ridAccelerationBiasZ = 0x07, ///< Sets acceleration zero point bias for Z axis
169+
ridAngularVelocityBiasX = 0x08, ///< Sets angular velocity zero point bias for X axis \note PROOFLESS
170+
ridAngularVelocityBiasY = 0x09, ///< Sets angular velocity zero point bias for Y axis \note PROOFLESS
171+
ridAngularVelocityBiasZ = 0x0A, ///< Sets angular velocity zero point bias for Z axis \note PROOFLESS
172+
ridMagnetometerBiasX = 0x0B, ///< Sets magnetometer zero point bias for X axis \note PROOFLESS
173+
ridMagnetometerBiasY = 0x0C, ///< Sets magnetometer zero point bias for Y axis \note PROOFLESS
174+
ridMagnetometerBiasZ = 0x0D, ///< Sets magnetometer zero point bias for Z axis \note PROOFLESS
131175
ridPortModeD0 = 0x0E,
132176
ridPortModeD1 = 0x0F,
133177
ridPortModeD2 = 0x10,

include/witmotion/wt901-uart.h

+2
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33

44
#include <QSerialPort>
55
#include <QSerialPortInfo>
6+
#include <QDateTime>
67

78
#include <iostream>
89
#include <string>
@@ -50,6 +51,7 @@ class QWitmotionWT901Sensor: public QAbstractWitmotionSensorController
5051
float y,
5152
float z);
5253
virtual void SetI2CAddress(const uint8_t address);
54+
virtual void SetRTC(const QDateTime datetime);
5355
virtual void ConfirmConfiguration();
5456
QWitmotionWT901Sensor(const QString device,
5557
const QSerialPort::BaudRate rate,

src/jy901-control.cpp

+18-1
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,11 @@ int main(int argc, char** args)
120120
"HEX",
121121
"50");
122122
parser.addOption(I2CAddressOption);
123+
QCommandLineOption RTCSetupOption("set-clock",
124+
"Set realtime clock to ISO 8601 datetime [yyyy-MM-dd HH:mm:ss.mss] or NOW",
125+
"DATE TIME",
126+
"NOW");
127+
parser.addOption(RTCSetupOption);
123128

124129
parser.process(app);
125130

@@ -458,7 +463,8 @@ int main(int argc, char** args)
458463
parser.isSet(AxisTransitionOption) ||
459464
parser.isSet(LEDOption) ||
460465
parser.isSet(DisableMeasurementOption) ||
461-
parser.isSet(AccelerationBiasOption))
466+
parser.isSet(AccelerationBiasOption) ||
467+
parser.isSet(RTCSetupOption))
462468
{
463469
std::cout << "Non-blocking configuration, please wait..." << std::endl;
464470
sensor.UnlockConfiguration();
@@ -592,6 +598,17 @@ int main(int argc, char** args)
592598
else
593599
std::cout << "ERROR: Cannot parse value list for acceleration biases. Please use <X:Y:Z> formulation" << std::endl;
594600
}
601+
if(parser.isSet(RTCSetupOption))
602+
{
603+
if(parser.value(RTCSetupOption).toUpper() == "NOW")
604+
sensor.SetRTC(QDateTime::currentDateTime());
605+
else
606+
{
607+
QDateTime datetime = QDateTime::fromString(parser.value(RTCSetupOption), Qt::ISODateWithMs);
608+
sensor.SetRTC(datetime);
609+
sleep(1);
610+
}
611+
}
595612
sensor.ConfirmConfiguration();
596613
std::cout << "Reconfiguration completed, proceeding to normal operation" << std::endl << std::endl;
597614
}

src/wt901-control.cpp

+18-1
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,11 @@ int main(int argc, char** args)
119119
"HEX",
120120
"50");
121121
parser.addOption(I2CAddressOption);
122+
QCommandLineOption RTCSetupOption("set-clock",
123+
"Set realtime clock to ISO 8601 datetime [yyyy-MM-dd HH:mm:ss.mss] or NOW",
124+
"DATE TIME",
125+
"NOW");
126+
parser.addOption(RTCSetupOption);
122127

123128
parser.process(app);
124129

@@ -441,7 +446,8 @@ int main(int argc, char** args)
441446
parser.isSet(AxisTransitionOption) ||
442447
parser.isSet(LEDOption) ||
443448
parser.isSet(DisableMeasurementOption) ||
444-
parser.isSet(AccelerationBiasOption))
449+
parser.isSet(AccelerationBiasOption) ||
450+
parser.isSet(RTCSetupOption))
445451
{
446452
std::cout << "Non-blocking configuration, please wait..." << std::endl;
447453
sensor.UnlockConfiguration();
@@ -571,6 +577,17 @@ int main(int argc, char** args)
571577
else
572578
std::cout << "ERROR: Cannot parse value list for acceleration biases. Please use <X:Y:Z> formulation" << std::endl;
573579
}
580+
if(parser.isSet(RTCSetupOption))
581+
{
582+
if(parser.value(RTCSetupOption).toUpper() == "NOW")
583+
sensor.SetRTC(QDateTime::currentDateTime());
584+
else
585+
{
586+
QDateTime datetime = QDateTime::fromString(parser.value(RTCSetupOption), Qt::ISODateWithMs);
587+
sensor.SetRTC(datetime);
588+
sleep(1);
589+
}
590+
}
574591
sensor.ConfirmConfiguration();
575592
std::cout << "Reconfiguration completed, proceeding to normal operation" << std::endl << std::endl;
576593
}

src/wt901-uart.cpp

+34-2
Original file line numberDiff line numberDiff line change
@@ -248,9 +248,7 @@ void QWitmotionWT901Sensor::SetAccelerationBias(float x, float y, float z)
248248
void QWitmotionWT901Sensor::SetI2CAddress(const uint8_t address)
249249
{
250250
if(address > 0x7F)
251-
{
252251
emit ErrorOccurred("I2C address is hexadecimal int, 7 bits long. Dropping request");
253-
}
254252
else
255253
{
256254
witmotion_config_packet config_packet;
@@ -265,6 +263,40 @@ void QWitmotionWT901Sensor::SetI2CAddress(const uint8_t address)
265263
}
266264
}
267265

266+
void QWitmotionWT901Sensor::SetRTC(const QDateTime datetime)
267+
{
268+
if(!datetime.isValid())
269+
emit ErrorOccurred("Invalid date string specified. Dropping request");
270+
else
271+
{
272+
witmotion_config_packet config_packet;
273+
config_packet.header_byte = WITMOTION_CONFIG_HEADER;
274+
config_packet.key_byte = WITMOTION_CONFIG_KEY;
275+
ttyout << "Setting up RTC date/time origin" << ENDL;
276+
config_packet.address_byte = ridTimeMilliseconds;
277+
uint16_t msec = static_cast<uint16_t>(datetime.time().msec());
278+
std::copy(&msec, &msec + 1, config_packet.setting.raw);
279+
emit SendConfig(config_packet);
280+
usleep(100000);
281+
config_packet.address_byte = ridTimeMinuteSecond;
282+
config_packet.setting.raw[0] = static_cast<uint8_t>(datetime.time().minute());
283+
config_packet.setting.raw[1] = static_cast<uint8_t>(datetime.time().second());
284+
emit SendConfig(config_packet);
285+
sleep(1);
286+
config_packet.address_byte = ridTimeDayHour;
287+
config_packet.setting.raw[0] = static_cast<uint8_t>(datetime.date().day());
288+
config_packet.setting.raw[1] = static_cast<uint8_t>(datetime.time().hour());
289+
emit SendConfig(config_packet);
290+
sleep(1);
291+
config_packet.address_byte = ridTimeYearMonth;
292+
config_packet.setting.raw[0] = static_cast<uint8_t>(datetime.date().year() - 2000);
293+
config_packet.setting.raw[1] = static_cast<uint8_t>(datetime.date().month());
294+
emit SendConfig(config_packet);
295+
sleep(1);
296+
}
297+
298+
}
299+
268300
QWitmotionWT901Sensor::QWitmotionWT901Sensor(const QString device,
269301
const QSerialPort::BaudRate rate,
270302
const uint32_t polling_period):

version

+1-1
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
0.8.22-alpha
1+
0.9.17-alpha

0 commit comments

Comments
 (0)