Set IMU timestamp to time of validity vs. data ready. Tweaked Ubx PPS output

Author: avtoku

boards/varmint_h7/common/drivers/Adc.cpp

@@ -188,12 +188,11 @@ void Adc::endDma(ADC_HandleTypeDef * hadc)
     double vcc = p.vRef;
 #endif
     for (int i = 0; i < ADC_CHANNELS; i++) {
-      //    	p.volts[i] = ((double)(adc_counts[i]&0xFFFF)/65535.0 - cfg_[i].offset) * p.vRef * cfg_[i].scaleFactor;
-      p.volts[i] = ((double) (adc_counts[i] & 0xFFFF) / 65535.0 * p.vRef - vcc * cfg_[i].offset) * cfg_[i].scaleFactor;
+      p.volts[i] = ((double) (adc_counts[i] & 0xFFFF) / 65535.0 * p.vRef - cfg_[i].offset) * cfg_[i].scaleFactor;
     }
 
-    p.header.timestamp = drdy_;
     p.header.complete = time64.Us();
+    p.header.timestamp = (drdy_+p.header.complete)/2;
     write((uint8_t *) &p, sizeof(p));
     ext_read = 0;
     int_read = 0;

boards/varmint_h7/common/drivers/Adis165xx.cpp

@@ -92,8 +92,8 @@ uint32_t Adis165xx::init(
   htim_ = htim;
   htimChannel_ = htim_channel;
 
-//  groupDelay_ = (uint64_t) 1510
-//    + (uint64_t) 500000 / sampleRateHz_; // us, Approximate, Accel is 1.57ms, Gyro x&y are 1.51ms, and Gyro z is 1.29ms.
+  groupDelay_ = (uint64_t) 1510 + (uint64_t) 500000 / sampleRateHz_- 250;
+  // us, Approximate, Accel is 1.57ms, Gyro x&y are 1.51ms, and Gyro z is 1.29ms.
 
   HAL_GPIO_WritePin(spi_.port_, spi_.pin_, GPIO_PIN_SET);
   HAL_GPIO_WritePin(resetPort_, resetPin_, GPIO_PIN_SET);
@@ -224,6 +224,8 @@ bool Adis165xx::startDma(void) // called to start dma read
 void Adis165xx::endDma(void) // called when DMA data is ready
 {
   uint8_t * rx = spi_.endDma();
+  ImuPacket p = {0};
+
   if (sampleRateHz_ == 2000) {
     // compute checksum
     uint16_t sum = 0;
@@ -233,9 +235,7 @@ void Adis165xx::endDma(void) // called when DMA data is ready
     for (int i = 0; i < ADIS_BUFFBYTES16 / 2; i++)
       data[i] = (int16_t) rx[2 * i] << 8 | ((int16_t) rx[2 * i + 1] & 0x00FF);
     if (sum == data[10]) {
-      ImuPacket p;
-      p.header.timestamp = drdy_;
-      p.header.complete = time64.Us();      p.header.status = (uint16_t) data[1];
+      p.header.status = (uint16_t) data[1];
       p.gyro[0] = -(double) data[2] * 0.001745329251994; // rad/s, or use 0.1 deg/s
       p.gyro[1] = -(double) data[3] * 0.001745329251994; // rad/s, or use 0.1 deg/s
       p.gyro[2] = (double) data[4] * 0.001745329251994;  // rad/s, or use 0.1 deg/s
@@ -244,7 +244,6 @@ void Adis165xx::endDma(void) // called when DMA data is ready
       p.accel[2] = (double) data[7] * 0.01225;           // m/s^2
       p.temperature = (double) data[8] * 0.1 + 273.15;   // K
       p.dataTime = (double) ((uint16_t) data[9]) / sampleRateHz_;
-      if (p.header.status == ADIS_OK) write((uint8_t *) &p, sizeof(p));
     }
   } else {
     // compute checksum
@@ -256,9 +255,6 @@ void Adis165xx::endDma(void) // called when DMA data is ready
       data[i] = (int16_t) rx[2 * i] << 8 | ((int16_t) rx[2 * i + 1] & 0x00FF);
 
     if (sum == data[16]) {
-      ImuPacket p;
-      p.header.timestamp = drdy_;
-      p.header.complete = time64.Us();
       p.header.status = (uint16_t) data[1];
       p.gyro[0] = val(rx + 4) * 0.001745329251994;     // rad/s, or use 0.1 deg/s
       p.gyro[1] = val(rx + 8) * 0.001745329251994;     // rad/s, or use 0.1 deg/s
@@ -268,14 +264,17 @@ void Adis165xx::endDma(void) // called when DMA data is ready
       p.accel[2] = val(rx + 24) * 0.01225;              // m/s^2
       p.temperature = (double) data[14] * 0.1 + 273.15; // K
       p.dataTime = (double) ((uint16_t) data[15]) / sampleRateHz_;
-      if (p.header.status == ADIS_OK)
-      {
-        rotate(p.gyro);
-        rotate(p.accel);
-        write((uint8_t *) &p, sizeof(p));
-      }
     }
   }
+  if (p.header.status == ADIS_OK)
+  {
+    p.header.timestamp = drdy_-groupDelay_;
+    rotate(p.gyro);
+    rotate(p.accel);
+    p.header.complete = time64.Us();
+    write((uint8_t *) &p, sizeof(p));
+  }
+
 }
 
 void Adis165xx::writeRegister(uint8_t address, uint16_t value)

boards/varmint_h7/common/drivers/Adis165xx.h

@@ -78,6 +78,7 @@ class Adis165xx : public Status , public MiscRotatable
   DoubleBuffer double_buffer_;
 
   uint16_t sampleRateHz_;
+  uint64_t groupDelay_;
   // SPI Stuff
   Spi spi_;
   uint16_t drdyPin_;

boards/varmint_h7/common/drivers/Bmi088.cpp

@@ -97,19 +97,19 @@ uint32_t Bmi088::init(
   if (sampleRateHz_ <= 400) {
     sampleRateHz_ = 400;
     syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_400HZ;
-   // groupDelay_ = 7000;
+    groupDelay_ = 7000;
   } else if (sampleRateHz_ <= 1000) {
     sampleRateHz_ = 1000;
     syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_1000HZ;
-  //  groupDelay_ = 2500;
+    groupDelay_ = 2500;
   } else if (sampleRateHz_ <= 2000) {
     sampleRateHz_ = 2000;
     syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_2000HZ;
-  //  groupDelay_ = 1500;
+    groupDelay_ = 1500;
   } else {
     sampleRateHz_ = 400;
     syncCfgMode_ = BMI08_ACCEL_DATA_SYNC_MODE_400HZ;
-  //  groupDelay_ = 7000;
+    groupDelay_ = 7000;
   }
 
   time64.dMs(50); // Some time to ensure power-on completed.
@@ -331,7 +331,7 @@ void Bmi088::endDma(void) // DMA complete routine
     p.gyro[2] = scale_factor * (double) data;
 
     p.header.complete = time64.Us();
-    p.header.timestamp = drdy_;
+    p.header.timestamp = drdy_-groupDelay_;
 
     rotate(p.gyro);
     rotate(p.accel);

boards/varmint_h7/common/drivers/Bmi088.h

@@ -82,6 +82,7 @@ class Bmi088 : public Status, public MiscRotatable
   bool write(uint8_t * data, uint16_t size) { return double_buffer_.write(data, size)==DoubleBufferStatus::OK; }
   DoubleBuffer double_buffer_;
   uint16_t sampleRateHz_;
+  uint64_t groupDelay_;
   // SPI Stuff
   Spi spiA_;
   Spi spiG_;

boards/varmint_h7/common/drivers/Ubx.cpp

@@ -471,10 +471,10 @@ uint16_t Ubx::cfgTp5(uint32_t hz)
   payload[3] = 0x00;                 // reserved
   SET(payload + 4, 0x0000, int16_t); // antenna delay
   SET(payload + 6, 0x0000, int16_t); // rf delay
-  SET(payload + 8, 0, uint32_t); // frequency when not locked
+  SET(payload + 8, hz, uint32_t); // frequency when not locked
   SET(payload + 12, hz, uint32_t); // frequency when locked
-  SET(payload + 16, pulse_len_us, uint32_t); // pulse length (1ms)
-  SET(payload + 20, pulse_len_us, uint32_t); // pulse length (1ms)
+  SET(payload + 16, 0, uint32_t); // pulse length when not locked (1ms)
+  SET(payload + 20, pulse_len_us, uint32_t); // pulse length when locked (1ms)
   SET(payload + 24, 0x0000, uint32_t); // delay
   SET(payload + 28, 0x007F, uint32_t); // 0x0111 1111
 

boards/varmint_h7/pixracer_pro/specific/BoardConfig.h

@@ -41,7 +41,7 @@
 #include "CommonConfig.h"
 
 #define SANDBOX false
-#define BOARD_STATUS_PRINT false
+#define BOARD_STATUS_PRINT (false|SANDBOX)
 #define USE_TELEM 0 // 1 = use UART, 0 = use VCP for link to companion computer.
 
 // UART used for printf's
@@ -264,7 +264,7 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // USB FS (48 MB/s)
 { \
   {ADC_REGULAR_RANK_1, ADC_CHANNEL_11, 1.000, 0.0},         /* ADC_RSSI_V */ \
   {ADC_REGULAR_RANK_2, ADC_CHANNEL_14, 12.62, 0.0},         /* ADC_BATTERY_VOLTS */ \
-  {ADC_REGULAR_RANK_3, ADC_CHANNEL_15, 60.5, 0.0},         /* ADC_BATTERY_CURRENT */ \
+  {ADC_REGULAR_RANK_3, ADC_CHANNEL_15, 60.5, 0.0747},         /* ADC_BATTERY_CURRENT */ \
   {ADC_REGULAR_RANK_4, ADC_CHANNEL_18, 2.000, 0.0},         /* ADC_5V0 */ \
   {ADC_REGULAR_RANK_1, ADC_CHANNEL_TEMPSENSOR, 1.000, 0.0}, /* ADC_STM_TEMPERATURE */ \
   {ADC_REGULAR_RANK_2, ADC_CHANNEL_VBAT, 4.000, 0.0},       /* ADC_STM_VBAT */ \