Skip to content

Commit 33a79e9

Browse files
tyethtyeth
committed
add(vector): enable vector type for magnetic field
1 parent 865ac5c commit 33a79e9

File tree

4 files changed

+118
-22
lines changed

4 files changed

+118
-22
lines changed

‎src/components/i2c/drivers/drvBase.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -440,6 +440,15 @@ class drvBase {
440440
*/
441441
virtual bool getEventCurrent(sensors_event_t *currentEvent) { return false; }
442442

443+
/*!
444+
@brief Gets a sensor's magnetic field value.
445+
@param magneticEvent
446+
The magnetic field vector (x, y, z) in microTesla.
447+
@returns True if the sensor value was obtained successfully, False
448+
otherwise.
449+
*/
450+
virtual bool getEventMagneticField(sensors_event_t *magneticEvent) { return false; }
451+
443452
/*!
444453
@brief Gets a sensor's Raw value.
445454
@param rawEvent
@@ -665,6 +674,10 @@ class drvBase {
665674
{wippersnapper_sensor_SensorType_SENSOR_TYPE_TVOC,
666675
[this](sensors_event_t *event) -> bool {
667676
return this->getEventTVOC(event);
677+
}},
678+
{wippersnapper_sensor_SensorType_SENSOR_TYPE_MAGNETIC_FIELD,
679+
[this](sensors_event_t *event) -> bool {
680+
return this->getEventMagneticField(event);
668681
}}}; ///< SensorType to function call map
669682

670683
wippersnapper_sensor_SensorType

‎src/components/i2c/drivers/drvQmc5883p.cpp‎

Lines changed: 49 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -65,14 +65,14 @@ bool drvQmc5883p::begin() {
6565

6666
/*******************************************************************************/
6767
/*!
68-
@brief Gets the QMC5883P's magnetometer sensor event.
69-
@param magEvent
68+
@brief Gets the QMC5883P's magnetometer sensor event as raw magnitude.
69+
@param rawEvent
7070
Pointer to the magnetometer sensor event.
7171
@returns True if the sensor event was obtained successfully, False
7272
otherwise.
7373
*/
7474
/*******************************************************************************/
75-
bool drvQmc5883p::getEventRaw(sensors_event_t *magEvent) {
75+
bool drvQmc5883p::getEventRaw(sensors_event_t *rawEvent) {
7676
// Check if data is ready before reading
7777
if (!_qmc->isDataReady()) {
7878
return false;
@@ -90,20 +90,7 @@ bool drvQmc5883p::getEventRaw(sensors_event_t *magEvent) {
9090
// Get Gauss field data
9191
if (!_qmc->getGaussField(&gx, &gy, &gz)) {
9292
WS_DEBUG_PRINTLN("Failed to read Gauss field data");
93-
WS_DEBUG_PRINT("Raw X: ");
94-
WS_DEBUG_PRINTLN(x);
95-
WS_DEBUG_PRINT("Raw Y: ");
96-
WS_DEBUG_PRINTLN(y);
97-
WS_DEBUG_PRINT("Raw Z: ");
98-
WS_DEBUG_PRINTLN(z);
9993
return false;
100-
} else {
101-
WS_DEBUG_PRINT("Gauss X: ");
102-
WS_DEBUG_PRINTLN(gx);
103-
WS_DEBUG_PRINT("Gauss Y: ");
104-
WS_DEBUG_PRINTLN(gy);
105-
WS_DEBUG_PRINT("Gauss Z: ");
106-
WS_DEBUG_PRINTLN(gz);
10794
}
10895

10996
// Check for overflow
@@ -114,12 +101,56 @@ bool drvQmc5883p::getEventRaw(sensors_event_t *magEvent) {
114101

115102
// Calculate magnitude in Gauss
116103
float magnitude_G = sqrtf(gx * gx + gy * gy + gz * gz);
117-
magEvent->data[0] = magnitude_G;
104+
rawEvent->data[0] = magnitude_G;
105+
return true;
106+
}
107+
108+
/*******************************************************************************/
109+
/*!
110+
@brief Gets the QMC5883P's magnetic field vector.
111+
@param magneticEvent
112+
Pointer to the magnetic field sensor event.
113+
@returns True if the sensor event was obtained successfully, False
114+
otherwise.
115+
*/
116+
/*******************************************************************************/
117+
bool drvQmc5883p::getEventMagneticField(sensors_event_t *magneticEvent) {
118+
// Check if data is ready before reading
119+
if (!_qmc->isDataReady()) {
120+
return false;
121+
}
122+
123+
int16_t x, y, z;
124+
float gx, gy, gz;
125+
126+
// Get raw magnetic data
127+
if (!_qmc->getRawMagnetic(&x, &y, &z)) {
128+
WS_DEBUG_PRINTLN("Failed to read raw magnetic data");
129+
return false;
130+
}
131+
132+
// Get Gauss field data
133+
if (!_qmc->getGaussField(&gx, &gy, &gz)) {
134+
WS_DEBUG_PRINTLN("Failed to read Gauss field data");
135+
return false;
136+
}
137+
138+
// Check for overflow
139+
if (_qmc->isOverflow()) {
140+
WS_DEBUG_PRINTLN("QMC5883P data overflow - skipping reading");
141+
return false;
142+
}
143+
144+
// Convert from Gauss to microTesla (1 Gauss = 100 microTesla)
145+
magneticEvent->magnetic.x = gx * 100.0f;
146+
magneticEvent->magnetic.y = gy * 100.0f;
147+
magneticEvent->magnetic.z = gz * 100.0f;
148+
118149
return true;
119150
}
120151

121152
void drvQmc5883p::ConfigureDefaultSensorTypes() {
122153
_default_sensor_types_count = 1;
123154
_default_sensor_types[0] =
124-
wippersnapper_sensor_SensorType_SENSOR_TYPE_RAW;
155+
wippersnapper_sensor_SensorType_SENSOR_TYPE_MAGNETIC_FIELD;
125156
}

‎src/components/i2c/drivers/drvQmc5883p.h‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,17 @@ class drvQmc5883p : public drvBase {
6969
/*******************************************************************************/
7070
bool getEventRaw(sensors_event_t *magEvent);
7171

72+
/*******************************************************************************/
73+
/*!
74+
@brief Gets the QMC5883P's magnetic field vector.
75+
@param magneticEvent
76+
Pointer to the magnetic field sensor event.
77+
@returns True if the sensor event was obtained successfully, False
78+
otherwise.
79+
*/
80+
/*******************************************************************************/
81+
bool getEventMagneticField(sensors_event_t *magneticEvent);
82+
7283
void ConfigureDefaultSensorTypes() override;
7384

7485
protected:

‎src/components/i2c/model.cpp‎

Lines changed: 45 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,33 @@ float GetValueFromSensorsEvent(wippersnapper_sensor_SensorType sensor_type,
128128
return value;
129129
}
130130

131+
/*!
132+
@brief Returns the vector event value mapped to a sensor event.
133+
@param sensor_type
134+
The SensorType.
135+
@param event
136+
The sensors_event_t event.
137+
@returns The value of the SensorType as an X/Y/Z vector.
138+
*/
139+
wippersnapper_sensor_SensorEvent_SensorEvent3DVector
140+
GetValueFromSensorsEventVector(wippersnapper_sensor_SensorType sensor_type,
141+
sensors_event_t *event) {
142+
wippersnapper_sensor_SensorEvent_SensorEvent3DVector value = {0.0, 0.0, 0.0};
143+
switch (sensor_type) {
144+
case wippersnapper_sensor_SensorType_SENSOR_TYPE_MAGNETIC_FIELD:
145+
value.x = event->magnetic.x;
146+
value.y = event->magnetic.y;
147+
value.z = event->magnetic.z;
148+
break;
149+
default:
150+
value.x = 0.0;
151+
value.y = 0.0;
152+
value.z = 0.0;
153+
break;
154+
}
155+
return value;
156+
}
157+
131158
/*!
132159
@brief Decodes a I2cDeviceRemove message from an input stream.
133160
@param stream
@@ -355,10 +382,24 @@ bool I2cModel::AddI2cDeviceSensorEvent(
355382
_msg_i2c_device_event
356383
.i2c_device_events[_msg_i2c_device_event.i2c_device_events_count]
357384
.type = sensor_type;
358-
float value = GetValueFromSensorsEvent(sensor_type, &event);
359-
_msg_i2c_device_event
360-
.i2c_device_events[_msg_i2c_device_event.i2c_device_events_count]
361-
.value.float_value = value;
385+
if (sensor_type ==
386+
wippersnapper_sensor_SensorType_SENSOR_TYPE_MAGNETIC_FIELD) {
387+
wippersnapper_sensor_SensorEvent_SensorEvent3DVector value_vect = GetValueFromSensorsEventVector(sensor_type, &event);
388+
_msg_i2c_device_event
389+
.i2c_device_events[_msg_i2c_device_event.i2c_device_events_count]
390+
.value.vector_value.x = value_vect.x;
391+
_msg_i2c_device_event
392+
.i2c_device_events[_msg_i2c_device_event.i2c_device_events_count]
393+
.value.vector_value.y = value_vect.y;
394+
_msg_i2c_device_event
395+
.i2c_device_events[_msg_i2c_device_event.i2c_device_events_count]
396+
.value.vector_value.z = value_vect.z;
397+
} else {
398+
float value = GetValueFromSensorsEvent(sensor_type, &event);
399+
_msg_i2c_device_event
400+
.i2c_device_events[_msg_i2c_device_event.i2c_device_events_count]
401+
.value.float_value = value;
402+
}
362403

363404
_msg_i2c_device_event.i2c_device_events_count++;
364405
return true;

0 commit comments

Comments
 (0)