Skip to content

Commit f1325ab

Browse files
avtokuavtoku
committed
Added rotation matrices for BSP sensors, reduced MAV_COMPONENT enum to 2 entries (xml)
1 parent c2f7375 commit f1325ab

File tree

24 files changed

+138
-201
lines changed

24 files changed

+138
-201
lines changed

‎boards/varmint_h7/common/CommonConfig.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,8 @@
9696
#define UBX_FAIL_BAUD_CHANGE (0x00000400)
9797
#define VOLTAGE_SET_FAIL (0x00000800)
9898
#define TIMERS_INVALID (0x00001000)
99+
#define AUAV_PITOT_ERROR (0x00002000)
100+
#define AUAV_BARO_ERROR (0x00002000)
99101

100102
extern ADC_HandleTypeDef hadc1;
101103
extern ADC_HandleTypeDef hadc2;

‎boards/varmint_h7/common/drivers/Adc.cpp‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -218,8 +218,8 @@ bool Adc::display(void)
218218
misc_printf("\n");
219219

220220
misc_printf(" %-8s : ", "Pwr");
221-
misc_f32(22.2 / 1.02, 22.2 * 1.02, p.volts[ADC_BATTERY_VOLTS], "Batt", "%5.1f", "V"); //
222-
misc_f32(0.1, 1.0, p.volts[ADC_BATTERY_CURRENT], "Batt", "%5.1f", "A"); //
221+
misc_f32(22.2 / 1.02, 22.2 * 1.02, p.volts[ADC_BATTERY_VOLTS], "BattV", "%5.1f", "V"); //
222+
misc_f32(0.1, 1.0, p.volts[ADC_BATTERY_CURRENT], "BattI", "%5.1f", "A"); //
223223
misc_printf("\n");
224224

225225
misc_printf(" %-8s : ", "PS_FC");
@@ -228,28 +228,28 @@ bool Adc::display(void)
228228
misc_f32(3.3 / 1.02, 3.3 * 1.02, p.volts[ADC_3V3], "3V3_FC", "%5.1f", "V"); //
229229
#endif
230230
#ifdef ADC_3V3_CURRENT
231-
misc_f32(0.0, 1.0, p.volts[ADC_3V3_CURRENT], "3V3_FC", "%5.1f", "A"); //
231+
misc_f32(0.0, 1.0, p.volts[ADC_3V3_CURRENT], "3I3_FC", "%5.1f", "A"); //
232232
#endif
233233

234234
#ifdef ADC_5V0
235235
misc_f32(5.0 / 1.02, 5.0 * 1.02, p.volts[ADC_5V0], "5V0_FC", "%5.1f", "V"); //
236236
#endif
237237
#ifdef ADC_5V0_CURRENT
238-
misc_f32(0.0, 1.0, p.volts[ADC_5V0_CURRENT], "5V0_FC", "%5.1f", "A"); //
238+
misc_f32(0.0, 1.0, p.volts[ADC_5V0_CURRENT], "5I0_FC", "%5.1f", "A"); //
239239
#endif
240240

241241
#ifdef ADC_12V
242242
misc_f32(12 / 1.02, 12 * 1.02, p.volts[ADC_12V], "12V_FC", "%5.1f", "V"); //
243243
#endif
244244
#ifdef ADC_12V_CURRENT
245-
misc_f32(0.0, 1.0, p.volts[ADC_12V_CURRENT], "12V_FC", "%5.1f", "A"); //
245+
misc_f32(0.0, 1.0, p.volts[ADC_12V_CURRENT], "12I_FC", "%5.1f", "A"); //
246246
#endif
247247

248248
#ifdef ADC_SERVO_VOLTS
249-
misc_f32(8.2 / 1.02, 8.2 * 1.02, p.volts[ADC_SERVO_VOLTS], "Servo", "%5.1f", "V"); //
249+
misc_f32(8.2 / 1.02, 8.2 * 1.02, p.volts[ADC_SERVO_VOLTS], "ServoV", "%5.1f", "V"); //
250250
#endif
251251
#ifdef ADC_SERVO_CURRENT
252-
misc_f32(0.0, 2.0, p.volts[ADC_SERVO_CURRENT], "Servo", "%5.1f", "A"); //
252+
misc_f32(0.0, 2.0, p.volts[ADC_SERVO_CURRENT], "ServoI", "%5.1f", "A"); //
253253
#endif
254254
misc_printf("\n");
255255

@@ -258,14 +258,14 @@ bool Adc::display(void)
258258
misc_f32(3.3 / 1.02, 3.3 * 1.02, p.volts[ADC_CC_3V3], "3V3_CC", "%5.1f", "V"); //
259259
#endif
260260
#ifdef ADC_CC_3V3_CURRENT
261-
misc_f32(0.0, 2.0, p.volts[ADC_CC_3V3_CURRENT], "3V3_CC", "%5.1f", "A"); //
261+
misc_f32(0.0, 2.0, p.volts[ADC_CC_3V3_CURRENT], "3I3_CC", "%5.1f", "A"); //
262262
#endif
263263

264264
#ifdef ADC_CC_5V0
265265
misc_f32(5.0 / 1.02, 5.0 * 1.02, p.volts[ADC_CC_5V0], "5V0_CC", "%5.1f", "V"); //
266266
#endif
267267
#ifdef ADC_CC_5V0_CURRENT
268-
misc_f32(0.0, 2.0, p.volts[ADC_CC_5V0_CURRENT], " ", "%5.1f", "A"); //
268+
misc_f32(0.0, 2.0, p.volts[ADC_CC_5V0_CURRENT], "5I0_CC", "%5.1f", "A"); //
269269
#endif
270270

271271
misc_printf("\n");

‎boards/varmint_h7/common/drivers/Adis165xx.cpp‎

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -72,8 +72,11 @@ uint32_t Adis165xx::init(
7272
// ADIS165xx initializers
7373
GPIO_TypeDef * reset_port, // Reset GPIO Port
7474
uint16_t reset_pin, // Reset GPIO Pin
75-
TIM_HandleTypeDef * htim, TIM_TypeDef * htim_instance, uint32_t htim_channel, uint32_t htim_period_us)
75+
TIM_HandleTypeDef * htim, TIM_TypeDef * htim_instance, uint32_t htim_channel, uint32_t htim_period_us,
76+
const double *rotation
77+
)
7678
{
79+
memcpy(rotation_, rotation, sizeof(double)*9);
7780
snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Adis165xx");
7881
initializationStatus_ = DRIVER_OK;
7982
sampleRateHz_ = sample_rate_hz;
@@ -260,15 +263,20 @@ void Adis165xx::endDma(void) // called when DMA data is ready
260263
p.drdy = drdy_;
261264
p.groupDelay = groupDelay_;
262265
p.header.status = (uint16_t) data[1];
263-
p.gyro[0] = -val(rx + 4) * 0.001745329251994; // rad/s, or use 0.1 deg/s
264-
p.gyro[1] = -val(rx + 8) * 0.001745329251994; // rad/s, or use 0.1 deg/s
266+
p.gyro[0] = val(rx + 4) * 0.001745329251994; // rad/s, or use 0.1 deg/s
267+
p.gyro[1] = val(rx + 8) * 0.001745329251994; // rad/s, or use 0.1 deg/s
265268
p.gyro[2] = val(rx + 12) * 0.001745329251994; // rad/s, or use 0.1 deg/s
266-
p.accel[0] = -val(rx + 16) * 0.01225; // m/s^2
267-
p.accel[1] = -val(rx + 20) * 0.01225; // m/s^2
269+
p.accel[0] = val(rx + 16) * 0.01225; // m/s^2
270+
p.accel[1] = val(rx + 20) * 0.01225; // m/s^2
268271
p.accel[2] = val(rx + 24) * 0.01225; // m/s^2
269272
p.temperature = (double) data[14] * 0.1 + 273.15; // K
270273
p.dataTime = (double) ((uint16_t) data[15]) / sampleRateHz_;
271-
if (p.header.status == ADIS_OK) rxFifo_.write((uint8_t *) &p, sizeof(p));
274+
if (p.header.status == ADIS_OK)
275+
{
276+
rotate(p.gyro);
277+
rotate(p.accel);
278+
rxFifo_.write((uint8_t *) &p, sizeof(p));
279+
}
272280
}
273281
}
274282
}
@@ -306,9 +314,11 @@ bool Adis165xx::display(void)
306314
char name[] = "Adis165xx (imu)";
307315
if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) {
308316
misc_header(name, p.drdy, p.header.timestamp, p.groupDelay);
309-
misc_f32(-0.1, 0.1, p.accel[0] / 9.80665, "ax", "%6.2f", "g");
310-
misc_f32(-0.1, 0.1, p.accel[1] / 9.80665, "ay", "%6.2f", "g");
311-
misc_f32(-1.1, -0.9, p.accel[2] / 9.80665, "az", "%6.2f", "g");
317+
misc_f32(nan(""), nan(""), p.accel[0] / 9.80665, "ax", "%6.2f", "g");
318+
misc_f32(nan(""), nan(""), p.accel[1] / 9.80665, "ay", "%6.2f", "g");
319+
misc_f32(nan(""), nan(""), p.accel[2] / 9.80665, "az", "%6.2f", "g");
320+
double a = sqrt(p.accel[0]*p.accel[0]+p.accel[1]*p.accel[1]+p.accel[2]*p.accel[2]);
321+
misc_f32(0.8, 1.2, a / 9.80665, "|a|", "%6.2f", "g");
312322

313323
misc_f32(-1, 1, (float) p.gyro[0] * 57.2958, "p", "%6.2f", "dps");
314324
misc_f32(-1, 1, (float) p.gyro[1] * 57.2958, "q", "%6.2f", "dps");

‎boards/varmint_h7/common/drivers/Adis165xx.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,14 +57,17 @@ class Adis165xx : public Driver
5757
// ADIS165xx initializers
5858
GPIO_TypeDef * reset_port, // Reset GPIO Port
5959
uint16_t reset_pin, // Reset GPIO Pin
60-
TIM_HandleTypeDef * htim, TIM_TypeDef * htim_instance, uint32_t htim_channel, uint32_t htim_period_us);
60+
TIM_HandleTypeDef * htim, TIM_TypeDef * htim_instance, uint32_t htim_channel, uint32_t htim_period_us,
61+
const double *rotation
62+
);
6163

6264
void endDma(void);
6365
bool startDma(void);
6466
bool display(void) override;
6567
bool isMy(uint16_t exti_pin) { return drdyPin_ == exti_pin; }
6668
bool isMy(SPI_HandleTypeDef * hspi) { return hspi == spi_.hspi(); }
6769
SPI_HandleTypeDef * hspi(void) { return spi_.hspi(); }
70+
void set_rotation(double rotation[9]) { memcpy(rotation_,&rotation, 9*sizeof(double));}
6871

6972
private:
7073
// SPI Stuff

‎boards/varmint_h7/common/drivers/Auav.cpp‎

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,7 @@ uint32_t Auav::init(uint16_t sample_rate_hz, //
6767
GPIO_TypeDef * baro_cs_port, uint16_t baro_cs_pin, // Baro CS
6868
SPI_HandleTypeDef * hspi)
6969
{
70-
//snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Auav");
70+
snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "AuavPitotBaro");
7171
initializationStatus_ = DRIVER_OK;
7272
sampleRateHz_ = sample_rate_hz;
7373

@@ -101,8 +101,8 @@ uint32_t Auav::init(uint16_t sample_rate_hz, //
101101

102102
rxFifo_[AUAV_PITOT].init(AUAV_FIFO_BUFFERS, sizeof(PressurePacket), auav_pitot_fifo_rx_buffer);
103103
char name[] = "Auav (pitot)";
104-
memset(name_[AUAV_PITOT], '\0', sizeof(name_[AUAV_PITOT]));
105-
strcpy(name_[AUAV_PITOT], name);
104+
memset(name_local_[AUAV_PITOT], '\0', sizeof(name_local_[AUAV_PITOT]));
105+
strcpy(name_local_[AUAV_PITOT], name);
106106
memset(cmdBytes_[AUAV_PITOT], 0, AUAV_CMD_BYTES);
107107
cmdBytes_[AUAV_PITOT][0] = 0xAD; // ~100 Hz
108108

@@ -128,8 +128,8 @@ uint32_t Auav::init(uint16_t sample_rate_hz, //
128128

129129
rxFifo_[AUAV_BARO].init(AUAV_FIFO_BUFFERS, sizeof(PressurePacket), auav_baro_fifo_rx_buffer);
130130
char name[] = "Auav (baro) ";
131-
memset(name_[AUAV_BARO], '\0', sizeof(name_[AUAV_BARO]));
132-
strcpy(name_[AUAV_BARO], name);
131+
memset(name_local_[AUAV_BARO], '\0', sizeof(name_local_[AUAV_BARO]));
132+
strcpy(name_local_[AUAV_BARO], name);
133133
memset(cmdBytes_[AUAV_BARO], 0, AUAV_CMD_BYTES);
134134
cmdBytes_[AUAV_BARO][0] = 0xAC; // ~100 Hz
135135

@@ -184,13 +184,14 @@ uint32_t Auav::init(uint16_t sample_rate_hz, //
184184
uint8_t sensor_status = 0x00;
185185
HAL_StatusTypeDef hal_status = spi_[i].rx(&tx, &sensor_status, 1, 200);
186186
misc_printf("HAL Status = 0x%04X : ", hal_status);
187-
misc_printf("%s Status = 0x%02X (0x%02X) - ", name_[i], sensor_status, sensor_status_ready_[i]);
187+
misc_printf("%s Status = 0x%02X (0x%02X) - ", name_local_[i], sensor_status, sensor_status_ready_[i]);
188188

189189
if (sensor_status == sensor_status_ready_[i]) {
190190
misc_printf("OK\n");
191191
} else {
192192
misc_printf("ERROR\n");
193-
initializationStatus_ |= DRIVER_SELF_DIAG_ERROR;
193+
if(i==AUAV_PITOT) initializationStatus_ |= AUAV_PITOT_ERROR; //DRIVER_SELF_DIAG_ERROR;
194+
else initializationStatus_ |= AUAV_BARO_ERROR; //DRIVER_SELF_DIAG_ERROR;
194195
}
195196
}
196197
for (int i = 0; i < 2; i++) {
@@ -395,26 +396,26 @@ bool Auav::display(void)
395396
PressurePacket p;
396397

397398
if (rxFifoReadMostRecent((uint8_t *) &p, sizeof(p), AUAV_BARO)) {
398-
misc_header(name_[AUAV_BARO], p.drdy, p.header.timestamp, p.groupDelay);
399+
misc_header(name_local_[AUAV_BARO], p.drdy, p.header.timestamp, p.groupDelay);
399400

400401
misc_f32(99, 101, p.pressure / 1000., "Press", "%6.2f", "kPa");
401-
misc_f32(19, 40, p.temperature - 273.15, "Temp", "%5.1f", "C");
402+
misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C");
402403
misc_x16(sensorOk(AUAV_BARO), p.header.status, "Status");
403404
misc_printf("\n");
404405
//return 1;
405406
} else {
406-
misc_printf("%s\n", name_[AUAV_BARO]);
407+
misc_printf("%s\n", name_local_[AUAV_BARO]);
407408
}
408409

409410
if (rxFifoReadMostRecent((uint8_t *) &p, sizeof(p), AUAV_PITOT)) {
410-
misc_header(name_[AUAV_PITOT], p.drdy, p.header.timestamp, p.groupDelay);
411-
misc_f32(2.5, 2.5, p.pressure, "Press", "%6.2f", "Pa");
412-
misc_f32(19, 40, p.temperature - 273.15, "Temp", "%5.1f", "C");
411+
misc_header(name_local_[AUAV_PITOT], p.drdy, p.header.timestamp, p.groupDelay);
412+
misc_f32(-5.0, 5.0, p.pressure, "Press", "%6.2f", "Pa");
413+
misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C");
413414
misc_x16(sensorOk(AUAV_PITOT), p.header.status, "Status");
414415
misc_printf("\n");
415416
//return 1;
416417
} else {
417-
misc_printf("%s\n", name_[AUAV_PITOT]);
418+
misc_printf("%s\n", name_local_[AUAV_PITOT]);
418419
}
419420

420421
return 0;

‎boards/varmint_h7/common/drivers/Auav.h‎

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -77,10 +77,6 @@ class Auav : public Driver
7777

7878
void drdyIsr(uint64_t timestamp, uint16_t exti_pin);
7979

80-
// virtual bool display(void) = 0;
81-
//
82-
// uint16_t rxFifoCount(void) { return rxFifo_.packetCount(); }
83-
// uint16_t rxFifoRead(uint8_t * data, uint16_t size) { return rxFifo_.read(data, size); }
8480
uint16_t rxFifoReadMostRecent(uint8_t * data, uint16_t size, uint8_t id)
8581
{
8682
return rxFifo_[id].readMostRecent(data, size);
@@ -90,9 +86,6 @@ class Auav : public Driver
9086
{
9187
return rxFifo_[AUAV_PITOT].readMostRecent(data, size);
9288
}
93-
// bool drdy(void) { return HAL_GPIO_ReadPin(drdyPort_, drdyPin_); }
94-
// bool dmaRunning(void) { return dmaRunning_; }
95-
uint32_t initializationStatus_ = DRIVER_NOT_INITIALIZED;
9689
uint8_t sensorOk(uint8_t id) { return sensor_status_ready_[id]; }
9790

9891
private:
@@ -107,7 +100,7 @@ class Auav : public Driver
107100
double LIN_A_[2], LIN_B_[2], LIN_C_[2], LIN_D_[2], Es_[2], TC50H_[2], TC50L_[2];
108101
double osDig_[2], fss_[2], off_[2];
109102
uint8_t sensor_status_ready_[2];
110-
char name_[2][16]; // for display
103+
char name_local_[2][16]; // for display
111104

112105
PacketFifo rxFifo_[2];
113106
GPIO_TypeDef * drdyPort_[2];

‎boards/varmint_h7/common/drivers/Bmi088.cpp‎

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -72,9 +72,11 @@ uint32_t Bmi088::init(
7272
uint16_t cs_pin_g, // Chip Select GPIO Pin
7373
// Sensor Specific
7474
uint8_t range_a, // 0,1,2,3 --> 3,6,12,24g for BMI088; 2, 4, 8, 16g for BMI085
75-
uint8_t range_g // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s
75+
uint8_t range_g, // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s
76+
const double *rotation
7677
)
7778
{
79+
memcpy(rotation_,rotation, sizeof(double)*9);
7880
snprintf(name_, STATUS_NAME_MAX_LEN, "%s", "Bmi088");
7981
initializationStatus_ = DRIVER_OK;
8082
sampleRateHz_ = sample_rate_hz;
@@ -287,9 +289,9 @@ void Bmi088::endDma(void) // DMA complete routine
287289

288290
int16_t data;
289291
data = (int16_t) rx[15] << 8 | (int16_t) rx[14];
290-
p.accel[0] = -scale_factor * (double) data;
292+
p.accel[0] = scale_factor * (double) data;
291293
data = (int16_t) rx[17] << 8 | (int16_t) rx[16];
292-
p.accel[1] = -scale_factor * (double) data;
294+
p.accel[1] = scale_factor * (double) data;
293295

294296
// Launch the Accel read for az
295297
HAL_StatusTypeDef hal_status = spiA_.startDma(BMI_ACCEL_SYNC_CMD, BMI_ACCEL_SYNC_BYTES);
@@ -321,9 +323,9 @@ void Bmi088::endDma(void) // DMA complete routine
321323

322324
int16_t data;
323325
data = (int16_t) rx[2] << 8 | (int16_t) rx[1];
324-
p.gyro[0] = -scale_factor * (double) data;
326+
p.gyro[0] = scale_factor * (double) data;
325327
data = (int16_t) rx[4] << 8 | (int16_t) rx[3];
326-
p.gyro[1] = -scale_factor * (double) data;
328+
p.gyro[1] = scale_factor * (double) data;
327329
data = (int16_t) rx[6] << 8 | (int16_t) rx[5];
328330
p.gyro[2] = scale_factor * (double) data;
329331

@@ -332,6 +334,8 @@ void Bmi088::endDma(void) // DMA complete routine
332334

333335
p.header.timestamp = time64.Us();
334336

337+
rotate(p.gyro);
338+
rotate(p.accel);
335339
rxFifo_.write((uint8_t *) &p, sizeof(p));
336340

337341
seqCount_ = 0;
@@ -381,14 +385,16 @@ bool Bmi088::display(void)
381385
char name[] = "Bmi088 (imu)";
382386
if (rxFifo_.readMostRecent((uint8_t *) &p, sizeof(p))) {
383387
misc_header(name, p.drdy, p.header.timestamp, p.groupDelay);
384-
misc_f32(-0.1, 0.1, p.accel[0] / 9.80665, "ax", "%6.2f", "g");
385-
misc_f32(-0.1, 0.1, p.accel[1] / 9.80665, "ay", "%6.2f", "g");
386-
misc_f32(-1.2, -0.8, p.accel[2] / 9.80665, "az", "%6.2f", "g");
388+
misc_f32(nan(""), nan(""), p.accel[0] / 9.80665, "ax", "%6.2f", "g");
389+
misc_f32(nan(""), nan(""), p.accel[1] / 9.80665, "ay", "%6.2f", "g");
390+
misc_f32(nan(""), nan(""), p.accel[2] / 9.80665, "az", "%6.2f", "g");
391+
double a = sqrt(p.accel[0]*p.accel[0]+p.accel[1]*p.accel[1]+p.accel[2]*p.accel[2]);
392+
misc_f32(0.8, 1.2, a / 9.80665, "|a|", "%6.2f", "g");
387393

388394
misc_f32(-0.5, 0.5, p.gyro[0] * 57.2958, "p", "%6.2f", "dps");
389395
misc_f32(-0.5, 0.5, p.gyro[1] * 57.2958, "q", "%6.2f", "dps");
390396
misc_f32(-0.5, 0.5, p.gyro[2] * 57.2958, "r", "%6.2f", "dps");
391-
misc_f32(18, 40, p.temperature - 273.15, "Temp", "%5.1f", "C");
397+
misc_f32(18, 50, p.temperature - 273.15, "Temp", "%5.1f", "C");
392398
misc_printf("Count %7.3f s |", p.dataTime);
393399

394400
misc_printf("\n");

‎boards/varmint_h7/common/drivers/Bmi088.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,8 @@ class Bmi088 : public Driver
6363
uint16_t cs_pin_g, // Chip Select GPIO Pin
6464
// Sensor Specific
6565
uint8_t range_a, // 0,1,2,3,4 --> 2000,1000,500,250,125 deg/s
66-
uint8_t range_g // // 0,1,2,3 --> 3,6,12,24g
66+
uint8_t range_g, // // 0,1,2,3 --> 3,6,12,24g
67+
const double *rotation
6768
);
6869

6970
void endDma(void);

boards/varmint_h7/pixracer_pro/CMakeLists.txt renamed to boards/varmint_h7/common/drivers/CMakeLists.txt

File renamed without changes.

‎boards/varmint_h7/common/drivers/Driver.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,13 @@ class Driver : public Status
6060
uint16_t rxFifoReadMostRecent(uint8_t * data, uint16_t size) { return rxFifo_.readMostRecent(data, size); }
6161
bool drdy(void) { return HAL_GPIO_ReadPin(drdyPort_, drdyPin_); }
6262
bool dmaRunning(void) { return dmaRunning_; }
63+
void rotate(double *x) {
64+
double y[3];
65+
y[0] = x[0]*rotation_[0] + x[1]*rotation_[1] + x[2]*rotation_[2];
66+
y[1] = x[0]*rotation_[3] + x[1]*rotation_[4] + x[2]*rotation_[5];
67+
y[2] = x[0]*rotation_[6] + x[1]*rotation_[7] + x[2]*rotation_[8];
68+
memcpy(x,y,sizeof(double)*3);
69+
}
6370

6471
protected:
6572
PacketFifo rxFifo_;
@@ -69,6 +76,7 @@ class Driver : public Status
6976
uint64_t drdy_, timeout_, launchUs_;
7077
uint64_t groupDelay_ = 0;
7178
bool dmaRunning_ = 0;
79+
double rotation_[9];
7280
};
7381

7482
#endif /* DRIVER_H_ */

0 commit comments

Comments
 (0)